2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 48:a1f97eb8c020
- Parent:
- 47:c52f9b4c90c4
- Child:
- 49:6515c045cfd6
--- a/main.cpp Fri Oct 23 20:13:06 2015 +0000 +++ b/main.cpp Fri Oct 23 20:40:31 2015 +0000 @@ -643,8 +643,7 @@ } void emg_min() -{ - +{ if(biceps_power>bicepsmin){ bicepsmin=biceps_power; } @@ -673,7 +672,9 @@ pc.printf("Starting sampling, to allow hidscope to normalize\r\n"); start_sampling(); wait(1); - pc.printf("Start minimum measurement, relax all muscles.\r\n"); + + /******************* All muscles: minimum measurement *************************/ + pc.printf("Start of minimum measurement, relax all muscles.\r\n"); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; @@ -682,8 +683,6 @@ 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_min,SAMPLE_RATE); wait(5); timer.detach(); @@ -692,12 +691,15 @@ pc.printf("\r\n Triceps min = %f \r\n",tricepsmin); wait(1); pc.printf("\r\n Flexor min = %f \r\n",flexormin); wait(1); pc.printf("\r\n Extensor min = %f \r\n",extensmin); wait(1); + /******************************** Done ****************************************/ - calibrate_time=0; - pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(1); + pc.printf("\r\n Now we will measure maximum amplitudes \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); + wait(1); + calibrate_time=0; + + /********************* 1st channel: MVC measurement ***************************/ pc.printf("\r\n----------------\r\n "); pc.printf(" Biceps is first.\r\n "); pc.printf("----------------\r\n "); @@ -709,7 +711,6 @@ 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); @@ -720,8 +721,9 @@ pc.printf("Calibrate_emg() exited \r\n"); wait(1); pc.printf("Measured time: %f seconds \r\n\n",calibrate_time); calibrate_time=0; + /******************************** Done ****************************************/ - // Triceps: + /********************* 2nd channel: MVC measurement ***************************/ muscle=2; pc.printf("\r\n----------------\r\n "); pc.printf(" Triceps is next.\r\n "); @@ -734,7 +736,7 @@ 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(); + timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); @@ -743,8 +745,9 @@ pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; - - //Flexor: + /******************************** Done ****************************************/ + + /********************* 3rd channel: MVC measurement ***************************/ muscle=3; pc.printf("\r\n----------------\r\n "); pc.printf(" Flexor is next.\r\n "); @@ -757,7 +760,7 @@ 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(); + timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); @@ -766,9 +769,9 @@ pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; + /******************************** Done ****************************************/ - //Extensor: - + /********************* 4th channel: MVC measurement ***************************/ muscle=4; pc.printf("\r\n----------------\r\n "); pc.printf(" Extensor is next.\r\n "); @@ -781,7 +784,7 @@ 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(); + timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); @@ -790,18 +793,20 @@ pc.printf("Calibrate_emg() exited \r\n"); pc.printf("Measured time: %f seconds \r\n",calibrate_time); calibrate_time=0; - //Stop sampling, detach ticker + /******************************** Done ****************************************/ + + //Stop sampling: detach ticker stop_sampling(); } -//Input error and Kp, Kd, Ki, output control signal +//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); + e_der = derfilter1.step(e_der); //derfilter1 - seperate 7hz low-pass biquad for this PID e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; @@ -810,12 +815,12 @@ } -//Input error and Kp, Kd, Ki, output control signal +//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); + e_der = derfilter2.step(e_der); //derfilter2 - seperate 7hz low-pass biquad for this PID e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; @@ -828,12 +833,17 @@ //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 emg_control_time += CONTROL_RATE; //if(emg_control_time < 5){ - // x=0; y=0; + // x=BLA; y=BLA; starting position maybe //} + //normalize emg to value between 0-1 biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin); triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin); @@ -845,7 +855,7 @@ keep_in_range(&flexor,0,1); keep_in_range(&extens,0,1); - + //send normalized emg to scope to compare with just filtered emg. scope.set(2,biceps); scope.set(3,triceps); scope.send(); @@ -874,9 +884,10 @@ extens_avg = sum4/window; */ - //analyze emg (= velocity) + + //Compare muscle amplitudes and determine their influence on x and y reference position. if (biceps>triceps && biceps > 0.2){ - xdir = 0; + xdir = 0; xpower = biceps;} else if (triceps>biceps && triceps>0.2){ xdir = 1; @@ -896,48 +907,53 @@ 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.1; //factor to slow or speed up + dx = xpower * CONTROL_RATE * 0.1; //last value is a factor to control how fast the reference (so also end effector) changes dy = ypower * CONTROL_RATE * 0.1; - //But: direction! Sum dx and dy but make sure xdir and ydir are considered. - if (xdir>0) - x += dx; - else + //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 (ydir>0) //if y direction of sample is positive, add it to reference position y += dy; else - y += -dy; + y += -dy; //if y direction of sample is negative, substract it from reference position + + //now we have x and y -> reference position. - //now we have x and y -> reference position. }//end emg_control if + /******************************** END OF EMG REFERENCE CALCULATION ****************************************/ + //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 + 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); + 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) currentpos = forward kinematics + //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) ; - sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) ); - + sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) ); //absolute to avoid imaginary numbers -> bigger steady state error + //Reference angle 2 dtheta2 = atan2(sintheta2,costheta2); double k1 = l1 + l2*costheta2; double k2 = l2*sintheta2; + //Reference angle 1 dtheta1 = atan2(y, x) - atan2(k2, k1); /* alternative: @@ -951,7 +967,10 @@ m1_error = dtheta1-theta1; m2_error = dtheta2-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) dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); @@ -966,13 +985,14 @@ 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. */ + + /* 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. + //lower limits: Negative error not allowed to go further. NEEDS MORE TESTING if (theta1 < theta1_lower){ if (m1_error > 0) m1_error = m1_error; @@ -999,11 +1019,11 @@ } //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_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1 + //keep u between limits, sign = direction, PWM = 0-1 + keep_in_range(&u1,-0.6,0.6); keep_in_range(&u2,-0.6,0.6); //send PWM and DIR to motor 1 @@ -1036,7 +1056,6 @@ } - void mainMenu() { //Title Box @@ -1056,6 +1075,7 @@ pc.printf("\n\r"); //endbox + wait(1); pc.printf("[C]alibration\r\n"); wait(0.2); pc.printf("[T]RIG Control with WASD\r\n"); wait(0.2); @@ -1121,7 +1141,7 @@ void controlMenu() { - //Title Box + //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); @@ -1138,6 +1158,7 @@ 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"); @@ -1148,7 +1169,7 @@ void caliMenu(){ - //Title Box + //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); @@ -1202,9 +1223,9 @@ pc.printf("\r\n 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(2); - pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1); - pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1); + 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 as much as possible for 5 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