Diff: main.cpp

Revision:

3:c18342e4fddd

Parent:

2:f30666d7838b

Child:

4:67f705d42f1e

--- a/main.cpp	Mon Dec 24 07:47:22 2018 +0000
+++ b/main.cpp	Wed Feb 06 11:49:05 2019 +0000
@@ -1,4 +1,8 @@
 #include "mbed.h"
+#include "MPU6050_DMP6.h"
+
+//MPU_check用
+#define PI 3.14159265358979
 
 #define servo_NEUTRAL_R    1460
 #define servo_NEUTRAL_L    1460
@@ -16,27 +20,54 @@
 #define MOVE_LEFT       2
 #define MOVE_RIGHT      3
 #define MOVE_BACK       4
-#define GOAL_FORWARD    5          //ゴール付近 ゆっくり
+#define GOAL_FORWARD    5          //ゴール付近_ゆっくり
 #define GOAL_LEFT       6
 #define GOAL_RIGHT      7
-#define MAX_FORWARD     8          //はやい 姿勢修正用
+#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);
@@ -47,9 +78,15 @@
     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);
@@ -126,7 +163,7 @@
         
         //pc.printf("%c",SFbuf[bufcounter]);
         
-        if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
+        if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
             
         if(bufcounter==5 && SFbuf[4]=='F'){
                 
@@ -200,6 +237,93 @@
     
     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");
+}
\ No newline at end of file