aaa
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
main.cpp@0:92024886c0be, 2017-08-01 (annotated)
- Committer:
- TUATBM
- Date:
- Tue Aug 01 12:27:13 2017 +0000
- Revision:
- 0:92024886c0be
- Child:
- 1:f31e17341659
??????????????????????; ????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TUATBM | 0:92024886c0be | 1 | #include "mbed.h" |
TUATBM | 0:92024886c0be | 2 | //#include "rtos.h" |
TUATBM | 0:92024886c0be | 3 | |
TUATBM | 0:92024886c0be | 4 | #include "math.h" |
TUATBM | 0:92024886c0be | 5 | #include "MPU9250.h" |
TUATBM | 0:92024886c0be | 6 | #include "BMP280.h" |
TUATBM | 0:92024886c0be | 7 | #include "SkipperSv2.h" |
TUATBM | 0:92024886c0be | 8 | #include "Estrela.h" |
TUATBM | 0:92024886c0be | 9 | #include "sbus.h" |
TUATBM | 0:92024886c0be | 10 | #include "pid.h" |
TUATBM | 0:92024886c0be | 11 | |
TUATBM | 0:92024886c0be | 12 | #define DEBUG_SEMIAUTO 1 |
TUATBM | 0:92024886c0be | 13 | #define DEBUG_PRINT_INLOOP 1 |
TUATBM | 0:92024886c0be | 14 | |
TUATBM | 0:92024886c0be | 15 | #define KP_ELE 2 |
TUATBM | 0:92024886c0be | 16 | #define KI_ELE 0 |
TUATBM | 0:92024886c0be | 17 | #define KD_ELE 0 |
TUATBM | 0:92024886c0be | 18 | #define KP_RUD 2 |
TUATBM | 0:92024886c0be | 19 | #define KI_RUD 0 |
TUATBM | 0:92024886c0be | 20 | #define KD_RUD 0 |
TUATBM | 0:92024886c0be | 21 | |
TUATBM | 0:92024886c0be | 22 | #define GAIN_CONTROLVALUE_TO_PWM 3 |
TUATBM | 0:92024886c0be | 23 | |
TUATBM | 0:92024886c0be | 24 | #define MAGBIAS_X 185.0 |
TUATBM | 0:92024886c0be | 25 | #define MAGBIAS_Y 220.0 |
TUATBM | 0:92024886c0be | 26 | #define MAGBIAS_Z -350.0 |
TUATBM | 0:92024886c0be | 27 | |
TUATBM | 0:92024886c0be | 28 | const uint16_t changeModeCount = 50; |
TUATBM | 0:92024886c0be | 29 | const int16_t lengthdivpwm = 320; |
TUATBM | 0:92024886c0be | 30 | |
TUATBM | 0:92024886c0be | 31 | const int16_t trim[4] = {0,14,-100,-10}; |
TUATBM | 0:92024886c0be | 32 | const float expMax[4] = {100,115,100,89}; |
TUATBM | 0:92024886c0be | 33 | const float expMin[4] = {100,47,100,110}; |
TUATBM | 0:92024886c0be | 34 | |
TUATBM | 0:92024886c0be | 35 | |
TUATBM | 0:92024886c0be | 36 | SBUS sbus(PA_9, PA_10); //SBUS |
TUATBM | 0:92024886c0be | 37 | |
TUATBM | 0:92024886c0be | 38 | PwmOut servo1(PC_6); // TIM3_CH1 |
TUATBM | 0:92024886c0be | 39 | PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 |
TUATBM | 0:92024886c0be | 40 | PwmOut servo3(PB_0); // TIM3_CH3 |
TUATBM | 0:92024886c0be | 41 | PwmOut servo4(PB_1); // TIM3_CH4 |
TUATBM | 0:92024886c0be | 42 | PwmOut servo5(PB_6); // TIM4_CH1 |
TUATBM | 0:92024886c0be | 43 | PwmOut servo6(PB_7); // TIM4_CH2 |
TUATBM | 0:92024886c0be | 44 | PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 |
TUATBM | 0:92024886c0be | 45 | PwmOut servo8(PB_9); // TIM4_CH4 |
TUATBM | 0:92024886c0be | 46 | |
TUATBM | 0:92024886c0be | 47 | Serial pc(PA_2, PA_3); |
TUATBM | 0:92024886c0be | 48 | |
TUATBM | 0:92024886c0be | 49 | DigitalOut led1(PA_0); //緑 |
TUATBM | 0:92024886c0be | 50 | DigitalOut led2(PA_1); //赤 |
TUATBM | 0:92024886c0be | 51 | DigitalOut led3(PB_4); |
TUATBM | 0:92024886c0be | 52 | DigitalOut led4(PB_5); |
TUATBM | 0:92024886c0be | 53 | |
TUATBM | 0:92024886c0be | 54 | InterruptIn switch2(PC_14); |
TUATBM | 0:92024886c0be | 55 | |
TUATBM | 0:92024886c0be | 56 | MPU9250 mpu9250(PC_9,PA_8,&pc); |
TUATBM | 0:92024886c0be | 57 | |
TUATBM | 0:92024886c0be | 58 | PID pid_ELE(KP_ELE,KI_ELE,KD_ELE); |
TUATBM | 0:92024886c0be | 59 | PID pid_RUD(KP_RUD,KI_RUD,KD_RUD); |
TUATBM | 0:92024886c0be | 60 | |
TUATBM | 0:92024886c0be | 61 | enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8}; |
TUATBM | 0:92024886c0be | 62 | enum Angle{ROLL, PITCH, YAW}; |
TUATBM | 0:92024886c0be | 63 | enum OperationMode{StartUp, SemiAuto}; |
TUATBM | 0:92024886c0be | 64 | enum OutputStatus{Manual, Auto}; |
TUATBM | 0:92024886c0be | 65 | |
TUATBM | 0:92024886c0be | 66 | static OutputStatus output_status = Manual; |
TUATBM | 0:92024886c0be | 67 | OperationMode operation_mode = StartUp; |
TUATBM | 0:92024886c0be | 68 | static int16_t autopwm[8] = {1500,1500,1000,1500}; |
TUATBM | 0:92024886c0be | 69 | |
TUATBM | 0:92024886c0be | 70 | static int16_t trimpwm[4] = {1500,1500,1000,1500}; |
TUATBM | 0:92024886c0be | 71 | int16_t maxpwm[4] = {1820,1820,1820,1820}; |
TUATBM | 0:92024886c0be | 72 | int16_t minpwm[4] = {1180,1180,1180,1180}; |
TUATBM | 0:92024886c0be | 73 | int16_t oldTHR = 1000; |
TUATBM | 0:92024886c0be | 74 | |
TUATBM | 0:92024886c0be | 75 | static float nowAngle[3] = {0,0,0}; |
TUATBM | 0:92024886c0be | 76 | const float trimAngle[3] = {0,0,0}; |
TUATBM | 0:92024886c0be | 77 | const float maxAngle[2] = {90, 90}; |
TUATBM | 0:92024886c0be | 78 | const float minAngle[2] = {-90, -90}; |
TUATBM | 0:92024886c0be | 79 | |
TUATBM | 0:92024886c0be | 80 | Timer t; |
TUATBM | 0:92024886c0be | 81 | |
TUATBM | 0:92024886c0be | 82 | //IWDG_HandleTypeDef hiwdg; |
TUATBM | 0:92024886c0be | 83 | |
TUATBM | 0:92024886c0be | 84 | /*-----関数のプロトタイプ宣言-----*/ |
TUATBM | 0:92024886c0be | 85 | void setup(); |
TUATBM | 0:92024886c0be | 86 | void loop(); |
TUATBM | 0:92024886c0be | 87 | |
TUATBM | 0:92024886c0be | 88 | void Init_PWM(); |
TUATBM | 0:92024886c0be | 89 | void Init_servo(); //サーボ初期化 |
TUATBM | 0:92024886c0be | 90 | void Init_sbus(); //SBUS初期化 |
TUATBM | 0:92024886c0be | 91 | void Init_sensors(); |
TUATBM | 0:92024886c0be | 92 | void DisplayClock(); //クロック状態確認 |
TUATBM | 0:92024886c0be | 93 | |
TUATBM | 0:92024886c0be | 94 | void SensingMPU(); |
TUATBM | 0:92024886c0be | 95 | void UpdateTargetAngle(float targetAngle[3]); |
TUATBM | 0:92024886c0be | 96 | void CalculateControlValue(float targetAngle[3], float controlValue[3]); |
TUATBM | 0:92024886c0be | 97 | void UpdateAutoPWM(float controlValue[3]); |
TUATBM | 0:92024886c0be | 98 | void ConvertPWMintoRAD(float targetAngle[3]); |
TUATBM | 0:92024886c0be | 99 | inline float CalcRatio(float value, float trim, float limit); |
TUATBM | 0:92024886c0be | 100 | bool CheckSW_Up(Channel ch); |
TUATBM | 0:92024886c0be | 101 | int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); |
TUATBM | 0:92024886c0be | 102 | |
TUATBM | 0:92024886c0be | 103 | //sbus割り込み |
TUATBM | 0:92024886c0be | 104 | void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 |
TUATBM | 0:92024886c0be | 105 | void Output_PWM(int16_t pwm[8]); //pwmをサーボへ出力 |
TUATBM | 0:92024886c0be | 106 | |
TUATBM | 0:92024886c0be | 107 | //switch2割り込み |
TUATBM | 0:92024886c0be | 108 | void ResetTrim(); |
TUATBM | 0:92024886c0be | 109 | |
TUATBM | 0:92024886c0be | 110 | //デバッグ用 |
TUATBM | 0:92024886c0be | 111 | void DebugPrint(); |
TUATBM | 0:92024886c0be | 112 | |
TUATBM | 0:92024886c0be | 113 | /*---関数のプロトタイプ宣言終わり---*/ |
TUATBM | 0:92024886c0be | 114 | |
TUATBM | 0:92024886c0be | 115 | int main() |
TUATBM | 0:92024886c0be | 116 | { |
TUATBM | 0:92024886c0be | 117 | setup(); |
TUATBM | 0:92024886c0be | 118 | while(1){ |
TUATBM | 0:92024886c0be | 119 | loop(); |
TUATBM | 0:92024886c0be | 120 | } |
TUATBM | 0:92024886c0be | 121 | } |
TUATBM | 0:92024886c0be | 122 | |
TUATBM | 0:92024886c0be | 123 | void setup(){ |
TUATBM | 0:92024886c0be | 124 | led1 = 1; |
TUATBM | 0:92024886c0be | 125 | led2 = 1; |
TUATBM | 0:92024886c0be | 126 | led3 = 0; |
TUATBM | 0:92024886c0be | 127 | led4 = 0; |
TUATBM | 0:92024886c0be | 128 | |
TUATBM | 0:92024886c0be | 129 | pc.baud(115200); |
TUATBM | 0:92024886c0be | 130 | Init_PWM(); |
TUATBM | 0:92024886c0be | 131 | Init_servo(); |
TUATBM | 0:92024886c0be | 132 | Init_sbus(); |
TUATBM | 0:92024886c0be | 133 | Init_sensors(); |
TUATBM | 0:92024886c0be | 134 | switch2.rise(ResetTrim); |
TUATBM | 0:92024886c0be | 135 | t.start(); |
TUATBM | 0:92024886c0be | 136 | DisplayClock(); |
TUATBM | 0:92024886c0be | 137 | |
TUATBM | 0:92024886c0be | 138 | led1 = 0; |
TUATBM | 0:92024886c0be | 139 | led2 = 0; |
TUATBM | 0:92024886c0be | 140 | led3 = 0; |
TUATBM | 0:92024886c0be | 141 | led4 = 0; |
TUATBM | 0:92024886c0be | 142 | |
TUATBM | 0:92024886c0be | 143 | pc.printf("All initialized\r\n"); |
TUATBM | 0:92024886c0be | 144 | |
TUATBM | 0:92024886c0be | 145 | mpu9250.sensingAcGyMg(); |
TUATBM | 0:92024886c0be | 146 | mpu9250.calculatePostureAngle(nowAngle); |
TUATBM | 0:92024886c0be | 147 | } |
TUATBM | 0:92024886c0be | 148 | |
TUATBM | 0:92024886c0be | 149 | void loop(){ |
TUATBM | 0:92024886c0be | 150 | static float targetAngle[3], controlValue[2]; |
TUATBM | 0:92024886c0be | 151 | |
TUATBM | 0:92024886c0be | 152 | SensingMPU(); |
TUATBM | 0:92024886c0be | 153 | UpdateTargetAngle(targetAngle); |
TUATBM | 0:92024886c0be | 154 | CalculateControlValue(targetAngle, controlValue); |
TUATBM | 0:92024886c0be | 155 | UpdateAutoPWM(controlValue); |
TUATBM | 0:92024886c0be | 156 | |
TUATBM | 0:92024886c0be | 157 | #if DEBUG_PRINT_INLOOP |
TUATBM | 0:92024886c0be | 158 | DebugPrint(); |
TUATBM | 0:92024886c0be | 159 | #endif |
TUATBM | 0:92024886c0be | 160 | } |
TUATBM | 0:92024886c0be | 161 | |
TUATBM | 0:92024886c0be | 162 | |
TUATBM | 0:92024886c0be | 163 | |
TUATBM | 0:92024886c0be | 164 | //サーボ初期化関数 |
TUATBM | 0:92024886c0be | 165 | void Init_servo() |
TUATBM | 0:92024886c0be | 166 | { |
TUATBM | 0:92024886c0be | 167 | |
TUATBM | 0:92024886c0be | 168 | servo1.period_ms(14); |
TUATBM | 0:92024886c0be | 169 | servo1.pulsewidth_us(trimpwm[AIL]); |
TUATBM | 0:92024886c0be | 170 | |
TUATBM | 0:92024886c0be | 171 | servo2.period_ms(14); |
TUATBM | 0:92024886c0be | 172 | servo2.pulsewidth_us(trimpwm[ELE]); |
TUATBM | 0:92024886c0be | 173 | |
TUATBM | 0:92024886c0be | 174 | servo3.period_ms(14); |
TUATBM | 0:92024886c0be | 175 | servo3.pulsewidth_us(trimpwm[THR]); |
TUATBM | 0:92024886c0be | 176 | |
TUATBM | 0:92024886c0be | 177 | servo4.period_ms(14); |
TUATBM | 0:92024886c0be | 178 | servo4.pulsewidth_us(trimpwm[RUD]); |
TUATBM | 0:92024886c0be | 179 | |
TUATBM | 0:92024886c0be | 180 | servo5.period_ms(14); |
TUATBM | 0:92024886c0be | 181 | servo5.pulsewidth_us(1500); |
TUATBM | 0:92024886c0be | 182 | |
TUATBM | 0:92024886c0be | 183 | servo6.period_ms(14); |
TUATBM | 0:92024886c0be | 184 | servo6.pulsewidth_us(1500); |
TUATBM | 0:92024886c0be | 185 | |
TUATBM | 0:92024886c0be | 186 | servo7.period_ms(14); |
TUATBM | 0:92024886c0be | 187 | servo7.pulsewidth_us(1500); |
TUATBM | 0:92024886c0be | 188 | |
TUATBM | 0:92024886c0be | 189 | servo8.period_ms(14); |
TUATBM | 0:92024886c0be | 190 | servo8.pulsewidth_us(1500); |
TUATBM | 0:92024886c0be | 191 | |
TUATBM | 0:92024886c0be | 192 | pc.printf("servo initialized\r\n"); |
TUATBM | 0:92024886c0be | 193 | } |
TUATBM | 0:92024886c0be | 194 | |
TUATBM | 0:92024886c0be | 195 | //Sbusを初期化する関数 |
TUATBM | 0:92024886c0be | 196 | void Init_sbus(){ |
TUATBM | 0:92024886c0be | 197 | sbus.initialize(); |
TUATBM | 0:92024886c0be | 198 | sbus.setLastfuncPoint(Update_PWM); |
TUATBM | 0:92024886c0be | 199 | sbus.startInterrupt(); |
TUATBM | 0:92024886c0be | 200 | } |
TUATBM | 0:92024886c0be | 201 | |
TUATBM | 0:92024886c0be | 202 | void Init_sensors(){ |
TUATBM | 0:92024886c0be | 203 | if(!mpu9250.Initialize()){ |
TUATBM | 0:92024886c0be | 204 | pc.printf("failed initialize\r\n"); |
TUATBM | 0:92024886c0be | 205 | while(1); |
TUATBM | 0:92024886c0be | 206 | } |
TUATBM | 0:92024886c0be | 207 | mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z); |
TUATBM | 0:92024886c0be | 208 | } |
TUATBM | 0:92024886c0be | 209 | |
TUATBM | 0:92024886c0be | 210 | void Init_PWM(){ |
TUATBM | 0:92024886c0be | 211 | for (uint8_t i = 0; i < 4; ++i){ |
TUATBM | 0:92024886c0be | 212 | trimpwm[i] = 1500 + trim[i]; |
TUATBM | 0:92024886c0be | 213 | maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100)); |
TUATBM | 0:92024886c0be | 214 | minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100)); |
TUATBM | 0:92024886c0be | 215 | } |
TUATBM | 0:92024886c0be | 216 | pc.printf("PWM initialized\r\n"); |
TUATBM | 0:92024886c0be | 217 | } |
TUATBM | 0:92024886c0be | 218 | |
TUATBM | 0:92024886c0be | 219 | void DisplayClock(){ |
TUATBM | 0:92024886c0be | 220 | pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); |
TUATBM | 0:92024886c0be | 221 | pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); |
TUATBM | 0:92024886c0be | 222 | pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); |
TUATBM | 0:92024886c0be | 223 | pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); |
TUATBM | 0:92024886c0be | 224 | pc.printf("\r\n"); |
TUATBM | 0:92024886c0be | 225 | } |
TUATBM | 0:92024886c0be | 226 | |
TUATBM | 0:92024886c0be | 227 | void UpdateTargetAngle(float targetAngle[3]){ |
TUATBM | 0:92024886c0be | 228 | static uint16_t count_op = 0, count_out = 0; |
TUATBM | 0:92024886c0be | 229 | |
TUATBM | 0:92024886c0be | 230 | #if DEBUG_SEMIAUTO |
TUATBM | 0:92024886c0be | 231 | switch(operation_mode){ |
TUATBM | 0:92024886c0be | 232 | case StartUp: |
TUATBM | 0:92024886c0be | 233 | if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){ |
TUATBM | 0:92024886c0be | 234 | count_op++; |
TUATBM | 0:92024886c0be | 235 | if(count_op > changeModeCount){ |
TUATBM | 0:92024886c0be | 236 | operation_mode = SemiAuto; |
TUATBM | 0:92024886c0be | 237 | pc.printf("Goto SemiAuto mode\r\n"); |
TUATBM | 0:92024886c0be | 238 | count_op = 0; |
TUATBM | 0:92024886c0be | 239 | } |
TUATBM | 0:92024886c0be | 240 | }else count_op = 0; |
TUATBM | 0:92024886c0be | 241 | break; |
TUATBM | 0:92024886c0be | 242 | |
TUATBM | 0:92024886c0be | 243 | case SemiAuto: |
TUATBM | 0:92024886c0be | 244 | /* 大会用では以下のif文を入れてoperation_modeを変える |
TUATBM | 0:92024886c0be | 245 | if(CheckSW_Up(Ch6)){ |
TUATBM | 0:92024886c0be | 246 | count_op++; |
TUATBM | 0:92024886c0be | 247 | if(count_op>changeModeCount){ |
TUATBM | 0:92024886c0be | 248 | output_status = XXX; |
TUATBM | 0:92024886c0be | 249 | led2 = 0; |
TUATBM | 0:92024886c0be | 250 | pc.printf("Goto XXX mode\r\n"); |
TUATBM | 0:92024886c0be | 251 | count_op = 0; |
TUATBM | 0:92024886c0be | 252 | }else count_op = 0; |
TUATBM | 0:92024886c0be | 253 | ConvertPWMintoRAD(targetAngle); |
TUATBM | 0:92024886c0be | 254 | } |
TUATBM | 0:92024886c0be | 255 | */ |
TUATBM | 0:92024886c0be | 256 | ConvertPWMintoRAD(targetAngle); |
TUATBM | 0:92024886c0be | 257 | break; |
TUATBM | 0:92024886c0be | 258 | |
TUATBM | 0:92024886c0be | 259 | default: |
TUATBM | 0:92024886c0be | 260 | operation_mode = SemiAuto; |
TUATBM | 0:92024886c0be | 261 | break; |
TUATBM | 0:92024886c0be | 262 | } |
TUATBM | 0:92024886c0be | 263 | |
TUATBM | 0:92024886c0be | 264 | if(CheckSW_Up(Ch5)){ |
TUATBM | 0:92024886c0be | 265 | output_status = Auto; |
TUATBM | 0:92024886c0be | 266 | led1 = 1; |
TUATBM | 0:92024886c0be | 267 | }else{ |
TUATBM | 0:92024886c0be | 268 | output_status = Manual; |
TUATBM | 0:92024886c0be | 269 | led1 = 0; |
TUATBM | 0:92024886c0be | 270 | } |
TUATBM | 0:92024886c0be | 271 | #endif |
TUATBM | 0:92024886c0be | 272 | } |
TUATBM | 0:92024886c0be | 273 | |
TUATBM | 0:92024886c0be | 274 | void CalculateControlValue(float targetAngle[3], float controlValue[3]){ |
TUATBM | 0:92024886c0be | 275 | static int t_last; |
TUATBM | 0:92024886c0be | 276 | int t_now; |
TUATBM | 0:92024886c0be | 277 | float dt; |
TUATBM | 0:92024886c0be | 278 | |
TUATBM | 0:92024886c0be | 279 | t_now = t.read_us(); |
TUATBM | 0:92024886c0be | 280 | dt = (float)((t_now - t_last)/1000000.0f) ; |
TUATBM | 0:92024886c0be | 281 | t_last = t_now; |
TUATBM | 0:92024886c0be | 282 | |
TUATBM | 0:92024886c0be | 283 | controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); |
TUATBM | 0:92024886c0be | 284 | controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); |
TUATBM | 0:92024886c0be | 285 | } |
TUATBM | 0:92024886c0be | 286 | |
TUATBM | 0:92024886c0be | 287 | void UpdateAutoPWM(float controlValue[3]){ |
TUATBM | 0:92024886c0be | 288 | int16_t addpwm[2]; //-500~500 |
TUATBM | 0:92024886c0be | 289 | addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正 レバー:右回転正 |
TUATBM | 0:92024886c0be | 290 | addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ:機首下げ正 レバー:機首上げ正 |
TUATBM | 0:92024886c0be | 291 | |
TUATBM | 0:92024886c0be | 292 | autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH]; |
TUATBM | 0:92024886c0be | 293 | autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL]; |
TUATBM | 0:92024886c0be | 294 | autopwm[THR] = oldTHR; |
TUATBM | 0:92024886c0be | 295 | |
TUATBM | 0:92024886c0be | 296 | autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); |
TUATBM | 0:92024886c0be | 297 | autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]); |
TUATBM | 0:92024886c0be | 298 | } |
TUATBM | 0:92024886c0be | 299 | |
TUATBM | 0:92024886c0be | 300 | void ConvertPWMintoRAD(float targetAngle[3]){ |
TUATBM | 0:92024886c0be | 301 | float ratio[2]; |
TUATBM | 0:92024886c0be | 302 | |
TUATBM | 0:92024886c0be | 303 | if(sbus.manualpwm[ELE] > trimpwm[ELE]){ |
TUATBM | 0:92024886c0be | 304 | ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]); |
TUATBM | 0:92024886c0be | 305 | targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH]; |
TUATBM | 0:92024886c0be | 306 | }else{ |
TUATBM | 0:92024886c0be | 307 | ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]); |
TUATBM | 0:92024886c0be | 308 | targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH]; |
TUATBM | 0:92024886c0be | 309 | } |
TUATBM | 0:92024886c0be | 310 | |
TUATBM | 0:92024886c0be | 311 | if(sbus.manualpwm[RUD] > trimpwm[RUD]){ |
TUATBM | 0:92024886c0be | 312 | ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]); |
TUATBM | 0:92024886c0be | 313 | targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL]; |
TUATBM | 0:92024886c0be | 314 | }else{ |
TUATBM | 0:92024886c0be | 315 | ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]); |
TUATBM | 0:92024886c0be | 316 | targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL]; |
TUATBM | 0:92024886c0be | 317 | } |
TUATBM | 0:92024886c0be | 318 | } |
TUATBM | 0:92024886c0be | 319 | |
TUATBM | 0:92024886c0be | 320 | inline float CalcRatio(float value, float trim, float limit){ |
TUATBM | 0:92024886c0be | 321 | return (value - trim) / (limit - trim); |
TUATBM | 0:92024886c0be | 322 | } |
TUATBM | 0:92024886c0be | 323 | |
TUATBM | 0:92024886c0be | 324 | bool CheckSW_Up(Channel ch){ |
TUATBM | 0:92024886c0be | 325 | if(SWITCH_CHECK < sbus.manualpwm[ch]){ |
TUATBM | 0:92024886c0be | 326 | return true; |
TUATBM | 0:92024886c0be | 327 | }else{ |
TUATBM | 0:92024886c0be | 328 | return false; |
TUATBM | 0:92024886c0be | 329 | } |
TUATBM | 0:92024886c0be | 330 | } |
TUATBM | 0:92024886c0be | 331 | |
TUATBM | 0:92024886c0be | 332 | int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){ |
TUATBM | 0:92024886c0be | 333 | if(value > max) return max; |
TUATBM | 0:92024886c0be | 334 | if(value < min) return min; |
TUATBM | 0:92024886c0be | 335 | return value; |
TUATBM | 0:92024886c0be | 336 | } |
TUATBM | 0:92024886c0be | 337 | |
TUATBM | 0:92024886c0be | 338 | |
TUATBM | 0:92024886c0be | 339 | |
TUATBM | 0:92024886c0be | 340 | /*---SBUS割り込み処理---*/ |
TUATBM | 0:92024886c0be | 341 | |
TUATBM | 0:92024886c0be | 342 | //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) |
TUATBM | 0:92024886c0be | 343 | //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) |
TUATBM | 0:92024886c0be | 344 | void Update_PWM() |
TUATBM | 0:92024886c0be | 345 | { |
TUATBM | 0:92024886c0be | 346 | static int16_t pwm[8]; |
TUATBM | 0:92024886c0be | 347 | if(sbus.flg_ch_update == true){ |
TUATBM | 0:92024886c0be | 348 | switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 |
TUATBM | 0:92024886c0be | 349 | case Manual: |
TUATBM | 0:92024886c0be | 350 | for(uint8_t i=0;i<8;i++){ |
TUATBM | 0:92024886c0be | 351 | pwm[i] = sbus.manualpwm[i]; |
TUATBM | 0:92024886c0be | 352 | } |
TUATBM | 0:92024886c0be | 353 | oldTHR = sbus.manualpwm[THR]; |
TUATBM | 0:92024886c0be | 354 | //pc.printf("update_manual\r\n"); |
TUATBM | 0:92024886c0be | 355 | break; |
TUATBM | 0:92024886c0be | 356 | |
TUATBM | 0:92024886c0be | 357 | case Auto: |
TUATBM | 0:92024886c0be | 358 | pwm[AIL] = sbus.manualpwm[AIL]; |
TUATBM | 0:92024886c0be | 359 | pwm[ELE] = autopwm[ELE]; |
TUATBM | 0:92024886c0be | 360 | pwm[THR] = autopwm[THR]; |
TUATBM | 0:92024886c0be | 361 | pwm[RUD] = autopwm[RUD]; |
TUATBM | 0:92024886c0be | 362 | pwm[Ch5] = sbus.manualpwm[Ch5]; |
TUATBM | 0:92024886c0be | 363 | pwm[Ch6] = sbus.manualpwm[Ch6]; |
TUATBM | 0:92024886c0be | 364 | pwm[Ch7] = sbus.manualpwm[Ch7]; |
TUATBM | 0:92024886c0be | 365 | pwm[Ch8] = sbus.manualpwm[Ch8]; |
TUATBM | 0:92024886c0be | 366 | |
TUATBM | 0:92024886c0be | 367 | //pc.printf("update_auto\r\n"); |
TUATBM | 0:92024886c0be | 368 | break; |
TUATBM | 0:92024886c0be | 369 | |
TUATBM | 0:92024886c0be | 370 | default: |
TUATBM | 0:92024886c0be | 371 | for(uint8_t i=0;i<8;i++){ |
TUATBM | 0:92024886c0be | 372 | pwm[i] = sbus.manualpwm[i]; |
TUATBM | 0:92024886c0be | 373 | } //pc.printf("update_manual\r\n"); |
TUATBM | 0:92024886c0be | 374 | break; |
TUATBM | 0:92024886c0be | 375 | } |
TUATBM | 0:92024886c0be | 376 | }else{ |
TUATBM | 0:92024886c0be | 377 | pc.printf("0\r\n"); |
TUATBM | 0:92024886c0be | 378 | } |
TUATBM | 0:92024886c0be | 379 | sbus.flg_ch_update = false; |
TUATBM | 0:92024886c0be | 380 | //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);} |
TUATBM | 0:92024886c0be | 381 | //pc.printf("\r\n"); |
TUATBM | 0:92024886c0be | 382 | Output_PWM(pwm); |
TUATBM | 0:92024886c0be | 383 | } |
TUATBM | 0:92024886c0be | 384 | |
TUATBM | 0:92024886c0be | 385 | |
TUATBM | 0:92024886c0be | 386 | //pwmをサーボに出力する関数。 |
TUATBM | 0:92024886c0be | 387 | void Output_PWM(int16_t pwm[8]) |
TUATBM | 0:92024886c0be | 388 | { |
TUATBM | 0:92024886c0be | 389 | servo1.pulsewidth_us(pwm[0]); |
TUATBM | 0:92024886c0be | 390 | servo2.pulsewidth_us(pwm[1]); |
TUATBM | 0:92024886c0be | 391 | servo3.pulsewidth_us(pwm[2]); |
TUATBM | 0:92024886c0be | 392 | servo4.pulsewidth_us(pwm[3]); |
TUATBM | 0:92024886c0be | 393 | servo5.pulsewidth_us(pwm[4]); |
TUATBM | 0:92024886c0be | 394 | servo6.pulsewidth_us(pwm[5]); |
TUATBM | 0:92024886c0be | 395 | servo7.pulsewidth_us(pwm[6]); |
TUATBM | 0:92024886c0be | 396 | servo8.pulsewidth_us(pwm[7]); |
TUATBM | 0:92024886c0be | 397 | } |
TUATBM | 0:92024886c0be | 398 | |
TUATBM | 0:92024886c0be | 399 | void ResetTrim(){ |
TUATBM | 0:92024886c0be | 400 | for(uint8_t i=0; i<4; i++){ |
TUATBM | 0:92024886c0be | 401 | trimpwm[i] = sbus.manualpwm[i]; |
TUATBM | 0:92024886c0be | 402 | } |
TUATBM | 0:92024886c0be | 403 | pc.printf("reset PWM trim\r\n"); |
TUATBM | 0:92024886c0be | 404 | } |
TUATBM | 0:92024886c0be | 405 | |
TUATBM | 0:92024886c0be | 406 | void SensingMPU(){ |
TUATBM | 0:92024886c0be | 407 | //static int16_t deltaT = 0, t_start = 0; |
TUATBM | 0:92024886c0be | 408 | //t_start = t.read_us(); |
TUATBM | 0:92024886c0be | 409 | NVIC_DisableIRQ(USART1_IRQn); |
TUATBM | 0:92024886c0be | 410 | if(!mpu9250.sensingAcGyMg()){ |
TUATBM | 0:92024886c0be | 411 | //pc.printf("failed\r\n"); |
TUATBM | 0:92024886c0be | 412 | return; |
TUATBM | 0:92024886c0be | 413 | } |
TUATBM | 0:92024886c0be | 414 | NVIC_EnableIRQ(USART1_IRQn); |
TUATBM | 0:92024886c0be | 415 | mpu9250.calculatePostureAngle(nowAngle); |
TUATBM | 0:92024886c0be | 416 | /* |
TUATBM | 0:92024886c0be | 417 | deltaT = t.read_us() - t_start; |
TUATBM | 0:92024886c0be | 418 | pc.printf("t:%d us, ",deltaT); |
TUATBM | 0:92024886c0be | 419 | mpu9250.displayAngle(); |
TUATBM | 0:92024886c0be | 420 | */ |
TUATBM | 0:92024886c0be | 421 | |
TUATBM | 0:92024886c0be | 422 | //mpu9250.sensingPostureAngle(nowAngle); |
TUATBM | 0:92024886c0be | 423 | } |
TUATBM | 0:92024886c0be | 424 | |
TUATBM | 0:92024886c0be | 425 | //デバッグ用 |
TUATBM | 0:92024886c0be | 426 | void DebugPrint(){ |
TUATBM | 0:92024886c0be | 427 | |
TUATBM | 0:92024886c0be | 428 | //static int16_t deltaT = 0, t_start = 0; |
TUATBM | 0:92024886c0be | 429 | //t_start = t.read_us(); |
TUATBM | 0:92024886c0be | 430 | for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); |
TUATBM | 0:92024886c0be | 431 | pc.printf("\r\n"); |
TUATBM | 0:92024886c0be | 432 | //deltaT = t.read_us() - t_start; |
TUATBM | 0:92024886c0be | 433 | //pc.printf("t:%d us, ",deltaT); |
TUATBM | 0:92024886c0be | 434 | //pc.printf("\r\n"); |
TUATBM | 0:92024886c0be | 435 | |
TUATBM | 0:92024886c0be | 436 | //mpu9250.displayAngle(); |
TUATBM | 0:92024886c0be | 437 | //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]); |
TUATBM | 0:92024886c0be | 438 | //pc.printf("\r\n"); |
TUATBM | 0:92024886c0be | 439 | } |
TUATBM | 0:92024886c0be | 440 |