Diff: main.cpp

Revision:

0:92024886c0be

Child:

1:f31e17341659

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 01 12:27:13 2017 +0000
@@ -0,0 +1,440 @@
+#include "mbed.h"
+//#include "rtos.h"
+
+#include "math.h"
+#include "MPU9250.h"
+#include "BMP280.h"
+#include "SkipperSv2.h"
+#include "Estrela.h"
+#include "sbus.h"
+#include "pid.h"
+
+#define DEBUG_SEMIAUTO 1  
+#define DEBUG_PRINT_INLOOP 1
+
+#define KP_ELE 2
+#define KI_ELE 0
+#define KD_ELE 0
+#define KP_RUD 2
+#define KI_RUD 0
+#define KD_RUD 0
+
+#define GAIN_CONTROLVALUE_TO_PWM 3
+
+#define MAGBIAS_X 185.0
+#define MAGBIAS_Y 220.0
+#define MAGBIAS_Z -350.0
+
+const uint16_t changeModeCount = 50; 
+const int16_t lengthdivpwm = 320; 
+
+const int16_t trim[4] = {0,14,-100,-10};
+const float expMax[4] = {100,115,100,89};
+const float expMin[4] = {100,47,100,110};
+
+
+SBUS sbus(PA_9, PA_10); //SBUS
+
+PwmOut servo1(PC_6); // TIM3_CH1
+PwmOut servo2(PC_7); // TIM3_CH2    //PC_7
+PwmOut servo3(PB_0); // TIM3_CH3
+PwmOut servo4(PB_1); // TIM3_CH4
+PwmOut servo5(PB_6); // TIM4_CH1
+PwmOut servo6(PB_7); // TIM4_CH2
+PwmOut servo7(PB_8); // TIM4_CH3    //PB_8
+PwmOut servo8(PB_9); // TIM4_CH4
+
+Serial pc(PA_2, PA_3);
+
+DigitalOut led1(PA_0);  //緑
+DigitalOut led2(PA_1);  //赤
+DigitalOut led3(PB_4); 
+DigitalOut led4(PB_5);
+
+InterruptIn switch2(PC_14);
+
+MPU9250 mpu9250(PC_9,PA_8,&pc);
+
+PID pid_ELE(KP_ELE,KI_ELE,KD_ELE);
+PID pid_RUD(KP_RUD,KI_RUD,KD_RUD);
+
+enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8};
+enum Angle{ROLL, PITCH, YAW};
+enum OperationMode{StartUp, SemiAuto};
+enum OutputStatus{Manual, Auto};
+
+static OutputStatus output_status = Manual;
+OperationMode operation_mode = StartUp;
+static int16_t autopwm[8] = {1500,1500,1000,1500};
+
+static int16_t trimpwm[4] = {1500,1500,1000,1500};
+int16_t maxpwm[4] = {1820,1820,1820,1820};
+int16_t minpwm[4] = {1180,1180,1180,1180};
+int16_t oldTHR = 1000;
+
+static float nowAngle[3] = {0,0,0};
+const float trimAngle[3] = {0,0,0};
+const float maxAngle[2] = {90, 90};
+const float minAngle[2] = {-90, -90};
+
+Timer t;
+
+//IWDG_HandleTypeDef hiwdg;
+
+/*-----関数のプロトタイプ宣言-----*/
+void setup();
+void loop();
+
+void Init_PWM();
+void Init_servo();              //サーボ初期化
+void Init_sbus();               //SBUS初期化
+void Init_sensors();
+void DisplayClock();              //クロック状態確認
+
+void SensingMPU();
+void UpdateTargetAngle(float targetAngle[3]);
+void CalculateControlValue(float targetAngle[3], float controlValue[3]);
+void UpdateAutoPWM(float controlValue[3]);
+void ConvertPWMintoRAD(float targetAngle[3]);
+inline float CalcRatio(float value, float trim, float limit);
+bool CheckSW_Up(Channel ch);
+int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
+
+//sbus割り込み
+void Update_PWM();              //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
+void Output_PWM(int16_t pwm[8]);    //pwmをサーボへ出力
+
+//switch2割り込み
+void ResetTrim();
+
+//デバッグ用
+void DebugPrint();
+
+/*---関数のプロトタイプ宣言終わり---*/
+
+int main()
+{
+    setup();   
+    while(1){
+        loop();
+    }
+}
+
+void setup(){
+    led1 = 1;
+    led2 = 1;
+    led3 = 0;
+    led4 = 0;
+    
+    pc.baud(115200);    
+    Init_PWM();
+    Init_servo();
+    Init_sbus();    
+    Init_sensors();
+    switch2.rise(ResetTrim);
+    t.start();
+    DisplayClock();
+
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+    
+    pc.printf("All initialized\r\n");
+    
+    mpu9250.sensingAcGyMg();
+    mpu9250.calculatePostureAngle(nowAngle);
+}
+
+void loop(){
+    static float targetAngle[3], controlValue[2];
+
+    SensingMPU();
+    UpdateTargetAngle(targetAngle);
+    CalculateControlValue(targetAngle, controlValue);
+    UpdateAutoPWM(controlValue);
+
+#if DEBUG_PRINT_INLOOP
+    DebugPrint();
+#endif
+}
+
+
+
+//サーボ初期化関数
+void Init_servo()
+{
+    
+    servo1.period_ms(14);
+    servo1.pulsewidth_us(trimpwm[AIL]);
+    
+    servo2.period_ms(14);
+    servo2.pulsewidth_us(trimpwm[ELE]);
+    
+    servo3.period_ms(14);
+    servo3.pulsewidth_us(trimpwm[THR]);
+    
+    servo4.period_ms(14);
+    servo4.pulsewidth_us(trimpwm[RUD]);
+    
+    servo5.period_ms(14);
+    servo5.pulsewidth_us(1500);
+    
+    servo6.period_ms(14);
+    servo6.pulsewidth_us(1500);
+    
+    servo7.period_ms(14);
+    servo7.pulsewidth_us(1500);
+    
+    servo8.period_ms(14);
+    servo8.pulsewidth_us(1500);
+
+    pc.printf("servo initialized\r\n");
+}
+
+//Sbusを初期化する関数
+void Init_sbus(){
+    sbus.initialize();
+    sbus.setLastfuncPoint(Update_PWM);
+    sbus.startInterrupt();
+}
+
+void Init_sensors(){
+    if(!mpu9250.Initialize()){
+        pc.printf("failed initialize\r\n");
+        while(1);
+    }
+    mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z);
+}
+
+void Init_PWM(){
+    for (uint8_t i = 0; i < 4; ++i){
+        trimpwm[i] = 1500 + trim[i];
+        maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100));
+        minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100));
+    }
+    pc.printf("PWM initialized\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");
+}
+
+void UpdateTargetAngle(float targetAngle[3]){
+    static uint16_t count_op = 0, count_out = 0;
+
+#if DEBUG_SEMIAUTO    
+    switch(operation_mode){
+        case StartUp:
+            if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){
+                count_op++;
+                if(count_op > changeModeCount){
+                    operation_mode = SemiAuto;
+                    pc.printf("Goto SemiAuto mode\r\n");
+                    count_op = 0;
+                }
+            }else count_op = 0;
+            break;
+
+        case SemiAuto:
+        /*  大会用では以下のif文を入れてoperation_modeを変える
+            if(CheckSW_Up(Ch6)){
+                count_op++;
+                if(count_op>changeModeCount){
+                    output_status = XXX;
+                    led2 = 0;
+                    pc.printf("Goto XXX mode\r\n");
+                    count_op = 0;
+                }else count_op = 0;
+                ConvertPWMintoRAD(targetAngle);
+            }
+        */
+            ConvertPWMintoRAD(targetAngle);
+            break;
+
+        default:
+            operation_mode = SemiAuto;
+            break;
+    }
+
+    if(CheckSW_Up(Ch5)){
+        output_status = Auto;
+        led1 = 1;
+    }else{
+        output_status = Manual;
+        led1 = 0;
+    }
+#endif
+}
+
+void CalculateControlValue(float targetAngle[3], float controlValue[3]){
+    static int t_last;
+    int t_now;
+    float dt;
+
+    t_now = t.read_us();
+    dt = (float)((t_now - t_last)/1000000.0f) ;
+    t_last = t_now;
+
+    controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);    
+    controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
+}
+
+void UpdateAutoPWM(float controlValue[3]){    
+    int16_t addpwm[2]; //-500~500
+    addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ：右回転正     レバー:右回転正
+    addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ：機首下げ正    レバー:機首上げ正
+
+    autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH];
+    autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL];
+    autopwm[THR] = oldTHR;
+
+    autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
+    autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]);
+}
+
+void ConvertPWMintoRAD(float targetAngle[3]){
+    float ratio[2];
+
+    if(sbus.manualpwm[ELE] > trimpwm[ELE]){    
+        ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]);
+        targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
+    }else{
+        ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]);
+        targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];    
+    }
+
+    if(sbus.manualpwm[RUD] > trimpwm[RUD]){
+        ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]);        
+        targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
+    }else{
+        ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]);        
+        targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];    
+    }
+}
+
+inline float CalcRatio(float value, float trim, float limit){
+    return  (value - trim) / (limit - trim);
+}
+
+bool CheckSW_Up(Channel ch){
+    if(SWITCH_CHECK < sbus.manualpwm[ch]){
+        return true;
+    }else{
+        return false;
+    }
+}
+
+int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
+    if(value > max) return max;
+    if(value < min) return min;
+    return value;
+}
+
+
+
+/*---SBUS割り込み処理---*/
+
+//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
+//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
+void Update_PWM()
+{    
+    static int16_t pwm[8];
+    if(sbus.flg_ch_update == true){
+        switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
+            case Manual: 
+                for(uint8_t i=0;i<8;i++){
+                    pwm[i] = sbus.manualpwm[i];
+                }
+                oldTHR = sbus.manualpwm[THR];
+                //pc.printf("update_manual\r\n");
+                break;
+        
+            case Auto:
+                pwm[AIL] = sbus.manualpwm[AIL];
+                pwm[ELE] = autopwm[ELE];
+                pwm[THR] = autopwm[THR];
+                pwm[RUD] = autopwm[RUD];
+                pwm[Ch5] = sbus.manualpwm[Ch5];
+                pwm[Ch6] = sbus.manualpwm[Ch6];
+                pwm[Ch7] = sbus.manualpwm[Ch7];
+                pwm[Ch8] = sbus.manualpwm[Ch8];
+ 
+                //pc.printf("update_auto\r\n");
+                break;
+                
+            default:
+                for(uint8_t i=0;i<8;i++){
+                    pwm[i] = sbus.manualpwm[i];
+                } //pc.printf("update_manual\r\n");
+                break;
+        }
+    }else{
+        pc.printf("0\r\n");
+    }
+    sbus.flg_ch_update = false;   
+    //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);}
+    //pc.printf("\r\n");
+    Output_PWM(pwm);  
+}
+
+
+//pwmをサーボに出力する関数。
+void Output_PWM(int16_t pwm[8])
+{
+    servo1.pulsewidth_us(pwm[0]);
+    servo2.pulsewidth_us(pwm[1]);
+    servo3.pulsewidth_us(pwm[2]);
+    servo4.pulsewidth_us(pwm[3]);
+    servo5.pulsewidth_us(pwm[4]);
+    servo6.pulsewidth_us(pwm[5]);
+    servo7.pulsewidth_us(pwm[6]);
+    servo8.pulsewidth_us(pwm[7]);    
+}
+
+void ResetTrim(){
+    for(uint8_t i=0; i<4; i++){
+        trimpwm[i] = sbus.manualpwm[i];
+    }
+    pc.printf("reset PWM trim\r\n");
+}
+
+void SensingMPU(){
+    //static int16_t deltaT = 0, t_start = 0;
+    //t_start = t.read_us();
+    NVIC_DisableIRQ(USART1_IRQn);
+    if(!mpu9250.sensingAcGyMg()){
+         //pc.printf("failed\r\n");
+         return;
+    }
+    NVIC_EnableIRQ(USART1_IRQn);
+    mpu9250.calculatePostureAngle(nowAngle);
+    /*
+    deltaT = t.read_us() - t_start;
+    pc.printf("t:%d us, ",deltaT);
+    mpu9250.displayAngle();
+    */
+    
+    //mpu9250.sensingPostureAngle(nowAngle);
+}
+
+//デバッグ用
+void DebugPrint(){    
+
+    //static int16_t deltaT = 0, t_start = 0;
+    //t_start = t.read_us();
+    for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
+    pc.printf("\r\n");
+    //deltaT = t.read_us() - t_start;
+    //pc.printf("t:%d us, ",deltaT);
+    //pc.printf("\r\n");
+
+    //mpu9250.displayAngle();
+    //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]);
+    //pc.printf("\r\n");
+}
+