Daniqe Kottelenberg
met knopjes, voor Wubs, zit PID in dus restrictie.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
a_pid_kal_end_def
by 
Floor Couwenberg
Diff: main.cpp
- Revision:
- 59:1725a3f02f37
- Parent:
- 58:c91723359f62
- Child:
- 60:c165691c4e86
--- a/main.cpp Tue Nov 08 07:16:08 2016 +0000
+++ b/main.cpp Tue Nov 08 20:31:45 2016 +0000
@@ -11,9 +11,9 @@
//Define objects
//EMG
-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++
+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
@@ -35,13 +35,13 @@
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
+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
+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
@@ -49,15 +49,15 @@
DigitalIn encoder1B(D12);
//controller
-BiQuad PID_controller;
+BiQuad PID_controller;
//=======================================================================================================================================================
//define variables
//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
+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
//calibration variables
@@ -67,93 +67,92 @@
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; //on/off signal created by the bicepssignal. (-1: left biceps contract, 0: nothing contracted, 1: right biceps contracted)
+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
-float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
+float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm at start
float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
int cw=0; //clockwise direction
int ccw=1; //counterclockwise direction
//encoder
-int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1.
-float rev_counts_motor1; //Calculated revolutions
+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
+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.
+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; //Time for diverse tickers. It is comparable to a frequency of 1000Hz.
+volatile float d_ref = 0; //reference angle, starts off 0
+const float w_ref = 1.5; //reference speed, constant
+volatile double t_start; //starttime of the timer
+volatile double w_var; //variable reference speed for making the reference signal
+const double Ts = 0.001; //time step 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 = 1.2614; //calculated value for the proportional action of the PID
+const double Ki = 0.4219; //calculated value for the integral action of the PID
+const double Kd = 0.8312; //calculated value for the derivative action of the PID
+const double N = 100; //specified value for the filter coefficient of the PID
+volatile double error1; //calculated error
+volatile double controlOutput; //output of the PID-controller
+bool start_motor = true; //bool to start the reference when the motor turns
+
//=======================================================================================================================================================
//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 10 Hz
-BiQuad filternotch1_b1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
+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 10 Hz
+BiQuad filternotch1_b1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 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 10 Hz
-BiQuad filternotch1_t1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
+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 10 Hz
+BiQuad filternotch1_t1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 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 10 Hz
-BiQuad filternotch1_b2 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz
+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 10 Hz
+BiQuad filternotch1_b2 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 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 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
+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.
- if(switch_signal_triceps==1)
- {
+void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
+ if(switch_signal_triceps==1){
switch_signal++;
- // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
+ // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
green=!green;
red=!red;
- 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");
- pc.printf("\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");
+ pc.printf("\r\n");
+ }
- else
- {pc.printf("If you contract the biceps, the robot will go up \r\n");
- pc.printf("If you contract the triceps, the robot will go down \r\n");
- pc.printf("\r\n");
- }
+ else{
+ pc.printf("If you contract the biceps, the robot will go up \r\n");
+ pc.printf("If you contract the triceps, the robot will go down \r\n");
+ pc.printf("\r\n");
+ }
}
- }
+}
//=======================================================================================================================================================
//functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
@@ -187,80 +186,87 @@
double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
//creating of on/off signal with the created on/off signals, with if statement for right arm!
- if (signal_biceps_sum>treshold_biceps_right)
- {onoffsignal_biceps=1;}
+ if (signal_biceps_sum>treshold_biceps_right){
+ onoffsignal_biceps=1;
+ }
- else if (signal_biceps_sum<treshold_biceps_left)
- { onoffsignal_biceps=-1; }
+ else if (signal_biceps_sum<treshold_biceps_left){
+ onoffsignal_biceps=-1;
+ }
- else
- {onoffsignal_biceps=0;}
+ else{
+ onoffsignal_biceps=0;
+ }
//creating on/off signal for switch (left arm)
- if (bicepstriceps_rightarm<treshold_triceps)
- { switch_signal_triceps=1; }
+ if (bicepstriceps_rightarm<treshold_triceps){
+ switch_signal_triceps=1;
+ }
- else
- { switch_signal_triceps=0; }
+ else{
+ switch_signal_triceps=0;
+ }
//send signals to scope to monitor the EMG signals
- scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
- scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
- scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
- scope.set(3, bicepstriceps_rightarm); // 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.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
+ scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
+ scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
+ scope.set(3, bicepstriceps_rightarm); // 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){
- timer.start();
+void reference(){
+ if (start_motor == true){ //bool that is true when the motor starts turning
+ timer.start(); //timer that starts counting in milliseconds
}
- if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even
- t_start = timer.read_ms();
- 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 (onoffsignal_biceps==-1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
+ t_start = timer.read_ms(); //read the current time passed from the timer
+ start_motor = false; //it means that the motor is not running or has started up
- if (t_start < 1.0){
- w_var = t_start*1.5;
- }
+ if (t_start < 1.0){ //the time passed is less than one second
+ w_var = t_start*1.5; //the reference velocity is the time passed multiplied with the eventual constant velocity it should reach
+ }
- else {
- w_var = 1.5;
+ else{
+ w_var = 1.5; //if the time passed is more than one second, the velocity is constant
+ }
+
+ d_ref = d_ref + w_var * Ts; //makes the reference angle
+
+ }
+ if (d_ref > 12){ //set the restrictions
+ d_ref = 12;
+ start_motor = true; //after the restriction is reached the motor (if turned the other way) will start up again so the bool has to be set to true
}
- 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;
+ d_ref = d_ref; //if there is no signal, the referance angle is constant
}
- if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even //left biceps contracted{
+ if (onoffsignal_biceps==1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
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;
-
+ }
+ d_ref = d_ref - w_var * Ts; //the motor should turn the other way now so the reference becomes negative
}
- if (d_ref < -12){
- d_ref = -12;
- start_motor = true;
+
+ if (d_ref < -12){ //negative restriction
+ d_ref = -12;
+ start_motor = true;
}
+
else{
d_ref = d_ref;
}
@@ -269,8 +275,8 @@
//=======================================================================================================================================================
//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);
+ error1 = d_ref-rev_counts_motor1_rad; //calculate the error = reference-output
+ controlOutput = PID_controller.step(error1); //give the error as input to the controller
}
//=======================================================================================================================================================
@@ -278,8 +284,7 @@
void encoders(){
counts_encoder1 = Encoder1.getPulses();
rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
- rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
-
+ rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; //calculate the angle in radians
}
//=======================================================================================================================================================
@@ -289,44 +294,45 @@
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;
+ //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;
+ 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;
+ 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 -
@@ -339,190 +345,192 @@
wait(0.2f);
//remind the person of what motor will go on an which direction
- if (switch_signal%2==0)
- {green=0;
- red=1;}
+ if (switch_signal%2==0){
+ green=0;
+ red=1;
+ }
- else {green=1;
- red=0;}
+ 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);
- }
+ }
+ //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.
+//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");
+ //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
+ //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;}
+ 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);
- }
+ //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()
-{
-
-pc.baud(115200); //connect with pc with baudrate 115200
-green=1; //led is off (1), at beginning
-blue=1; //led is off (1), at beginning
-red=0; //led is on (0), at beginning
+int main(){
-//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); //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);
+ pc.baud(115200); //connect with pc with baudrate 115200
+ green=1; //led is off (1), at beginning
+ blue=1; //led is off (1), at beginning
+ 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); //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);
+
+ //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");
-//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");
-
- 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");
- pc.printf("\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");
+ pc.printf("\r\n");
+ }
- else
- {pc.printf("If you contract the biceps, the robot will go up \r\n");
- pc.printf("If you contract the triceps, the robot will go down \r\n");
- pc.printf("\r\n");
- }
+ else{
+ pc.printf("If you contract the biceps, the robot will go up \r\n");
+ pc.printf("If you contract the triceps, the robot will go down \r\n");
+ pc.printf("\r\n");
+ }
//=======================================================================================================================================================
//endless loop
- while (true) { // neverending loop
+ while (true) { //neverending loop
- if (onoffsignal_biceps==-1){ //left biceps contracted
+ if (onoffsignal_biceps==-1){ //left biceps contracted
- if (switch_signal%2==0){ //switch even
+ if (switch_signal%2==0){ //switch even
- speedmotor1=controlOutput;
+ speedmotor1=controlOutput; //output PID-controller is the speed for motor1
- if (speedmotor1<0){
- richting_motor1 = cw; // motor 1, right
+ if (speedmotor1<0){ //if the output of the controller is negative, the direction is clockwise
+ richting_motor1 = cw; //motor 1, right
}
- else {
- richting_motor1 = ccw; //motor 1, left
+ else { //if the output is positive, the direction is counterclockwise
+ richting_motor1 = ccw; //motor 1, left
}
- pwm_motor1 = fabs(speedmotor1); //speed of motor 1
+ pwm_motor1 = fabs(speedmotor1); //speed of motor 1, absolute because pwm cannot be negative
- }
+ }
- else //switch odd
- {
- richting_motor2 = ccw; //motor 2, up
- pwm_motor2 = speedmotor2;//speed of motor 2
+ else{ //switch odd
+
+ richting_motor2 = ccw; //motor 2, up
+ pwm_motor2 = speedmotor2; //speed of motor 2
- }
+ }
- }
- else if (onoffsignal_biceps==1) //right biceps contracted
- {
- if (switch_signal%2==0) //switch signal even
- {
- speedmotor1=controlOutput;
+ }
+ else if (onoffsignal_biceps==1){ //right biceps contracted
+
+ if (switch_signal%2==0){ //switch signal even
+ speedmotor1=controlOutput;
- if (speedmotor1<0){
- richting_motor1 = cw; //motor 1, right
+ if (speedmotor1<0){ //the same as for the left biceps, the robot turns in the right direction because of the reference signal
+ richting_motor1 = cw; //motor 1, right
}
- else {
- richting_motor1 = ccw; //motor 1, left
+ else {
+ richting_motor1 = ccw; //motor 1, left
}
pwm_motor1 = fabs(speedmotor1); //speed of motor 1
- }
- else //switch signal odd
- {
- richting_motor2 = cw; //motor 2, down
- pwm_motor2 = speedmotor2; //speed motor 2
+ }
+ else{ //switch signal odd
+ richting_motor2 = cw; //motor 2, down
+ pwm_motor2 = speedmotor2; //speed motor 2
- }
- }
- else{
- //no contraction of biceps, thus no motoraction.
- pwm_motor2=0;
- pwm_motor1=0;
- start_motor = true;
+ }
+ }
+ else{
+ //no contraction of biceps, thus no motoraction.
+ pwm_motor2=0;
+ pwm_motor1=0;
+ start_motor = true; //every time the motor is off, the bool is reset so that the reference void can start when the motor starts
-
- }
+ }
-}//while true closed
+ } //while true closed
} //int main closed