Floor Couwenberg
/
another_try_from_scratch_on_emg
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
another_try_from_scratch_on_emg
by 
sibren vuurberg
Diff: main.cpp
- Revision:
- 43:7d0b2dc05b80
- Parent:
- 42:7164ccd2aa14
diff -r 7164ccd2aa14 -r 7d0b2dc05b80 main.cpp
--- a/main.cpp Tue Nov 01 10:30:56 2016 +0000
+++ b/main.cpp Tue Nov 01 14:38:11 2016 +0000
@@ -1,327 +1,486 @@
//libraries
-#include "mbed.h"
-#include "HIDScope.h"
-#include "BiQuad.h"
-#include "MODSERIAL.h"
+#include "mbed.h" //mbed library
+#include "HIDScope.h" //hidscope library
+#include "QEI.h" //library for encoder functions
+#include "BiQuad.h" //library for filtering with BiQuad
+#include "MODSERIAL.h" //library for connect pc with mbed
//Define objects
-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++
+
+ //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++
-InterruptIn button_calibration_biceps (SW3); //button to start calibration biceps
-InterruptIn button_calibration_triceps (SW2); // button to start calibration tricps
+ //Encoder
+ DigitalIn encoder1A(D13);
+ DigitalIn encoder1B(D12);
+ DigitalIn encoder2A(D11);
+ DigitalIn encoder2B(D10);
+
+ //callibration button
+ InterruptIn button_calibration_biceps (SW3); //button to start calibration biceps
+ InterruptIn button_calibration_triceps (SW2); // button to start calibration tricps
-Ticker sample_timer; //ticker
-Ticker switch_function; //ticker
-HIDScope scope(5); //open 3 channels in hidscope
-MODSERIAL pc(USBTX, USBRX); //pc connection
-DigitalOut red(LED_RED);
-DigitalOut green(LED_GREEN);
-DigitalOut blue(LED_BLUE);
-//motors
-DigitalOut richting_motor1(D4);
-PwmOut pwm_motor1(D5);
-DigitalOut richting_motor2(D7);
-PwmOut pwm_motor2(D6);
+ // reset button
+ DigitalIn resetbutton(D9);
+
+ //tickers
+
+ Ticker sample_timer; //ticker for emg sampling
+ Ticker switch_function; //ticker for switch
+ Ticker speed_measuring; //ticker for speed measurment
+
+ //everything for monitoring
+ HIDScope scope(5); //open 3 channels in hidscope
+ MODSERIAL pc(USBTX, USBRX); //pc connection
+ DigitalOut red(LED_RED);
+ DigitalOut green(LED_GREEN);
+ DigitalOut blue(LED_BLUE);
+
+ //motors
+ DigitalOut direction_motor1(D4);
+ PwmOut pwm_motor1(D5);
+ DigitalOut direction_motor2(D7);
+ PwmOut pwm_motor2(D6);
//define variables
-//other
-int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
-int switch_signal_triceps=0; // switching between motors.
-volatile double cut_off_value_biceps_right =0.04; //gespecificeerd door floortje
-volatile double cut_off_value_biceps_left =-0.04;
-volatile double cut_off_value_triceps=0.04; //gespecificeerd door floortje
-double signal_biceps_sum;
-int motorswitch= 0; //start van de teller wordt op nul gesteld
-
-//variables and constants for calibration
-const float percentage_max_triceps=0.3;
-const float percentage_max_biceps =0.3;
-double max_biceps; //calibration maximum biceps
-double max_triceps; //calibration maximum triceps
-//biceps arm 1, right arm
-double emg_biceps_right;
-double emg_filtered_high_biceps_right;
-double emg_abs_biceps_right;
-double emg_filtered_biceps_right;
-double emg_filtered_high_notch_1_biceps_right;
-//double emg_filtered_high_notch_1_2_biceps_right;
+ //for motorcontrol
+ const int cw = 0; // motor should turn clockwise
+ const int ccw =1; // motor should turn counterclockwise
+ const float gearboxratio=131.25; // gearboxratio from encoder to motor
+ const float rev_rond=64.0; // revolutions per round of encoder
+ int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
+ int switch_signal_triceps=0; // switching between motors.
+
+ volatile double cut_off_value_biceps_right = 0.04; //tested, normal values. Can be changed by calibration
+ volatile double cut_off_value_biceps_left = -0.04; //volatiles becaused changen in interrupt
+ volatile double cut_off_value_triceps=-0.03;
+ double signal_biceps_sum;
+ double bicepstriceps_rightarm;
-//triceps arm 1, right arm
-double emg_triceps_right;
-double emg_filtered_high_triceps_right;
-double emg_abs_triceps_right;
-double emg_filtered_triceps_right;
-double emg_filtered_high_notch_1_triceps_right;
-
-//biceps arm 1, left arm
-double emg_biceps_left;
-double emg_filtered_high_biceps_left;
-double emg_abs_biceps_left;
-double emg_filtered_biceps_left;
-double emg_filtered_high_notch_1_biceps_left;
-
-//before abs filtering
+ volatile double voltage_motor1=0.18; //pwm is de pulse with tussen geen ampere en wel ampere motor 1
+ volatile double voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1
+
+ int motorswitch= 0; //start van de teller wordt op nul gesteld
-//b1 = biceps right arm
-BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
-
-//t1= triceps right arm
-BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+ //variables and constants for calibration
+
+ const float percentage_max_triceps=0.3;
+ const float percentage_max_biceps =0.3;
+ double max_biceps; //calibration maximum biceps
+ double max_triceps; //calibration maximum triceps
+
+ //variables for feedback loop, manual calibration
+
+ volatile double current_position_motor1 = 0; // at start, the position is 0. Manual calibration
+ volatile double previous_position_motor1 = 0; // at start, the position is 0. Manual calibration
+ volatile double current_position_motor2 = 0; // at start, the position is 0. Manual calibration
+ volatile double previous_position_motor2 = 0; // at start, the position is 0. Manual calibration
+ volatile double rev_counts_motor1=0;
+ volatile double rev_counts_motor2=0;
+ volatile double counts_encoder1;
+ volatile double counts_encoder2;
+
+ volatile bool tickerflag; //tickerflag is true or false, implicated by bool.
+ volatile double speed_motor1; // speed in rad/s
+ volatile double speed_motor2; // speed in rad/s
+
+
+ //speedmeasuring
+ double ticker_TS=0.001; // time step to derivate position to speed, in seconds.
+ volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
+
+ //resetbuttons
+ volatile double value1_resetbutton = 0;
+ volatile double value2_resetbutton = 0;
+
+ //filter defining
+
+ //b1 = biceps right arm
+ BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //t1= triceps right arm
+ BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //b2= biceps left arm
+ BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
+ BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+
+ //after abs filtering
+ BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
+ BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
+ BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-//b2= biceps left arm
-BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
-BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
+//voids
-//after abs filtering
-BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
-
-//function teller
-void SwitchN() { // maakt simpele functie die 1 bij n optelt
- if(switch_signal_triceps==1)
+ //to make a tickerfunction for speedmeasurment
+
+ void speed_sampling() // maakt een ticker functie aan
{
- motorswitch++;
-
- if (motorswitch%2==0)
- {pc.printf("If you contract the right arm, the robot will go right \r\n");
- pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
- pc.printf("\r\n");
- green=0;
- red=1;
- }
-
- else
- {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
- pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
- pc.printf("\r\n");
- green=1;
- red=0;
-
- }
-
- }
+ tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
}
-//functions which are called in ticker to sample the analog signal
-
-//callibration
-void calibration_biceps(){
- pc.printf("start of calibration biceps, contract maximal \n");
- red=1;
- green=1;
- blue=0;
-
- for(int n =0; n<3000;n++) //read for 2000 samples as calibration
- {
- emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
- emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
- emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
- emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
- emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
-
- if (emg_filtered_biceps_right > max_biceps) //determine what the highest reachable emg signal is
- {
- max_biceps = emg_filtered_biceps_right;
-
- }
- wait(0.001f); //to sample at same freq; 1000Hz
- }
- cut_off_value_biceps_right=percentage_max_biceps*max_biceps;
- cut_off_value_biceps_left=-cut_off_value_biceps_right;
- //toggle lights
- blue=!blue;
-
- pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );
- pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
-
- wait(0.5f);
-
- if (motorswitch%2==0)
- {green=0;
- red=1;}
-
- else {green=1;
- red=0;}
+ //void to make the switch between the motors with triceps
+
+ void SwitchN() {
+ if(switch_signal_triceps==1)
+ {
+ motorswitch++;
- }
-
-void calibration_triceps(){
- red=1;
- green=1;
- blue=0;
-
- pc.printf("start of calibration triceps\r\n");
-
- for(int n =0; n<3000;n++) //read for 2000 samples as calibration
- {
- emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
- emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
- emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
- emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
- emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
-
- if (emg_filtered_triceps_right > max_triceps) //determine what the highest reachable emg signal is
- {
- max_triceps = emg_filtered_triceps_right;
-
- }
- wait(0.001f); //to sample at same freq; 1000Hz
- }
- cut_off_value_triceps=percentage_max_triceps*max_triceps;
- pc.printf(" end of calibration\r\n");
- pc.printf(" change of cv triceps: %f ",cut_off_value_triceps );
- blue=!blue;
- wait(0.5f);
- if (motorswitch%2==0)
- {green=0;
- red=1;}
-
- else {green=1;
- red=0;}
-
- }
-
-void filter(){
- //biceps right arm read+filtering
- emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
- emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
- emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
- emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
- emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
-
- //triceps right arm read+filtering
- emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
- emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
- emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
- emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
- emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
-
- //biceps left arm read+filtering
- emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
- emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
- emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
- emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
- emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
-
- //signal substraction of filter biceps and triceps. right Biceps + left biceps -
- signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
-
- //creating of on/off signal with the created on/off signals, with if statement for right arm!
- if (signal_biceps_sum>cut_off_value_biceps_right)
- {onoffsignal_biceps=1;}
-
- else if (signal_biceps_sum<cut_off_value_biceps_left)
- {
- onoffsignal_biceps=-1;
- }
+ if (motorswitch%2==0)
+ { pc.printf("If you contract the right arm, the robot will go right \r\n");
+ pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
+ pc.printf("\r\n");
+ green=0;
+ red=1;
+ }
else
- {onoffsignal_biceps=0;}
-
- //creating on/off signal for switch (left arm)
-
- if (emg_filtered_triceps_right>cut_off_value_triceps)
- {
- switch_signal_triceps=1;
- }
+ {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
+ pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
+ pc.printf("\r\n");
+ green=1;
+ red=0;
+ }
+
+ }
+ }
- else
- {
- switch_signal_triceps=0;
- }
-
- //send signals to scope
- scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0
- scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1
- scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2
-
-
- scope.send(); //send all the signals to the scope
- }
+ //callibration
+ void calibration_biceps(){
+ pc.printf("start of calibration biceps, contract maximal \n");
+ red=1;
+ green=1;
+ blue=0;
+
+ for(int n =0; n<3000;n++) //read for 2000 samples as calibration
+ {
+ 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);
+ double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
+ double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
+ double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
+
+ if (emg_filtered_biceps_right > max_biceps) //determine what the highest reachable emg signal is
+ {
+ max_biceps = emg_filtered_biceps_right;
+
+ }
+ wait(0.001f); //to sample at same freq; 1000Hz
+ }
+ cut_off_value_biceps_right=percentage_max_biceps*max_biceps;
+ cut_off_value_biceps_left=-cut_off_value_biceps_right;
+ //toggle lights
+ blue=!blue;
+
+ pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );
+ pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
+
+ wait(0.5f);
+
+ if (motorswitch%2==0)
+ {green=0;
+ red=1;}
+
+ else {green=1;
+ red=0;}
+
+ }
+
+ void calibration_triceps(){
+ red=1;
+ green=1;
+ blue=0;
+
+ pc.printf("start of calibration triceps\r\n");
+
+ for(int n =0; n<3000;n++) //read for 2000 samples as calibration
+ {
+ 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);
+ double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
+ double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
+ double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
+
+ if (emg_filtered_triceps_right > max_triceps) //determine what the highest reachable emg signal is
+ {
+ max_triceps = emg_filtered_triceps_right;
+
+ }
+ wait(0.001f); //to sample at same freq; 1000Hz
+ }
+ cut_off_value_triceps=-percentage_max_triceps*max_triceps;
+ pc.printf(" end of calibration\r\n");
+ pc.printf(" change of cv triceps: %f ",cut_off_value_triceps );
+ blue=!blue;
+ wait(0.5f);
+ if (motorswitch%2==0)
+ {green=0;
+ red=1;}
+
+ else {green=1;
+ red=0;}
+
+ }
+
+
+ //sampling emg with filters as defined before
+ 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);
+ double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
+ double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
+ double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
+
+ //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);
+ double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
+ double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
+ double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
+
+ //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);
+ double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
+ double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
+ double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
+
+ //signal substraction of filter biceps and triceps. right Biceps + left biceps -
+ signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
+ 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>cut_off_value_biceps_right)
+ {onoffsignal_biceps=1;}
+
+ else if (signal_biceps_sum<cut_off_value_biceps_left)
+ {
+ onoffsignal_biceps=-1;
+ }
+
+ else
+ {onoffsignal_biceps=0;}
+
+ //creating on/off signal for switch (left arm)
+
+ if (bicepstriceps_rightarm<cut_off_value_triceps)
+ {
+ switch_signal_triceps=1;
+ }
+
+ else
+ {
+ switch_signal_triceps=0;
+ }
+
+ //send signals to scope
+ scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0
+ scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1
+ scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2
+
+
+ scope.send(); //send all the signals to the scope
+ }
//program
int main()
{
-pc.baud(115200); //connect with pc with baudrate 115200
-
-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
-switch_function.attach(&SwitchN,1.0); //switch is every second available
-button_calibration_biceps.fall (&calibration_biceps); //to call calibration biceps, stop everything else
-button_calibration_triceps.fall(&calibration_triceps); //to call calibration triceps, stop everything else
+ pc.baud(115200); //connect with pc with baudrate 115200
+ QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING); // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen.
+ QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
- if (motorswitch%2==0)
- { pc.printf("If you contract the right arm, the robot will go right \r\n");
- pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
- pc.printf("\r\n");
- green=0;
- red=1;
- blue=1;
- }
+ //attach voids to tickers and interrupts
+ speed_measuring.attach(&speed_sampling,ticker_TS); //sampling function, for speed measurement
+ 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
+ switch_function.attach(&SwitchN,1.0); //switch is every second available
+ button_calibration_biceps.fall (&calibration_biceps); //to call calibration biceps, stop everything else
+ button_calibration_triceps.fall(&calibration_triceps); //to call calibration triceps, stop everything else
+
+ //print which motor will be controlled by text and light. Red: up/down=motor 2. Green: left/right=motor 1.
+ if (motorswitch%2==0)
+ { pc.printf("If you contract the right arm, the robot will go right \r\n");
+ pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
+ pc.printf("\r\n");
+ green=0;
+ red=1;
+ blue=1;
+ } //if loop closed
else
- {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
- pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
- pc.printf("\r\n");
- green=1;
- red=0;
- blue=1;
-
- }
+ {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
+ pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
+ pc.printf("\r\n");
+ green=1;
+ red=0;
+ blue=1;
+ } //else loop closed
+
+
//endless loop
-
while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
-
+
+ //encoder and velocity measurement
+ if (tickerflag ==1)
+ {
+ //position change, 'memory function'
+ previous_position_motor1 = current_position_motor1; // zorgt er voor dat de huidige positie wordt gedefineerd als de vorige positie is
+ current_position_motor1 = rev_counts_motor1; // zorgt dat de huidige positie wordt gedefineerd als het huidige aantal rondejs dat gedraaid is
+ previous_position_motor2 = current_position_motor2; // zelfde maar dan voor motor2
+ current_position_motor2 = rev_counts_motor2;
+
+ //speed calculation
+ speed_motor1=((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
+ speed_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
+
+ tickerflag = 0; // reset de tickerflag weer op 0 zodat het loopje niet wordt doorlopen tot de volgende tick zo kan de tijd tussen het lopen van ieder loopje gecontroleerd worden
+ } //if tickerflag==1 closed
+
+ //control and monitor motor with EMG signal, with build in restrictions.
+
+ //left biceps contracted:
+ if (onoffsignal_biceps==-1) //left biceps contracted
+ { //open if loop for left biceps
- if (onoffsignal_biceps==-1) // als s ingedrukt wordt gebeurd het volgende
- {
- if (motorswitch%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
- {
- richting_motor1 = 1;
- pwm_motor1 = 1;
-
-
-
- }
+ if (motorswitch%2==0) //-3.4 is limitationpoint, when motor turns clockwise
+ {
+ direction_motor1 = cw;
+ pwm_motor1 = voltage_motor1;
+ counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ value1_resetbutton = 0;
+ } //if loop closed
+
+ else if (motorswitch%2!=0 && rev_counts_motor2<2.0) //2.0 is limitation for motor 2 when clockwise
+ {
+ direction_motor2 = cw;
+ pwm_motor2 = voltage_motor2;
+ counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
+ value2_resetbutton = 0;
+ } //else if loop closed
+
+ //speed control motor 1
+ if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
+ voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
+
+ else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
+ { voltage_motor1 = voltage_motor1+0.005; }
+
+ //speed control motor 2
+ if (fabs(speed_motor2) > 5.0)
+ { voltage_motor2 = voltage_motor2-0.005; }
+
+ else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
+ { voltage_motor2 = voltage_motor2+0.005; }
+
+ } //if left biceps is contracted closed
+
+
+ //right biceps contract (else if case)
+ else if (onoffsignal_biceps==1) //right biceps contracted
+ {
+ if (motorswitch%2==0) //limitation of motor turning right
+ {
+ direction_motor1 = ccw; //turning right
+ pwm_motor1 = voltage_motor1;
+ counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
+ value1_resetbutton = 0;
+ } //if loop; motor turning right closed
+
+ else if (motorswitch%2!=0 && rev_counts_motor2>-2.0) //limitation of motor turning down
+ {
+ direction_motor2 = ccw;
+ pwm_motor2 = voltage_motor2;
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
+ value2_resetbutton = 0;
+ } //if loop closed
+
+ //speed control motor 1
+ if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
+ voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
+
+ else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
+ { voltage_motor1 = voltage_motor1+0.005; }
+
+ //speed control motor 2
+ if (fabs(speed_motor2) > 5.0)
+ { voltage_motor2 = voltage_motor2-0.005; }
- else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
- {
- richting_motor2 = 1;
- pwm_motor2 = 1;
-
- }
-
- }
- else if (onoffsignal_biceps==1) // als d ingedrukt wordt gebeurd het volgende
- {
- if (motorswitch%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
- {
- richting_motor1 = 0;
- pwm_motor1 = 1;
-
-
- }
- else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
- {
- richting_motor2 = 0;
- pwm_motor2 = 1;
-
-
- }
- }
- else{
-
+ else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
+ { voltage_motor2 = voltage_motor2+0.005; }
+
+ } //else if loop closed; right biceps contracted
+
+ else{ //no signal of both biceps!
+ //encoders because even when signal off, motor can turn for a while.
+
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
pwm_motor2=0;
pwm_motor1=0;
- }
-
-}
+ }
+
+// all lopes are closed, except while (true)!
+//back to beginposition by button !
+
+ // motor 1
+ while(resetbutton==0 && rev_counts_motor1<-0.1 && value1_resetbutton >= 0){
+ direction_motor1 = ccw;
+ pwm_motor1 = 0.1;
+
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+ value1_resetbutton = 1;
+ } //while loop closed
+
+ while (resetbutton==0 && rev_counts_motor1>0.1 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting.
+
+ direction_motor1 = cw;
+ pwm_motor1 = 0.1;
+
+ counts_encoder1 = Encoder1.getPulses();
+ rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+
+ value1_resetbutton = -1;
+ } //while loop closed
+
+ //motor 2
+ while(resetbutton==0 && rev_counts_motor2<-0.1 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2
+
+ direction_motor2 = cw;
+ pwm_motor2 = 0.1;
+ pwm_motor1 = 0;
+
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
+ value2_resetbutton = 1;
+ } //while loop closed
+
+ while (resetbutton==0 && rev_counts_motor2>0.1 && value2_resetbutton <=0){
+
+ direction_motor2 = ccw;
+ pwm_motor2 = 0.1;
+ pwm_motor1=0;
+
+ counts_encoder2 = Encoder2.getPulses();
+ rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
+
+ value2_resetbutton = -1;
+ }//while loop closed
+pc.printf("rev count motor 1 is %f \r\n",rev_counts_motor1);
+pc.printf("speed motor 1: %f\r\n", speed_motor2);
+ }//while true closed
-}
\ No newline at end of file
+}//int main closed