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();
}
}
int straight(){
Read_Compass();
newAngle= 180-initialDirection+angle;
if(newAngle<0){
newAngle= newAngle+360;
}else if(newAngle>360){
newAngle= newAngle-360;
}
if(newAngle<170&& newAngle>0){
// rotateRight();
PORTB.F0=0;
PORTB.F1=1;
// right 10;
}else if(newAngle>190&& newAngle<360){
//rotateLeft();
PORTB.F0=1;
PORTB.F1=0;
// right 01;
}else if((newAngle<345 && newAngle>335) || (newAngle<115 && newAngle>105)){
//stopAll();
PORTB.F0=1;
PORTB.F1=1;
// right 11;
}
}
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();
}
}
void main(void) {
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
Port_Init();
PCA_Init();
SMB_Init();
Interrupt_Init();
printf("Starting\n\r");
//print beginning message
printf("Embedded Control Drive Motor Control\r\n");
// Initialize motor in neutral and wait for 1 second
MOTOR_PW = PW_NEUT;
motorPW = 0xFFFF-MOTOR_PW;
PCA0CPL2 = motorPW;
PCA0CPH2 = motorPW>>8;
printf("Pulse Width = %d\r\n",MOTOR_PW);
c = 0;
while (c < 50); //wait 1 second in neutral
printf("end wait \r\n");
//Main Functionality
while (1) {
if (!SS1) { // If the slide switch is active, set PW to center
PW = PWCENTER;
PCA0CP0 = 0xFFFF - PW; // Update comparator with new PW value
while (!SS1); // Wait...
} else if (take_heading) { // Otherwise take a new heading
reading = Read_Compass(); // Get current heading
printf("%d\n\r", reading);
Steering_Servo(reading); // Change PW based on current heading
PCA0CP0 = 0xFFFF - PW; // Update comparator with new PW value
}
if (getRange) {
getRange = 0; // Reset 80 ms flag
range_val = read_ranger(); // Read the distance from the ranger
printf("Range: %d cm \r\n", range_val);
printf("Pulse Width: %d \r\n", MOTOR_PW);
// Start a new ping
Data[0] = 0x51; // write 0x51 to reg 0 of the ranger:
i2c_write_data(addr, 0, Data, 1); // write one byte of data to reg 0 at addr
}
if (SS0) Drive_Motor(range_val);
else Drive_Motor(45); // Hold the motor in neutral if the slide switch is off
}
}
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 main(){
Lcd_init(); // Initialize LCD
Lcd_Cmd(_LCD_CLEAR); // Clear display
Lcd_Cmd(_LCD_CURSOR_OFF);
I2C1_Init(100000);
TRISB.F0=0;
TRISB.F1=0;
while(1){
Read_Compass();
WordToStr(angle,to_LCD);
Lcd_Out(1,1,to_LCD);
straight();
// delay_ms(500);
// Lcd_Out(1,1,"Piyumal");
//delay_ms(1000);
}
}
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);
}
