Motion sequence

This example describes how to create a motion sequence divided into 5 steps. In step 0, the servo is at standstill and the motion sequence is ready to start. When the sequence is started, steps 1 to 4 are executed one after the other and different motions are performed during these steps. When step 4 finishes, the sequence is reinitialized to step 0.

 

1) Sketch: 5_MotionControl \ 2_ServoMotionSequence

We start by creating a variable called step whose value represents the number of the step being executed during the motion sequence. Step will take the values 0 to 4.

During the setup(), we initialize the Servo (ID 0, pin 8, inverted direction, position offset of -4°, initial position of 20°).

#include <HITIComm.h>
#include <HC_Servo.h>


// pins assignment
const int pin_Servo   = 8;

// Analog Data assignment
const int ad_Step     = 0; // sequence step
const int ad_Position = 1; // servo position

// Digital Data assignment
const int dd_Start    = 0; // virtual switches
const int dd_Stop     = 1;

// motion sequence in 5 steps: 0 (Ready), 1-4 (Motions)
int step = 0;

// HITI Servo
HC_Servo servo;


// run once at power on
void setup()
{
  // initialize library
  HC_begin();

  // initialize HITI Servo
  servo.init(
      0,         // ID = 0
      pin_Servo, // attached to pin 8
      true,      // true => invert direction
      -4,        // position offset = -4°
      20.0);     // initial position = 20°
}

We then prepare 2 functions that will be used several times in our program.

The first function is doStep(position, time). This function is called by steps 1 to 4. It moves the servo to a requested position in the requested time.

The second function is onMotionDone(). This function is called by doStep() when a motion ends. Its role is to update the steps to create the motion sequence.

// called by steps 1 to 4
void doStep(float position, float time)
{
    // move to requested position in requested time
    servo.absolutePosition(position);
    servo.motionTime(time);
    servo.moveNow();
    
    // when motion ends
    if (servo.isEnding())
    {
        onMotionDone(); // update step
    }
}


// called each time a motion ends
void onMotionDone()
{
    // at end of steps 0-3
    if (step < 4)
    {
        step++;   // go to next step
    }
    // at end of step 4
    else
    {
        step = 0; // go to step 0
    }
}

Inside the loop(), we start the motion sequence if the “START” virtual switch is activated and if the sequence is ready to start (step 0).

Immediately after, we check if the “STOP” virtual switch is activated to know if the sequence must be stopped. The sequence can be stopped at any steps and at any moment. The STOP action has priority over the processing of steps 1 to 4, this is why it is checked before executing these steps.

Afterwards, the sequence steps are executed one after the other. At each step, the servo moves to a predefined position in a predefined time. At last step (step 4), the servo is moved back to its initial position of 20°.

Then, we systematically deactivate the virtual switches as we don’t want them to stay activated.

Finally, we send the step value and the Servo position to HITIPanel.

// run repeatedly after setup()
void loop()
{
    // communicate with HITIPanel (to place at the beginning)
    HC_communicate();
    
    
    // start/stop motion sequence --------------------------------------------
    
    // step 0: ready to start sequence with the START virtual switch
    if ((step == 0) && HC_digitalDataRead(dd_Start))
    {
        step = 1; // start sequence
    }
    
    // at any step: stop sequence with the STOP virtual switch
    if(HC_digitalDataRead(dd_Stop))
    {	
        servo.stopNow();  // stop servo
        step = 0;         // reset sequence
    }
    
    // step 1:
    else if(step == 1)
    {
        doStep(165, 0.5); // move to position 165° in 0.5s
    }
    // step 2
    else if (step == 2)
    {
        doStep(30, 5);    // move to position 30° in 5s
    }
    // step 3
    else if (step == 3)
    {
        doStep(120, 2.5); // move to position 120° in 2.5s
    }
    // step 4
    else if (step == 4)
    {
        doStep(20,  1.5); // move back to position 20° in 1.5s
    }
    
    // deactivate the Virtual Switches
    HC_digitalDataWrite(dd_Start, false);
    HC_digitalDataWrite(dd_Stop, false);
    
    
    // send data to HITIPanel ------------------------------------------------
    
    HC_analogDataWrite(ad_Step,     step);
    HC_analogDataWrite(ad_Position, servo.getCurrentPosition());
}

 

2) Control Panels

  1. Display the HITI DATA Control Panels.
  2. Click on “START” to start the motion sequence. You can follow the motion sequence by looking at the step being executed.
    image336.png

    image337.png

 

3) Chart

  1. Open the Chart window.
  2. Start data acquisition. The servo position is then plotted. New data are added every 10ms and only the last 12s are displayed.
  3. In the HITI DATA Control Panels, click on “START” to start a new motion sequence.
  4. When the motion is finished, stop data acquisition. You then get this position profile:
    image340.png

  5. Let’s make some measurement to check the motion times of the different steps. Zoom in and out if required. In our measurement, we got the following for step 2:
    • start time = 3.390s
    • stop time = 8.400s
    • motion time = 8.400 – 3.390s = 5.010s
      image342.png

      image344.png

 

Was this article helpful?
0 out of 0 found this helpful