yal kaiyo / Mbed 2 deprecated rotary3

ロータリーエンコーダー読み取り＋タイマー（あ、これyaw_plusと同じだったｗ）

Dependencies:   TextLCD mbed

main.cpp@0:f3d65e123fda, 2016-06-09 (annotated)

Committer:
yal_kaiyo
Date:
Thu Jun 09 01:03:04 2016 +0000
Revision:
0:f3d65e123fda
???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yal_kaiyo 0:f3d65e123fda 1 #include "mbed.h"
yal_kaiyo 0:f3d65e123fda 2 #include "TextLCD.h"
yal_kaiyo 0:f3d65e123fda 3 //#include "I2cLCD.h"
yal_kaiyo 0:f3d65e123fda 4
yal_kaiyo 0:f3d65e123fda 5 DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4);
yal_kaiyo 0:f3d65e123fda 6 AnalogIn rotary1(p15);
yal_kaiyo 0:f3d65e123fda 7 Serial pc(USBTX,USBRX); //(USBTX,USBRX)
yal_kaiyo 0:f3d65e123fda 8 Timer t1,t2;
yal_kaiyo 0:f3d65e123fda 9 InterruptIn ch1(p16),ch2(p17);
yal_kaiyo 0:f3d65e123fda 10 PwmOut yawyaw(p21),ch5out(p22);
yal_kaiyo 0:f3d65e123fda 11
yal_kaiyo 0:f3d65e123fda 12 double t1_pwm;
yal_kaiyo 0:f3d65e123fda 13 double t2_pwm;
yal_kaiyo 0:f3d65e123fda 14
yal_kaiyo 0:f3d65e123fda 15 /////////////timer//////////////
yal_kaiyo 0:f3d65e123fda 16 void t1_start(){
yal_kaiyo 0:f3d65e123fda 17 t1.start();
yal_kaiyo 0:f3d65e123fda 18 }
yal_kaiyo 0:f3d65e123fda 19
yal_kaiyo 0:f3d65e123fda 20 void t1_stop (){
yal_kaiyo 0:f3d65e123fda 21 t1.stop();
yal_kaiyo 0:f3d65e123fda 22 t1_pwm = t1.read();
yal_kaiyo 0:f3d65e123fda 23 t1.reset();
yal_kaiyo 0:f3d65e123fda 24 }
yal_kaiyo 0:f3d65e123fda 25
yal_kaiyo 0:f3d65e123fda 26 void t2_start(){
yal_kaiyo 0:f3d65e123fda 27 t2.start();
yal_kaiyo 0:f3d65e123fda 28 }
yal_kaiyo 0:f3d65e123fda 29
yal_kaiyo 0:f3d65e123fda 30 void t2_stop (){
yal_kaiyo 0:f3d65e123fda 31 t2.stop();
yal_kaiyo 0:f3d65e123fda 32 t2_pwm = t2.read();
yal_kaiyo 0:f3d65e123fda 33 t2.reset();
yal_kaiyo 0:f3d65e123fda 34 }
yal_kaiyo 0:f3d65e123fda 35
yal_kaiyo 0:f3d65e123fda 36 ///////////////////////////////////
yal_kaiyo 0:f3d65e123fda 37
yal_kaiyo 0:f3d65e123fda 38
yal_kaiyo 0:f3d65e123fda 39
yal_kaiyo 0:f3d65e123fda 40
yal_kaiyo 0:f3d65e123fda 41 int main(void){
yal_kaiyo 0:f3d65e123fda 42
yal_kaiyo 0:f3d65e123fda 43 pc.baud(115200);
yal_kaiyo 0:f3d65e123fda 44
yal_kaiyo 0:f3d65e123fda 45 double yaw_gain_p = 0.001;
yal_kaiyo 0:f3d65e123fda 46 double yaw_s;
yal_kaiyo 0:f3d65e123fda 47
yal_kaiyo 0:f3d65e123fda 48 ch1.rise(&t1_start) ;
yal_kaiyo 0:f3d65e123fda 49 ch1.fall(&t1_stop) ;
yal_kaiyo 0:f3d65e123fda 50
yal_kaiyo 0:f3d65e123fda 51 ch2.rise(&t2_start) ;
yal_kaiyo 0:f3d65e123fda 52 ch2.fall(&t2_stop) ;
yal_kaiyo 0:f3d65e123fda 53
yal_kaiyo 0:f3d65e123fda 54
yal_kaiyo 0:f3d65e123fda 55 yawyaw.period(0.020);
yal_kaiyo 0:f3d65e123fda 56
yal_kaiyo 0:f3d65e123fda 57 double rot_read = rotary1.read();
yal_kaiyo 0:f3d65e123fda 58
yal_kaiyo 0:f3d65e123fda 59
yal_kaiyo 0:f3d65e123fda 60 while(1){
yal_kaiyo 0:f3d65e123fda 61
yal_kaiyo 0:f3d65e123fda 62
yal_kaiyo 0:f3d65e123fda 63 if (t2_pwm > 0.0015){
yal_kaiyo 0:f3d65e123fda 64
yal_kaiyo 0:f3d65e123fda 65 double yaw_error = (rotary1.read() - 0.4785);
yal_kaiyo 0:f3d65e123fda 66
yal_kaiyo 0:f3d65e123fda 67 yaw_s = 0.00148 + yaw_gain_p * yaw_error;
yal_kaiyo 0:f3d65e123fda 68
yal_kaiyo 0:f3d65e123fda 69 if (yaw_s > 0.00175){yaw_s = 0.00175;}
yal_kaiyo 0:f3d65e123fda 70 if (yaw_s < 0.0012){yaw_s = 0.0012;}
yal_kaiyo 0:f3d65e123fda 71
yal_kaiyo 0:f3d65e123fda 72
yal_kaiyo 0:f3d65e123fda 73 //if (t2_pwm > 0.0015){
yal_kaiyo 0:f3d65e123fda 74 yawyaw.pulsewidth(yaw_s); // servo position determined by a pulsewidth between 1-2ms
yal_kaiyo 0:f3d65e123fda 75
yal_kaiyo 0:f3d65e123fda 76 }
yal_kaiyo 0:f3d65e123fda 77
yal_kaiyo 0:f3d65e123fda 78
yal_kaiyo 0:f3d65e123fda 79 else{
yal_kaiyo 0:f3d65e123fda 80 yawyaw.pulsewidth(t1_pwm);}
yal_kaiyo 0:f3d65e123fda 81
yal_kaiyo 0:f3d65e123fda 82
yal_kaiyo 0:f3d65e123fda 83 //lcd.locate(8,1);
yal_kaiyo 0:f3d65e123fda 84 //rot_read = (rotary1.read())*1000000;
yal_kaiyo 0:f3d65e123fda 85 //pc.printf("rotary1 = %f\r\n", rotary1.read());
yal_kaiyo 0:f3d65e123fda 86 pc.printf("yaw_s = %f\r\n", yaw_s);
yal_kaiyo 0:f3d65e123fda 87 pc.printf("t1_pwm = %f\r\n", t1_pwm);
yal_kaiyo 0:f3d65e123fda 88 pc.printf("t2_pwm = %f\r\n", t2_pwm);
yal_kaiyo 0:f3d65e123fda 89 //wait(1);
yal_kaiyo 0:f3d65e123fda 90 ch5out.pulsewidth(t2_pwm);
yal_kaiyo 0:f3d65e123fda 91 }
yal_kaiyo 0:f3d65e123fda 92
yal_kaiyo 0:f3d65e123fda 93 }
yal_kaiyo 0:f3d65e123fda 94
yal_kaiyo 0:f3d65e123fda 95