char DoCommand()
{
switch(rxbuffer[0])
{
case CMD_EMPTY:
return 1;
case CMD_READ_BATTERY_VOLTAGE:
SendWord(BatteryValue);
return 1;
case CMD_SET_ACCELERATION_VAR:
Acceleration = rxbuffer[4];
return 1;
case CMD_SET_PWM_ENABLED:
TMR2ON = rxbuffer[4] > 0;
return 1;
case CMD_SET_MOTORS_ENABLED:
Enable1pin = rxbuffer[4] > 0;
Enable2pin = rxbuffer[4] > 0;
return 1;
case CMD_SET_PWMSPEED_A_VAR:
PWMSpeedA.hi = rxbuffer[3];
PWMSpeedA.lo = rxbuffer[4];
return 1;
case CMD_SET_PWMSPEED_B_VAR:
PWMSpeedB.hi = rxbuffer[3];
PWMSpeedB.lo = rxbuffer[4];
return 1;
case CMD_GET_TOTALTICKS_A:
SendLong(AbsolutTickCountA);
return 1;
case CMD_GET_TOTALTICKS_B:
SendLong(AbsolutTickCountB);
return 1;
case CMD_GET_TICKSPEED_A:
SendWord(CurrentTickSpeedA);
return 1;
case CMD_GET_TICKSPEED_B:
SendWord(CurrentTickSpeedB);
return 1;
case CMD_GET_CURRENT_A:
SendWord(SenseValueA);
return 1;
case CMD_GET_CURRENT_B:
SendWord(SenseValueB);
return 1;
case CMD_SET_SERVOA_POS:
ServoAPos = rxbuffer[4];
return 1;
case CMD_SET_SERVOB_POS:
ServoBPos = rxbuffer[4];
return 1;
case CMD_SET_SERVOC_POS:
ServoCPos = rxbuffer[4];
return 1;
default:
return 0;
}
}
uint16_t SPI::TransmitWord (uint16_t word)
{
WaitTXE();
SendWord (word);
WaitRXNE();
return ReceiveWord();
}
void Network_Message::Start(int messageID) {
//if (Network.msgSendLock) {
//logError("Starting new networking message without finishing old one!");
//}
Network.msgSendLock = Thread.EnterLock(MUTEX_NETWORK_SENDBUF);
rawSize = 0;
if (messageID != BAD_ID) {
SendWord(0);
SendByte(messageID & 0xFF);
}
}
