OnTick function

Each module must supply an OnTick function which is called every millisecond to generate motor commands and delivers sensor reports. All of necessary data types are defined in SB3/Embedded/Shared/datastruct.h which is generated by SB3/Embedded/Scripts/generate_datastruct.py which has a configuration file SB3/Embedded/Scripts/datastruct.txt

The OnTick function must have the prototype: void XXX_OnTick(CmdPacketStruct * Cmd, RptPacketStruct * Rpt); and the existing values in Cmd are undefined.

CmdPacketStruct

At the top level, the CmdPacketStruct has three members: header, a reserved single byte currently used for data logging debugging, servos an array of servo command packets, hash_code which is used for ensuring transmission integrity. The OnTick function does not need to worry about the hash_code value, it will be computed automatically by the communication code.

servos

This member is an array of structs with type ServoCmdStruct and a length of NUM_SERVO_NODES both which are defined in datastruct.h. The numbering scheme for the servos can be found at SB3ServoConfigurations. Therefore, if you want to access the command structure for the front-right stroke motor, you would type Cmd->servos[5]

The servo struct has four members

Note the position and force commands are unsigned. This is because any semblance of zero-position is encoded on the main board side and not the servo board size. The main board must remember what the zero values are and then transmit them to the board (this is subject to change since we have have issues with the comm having 8-bits while the internal servo boards have 10-bit)

If we want to send the front right stroke motor to position 140, we would execute the following code:

Cmd->servos[5].pos = 140;

Also, note for the gains, the command sent is an index, not an actual. The boards have a look-up table that allows you to select from any gains (of course, we can reprogram the gains to be whatever we want). The actual gains used are available at SB3ServoConfigurations.

RptPacketStruct

The report structure has two top level members: servos and accel

servos

servos is another array of structs similar to their counterpart in Cmd but they are of type ServoRptStruct.

The hash_code indicates if the main board is correctly hearing the transmission from the servo. It should be noted that during the actual communication, this field contains the error detection hash, but once received by the main board, it is overwritten with a logical true/false indicating if an error was detected or not. If an error was detected, then the values in the pos and force are undefined and garbage.

accel

This shows the values pulled directly from the accelerometer channels without any scaling or offsets (there is a small amount of filtering). To access the x-axis, one would write: Rpt->accel.rawX

Alternative data structures

Warning: this code is technically non-portable, but it works correctly on these microcontrollers. Other processors might mangle the translation due to alignment issues.

Example Code

This example shows how to command a smooth line trajectory. The module has two functions, the OnTick function and the start function. The code implements a state machine with three states: WAITING, MOVING and HOLDING. Essentially, the robot waits with all motors off until start is called and then it smoothly moves its front right wing motor 20 servo ticks over 1/4 of a second. It should be noted that this code is incomplete because it does not check the hash_code from the report values to see if the incoming value is correct.

example code

// State machine states
#define WAITING   0
#define MOVING    1
#define HOLDING   2

//Distance and time duration for movement state
#define DIST      20
#define DUR_TIME  250

//State machine's current state
static unsigned char state = WAITING;

//Boolean that triggers a snapshot taken of the current position
static unsigned char take_snapshot;

//The starting position of the motor
static unsigned char start_pos;

//The current time during the movement state
static unsigned int stopwatch;


void start(void)
{
    //Signal start of move
    state = MOVING;

    //Reset clock
    stopwatch = 0;

    //We need the report structure to find out our current position
    //So we must trigger a snapshot
    take_snapshot = 1;
}

void OnTick(CmdPacketStruct * Cmd, RptPacketStruct * Rpt)
{
    static unsigned char i;

    //First, turn off all motors
    for (i = 0; i < NUM_SERVO_NODES; i++)
        Cmd->servos[i].gain.motorOn = 0;

    //See if we need to take a snapshot
    if (take_snapshot)
    {
        //Clear flag
        take_snapshot = 0;

        //Take the snapshot of the position
        start_pos = RS_Rpt2Robot(Rpt)->RF.wing.pos
    }

    //Implement state machine
    switch (state)
    {
        case WAITING:
        break;
        case MOVING:
             //Calculate linear motion from start to end
             //(Be careful to multiply first, divide second
             // to avoid integer problems and make sure there
             // is enough room in the data type)
             RS_Cmd2Robot(Cmd)->RF.wing.pos = start_pos +
                 (DIST * stopwatch / DUR_TIME);
             //Increment clock
             stopwatch++;

             //Determine if have reached the end
             if (stopwatch >= DUR_TIME)
                 state = HOLDING;

             //Turn joint on
             RS_Cmd2Robot(Cmd)->RF.wing.gain.motorOn = 1;
        break;
        case HOLDING:
             //Hold motor at end position
             RS_Cmd2Robot(Cmd)->RF.wing.pos = start_pos + DIST;
             RS_Cmd2Robot(Cmd)->RF.wing.gain.motorOn = 1;
        break;
    }
}

-- SalomonTrujillo - 24 Jun 2010