2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 48:0bd406fa4a7f
- Parent:
- 47:5e3a836deaba
- Child:
- 49:8522856fe0cd
- Child:
- 52:ad28166cf6d3
--- a/main.cpp Thu Mar 23 08:50:40 2017 +0000 +++ b/main.cpp Thu Mar 23 15:58:55 2017 +0000 @@ -16,18 +16,16 @@ #define MPU_LOOP_TIME 0.01 #define MPU_DELT_MIN 250 -#define ERURON_MOVE_DEG_INI_R 1.0 -#define DRUG_MOVE_DEG_INI_R -0.32 +#define ERURON_MOVE_DEG_INI_R 0.9 +#define DRUG_MOVE_DEG_INI_R -0.07 #define ERURON_TRIM_INI_R 0.35 #define DRUG_TRIM_INI_R 0.62 -#define ERURON_MOVE_DEG_INI_L 0.8 -#define DRUG_MOVE_DEG_INI_L -0.43 +#define ERURON_MOVE_DEG_INI_L 0.7 +#define DRUG_MOVE_DEG_INI_L -0.10 #define ERURON_TRIM_INI_L 0.4 #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側 -#define ERURON_MOVE_RANGE 0.24 - #define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください /*ドラッグラダー @@ -45,7 +43,7 @@ DigitalOut led1(LED1); AnalogIn drugAna(p20); AnalogIn eruronAna(p19); -DigitalIn LRstatePin(p11); +DigitalIn IsRPin(p11); DigitalIn setTrimPin(p12); DigitalIn EDstatePin(p14); DigitalIn setMaxDegPin(p15); @@ -161,7 +159,7 @@ pitch *= 180.0f / PI; yaw *= 180.0f / PI; roll *= 180.0f / PI; - // myled= !myled; + // myled= !myled; count = t.read_ms(); sum = 0; sumCount = 0; @@ -188,16 +186,16 @@ void init() { - if(!LRstatePin) { + if(IsRPin) { + eruronTrim = ERURON_TRIM_INI_R; + drugTrim = DRUG_TRIM_INI_R; + eruronMoveDeg = 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; drugMoveDeg = DRUG_MOVE_DEG_INI_L; - } else { - eruronTrim = ERURON_TRIM_INI_R; - drugTrim = DRUG_TRIM_INI_R; - eruronMoveDeg = ERURON_MOVE_DEG_INI_R; - drugMoveDeg =DRUG_MOVE_DEG_INI_R; } SERVO_FLAG = servoInit(); INA_FLAG = inaInit(); @@ -250,8 +248,8 @@ void WriteServo() { drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2])); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * ((1.0/PHASE_NUM) * eruronint) * ERURON_MOVE_RANGE)); - pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint* ERURON_MOVE_RANGE)); + eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint)); + pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint)); pc.printf("drValue::%f ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronint); // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); } @@ -293,7 +291,7 @@ setTrimPin.mode(PullDown); setMaxDegPin.mode(PullDown); EDstatePin.mode(PullDown); - LRstatePin.mode(PullDown); + IsRPin.mode(PullDown); Thread mpu_thread(&mpuProcessing); // start motion