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); }