/*! \brief Test function.
*
* This function tests the SPI master and slave drivers in polled operation,
* with a master (on port C) communicating with a slave (on port D).
*
* Hardware setup:
*
* - Connect PC4 to PD4 (SS)
* - Connect PC5 to PD5 (MOSI)
* - Connect PC6 to PD6 (MISO)
* - Connect PC7 to PD7 (SCK)
*
* The drivers are tested in two phases:
*
* 1: Data is transmitted on byte at a time from the master to the slave.
* The slave increments the received data and sends it back. The master reads
* the data from the slave and verifies that it equals the data sent + 1.
*
* 2: Data is transmitted 4 bytes at a time to the slave. As the master sends
* a byte to the slave, the preceding byte is sent back to the master.
* When all bytes have been sent, it is verified that the last 3 bytes
* received at the master, equal the first 3 bytes sent.
*
* The variable, 'success', will be non-zero when the function reaches the
* infinite for-loop if the test was successful.
*/
int main(void)
{
/* Init SS pin as output with wired AND and pull-up. */
PORTC.DIRSET = PIN4_bm;
PORTC.PIN4CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTC.OUTSET = PIN4_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTC;
/* Initialize SPI master on port C. */
SPI_MasterInit(&spiMasterC,
&SPIC,
&PORTC,
false,
SPI_MODE_0_gc,
SPI_INTLVL_LO_gc,
false,
SPI_PRESCALER_DIV4_gc);
/* Initialize SPI slave on port D. */
SPI_SlaveInit(&spiSlaveD,
&SPID,
&PORTD,
false,
SPI_MODE_0_gc,
SPI_INTLVL_LO_gc);
/* PHASE 1: Transceive individual bytes. */
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN4_bm);
for(uint8_t i = 0; i < NUM_BYTES; i++) {
/* MASTER: Transmit data from master to slave. */
SPI_MasterTransceiveByte(&spiMasterC, masterSendData[i]);
/* SLAVE: Wait for data to be available. */
while (SPI_SlaveDataAvailable(&spiSlaveD) == false) {
}
/* SLAVE: Get the byte received. */
uint8_t slaveByte = SPI_SlaveReadByte(&spiSlaveD);
/* SLAVE: Increment received byte and send back. */
slaveByte++;
SPI_SlaveWriteByte(&spiSlaveD, slaveByte);
/* MASTER: Transmit dummy data to shift data from slave to master. */
uint8_t masterReceivedByte = SPI_MasterTransceiveByte(&spiMasterC, 0x00);
/* MASTER: Check if the correct value was received. */
if (masterReceivedByte != (masterSendData[i] + 1) ) {
success = false;
}
}
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN4_bm);
/* PHASE 2: Transceive data packet. */
/* Create data packet (SS to slave by PC4). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
masterReceivedData,
NUM_BYTES,
&PORTC,
PIN4_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* Check that correct data was received. Assume success at first. */
for (uint8_t i = 0; i < NUM_BYTES - 1; i++) {
if (masterReceivedData[i + 1] != masterSendData[i]) {
success = false;
}
}
while(true) {
nop();
}
}
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)
{
unsigned int XXL,YXL,ZXL,XGL,YGL,ZGL;
unsigned char XYZ_buffer[100];
clk_init();
port_init();
USART_INIT();
// spi_init();
/* Init SS pin as output with wired AND and pull-up. */
PORTC.DIRSET = PIN0_bm;
PORTC.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc;
PORTC.DIRSET = PIN1_bm;
PORTC.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTC.OUTSET = PIN0_bm;
PORTC.OUTSET = PIN1_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTC;
/* Initialize SPI master on port C. */
SPI_MasterInit(&spiMasterC,
&SPIC,
&PORTC,
false,
SPI_MODE_3_gc,
SPI_INTLVL_OFF_gc,
false,
SPI_PRESCALER_DIV16_gc);
/* Initialize ACC & Gyro */
Init_L3G4200DH();
Init_LIS331DLH();
/* Read Sensor data */
while(true) {
/* Create data packet (SS to slave by PC0). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
ACC_DATA,
NUM_BYTES,
&PORTC,
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);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN0_bm);
/* Create data packet (SS to slave by PC1). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
GYRO_DATA,
NUM_BYTES,
&PORTC,
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. */
SPI_MasterTransceivePacket(&spiMasterC, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN1_bm);
ACC_DATA[2]=ACC_DATA[2]+0x80;
ACC_DATA[4]=ACC_DATA[4]+0x80;
ACC_DATA[6]=ACC_DATA[6]+0x80;
XXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1];
YXL= (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;
XGL= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1];
YGL= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3];
ZGL= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5];
sprintf((char*)XYZ_buffer,"[XXL%6u][YXL%6u][ZXL%6u][XGY%6u][YGY%6u][ZGY%6u]\r\n",XXL,YXL,ZXL,XGL,YGL,ZGL);
//uartSendTXbit('3');
//sprintf((char*)XYZ_buffer,"abcdefgefgh12341\n\r");
uartSendTX(XYZ_buffer);
}
while(true) {
nop();
}
}
