int main(void){
//----------------------------------------------------------------
// Low Level Peripheral INIT [must be done before RTOS Init]
//----------------------------------------------------------------
Init_USARTS();
os_dly_wait(1);
Acc_Init();
os_dly_wait(1);
//----------------------------------------------------------------
//
//----------------------------------------------------------------
os_sys_init(Init_System);
//----------------------------------------------------------------
}
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);
}
