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
}
void SendStringSABER_DOS(char *present){
for(int i = 0 ; present[i] != '\0' ; i++){
while(!USART_IsTXDataRegisterEmpty(&USARTE1));
USART_PutChar(&USARTE1, present[i]);
_delay_us(500); //DEGBUGGING
}
}
/*
Turn on the UART that is connected to the UC3 so that a user can connect
to a PC if that is desired.
*/
void sendUARTC0(uint8_t* dat)
{
// Wait until it is possible to put data into TX data register.
// NOTE: If TXDataRegister never becomes empty this will be a DEADLOCK.
while(!USART_IsTXDataRegisterEmpty(&UARTC0));
USART_PutChar(&UARTC0, dat);
}
void USART_Print_Text(USART_t * usart, char *text)
{
uint8_t idx;
for (idx = 0; idx<strlen(text); idx++) {
while( !USART_IsTXDataRegisterEmpty(usart) ) {}
USART_PutChar(usart,text[idx]);
}
}
void put_USART_char(uint8_t sendThis)
{
/* Send one char. */
do {
/* Wait until it is possible to put data into TX data register.
* NOTE: If TXDataRegister never becomes empty this will be a DEADLOCK. */
} while(!USART_IsTXDataRegisterEmpty(&IMU_USART));
USART_PutChar(&IMU_USART, sendThis);
}
void USART_Print_Buf(USART_t * usart, uint8_t *pbuf, uint8_t buflen)
{
uint8_t idx;
for (idx = 0; idx<buflen; idx++) {
while( !USART_IsTXDataRegisterEmpty(usart) ) {}
USART_PutChar(usart,pbuf[idx]);
}
}
/** Main program entry point. This routine contains the overall program flow, including initial
* setup of all components and the main program loop.
*/
int main(void)
{
SetupHardware();
GlobalInterruptEnable();
uint8_t sending = 0;
for (;;) {
while (1) {
int16_t ReceivedByte = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
if (ReceivedByte < 0)
break;
if (!configured) continue;
if (!sending) {
PORTC.OUTSET = PIN1_bm;
sending = 1;
}
PORTD.OUTTGL = PIN5_bm;
while(!USART_IsTXDataRegisterEmpty(&USART));
USART_PutChar(&USART, ReceivedByte & 0xff);
}
if (sending) {
USART_ClearTXComplete(&USART);
while(!USART_IsTXComplete(&USART));
PORTC.OUTCLR = PIN1_bm;
sending = 0;
}
Endpoint_SelectEndpoint(VirtualSerial_CDC_Interface.Config.DataINEndpoint.Address);
/* Check if a packet is already enqueued to the host - if so, we shouldn't try to send more data
* until it completes as there is a chance nothing is listening and a lengthy timeout could occur */
if (configured && Endpoint_IsINReady()) {
uint8_t maxbytes = CDC_TXRX_EPSIZE;
while (USART_RXBufferData_Available(&USART_data) && maxbytes-->0) {
uint8_t b = USART_RXBuffer_GetByte(&USART_data);
CDC_Device_SendByte(&VirtualSerial_CDC_Interface, b);
PORTD.OUTTGL = PIN5_bm;
}
}
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
if (loop++) continue;
if (!configured) continue;
PORTD.OUTTGL = PIN5_bm;
}
}
void USART_Print_uint32(USART_t * usart, uint32_t data)
{
uint32_t mask = 0xFF000000;
uint8_t idx;
for (idx = 0; idx<4; idx++) {
while( !USART_IsTXDataRegisterEmpty(usart) ) {}
USART_PutChar(usart,(uint8_t)((data & (mask>>(idx*8)))>>(3-idx)*8));
}
}
// We don't use the USART driver's built-in ring buffer because the function gets called
// as an interrupt, so if the buffer is full and we block here, no output will be printed.
static int naiboard_uart_putchar(char c, FILE *stream) {
if (c == '\n')
naiboard_uart_putchar('\r', stream);
while (!USART_IsTXDataRegisterEmpty(&USARTMODULE))
;
USART_PutChar(&USARTMODULE, c);
return 0;
}
void sendUARTF0(uint8_t *array, uint8_t length)
{
uint8_t i;
for(i=0; i<length; i++)
{
// Wait until it is possible to put data into TX data register.
// NOTE: If TXDataRegister never becomes empty this will be a DEADLOCK.
while(!USART_IsTXDataRegisterEmpty(&UARTF0));
USART_PutChar(&UARTF0, array[i]);
}
}
void testUartTx(void)
{
uint8_t sendData;
// Send data from 122 down to 32 - Readable in a console.
sendData = 122;
while(sendData > 32)
{
// Send one char.
do {
/*
Wait until it is possible to put data into TX data register.
NOTE: If TXDataRegister never becomes empty this will be a DEADLOCK.
*/
} while(!USART_IsTXDataRegisterEmpty(&UARTC0));
USART_PutChar(&UARTC0, sendData);
sendData--;
}
}
void uartE0SendTXbit(unsigned char data){
do{}while(!USART_IsTXDataRegisterEmpty(&USARTE0));
USART_PutChar(&USARTE0, data);
}
void usart_putc1(unsigned char letra)
{
while(!USART_IsTXDataRegisterEmpty(&USARTC1));
USART_PutChar(&USARTC1, letra);
}
int main(void)
{
SetXMEGA32MhzCalibrated(); //Set XMega to user 32Mhz internal oscillator with 32Khz crystal calibration
///////Setup Inputs and Outputs///////
PORTC.DIRSET = (PIN5_bm | PIN6_bm | PIN7_bm | PIN3_bm); //Sets outputs on port C
PORTC.DIRCLR = PIN2_bm; //Sets inputs on PORT C
PORTA.DIRCLR = XBEEDIO0;
PORTE.DIRSET = PIN3_bm; //Sets inputs on PORTA
///////Initialize Serial Communcations///////
SetupPCComms(); //Initializes PC Communications at 9600 baud0
_delay_ms(500); //Delay to make sabertooth initialize
Sabertooth DriveSaber(&USARTD0, &PORTD); //Initializes Sabertooth Communications at 9600 Baud
//////////////////Timers///////////////
TCC0.CTRLA = TC_CLKSEL_DIV1024_gc; //31250 counts per second with 32Mhz Processor
TCC0.CTRLB = TC_WGMODE_NORMAL_gc;
TCC0.PER = 15625;
TCC0.INTCTRLA = TC_OVFINTLVL_LO_gc;
TCD0.CTRLA = TC_CLKSEL_DIV1024_gc; //31250 counts per second with 32Mhz Processor
TCD0.CTRLB = TC_WGMODE_NORMAL_gc;
TCD0.PER = 31250;
TCD0.INTCTRLA = TC_OVFINTLVL_LO_gc;
///////////////////Timers//////////////
sei(); //Enables global interrupts so the interrupt serial can work
////Semi-global vars//////
unsigned char BufferIdx = 0;
const char XMegaID[] = "ID: MainDrive\r\n";
enum MicroState{
WaitForHost,
Driving
}XMegaState = WaitForHost;
while(1){
switch(XMegaState){
case WaitForHost:
for(int i = 0 ; XMegaID[i] != '\0'; i++){
while(!USART_IsTXDataRegisterEmpty(&USARTC0));
USART_PutChar(&USARTC0, XMegaID[i]);
}
_delay_ms(500);
if(USART_RXBufferData_Available(&USART_PC_Data)){
if(USART_RXBuffer_GetByte(&USART_PC_Data) == 'r'){
XMegaState = Driving;
//DriveSaber.ResetSaber();
FlushSerialBuffer(&USART_PC_Data);
USART_PutChar(&USARTC0, 'r');
}
}
TimePrevious = TimeSinceInit;
break;
case Driving:
if(USART_RXBufferData_Available(&USART_PC_Data)){
receiveArray[BufferIdx] = USART_RXBuffer_GetByte(&USART_PC_Data);
BufferIdx++;
}
if(BufferIdx == RECEIVE_PACKET_SIZE){
FlushSerialBuffer(&USART_PC_Data);
if(IsRoving){
if(receiveArray[4] == PCComsChecksum(receiveArray[1], receiveArray[2], receiveArray[3])){
DriveSaber.ParsePacket(receiveArray[2], receiveArray[3]);
}
}else if(!IsRoving){
DriveSaber.ParsePacket(127, 127);
}
BufferIdx = 0;
SendDriveControlStatus(&USARTC0, IsRoving, false);
TimePrevious = TimeSinceInit;
}
if((TimeSinceInit - TimePrevious) > TIMEOUTMAX){
DriveSaber.StopAll();
XMegaState = WaitForHost;
TimePrevious = TimeSinceInit;
}
break;
};
/*
if(!IsRoving){
DriveSaber.StopAll();
}
*/
if((PORTA.IN & XBEEDIO0)){
ERROR_CLR();
IsRoving = true;
}else if((!(PORTA.IN & XBEEDIO0))){
ERROR_SET();
IsRoving = false;
}
}
}
//Gimbal send string function, its all by itself with its init so far
void SendStringGim(char *present){
for(int i = 0 ; present[i] != '\0' ; i++){
while(!USART_IsTXDataRegisterEmpty(&USARTD0));
USART_PutChar(&USARTD0, present[i]);
}
} //End gimbal send string functions, USARTD0
void SendStringSABER_TRES(char *present){
for(int i = 0 ; present[i] != '\0' ; i++){
while(!USART_IsTXDataRegisterEmpty(&USARTF0));
USART_PutChar(&USARTF0, present[i]);
}
}//Drive saber send functions end