void uartInit(USART_t* uart, uint16_t bsel)
{
if(uart == &USARTC0)
{
// Setup UARTC0
// PIN3 (TXD0) as output.
PORTC.DIRSET = PIN3_bm;
// PC2 (RXD0) as input.
PORTC.DIRCLR = PIN2_bm;
// USARTC0, 8 Data bits, No Parity, 1 Stop bit.
USART_Format_Set(uart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/*
Set Baudrate to 115200 bps, use the I/O clock frequency that is 32 MHz.
Do not use the baudrate scale factor
Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
= 8
*/
USART_Baudrate_Set(uart, bsel, 0);
}
else if(uart == &USARTF0)
{
// Setup UARTF0
// PIN3 (TXD0) as output.
PORTF.DIRSET = PIN3_bm;
// PF2 (RXD0) as input.
PORTF.DIRCLR = PIN2_bm;
// Enable RXC interrupt.
USART_RxdInterruptLevel_Set(uart, USART_RXCINTLVL_LO_gc);
// Enable PMIC interrupt level low.
PMIC.CTRL |= PMIC_LOLVLEX_bm;
// Don't forget to enable global interrupts in main!
// USARTC0, 8 Data bits, No Parity, 1 Stop bit.
USART_Format_Set(uart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/*
Set Baudrate to 115200 bps, use the I/O clock frequency that is 32 MHz.
Do not use the baudrate scale factor
Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
= 8
*/
USART_Baudrate_Set(uart, bsel, 0);
}
// Enable both RX and TX.
USART_Rx_Enable(uart);
USART_Tx_Enable(uart);
}
void setup_USART1(){
PORTC.DIRSET = PIN7_bm; //PINC3 (TXD0) as output
PORTC.DIRCLR = PIN6_bm; //PC2 (RXD0) as input
USART_InterruptDriver_Initialize(&USART_data1, &USARTC1, USART_DREINTLVL_LO_gc); // Use USARTC0 and initialize buffers.
/* USARTC0, 8 Data bits, Odd parity, 1 Stop bit. */
USART_Format_Set(USART_data1.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_ODD_gc, false);
USART_RxdInterruptLevel_Set(USART_data1.usart, USART_RXCINTLVL_LO_gc); // Enable RXC interrupt.
/* Set Baudrate to 9600 bps:
* Use the default I/O clock fequency that is 2 MHz.
* Do not use the baudrate scale factor
*
* Baudrate select = ((I/O clock frequency)/(16*Baudrate))-1
* = 207 //32mhz
* = 12 //2mhz
*/
USART_Baudrate_Set(&USARTC1, 207 , 0);
/* Enable both RX and TX. */
USART_Rx_Enable(USART_data1.usart);
USART_Tx_Enable(USART_data1.usart);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_LOLVLEX_bm;
}
void USART_INIT(void)
{
/* This PORT setting is only valid to USARTC0 if other USARTs is used a
* different PORT and/or pins is used. */
/* PIN3 (TXD0) as output. */
PORTD.DIRSET = PIN3_bm;
/* PC2 (RXD0) as input. */
PORTD.DIRCLR = PIN2_bm;
/* USARTD0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(&USARTD0, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/* Set Baudrate to 115200 bps:
* Use the default I/O clock fequency that is 16 MHz.
* Do not use the baudrate scale factor
*
* Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
* = 12
*/
USART_Baudrate_Set(&USARTD0, 8 , 0);
/* Enable both RX and TX. */
USART_Rx_Enable(&USARTD0);
USART_Tx_Enable(&USARTD0);
}
void USART_Open(USART * serial, unsigned char port, unsigned char baud_rate, unsigned short tx_buf, unsigned short rx_buf, bool use_rs485, bool isSerialProtocol) {
serial->ref = 0;
serial->CharacterReceived = 0;
serial->CharacterTransmitted = 0;
serial->charCtr = 0;
serial->port_num = port;
serial->port = usart_ports[port];
serial->baud_rate = baud_rate;
serial->use_rs485 = use_rs485;
if (isSerialProtocol) {
serial->isSerialProtocol=true;
}
else {
serial->isSerialProtocol=false;
RingBufferInit(&serial->tx_buffer, tx_buf);
RingBufferInit(&serial->rx_buffer, rx_buf);
}
USART_Table[port] = serial;
serial->port.gpio_port->DIRSET = serial->port.tx_pin_bm;
serial->port.gpio_port->DIRCLR = serial->port.rx_pin_bm;
#ifdef USE_RS485
if (use_rs485) {
serial->port.gpio_port->DIRSET = serial->port.txen_pin_bm;
serial->port.gpio_port->OUTCLR = serial->port.txen_pin_bm;
}
#endif
cli();
USART_Format_Set(serial->port.usart_port, USART_CHSIZE_8BIT_gc,
USART_PMODE_DISABLED_gc, false); // 8 bits, no parity, one stop bit
serial->port.usart_port->CTRLA = ((serial->port.usart_port)->CTRLA & ~USART_RXCINTLVL_gm) | USART_RXCINTLVL_LO_gc;
if (use_rs485 || isSerialProtocol) { // only enable the TXC interrupt if we're in RS485 mode, as it's only used to clear the TXEN line
serial->port.usart_port->CTRLA = ((serial->port.usart_port)->CTRLA & ~USART_TXCINTLVL_gm) | USART_TXCINTLVL_LO_gc;
}
if (isSerialProtocol) {
// serial->port.usart_port->CTRLA = ((serial->port.usart_port)->CTRLA & ~USART_DREINTLVL_gm) | USART_DREINTLVL_LO_gc;
}
serial->port.usart_port->BAUDCTRLA = USART_BAUD_TABLE[baud_rate] & 0xFF;
serial->port.usart_port->BAUDCTRLB = (USART_BAUD_TABLE[baud_rate] >> 8);
serial->port.usart_port->BAUDCTRLB |= ((USART_BSCALE_TABLE[baud_rate]&0x0F) << USART_BSCALE0_bp);
USART_Rx_Enable(serial->port.usart_port);
USART_Tx_Enable(serial->port.usart_port);
PMIC.CTRL |= PMIC_LOLVLEX_bm;
sei();
}
// set up the serial port for sending data back and forth to the PC
// via the XBEE radio.
// 8 bits, no parity, 2 stop bits
void initUART()
{
// XBEE on USARTD1
/* PD6 (RXD1) input*/
PORTD.DIRCLR = PIN6_bm;
/* PD7 (TXD1) as output. */
PORTD.DIRSET = PIN7_bm;
// IMU on USARTC1
/* PC6 (RXD1) input*/
PORTC.DIRCLR = PIN6_bm;
/* PD7 (TXD1) as output. */
PORTC.DIRSET = PIN7_bm;
USART_Format_Set(&XBEE_USART, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, true);
USART_Baudrate_Set(&XBEE_USART, 1047 , -6); // set for 32MHZ and 115200
/* Enable both RX and TX. */
USART_Rx_Enable(&XBEE_USART);
USART_Tx_Enable(&XBEE_USART);
}
void ConfigUart(void){
/* This PORT setting is only valid to USARTE0 if other USARTs is used a
* different PORT and/or pins are used. */
/* PC3 (TXD0) as output. */
PORTE.DIRSET = PIN3_bm;
/* PC2 (RXD0) as input. */
PORTE.DIRCLR = PIN2_bm;
/* Use USARTE0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USART_data, &USART, USART_DREINTLVL_LO_gc);
/* USARTE0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(USART_data.usart, USART_CHSIZE_8BIT_gc,
USART_PMODE_DISABLED_gc, false);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USART_data.usart, USART_RXCINTLVL_LO_gc);
/* Set Baudrate to 115200 bps:
* Use the default I/O clock frequency that is 2 MHz.
* Do not use the baudrate scale factor
*
* Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
* = 12
*/
USART_Baudrate_Set(&USART, 2094 , -7);
/* Enable both RX and TX. */
USART_Rx_Enable(USART_data.usart);
USART_Tx_Enable(USART_data.usart);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_LOLVLEX_bm;
}
void ARM_INIT(){ //USARTD1
PORTD.DIRSET = PIN7_bm; //Sets TX Pin as output
PORTD.DIRCLR = PIN6_bm; //Sets RX pin as input
USART_InterruptDriver_Initialize(&ARM_USART, &USARTD1, USART_DREINTLVL_LO_gc); //Initialize USARTD1 as interrupt driven serial and clear it's buffers
USART_Format_Set(ARM_USART.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(ARM_USART.usart, USART_RXCINTLVL_LO_gc); //Enable the receive interrupt
USART_Baudrate_Set(&USARTD1, 207 , 0); //Set baudrate to 9600 with 32Mhz system clock
USART_Rx_Enable(ARM_USART.usart); //Enable receiving over serial
USART_Tx_Enable(ARM_USART.usart);
} //End of arm init, may want to double check everything for correct pins and what not
void GIM_BAL_INIT(){//USARTD0
PORTD.DIRSET = PIN3_bm; //Sets TX Pin as output
PORTD.DIRCLR = PIN2_bm; //Sets RX pin as input
USART_InterruptDriver_Initialize(&GIMBAL_USART, &USARTD0, USART_DREINTLVL_LO_gc); //Initialize USARTD0 as interrupt driven serial and clear it's buffers
USART_Format_Set(GIMBAL_USART.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(GIMBAL_USART.usart, USART_RXCINTLVL_LO_gc); //Enable the receive interrupt
USART_Baudrate_Set(&USARTD0, 207 , 0); //Set baudrate to 9600 with 32Mhz system clock
USART_Rx_Enable(GIMBAL_USART.usart); //Enable receiving over serial
USART_Tx_Enable(GIMBAL_USART.usart);
}//end of gimbal usart init, may want to double check as well
void init_usart_driver(register8_t dirset, register8_t dirclr, USART_data_t *data, USART_t *module)
{
PORTD.DIRSET = dirset;
PORTD.DIRCLR = dirclr;
USART_InterruptDriver_Initialize(data, module, USART_DREINTLVL_LO_gc);
USART_Format_Set(data->usart, USART_CHSIZE_8BIT_gc, USART_PMODE_EVEN_gc, false);
USART_RxdInterruptLevel_Set(data->usart, USART_RXCINTLVL_LO_gc);
USART_Baudrate_Set(module, 207, 0);
USART_Rx_Enable(data->usart);
USART_Tx_Enable(data->usart);
PMIC.CTRL |= PMIC_LOLVLEN_bm;
}
void Saber_init_tres(){ //USARTF0
PORTF.DIRSET = PIN3_bm; //Sets TX Pin as output
PORTF.DIRCLR = PIN2_bm; //Sets RX pin as input
USART_InterruptDriver_Initialize(&SABER_TRES, &USARTF0, USART_DREINTLVL_LO_gc); //Initialize USARTF0 as interrupt driven serial and clear it's buffers
USART_Format_Set(SABER_TRES.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_TRES.usart, USART_RXCINTLVL_LO_gc); //Enable the receive interrupt
USART_Baudrate_Set(&USARTF0, 207 , 0); //Set baudrate to 9600 with 32Mhz system clock
USART_Rx_Enable(SABER_TRES.usart); //Enable receiving over serial
USART_Tx_Enable(SABER_TRES.usart);
_delay_ms(100); //Delay -- allowing things to settle
USART_PutChar(&USARTF0, AUTOBAUD_BYTE);
} //End drive inits
Sabertooth::Sabertooth(USART_t *USART_SaberUsart, PORT_t * SaberPORT)
{
Sabertooth_USART = USART_SaberUsart; //Sets the private variable to the USART being used
Sabertooth_PORT = SaberPORT; //Sets the private variable for the PORT the USART is on
Sabertooth_PORT->DIRSET = PIN3_bm; //Sets the TX pin for the USART to an output
USART_Format_Set(Sabertooth_USART, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false); //Sets the Sabertooth USART to run in 8 bit data, no parity, and 1 stop bit,
USART_Baudrate_Set(Sabertooth_USART, 207 , 0); //Sets the Sabertooth baud rate to 9600 when running at 32Mhz system clock
USART_Tx_Enable(Sabertooth_USART); //Enable the USART transmit capabilities
_delay_ms(100); //Delay to let things settle
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.
StopAll(); //Everything is now initialized, stop all motor movement to account for random noise or failed startups
}
int main(void){
char xbeebuffer[100];
int adcSample;
/**Setup Xbee*/
PORTD.DIR = 0b00001000;
PORTF.DIR = 3;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
USART_InterruptDriver_Initialize(&xbee, &USARTD0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTD0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
ADC_Ch_InputMode_and_Gain_Config(&ADC_BK.CH0, ADC_CH_INPUTMODE_DIFF_gc, ADC_DRIVER_CH_GAIN_NONE); // differential mode, no gain
ADC_Ch_InputMux_Config(&ADC_BK.CH0, pin, ADC_CH_MUXNEG_PIN1_gc);
ADC_Reference_Config(&ADC_BK, ADC_REFSEL_VCC_gc); // use Vcc/1.6 as ADC reference
ADC_ConvMode_and_Resolution_Config(&ADC_BK, ADC_ConvMode_Signed, ADC_RESOLUTION_12BIT_gc);
ADC_Prescaler_Config(&ADC_BK, ADC_PRESCALER_DIV32_gc);
while(1){
if(readdata){
readdata = 0;
if(input == 'r'){
adc_start_conversion(&ADCA, ADC_CH0);
adc_wait_for_interrupt_flag(&ADCA, ADC_CH0);
adcSample = adcch_get_signed_result(&ADCA, 0);
sprintf(xbeebuffer, " %d\n\r", adcSample);
sendstring(&xbee, xbeebuffer);
}
}
}
}
/*! \brief Initializes the UART
*
* \param uart pointer to a UART datastructure with buffers
* \param usart pointer to a UART datastructure
* \param f_cpu system clock (F_CPU)
* \param baud desired baud rate
* \param clk2x clock speed double (1 for double, 0 for no double)
*
* It calculates the scale factor BSCALE and the baud selection value BSEL.
* It selects what USART module to use and it initializes receive and transmit buffer.
* It initializes the USART module and sets the direction of TXD and RXD pin.
* The interrupt levels of the DRE interrupt function and the RXC interrupt function
* are both set to a low level.
*
* \return void
*/
void init_uart(USART_data_t *uart, USART_t *usart, uint32_t f_cpu, uint32_t baud, uint8_t clk2x)
{
uint16_t bsel;
int8_t bscale;
bscale = calc_bscale(f_cpu, baud, clk2x);
bsel = calc_bsel(f_cpu, baud, bscale, clk2x);
USART_InterruptDriver_Initialize(uart, usart, USART_DREINTLVL_LO_gc);
USART_Format_Set(uart->usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, !USART_SBMODE_bm);
USART_Rx_Enable(uart->usart);
USART_Tx_Enable(uart->usart);
USART_RxdInterruptLevel_Set(uart->usart, USART_RXCINTLVL_LO_gc);
USART_Baudrate_Set(uart->usart, bsel, bscale);
set_usart_txrx_direction(uart->usart);
}
void naiboard_uart_init(void) {
sysclk_enable_peripheral_clock(&USARTMODULE);
// We're only using the stdout, stdin is handled by the AVR USART driver.
stdout = &mystdout;
PORT_SetPinsAsOutput(&USARTPORT, USARTTXPIN);
PORT_ConfigurePins(&USARTPORT, USARTTXPIN, false, false, PORT_OPC_WIREDANDPULL_gc, PORT_ISC_INPUT_DISABLE_gc);
PORT_SetPinsAsInput(&USARTPORT, USARTRXPIN);
PORT_ConfigurePins(&USARTPORT, USARTRXPIN, false, false, PORT_OPC_PULLDOWN_gc, PORT_ISC_FALLING_gc);
USART_InterruptDriver_Initialize(&naiboard_uart, &USARTMODULE, USART_DREINTLVL_LO_gc);
USART_RxdInterruptLevel_Set(&USARTMODULE, USART_RXCINTLVL_LO_gc);
USART_Format_Set(&USARTMODULE, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_Baudrate_Set(&USARTMODULE, 1603, -6); // 48MHz, 115200bps
USART_Rx_Enable(&USARTMODULE);
USART_Tx_Enable(&USARTMODULE);
}
int main(void)
{
init32MHzClock();
PORTE.DIRSET = PIN3_bm;
/* PC2 (RXD0) as input. */
PORTE.DIRCLR = PIN2_bm;
/* USARTC0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(&IMU_USART, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
//USART_Format_Set(&PC_USART, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_Baudrate_Set(&IMU_USART, 1079 , -5);
//USART_Baudrate_Set(&PC_USART, 1079 , -5);
/* Enable both RX and TX. */
USART_Rx_Enable(&IMU_USART);
USART_Tx_Enable(&IMU_USART);
uint8_t i = 0;
while(1)
{
char *p;
long li;
char myData[10] = {'1', '2', '3'};
s = "20y";
li = strtol(myData,&p,0);
put_USART_char(li);
//put_USART_char(myData[3]);
//put_USART_char(0x0D);
/*
* At this point, "p" will point at the character 'y'
* in the string constant, and "li" would have the
* value 20
*/
//li = strtol(s,NULL,10); /* assigns 20 to li */
//put_USART_char(li);
//put_USART_char(0x0D);
_delay_ms(250);
}
}
/************************************************************************
* \brief Initializes the telemetry module.
* \return 0 Success.
* \return -1 Error.
************************************************************************/
uint8_t telemetry_init(void)
{
// Enable the system clock for the serial interface
sysclk_enable_peripheral_clock(TELEMETRY_USART_INTERFACE);
// Use USARTE0 and initialize buffers
USART_InterruptDriver_Initialize(&telemetry_usart_data, TELEMETRY_USART_INTERFACE,
USART_DREINTLVL_HI_gc);
// USARTE0, 8 data bits, no parity, 1 stop bit
USART_Format_Set(telemetry_usart_data.usart, USART_CHSIZE_8BIT_gc,
USART_PMODE_DISABLED_gc, false);
// Enable RXC interrupt
USART_RxdInterruptLevel_Set(telemetry_usart_data.usart, USART_RXCINTLVL_HI_gc);
// TODO: Read telemetry settings from VTOL object
// Set baudrate to 9600 bps; scale factor is set to zero
// BSEL = ((I/O clock frequency)/(2^(ScaleFactor)*16*Baudrate))-1
// BSEL for 9600 bps => 207
// BSEL for 57600 bps => 33 (33.722)
USART_Baudrate_Set(telemetry_usart_data.usart, 33, 0);
// Initialize connection data
vtolLinkConnection.rx_packet_length = 0;
vtolLinkConnection.rx_state = VTOLLINK_STATE_SYNC;
vtolLinkConnection.outputStream = &transmit_data;
memset(&vtolLinkConnection.rx_buffer, 0, VTOL_LINK_MAX_PAYLOAD_LENGTH);
memset(&vtolLinkConnection.tx_buffer, 0, VTOL_LINK_MAX_PAYLOAD_LENGTH);
memset(&vtolLinkConnection.stats, 0, sizeof(VTOLLinkStats_t));
// Initialize VTOL link
uint8_t res = vtol_link_init();
if (res < 0)
return -1;
// Enable both RX and TX
USART_Rx_Enable(telemetry_usart_data.usart);
USART_Tx_Enable(telemetry_usart_data.usart);
return 0;
}
void
usart_stdio_init (USART_t *usart, uint16_t bsel, int8_t bscale)
{
USART_InterruptDriver_Initialize (&usart_stdio_data, usart,
USART_DREINTLVL_LO_gc);
USART_Format_Set (usart_stdio_data.usart, USART_CHSIZE_8BIT_gc,
USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set (usart_stdio_data.usart, USART_RXCINTLVL_LO_gc);
USART_Baudrate_Set (usart, bsel, bscale);
USART_Rx_Enable (usart_stdio_data.usart);
USART_Tx_Enable (usart_stdio_data.usart);
PMIC.CTRL |= PMIC_LOLVLEN_bm;
stdout = &usart_stdout;
stdin = &usart_stdin;
}
// This function initializes the usart1 on port d.
void Init_USARTD1( void )
{
PORTD.OUTSET = PIN7_bm; // Set TxD1 high
PORTD.DIRSET = PIN7_bm; // Set TxD1 as output
PORTD.DIRCLR = PIN6_bm; // Set RxD1 as input
rx_d1_buf_ptr = 0;
tx_d1_buf_ptr = 0;
USART_Baud_Set(&USARTD1, USARTD1_BAUD, 0);
USART_Format_Set(&USARTD1, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_DreIntLevel_Set(&USARTD1, USART_DREINTLVL_LO_gc);
USART_TxdIntLevel_Set(&USARTD1, USART_TXCINTLVL_OFF_gc);
USART_RxdIntLevel_Set(&USARTD1, USART_RXCINTLVL_LO_gc);
USART_SetMode(&USARTD1, USART_CMODE_ASYNCHRONOUS_gc);
USART_Rx_Enable(&USARTD1);
USART_Tx_Enable(&USARTD1);
}
/* -----------------------------------------------------------------------------------------------------------*/
void UART_E0_init( void )
{
// FIFO reservieren für RX und TX
RX_E0_fifo = Get_FIFO( RX_E0_Buffer, RX_Bufferlen );
TX_E0_fifo = Get_FIFO( TX_E0_Buffer, TX_Bufferlen );
// TX_state auf complete setzen, da ja nix gesendet wurde
TX_E0_state = TX_complete;
/* PIN3 (TXE0) as output. */
PORTC.DIRSET = PIN3_bm;
/* PC2 (RXE0) as input. */
PORTC.DIRCLR = PIN2_bm;
/* USARTE0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set( &USARTE0 , USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, 0 );
/* Set Baudrate to 9600 bps */
USART_Baudrate_Set( &USARTE0 , ( UBRR_VAL ) , 0 );
/* Enable both RX and TX. */
USART_Rx_Enable( &USARTE0 );
USART_Tx_Enable( &USARTE0 );
USART_RxdInterruptLevel_Set( &USARTE0 , USART_RXCINTLVL_HI_gc );
return;
}
int main(void){
char irinput;
char xbeebuffer[100];
PORTA.DIR = 0;
PORTB.DIR = 0b00000101;
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(readdata){
readdata = 0;
sendchar(&xbee, &input);
if(input == 'r'){
PORTB.OUT |= 0x01;
_delay_ms(1);
while(PORTB.IN & 0x02);
PORTB.OUT &= ~0x01;
PORTB.OUT &= ~0x04;
_delay_ms(1);
PORTB.OUT |= 0x04;
irinput = PORTA.IN;
sprintf(xbeebuffer, " %d\n\r", irinput);
sendstring(&xbee, xbeebuffer);
}
}
}
}
int main(void) {
int translen = 0;
char xbeebuffer[100];
int i;
int bytetobuffer;
char rollread = 0;
char accelread = 0;
int dataready = 0;
char receive;
char count = 0;
uint8_t accelsetupbuffer[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
short int motorr = 0;
short int motorl = 0;
short int servor = 0;
short int servol = 0;
enum states {running, stopped} state = stopped;
/**Setup directions for serial interfaces*/
PORTC.DIR = 0b00001000;
PORTC.OUT = 0b00001000;
PORTE.DIR = 0b00001000;
PORTF.DIR = 0x03;
PORTD.DIR = 0x0F;
//Pulse width modulation setup for servos, port D
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCCEN_bm | TC0_CCDEN_bm;
TCD0.PER = 40000;
/**Enable pullup resistors for imu*/
PORTCFG.MPCMASK = 0x03;
PORTC.PIN0CTRL = (PORTC.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/**Setup IMU*/
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
/**Setup Xbee*/
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
setup = 0;
readdata = 0;
while(1) {
if(USART_RXBufferData_Available(&xbee)) {
receive = USART_RXBuffer_GetByte(&xbee);
if(receive == 's') {
state = running;
}
else if(receive == 'n') {
state = stopped;
}
}
switch(state) {
case stopped:
PORTF.OUT = 3;
TCD0.CCA = 2000;
TCD0.CCC = 2000;
TCD0.CCB = 3000;
TCD0.CCD = 3000;
for(i = 0; i < 3; i ++) {
accelcash[i] = 0;
rollcash[i] = 0;
}
break;
case running:
PORTF.OUT = 0;
//Roll reading
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
while(!readdata);
readdata = 0;
PORTF.OUT |= 0x01;
for(i = 0; i < 5; i += 2) {
if(imu.readData[i + 3] & 0x80) {
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else {
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
//Accel reading
while(imu.status != TWIM_STATUS_READY);
PORTF.OUT ^= 1;
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
while(!readdata);
readdata = 0;
for(i = 0; i < 5; i += 2) {
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
PORTF.OUT |= 0x02;
count ++;
if(count > 4) {
for(i = 0; i < 3; i ++) {
accelcash[i] /= 5;
rollcash[i] /= 2;
}
//motor updates
rollcash[0] -= RXN;
rollcash[1] -= RYN;
rollcash[2] -= RZN;
accelcash[0] -= AXN;
accelcash[1] -= AYN;
accelcash[2] -= AZN;
ValueFunk(accelcash[0],accelcash[1],accelcash[2],rollcash[0],rollcash[1],rollcash[2],&servol,&servor,&motorl,&motorr);
while(TCD0.CNT < 4000);
TCD0.CCA = motorr;
TCD0.CCB = servor;
TCD0.CCC = motorl;
TCD0.CCD = servol;
sprintf(xbeebuffer, " X%4d Y%4d Z%4d x%4d y%4d z%4d R%4d r%4d L%4d l%4d\n\r", rollcash[0], rollcash[1], rollcash[2], accelcash[0], accelcash[1], accelcash[2], motorr, servor, motorl, servol);
for(i = 0; xbeebuffer[i] != 0; i ++) {
bytetobuffer = 0;
while(!bytetobuffer) {
bytetobuffer = USART_TXBuffer_PutByte(&xbee, xbeebuffer[i]);
}
}
for(i = 0; i < 3; i ++) {
accelcash[i] = 0;
}
}
break;
}
}
return 0;
}
/** Event handler for the CDC Class driver Line Encoding Changed event.
*
* \param[in] CDCInterfaceInfo Pointer to the CDC class interface configuration structure being referenced
*/
void EVENT_CDC_Device_LineEncodingChanged(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo)
{
uint8_t pmode = 0;
uint8_t csize = 0;
uint8_t twosb = 0;
int8_t bscale = 4;
uint8_t bsel = 12;
switch (CDCInterfaceInfo->State.LineEncoding.ParityType)
{
case CDC_PARITY_Odd:
pmode = USART_PMODE_ODD_gc;
break;
case CDC_PARITY_Even:
pmode = USART_PMODE_EVEN_gc;
break;
default:
pmode = USART_PMODE_DISABLED_gc;
}
if (CDCInterfaceInfo->State.LineEncoding.CharFormat == CDC_LINEENCODING_TwoStopBits)
twosb = 1;
switch (CDCInterfaceInfo->State.LineEncoding.DataBits)
{
case 6:
csize = USART_CHSIZE_6BIT_gc;
break;
case 7:
csize = USART_CHSIZE_7BIT_gc;
break;
case 8:
csize = USART_CHSIZE_8BIT_gc;
break;
default:
return;
}
switch (CDCInterfaceInfo->State.LineEncoding.BaudRateBPS)
{
case 300:
bsel = 12;
bscale = 9;
break;
case 600:
bsel = 12;
bscale = 8;
break;
case 1200:
bsel = 12;
bscale = 7;
break;
case 2400:
bsel = 12;
bscale = 6;
break;
case 4800:
bsel = 12;
bscale = 5;
break;
case 9600:
bsel = 12;
bscale = 4;
break;
case 14400:
bsel = 138;
bscale = 0;
break;
case 19200:
bsel = 12;
bscale = 3;
break;
case 28800:
bsel = 137;
bscale = -1;
break;
case 38400:
bsel = 12;
bscale = 2;
break;
case 57600:
bsel = 135;
bscale = -2;
break;
case 76800:
bsel = 12;
bscale = 1;
break;
case 115200:
bsel = 131;
bscale = -3;
break;
case 230400:
bsel = 123;
bscale = -4;
break;
case 460800:
bsel = 107;
bscale = -5;
break;
default:
return;
}
USART_Format_Set(&USART, csize, pmode, twosb);
USART_Baudrate_Set(&USART, bsel, bscale);
USART_Rx_Enable(&USART);
USART_Tx_Enable(&USART);
configured = 1;
}
int main(void){
enum states{running, stopped} state = stopped;
char input;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(USART_RXBufferData_Available(&xbee)){
input = USART_RXBuffer_GetByte(&xbee);
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
PORTF.OUT ^= 0x02;
state = stopped;
}
}
switch(state){
case stopped:
break;
case running:
if(TCC0.INTFLAGS & 0x01){
for(i = 0; i < 3; i ++){
rollcash[i] = 0;
accelcash[i] = 0;
}
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
PORTF.OUT = 1;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
sprintf(xbeebuffer, "%d %d\n\r", rollcash[0], accelcash[0]);
sendstring(&xbee, xbeebuffer);
}
break;
}
}
return 0;
}
void USART_INIT(void)
{
/* This PORT setting is only valid to USARTC0 if other USARTs is used a
* different PORT and/or pins is used. */
/* PIN3 (TXC0) as output. */
PORTC.DIRSET = PIN3_bm;
/* PC2 (RXC0) as input. */
PORTC.DIRCLR = PIN2_bm;
/* Use USARTC0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTC0_data, &USARTC0, USART_DREINTLVL_LO_gc);
/* USARTC0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(&USARTC0, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTC0_data.usart, USART_RXCINTLVL_HI_gc);
/* Set Baudrate to 115200 bps:
* Use the default I/O clock fequency that is 16 MHz.
* Do not use the baudrate scale factor
*
* Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
* = 12
*/
// USART_Baudrate_Set(&USARTC0, 8 , 0);
USART_Baudrate_Set(&USARTC0, 16 , 1); // 57600
// USART_Baudrate_Set(&USARTC0, 16 , 0); // 115200
USARTC0.CTRLB|=0x04; //CLK2X
/* Enable both RX and TX. */
USART_Rx_Enable(&USARTC0);
USART_Tx_Enable(&USARTC0);
/* This PORT setting is only valid to USARTD0 if other USARTs is used a
* different PORT and/or pins is used. */
/* PIN3 (TXD0) as output. */
PORTD.DIRSET = PIN3_bm;
/* PC2 (RXD0) as input. */
PORTD.DIRCLR = PIN2_bm;
/* Use USARTD0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTD0_data, &USARTD0, USART_DREINTLVL_LO_gc);
/* USARTD0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(&USARTD0, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTD0_data.usart, USART_RXCINTLVL_HI_gc);
/* Set Baudrate to 115200 bps:
* Use the default I/O clock fequency that is 16 MHz.
* Do not use the baudrate scale factor
*
* Baudrate select = (1/(16*(((I/O clock frequency)/Baudrate)-1)
* = 12
*/
// USART_Baudrate_Set(&USARTC0, 8 , 0);
// USART_Baudrate_Set(&USARTD0, 16 , 1); // 57600
USART_Baudrate_Set(&USARTD0, 16, 0); // 115200, 하기소닉 x,y,theta 받을때의 baudrate
USARTD0.CTRLB|=0x04; //CLK2X
/* Enable both RX and TX. */
USART_Rx_Enable(&USARTD0);
USART_Tx_Enable(&USARTD0);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_LOLVLEX_bm;
/* This PORT setting is only valid to USARTE0 if other USARTs is used a
* different PORT and/or pins is used. */
/* PIN3 (TXE0) as output. */
PORTE.DIRSET = PIN3_bm;
/* PC2 (RXE0) as input. */
PORTE.DIRCLR = PIN2_bm;
/* Use USARTE0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTE0_data, &USARTE0, USART_DREINTLVL_LO_gc);
/* USARTE0, 8 Data bits, No Parity, 1 Stop bit. */
USART_Format_Set(&USARTE0, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTE0_data.usart, USART_RXCINTLVL_LO_gc);
USART_Baudrate_Set(&USARTE0, 16 , 1); // 57600, 하기소닉 보드에 encoder 값 넣을 때의 baudrate
//USART_Baudrate_Set(&USARTE0, 16, 0); // 115200
USARTE0.CTRLB|=0x04;
/* Enable both RX and TX. */
USART_Rx_Enable(&USARTE0);
USART_Tx_Enable(&USARTE0);
/* Enable PMIC interrupt level low. */
// PMIC.CTRL |= PMIC_LOLVLEX_bm;
}
int main(void){
enum states{running, stopped} state = stopped;
/**Move cmd vars*/
short int rise = 0;
short int rotate = 0;
short int forward = 0;
short int tilt = 0;
short int motorr = 0;
short int motorl = 0;
short int servor = 0;
short int servol = 0;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
//Pulse width modulation setup for servos, port D
TCD1.CTRLA = TC_CLKSEL_DIV1_gc;
TCD1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCD1.PER = 40000;
TCC1.CTRLA = TC_CLKSEL_DIV1_gc;
TCC1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCC1.PER = 40000;
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTD.DIR = 0x30;
PORTC.DIR = 0b00111100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWIC.MASTER.CTRLB |= 0x0C;
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(readdata){
readdata = 0;
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
state = stopped;
sprintf(xbeebuffer, "rise %4d tilt %4d rot %4d for %4d \n\r", rise, tilt, rotate, forward);
sendstring(&xbee, xbeebuffer);
}
else if(input == 'u'){
rise += 25;
}
else if(input == 'd'){
rise -= 25;
}
else if(input == 'c'){
rotate += 10;
}
else if(input == 'x'){
rotate -= 10;
}
else if(input == 'a'){
tilt += 10;
}
else if(input == 'e'){
tilt -= 10;
}
else if(input == 't'){
forward += 10;
}
else if(input == 'b'){
forward -= 10;
}
}
switch(state){
case stopped:
TCD1.CCA = 2000;
TCC1.CCA = SERVOLINI;
TCD1.CCB = 2000;
TCC1.CCB = SERVORINI;
break;
case running:
if(TCC0.INTFLAGS & 0x01){
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
PORTF.OUT = 2;
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
PORTF.OUT = 3;
while(imu.result == TWIM_RESULT_UNKNOWN);
PORTF.OUT = 1;
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
PORTF.OUT = 0;
}
for(i = 0; i < 3; i ++){
accelcash[i] /= DAMPENACCEL;
rollcash[i] /= DAMPENROLL;
}
ValueFunk(accelcash[0],accelcash[1],accelcash[2],rollcash[0],rollcash[1],rollcash[2],&servol,&servor,&motorl,&motorr);
while(TCD1.CNT < 4000);
TCD1.CCA = motorl + rise - tilt;
TCD1.CCB = motorr + rise + tilt;
/*
while(TCC1.CNT < 4000);
TCC1.CCA = servol + rotate + forward;
TCC1.CCB = servor - rotate + forward;
*/
sprintf(xbeebuffer, " X%4d x%4d R%4d L%4d\n\r", rollcash[1], accelcash[0],motorr, motorl);
sendstring(&xbee, xbeebuffer);
for(i = 0; i < 3; i ++){
accelcash[i] *= INTEGRATEACCEL;
rollcash[i] *= INTEGRATEROLL;
}
break;
}
}
return 0;
}
