2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 75:4b6f1b976bec
- Parent:
- 74:8ccd04302a7f
--- a/main.cpp Sat Dec 23 05:30:55 2017 +0000 +++ b/main.cpp Wed Feb 07 13:11:57 2018 +0000 @@ -8,6 +8,11 @@ * ----- * ****************************************** +*LED1: blink in receiveDatas +*LED2: light in setTrim +*LED3: blink in main loop +*LED4: light in setMaxDeg +****************************************** **magic character of debug** * *(0)s:sending datas: mpu, servoV @@ -74,16 +79,16 @@ CANMessage recmsg; Serial pc(USBTX,USBRX); I2C ina226_i2c(p28,p27); -INA226 VCmonitor(ina226_i2c); +INA226 VCmonitor(ina226_i2c,0x4A,400000); PwmOut drugServo(p23); PwmOut eruronServo(p25); 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); @@ -362,7 +367,7 @@ double calcPulse(float analog) { - return (0.0006 + (analog)*(0.00240-0.00060) ); + return (0.6 + (analog)*(2.40-0.60) ); } void WriteServo() @@ -372,59 +377,78 @@ // } // pc.printf(" : %f",eruronfloat); // pc.printf("\n\r"); - drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput)); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); + drugServo.pulsewidth_ms(calcPulse(drugTrim + drugMoveDeg *drugInput)); + eruronServo.pulsewidth_ms(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat)); print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff)); print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer); } -void setTrim() +void setTrim(float baseinp) { + float inp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10 - baseinp; led2 = 1; if(EDstatePin) { - eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; - eruronServo.pulsewidth(calcPulse(eruronTrim)); + eruronTrim = eruronServo.read(); + + eruronServo.pulsewidth_ms(calcPulse(inp)); } else { - drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; - drugServo.pulsewidth(calcPulse(drugTrim)); + drugTrim = drugServo.read(); + drugServo.pulsewidth_ms(calcPulse(inp)); } - //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); - pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); } -void setMaxDeg() +void setMaxDeg(float baseinp) { + float inp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10 - baseinp; led4 = 1; - float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; - float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10; if(EDstatePin) { + float eruronTemp = eruronServo.read(); eruronMoveDeg = eruronTemp-eruronTrim; - eruronServo.pulsewidth(calcPulse(eruronTemp)); + eruronServo.pulsewidth_ms(calcPulse(inp)); } else { + float drugTemp = drugServo.read(); drugMoveDeg = drugTemp-drugTrim; - drugServo.pulsewidth(calcPulse(drugTemp)); + drugServo.pulsewidth_ms(calcPulse(inp)); } - pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); - pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); wait_us(10); } +void setServoConfig(PwmOut servo, float* trim, float* movedeg, float base){ + + float neji = (setDeg10.read()*9)/10.0 + setDeg1.read()/10; + printf("\nneji:%f\n\r",neji); + + if(TrimMaxDegPin){ + servo.write(base + neji); + *trim = base + neji; + led2 = !led2; + }else{ + servo.write(base + neji); + *movedeg = base + neji - *trim; + led4 = !led4; + } +} int main() { init(); Thread mpu_thread(&mpuProcessing); -// start motion -// gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); - while(1) { if(InSetModePin){ + float baseinp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10; + float eruronIni = eruronServo.read(); + float drugIni = drugServo.read(); + do{ - if(TrimMaxDegPin) - setTrim(); - else - setMaxDeg(); + if(EDstatePin){ + setServoConfig(eruronServo, &eruronTrim, &eruronMoveDeg, eruronIni - baseinp); + }else{ + setServoConfig(drugServo, &drugTrim, &drugMoveDeg, drugIni - baseinp); + } + + pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); + pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); + }while(InSetModePin); writeConfig(); }