volatile void initialize_IMU(void)
{
twim_init();
Enable_Acc(TWIM);
Enable_Gyro(TWIM);
Enable_Magn(TWIM);
delay_ms(20);
for (int i = 0; i < 32; i++)
{
Read_Gyro();
Read_Accel();
RAW_OFFSET[0] += RAW[0];
RAW_OFFSET[1] += RAW[1];
RAW_OFFSET[2] += RAW[2];
RAW_OFFSET[3] += RAW[3];
RAW_OFFSET[4] += RAW[4];
RAW_OFFSET[5] += RAW[5];
delay_ms(20);
}
RAW_OFFSET[0] = 0;
RAW_OFFSET[1] = 0;
RAW_OFFSET[2] = 0;
RAW_OFFSET[3] = 0;
RAW_OFFSET[4] = 0;
RAW_OFFSET[5] = 0;
timer_current = rtc_get_value(&AVR32_RTC);
delay_ms(20);
get_Angles();
}
void sample_MiniMU9() //Main Loop
{
if((millis()-timer)>=100) //20 // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;
// *** DCM algorithm
// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer
if (counter > 5) // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
Read_Compass(); // Read I2C magnetometer
Compass_Heading(); // Calculate magnetic heading
}
// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***
printdata();
}
}
void init_MiniMU9()
{
I2C_Init();
delay(1500);
Accel_Init();
Compass_Init();
Gyro_Init();
delay(20);
for(int i=0;i<32;i++) // We take some readings...
{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}
for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;
AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
delay(2000);
timer=millis();
delay(20);
counter=0;
}
int main(void)
{
My_Init();
Init_Timer();
Init_I2C();
Init_Sensors();
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1);
/////////////////////////////////
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); //enable GPIO port for LED
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2); //enable pin for LED PF2
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
IntMasterEnable(); //enable processor interrupts
IntEnable(INT_UART0); //enable the UART interrupt
UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT); //only enable RX and TX interrupts
/////////////////////////////////
Kalman_Sim_initialize();
while(1)
{
Read_Accelerometer();
Calculate_Acc();
Read_Compass();
Compass_Heading();
Calculate_Compass();
Read_Gyro();
Calculate_Gyro();
fgyro[0] = sen_data.gyro_x;
fgyro[1] = sen_data.gyro_y;
fgyro[2] = sen_data.gyro_z;
facc[0] = sen_data.accel_x;
facc[1] = sen_data.accel_y;
facc[2] = sen_data.accel_z;
fmag[0] = sen_data.magnetom_x;
fmag[1] = sen_data.magnetom_y;
fmag[2] = sen_data.magnetom_z;
Kalman_Sim_step();
data[0]=Out1[0];
data[1]=Out1[1];
data[2]=Out1[2];
Timer_CyRun();
}
}
volatile void get_Angles()
{
static float prev_g_d_roll = 0;
timer_old = timer_current;
timer_current = rtc_get_value(&AVR32_RTC);
if (timer_current > timer_old)
{
g_dt = (float)((timer_current-timer_old))*0.001*0.125*0.98; // Time elapsed in seconds -- sensors are in seconds
}
else
{
g_dt = 0;
}
// GET DATA
Read_Gyro();
Read_Accel();
// GYROSCOPE
// Y = roll
float g_d_roll = gyro_x * GYRO_GAIN_RPS; // delta roll in radians/sec
//g_roll = roll + g_d_roll * g_dt;
g_roll = roll + ((g_d_roll+prev_g_d_roll)/2)*g_dt;
prev_g_d_roll = g_d_roll;
// X = pitch
float g_d_pitch = gyro_y * GYRO_GAIN_RPS; // delta pitch in radians/sec
g_pitch = pitch + g_d_pitch * g_dt;
// Z = yaw
float g_d_yaw = gyro_z * GYRO_GAIN_RPS; // delta yaw in radians/sec
g_yaw = yaw + g_d_yaw * g_dt;
// ACCELEROMETER
float acc_x = accel_x * 0.002;
float acc_y = accel_y * 0.002;
float acc_z = accel_z * 0.002;
a_roll = atan2(-acc_y, acc_z);
a_pitch = atan(acc_x/sqrt(acc_y*acc_y + acc_z*acc_z));
// Weigh
roll = TRUST_GAIN * a_roll + (1-TRUST_GAIN) * g_roll;
pitch = TRUST_GAIN * a_pitch + (1-TRUST_GAIN) * g_pitch;
}
void Send_IMU_Info(void){
int16_t IMU_Buffer[9];
Read_Acc(&IMU_Buffer[0]);
//VCP_send_IMU_Multi(ACC_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);
Read_Gyro(&IMU_Buffer[3]);
//VCP_send_IMU_Multi(GYRO_INDEX,IMU_Buffer[3],IMU_Buffer[4],IMU_Buffer[5]);
Read_Compass(&IMU_Buffer[6]);
//VCP_send_IMU_Multi(COMPASS_INDEX,IMU_Buffer[6],IMU_Buffer[7],IMU_Buffer[8]);
/*Read_Acc(&IMU_Buffer[0]);
Read_Gyro(&IMU_Buffer[3]);
Read_Compass(&IMU_Buffer[6]);*/
VCP_send_IMU_All(IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2],IMU_Buffer[3],IMU_Buffer[4],IMU_Buffer[5],IMU_Buffer[6],IMU_Buffer[7],IMU_Buffer[8]);
}
void read_sensors()
{
Read_Gyro(); // Read gyroscope
Read_Accel(); // Read accelerometer
Read_Magn(); // Read magnetometer
}
void run_function2(void){
uint8_t Xval, Yval = 0x00;
int16_t IMU_Buffer[3];
unsigned char buff[100];
float IMU_Buffer1[3];
uint8_t speed=0, accuracy=0;
int sum = 0;
uint32_t ADC_temp = 0;
if(VCP_receive_string(buff)){//if receive command
if(buff[0]=='C'){
Button_state = BUTTON_STATE_1;//stop sending message
switch(buff[1]){
case 'A':
Read_Acc(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(ACC_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'G': //GyroReadAngRate(IMU_Buffer);
Read_Gyro(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(GYRO_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'C': //LSM303DLHC_CompassReadMag(IMU_Buffer);
Read_Compass(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(COMPASS_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'I': speed = buff[3]-'0';accuracy = buff[4]-'0';
switch(buff[2]){
case 'A': Acc_Init(speed,accuracy);break;
case 'C': Compass_Init(speed,accuracy);break;
case 'G': Gyro_Init(speed,accuracy);break;
default : break;
}
break;
case 'M':sum =(buff[5]-'0')*100 + (buff[6]-'0')*10+(buff[7]-'0');
if(buff[2] == 'P'){//percentage
if(sum>100)
sum =100;
if(sum<0)
sum = 0;
//convert to steps
if(buff[4]=='-')
sum = 100 - sum;
else if(buff[4]=='+')
sum = 100 + sum;
else
break;
sum = sum * 255 / 200;
//set the speed
if(buff[3] == 'L')
Potentialmeter_SetValue(sum,CHIP1);
else if(buff[3] == 'R')
Potentialmeter_SetValue(sum,CHIP2);
}else if(buff[2]=='S'){//steps
if(sum>255)
sum =0;
if(sum<0)
sum = 0;
//buffer4 is no use
//set the speed
if(buff[3] == 'L')
Potentialmeter_SetValue(sum,CHIP1);
else if(buff[3] == 'R')
Potentialmeter_SetValue(sum,CHIP2);
}else
break;
break;
case 'L':if(buff[3]=='1') STM_EVAL_LEDOn(buff[2]-'1'); else STM_EVAL_LEDOff(buff[2]-'1'); break;
case 'S': sum = (buff[2]-'0')*100+(buff[3]-'0')*10+(buff[4]-'0') ;Sample_time=sum; break;
default :break;
}
Button_state = BUTTON_STATE_3;
}
}
State2_count ++;
if(State2_count > 200000){
State2_count =0;
STM_EVAL_LEDToggle(LED4);
}
/*
ADC_temp = ADC3ConvertedValue*3000/0xFFF;
if(ADC_temp > 0&&ADC_temp<1000)
STM_EVAL_LEDOn(LED3);
else if(ADC_temp > 1000&&ADC_temp<2000 )
STM_EVAL_LEDOn(LED4);
else if(ADC_temp > 2000&&ADC_temp<3000 )
STM_EVAL_LEDOn(LED5);
else{
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
}
VCP_send_int((uint16_t)ADC_temp);
wait(1000);*/
}
void run_function1(void){
//DATA TRANSMISSION
unsigned char buff[100];
float IMU_Buffer1[3];
//float Buffer[6];
uint8_t Xval, Yval = 0x00;
int16_t IMU_Buffer[3];
// unsigned char start[] = "start";
// LSM303DLHC_MEMS_Test();
int speed=0, accuracy=0;
int sum = 0;
if(VCP_receive_string(buff)){//if receive command
if(buff[0]=='C'){
switch(buff[1]){
case 'A':
Read_Acc(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(ACC_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'G': //GyroReadAngRate(IMU_Buffer);
Read_Gyro(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(GYRO_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'C': //LSM303DLHC_CompassReadMag(IMU_Buffer);
Read_Compass(IMU_Buffer);
switch(buff[2]){
case 'X': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[0]);break;
case 'Y': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[1]);break;
case 'Z': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[2]);break;
case 'A': VCP_send_IMU_Multi(COMPASS_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
default :break;
}
break;
case 'I': speed = buff[3]-'0';accuracy = buff[4]-'0';
switch(buff[2]){
case 'A': Acc_Init(speed,accuracy);break;
case 'C': Compass_Init(speed,accuracy);break;
case 'G': Gyro_Init(speed,accuracy);break;
default : break;
}
break;
case 'M':sum =(buff[5]-'0')*100 + (buff[6]-'0')*10+(buff[7]-'0');
if(buff[2] == 'P'){//percentage
if(sum>100)
sum =100;
if(sum<0)
sum = 0;
//convert to steps
if(buff[4]=='-')
sum = 100 - sum;
else if(buff[4]=='+')
sum = 100 + sum;
else
break;
sum = sum * 255 / 200;
//set the speed
if(buff[3] == 'L')
Potentialmeter_SetValue(sum,CHIP1);
else if(buff[3] == 'R')
Potentialmeter_SetValue(sum,CHIP2);
}else if(buff[2]=='S'){//steps
if(sum>255)
sum =0;
if(sum<0)
sum = 0;
//buffer4 is no use
//set the speed
if(buff[3] == 'L')
Potentialmeter_SetValue(sum,CHIP1);
else if(buff[3] == 'R')
Potentialmeter_SetValue(sum,CHIP2);
}else
break;
break;
case 'L':if(buff[3]=='1') STM_EVAL_LEDOn(buff[2]-'1'); else STM_EVAL_LEDOff(buff[2]-'1'); break;
case 'S': sum = (buff[2]-'0')*100+(buff[3]-'0')*10+(buff[4]-'0') ;Sample_time=sum; break;
default :break;
}
}
}
// Potentialmeter_SetValue(i++,CHIP1);
// VCP_receive_string(buff);
// VCP_send_str(buff);
/*
//MEMS303
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(100);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(100);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(100);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(100);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(100);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, 6, buff);
VCP_send_str(buff);
VCP_put_char(101);
//VCP_put_char('\n');
L3GD20_Read(buff,L3GD20_OUT_X_L_ADDR,6);
VCP_send_str(buff);
VCP_put_char(102);
L3GD20_Read(buff,L3GD20_OUT_X_H_ADDR,6);
VCP_send_str(buff);
VCP_put_char(102);
L3GD20_Read(buff,L3GD20_OUT_Y_L_ADDR,6);
VCP_send_str(buff);
VCP_put_char(102);
L3GD20_Read(buff,L3GD20_OUT_Y_H_ADDR,6);
VCP_send_str(buff);
VCP_put_char(102);
L3GD20_Read(buff,L3GD20_OUT_Z_L_ADDR,6);
VCP_send_str(buff);
VCP_put_char(102);
L3GD20_Read(buff,L3GD20_OUT_Z_H_ADDR,6);
VCP_send_str(buff);
VCP_put_char(103);
*/
/* Demo Gyroscope */
//Demo_GyroConfig();
/*
// Read Gyro Angular data
Read_Gyro(IMU_Buffer);
// Update autoreload and capture compare registers value
Xval = ABS(IMU_Buffer[0]);
Yval = ABS(IMU_Buffer[1]);
if ( Xval>Yval)
{
if (IMU_Buffer[0] > 1115.0f)
{
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}
if (IMU_Buffer[0] < -1115.0f)
{
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED6);
}
}
else
{
if (IMU_Buffer[1]< -1115.0f)
{
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}
if (IMU_Buffer[1] > 1115.0f)
{
STM_EVAL_LEDOn(LED6);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
}
}
LSM303DLHC_CompassReadAcc(IMU_Buffer1);
Xval = ABS(IMU_Buffer1[0]);
Yval = ABS(IMU_Buffer1[1]);
if ( Xval>Yval)
{
if (IMU_Buffer1[0] > 1115.0f)
{
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}
if (IMU_Buffer1[0] < -1115.0f)
{
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED6);
}
}
else
{
if (IMU_Buffer1[1]< -1115.0f)
{
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED6);
}
if (IMU_Buffer1[1] > 1115.0f)
{
STM_EVAL_LEDOn(LED6);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
}
}
//MEMS303
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_H_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_A, 6, buff);
VCP_put_char(buff[0]);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, 6, buff);
VCP_send_str(buff);
//VCP_put_char('\n');*/
STM_EVAL_LEDOn(LED5);
wait(10000);
// Potentialmeter_SetValue(i++,CHIP2);
STM_EVAL_LEDOff(LED5);
wait(10000);
}
