2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Revision 39:e77f844d10d9, committed 2015-10-23
- Comitter:
- Vigilance88
- Date:
- Fri Oct 23 00:36:37 2015 +0000
- Parent:
- 38:c8ac615d0c8f
- Child:
- 40:d62f96ed44c0
- Commit message:
- added EMG reference calculation - needs lots of testing
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 23 00:02:19 2015 +0000 +++ b/main.cpp Fri Oct 23 00:36:37 2015 +0000 @@ -89,9 +89,12 @@ 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; - +//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; //integration of EMG (velocity) to position + int muscle; //Muscle selector for MVC measurement, 1 = first emg etc. double calibrate_time; //Elapsed time for each MVC measurement @@ -161,8 +164,9 @@ //Reference position double x; double y; -//Select whether to use Trig or DLS method +//Select whether to use Trig or DLS method, emg 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. @@ -325,10 +329,11 @@ break; case 't': case 'T': - pc.printf("=> You chose TRIG control \r\n\n"); + pc.printf("=> You chose TRIG button control \r\n\n"); wait(1); start_sampling(); wait(1); + emg_control=false; control_method=1; start_control(); wait(1); @@ -336,15 +341,28 @@ break; case 'd': case 'D': - pc.printf("=> You chose DLS control \r\n\n"); + pc.printf("=> You chose DLS button control \r\n\n"); wait(1); start_sampling(); wait(1); + emg_control=false; control_method=2; start_control(); wait(1); controlButtons(); break; + case 'e': + case 'E': + pc.printf("=> You chose EMG DLS control \r\n\n"); + wait(1); + start_sampling(); + wait(1); + emg_control=true; + control_method=2; + start_control(); + wait(1); + controlButtons(); + break; case 'R': red=!red; pc.printf("=> Red LED triggered \n\r"); @@ -781,13 +799,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() -{ - +{ + 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 seconds, reference is x=0; y=0; //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); + scope.set(2,biceps); scope.set(3,triceps); @@ -795,32 +822,32 @@ //threshold detection! buffer? - //TODO + //TODO - /* //analyze emg (= velocity) - if (biceps>triceps && biceps > 0.1) + //analyze emg (= velocity) + if (biceps>triceps && biceps > 0.2){ xdir = 0; - xpower = biceps; - else if (triceps>biceps && triceps>0.1) + xpower = biceps;} + else if (triceps>biceps && triceps>0.2){ xdir = 1; - xpower = triceps; + xpower = triceps;} else xpower=0; - if (flexor>extensor && flexor > 0.1){ + if (flexor>extens && flexor > 0.2){ ydir = 0; ypower = flexor; } - else if (extensor>flexor && extensor > 0.1){ + else if (extens>flexor && extens > 0.2){ ydir = 1; - ypower = extensor; + ypower = extens; } 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; - dy = ypower * CONTROL_RATE; + dx = xpower * CONTROL_RATE * 0.1; //factor to slow or speed up + dy = ypower * CONTROL_RATE * 0.1; //But: direction! Sum dx and dy but make sure xdir and ydir are considered. if (xdir>0) @@ -833,29 +860,8 @@ 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 - - - */ + //now we have x and y -> reference position. + }//end emg_control if //Current position - Encoder counts -> current angle -> Forward Kinematics rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 @@ -894,7 +900,7 @@ //Angle error m1_error = dtheta1-theta1; m2_error = dtheta2-theta2; - } + }// end control method 1 if(control_method==2){ //inverse kinematics (error in position to error in angles) @@ -909,7 +915,30 @@ //Angle error m1_error = q1_error; m2_error = q2_error; - } + }//end control method 2 + + /* + //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 + + + */ //PID controller