Daniqe Kottelenberg
Wil je hier nog je PID controler kort uitleggen plus waarden aanpassen?
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
a_pid_kal_end
by 
Daniqe Kottelenberg
Diff: main.cpp
- Revision:
- 57:c546edf67c5c
- Parent:
- 56:a38412383477
- Child:
- 58:c91723359f62
--- a/main.cpp Thu Nov 03 16:41:10 2016 +0000
+++ b/main.cpp Fri Nov 04 15:12:52 2016 +0000
@@ -1,4 +1,4 @@
-//=====================================================================================
+//=======================================================================================================================================================
//libraries
#include "mbed.h" //mbed revision 113
#include "HIDScope.h" //Hidscope by Tom Lankhorst
@@ -7,7 +7,7 @@
#include "QEI.h" //QEI library for the encoders
-//=====================================================================================
+//=======================================================================================================================================================
//Define objects
//EMG
@@ -20,7 +20,9 @@
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;
+Ticker ticker_encoder; //ticker for encoderfunction motor 1
+Ticker ticker_calibration_biceps; //ticker for calibration biceps
+Ticker ticker_calibration_triceps; //ticker for calibation triceps
//Timer
Timer timer;
@@ -30,6 +32,11 @@
MODSERIAL pc(USBTX, USBRX); //pc connection
DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
+DigitalOut blue(LED_BLUE); //LED on K64f board, 1 is out; o is on
+
+//buttons
+DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
+DigitalIn button_calibration_triceps (SW2); // button to start calibration triceps
//motors
DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
@@ -40,14 +47,11 @@
//encoders
DigitalIn encoder1A(D13);
DigitalIn encoder1B(D12);
-DigitalIn encoder2A(D11);
-DigitalIn encoder2B(D10);
//controller
BiQuad PID_controller;
-
-//=====================================================================================
+//=======================================================================================================================================================
//define variables
//thresholds
@@ -55,9 +59,16 @@
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
+
+//calibration variables
+const float percentage_max_triceps=0.25; //percentage from max to calculate new treshold
+const float percentage_max_biceps =0.3; //percentage from max to calculate new treshold
+double max_biceps; //calibration maximum biceps
+double max_triceps; //calibration maximum triceps
+
//on/off and switch signals
-int switch_signal = 0; //start of counter, switch made by even and odd numbers
-int onoffsignal_biceps;
+int switch_signal = 0; //start of counter, switch made by even and odd numbers
+int onoffsignal_biceps; //on/off signal created by the bicepssignal. (-1: left biceps contract, 0: nothing contracted, 1: right biceps contracted)
int switch_signal_triceps;
//motorvariables
@@ -68,53 +79,54 @@
int ccw=1; //counterclockwise direction
//encoder
-int counts_encoder1;
-//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
-QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING);
+int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1.
+float rev_counts_motor1; //Calculated revolutions
+float rev_counts_motor1_rad; //calculated revolutions in rad!
+const float gearboxratio=131.25; // gearboxratio from encoder to motor
+const float rev_rond=64.0; // number of revolutions per rotation
+
+QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the Encoder.
//reference
-volatile float d_ref = 0;
-const float w_ref = 1.5;
-volatile double t_start;
-volatile double w_var;
-const double Ts = 0.001;
+volatile float d_ref = 0;
+const float w_ref = 1.5;
+volatile double t_start;
+volatile double w_var;
+const double Ts = 0.001; //Time for diverse tickers. It is comparable to a frequency of 1000Hz.
//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;
-bool start_motor = true;
-volatile double starttime;
-//=======================================
+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;
+bool start_motor = true;
+volatile double starttime;
+
+//=======================================================================================================================================================
//filter coefficients
//b1 = biceps right arm
-BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
-BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
+BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
+BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
//t1= triceps right arm
-BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
-BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
+BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
+BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
//b2= biceps left arm
-BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
-BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
+BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 20 Hz
+BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 Hz
//after abs filtering
-BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
-BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
-BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
+BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
+BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
+BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz
-//======================================================================
+//=======================================================================================================================================================
//voids
-//======================================================================
+//=======================================================================================================================================================
//function teller
void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
@@ -143,9 +155,10 @@
}
}
-//======================================================================
+//=======================================================================================================================================================
//functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
+//Filter void :// funciton which is called in ticker to sample the analog signal and make the on/off and switch signal.
void filter(){
//biceps right arm read+filtering
double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
@@ -198,16 +211,18 @@
scope.set(3, onoffsignal_biceps); // set on/off signal for the motors to scope in channel 3
scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
- scope.send(); //send all the signals to the scope
+ scope.send(); //send all the signals to the scope
}
-
+//=======================================================================================================================================================
+
+//reference void makes the reference that the controllor should follow. There is only a controller for motor 1.
void reference(){
- if (start_motor == true){
+ if (start_motor == true){
timer.start();
}
if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even
t_start = timer.read_ms();
- start_motor = false;
+ start_motor = false; //It means that motor 2 is running and therefore the PID controllor should not be working. Therefore the bool is set on false.
if (t_start < 1.0){
w_var = t_start*1.5;
@@ -251,12 +266,15 @@
}
}
-
+//=======================================================================================================================================================
+//This void calculates the error and makes the control output.
void m1_controller(){
error1 = d_ref-rev_counts_motor1_rad;
controlOutput = PID_controller.step(error1);
}
+//=======================================================================================================================================================
+//This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously.
void encoders(){
counts_encoder1 = Encoder1.getPulses();
rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
@@ -264,9 +282,144 @@
}
-//======================================================================
+//=======================================================================================================================================================
+
+//The calibration of the Biceps threshold is started by a button.
+//It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
+void calibration_biceps(){
+ if (button_calibration_biceps==0){ //only runs when button is pressed
+
+ //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
+ ticker_switch.detach();
+ sample_timer.detach();
+
+ //let the user know what is happening, blue led on: calibration is going.
+ pc.printf("start of calibration biceps, contract maximal \r\n");
+ pc.printf("\r\n");
+ red=1;
+ green=1;
+ blue=0;
+
+ //start callibration of biceps
+ for(int n =0; n<1500;n++) //read for 1500 samples as calibration
+ {
+ //biceps right arm read+filtering
+ double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
+ double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
+ double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
+ double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
+ double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
+
+ //triceps right arm read+filtering
+ double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
+ double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
+ double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
+ double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
+ double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
+
+ //biceps is +, triceps is -
+ double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
+
+ if (bicepstriceps_rightarm > max_biceps) //determine what the highest reachable emg signal is
+ {
+ max_biceps = bicepstriceps_rightarm;
+
+ }
+ wait(0.001f); //to sample at same freq; 1000Hz
+ }
+ treshold_biceps_right=percentage_max_biceps*max_biceps; //determine new treshold, right biceps is +
+ treshold_biceps_left=-treshold_biceps_right; //determine new treshold, right biceps is -
+
+ //toggle lights to see the calibration is done. Also show in putty that the calibration is done.
+ blue=!blue;
+
+ pc.printf(" end of calibration\r\n",treshold_biceps_right );
+ pc.printf(" change of cv biceps: %f ",treshold_biceps_right );
+
+ wait(0.2f);
+
+ //remind the person of what motor will go on an which direction
+ if (switch_signal%2==0)
+ {green=0;
+ red=1;}
+
+ else {green=1;
+ red=0;}
+ }
+ //reattach the functions to the tickers that were detached.
+ ticker_switch.attach(&switch_function,1.0);
+ sample_timer.attach(&filter, 0.001);
+ }
+//=======================================================================================================================================================
+
+//The calibration of the Triceps threshold is started by a button.
+//It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold.
+void calibration_triceps(){
+ if(button_calibration_triceps==0){ //only runs when button is pressed
+
+ //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors.
+ ticker_switch.detach();
+ sample_timer.detach();
+
+ //toggel LEDS and let the user know that callibration of triceps is starting.
+ red=1;
+ green=1;
+ blue=0;
+
+ pc.printf("start of calibration triceps\r\n");
+ pc.printf("\r\n");
+
+//start calibration of triceps
+ for(int n =0; n<1500;n++) //read for 2000 samples as calibration
+ {
+ //biceps right arm read+filtering
+ double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
+ double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
+ double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
+ double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
+ double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
+
+ //triceps right arm read+filtering
+ double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
+ double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
+ double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
+ double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
+ double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
+
+ //biceps is +, triceps is -
+ double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
+
+ if (bicepstriceps_rightarm < max_triceps) //determine what the lowest reachable emg of triceps (max in negative part) signal is
+ {
+ max_triceps = bicepstriceps_rightarm;
+
+ }
+ wait(0.001f); //to sample at same freq; 1000Hz
+ }
+ treshold_triceps=percentage_max_triceps*max_triceps; //calculate the new treshold. This is a negative number due to the sum!
+
+ //Let the user know that the calibration is done.
+ pc.printf(" end of calibration\r\n");
+ pc.printf(" change of cv triceps: %f ",treshold_triceps );
+ blue=!blue;
+ wait(0.2f);
+ if (switch_signal%2==0)
+ {green=0;
+ red=1;}
+
+ else {green=1;
+ red=0;}
+ }
+
+ //reattach the functions to the tickers that were detached.
+ sample_timer.attach(&filter, 0.001);
+ ticker_switch.attach(&switch_function,1.0);
+ }
+//=======================================================================================================================================================
+
+//=======================================================================================================================================================
//program
-//======================================================================
+//=======================================================================================================================================================
int main()
{
@@ -275,11 +428,13 @@
red=0; //led is on (0), at beginning
//attach tickers to functions
-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);
+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); //it is possible to switch only once in a second, this ensures that the switch is not reacting on one signal multiple times.
ticker_referenceangle.attach(&reference, Ts);
ticker_controllerm1.attach(&m1_controller, Ts);
ticker_encoder.attach(&encoders, Ts);
+ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop EMG sampling and switch
+ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop EMG sampling and switch
//PID controller
PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
@@ -304,37 +459,26 @@
pc.printf("\r\n");
}
-//==============================================================================================
+//=======================================================================================================================================================
//endless loop
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 %f \r \n", d_ref, 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
if (switch_signal%2==0){ //switch even
speedmotor1=controlOutput;
- //richting_motor1 = ccw; //motor 1, left
- //pwm_motor1 = speedmotor1; //speed of motor 1
+
if (speedmotor1<0){
- richting_motor1 = cw;
+ richting_motor1 = cw; // motor 1, right
}
else {
- richting_motor1 = ccw;
+ richting_motor1 = ccw; //motor 1, left
}
- pwm_motor1 = fabs(speedmotor1); //speed of motor 1
- // pc.printf("%f\r\n", pwm_motor1.read());
+ pwm_motor1 = fabs(speedmotor1); //speed of motor 1
+
}
@@ -351,26 +495,25 @@
if (switch_signal%2==0) //switch signal even
{
speedmotor1=controlOutput;
- //richting_motor1 = ccw; //motor 1, left
- //pwm_motor1 = speedmotor1; //speed of motor 1
+
if (speedmotor1<0){
- richting_motor1 = cw;
+ richting_motor1 = cw; //motor 1, right
}
else {
- richting_motor1 = ccw;
+ richting_motor1 = ccw; //motor 1, left
}
pwm_motor1 = fabs(speedmotor1); //speed of motor 1
- // pc.printf("%f\r\n", pwm_motor1.read());
+
}
- else //switch signal odd
+ else //switch signal odd
{
- richting_motor2 = cw; //motor 2. down
+ richting_motor2 = cw; //motor 2, down
pwm_motor2 = speedmotor2; //speed motor 2
}
}
else{
- //no contraction of biceps
+ //no contraction of biceps, thus no motoraction.
pwm_motor2=0;
pwm_motor1=0;
start_motor = true;
@@ -382,4 +525,4 @@
} //int main closed
-//=============================================================================================1
\ No newline at end of file
+//=======================================================================================================================================================