void Sabertooth::SendDriveCmd(char command, char value){
////////////////////////////////Testing.....
//while(!USART_IsTXDataRegisterEmpty(Sabertooth_USART)); //Necessary to make sure we don't overwrite data in the buffer
//USART_PutChar(Sabertooth_USART, AUTOBAUD_BYTE); //Send the autobaud byte to get the sabertooth communicating
////////////////////////////////
while(!USART_IsTXDataRegisterEmpty(Sabertooth_USART)); //Necessary to make sure we don't overwrite data in the buffer
USART_PutChar(Sabertooth_USART, SABERTOOTHADDRESS); //Sends the address to the sabertooth
while(!USART_IsTXDataRegisterEmpty(Sabertooth_USART));
USART_PutChar(Sabertooth_USART, command); //Sends the command to the sabertooth
while(!USART_IsTXDataRegisterEmpty(Sabertooth_USART));
USART_PutChar(Sabertooth_USART, value); //Sends the value or speed to the sabertooth
while(!USART_IsTXDataRegisterEmpty(Sabertooth_USART));
USART_PutChar(Sabertooth_USART, SaberChecksum(command, value)); //Send the checksum of all these values to the sabertooth
}
void SendDriveCommand_SaberThree(unsigned char command, unsigned char value){
//&USARTF0 is the USART for saber three
while(!USART_IsTXDataRegisterEmpty(&USARTF0)); //Necessary to make sure we don't overwrite data in the buffer
USART_PutChar(&USARTF0, SABERTOOTHADDRESS);
while(!USART_IsTXDataRegisterEmpty(&USARTF0));
USART_PutChar(&USARTF0, command); //Sends the command
while(!USART_IsTXDataRegisterEmpty(&USARTF0));
USART_PutChar(&USARTF0, value); //Sends the value or speed to the sabertooth
while(!USART_IsTXDataRegisterEmpty(&USARTF0));
USART_PutChar(&USARTF0, SaberChecksum(command, value)); //Send the checksum of all these values to the Sabertooth
}