Wil je hier nog je PID controler kort uitleggen? Is sneller denk ik. Rest is gedaan volgens mij. Hier zit kall in.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of another_try_from_scratch_on_emg by
Diff: main.cpp
- Revision:
- 55:d742332ced11
- Parent:
- 54:fb72c58a7150
- Child:
- 56:a38412383477
--- a/main.cpp Thu Nov 03 12:18:53 2016 +0000 +++ b/main.cpp Thu Nov 03 15:27:59 2016 +0000 @@ -20,6 +20,10 @@ Ticker ticker_switch; //ticker for switch, every second it is possible to switch Ticker ticker_referenceangle; //ticker for the reference angle Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1 +Ticker ticker_encoder; + +//Timer +Timer timer; //Monitoring HIDScope scope(5); //open 5 channels in hidscope @@ -70,10 +74,13 @@ 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 +QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //reference volatile float d_ref = 0; -const float w_ref = 3; +const float w_ref = 1.5; +volatile double t_start; +volatile double w_var; const double Ts = 0.001; //controller @@ -83,6 +90,8 @@ const double N = 100; volatile double error1; volatile double controlOutput; +bool start_motor = true; +volatile double starttime; //======================================= //filter coefficients @@ -193,24 +202,50 @@ } void reference(){ - if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even //right biceps contracted{ - d_ref = d_ref + w_ref * Ts; + if (start_motor == true){ + timer.start(); } - if (d_ref > 8){ - d_ref = 8; + if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even + t_start = timer.read_ms(); + start_motor = false; + + if (t_start < 1.0){ + w_var = t_start*1.5; + } + + else { + w_var = 1.5; + } + + d_ref = d_ref + w_var * Ts; + + } + if (d_ref > 12){ + d_ref = 12; + start_motor = true; //d_ref_const_cw = 1; - } + } else{ d_ref = d_ref; } if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even //left biceps contracted{ - d_ref = d_ref - w_ref * Ts; - } - if (d_ref < -8){ - d_ref = -8; + t_start = timer.read_ms(); + start_motor = false; + + if (t_start < 1.0){ + w_var = t_start*1.5; + } + else { + w_var = 1.5; + } + d_ref = d_ref - w_var * Ts; } + if (d_ref < -12){ + d_ref = -12; + start_motor = true; + } else{ d_ref = d_ref; } @@ -222,6 +257,13 @@ controlOutput = PID_controller.step(error1); } +void encoders(){ + counts_encoder1 = Encoder1.getPulses(); + rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond); + rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; + +} + //====================================================================== //program //====================================================================== @@ -237,12 +279,13 @@ ticker_switch.attach(&switch_function,1.0); ticker_referenceangle.attach(&reference, Ts); ticker_controllerm1.attach(&m1_controller, Ts); +ticker_encoder.attach(&encoders, Ts); //PID controller PID_controller.PIDF(Kp,Ki,Kd,N,Ts); //Encoder -QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); +//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"); @@ -272,7 +315,10 @@ rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; pc.printf("%f \r\n", d_ref); -pc.printf("%f ", rev_counts_motor1_rad); +//pc.printf("%f ", rev_counts_motor1_rad); +//pc.printf("%f",w_var); +pc.printf("%d\n",start_motor); + if (onoffsignal_biceps==-1) //left biceps contracted { @@ -327,6 +373,7 @@ //no contraction of biceps pwm_motor2=0; pwm_motor1=0; + start_motor = true; } }//while true closed