aaa
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
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"); +} +