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
Revision 60:c165691c4e86, committed 2016-11-14
- Comitter:
- daniQQue
- Date:
- Mon Nov 14 12:13:38 2016 +0000
- Parent:
- 59:1725a3f02f37
- Commit message:
- met buttons
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1725a3f02f37 -r c165691c4e86 main.cpp
--- a/main.cpp Tue Nov 08 20:31:45 2016 +0000
+++ b/main.cpp Mon Nov 14 12:13:38 2016 +0000
@@ -9,34 +9,23 @@
//=======================================================================================================================================================
//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++
-
//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
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;
//Monitoring
-HIDScope scope(5); //open 5 channels in hidscope
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
+DigitalIn button__right_biceps(D9);
+DigitalIn button__left_biceps(PTC12);
+InterruptIn button_switch(SW3);
//motors
DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
@@ -54,22 +43,8 @@
//=======================================================================================================================================================
//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
-
-
-//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; //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.18 pwm at start
@@ -105,32 +80,13 @@
//=======================================================================================================================================================
-//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
-
-//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
-
-//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
-
-//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
-
//=======================================================================================================================================================
//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){
+ if(button_switch==0){
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.
@@ -139,92 +95,28 @@
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("If you push button 1 , the robot will go right \r\n");
+ pc.printf("If you push button 2, 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("If you push button 1, the robot will go up \r\n");
+ pc.printf("If you push button 2, 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.
-
-//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
- 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 left arm read+filtering
- double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
- double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
- double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
- double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
- double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
-
- //creating of on/off signal with the created on/off signals, with if statement for right arm!
- //signal substraction of filter biceps and triceps. right Biceps + left biceps -
- double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
- 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;
- }
-
- else if (signal_biceps_sum<treshold_biceps_left){
- onoffsignal_biceps=-1;
- }
-
- else{
- onoffsignal_biceps=0;
- }
-
- //creating on/off signal for switch (left arm)
-
- if (bicepstriceps_rightarm<treshold_triceps){
- switch_signal_triceps=1;
- }
-
- 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.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){ //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){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
+ if (button__left_biceps==0 && 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
@@ -248,7 +140,7 @@
d_ref = d_ref; //if there is no signal, the referance angle is constant
}
- 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
+ if (button__right_biceps==0 && 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;
@@ -284,151 +176,9 @@
void encoders(){
counts_encoder1 = Encoder1.getPulses();
rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
- rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; //calculate the angle in radians
+ rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
+
}
-
-//=======================================================================================================================================================
-
-//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
//=======================================================================================================================================================
@@ -436,18 +186,13 @@
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);
@@ -476,8 +221,8 @@
while (true) { //neverending loop
-
- if (onoffsignal_biceps==-1){ //left biceps contracted
+ button_switch.fall(&switch_function);
+ if (button__left_biceps==0){ //left biceps contracted
if (switch_signal%2==0){ //switch even
@@ -502,7 +247,7 @@
}
}
- else if (onoffsignal_biceps==1){ //right biceps contracted
+ else if (button__right_biceps==0){ //right biceps contracted
if (switch_signal%2==0){ //switch signal even
speedmotor1=controlOutput;