pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 51:b344a92b6a5f
- Parent:
- 50:2c03357de7cc
- Child:
- 52:0135deb3b07f
--- a/main.cpp Thu Nov 03 09:55:11 2016 +0000 +++ b/main.cpp Thu Nov 03 10:37:29 2016 +0000 @@ -68,7 +68,8 @@ //int counts_encoder2; float rev_counts_motor1; float rev_counts_motor1_rad; - +const float gearboxratio=131.25; // gearboxratio van encoder naar motor +const float rev_rond=64.0; // aantal revoluties per omgang van de encoder //reference volatile float d_ref = 0; @@ -192,7 +193,7 @@ } void reference(){ - if (button_cw == 0){ + if (onoffsignal_biceps==1){ //right biceps contracted{ d_ref = d_ref + w_ref * Ts; } if (d_ref > 21.36 ){ @@ -203,7 +204,7 @@ d_ref = d_ref; } - if (button_ccw == 0){ + if (onoffsignal_biceps==-1){ //left biceps contracted{ d_ref = d_ref - w_ref * Ts; } if (d_ref < -21.36){ @@ -218,7 +219,7 @@ void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; - ctrlOutput = PID_controller.step(error1); + controlOutput = PID_controller.step(error1); } //====================================================================== @@ -240,6 +241,9 @@ //PID controller PID_controller.PIDF(Kp,Ki,Kd,N,Ts); +//Encoder +QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); + //Show the user what the starting motor will be and what will happen pc.printf("We will start the demonstration\r\n"); pc.printf("\r\n\r\n\r\n"); @@ -263,14 +267,29 @@ while (true) { // neverending loop +counts_encoder1 = Encoder1.getPulses(); +rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); +rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; +pc.printf("%f", pwm_motor1.read()); +pc.printf(" %f", d_ref); +pc.printf(" %f \r\n", rev_counts_motor1_rad); + if (onoffsignal_biceps==-1) //left biceps contracted { if (switch_signal%2==0) //switch even { - richting_motor1 = ccw; //motor 1, left - pwm_motor1 = speedmotor1; //speed of motor 1 - + speedmotor1=controlOutput; + //richting_motor1 = ccw; //motor 1, left + //pwm_motor1 = speedmotor1; //speed of motor 1 + if (speedmotor1<0){ + richting_motor1 = 0; + } + else { + richting_motor1 = 1; + } + pwm_motor1 = fabs(speedmotor1); //speed of motor 1 } + else //switch odd { @@ -284,8 +303,16 @@ { if (switch_signal%2==0) //switch signal even { - richting_motor1 = cw; //motor 1, right - pwm_motor1 = speedmotor1; //speed motor 1 + speedmotor1=controlOutput; + //richting_motor1 = ccw; //motor 1, left + //pwm_motor1 = speedmotor1; //speed of motor 1 + if (speedmotor1<0){ + richting_motor1 = 0; + } + else { + richting_motor1 = 1; + } + pwm_motor1 = fabs(speedmotor1); //speed of motor 1 } else //switch signal odd