2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 46:c8c5c455dd51
- Parent:
- 45:198654304238
- Child:
- 47:c52f9b4c90c4
diff -r 198654304238 -r c8c5c455dd51 main.cpp --- a/main.cpp Fri Oct 23 10:18:29 2015 +0000 +++ b/main.cpp Fri Oct 23 14:07:58 2015 +0000 @@ -93,7 +93,10 @@ 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 +double dx, dy; +double emg_control_time; + + //Threshold moving average const int window=100; //00 samples int i=0; //buffer index @@ -101,7 +104,9 @@ double movavg2[window]; double movavg3[window]; double movavg4[window]; - +double sum1, sum2, sum3, sum4 ; +double biceps_avg, triceps_avg,flexor_avg, extens_avg; + int muscle; //Muscle selector for MVC measurement, 1 = first emg etc. double calibrate_time; //Elapsed time for each MVC measurement @@ -225,8 +230,8 @@ biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope //PID filter (lowpass ??? Hz) -biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter - +biquadFilter derfilter1( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter +biquadFilter derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ @@ -274,6 +279,16 @@ pc.baud(115200); //serial baudrate red=1; green=1; blue=1; //Make sure debug LEDs are off + theta1_cal = floor(theta1_lower * (4200/(2*PI))); + Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses() + + //Mechanical limit 43 degrees -> 43*(4200/360) = 350 + theta2_cal = floor(theta2_lower * (4200/(2*PI))); + Encoder2.setPulses(theta2_cal); + + x = 0.2220; + y = 0.4088; + //Set PwmOut frequency to 10k Hz pwm_motor1.period(0.001); pwm_motor2.period(0.001); @@ -369,7 +384,8 @@ pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1); start_sampling(); - x=0; y=0; + + emg_control_time = 0; wait(1); emg_control=true; control_method=2; @@ -545,12 +561,14 @@ while(calibrating){ shoulder_limit.rise(&shoulder); elbow_limit.rise(&elbow); - while(!done1){ - pwm_motor1.write(0.1); //move upper arm slowly cw - } - if(done1==true){ - pwm_motor2.write(0.1); //move forearm slowly cw - } + //while(!done1){ + //pwm_motor1=0.1; //move upper arm slowly cw + + //} + //if(done1==true){ + // pwm_motor2=0.1; //move forearm slowly cw + + //} if(done1 && done2){ calibrating=false; //Leave while loop when both limits are reached @@ -789,7 +807,7 @@ { // Derivative double e_der = (error-e_prev)/ CONTROL_RATE; - e_der = derfilter.step(e_der); + e_der = derfilter1.step(e_der); e_prev = error; // Integral e_int = e_int + CONTROL_RATE * error; @@ -798,14 +816,30 @@ } +//Input error and Kp, Kd, Ki, output control signal +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_prev = error; + // Integral + e_int = e_int + CONTROL_RATE * error; + // PID + return kp*error + ki*e_int + kd * e_der; + +} + + //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){ + 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; - + emg_control_time += CONTROL_RATE; + //if(emg_control_time < 5){ + // x=0; y=0; + //} //normalize emg to value between 0-1 biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin); triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin); @@ -823,8 +857,8 @@ scope.send(); //threshold detection! buffer or two thresholds? If average of 100 samples > threshold, then muscle considered on. - /* - + + /* movavg1[i]=biceps; //fill array with 100 normalized samples movavg2[i]=triceps; movavg3[i]=flexor; @@ -844,8 +878,8 @@ triceps_avg = sum2/window; flexor_avg = sum3/window; extens_avg = sum4/window; + */ - //analyze emg (= velocity) if (biceps>triceps && biceps > 0.2){ xdir = 0; @@ -973,7 +1007,7 @@ //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 + 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_in_range(&u2,-0.6,0.6);