TeensyStep Help

[Cryoschrome]

Hi,
I have a trajectory, which contains 3 arrays (positions, velocity, and acceleration). Sample arrays are given below.

The logic I need to implement is such that at position[0] velocity and acceleration should also be velocity[0] and acceleration[0] and when position[1] has been reached the update velocity and acceleration with velocity[1] and acceleration[1] as factors and so on.
The problem I am facing two issues:
1: Since I am using micro_ros for communicating the above trajector to teensy for execution, I am not able to accurately check if the position has been reached to then override the speed and acceleration, due to the latencies introduced in the loop due to micro_ros. Is there a better way I could approach the execution of the trajectory?

...

void moveCallback(const void *req, void *res)
{
  custom_interfaces__srv__MoveitControl_Request *req_in  = (custom_interfaces__srv__MoveitControl_Request *)req;
  custom_interfaces__srv__MoveitControl_Response *res_in = (custom_interfaces__srv__MoveitControl_Response *)res;

  if (valuesInitialised) {
    if(notMotorActive()) {
      if(req_in->joint_positions.size > 0)
      {
        positions       = req_in->joint_positions.data;
        velocities      = req_in->joint_velocities.data;
        positionsSize   = req_in->joint_positions.size;

        moveFlag = true;
        rotateMotor.rotateAsync(motor);
        rotateMotor.overrideSpeed(0.0f);

        res_in->success             = true;
      }
      else {
        res_in->success             = false;
      }
    }
    else {
      res_in->success             = false;
    }
  }
  else {
    res_in->success             = false;
  }
}

void loop() 
{
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
  if(moveFlag)
  {
	if(moveCount == 9) {
	  moveCount = 0;
	  moveFlag = false;
	  rotateMotor.stop();
	}
	if(speedFlag) {
	  rotateMotor.overrideSpeed(speed);
	  speedFlag = false;
	}
	if(motor.getPosition() == positions[moveCount]) {
	  speedFlag = true;
	  moveCount++;
	}
  }
}

2: Since I want to control the velocity and acceleration, I figured that RotateControl was the way to move forward. But the motor overshoots at the end of the trajectory due to the deceleration. Is there a direct correlation between the speed and acceleration values and the number of steps it will overshoot before coming to a complete stop?

[luni]

1: Since I am using micro_ros for communicating the above trajector to teensy for execution, I am not able to accurately check if the position has been reached to then override the speed and acceleration, due to the latencies introduced in the loop due to micro_ros. Is there a better way I could approach the execution of the trajectory?

I'm afraid you can't do this with the current library but tweaking it a bit could help:

I don't know anything about micro_ros but it looks like timer interrupts are handled without waiting for a time slice, otherwise TeensyStep wouldn't work at all, right?
So, it would be best to add a callback mechanism to the step ISR, which would be called whenever some position is reached. In this callback you would do your speed and acceleration overrides. Fortunately there already is a callback mechanism implemented for the StepControler. Here the relevant code:

https://github.dev/luni64/TeensyStep...rolBase.h#L115

For a quick test I'd comment the 'mode == Mode::target &&' in the condition in line 132 to enable it for the rotation controller. You should also comment out line 135 since you don't want to stop the motor when it reached the target.
In setup you'd attach your callback (maybe start with just printing something in the callback to see if it works).

2: Since I want to control the velocity and acceleration, I figured that RotateControl was the way to move forward. But the motor overshoots at the end of the trajectory due to the deceleration. Is there a direct correlation between the speed and acceleration values and the number of steps it will overshoot before coming to a complete stop?

The RotateControl class doesn't have a concept of target position. It is meant to rotate one or motors synchronized with adjustable speed and acceleration. If you want to use it to position a motor you could (in principle) calculate the required deceleration and the point were you need to start decelerating. However, TeensyStep does not change motor speed at every step but only at fixed time intervals. Thus, calculating this can will be quite difficult.
If you need to position the motor precisely you should use the step controller this will pre-calculate the required motion parameters to make sure to hit the target exactly.

If you can explain what you want to achieve with your trajectories, I can probably tell you if TeensyStep is the right tool or if you would be better off using something else.

[Cryoschrome]

Thanks for the reply!
I finally got the chance to work on the changes you mentioned.

For a quick test I'd comment the 'mode == Mode::target &&' in the condition in line 132 to enable it for the rotation controller. You should also comment out line 135 since you don't want to stop the motor when it reached the target.
In setup you'd attach your callback (maybe start with just printing something in the callback to see if it works).

After making the changes, I tried it moving the motor using StepController and the following program.

#include <Arduino.h>
#include <TeensyStep.h>

#define STEP    2
#define DIR     3
#define ENABLE  4
#define ALARM   5
#define BRAKE   6

Stepper motor(2, 3);         
// RotateControl controller;    
StepControl stepController;

int positionArr[] = {800, 1200, 1600, 2000, 3000, 5000};
int arrayLength = 6;
int arrayCount = 0;

float speedFactor = 0.3;
int previous = 0;

void handleCommands()
{
  if (Serial.available() > 0)
  {
    int cmd = Serial.read();
    switch (cmd)
    {
      case '1':                              
        if(!stepController.isRunning())
        {
          Serial.println("Starting Motor");
          motor.setTargetAbs(positionArr[arrayCount]);
          motor.setMaxSpeed(speedFactor * 3500);
          stepController.moveAsync(motor);
        }
        break;
      case '2':
        stepController.stop();            
        Serial.println("Stopping motor");
        break;
      default:
        break;
    }
  }
}

void positionCallback()
{
  Serial.println("CALLBACK");
  motor.setTargetAbs(positionArr[arrayCount]);
  motor.setMaxSpeed(speedFactor * 3500);
  arrayCount++;
  speedFactor = speedFactor + 0.1;
  if(arrayCount == arrayLength)
  {
    Serial.println("STOPPING");
    stepController.stop();
  }
}

void setup() {
  motor.setMaxSpeed(3500);
  motor.setAcceleration(3500);
  motor.setStepPinPolarity(LOW);
  motor.setInverseRotation(true);

  stepController.setCallback(positionCallback);

  pinMode(ENABLE, OUTPUT);
  digitalWrite(ENABLE, HIGH);
  pinMode(BRAKE, OUTPUT);
  digitalWrite(BRAKE, LOW);
}

void loop() {
  handleCommands();
  int current = motor.getPosition();
  if(current - previous == 1 || current - previous == -1)
  {
    Serial.print("Position: ");
    Serial.println(current);
    previous = current;
  }
}

The motor stops even thought I commented the line you mentioned. I moved the motor using RotateControl and the callback function was called only when the controller.stop() was called.

If you can explain what you want to achieve with your trajectories, I can probably tell you if TeensyStep is the right tool or if you would be better off using something else.

I am trying to create a manipulator arm, so accuracy is important, and the trajectory is provided by a motion planning package for ROS. So i need to execute the trajectory to move the motor from one pose to another.

[Cryoschrome]

Any help with this?