yal kaiyo
/
YAWplus
yaw p control
main.cpp
- Committer:
- yal_kaiyo
- Date:
- 2015-12-18
- Revision:
- 0:35f3ade1a424
File content as of revision 0:35f3ade1a424:
#include "mbed.h" #include "TextLCD.h" //#include "I2cLCD.h" DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4); AnalogIn rotary1(p15); Serial pc(USBTX,USBRX); //(USBTX,USBRX) Timer t1,t2; InterruptIn ch1(p16),ch2(p17); PwmOut yawyaw(p21),ch5out(p22); double t1_pwm; double t2_pwm; /////////////timer////////////// void t1_start(){ t1.start(); } void t1_stop (){ t1.stop(); t1_pwm = t1.read(); t1.reset(); } void t2_start(){ t2.start(); } void t2_stop (){ t2.stop(); t2_pwm = t2.read(); t2.reset(); } /////////////////////////////////// int main(void){ pc.baud(115200); double yaw_gain_p = 0.001; double yaw_s; ch1.rise(&t1_start) ; ch1.fall(&t1_stop) ; ch2.rise(&t2_start) ; ch2.fall(&t2_stop) ; yawyaw.period(0.020); double rot_read = rotary1.read(); while(1){ if (t2_pwm > 0.0015){ double yaw_error = (rotary1.read() - 0.4785); yaw_s = 0.00148 + yaw_gain_p * yaw_error; if (yaw_s > 0.00175){yaw_s = 0.00175;} if (yaw_s < 0.0012){yaw_s = 0.0012;} //if (t2_pwm > 0.0015){ yawyaw.pulsewidth(yaw_s); // servo position determined by a pulsewidth between 1-2ms } else{ yawyaw.pulsewidth(t1_pwm);} //lcd.locate(8,1); //rot_read = (rotary1.read())*1000000; //pc.printf("rotary1 = %f\r\n", rotary1.read()); pc.printf("yaw_s = %f\r\n", yaw_s); pc.printf("t1_pwm = %f\r\n", t1_pwm); pc.printf("t2_pwm = %f\r\n", t2_pwm); //wait(1); ch5out.pulsewidth(t2_pwm); } }