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
Name | size | signed? | Description |
---|---|---|---|
pos | 8-bit | no | Desired position |
force | 8-bit | no | Desired force |
volt | 8-bit | yes | Feed-forward voltage command |
gain.forceGain | 4-bit | no | Force gain index |
gain.posGain | 3-bit | no | Position gain index |
gain.motorOn | 1-bit | no | 1 to drive motor, 0 to coast |
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
.
Name | size | signed? | Description |
---|---|---|---|
pos | 8-bit | no | Sensed position |
force | 8-bit | no | Sensed force |
hash_code | 8-bit | no | Boolean indicating proper reception |
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
Name | size | signed? | Description |
---|---|---|---|
rawX | 8-bit | no | 8-bit value from X-axis of accelerometer |
rawY | 8-bit | no | 8-bit value from Y-axis of accelerometer |
rawZ | 8-bit | no | 8-bit value from Z-axis of accelerometer |
hash_code | 8-bit | no | Boolean indicating proper reception |
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
Warning: this code is technically non-portable, but it works correctly on these microcontrollers. Other processors might mangle the translation due to alignment issues.
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.
// 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