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 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 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 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 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 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
/*! \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);
}
/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware(void)
{
/* Start the PLL to multiply the 2MHz RC oscillator to 32MHz and switch the CPU core to run from it */
XMEGACLK_StartPLL(CLOCK_SRC_INT_RC2MHZ, 2000000, F_CPU);
XMEGACLK_SetCPUClockSource(CLOCK_SRC_PLL);
/* Start the 32MHz internal RC oscillator and start the DFLL to increase it to 48MHz using the USB SOF as a reference */
XMEGACLK_StartInternalOscillator(CLOCK_SRC_INT_RC32MHZ);
XMEGACLK_StartDFLL(CLOCK_SRC_INT_RC32MHZ, DFLL_REF_INT_USBSOF, F_USB);
PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
// EXOR Gatter
PORTE.DIRSET = PIN0_bm;
PORTE.DIRSET = PIN1_bm;
// PORTE.OUTSET = PIN0_bm; // V1.0
PORTE.OUTCLR = PIN0_bm; // V1.1
PORTE.OUTCLR = PIN1_bm;
// LED
PORTA.DIRSET = PIN2_bm; // Red
PORTA.DIRSET = PIN3_bm; // Green
PORTA.DIRSET = PIN4_bm; // Blue
// LEDS OFF
PORTA.OUTSET = PIN2_bm;
PORTA.OUTCLR = PIN3_bm;
PORTA.OUTSET = PIN4_bm;
/* Hardware Initialization */
// USART RX/TX 1
/* PIN3 (TXD0) as output. */
PORTE.DIRSET = PIN3_bm;
/* PC2 (RXD0) as input. */
PORTE.DIRCLR = PIN2_bm;
USART_InterruptDriver_Initialize(&USART_data, &USART, USART_DREINTLVL_OFF_gc);
USART_RxdInterruptLevel_Set(USART_data.usart, USART_RXCINTLVL_LO_gc);
/* Enable global interrupts. */
sei();
USB_Init();
}
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);
}
}
}
}
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);
}
/************************************************************************
* \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;
}
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);
}
}
}
}
/* -----------------------------------------------------------------------------------------------------------*/
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)
{
unsigned int XXL,YXL,ZXL;
//unsigned int ADC1,ADC2,ADC3;
// int16_t ZGY;
unsigned int XGY,YGY,ZGY;
// unsigned char XYZ_buffer[100];
unsigned int timer=0;
unsigned int timer_old=0;
unsigned int diff_counter=0;
unsigned int m_IMU_count=0;
double m_XXL_sum=0;
double m_YXL_sum=0;
double m_ZXL_sum=0;
double m_XGY_sum=0;
double m_YGY_sum=0;
double m_ZGY_sum=0;
clk_init();
port_init();
USART_INIT();
timer_Init();
int_init();
// sei();
// spi_init();
/* Init SS pin as output with wired AND and pull-up. */
//120813 portC를 D로 바꿈 SPI인거 같아서
PORTD.DIRSET = PIN0_bm;
PORTD.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc;
PORTD.DIRSET = PIN1_bm;
PORTD.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTD.OUTSET = PIN0_bm;
PORTD.OUTSET = PIN1_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTD;
/* Initialize SPI master on port D. */
SPI_MasterInit(&spiMasterD,
&SPID,
&PORTD,
false,
SPI_MODE_3_gc,
SPI_INTLVL_OFF_gc,
false,
SPI_PRESCALER_DIV16_gc);
/* Use USARTD0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTD0_data, &USARTD0, USART_DREINTLVL_HI_gc);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTD0_data.usart, USART_RXCINTLVL_HI_gc);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_HILVLEX_bm;
/* Enable global interrupts.*/
sei();
/* Initialize ACC & Gyro */
// Init_L3G4200DH();
Init_LIS3DH();
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro_init,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN1_bm);
// USART_Rx_Enable(&USARTD0);
// USART_Tx_Enable(&USARTD0);
double z_deg=0;
unsigned int oldT=0;
unsigned int newT=0;
unsigned int dT=0;
int i;
double x1m=0;
double x1p=0;
double P1p=1.0;
double x2m[4]={0,0,0,0};
double x2p[4]={0,0,0,0};
double P2p[4][4]={{0.1,0,0,0},
{0,0.1,0,0},
{0,0,0.1,0},
{0,0,0,0.1}};
double enc_time_cnt=0;
for(i=0;i<DELAY_COMP_G;i++)
{
m_XGY_buf[i]=0.0;
m_YGY_buf[i]=0.0;
m_ZGY_buf[i]=0.0;
m_XXL_buf[i]=0.0;
m_YXL_buf[i]=0.0;
m_ZXL_buf[i]=0.0;
m_enc_buf[i]=0.0;
m_enc_timebuf[i]=0.0;
m_T_buf[i]=0.0;
}
//char XYZ_buffer_debug[20];
//sprintf((char*)XYZ_buffer_debug,"Hello W");
//uartSendTX((unsigned char*)XYZ_buffer_debug);
while(1)
{
//if(1)
if(samplingFlag)
{
// adc_start_conversion(&ADCA, ADC_CH0);
samplingFlag=0;
timer=TCC0.CNT;
diff_counter=timer-timer_old;
/* Create data packet (SS to slave by PC0). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
ACC_DATA,
NUM_BYTES,
&PORTD,
PIN0_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(5);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN0_bm);
/* Create data packet (SS to slave by PC1). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
_delay_us(5);
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN1_bm);
timer_old=timer;
T=(double)diff_counter/2000000.0*32.0;
ACC_DATA[2]=ACC_DATA[2]+0x80;
ACC_DATA[4]=ACC_DATA[4]+0x80;
ACC_DATA[6]=ACC_DATA[6]+0x80;
YXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1];
XXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3];
ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5];
GYRO_DATA[2]=GYRO_DATA[2]+0x80;
GYRO_DATA[4]=GYRO_DATA[4]+0x80;
GYRO_DATA[6]=GYRO_DATA[6]+0x80;
XGY= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3];
YGY= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1];
ZGY= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5];
if(m_IMU_count<Tsample)
{
m_IMU_count++;
}
else if(m_IMU_count<Tsample+Bsample)
{
m_XXL_sum+=XXL;
m_YXL_sum+=YXL;
m_ZXL_sum+=ZXL;
//m_XGY_sum+=XGY;
//m_YGY_sum+=YGY;
m_ZGY_sum+=ZGY;
m_IMU_count++;
}
else if(m_IMU_count==Tsample+Bsample)
{
//SetTimer(25,1,NULL);
m_biasXXL=(double)m_XXL_sum/(double)Bsample;
m_biasYXL=(double)m_YXL_sum/(double)Bsample;
m_biasZXL=(double)m_ZXL_sum/(double)Bsample-GRAVITY_COUNT;
//m_biasXGY=(double)m_XGY_sum/(double)Bsample;
//m_biasYGY=(double)m_YGY_sum/(double)Bsample;
m_biasZGY=(double)m_ZGY_sum/(double)Bsample;
gravityVect[0]=0;
gravityVect[1]=0;
gravityVect[2]=SCALE_ZXL*GRAVITY_COUNT;
m_IMU_count++;
}
else
{
//encoder_interface(0);
//encoder_interface(1);
//host_interface();
//unsigned int a=TCC0.CNT;
//position_estimator(XXL,YXL,ZXL,XGY,YGY,ZGY);
//unsigned int b=TCC0.CNT;
//TJX=(double)(b-a)/2000000.0*32.0;
//FF_controller();
newT=TCC0.CNT;
dT=newT-oldT;
////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
int i,j,k;
m_XXL=-SCALE_XXL*((double)XXL-(double)m_biasXXL);
m_YXL=-SCALE_YXL*((double)YXL-(double)m_biasYXL);
m_ZXL=SCALE_ZXL*((double)ZXL-(double)m_biasZXL);
m_XGY=-SCALE_XGY*((double)XGY-(double)m_biasXGY);//-0.001212142/0.00015711
m_YGY=SCALE_YGY*((double)YGY-(double)m_biasYGY);//+
//if(ZGY<3000) m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY+65536.0);
//else m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);//+
m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);
sprintf(XYZ_buffer,"%u %u %u %u %u %u \n",XXL, YXL, ZXL, XGY, YGY, ZGY);
uartC0SendTX((unsigned char*)XYZ_buffer);
}
}
}
while(true) {
nop();
}
}
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;
}
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;
}
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;
}
