航空研究会 / Mbed 2 deprecated servo_neutral_check

Dependencies:   mbed MPU6050_2 SDFileSystem3 HCSR04_2

main.cpp@46:ebbe73af073d, 2018-12-22 (annotated)

Committer:
HARUKIDELTA
Date:
Sat Dec 22 06:27:25 2018 +0000
Revision:
46:ebbe73af073d
Parent:
45:eebdf6fa7b15
Child:
47:b3d78207b08f
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:17f575135219 1 //mbed
HARUKIDELTA 0:17f575135219 2 #include "mbed.h"
HARUKIDELTA 0:17f575135219 3 #include "FATFileSystem.h"
HARUKIDELTA 0:17f575135219 4 #include "SDFileSystem.h"
HARUKIDELTA 0:17f575135219 5 //C
HARUKIDELTA 0:17f575135219 6 #include "math.h"
HARUKIDELTA 0:17f575135219 7 //sensor
HARUKIDELTA 0:17f575135219 8 #include "MPU6050_DMP6.h"
HARUKIDELTA 0:17f575135219 9 //#include "MPU9250.h"
HARUKIDELTA 0:17f575135219 10 //#include "BMP280.h"
HARUKIDELTA 0:17f575135219 11 #include "hcsr04.h"
HARUKIDELTA 0:17f575135219 12 //device
HARUKIDELTA 0:17f575135219 13 #include "sbus.h"
HARUKIDELTA 0:17f575135219 14 //config
HARUKIDELTA 0:17f575135219 15 #include "SkipperSv2.h"
HARUKIDELTA 0:17f575135219 16 #include "falfalla.h"
HARUKIDELTA 0:17f575135219 17 //other
HARUKIDELTA 0:17f575135219 18 #include "pid.h"
HARUKIDELTA 0:17f575135219 19
HARUKIDELTA 0:17f575135219 20 #define DEBUG_SEMIAUTO 0
HARUKIDELTA 0:17f575135219 21 #define DEBUG_PRINT_INLOOP 1
HARUKIDELTA 0:17f575135219 22
TUATBM 9:f6367b7fd7be 23 #define KP_ELE 15.0 //2.0
HARUKIDELTA 0:17f575135219 24 #define KI_ELE 0.0
TUATBM 9:f6367b7fd7be 25 #define KD_ELE 0.0 //0/0
HARUKIDELTA 0:17f575135219 26 #define KP_RUD 3.0
HARUKIDELTA 0:17f575135219 27 #define KI_RUD 0.0
HARUKIDELTA 0:17f575135219 28 #define KD_RUD 0.0
HARUKIDELTA 2:23daa5fa28b4 29 #define KP_AIL 0.1
HARUKIDELTA 2:23daa5fa28b4 30 #define KI_AIL 0.2
HARUKIDELTA 2:23daa5fa28b4 31 #define KD_AIL 0.2
taknokolat 1:f383708a5a52 32
HARUKIDELTA 2:23daa5fa28b4 33 //#define g_AIL_L_Ratio_rightloop 0.5
HARUKIDELTA 0:17f575135219 34
HARUKIDELTA 0:17f575135219 35 #define GAIN_CONTROLVALUE_TO_PWM 3.0
HARUKIDELTA 0:17f575135219 36
HARUKIDELTA 0:17f575135219 37 #define RIGHT_ROLL -12.0
TUATBM 9:f6367b7fd7be 38 #define RIGHT_PITCH -10.0 //5.0
HARUKIDELTA 0:17f575135219 39 #define LEFT_ROLL 12.0
HARUKIDELTA 0:17f575135219 40 #define LEFT_PITCH -5.0
HARUKIDELTA 0:17f575135219 41 #define STRAIGHT_ROLL 4.0
HARUKIDELTA 0:17f575135219 42 #define STRAIGHT_PITCH 3.0
HARUKIDELTA 0:17f575135219 43 #define TAKEOFF_THR 0.8
HARUKIDELTA 0:17f575135219 44 #define LOOP_THR 0.6
HARUKIDELTA 0:17f575135219 45
HARUKIDELTA 13:77b13d5f4d78 46 //#define g_rightloopRUD 1500
HARUKIDELTA 2:23daa5fa28b4 47
HARUKIDELTA 0:17f575135219 48 #define RIGHT_ROLL_SHORT -12.0
HARUKIDELTA 0:17f575135219 49 #define RIGHT_PITCH_SHORT -5.0
HARUKIDELTA 0:17f575135219 50 #define LEFT_ROLL_SHORT 12.0
HARUKIDELTA 0:17f575135219 51 #define LEFT_PITCH_SHORT -5.0
HARUKIDELTA 39:b8d5be233b70 52 #define RIGHTLOOPROLL_APPROACH -17.0
HARUKIDELTA 39:b8d5be233b70 53 #define LEFTLOOPROLL_APPROACH 16.0
HARUKIDELTA 42:74d72339a8a8 54 #define RIGHTLOOPPITCH_APPROACH -15.0
HARUKIDELTA 42:74d72339a8a8 55 #define LEFTLOOPPITCH_APPROACH -13.0
HARUKIDELTA 0:17f575135219 56
HARUKIDELTA 27:61876b34ded4 57 #define rightloopROLL2 -10.0
HARUKIDELTA 27:61876b34ded4 58
HARUKIDELTA 17:55249ea37dff 59 /*#define rightloopRUD 1300 //1250
HARUKIDELTA 17:55249ea37dff 60 #define rightloopshortRUD 1250
HARUKIDELTA 17:55249ea37dff 61 #define leftloopRUD 1500
HARUKIDELTA 17:55249ea37dff 62 #define leftloopshortRUD 1500
HARUKIDELTA 17:55249ea37dff 63 #define glideloopRUD 1300
HARUKIDELTA 17:55249ea37dff 64 */
TUATBM 9:f6367b7fd7be 65 #define AIL_R_correctionrightloop 0
HARUKIDELTA 18:cce82f3374fc 66 #define AIL_L_correctionrightloop 0
HARUKIDELTA 3:206b17251f5b 67 #define AIL_L_correctionrightloopshort 0
HARUKIDELTA 3:206b17251f5b 68 #define AIL_L_correctionleftloop -0
HARUKIDELTA 3:206b17251f5b 69 #define AIL_L_correctionleftloopshort 0
HARUKIDELTA 3:206b17251f5b 70
HARUKIDELTA 19:0955311b0db6 71
HARUKIDELTA 8:66bba39b95a9 72 #define RIGHTLOOP_RUD 1250
HARUKIDELTA 8:66bba39b95a9 73 #define RIGHTLOOPSHORT_RUD 1250
HARUKIDELTA 8:66bba39b95a9 74 #define LEFTLOOP_RUD 1500
HARUKIDELTA 8:66bba39b95a9 75 #define LEFTLOOPSHORT_RUD 1500
HARUKIDELTA 39:b8d5be233b70 76 #define GLIDELOOP_RUD 1300
HARUKIDELTA 39:b8d5be233b70 77 #define RIGHTLOOPRUD_APPROACH 1500
HARUKIDELTA 39:b8d5be233b70 78 #define LEFTLOOPRUD_APPROACH 1500
HARUKIDELTA 39:b8d5be233b70 79
HARUKIDELTA 8:66bba39b95a9 80 #define AIL_L_CORRECTION_RIGHTLOOP 0
HARUKIDELTA 8:66bba39b95a9 81 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
HARUKIDELTA 8:66bba39b95a9 82 #define AIL_L_CORRECTION_LEFTLOOP 0
HARUKIDELTA 8:66bba39b95a9 83 #define AIL_L_CORRECTION_LEFTLOOPSHORT 0
HARUKIDELTA 3:206b17251f5b 84
HARUKIDELTA 0:17f575135219 85 #define GLIDE_ROLL -12.0
HARUKIDELTA 0:17f575135219 86 #define GLIDE_PITCH -3.0
HARUKIDELTA 0:17f575135219 87
HARUKIDELTA 3:206b17251f5b 88
HARUKIDELTA 3:206b17251f5b 89 #define AIL_L_RatioRising 0.5
HARUKIDELTA 3:206b17251f5b 90 #define AIL_L_RatioDescent 2
HARUKIDELTA 3:206b17251f5b 91
HARUKIDELTA 0:17f575135219 92 //コンパスキャリブレーション
HARUKIDELTA 0:17f575135219 93 //SkipperS2基板
HARUKIDELTA 0:17f575135219 94 /*
HARUKIDELTA 0:17f575135219 95 #define MAGBIAS_X -35.0
HARUKIDELTA 0:17f575135219 96 #define MAGBIAS_Y 535.0
HARUKIDELTA 0:17f575135219 97 #define MAGBIAS_Z -50.0
HARUKIDELTA 0:17f575135219 98 */
HARUKIDELTA 0:17f575135219 99 //S2v2 1番基板
HARUKIDELTA 0:17f575135219 100 #define MAGBIAS_X 395.0
HARUKIDELTA 0:17f575135219 101 #define MAGBIAS_Y 505.0
HARUKIDELTA 0:17f575135219 102 #define MAGBIAS_Z -725.0
HARUKIDELTA 0:17f575135219 103 //S2v2 2番基板
HARUKIDELTA 0:17f575135219 104 /*
HARUKIDELTA 0:17f575135219 105 #define MAGBIAS_X 185.0
HARUKIDELTA 0:17f575135219 106 #define MAGBIAS_Y 220.0
HARUKIDELTA 0:17f575135219 107 #define MAGBIAS_Z -350.0
HARUKIDELTA 0:17f575135219 108 */
HARUKIDELTA 0:17f575135219 109
HARUKIDELTA 0:17f575135219 110 #define ELEMENT 1
HARUKIDELTA 0:17f575135219 111 #define LIMIT_STRAIGHT_YAW 5.0
HARUKIDELTA 0:17f575135219 112 #define THRESHOLD_TURNINGRADIUS_YAW 60.0
HARUKIDELTA 0:17f575135219 113 #define ALLOWHEIGHT 15
HARUKIDELTA 0:17f575135219 114
HARUKIDELTA 0:17f575135219 115 #ifndef PI
HARUKIDELTA 0:17f575135219 116 #define PI 3.14159265358979
HARUKIDELTA 0:17f575135219 117 #endif
HARUKIDELTA 0:17f575135219 118
HARUKIDELTA 0:17f575135219 119 const int16_t lengthdivpwm = 320;
HARUKIDELTA 0:17f575135219 120 const int16_t changeModeCount = 6;
HARUKIDELTA 0:17f575135219 121
HARUKIDELTA 0:17f575135219 122
HARUKIDELTA 0:17f575135219 123 SBUS sbus(PA_9, PA_10); //SBUS
HARUKIDELTA 0:17f575135219 124
HARUKIDELTA 0:17f575135219 125 PwmOut servo1(PC_6); // TIM3_CH1 //old echo
HARUKIDELTA 0:17f575135219 126 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
HARUKIDELTA 0:17f575135219 127 PwmOut servo3(PB_0); // TIM3_CH3
HARUKIDELTA 0:17f575135219 128 PwmOut servo4(PB_1); // TIM3_CH4
HARUKIDELTA 0:17f575135219 129 PwmOut servo5(PB_6); // TIM4_CH1
HARUKIDELTA 0:17f575135219 130 PwmOut servo6(PB_7); // TIM4_CH2 //old trigger
HARUKIDELTA 0:17f575135219 131 //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo
HARUKIDELTA 0:17f575135219 132 //PwmOut servo8(PB_9); // TIM4_CH4 //new trigger
HARUKIDELTA 0:17f575135219 133
HARUKIDELTA 0:17f575135219 134 RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
taknokolat 1:f383708a5a52 135 //RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用
HARUKIDELTA 0:17f575135219 136 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
HARUKIDELTA 0:17f575135219 137
HARUKIDELTA 0:17f575135219 138 DigitalOut led1(PA_0); //黄色のコネクタ
HARUKIDELTA 0:17f575135219 139 DigitalOut led2(PA_1);
taknokolat 45:eebdf6fa7b15 140 DigitalOut led3(PB_5);
taknokolat 45:eebdf6fa7b15 141 DigitalOut led4(PB_4);
HARUKIDELTA 0:17f575135219 142
HARUKIDELTA 0:17f575135219 143 //InterruptIn switch2(PC_14);
HARUKIDELTA 0:17f575135219 144 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
HARUKIDELTA 31:dba3216c2755 145 HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8
HARUKIDELTA 0:17f575135219 146
taknokolat 1:f383708a5a52 147 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
HARUKIDELTA 0:17f575135219 148 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
HARUKIDELTA 0:17f575135219 149 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
HARUKIDELTA 0:17f575135219 150
HARUKIDELTA 0:17f575135219 151 enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
HARUKIDELTA 0:17f575135219 152 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
HARUKIDELTA 0:17f575135219 153 enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
HARUKIDELTA 0:17f575135219 154 enum BombingMode{Takeoff, Chicken, Transition, Approach};
HARUKIDELTA 0:17f575135219 155 enum OutputStatus{Manual, Auto};
HARUKIDELTA 0:17f575135219 156
HARUKIDELTA 0:17f575135219 157 static OutputStatus output_status = Manual;
HARUKIDELTA 0:17f575135219 158 OperationMode operation_mode = StartUp;
HARUKIDELTA 0:17f575135219 159 BombingMode bombing_mode = Takeoff;
TUATBM 15:43427b0241d9 160
HARUKIDELTA 42:74d72339a8a8 161 static int16_t autopwm[8] = {1455,1450,1176,1628,1417,1452};
taknokolat 26:bc185a3d16b6 162
HARUKIDELTA 46:ebbe73af073d 163 int main(void){
HARUKIDELTA 0:17f575135219 164 while(1){
HARUKIDELTA 46:ebbe73af073d 165 printf("Hello World\r\n");
HARUKIDELTA 46:ebbe73af073d 166 //servo1.period_ms(14);
HARUKIDELTA 46:ebbe73af073d 167 servo1.pulsewidth_us(1300);
HARUKIDELTA 46:ebbe73af073d 168 wait(1);
HARUKIDELTA 46:ebbe73af073d 169 //servo1.period_ms(14);
HARUKIDELTA 46:ebbe73af073d 170 servo1.pulsewidth_us(1200);
HARUKIDELTA 42:74d72339a8a8 171 }
HARUKIDELTA 46:ebbe73af073d 172 }