// speed is 0 to 127
static void __saberOutput(SABERTOOTH_MOTOR* remote, boolean fwd, uint8_t speed){
UART* uart = remote->driver->uart;
switch(remote->driver->mode){
case PACKETIZED:{
uint8_t checksum=0;
checksum += _uartSendByte(uart,remote->address);
uint8_t cmd;
if(remote->motorNumber==1){
// motor 1
cmd = (fwd) ? 0 : 1;
}else{
// motor 2
cmd = (fwd) ? 4 : 5;
}
checksum += _uartSendByte(uart,cmd);
checksum += _uartSendByte(uart,speed);
_uartSendByte(uart, (checksum &0x7FU));
}
break;
case SIMPLE:{
if(remote->motorNumber==1){
// 1 is full reverse, 64 is stop, 127 is full fwd
speed = interpolate(speed, 0, 127, 64 , (fwd) ? 127 : 1);
}else{
// 128 is full reverse, 192 is stop, 255 is full fwd
speed = interpolate(speed, 0, 127, 192 , (fwd) ? 255 : 128);
}
_uartSendByte(uart, speed);
}
break;
}
}
void sabertoothInit(SABERTOOTH_DRIVER* driver){
switch(driver->mode){
case PACKETIZED:{
// Pause for 2s from power on
delay_ms(2000);
// Set baud rate
_uartInit(driver->uart,driver->baudRate);
// send bauding character
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
_uartSendByte(driver->uart,0xaa);
delay_ms(20);
}
break;
case SIMPLE:{
_uartInit(driver->uart,driver->baudRate);
}
break;
}
// set each motor to stop
for(int i=0;i<driver->numMotors;i++){
SABERTOOTH_MOTOR* motor = (SABERTOOTH_MOTOR*)pgm_read_word(&driver->motors[i]);
motor->actuator.sclass=&c_Sabertooth;
motor->driver = driver;
act_setSpeed(motor,0);
act_setConnected(motor,TRUE);
}
}
static void _read(SENSOR* sensor){
int chIn = -1;
char response[16];
uint8_t responseIndex;
int16_t values[NUM_VALUES];
uint8_t fieldCount;
RAZOR* razor = (RAZOR*)sensor;
uint16_t timeout;
// Send a '1' and await a response
_uartSendByte(razor->uart, '1');
start:
responseIndex = fieldCount = timeout = 0;
while( chIn != ';'){
chIn = __uartGetByte(razor->uart);
if(chIn == -1){
// No character available
if(--timeout == 0){
// We have timed out - so keep values as before
return;
}
}else{
// Get the character
char c = chIn & 0xff;
// Reset the timeout detector
timeout = 0;
// Ignore CR and LFs
if(c == '\r' || c == '\n'){
goto start;
}
// Append character to the response
if(responseIndex < sizeof(response)-1){
response[responseIndex++] = c;
}
// Check for end of field
if(c ==',' || c==';'){
response[responseIndex] = 0; // Terminate the string
int16_t value = atoi(response); // Convert number to an integer
if(fieldCount < NUM_VALUES){
values[fieldCount] = value; // And store it if we have room
}
fieldCount ++;
responseIndex = 0; // Reset for next field
}
}
}
if(fieldCount == NUM_VALUES ){
// We have a complete message with 9 values
razor->imu.x_axis_degrees_per_second = values[0];
razor->imu.y_axis_degrees_per_second = values[1];
razor->imu.z_axis_degrees_per_second = values[2];
razor->imu.x_axis_mG = values[3];
razor->imu.y_axis_mG = values[4];
razor->imu.z_axis_mG = values[5];
razor->imu.bearingDegrees = values[6];
razor->imu.pitchDegrees = values[7];
razor->imu.rollDegrees = values[8];
}
}
