met knopjes, voor Wubs, zit PID in dus restrictie.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 43:7d0b2dc05b80
- Parent:
- 42:7164ccd2aa14
--- 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