2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 68:ae3c3500074c
- Parent:
- 67:94fe2180f39a
- Child:
- 69:073116d31a56
--- a/main.cpp Sat Jul 15 02:45:03 2017 +0000 +++ b/main.cpp Fri Sep 29 08:13:16 2017 +0000 @@ -46,12 +46,12 @@ PwmOut drugServo(p22); PwmOut eruronServo(p23); DigitalOut led1(LED1); -AnalogIn drugAna(p20); -AnalogIn eruronAna(p19); -DigitalIn IsRPin(p11); -DigitalIn setTrimPin(p12); -DigitalIn EDstatePin(p14); -DigitalIn setMaxDegPin(p15); +AnalogIn eruronAna(p20); +AnalogIn drugAna(p19); +DigitalIn IsRPin(p11,PullDown); +DigitalIn InSetModePin(p12,PullDown); +DigitalIn EDstatePin(p14,PullDown); +DigitalIn TrimMaxDegPin(p15,PullDown); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); @@ -317,7 +317,7 @@ // pc.printf("\n\r"); drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); -// pc.printf("ef:%10f %5f\n\r",eruronfloat,drugInput); + pc.printf("ef:%10f %5f\n\r",eruronfloat,drugInput); } void setTrim() @@ -344,8 +344,8 @@ eruronMoveDeg = eruronTemp-eruronTrim; eruronServo.pulsewidth(calcPulse(eruronTemp)); } else { + drugMoveDeg = drugTemp-drugTrim; drugServo.pulsewidth(calcPulse(drugTemp)); - drugMoveDeg = drugTemp-drugTrim; } pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); @@ -355,32 +355,24 @@ int main() { init(); - - setTrimPin.mode(PullDown); - setMaxDegPin.mode(PullDown); - EDstatePin.mode(PullDown); - IsRPin.mode(PullDown); Thread mpu_thread(&mpuProcessing); // start motion // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); while(1) { - if(setTrimPin) { - do { - setTrim(); - } while(setTrimPin); - writeConfig(); - } - if(setMaxDegPin) { - do { - setMaxDeg(); - } while(setMaxDegPin); + if(InSetModePin){ + do{ + if(TrimMaxDegPin) + setTrim(); + else + setMaxDeg(); + }while(InSetModePin); writeConfig(); } led4 = 0; led2 = 0; - //receiveDatas(); + receiveDatas(); // sendDatas(); WriteServo(); updateDatas();