2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 36:4d4fc200171b
- Parent:
- 35:7d9fca0b1545
- Child:
- 37:4d7b7ced20ef
--- a/main.cpp Thu Oct 22 14:18:41 2015 +0000 +++ b/main.cpp Thu Oct 22 23:19:50 2015 +0000 @@ -23,7 +23,7 @@ MODSERIAL pc(USBTX,USBRX); //serial communication -//Debug legs +//Debug LEDs DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); DigitalOut blue(LED_BLUE); @@ -83,24 +83,25 @@ ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction -double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin; -double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin; -double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin; -double emg_extens; double extens_power; double extensMVC = 0; double extensmin; +//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; -int muscle; //Muscle selector for MVC measurement +int muscle; //Muscle selector for MVC measurement, 1 = first emg etc. 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.1; const double m1_ki=0.0125; const double m1_kd=0.02; //Proportional, integral and derivative gains. +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 m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error -const double m2_kp=0.1; const double m2_ki=0.0125; const double m2_kd=0.02; //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. -//Calibration, checks if elbow/shoulder are done +//Calibration bools, checks if elbow/shoulder limits are hit bool done1 = false; bool done2 = false; @@ -112,10 +113,7 @@ 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 @@ -152,19 +150,28 @@ 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; +//Forward Kinematics +const double l1 = 0.25; const double l2 = 0.25; //Arm lengths +double current_x; double current_y; //Current position +double theta1; double theta2; //Current angles +double rpc; //Encoder resolution: radians per count + +//Reference position double x; double y; -double x_error; double y_error; -double costheta1; double sintheta1; -double costheta2; double sintheta2; -double dls1, dls2, dls3, dls4; -double q1_error, q2_error; -double lambda=0.1; + +//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 +double costheta1; double sintheta1; //helper variables +double costheta2; double sintheta2; // + +//Inverse Kinematics - Damped least squares method. +//Angle error is directly calculated from position error: dq = [DLS matrix] * position_error + // |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 lambda=0.1; //Damping constant /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- @@ -253,24 +260,17 @@ // make a menu, user has to initiate next step titleBox(); mainMenu(); - //caliMenu(); // Menu function - //calibrate_arm(); //Start Calibration - - //calibrate_emg(); - + //set initial reference position - x=0.5; y=0; - - //start_control(); //100 Hz control + x = 0.5; + y = 0; + theta1=0.6980; + theta2=0.5200; //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); //start_sampling(); - //char c; - - - char command=0; while(command != 'Q' && command != 'q') @@ -425,7 +425,9 @@ 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("Pos Error: %f and %f \r\n",x_error,y_error); pc.printf("Current angles: %f and %f \r\n",theta1,theta2); + pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4); 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"); @@ -486,6 +488,9 @@ 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) ) + //Encoder1.setPulses(467); //edited QEI library: added setPulses() + } void elbow(){ @@ -493,6 +498,9 @@ Encoder2.reset(); done2 = true; pc.printf("Elbow Limit hit - shutting down motor 2\r\n"); + //Mechanical limit 30 degrees -> 30*(4200/360) = 350 + + //Encoder2.setPulses(350); //edited QEI library: added setPulses() } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. @@ -508,14 +516,17 @@ 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 + dir_motor1=0; //cw + dir_motor2=1; //cw + while(calibrating){ shoulder_limit.rise(&shoulder); elbow_limit.rise(&elbow); + while(!done1){ + pwm_motor1.write(0.2); //move upper arm slowly cw + } if(done1==true){ pwm_motor2.write(0.2); //move forearm slowly cw } @@ -528,9 +539,7 @@ }//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); @@ -593,9 +602,7 @@ //} calibrate_time = calibrate_time + 0.002; - - - + } void emg_min() @@ -628,7 +635,7 @@ pc.printf("Testcode calibration \r\n"); wait(1); - pc.printf("Starting minimum measurement, relax all muscles."); + pc.printf("Starting minimum measurement, relax all muscles.\r\n"); wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; @@ -638,6 +645,7 @@ pc.printf(" \r\n Starting in 1... \r\n"); wait(1); start_sampling(); + timer.attach(&emg_min,SAMPLE_RATE); wait(5); timer.detach(); @@ -1221,12 +1229,14 @@ } void emgInstructions(){ - pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); wait(1); + 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 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); + 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("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1); } //keeps input limited between min max