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:
- 50:2c03357de7cc
- Parent:
- 49:818a0e90ed9c
- Child:
- 51:b344a92b6a5f
--- a/main.cpp Thu Nov 03 09:23:48 2016 +0000 +++ b/main.cpp Thu Nov 03 09:55:11 2016 +0000 @@ -4,19 +4,22 @@ #include "HIDScope.h" //Hidscope by Tom Lankhorst #include "BiQuad.h" //BiQuad by Tom Lankhorst #include "MODSERIAL.h" //Modserial +#include "QEI.h" //QEI library for the encoders //===================================================================================== //Define objects //EMG -AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++ +AnalogIn emg_biceps_right_in (A0); //analog in to get EMG biceps (r) in to c++ AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++ AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++ //Tickers Ticker sample_timer; //ticker for EMG signal sampling, analog becomes digital 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 //Monitoring HIDScope scope(5); //open 5 channels in hidscope @@ -25,18 +28,28 @@ DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on //motors -DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable +DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable PwmOut pwm_motor1(D6); -DigitalOut richting_motor2(D4); ///motor 2 connected to motor 2 at k64f board; for linear actuator +DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator PwmOut pwm_motor2(D5); +//encoders +DigitalIn encoder1A(D13); +DigitalIn encoder1B(D12); +DigitalIn encoder2A(D11); +DigitalIn encoder2B(D10); + +//controller +BiQuad PID_controller; + + //===================================================================================== //define variables -//tresholds -double treshold_biceps_right =0.04; //common values that work. -double treshold_biceps_left=-0.04; // tested on multiple persons -double treshold_triceps=-0.04; //triceps and left biceps is specified negative, thus negative treshold +//thresholds +double treshold_biceps_right = 0.04; //common values that work. +double treshold_biceps_left = -0.04; // tested on multiple persons +double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold //on/off and switch signals int switch_signal = 0; //start of counter, switch made by even and odd numbers @@ -49,7 +62,26 @@ int cw=0; //clockwise direction int ccw=1; //counterclockwise direction - + +//encoder +int counts_encoder1; +//int counts_encoder2; +float rev_counts_motor1; +float rev_counts_motor1_rad; + + +//reference +volatile float d_ref = 0; +const float w_ref = 3; +const double Ts = 0.001; + +//controller +const double Kp = 0.3823; +const double Ki = 0.1279; +const double Kd = 0.2519; +const double N = 100; +volatile double error1; +volatile double controlOutput; //======================================= //filter coefficients @@ -158,6 +190,37 @@ scope.send(); //send all the signals to the scope } + +void reference(){ + if (button_cw == 0){ + d_ref = d_ref + w_ref * Ts; + } + if (d_ref > 21.36 ){ + d_ref = 21.36; + //d_ref_const_cw = 1; + } + else{ + d_ref = d_ref; + } + + if (button_ccw == 0){ + d_ref = d_ref - w_ref * Ts; + } + if (d_ref < -21.36){ + d_ref = -21.36; + + } + else{ + d_ref = d_ref; + } + +} + +void m1_controller(){ + error1 = d_ref-rev_counts_motor1_rad; + ctrlOutput = PID_controller.step(error1); +} + //====================================================================== //program //====================================================================== @@ -169,14 +232,18 @@ red=0; //led is on (0), at beginning //attach tickers to functions -sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds +sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds ticker_switch.attach(&switch_function,1.0); +ticker_referenceangle.attach(&reference, Ts); +ticker_controllerm1.attach(&m1_controller, Ts); + +//PID controller +PID_controller.PIDF(Kp,Ki,Kd,N,Ts); //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"); - if (switch_signal%2==0) {pc.printf("If you contract the biceps, the robot will go right \r\n"); pc.printf("If you contract the triceps, the robot will go left \r\n");