pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
main.cpp
- Committer:
- daniQQue
- Date:
- 2016-11-01
- Revision:
- 43:7d0b2dc05b80
- Parent:
- 42:7164ccd2aa14
File content as of revision 43:7d0b2dc05b80:
//libraries #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 //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++ //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 // 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 //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; 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 //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); //voids //to make a tickerfunction for speedmeasurment void speed_sampling() // maakt een ticker functie aan { tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is } //void to make the switch between the motors with triceps void SwitchN() { if(switch_signal_triceps==1) { 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; } } } //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 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); //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; } //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 (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 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 }//int main closed