2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 57:d7b709dd1c4f
- Parent:
- 56:2b947d31d307
- Child:
- 58:f84bd22fd586
--- a/main.cpp Fri Apr 28 15:25:10 2017 +0000 +++ b/main.cpp Sat May 13 15:20:02 2017 +0000 @@ -4,6 +4,7 @@ #include "INA226.hpp" #include "rtos.h" #include "XBusServo.h" +#include "math.h" #define TO_SEND_DATAS_NUM 7 #define INIT_SERVO_PERIOD_MS 20 @@ -16,17 +17,17 @@ #define MPU_LOOP_TIME 0.01 #define MPU_DELT_MIN 250 -#define ERURON_MOVE_DEG_INI_R 0.45 +#define ERURON_MOVE_DEG_INI_R 18.0 //degree #define DRUG_MOVE_DEG_INI_R 0.49 -#define ERURON_TRIM_INI_R 0.41 //値36werすると頭上げ +#define ERURON_TRIM_INI_R 0.38 //値lowerすると頭上げ #define DRUG_TRIM_INI_R 0.37 -#define ERURON_MOVE_DEG_INI_L -0.45 +#define ERURON_MOVE_DEG_INI_L -19.4 //degree #define DRUG_MOVE_DEG_INI_L -0.44 -#define ERURON_TRIM_INI_L 0.48 // 値をお大きいくすると頭上げ +#define ERURON_TRIM_INI_L 0.402 // 値をお大きいくすると頭上げ #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側 -#define PHASE_NUM 23 //奇数にしてください。そしてメインコードと必ず同じ値にしてください +#define PHASE_NUM 15 //奇数にしてください。そしてメインコードと必ず同じ値にしてください #define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.0 /*ドラッグラダー @@ -185,17 +186,37 @@ return true; } +//動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。 +double ConvertDeg(double servo) +{ + double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105; + return result; +} + +//ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化 +double GetValueByHorn(double deg) +{ + return deg*(0.10/25.7); +} + +double GetFloatByErebon(double erebonDeg) +{ + double servoDeg = ConvertDeg(erebonDeg); + double FirstMoveDeg = GetValueByHorn(servoDeg); + return FirstMoveDeg; +} + void init() { if(IsRPin) { eruronTrim = ERURON_TRIM_INI_R; drugTrim = DRUG_TRIM_INI_R; - eruronMoveDeg = ERURON_MOVE_DEG_INI_R; + eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R); drugMoveDeg =DRUG_MOVE_DEG_INI_R; } else { eruronTrim = ERURON_TRIM_INI_L; drugTrim = DRUG_TRIM_INI_L; - eruronMoveDeg = ERURON_MOVE_DEG_INI_L; + eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L); drugMoveDeg = DRUG_MOVE_DEG_INI_L; } SERVO_FLAG = servoInit(); @@ -257,7 +278,7 @@ void WriteServo() { drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2])); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE))); + eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE))); pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint)); pc.printf("drValue::%f ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE)); // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); @@ -315,9 +336,6 @@ while (setMaxDegPin) { setMaxDeg(); } - // pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - //// pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg); - // pc.printf("pitch:%d roll:%d yaw:%d\n\r",pitch,roll,yaw); led4 = 0; led2 = 0; //receiveDatas();