2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 77:ca4ab599ba2b
- Parent:
- 74:8ccd04302a7f
- Child:
- 78:e272e65f5a0e
--- a/main.cpp Sat Dec 23 05:30:55 2017 +0000 +++ b/main.cpp Wed Feb 28 03:36:22 2018 +0000 @@ -53,7 +53,7 @@ #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0) #define SENDING_DATA_DEBUG_FLAG debugflag[0].flag #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag -#define SERVO_DONFIG_DEBUG_FLAG debugflag[2].flag +#define SERVO_CONFIG_DEBUG_FLAG debugflag[2].flag #define DEBUG_FLAG debugflag[3].flag struct flaglist{ @@ -80,10 +80,10 @@ DigitalOut led1(LED1); AnalogIn setDeg10(p20); AnalogIn setDeg1(p19); -DigitalIn IsRPin(p13,PullDown); -DigitalIn InSetModePin(p14,PullDown); -DigitalIn EDstatePin(p15,PullDown); -DigitalIn TrimMaxDegPin(p16,PullDown); +DigitalIn IsRPin(p16,PullDown); +DigitalIn InSetModePin(p15,PullDown); +DigitalIn EDstatePin(p14,PullDown); +DigitalIn TrimMaxDegPin(p13,PullDown); DigitalOut servoOff(p17); DigitalOut led2(LED2); DigitalOut led3(LED3); @@ -375,29 +375,34 @@ drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff)); + print2pc(SERVO_CONFIG_DEBUG_FLAG,"eTr:%f dTr:%f eMD:%f dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg); print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer); } -void setTrim() +void setTrim(float initEruronState, float initDrugState, float initPinState) { - led2 = 1; + led2 = 1; + led4 = 0; + if(eruronTrim < 0 || drugTrim < 0)led2 = 0; if(EDstatePin) { - eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; + eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState; eruronServo.pulsewidth(calcPulse(eruronTrim)); } else { - drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; + drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState; drugServo.pulsewidth(calcPulse(drugTrim)); } //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); - pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); + pc.printf("eTr:%f dTr:%f ",eruronTrim,drugTrim); pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); } -void setMaxDeg() +void setMaxDeg(float initEruronState, float initDrugState, float initPinState) { led4 = 1; - float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; - float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; + led2 = 0; + float eruronTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState; + float drugTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState; + if(eruronTemp < 0 || drugTemp < 0)led4 = 0; if(EDstatePin) { eruronMoveDeg = eruronTemp-eruronTrim; eruronServo.pulsewidth(calcPulse(eruronTemp)); @@ -405,7 +410,7 @@ drugMoveDeg = drugTemp-drugTrim; drugServo.pulsewidth(calcPulse(drugTemp)); } - pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); + pc.printf("eTr:%f dTr:%f ",eruronTrim,drugTrim); pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); wait_us(10); } @@ -420,11 +425,14 @@ while(1) { if(InSetModePin){ + float initEruronState = eruronTrim; + float initDrugState = drugTrim; + float initPinState = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; do{ if(TrimMaxDegPin) - setTrim(); + setTrim(initEruronState,initDrugState,initPinState); else - setMaxDeg(); + setMaxDeg(initEruronState,initDrugState,initPinState); }while(InSetModePin); writeConfig(); }