Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 4 of 4

Thread: TeensyStep Help

  1. #1
    Junior Member
    Join Date
    Apr 2022
    Posts
    7

    TeensyStep Help

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

Name:	posititon_values.png 
Views:	4 
Size:	466.5 KB 
ID:	28700 Click image for larger version. 

Name:	velocity_values.png 
Views:	5 
Size:	461.7 KB 
ID:	28703 Click image for larger version. 

Name:	accel_values.png 
Views:	4 
Size:	510.6 KB 
ID:	28702
    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?

    Code:
    ...
    
    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?

  2. #2
    Senior Member
    Join Date
    Apr 2014
    Location
    Germany
    Posts
    1,849
    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.

  3. #3
    Junior Member
    Join Date
    Apr 2022
    Posts
    7
    Thanks for the reply!
    I finally got the chance to work on the changes you mentioned.
    Quote Originally Posted by luni View Post
    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.
    Code:
    #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.

    Quote Originally Posted by luni View Post
    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.

  4. #4
    Junior Member
    Join Date
    Apr 2022
    Posts
    7
    Any help with this?

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •