2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-11-03
- Revision:
- 64:21fbff25d80b
- Parent:
- 63:08357f5c497b
File content as of revision 64:21fbff25d80b:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" #include "Servo.h" #include "QEI.h" #include "math.h" #include <string> /*-------------------------------------------------------------------------------------------------------------------- -------------------------------- BIOROBOTICS GROUP 14 ---------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Define important constants in memory #define PI 3.14159265 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate #define CONTROL_RATE 0.01 //100 Hz Control rate #define SERVO_RATE 0.05 //50 Hz Servo Control rate #define ENCODER_CPR 4200 //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2), #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly /*-------------------------------------------------------------------------------------------------------------------- ---- OBJECTS --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ MODSERIAL pc(USBTX,USBRX); //serial communication //Debug LEDs DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); DigitalOut blue(LED_BLUE); //EMG shields AnalogIn emg1(A0); //Analog input - Right Flexor EMG AnalogIn emg2(A1); //Analog input - Right Extensor EMG AnalogIn emg3(A2); //Analog input - Left Flexor EMG AnalogIn emg4(A3); //Analog input - Left Extensor EMG Ticker sample_timer; //Ticker for EMG sampling Ticker control_timer; //Ticker for control loop Ticker servo_timer; //Ticker for servo control Ticker debug_timer; //Ticker for debug printf //Turn hidscope off if not needed anymore //HIDScope scope(2); //Scope 4 channels //Encoders QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1 PwmOut pwm_motor1(D6); //PWM motor 1 PwmOut pwm_motor2(D5); //PWM motor 2 Servo servoPwm(D11); //PWM servomotor DigitalOut dir_motor1(D7); //Direction motor 1 DigitalOut dir_motor2(D4); //Direction motor 2 //Limit Switches InterruptIn shoulder_limit(D2); //using BioShield buttons InterruptIn elbow_limit(D3); //using BioShield buttons //Other buttons InterruptIn debugbtn(PTA4); //using FRDM buttons - debug on or off DigitalIn button2(PTC6); //using FRDM buttons - not used /*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m \ 0 3 3 - ESC [ 3 0 m - black [ 3 1 m - red [ 3 2 m - green [ 3 3 m - yellow [ 3 4 m - blue [ 3 5 m - magenta [ 3 6 m - cyan [ 3 7 m - white [ 0 m - reset attributes Put the text you want to color between GREEN_ and _GREEN */ string GREEN_ = "\033[32m"; //esc - green string _GREEN = "\033[0m"; //esc - reset /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Debugging on or off volatile bool debug = true; //default is on //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation. double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0; double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=0; double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=0; double emg_extens; double extens_power; double extensMVC = 0; double extensmin=0; //Normalize and compare variables double biceps, triceps, flexor, extens; //Storage for normalized emg double xdir, ydir; //EMG reference position directions double xpower, ypower; //EMG reference magnitude double dx, dy; //Integral double emg_control_time; //Elapsed time in EMG control //Threshold moving average window const int window=30; //30 samples int i=0; //movavg array index double movavg1[window]; //moving average arrays with size of window double movavg2[window]; double movavg3[window]; double movavg4[window]; double biceps_avg, triceps_avg,flexor_avg, extens_avg; //sum divided by window size int muscle; //Muscle selector for MVC measurement, 1 = first emg etc. double calibrate_time; //Elapsed time for each measurement //PID variables double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error motor 1 const double m1_kp=1; const double m1_ki=0.01; const double m1_kd=0.05; //Proportional, integral and derivative gains. double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error motor 2 const double m2_kp=1; const double m2_ki=0.01; const double m2_kd=0.05; //Proportional, integral and derivative gains. //Calibration bools, checks if elbow/shoulder limits are hit and if calibration is complete volatile bool done1 = false; volatile bool done2 = false; volatile bool calibrating = false; //highpass filter 20 Hz const double high_b0 = 0.956543225556877; const double high_b1 = -1.91308645113754; const double high_b2 = 0.956543225556877; const double high_a1 = -1.91197067426073; const double high_a2 = 0.9149758348014341; //notchfilter 50Hz /* Method = Butterworth Biquad = Yes Stable = Yes Sampling Frequency = 500Hz Filter Order = 2 Band Frequencies (Hz) Att/Ripple (dB) Biquad #1 Biquad #2 1 0, 48 0.001 Gain = 1.000000 Gain = 0.973674 2 49, 51 -60.000 B = [ 1.00000000000, -1.61816176147, 1.00000000000] B = [ 1.00000000000, -1.61816176147, 1.00000000000] 3 52, 250 0.001 A = [ 1.00000000000, -1.58071559235, 0.97319685401] A = [ 1.00000000000, -1.61244708381, 0.97415116257] */ //biquad 1 const double notch1gain = 1.000000; const double notch1_b0 = 1; const double notch1_b1 = -1.61816176147 * notch1gain; const double notch1_b2 = 1.00000000000 * notch1gain; const double notch1_a1 = -1.58071559235 * notch1gain; const double notch1_a2 = 0.97319685401 * notch1gain; //biquad 2 const double notch2gain = 0.973674; const double notch2_b0 = 1 * notch2gain; const double notch2_b1 = -1.61816176147 * notch2gain; const double notch2_b2 = 1.00000000000 * notch2gain; const double notch2_a1 = -1.61244708381 * notch2gain; const double notch2_a2 = 0.97415116257 * notch2gain; //lowpass filter 7 Hz - envelope const double low_b0 = 0.000119046743110057; const double low_b1 = 0.000238093486220118; const double low_b2 = 0.000119046743110057; const double low_a1 = -1.968902268531908; const double low_a2 = 0.9693784555043481; //Derivative lowpass filter 60 Hz - remove derivative error noise const double derlow_b0 = 0.027859766117136; const double derlow_b1 = 0.0557195322342721; const double derlow_b2 = 0.027859766117136; const double derlow_a1 = -1.47548044359265; const double derlow_a2 = 0.58691950806119; //Forward Kinematics const double l1 = 0.25; const double l2 = 0.25; //Arm lengths double current_x; double current_y; //Current position double theta1; double theta2; double theta3; //Current angles double deltatheta1; double deltatheta2; //Change in angles compared to mechanical lower limit - for servo double servopos; //servo position in usec double rpc; //Encoder resolution: radians per count //Reference position double x; double y; //Select whether to use Trig (1) or DLS method (2), emg control true or false int control_method; bool emg_control; //Inverse Kinematics - Trig / Gonio method. //First convert reference position to angle needed, then compare that angle to current angle. double reftheta1; double reftheta2; //reference angles double costheta1; double sintheta1; //helper variables double costheta2; double sintheta2; // //Inverse Kinematics - Damped least squares method. //Angle error is directly calculated from position error: dq = [DLS matrix] * position_error // |DLS1 DLS2| double dls1, dls2, dls3, dls4; //DLS matrix: |DLS3 DLS4| double q1_error, q2_error; //Angle errors double x_error, y_error; //Position errors double lambda=0.1; //Damping constant double powlambda2, powlambda4; //helper variables to reduce calculation time double powl1, powl2; // double sintheta1theta2, costheta1theta2; // //Mechanical Limits (pulse counts and radians) int theta1_cal, theta2_cal; //Pulse counts at mechanical limits. double theta1_lower=0.698132, theta1_upper=2.35619; //motor1: lower limit 40 degrees, upper limit 135 double theta2_lower=0.750492, theta2_upper=2.40855; //motor2: lower limit 43 degrees, upper limit 138 degrees. /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Using biquadFilter library //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. //Each biquadFilter object can only be used by one signal - memory variables are unique for each emg. //This means 4 biquads for each muscle. //Biceps biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope //Triceps biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope //Flexor biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope //Extensor biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope //PID filter (lowpass 60 Hz, 6*crossoverfreq) biquadFilter derfilter1( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter biquadFilter derfilter2( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ --------------------------------------------------------------------------------------------------------------------*/ void sample_filter(void); //Sampling and filtering void control(); //Control loop - reference -> error -> pid -> motor output void servo_control(); //Mouse alignment feed forward for servo void calibrate_emg(); //Instructions + measurement of Min and MVC of each muscle void emg_mvc(); //Helper funcion for storing MVC value void emg_min(); //Helper function for storing Min value void calibrate_arm(void); //Calibration of the arm with limit switches void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker void stop_sampling(void); //Stops sample_filter void start_control(void); //Attaches the control function to a 100Hz ticker and the servo_control to a 50Hz ticker void stop_control(void); //Stops control and servo control //Keeps the input between min and max value void keep_in_range(double * in, double min, double max); //Reusable PID controller, previous and integral error need to be passed by reference. //Need two because of different derivative filter biquads. double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev); double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev); //Menu functions void debugging(); //Prints useful debug parameters if debugging is turned on. void debug_trigger(); //Triggers debug on or off - attach to 1 hz ticker void mainMenu(); //Prints the main menu void caliMenu(); //Prints the calibration menu void controlMenu(); //Prints the control menu with WASD button control void controlButtons(); // void clearTerminal(); //Clears the putty window void emgInstructions(); //Optional - prints instructions for preparing the skin for EMG void titleBox(); //Prints a fancy box. To view correctly putty translation-character set needs to be set to CP437. //Limit switches - power off motors if switches hit (rising edge interrupt) void calibrate(void); //Rotates arm clockwise slowly untill switches are hit void shoulder(); //Functions attached to buttons - when hit: encoder pulses are set to mechanical limit angles and motor turned off. void elbow(); // /*-------------------------------------------------------------------------------------------------------------------- ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ int main() { pc.baud(115200); //serial baudrate red=1; green=1; blue=1; //Make sure debug LEDs are off servoPwm.Enable(602,20000); //Start position servo, PWM period in usecs //Set PwmOut frequency to 50 Hz pwm_motor1.period(0.02); pwm_motor2.period(0.02); debugbtn.fall(&debug_trigger); //turn debug printf's on or off clearTerminal(); //Clear the putty window // make a menu, user has to initiate next step titleBox(); mainMenu(); char command=0; //Main menu: while(command != 'Q' && command != 'q') { if(pc.readable()){ command = pc.getc(); switch(command){ //User chooses 'c' case 'c': case 'C': { pc.printf("\n\r => You chose calibration.\r\n\n"); caliMenu(); char command2=0; //Calibration menu: while(command2 != 'B' && command2 != 'b'){ command2 = pc.getc(); switch(command2){ //user chooses 'a' case 'a': case 'A': pc.printf("\n\r => Arm Calibration Starting... please wait \n\r"); calibrate_arm(); wait(1); caliMenu(); break; //user chooses 'e' case 'e': case 'E': pc.printf("\n\r => EMG Calibration Starting... please wait \n\r"); wait(1); emgInstructions(); calibrate_emg(); pc.printf("\n\r------------------------- \n\r"); pc.printf("\n\r EMG Calibration complete \n\r"); pc.printf("\n\r------------------------- \n\r"); caliMenu(); break; //user chooses 'b' case 'b': case 'B': pc.printf("\n\r => Going back to main menu.. \n\r"); mainMenu(); break; }//end switch }//end while break; }//end case c C //user chooses 't' case 't': case 'T': pc.printf("=> You chose TRIG button control \r\n\n"); wait(1); emg_control=false; control_method=1; start_control(); wait(1); if(debug) { debug_timer.attach(&debugging,1); } controlButtons(); break; //user chooses 'd' case 'd': case 'D': pc.printf("=> You chose DLS button control \r\n\n"); wait(1); emg_control=false; control_method=2; start_control(); wait(1); if(debug) { debug_timer.attach(&debugging,1); } controlButtons(); break; //user chooses 'e' case 'e': case 'E': pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1); start_sampling(); wait(1); emg_control_time = 0; emg_control=true; control_method=2; start_control(); wait(1); controlButtons(); break; //user chooses 'q' case 'q': case 'Q': break; //other inputs default: pc.printf("=> Invalid Input \n\r"); break; } //end switch } // end if pc readable } // end while loop //When end of while loop reached (user chose quit program). pc.printf("\r\n------------------------------ \n\r"); pc.printf("Program Offline \n\r"); pc.printf("Reset to start\r\n"); pc.printf("------------------------------ \n\r"); } //end of main /*-------------------------------------------------------------------------------------------------------------------- ---- FUNCTIONS ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Debug function: prints important variables to check if things are calculating correctly - find errors void debug_trigger(){ debug=!debug; printf("Debug triggered: %s \r\n", debug ? "ON" : "OFF"); } void debugging() { if(debug==true){ //Choose which debugging values to show: pc.printf("\r\nRef pos: %f and %f \r\n",x,y); pc.printf("Cur pos: %f and %f \r\n",current_x,current_y); //pc.printf("Pos Error: %f and %f \r\n",x_error,y_error); //pc.printf("Cur angles: %f and %f \r\n",theta1,theta2); //pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4); //pc.printf("Error angles: %f and %f \r\n",m1_error,m2_error); pc.printf("PID output: %f and %f \r\n",u1,u2); pc.printf("----------------------------------------\r\n"); pc.printf("Buffer1: %f \r\n",biceps_avg); pc.printf("Buffer2: %f \r\n",triceps_avg); pc.printf("Buffer3: %f \r\n",flexor_avg); pc.printf("Buffer4: %f \r\n",extens_avg); //pc.printf("----------------------------------------\r\n"); //pc.printf("Theta3: %f \r\n",theta3); //pc.printf("Servopos us: %f \r\n",servopos); pc.printf("----------------------------------------\r\n"); } } //Calculates how much servo needs to move to keep mouse aligned void servo_control(){ //Servo alignment //When shoulder or elbow angle increases from starting position --> servo needs to turn counterclockwise to keep mouse aligned. deltatheta1 = theta1 - theta1_lower; deltatheta2 = theta2 - theta2_lower; theta3 = deltatheta1 + deltatheta2; servopos = (2100/PI)*theta3 + 600; servoPwm.SetPosition(servopos); } //Use WASD keys to change reference position, x is a and d, y is w and s. void controlButtons() { controlMenu(); debug_timer.attach(&debugging,1); //debug printing at 1 hz char c=0; //control menu while(c != 'Q' && c != 'q') { if( pc.readable() ){ c = pc.getc(); switch (c) { //user chooses 'd' case 'd' : x = x + 0.01; break; //user chooses 'a' case 'a' : x-=0.01; break; //user chooses 'w' case 'w' : y+=0.01; break; //user chooses 's' case 's' : y-=0.01; break; case 'g' : debug=true; break; //user chooses 'q' case 'q' : case 'Q' : pc.printf("=> Quitting control... \r\n"); wait(1); stop_sampling(); stop_control(); debug_timer.detach(); pc.printf("Sampling and Control detached \n\r"); wait(1); pc.printf("Returning to Main Menu \r\n\n"); wait(1); mainMenu(); break; }//end switch } //end if pc readable } //end of while loop } //Sample and Filter void sample_filter(void) { emg_biceps = emg1.read(); //Biceps emg_triceps = emg2.read(); //Triceps emg_flexor = emg3.read(); //Flexor emg_extens = emg4.read(); //Extensor //Filter: high-pass -> notch -> rectify -> lowpass //each muscle need its own biquad per filter biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens); biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power); biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power); biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power); biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power); //send filtered emg to scope /*scope.set(0,biceps_power); scope.set(1,triceps_power); scope.set(2,flexor_power); scope.set(3,extens_power); scope.send(); */ //send normalized emg to scope //scope.set(0,biceps); //scope.set(1,triceps); //scope.set(2,flexor); //scope.set(3,extens); //scope.send(); } //Send arm to mechanical limits, and set encoder to these angles. void calibrate_arm(void) { pc.printf("Calibrate_arm() entered\r\n"); calibrating = true; done1 = false; done2 = false; pc.printf("To start arm calibration, press any key =>"); pc.getc(); pc.printf("\r\n Calibrating... \r\n"); red=0; blue=0; //Debug light is purple during arm calibration dir_motor1=0; //cw dir_motor2=1; //cw control_timer.attach(&calibrate,CONTROL_RATE); while(calibrating){ shoulder_limit.fall(&shoulder); elbow_limit.fall(&elbow); if(done1 && done2){ pwm_motor2=0; control_timer.detach(); //Leave while loop when both limits are reached red=1; blue=1; //Turn debug light off when calibration complete //set reference position to mechanical limits calibrating=false; x = l1 * cos(theta1_lower) + l2 * cos(theta1_lower + theta2_lower); y = l1 * sin(theta1_lower) + l2 * sin(theta1_lower + theta2_lower); //x = 0.2220; position at limits //y = 0.4088; } } pc.printf("Current pulsecount motor 1: %i, motor 2: %i \r\n",Encoder1.getPulses(),Encoder2.getPulses()); pc.printf("Current reference. X: %f, Y: %f \r\n",x,y); wait(1); pc.printf("\n\r-------------------------- \n\r"); pc.printf(" Arm Calibration Complete\r\n"); pc.printf("-------------------------- \n\r"); } //Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit void shoulder() { pwm_motor1=0; done1 = true; pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) ) theta1_cal = floor(theta1_lower * (4200/(2*PI))); Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses(int p) } void elbow(){ pwm_motor2=0; done2 = true; pc.printf("Elbow Limit hit - shutting down motor 2\r\n"); //Mechanical limit 43 degrees -> 43*(4200/360) = 350 theta2_cal = floor(theta2_lower * (4200/(2*PI))); Encoder2.setPulses(theta2_cal); //edited QEI library: added setPulses(int p) } //Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker void calibrate(void){ if(done1==false){ //if motor 1 limit has not been hit yet pwm_motor1=0.1; //move upper arm slowly cw // pc.printf("Motor 1 running %f \r\n"); } if(done1==true && done2==false){ //if limit motor 1 has been hit pwm_motor1=0; //stop motor1 pwm_motor2=0.1; //start moving forearm slowly cw // pc.printf("Motor 2 running %f \r\n"); } } //EMG calibration void calibrate_emg() { Ticker timer; start_sampling(); wait(0.5); /******************* All muscles: minimum measurement *************************/ pc.printf("Start of minimum measurement, relax all muscles.\r\n"); wait(0.5); pc.printf(" Press any key to begin... "); wait(1); char input; input=pc.getc(); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); pc.printf(" \r\n Measuring... \r\n"); timer.attach(&emg_min,SAMPLE_RATE); wait(3); timer.detach(); pc.printf("\r\n Measurement complete."); wait(1); pc.printf("\r\n Right Flexor min = %f \r\n",bicepsmin); pc.printf("\r\n Right Extensor min = %f \r\n",tricepsmin); pc.printf("\r\n Left Flexor min = %f \r\n",flexormin); pc.printf("\r\n Left Extensor min = %f \r\n",extensmin); wait(1); /******************************** Done ****************************************/ pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(0.5); pc.printf("+ means current sample is higher than stored MVC\r\n"); pc.printf("- means current sample is lower than stored MVC\r\n"); wait(1); calibrate_time=0; /********************* 1st channel: MVC measurement ***************************/ pc.printf("\r\n---------------------\r\n "); pc.printf("Right Flexor is first.\r\n "); pc.printf("--------------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); muscle=1; timer.attach(&emg_mvc,SAMPLE_RATE); wait(3); timer.detach(); pc.printf("\r\n Measurement complete."); wait(1); pc.printf("\r\n Right Flexor MVC = %f \r\n",bicepsMVC); wait(1); pc.printf("Measured time: %f seconds \r\n\n",calibrate_time); calibrate_time=0; /******************************** Done ****************************************/ /********************* 2nd channel: MVC measurement ***************************/ muscle=2; pc.printf("\r\n-------------------\r\n "); pc.printf("Right Extensor is next.\r\n "); pc.printf("---------------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); timer.attach(&emg_mvc,0.002); wait(3); timer.detach(); pc.printf("\r\n Right Extensor MVC = %f \r\n",tricepsMVC); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; /******************************** Done ****************************************/ /********************* 3rd channel: MVC measurement ***************************/ muscle=3; pc.printf("\r\n--------------------\r\n "); pc.printf("Left Flexor is next.\r\n "); pc.printf("--------------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); timer.attach(&emg_mvc,0.002); wait(3); timer.detach(); pc.printf("\r\n Left Flexor MVC = %f \r\n",flexorMVC); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; /******************************** Done ****************************************/ /********************* 4th channel: MVC measurement ***************************/ muscle=4; pc.printf("\r\n--------------------\r\n "); pc.printf("Left Extensor is next.\r\n "); pc.printf("--------------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); timer.attach(&emg_mvc,0.002); wait(3); timer.detach(); pc.printf("\r\n Left Extensor MVC = %f \r\n",extensMVC); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; /******************************** Done ****************************************/ //Stop sampling: detach ticker stop_sampling(); } //EMG Maximum Voluntary Contraction measurement void emg_mvc() { if(muscle==1){ if(biceps_power>bicepsMVC){ pc.printf("%s+ %s",GREEN_,_GREEN); bicepsMVC=biceps_power; } else pc.printf("- "); } if(muscle==2){ if(triceps_power>tricepsMVC){ pc.printf("%s+ %s",GREEN_,_GREEN); tricepsMVC=triceps_power; } else pc.printf("- "); } if(muscle==3){ if(flexor_power>flexorMVC){ pc.printf("%s+ %s",GREEN_,_GREEN); flexorMVC=flexor_power; } else pc.printf("- "); } if(muscle==4){ if(extens_power>extensMVC){ pc.printf("%s+ %s",GREEN_,_GREEN); extensMVC=extens_power; } else pc.printf("- "); } //} calibrate_time = calibrate_time + 0.002; } //Minimum measurement during relaxation void emg_min() { if(biceps_power>bicepsmin){ bicepsmin=biceps_power; } if(triceps_power>tricepsmin){ tricepsmin=triceps_power; } if(flexor_power>flexormin){ flexormin=flexor_power; } if(extens_power > extensmin){ extensmin = extens_power; } calibrate_time = calibrate_time + 0.002; } //PID motor 1 - Input error and Kp, Kd, Ki, output control signal double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev) { // Derivative double e_der = (error-e_prev)/ CONTROL_RATE; e_der = derfilter1.step(e_der); //derfilter1 - seperate 60hz low-pass biquad for this PID e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; // PID return kp*error + ki*e_int + kd * e_der; } //PID for motor 2 - needed because of biquadfilter memory variables double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev) { // Derivative double e_der = (error-e_prev)/ CONTROL_RATE; e_der = derfilter2.step(e_der); //derfilter2 - seperate 60hz low-pass biquad for this PID e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; // PID return kp*error + ki*e_int + kd * e_der; } //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position, //convert to angles, send error through pid(), send PWM and DIR to motors void control() { /********************* START OF EMG REFERENCE CALCULATION ***************************/ if(emg_control==true){ //TODO some idle time with static reference before EMG kicks in. or go to reference in the first 5 seconds. //normalize emg to value between 0-1 biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin); triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin); flexor = (flexor_power - flexormin) / (flexorMVC - flexormin); extens = (extens_power - extensmin) / (extensMVC - extensmin); //make sure values stay between 0-1 over time keep_in_range(&biceps,0,1); keep_in_range(&triceps,0,1); keep_in_range(&flexor,0,1); keep_in_range(&extens,0,1); //threshold detection! buffer or two thresholds? If average of 100 samples > threshold, then muscle considered on. movavg1[i]=biceps; //fill array with 100 normalized samples movavg2[i]=triceps; movavg3[i]=flexor; movavg4[i]=extens; i++; if(i==window){ //if array full,set i to 0 i=0; } //Sum all values in the array. The sum needs to be overwritten or it will continue to sum the next 100 samples on top it //and will grow out of control. //So the variable name for the sum in the for loop is not really correct since the average is calculated after the loop. for(int j = 0; j < window; j++){ biceps_avg += movavg1[j]; triceps_avg += movavg2[j]; flexor_avg += movavg3[j]; extens_avg += movavg4[j]; } biceps_avg = biceps_avg/window; //divide sum by number of samples -> average triceps_avg = triceps_avg/window; flexor_avg = flexor_avg/window; extens_avg = extens_avg/window; emg_control_time += CONTROL_RATE; //Move mouse to starting position - bottom right corner of used workspace - when EMG control starts. After 5 seconds reference can be changed with EMG. if(emg_control_time < 5){ x=0; y=0.3; } else{ //Compare muscle amplitudes and determine their influence on x and y reference position. if (biceps_avg>triceps_avg && biceps_avg > 0.2){ xdir = 0; xpower = biceps_avg;} else if (triceps_avg>biceps_avg && triceps_avg>0.2){ xdir = 1; xpower = triceps_avg;} else xpower=0; if (flexor_avg>extens_avg && flexor_avg > 0.2){ ydir = 0; ypower = flexor_avg; } else if (extens_avg>flexor_avg && extens_avg > 0.2){ ydir = 1; ypower = extens_avg; } else ypower = 0; //power: the longer a signal is active, the further the reference goes. So integrate to determine reference position dx = xpower * CONTROL_RATE * 0.15; //last value is a factor to control how fast the reference (so also end effector) changes dy = ypower * CONTROL_RATE * 0.15; //Direction! Sum dx and dy but make sure xdir and ydir are considered. if (xdir>0) //if x direction of sample is positive, add it to reference position x += dx; else //if x direction of sample is negative, substract it from reference position x += -dx; if (ydir>0) //if y direction of sample is positive, add it to reference position y += dy; else y += -dy; //if y direction of sample is negative, substract it from reference position }//end else control time>5 //now we have x and y -> reference position. Keep in desired range. keep_in_range(&x,-0.5,0); keep_in_range(&y,0.2,0.55); }//end emg_control if /******************************** END OF EMG REFERENCE CALCULATION ****************************************/ //Current position - Encoder counts -> current angle -> Forward Kinematics rpc=(2*PI)/ENCODER_CPR; //radians per count (resolution) - 2pi divided by 4200 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts to get current angles theta2 = Encoder2.getPulses() * rpc; current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); //Forward kinematics for current position current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2); //calculate error (refpos-currentpos) x_error = x - current_x; y_error = y - current_y; /******************************** START OF TRIG INVERSE KINEMATICS ****************************************/ if (control_method==1){ //inverse kinematics (refpos to refangle) costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; //absolute in sqrt to avoid imaginary numbers -> bigger steady state error when reference out of workspace sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) ); //Reference angle 2 reftheta2 = atan2(sintheta2,costheta2); double k1 = l1 + l2*costheta2; double k2 = l2*sintheta2; //Reference angle 1 reftheta1 = atan2(y, x) - atan2(k2, k1); /* alternative, but extra square root costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) ); reftheta1 = atan2(sintheta1,costheta1); */ //Angle error m1_error = reftheta1-theta1; m2_error = reftheta2-theta2; }// end control method 1 /******************************** END OF TRIG INVERSE KINEMATICS ****************************************/ /******************************** START OF DLS INVERSE KINEMATICS ****************************************/ if(control_method==2){ //inverse kinematics (error in position to error in angles) powlambda2 = pow(lambda,2); //help functions to reduce amount of calculations powlambda4 = pow(lambda,4); // powl2 = pow(l2,2); // powl1 = pow(l1,2); // sintheta1theta2 = sin(theta1 + theta2); // costheta1theta2 = cos(theta1 + theta2); // //calculate DLS matrix dls1= -(l2*powlambda2*sintheta1theta2 + l1*powlambda2*sin(theta1) + l1*powl2*pow(costheta1theta2,2)*sin(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls2= (l2*powlambda2*costheta1theta2 + l1*powlambda2*cos(theta1) + l1*powl2*pow(sintheta1theta2,2)*cos(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls3= -(l2*powlambda2*sintheta1theta2 - l1*powl2*pow(costheta1theta2,2)*sin(theta1) + powl1*l2*sintheta1theta2*pow(cos(theta1),2) - powl1*l2*costheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); dls4= (l2*powlambda2*costheta1theta2 - l1*powl2*pow(sintheta1theta2,2)*cos(theta1) + powl1*l2*costheta1theta2*pow(sin(theta1),2) - powl1*l2*sintheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1)); //calculate angle errors from position error q1_error = dls1 * x_error + dls2 * y_error; q2_error = dls3 * x_error + dls4 * y_error; //Angle error m1_error = q1_error; m2_error = q2_error; }//end control method 2 /******************************** END OF DLS INVERSE KINEMATICS ****************************************/ /* Set limits to the error! motor1: lower limit 40 degrees, upper limit 135 motor2: lower 43 degrees, upper limit 138 degrees. */ //lower limits: Negative error not allowed to go further. if (theta1 < theta1_lower){ if (m1_error > 0) m1_error = m1_error; else m1_error = 0; } if (theta2 < theta2_lower){ if (m2_error > 0) m2_error = m2_error; else m2_error = 0; } //upper limit: Positive error not allowed to go further if (theta1 > theta1_upper){ if (m1_error < 0 ) m1_error = m1_error; else m1_error = 0; } if (theta2 > theta2_upper){ if (m2_error < 0 ) m2_error = m2_error; else m2_error = 0; } //PID controller u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 //keep PWM between limits, sign = direction keep_in_range(&u1,-0.3,0.3); keep_in_range(&u2,-0.3,0.3); //send PWM and DIR to motor 1 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. pwm_motor1.write(abs(u1)); //send PWM and DIR to motor 2 dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below pwm_motor2.write(abs(u2)); /*if(u1 > 0) { dir_motor1 = 0; else{ dir_motor1 = 1; } } pwm_motor1.write(abs(u1)); if(u2 > 0) { dir_motor1 = 1; else{ dir_motor1 = 0; } } pwm_motor1.write(abs(u2));*/ } //Prints the main menu void mainMenu() { //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); pc.putc(186); pc.printf(" Main Menu "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ pc.putc(205); } pc.putc(188); pc.printf("\n\r"); //endbox wait(0.2); pc.printf("[C]alibration\r\n"); wait(0.2); pc.printf("[T]RIG Control with WASD\r\n"); wait(0.2); pc.printf("[D]LS Control with WASD\r\n"); wait(0.2); pc.printf("[E]MG Control - DLS \r\n"); wait(0.2); pc.printf("[Q]uit Robot Program\r\n"); wait(0.2); pc.printf("Please make a choice => \r\n"); } //Start sampling void start_sampling(void) { sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG //Debug LED will be green when sampling is active green=0; pc.printf("||- started sampling -|| \r\n"); } //stop sampling void stop_sampling(void) { sample_timer.detach(); //Debug LED will be turned off when sampling stops green=1; pc.printf("||- stopped sampling -|| \r\n"); } //Start control void start_control(void) { control_timer.attach(&control,CONTROL_RATE); //100 Hz control servo_timer.attach(&servo_control, SERVO_RATE); //50 Hz control //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan. blue=0; pc.printf("||- started control -|| \r\n"); } //stop control void stop_control(void) { control_timer.detach(); servo_timer.detach(); pwm_motor1=0; pwm_motor2=0; //Debug LED will be off when control is off blue=1; pc.printf("||- stopped control -|| \r\n"); } //Clears the putty (or other terminal) window void clearTerminal() { pc.putc(27); pc.printf("[2J"); // clear screen pc.putc(27); // ESC pc.printf("[H"); // cursor to home } //Prints control menu void controlMenu() { //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); pc.putc(186); pc.printf(" Control Menu "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ pc.putc(205); } pc.putc(188); pc.printf("\n\r"); //endbox pc.printf("A) Move Arm Left\r\n"); pc.printf("D) Move Arm Right\r\n"); pc.printf("W) Move Arm Up\r\n"); pc.printf("S) Move Arm Down\r\n"); pc.printf("g) Turn debugging on / off \r\n"); pc.printf("q) Exit \r\n"); pc.printf("Please make a choice => \r\n"); } //prints calibration menu void caliMenu(){ //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); pc.putc(186); pc.printf(" Calibration Menu "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ pc.putc(205); } pc.putc(188); pc.printf("\n\r"); //endbox pc.printf("[A]rm Calibration\r\n"); pc.printf("[E]MG Calibration\r\n"); pc.printf("[B]ack to main menu\r\n"); pc.printf("Please make a choice => \r\n"); } //prints square title box void titleBox(){ //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186); pc.printf("\n\r"); pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ pc.putc(205); } pc.putc(188); pc.printf("\n\r"); //endbox } //prints emg instructions void emgInstructions(){ pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1); //pc.printf(" Check whether EMG signal looks normal. \n\r "); wait(1); pc.printf("\r\n To calibrate the EMG signals we will measure: \n\r "); pc.printf("- Minimum amplitude, while relaxing all muscles. \n\r "); pc.printf("- Maximum Voluntary Contraction of each muscle. \n\r"); wait(1); pc.printf("\r\nFor the MVC you need to flex the mentioned muscle for 3 seconds \n\r"); wait(0.5); pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(0.5); } //keeps input limited between min max void keep_in_range(double * in, double min, double max) { *in > min ? *in < max? : *in = max: *in = min; }