HITI Motor Group (Arrays of Setpoints)

Modified on Thu, 05 Jan 2023 at 02:23 PM

This example shows how to create a motion sequence of several synchronized HITI Servos placed inside a HITI Motor Group using an array of setpoints (positions, motion times) for each servo.

The resulting motion sequence is identical to the one we got using the Group Rearrangement method. The main advantages of using Arrays of Setpoints compared to using Group Rearrangement are the following:

All positions and motion times of all the servos are defined and known at all time.
The resulting code is much shorter.

Sketch

Upload this sketch : 5_MotionControl \ 6_GroupAndSetpointsArrays

We start our program by creating 4 HITI Servo variables and 1 HITI Motor Group variable. As usual, we create a variable called step to manage the motion sequence. Step will take the values 0 to 5.

Then, we define the Arrays of Setpoints containing the setpoints for steps 1 to 5. There is an array of Position for each servo, and an array of Motion Times for the Motor Group.

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

// pins assignment 
const int pin_Servo_1 = 8;  // servos
const int pin_Servo_2 = 9;
const int pin_Servo_3 = 10;
const int pin_Servo_4 = 11;

// Analog Data assignment
const int ad_Step        = 0; // sequence step
const int ad_Position_S1 = 1; // current positions (Servo 1-4)
const int ad_Position_S2 = 2;
const int ad_Position_S3 = 3;
const int ad_Position_S4 = 4;

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

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

// Initial position
const int initPos_S1 = 70;
const int initPos_S2 = 80;
const int initPos_S3 = 90;
const int initPos_S4 = 100;

// Positions setpoints              Step 1  Step 2  Step 3  Step 4  Step 5
const float positionArray_S1[5] = { 10,     40,     40,     20,     initPos_S1 };
const float positionArray_S2[5] = { 60,     50,     100,    70,     initPos_S2 };
const float positionArray_S3[5] = { 110,    110,    60,     140,    initPos_S3 };
const float positionArray_S4[5] = { 160,    160,    120,    160,    initPos_S4 };

// Motion Times setpoints
const float motionTimeArray[5]  = { 1,      2.5,    3.2,    0.8,    4 };

// HITI Motor Group
HC_MotorGroup group;

// HITI Servo
HC_Servo servo_1;
HC_Servo servo_2;
HC_Servo servo_3;
HC_Servo servo_4;

During the setup(), we initialize the 4 Servos individually and we give them different servo IDs (1, 2, 3, 4). These IDs will be used by the Group to address them.

Next, we add all the Servos to the Group.

void setup()
{
    // 1) initialize library
    HC_begin();

    // 2) initialize HITI Servos
    // param: custom id, pin, invert direction, position offset, absolute init position
    servo_1.init(1, pin_Servo_1, true,  3,  initPos_S1);
    servo_2.init(2, pin_Servo_2, false, -1, initPos_S2);
    servo_3.init(3, pin_Servo_3, false, 0,  initPos_S3);
    servo_4.init(4, pin_Servo_4, true,  5,  initPos_S4);

    // 3) initialize group and assign it all servos
    group.init(4);
    group.add(&servo_1);
    group.add(&servo_2);
    group.add(&servo_3);
    group.add(&servo_4);
}

We also create the function onMotionDone() to manage the motion sequence.

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

    // at end of step 5
    else
        step = 0; // go to step 0
}

Inside the loop(), the motion sequence is started by clicking on the “START” button and can be stopped by clicking on the “STOP” button.

The sequence steps are executed one after the other. At each step, the Servos perform a synchronized motion according to their position setpoints and to the motion time of the Group. The setpoints are fetched from the Arrays.

Finally, we display the sequence step and the current positions of all servos in HITIPanel.

void loop()
{
    // communicate with HITIPanel
    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))
    {
        group.stopNow(); // stop group
        step = 0;        // reset sequence
    }

    // step 1 to 5
    else if ((1 <= step) && (step <= 5))
    {
        // move servos of the group 
        group.absolutePosition(1, positionArray_S1[step - 1]);
        group.absolutePosition(2, positionArray_S2[step - 1]);
        group.absolutePosition(3, positionArray_S3[step - 1]);
        group.absolutePosition(4, positionArray_S4[step - 1]);
        group.motionTime(motionTimeArray[step - 1]);
        group.moveNow();

        // when motion ends
        if (group.isEnding())
            onMotionDone();
    }

    // deactivate the Virtual Buttons
    HC_digitalDataWrite(dd_Start, false);
    HC_digitalDataWrite(dd_Stop, false);


    // display data in HITIPanel ---------------------------------------------

    // sequence step
    HC_analogDataWrite(ad_Step, step);

    // current positions (servos)
    HC_analogDataWrite(ad_Position_S1, servo_1.getCurrentPosition());
    HC_analogDataWrite(ad_Position_S2, servo_2.getCurrentPosition());
    HC_analogDataWrite(ad_Position_S3, servo_3.getCurrentPosition());
    HC_analogDataWrite(ad_Position_S4, servo_4.getCurrentPosition());
}

Control Panels

1) Display the DATA Control Panels.

2) The Servos are at their initial positions. Click on “START” to start the motion sequence.

3) You can follow the motion sequence by looking at the step being executed.

Chart

1) Open the Chart window.

2) Start data acquisition. The servos positions are then plotted, and new data are added every 10ms and only the last 12s are displayed.

3) In the DATA Control Panels, click on “START” to start a new motion sequence.

4) When the motion is finished, stop data acquisition, and check the position profiles.