2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 47:c52f9b4c90c4
- Parent:
- 46:c8c5c455dd51
- Child:
- 48:a1f97eb8c020
--- a/main.cpp Fri Oct 23 14:07:58 2015 +0000 +++ b/main.cpp Fri Oct 23 20:13:06 2015 +0000 @@ -83,44 +83,44 @@ ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - minimum amplitude during relaxation. -//minimum declared as 1 so the comparison with EMG during calibration works correctly - function emg_min() +//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; -double emg_control_time; - +double dx, dy; //Integral +double emg_control_time; //Elapsed time in EMG control //Threshold moving average -const int window=100; //00 samples -int i=0; //buffer index -double movavg1[window]; //moving average arrays +const int window=100; //100 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 sum1, sum2, sum3, sum4 ; -double biceps_avg, triceps_avg,flexor_avg, extens_avg; +double sum1, sum2, sum3, sum4; //sum of the entire 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 MVC measurement +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 -const double m1_kp=8; const double m1_ki=0.125; const double m1_kd=0.5; //Proportional, integral and derivative gains. +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=8; const double m1_ki=0.125; const double m1_kd=0.5; //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=8; const double m2_ki=0.125; const double m2_kd=0.5; //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=8; const double m2_ki=0.125; const double m2_kd=0.5; //Proportional, integral and derivative gains. //Calibration bools, checks if elbow/shoulder limits are hit -bool done1 = false; -bool done2 = false; +volatile bool done1 = false; +volatile bool done2 = false; +volatile bool calibrating = false; //highpass filter 20 Hz const double high_b0 = 0.956543225556877; @@ -191,7 +191,7 @@ // |DLS1 DLS2| double dls1, dls2, dls3, dls4; //DLS matrix: |DLS3 DLS4| double q1_error, q2_error; //Angle errors -double x_error; double y_error; //Position errors +double x_error, y_error; //Position errors double lambda=0.1; //Damping constant //Mechanical Limits @@ -247,15 +247,14 @@ 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 start_dlscontrol(void);//Attaches DLS 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); +double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev); //Menu functions void mainMenu(); @@ -267,6 +266,7 @@ void titleBox(); //Limit switches - power off motors if switches hit (rising edge interrupt) +void calibrate(void); void shoulder(); void elbow(); @@ -298,15 +298,7 @@ // make a menu, user has to initiate next step titleBox(); mainMenu(); - - //set initial reference position - //x = 0.5; - //y = 0; - - //maybe some stop commands when a button is pressed: detach from timers. - //stop_control(); - //start_sampling(); - + char command=0; while(command != 'Q' && command != 'q') @@ -317,7 +309,7 @@ switch(command){ case 'c': - case 'C': + case 'C': { pc.printf("\n\r => You chose calibration.\r\n\n"); caliMenu(); @@ -355,6 +347,7 @@ }//end while break; + }//end case c C case 't': case 'T': pc.printf("=> You chose TRIG button control \r\n\n"); @@ -384,11 +377,11 @@ pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1); start_sampling(); - + wait(1); emg_control_time = 0; - wait(1); emg_control=true; control_method=2; + start_control(); wait(1); controlButtons(); @@ -419,7 +412,7 @@ - //When end of while loop reached (user chose quit program) - detach all timers and stop motors. + //When end of while loop reached (user chose quit program). pc.printf("\r\n------------------------------ \n\r"); pc.printf("Program Offline \n\r"); @@ -432,6 +425,7 @@ ---- FUNCTIONS ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ +//Use WASD keys to change reference position, x is a and d, y is w and s. void controlButtons() { controlMenu(); @@ -472,10 +466,10 @@ 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); @@ -485,9 +479,9 @@ pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error); pc.printf("PID output: %f and %f \r\n",u1,u2); pc.printf("----------------------------------------\r\n\n"); - } + }//end if } - //end if + //end if pc readable } //end of while loop } @@ -517,91 +511,91 @@ } - +//Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit void shoulder(){ pwm_motor1=0; - //Encoder1.reset(); 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() + Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses(int p) } void elbow(){ pwm_motor2=0; - //Encoder2.reset(); 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() + 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"); + } + // if(done1==true && done2==true) //if both limits are hit + // pwm_motor2=0; //stop motor2 + // calibrating=false; //stop calibrating +} //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"); - red=0; blue=0; //Debug light is purple during arm calibration - + + calibrating = true; done1 = false; done2 = false; - bool calibrating = true; pc.printf("To start arm calibration, press any key =>"); - pc.getc(); + 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.rise(&shoulder); - elbow_limit.rise(&elbow); - //while(!done1){ - //pwm_motor1=0.1; //move upper arm slowly cw - - //} - //if(done1==true){ - // pwm_motor2=0.1; //move forearm slowly cw - - //} + 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; - 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: - - //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); - - //set reference position to mechanical limits - x = 0.2220; - y = 0.4088; - - wait(1); - pc.printf("\n\r ------------------------ \n\r"); - pc.printf("Arm Calibration Complete\r\n"); - pc.printf(" ------------------------ \n\r"); - + 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; + //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"); + } //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){