/*! \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(); } }