2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-10-21
- Revision:
- 29:948b0b14f6be
- Parent:
- 28:743485bb51e4
- Child:
- 30:a9fdd3202ca2
File content as of revision 29:948b0b14f6be:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.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 ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft #define ENCODER2_CPR 4200 //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 legs DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); DigitalOut blue(LED_BLUE); //EMG shields AnalogIn emg1(A0); //Analog input - Biceps EMG AnalogIn emg2(A1); //Analog input - Triceps EMG AnalogIn emg3(A2); //Analog input - Flexor EMG AnalogIn emg4(A3); //Analog input - Extensor EMG Ticker sample_timer; //Ticker for EMG sampling Ticker control_timer; //Ticker for control loop HIDScope scope(4); //Scope 4 channels // AnalogIn potmeter(A4); //potmeters // AnalogIn potmeter2(A5); // //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 DigitalOut dir_motor1(D7); //Direction motor 1 DigitalOut dir_motor2(D4); //Direction motor 2 //Limit Switches InterruptIn shoulder_limit(D2); //using FRDM buttons InterruptIn elbow_limit(D3); //using FRDM buttons //Other buttons DigitalIn button1(PTA4); //using FRDM buttons DigitalIn button2(PTC6); //using FRDM buttons /*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 ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction double emg_biceps; double biceps_power; double bicepsMVC = 0; double emg_triceps; double triceps_power; double tricepsMVC = 0; double emg_flexor; double flexor_power; double flexorMVC = 0; double emg_extens; double extens_power; double extensMVC = 0; int muscle; //Muscle selector for MVC measurement double calibrate_time; //Elapsed time for each MVC 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 const double m1_kp=0.001; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains. double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. //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 /* ** Primary Filter (H1)** Filter Arithmetic = Floating Point (Double Precision) Architecture = IIR Response = Bandstop 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; //Forward and Inverse Kinematics const double l1 = 0.25; const double l2 = 0.25; double current_x; double current_y; double theta1; double theta2; double dtheta1; double dtheta2; double rpc; double x; double y; double x_error; double y_error; double costheta1; double sintheta1; double costheta2; double sintheta2; /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //Using biquadFilter library //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. //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 ??? Hz) biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ --------------------------------------------------------------------------------------------------------------------*/ void sample_filter(void); //Sampling and filtering void control(); //Control - reference -> error -> pid -> motor output void calibrate_emg(); //Instructions + measurement of MVC of each muscle void emg_mvc(); //Helper funcion for storing MVC 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 void stop_control(void); //Stops control function //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 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev); //Menu functions void mainMenu(); void caliMenu(); void controlMenu(); void controlButtons(); void clearTerminal(); void emgInstructions(); void titleBox(); /*-------------------------------------------------------------------------------------------------------------------- ---- MAIN LOOP ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ int main() { pc.baud(115200); //serial baudrate red=1; green=1; blue=1; //Make sure debug LEDS are off //Set PwmOut frequency to 10k Hz //pwm_motor1.period(PWM_PERIOD); //pwm_motor2.period(PWM_PERIOD); clearTerminal(); //Clear the putty window // make a menu, user has to initiate next step titleBox(); mainMenu(); //caliMenu(); // Menu function //calibrate_arm(); //Start Calibration //calibrate_emg(); x=0.5; y=0; //start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); start_sampling(); wait(60); //char c; char command; while(command != 'Q' && command != 'q') { if(pc.readable()){ command = pc.getc(); switch(command){ case 'c': case 'C': pc.printf("\n\r => You chose calibration.\r\n\n"); caliMenu(); char command2=0; while(command2 != 'B' && command2 != 'b'){ command2 = pc.getc(); switch(command2){ case 'a': case 'A': pc.printf("\n\r => Arm Calibration Starting... please wait \n\r"); calibrate_arm(); wait(1); caliMenu(); break; 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; case 'b': case 'B': pc.printf("\n\r => Going back to main menu.. \n\r"); mainMenu(); break; }//end switch }//end while break; case 's': case 'S': pc.printf("=> You chose control \r\n\n"); wait(1); start_sampling(); wait(1); start_control(); wait(1); controlButtons(); break; case 'R': red=!red; pc.printf("=> Red LED triggered \n\r"); break; case 'G': green=!green; pc.printf("=> Green LED triggered \n\r"); break; case 'B': blue=!blue; pc.printf("=> Blue LED triggered \n\r"); break; case 'q': case 'Q': break; 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) - detach all timers and stop motors. 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 ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ void controlButtons() { controlMenu(); char c=0; while(c != 'Q' && c != 'q') { if( pc.readable() ){ c = pc.getc(); switch (c) { case '1' : x = x + 0.01; //controlMenu(); //running=false; break; case '2' : x-=0.01; //controlMenu(); //running=false; break; case '3' : y+=0.01; //controlMenu(); //running=false; break; case '4' : y-=0.01; //controlMenu(); //running=false; break; case 'q' : case 'Q' : pc.printf("=> Quitting control... \r\n"); wait(1); stop_sampling(); stop_control(); pwm_motor1=0; pwm_motor2=0; pc.printf("Sampling and Control detached \n\r"); wait(1); pc.printf("Returning to Main Menu \r\n\n"); wait(1); mainMenu(); //running = false; break; }//end switch if(c!='q' && c!='Q'){ pc.printf("Reference position: %f and %f \r\n",x,y); pc.printf("Current position: %f and %f \r\n",current_x,current_y); pc.printf("Current angles: %f and %f \r\n",theta1,theta2); pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2); pc.printf("PID output: %f and %f \r\n",u1,u2); pc.printf("----------------------------------------\r\n\n"); } } //end if } //end of while loop } //Sample and Filter void sample_filter(void) { double emg_biceps = emg1.read(); //Biceps double emg_triceps = emg2.read(); //Triceps double emg_flexor = emg3.read(); //Flexor double emg_extens = emg4.read(); //Extensor //Filter: high-pass -> notch -> rectify -> lowpass or moving average // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad? 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); scope.set(0,emg_biceps); scope.set(1,biceps_power); scope.set(2,biceps_power); scope.set(3,biceps_power); scope.send(); /* alternative for lowpass filter: moving average window=30; //30 samples int i=0; //buffer index bicepsbuffer[i]=biceps_power //fill array i++; if(i==window){ i=0; } for(int x = 0; x < window; x++){ avg1 += bicepsbuffer[x]; } avg1 = avg1/window; */ } 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("1) Move Arm Left\r\n"); pc.printf("2) Move Arm Right\r\n"); pc.printf("3) Move Arm Up\r\n"); pc.printf("4) Move Arm Down\r\n"); pc.printf("q) Exit \r\n"); pc.printf("Please make a choice => \r\n"); } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { pc.printf("Calibrate_arm() entered\r\n"); bool calibrating = true; bool done1 = false; bool done2 = false; pc.printf("To start arm calibration, press any key =>"); pc.getc(); pc.printf("\r\n Calibrating... \r\n"); dir_motor1=1; //cw dir_motor2=0; //cw pwm_motor1.write(0.2); //move upper arm slowly cw while(calibrating){ red=0; blue=0; //Debug light is purple during arm calibration if(done1==true){ pwm_motor2.write(0.2); //move forearm slowly cw } //when limit switches are hit, stop motor and reset encoder if(shoulder_limit.read() < 0.5){ //polling pwm_motor1.write(0); Encoder1.reset(); done1 = true; pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); } if(elbow_limit.read() < 0.5){ //polling pwm_motor2.write(0); Encoder2.reset(); done2 = true; pc.printf("Elbow Limit hit - shutting down motor 2. \r\n"); } if(done1 && done2){ calibrating=false; //Leave while loop when both limits are reached red=1; blue=1; //Turn debug light off when calibration complete } }//end while //TO DO: //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) ) //Encoder1.setPulses(100); //edited QEI library: added setPulses() //Encoder2.setPulses(100); //edited QEI library: added setPulses() //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); wait(1); pc.printf("\n\r ------------------------ \n\r"); pc.printf("Arm Calibration Complete\r\n"); pc.printf(" ------------------------ \n\r"); } //EMG Maximum Voluntary Contraction measurement void emg_mvc() { //double sampletime=0; //sampletime=+SAMPLE_RATE; // // if(sampletime<5) //int muscle=1; //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples if(muscle==1){ if(biceps_power>bicepsMVC){ //printf("+ "); printf("%s+ %s",GREEN_,_GREEN); bicepsMVC=biceps_power; } else printf("- "); } if(muscle==2){ if(triceps_power>tricepsMVC){ printf("%s+ %s",GREEN_,_GREEN); tricepsMVC=triceps_power; } else printf("- "); } if(muscle==3){ if(flexor_power>flexorMVC){ printf("%s+ %s",GREEN_,_GREEN); flexorMVC=flexor_power; } else printf("- "); } if(muscle==4){ if(extens_power>extensMVC){ printf("%s+ %s",GREEN_,_GREEN); extensMVC=extens_power; } else printf("- "); } //} calibrate_time = calibrate_time + 0.002; } //EMG calibration void calibrate_emg() { Ticker timer; pc.printf("Testcode calibration \r\n"); wait(1); 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(2); pc.printf("\r\n----------------\r\n "); pc.printf(" Biceps is first.\r\n "); pc.printf("----------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; 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); start_sampling(); muscle=1; timer.attach(&emg_mvc,SAMPLE_RATE); wait(5); timer.detach(); pc.printf("\r\n Measurement complete."); wait(1); pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1); pc.printf("Calibrate_emg() exited \r\n"); wait(1); pc.printf("Measured time: %f seconds \r\n\n",calibrate_time); calibrate_time=0; // Triceps: muscle=2; pc.printf("\r\n----------------\r\n "); pc.printf(" Triceps 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); start_sampling(); muscle=1; timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC); pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; //Flexor: muscle=3; //Extensor: muscle=4; //Stop sampling, detach ticker stop_sampling(); } //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 = derfilter.step(e_der); 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() { //analyze emg (= velocity, averages?) //calculate reference position based on the average emg (integrate) //Current position - Encoder counts -> current angle -> Forward Kinematics rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts theta2 = Encoder2.getPulses() * rpc; current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2); //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y); //pc.printf("X is %f and Y is %f \r\n",x,y); //calculate error (refpos-currentpos) currentpos = forward kinematics x_error = x - current_x; y_error = y - current_y; //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error); //inverse kinematics (refpos to refangle) costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; sintheta2 = sqrt( 1 - pow(costheta2,2) ); //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2); dtheta2 = atan2(sintheta2,costheta2); costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); sintheta1 = sqrt( 1 - pow(costheta1,2) ); //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1); dtheta1 = atan2(sintheta1,costheta1); //Angle error m1_error = dtheta1-theta1; m2_error = dtheta2-theta2; //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error); //PID controller u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1 keep_in_range(&u2,-0.6,0.6); //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));*/ } 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(1); pc.printf("[C]alibration\r\n"); wait(0.2); pc.printf("[S]tart Control with buttons\r\n"); wait(0.2); pc.printf("[Q]uit Robot Program\r\n"); wait(0.2); pc.printf("[R]ed LED\r\n"); wait(0.2); pc.printf("[G]reen LED\r\n"); wait(0.2); pc.printf("[B]lue LED\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 //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(); //Debug LED will be off when control is off blue=1; pc.printf("||- stopped control -|| \r\n"); } void calibrate() { } //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 } 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"); } 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 } void emgInstructions(){ pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); wait(1); pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1); pc.printf("\n\r Relax for a few minutes after electrodes are placed and check if EMG signal looks normal \n\r "); wait(1); pc.printf("\n\r To calibrate the EMG signals we will measure the Maximum Voluntary Contraction of each muscle \n\r"); wait(1); pc.printf("You will need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1); pc.printf("The measurement will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1); } //keeps input limited between min max void keep_in_range(double * in, double min, double max) { *in > min ? *in < max? : *in = max: *in = min; }