void setDutyCycle(char DC, uchar motor)
{
if(motor==Left)
SetLeftMotor(DC);
else
SetRightMotor(DC);
}
void handleCommMessage(void){
char buffer[COMM_BUFFER_SIZE];
unsigned char function_code = 0;
long i;
buffer[64]=0;
buffer[79]=0;
UARTgets((uint8*)buffer, COMM_BUFFER_SIZE);
//UARTgetMessage(buffer);
//UARTprintf("MESSAGE GET!!%s\r\n", buffer);
if(isValidMessage(buffer))
{
for(i = 0; i < CODE_LENGTH; i++) //generate hash of function code
{
function_code ^= buffer[CODE_START + i];
}
switch(function_code)
{
case SPLM: //JoystickXOut(SATURATE(atoi(&buffer[DATA_START]),0,255));
//UARTprintf("GOOD MESSAGE- SVXA: %s\r\n", buffer);
SetLeftMotor(SATURATE(atoi(&buffer[DATA_START]),-128,127));
VelCtrlRunning = 0;
ResetWatchdog(); //we got a valid message, so they are still talking to us
break;
case SPRM: //JoystickYOut(SATURATE(atoi(&buffer[DATA_START]),0,255));
//UARTprintf("GOOD MESSAGE- SVYA: %s\r\n", buffer);
SetRightMotor(SATURATE(atoi(&buffer[DATA_START]),-128,127));
VelCtrlRunning = 0;
ResetWatchdog(); //we got a valid message, so they are still talking to us
break;
case SAHS: SetHokuyoServo(SATURATE(atoi(&buffer[DATA_START]),-128,127));
break;
case SVLX: UpdateLinearX(atoi(&buffer[DATA_START]));
ResetWatchdog();
VelCtrlRunning = 1;
break;
case SVAZ: UpdateAngularZ(atoi(&buffer[DATA_START]));
VelCtrlRunning = 1;
ResetWatchdog();
break;
case ETFM: EnableSensorFeedbackMessages = 1;
break;
case DTFM: EnableSensorFeedbackMessages = 0;
break;
case RSTE: ResetEncoders();
break;
case STMR: if(1000 % atoi(&buffer[DATA_START]) == 0)
SetMessageRate(atoi(&buffer[DATA_START]));
else
UARTprintf("INVALID MESSAGE RATE (1000%rate must = 0): %s", buffer);
break;
case SETT: SetTime(atoi(&buffer[DATA_START]));
break;
case EHST: EnableHokuyoTilting();
break;
case DHST: DisableHokuyoTilting();
break;
case SHTR: if(1000 % atoi(&buffer[DATA_START]) == 0)
SetHokuyoTiltRate(atoi(&buffer[DATA_START]));
else
UARTprintf("INVALID RATE (1000%rate must = 0): %s", buffer);
break;
case SACD: SetAccelDivisor(atoi(&buffer[DATA_START]));
break;
/*case FLPC: Servo_0_WriteCompare1(atoi(&buffer[DATA_START]));
break;
case FRPC: Servo_0_WriteCompare2(atoi(&buffer[DATA_START]));
break;*/
default: UARTprintf("UNRECOGNIZED MESSAGE: %s\r\n", buffer);
break;
}
}
else
{
UARTprintf("INVALID START CHARACTER: %s\r\n", buffer);
}
}
void WatchdogTimeout(void){
SetLeftMotor(0); //stop the bot
SetRightMotor(0);
VelCtrlRunning = 0;
Err_LED_1_Write(1);
}
