SDの設定が反映されるように修正
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
main.cpp
- Committer:
- Skykon
- Date:
- 2019-02-06
- Revision:
- 3:c18342e4fddd
- Parent:
- 2:f30666d7838b
- Child:
- 4:67f705d42f1e
File content as of revision 3:c18342e4fddd:
#include "mbed.h" #include "MPU6050_DMP6.h" //MPU_check用 #define PI 3.14159265358979 #define servo_NEUTRAL_R 1460 #define servo_NEUTRAL_L 1460 #define servo_FORWARD_R 1860 #define servo_FORWARD_L 1860 #define servo_back_R 1060 #define servo_back_L 1060 #define servo_slow_FORWARD_R 1560 #define servo_slow_FORWARD_L 1560 #define servo_slow_back_R 1360 #define servo_slow_back_L 1360 #define MOVE_NEUTRAL 0 #define MOVE_FORWARD 1 #define MOVE_LEFT 2 #define MOVE_RIGHT 3 #define MOVE_BACK 4 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり #define GOAL_LEFT 6 #define GOAL_RIGHT 7 #define MAX_FORWARD 8 //はやい_姿勢修正用 #define MAX_BACK 9 void getSF_Serial_jevois(); void getSF_Serial_pi(); //MPU_check用 void SensingMPU(); void setup(); void Init_sensors(); void DisplayClock(); static float nowAngle[3] = {0,0,0}; float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 Timer t; PwmOut servoR(PC_6); //pc6:TIM3_CH1 PwmOut servoL(PC_7); //pc7:TIM3_CH2 RawSerial pc(PA_2,PA_3,115200); //uart2 //pa2:UART2_TX,pa3:UART2_RX RawSerial pc2(PB_6,PB_7,115200); //uart1 //pb6:UART1_TX,pb7:UART1_RX char g_landingcommand='N'; void MoveCansat(char g_landingcommand); //MPU_check用 MPU6050DMP6 mpu6050(PC_0,&pc); int main() { //MPU_check setup(); // シリアル通信受信の割り込みイベント登録 pc.attach(getSF_Serial_jevois, Serial::RxIrq); pc2.attach(getSF_Serial_pi, Serial::RxIrq); NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度 NVIC_SetPriority(USART2_IRQn,1); while(1) { //pc.printf("Hello World!!"); MoveCansat(g_landingcommand); SensingMPU(); wait_ms(23); //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用 //pc.printf("\r\n"); } } void MoveCansat(char g_landingcommand) { //NVIC_DisableIRQ(USART1_IRQn); NVIC_DisableIRQ(USART2_IRQn); switch(g_landingcommand){ case 'N': //MOVE_NEUTRAL servoR.pulsewidth_us(servo_NEUTRAL_R); servoL.pulsewidth_us(servo_NEUTRAL_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; case 'Y': //MOVE_FORWARD servoR.pulsewidth_us(servo_FORWARD_R); servoL.pulsewidth_us(servo_FORWARD_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; case 'L': //MOVE_LEFT servoR.pulsewidth_us(servo_slow_FORWARD_R); servoL.pulsewidth_us(servo_slow_back_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); case 'R': //MOVE_RIGHT servoR.pulsewidth_us(servo_slow_back_R); servoL.pulsewidth_us(servo_slow_FORWARD_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; case 'B': //MOVE_BACK servoR.pulsewidth_us(servo_back_R); servoL.pulsewidth_us(servo_back_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; case 'G': //GOAL_FORWARD servoR.pulsewidth_us(servo_slow_FORWARD_R); servoL.pulsewidth_us(servo_slow_FORWARD_L); //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; default : //NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); break; } return; } void getSF_Serial_jevois(){ static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; static int bufcounter=0; if(pc.readable()) { // 受信確認 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し if(SFbuf[0]!='S'){ //pc.printf("x"); return; } //pc.printf("%c",SFbuf[bufcounter]); if(SFbuf[0]=='S' && bufcounter<5)bufcounter++; if(bufcounter==5 && SFbuf[4]=='F'){ g_landingcommand = SFbuf[1]; wait_ms(31);//信号が速すぎることによる割り込み防止 //pc.printf("%c",g_landingcommand); //wait_ms(20); //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); bufcounter = 0; memset(SFbuf, 0, sizeof(SFbuf)); NVIC_ClearPendingIRQ(USART2_IRQn); //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); } else if(bufcounter>=5){ //pc.printf("Communication Falsed.\r\n"); memset(SFbuf, 0, sizeof(SFbuf)); bufcounter = 0; NVIC_ClearPendingIRQ(USART2_IRQn); } } } void getSF_Serial_pi(){ NVIC_DisableIRQ(USART2_IRQn); static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; static int bufcounter=0; if(pc2.readable()) { // 受信確認 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し if(SFbuf[0]!='S'){ //pc.printf("x"); return; } //pc.printf("%c",SFbuf[bufcounter]); if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++; if(bufcounter==5 && SFbuf[4]=='F'){ g_landingcommand = SFbuf[1]; wait_ms(31);//信号が速すぎることによる割り込み防止 //pc.printf("%c",g_landingcommand); //wait_ms(20); //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); bufcounter = 0; memset(SFbuf, 0, sizeof(SFbuf)); NVIC_ClearPendingIRQ(USART2_IRQn); //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); } else if(bufcounter>=5){ //pc.printf("Communication Falsed.\r\n"); memset(SFbuf, 0, sizeof(SFbuf)); bufcounter = 0; NVIC_ClearPendingIRQ(USART2_IRQn); } } NVIC_EnableIRQ(USART2_IRQn); } void setup(){ Init_sensors(); //switch2.rise(ResetTrim); NVIC_SetPriority(USART1_IRQn,0); NVIC_SetPriority(EXTI0_IRQn,1); NVIC_SetPriority(TIM5_IRQn,2); NVIC_SetPriority(EXTI9_5_IRQn,3); NVIC_SetPriority(USART2_IRQn,4); DisplayClock(); t.start(); pc.printf("MPU calibration start\r\n"); float offsetstart = t.read(); while(t.read() - offsetstart < 26){ SensingMPU(); for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); pc.printf("\r\n"); } FirstROLL = nowAngle[ROLL]; FirstPITCH = nowAngle[PITCH]; nowAngle[ROLL] -=FirstROLL; nowAngle[PITCH] -=FirstPITCH; wait(0.2); pc.printf("All initialized\r\n"); } void SensingMPU(){ //static int16_t deltaT = 0, t_start = 0; //t_start = t.read_us(); float rpy[3] = {0}, oldrpy[3] = {0}; static uint16_t count_changeRPY = 0; static bool flg_checkoutlier = false; NVIC_DisableIRQ(USART1_IRQn); NVIC_DisableIRQ(USART2_IRQn); NVIC_DisableIRQ(TIM5_IRQn); NVIC_DisableIRQ(EXTI0_IRQn); NVIC_DisableIRQ(EXTI9_5_IRQn); mpu6050.getRollPitchYaw_Skipper(rpy); NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); NVIC_EnableIRQ(TIM5_IRQn); NVIC_EnableIRQ(EXTI0_IRQn); NVIC_EnableIRQ(EXTI9_5_IRQn); //外れ値対策 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; rpy[ROLL] -= FirstROLL; rpy[PITCH] -= FirstPITCH; rpy[YAW] -= FirstYAW; for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} if(!flg_checkoutlier || count_changeRPY >= 2){ for(uint8_t i=0; i<3; i++){ nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 } count_changeRPY = 0; }else count_changeRPY++; flg_checkoutlier = false; } void Init_sensors(){ if(mpu6050.setup() == -1){ pc.printf("failed initialize\r\n"); } } void DisplayClock(){ pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); pc.printf("\r\n"); }