2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 38:c8ac615d0c8f
- Parent:
- 37:4d7b7ced20ef
- Child:
- 39:e77f844d10d9
--- a/main.cpp Thu Oct 22 23:26:37 2015 +0000 +++ b/main.cpp Fri Oct 23 00:02:19 2015 +0000 @@ -85,10 +85,12 @@ //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() -double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=1; -double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=1; -double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=1; -double emg_extens; double extens_power; double extensMVC = 0; double extensmin=1; +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; +//Normalized emg values +double biceps, triceps, flexor, extens; int muscle; //Muscle selector for MVC measurement, 1 = first emg etc. double calibrate_time; //Elapsed time for each MVC measurement @@ -159,6 +161,9 @@ //Reference position double x; double y; +//Select whether to use Trig or DLS method +int control_method; + //Inverse Kinematics - Trig / Gonio method. //First convert reference position to angle needed, then compare that angle to current angle. double dtheta1; double dtheta2; //reference angles @@ -324,6 +329,7 @@ wait(1); start_sampling(); wait(1); + control_method=1; start_control(); wait(1); controlButtons(); @@ -334,7 +340,8 @@ wait(1); start_sampling(); wait(1); - start_dlscontrol(); + control_method=2; + start_control(); wait(1); controlButtons(); break; @@ -387,23 +394,23 @@ c = pc.getc(); switch (c) { - case '1' : + case 'd' : x = x + 0.01; break; - case '2' : + case 'a' : x-=0.01; break; - case '3' : + case 'w' : y+=0.01; break; - case '4' : + case 's' : y-=0.01; break; @@ -456,8 +463,8 @@ scope.set(0,biceps_power); scope.set(1,triceps_power); - scope.set(2,flexor_power); - scope.set(3,extens_power); + //scope.set(2,flexor_power); + //scope.set(3,extens_power); scope.send(); // on - offset. If above a value it is on. @@ -607,19 +614,19 @@ void emg_min() { - if(biceps_power<bicepsmin){ + if(biceps_power>bicepsmin){ bicepsmin=biceps_power; } - if(triceps_power<tricepsmin){ + if(triceps_power>tricepsmin){ tricepsmin=triceps_power; } - if(flexor_power<flexormin){ + if(flexor_power>flexormin){ flexormin=flexor_power; } - if(extens_power < extensmin){ + if(extens_power > extensmin){ extensmin = extens_power; } @@ -632,9 +639,10 @@ { Ticker timer; - pc.printf("Testcode calibration \r\n"); + pc.printf("Starting sampling, to allow hidscope to normalize\r\n"); + start_sampling(); wait(1); - pc.printf("Starting minimum measurement, relax all muscles.\r\n"); + pc.printf("Start minimum measurement, relax all muscles.\r\n"); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; @@ -643,7 +651,7 @@ 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_min,SAMPLE_RATE); wait(5); @@ -774,18 +782,22 @@ //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() { - /* - + //normalize emg to value between 0-1 - norm_biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin) - norm_triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin) - norm_flexor = (flexor_power - flexormin) / (flexorMVC - flexormin) - norm_extens = (extens_power - extensmin) / (extensMVC - extensmin) + biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin); + triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin); + flexor = (flexor_power - flexormin) / (flexorMVC - flexormin); + extens = (extens_power - extensmin) / (extensMVC - extensmin); + + scope.set(2,biceps); + scope.set(3,triceps); + scope.send(); + //threshold detection! buffer? - TODO + //TODO - //analyze emg (= velocity) + /* //analyze emg (= velocity) if (biceps>triceps && biceps > 0.1) xdir = 0; xpower = biceps; @@ -806,7 +818,7 @@ else ypower = 0; - //We have power: the longer a signal is active, the further you go. So integrate to determine reference position + //power: the longer a signal is active, the further the reference goes. So integrate to determine reference position dx = xpower * CONTROL_RATE; dy = ypower * CONTROL_RATE; @@ -858,7 +870,7 @@ y_error = y - current_y; - if (control_method=1){ + 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) ; @@ -884,7 +896,7 @@ m2_error = dtheta2-theta2; } - if(control_method=2){ + 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)); dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(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)); @@ -936,143 +948,11 @@ } -void dlscontrol() -{ - /* - - //normalize emg to value between 0-1 - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - - //analyze emg (= velocity) - if (biceps>triceps && biceps > 0.1) - xdir = 0; - xpower = biceps; - else if (triceps>biceps && triceps>0.1) - xdir = 1; - xpower = triceps; - else - xpower=0; - - if (flexor>extensor && flexor > 0.1){ - ydir = 0; - ypower = flexor; - } - else if (extensor>flexor && extensor > 0.1){ - ydir = 1; - ypower = extensor; - } - else - ypower = 0; - - //We have power: the longer a signal is active, the further you go. So integrate to determine reference position - dx = xpower * CONTROL_RATE; - dy = ypower * CONTROL_RATE; - - //But: direction! Sum dx and dy but make sure xdir and ydir are considered. - if (xdir>0) - x += dx; - else - x += -dx; - - if (ydir>0) - y += dy; - else - y += -dy; - - //now we have x and y -> reference position. - - //Set limits to the error! - //lower limit: Negative error not allowed to go further. - if (theta1 < limitangle) - if (error1 > 0) - error1 = error1; - else - error1 = 0; - if (theta2 < limitangle) - same as above - - //upper limit: Positive error not allowed to go further - if (theta1 > limitangle) - if (error1 < 0 ) - error1 = error1; - else - error1 = 0; - if (theta2 > limitangle) - same as above - - - */ - - //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); - - - //calculate error (refpos-currentpos) currentpos = forward kinematics - x_error = x - current_x; - y_error = y - current_y; - - - //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)); - dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(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)); - dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*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)); - dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(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)); - - 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; - - - //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 + //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); @@ -1130,16 +1010,6 @@ pc.printf("||- started control -|| \r\n"); } -void start_dlscontrol(void) -{ - control_timer.attach(&dlscontrol,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) { @@ -1151,11 +1021,6 @@ } -void calibrate() -{ - -} - //Clears the putty (or other terminal) window void clearTerminal() { @@ -1185,10 +1050,10 @@ 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("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("q) Exit \r\n"); pc.printf("Please make a choice => \r\n"); } @@ -1246,11 +1111,11 @@ 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("\n\r Check if EMG signal looks normal \n\r "); wait(1); - pc.printf("\n\r To calibrate the EMG signals we will measure: \n\r "); - pc.printf("\n\r - Minimum amplitude, while relaxing all muscles. \n\r "); - pc.printf("\n\r - Maximum Voluntary Contraction of each muscle. \n\r"); wait(2); - pc.printf("For the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1); + 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); }