/* -----------------------------------------------------------------------------------------------------------*/
void UART_F1_SendByte( char Byte )
{
char temp;
temp = SREG;
cli();
if ( Get_Bytes_in_FIFO ( TX_F1_fifo ) == 0 )
{
// Wenn Controller noch mit senden eines Byte beschäftig, ab in den Puffer
if ( TX_F1_state == TX_sending )
{
// Byte in Buffer schreiben
Put_Byte_in_FIFO ( TX_F1_fifo, Byte );
// Buffer Empty Interrupt freigeben, TX_complete sperren
USART_TxdInterruptLevel_Set( &USARTF1 , USART_TXCINTLVL_OFF_gc );
USART_DreInterruptLevel_Set( &USARTF1 , USART_DREINTLVL_HI_gc );
}
else
{
// Buffer Empty sperren, TX_complete freigeben
USART_PutChar( &USARTF1, Byte );
// Buffer Emty sperren, TX_complete freigeben
USART_TxdInterruptLevel_Set( &USARTF1 , USART_TXCINTLVL_HI_gc );
USART_DreInterruptLevel_Set( &USARTF1 , USART_DREINTLVL_OFF_gc );
TX_F1_state = TX_sending;
}
}
else
{
// Wenn Puffer voll, warten bis wieder was rein paßt
while ( 1 )
{
cli();
if ( Get_Bytes_in_FIFO ( TX_F1_fifo ) < TX_Bufferlen ) break;
SREG = temp;
}
// Byte in Buffer schreiben
Put_Byte_in_FIFO ( TX_F1_fifo, Byte );
}
SREG = temp;
}
//Two has been verified to work
void Saber_init_dos(){ //USARTE1
PORTE.DIRSET = PIN7_bm; //Sets TX Pin as output
PORTE.DIRCLR = PIN6_bm; //Sets RX pin as input
USART_InterruptDriver_Initialize(&SABER_DOS, &USARTE1, USART_DREINTLVL_LO_gc); //Initialize USARTE1 as interrupt driven serial and clear it's buffers
USART_Format_Set(SABER_DOS.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false); //Set the data format of 8 bits, no parity, 1 stop bit
USART_RxdInterruptLevel_Set(SABER_DOS.usart, USART_RXCINTLVL_LO_gc); //Enable the receive interrupt
USART_Baudrate_Set(&USARTE1, 207 , 0); //Set baudrate to 9600 with 32Mhz system clock
USART_Rx_Enable(SABER_DOS.usart); //Enable receiving over serial
USART_Tx_Enable(SABER_DOS.usart);
//Sabertooth autobaud byte
_delay_ms(100); //Delay -- allowing things to settle
USART_PutChar(&USARTE1, AUTOBAUD_BYTE);
}
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);
}
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
void usart_putc1(unsigned char letra)
{
while(!USART_IsTXDataRegisterEmpty(&USARTC1));
USART_PutChar(&USARTC1, letra);
}
void USART_PutString(char *s)
{
while(*s)
USART_PutChar(*s++);
}
void Sabertooth::ResetSaber(){
USART_PutChar(Sabertooth_USART, AUTOBAUD_BYTE); //Send the autobaud byte to get the sabertooth communicating
SendDriveCmd(14, 20); //Sets the communication watchdog on the sabertooth to (x*100ms) It's currently set to two seconds.
}
