group14 - k9
testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
testPID
by 
Martijn Kern
Diff: main.cpp
- Revision:
- 27:0cefe83f83d3
- Parent:
- 26:fe3a5469dd6b
- Child:
- 28:0460786f9573
--- a/main.cpp Sun Oct 18 13:13:07 2015 +0000
+++ b/main.cpp Sun Oct 18 23:24:30 2015 +0000
@@ -28,11 +28,6 @@
DigitalOut green(LED_GREEN);
DigitalOut blue(LED_BLUE);
-//EMG shields
-AnalogIn emg1(A0); //Analog input - Biceps EMG
-AnalogIn emg2(A1); //Analog input - Triceps EMG
-AnalogIn emg3(A2); //Analog input - Flexor EMG
-AnalogIn emg4(A3); //Analog input - Extensor EMG
Ticker sample_timer; //Ticker for EMG sampling
Ticker control_timer; //Ticker for control loop
@@ -53,94 +48,29 @@
DigitalOut dir_motor2(D4); //Direction motor 2
//Limit Switches
-InterruptIn shoulder_limit(D3); //using FRDM buttons
-InterruptIn elbow_limit(D4); //using FRDM buttons
+InterruptIn shoulder_limit(D2); //using FRDM buttons
+InterruptIn elbow_limit(D3); //using FRDM buttons
//Other buttons
DigitalIn button1(PTA4); //using FRDM buttons
DigitalIn button2(PTC6); //using FRDM buttons
-/*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m
-
-\ 0 3 3 - ESC
-[ 3 0 m - black
-[ 3 1 m - red
-[ 3 2 m - green
-[ 3 3 m - yellow
-[ 3 4 m - blue
-[ 3 5 m - magenta
-[ 3 6 m - cyan
-[ 3 7 m - white
-[ 0 m - reset attributes
-
-Put the text you want to color between GREEN_ and _GREEN
-*/
-string GREEN_ = "\033[32m"; //esc - green
-string _GREEN = "\033[0m"; //esc - reset
/*--------------------------------------------------------------------------------------------------------------------
---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction
-double emg_biceps; double biceps_power; double bicepsMVC = 0;
-double emg_triceps; double triceps_power; double tricepsMVC = 0;
-double emg_flexor; double flexor_power; double flexorMVC = 0;
-double emg_extens; double extens_power; double extensMVC = 0;
-
-int muscle; //Muscle selector for MVC measurement
-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; const double m1_ki=0; const double m1_kd=0; //Proportional, integral and derivative gains.
+const double m1_kp=0.001; const double m1_ki=1; const double m1_kd=0; //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; const double m2_ki=0; const double m2_kd=0; //Proportional, integral and derivative gains.
-
-//highpass filter 20 Hz
-const double high_b0 = 0.956543225556877;
-const double high_b1 = -1.91308645113754;
-const double high_b2 = 0.956543225556877;
-const double high_a1 = -1.91197067426073;
-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
-Sampling Frequency = 500Hz
-Filter Order = 2
-
-Band Frequencies (Hz) Att/Ripple (dB) Biquad #1 Biquad #2
+const double m2_kp=0.001; const double m2_ki=2; const double m2_kd=0; //Proportional, integral and derivative gains.
-1 0, 48 0.001 Gain = 1.000000 Gain = 0.973674
-2 49, 51 -60.000 B = [ 1.00000000000, -1.61816176147, 1.00000000000] B = [ 1.00000000000, -1.61816176147, 1.00000000000]
-3 52, 250 0.001 A = [ 1.00000000000, -1.58071559235, 0.97319685401] A = [ 1.00000000000, -1.61244708381, 0.97415116257]
-*/
-//biquad 1
-const double notch1gain = 1.000000;
-const double notch1_b0 = 1;
-const double notch1_b1 = -1.61816176147 * notch1gain;
-const double notch1_b2 = 1.00000000000 * notch1gain;
-const double notch1_a1 = -1.58071559235 * notch1gain;
-const double notch1_a2 = 0.97319685401 * notch1gain;
-
-//biquad 2
-const double notch2gain = 0.973674;
-const double notch2_b0 = 1 * notch2gain;
-const double notch2_b1 = -1.61816176147 * notch2gain;
-const double notch2_b2 = 1.00000000000 * notch2gain;
-const double notch2_a1 = -1.61244708381 * notch2gain;
-const double notch2_a2 = 0.97415116257 * notch2gain;
-
//lowpass filter 7 Hz - envelope
const double low_b0 = 0.000119046743110057;
const double low_b1 = 0.000238093486220118;
@@ -148,48 +78,34 @@
const double low_a1 = -1.968902268531908;
const double low_a2 = 0.9693784555043481;
-
+//Forward 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;
+double x=0.5; double y=0;
+double x_error; double y_error;
+double costheta1; double sintheta1;
+double costheta2; double sintheta2;
/*--------------------------------------------------------------------------------------------------------------------
---- Filters ---------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
//Using biquadFilter library
//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
-//Biceps
-biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
-biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
-biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
-biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
-
-//Triceps
-biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
-biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
-biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
-biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
-
-//Flexor
-biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
-biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
-biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
-biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
-
-//Extensor
-biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
-biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
-biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
-biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
//PID filter (lowpass ??? Hz)
-biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter
+biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter
/*--------------------------------------------------------------------------------------------------------------------
---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-void sample_filter(void); //Sampling and filtering
+
void control(); //Control - reference -> error -> pid -> motor output
-void calibrate_emg(); //Instructions + measurement of MVC of each muscle
-void emg_mvc(); //Helper funcion for storing MVC value
+
+
void calibrate_arm(void); //Calibration of the arm with limit switches
void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker
void stop_sampling(void); //Stops sample_filter
@@ -206,7 +122,7 @@
void mainMenu();
void caliMenu();
void clearTerminal();
-
+void controlMenu();
/*--------------------------------------------------------------------------------------------------------------------
---- MAIN LOOP -------------------------------------------------------------------------------------------------------
@@ -217,9 +133,15 @@
pc.baud(115200); //terminal baudrate
red=1; green=1; blue=1; //Make sure debug LEDS are off
- //Set PwmOut frequency to 10k Hz
- pwm_motor1.period(PWM_PERIOD);
- pwm_motor2.period(PWM_PERIOD);
+ //Set PwmOut frequency to 10k Hz???
+ //pwm_motor1.period(0.01);
+ //pwm_motor2.period(0.01);
+
+ dir_motor1.write(1); //for motor 1 - 1 = counterclockwise
+ dir_motor2.write(1); //for motor 2 - 1 = clockwise
+ pwm_motor1=0;
+ pwm_motor2=0;
+
clearTerminal(); //Clear the putty window
@@ -228,16 +150,59 @@
//caliMenu(); // Menu function
//calibrate_arm(); //Start Calibration
- calibrate_emg();
-
- //start_control(); //100 Hz control
+ //calibrate_emg();
+ wait(1);
+ start_control(); //100 Hz control
//maybe some stop commands when a button is pressed: detach from timers.
//stop_control();
//stop_sampling();
-
+ char c;
+ pc.printf("entering while loop \r\n");
while(1) {
-
+
+ if( pc.readable() ){
+ c = pc.getc();
+ switch (c)
+ {
+ case '1' :
+ x = x + 0.01;
+ //controlMenu();
+ //running=false;
+ break;
+
+ case '2' :
+ x-=0.01;
+ //controlMenu();
+ //running=false;
+ break;
+
+ case '3' :
+ y+=0.01;
+ //controlMenu();
+ //running=false;
+ break;
+
+
+ case '4' :
+ y-=0.01;
+ //controlMenu();
+ //running=false;
+ break;
+
+ case 'q' :
+ pc.printf("Quit");
+ //running = false;
+ break;
+ }//end switch
+ 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("Current angles: %f and %f \r\n",theta1,theta2);
+ pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
+ pc.printf("PID output: %f and %f \r\n",u1,u2);
+ pc.printf("----------------------------------------\r\n\n");
+ }
+ //end if
}
//end of while loop
}
@@ -247,192 +212,58 @@
---- FUNCTIONS -------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-//Sample and Filter
-void sample_filter(void)
-{
- double emg_biceps = emg1.read(); //Biceps
- double emg_triceps = emg2.read(); //Triceps
- double emg_flexor = emg3.read(); //Flexor
- double emg_extens = emg4.read(); //Extensor
-
- //Filter: high-pass -> notch -> rectify -> lowpass or moving average
- // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
- biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens);
- biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power);
- biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power);
- biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
- biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
-
-
- /* alternative for lowpass filter: moving average
- window=30; //30 samples
- int i=0; //buffer index
- bicepsbuffer[i]=biceps_power //fill array
-
- i++;
- if(i==window){
- i=0;
- }
-
- for(int x = 0; x < window; x++){
- avg1 += bicepsbuffer[x];
- }
- avg1 = avg1/window;
- */
-
-}
+
//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
void calibrate_arm(void)
{
- red=0; blue=0; //Debug light is purple during arm calibration
+ pc.printf("Calibrate_arm() entered\r\n");
bool calibrating = true;
bool done1 = false;
bool done2 = false;
+ pc.printf("To start arm calibration, press any key =>");
+ pc.getc();
+ pc.printf("\r\n Calibrating... \r\n");
dir_motor1=1; //cw
- dir_motor2=1; //cw
- pwm_motor1.write(0.2f); //move upper arm slowly cw
- pwm_motor2.write(0.2f); //move forearm slowly cw
+ dir_motor2=0; //cw
+ pwm_motor1.write(0.2); //move upper arm slowly cw
+
+
while(calibrating){
-
+ red=0; blue=0; //Debug light is purple during arm calibration
+
+ if(done1==true){
+ pwm_motor2.write(0.2); //move forearm slowly cw
+ }
+
//when limit switches are hit, stop motor and reset encoder
if(shoulder_limit.read() < 0.5){ //polling
pwm_motor1.write(0);
Encoder1.reset();
done1 = true;
+ pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
}
if(elbow_limit.read() < 0.5){ //polling
pwm_motor2.write(0);
Encoder2.reset();
done2 = true;
+ pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
}
if(done1 && done2){
calibrating=false; //Leave while loop when both limits are reached
red=1; blue=1; //Turn debug light off when calibration complete
}
-
+
}//end while
-
-}
-
-//EMG Maximum Voluntary Contraction measurement
-void emg_mvc()
-{
- //double sampletime=0;
- //sampletime=+SAMPLE_RATE;
- //
- // if(sampletime<5)
- //int muscle=1;
- //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
-
- if(muscle==1){
-
- if(biceps_power>bicepsMVC){
- //printf("+ ");
- printf("%s+ %s",GREEN_,_GREEN);
- bicepsMVC=biceps_power;
- }
- else
- printf("- ");
- }
-
- if(muscle==2){
-
- if(triceps_power>tricepsMVC){
- printf("%s+ %s",GREEN_,_GREEN);
- tricepsMVC=triceps_power;
- }
- else
- printf("- ");
- }
-
- if(muscle==3){
-
- if(flexor_power>flexorMVC){
- printf("%s+ %s",GREEN_,_GREEN);
- flexorMVC=flexor_power;
- }
- else
- printf("- ");
- }
-
- if(muscle==4){
-
- if(extens_power>extensMVC){
- printf("%s+ %s",GREEN_,_GREEN);
- extensMVC=extens_power;
- }
- else
- printf("- ");
- }
-
- //}
- calibrate_time = calibrate_time + 0.002;
-
-
-}
-
-//EMG calibration
-void calibrate_emg()
-{
- Ticker timer;
-
- pc.printf("Testcode calibration \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);
- pc.printf(" Biceps is first. "); wait(1);
- pc.printf(" Press any key to begin... "); wait(1);
- char input;
- input=pc.getc();
- pc.putc(input);
- 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();
- muscle=1;
- timer.attach(&emg_mvc,0.002);
- wait(5);
- timer.detach();
-
- pc.printf("\r\n Measurement complete."); wait(1);
- pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1);
- pc.printf("Calibrate_emg() exited \r\n"); wait(1);
- pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
- calibrate_time=0;
-
- // Triceps:
- muscle=2;
- pc.printf(" Triceps is next "); wait(1);
- pc.printf(" Press any key to begin... "); wait(1);
- input=pc.getc();
- pc.putc(input);
- 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();
- muscle=1;
- timer.attach(&emg_mvc,0.002);
- wait(5);
- timer.detach();
- pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
-
- pc.printf("Calibrate_emg() exited \r\n");
- pc.printf("Measured time: %f seconds \r\n",calibrate_time);
- calibrate_time=0;
-
- //Flexor:
- muscle=3;
- //Extensor:
- muscle=4;
-
- //Stop sampling, detach ticker
- stop_sampling();
-
+ //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);
+ pc.printf("Arm Calibration Complete\r\n");
}
@@ -441,25 +272,73 @@
{
// Derivative
double e_der = (error-e_prev)/ CONTROL_RATE;
- e_der = derfilter.step(e_der);
+ //e_der = derfilter.step(e_der);
e_prev = error;
// Integral
e_int = e_int + CONTROL_RATE * error;
// PID
- return kp*error + ki*e_int + kd * e_der;
-
+ return kp*error + ki*e_int;
+
+ //+ kd * e_der;
}
+void controlMenu()
+{
+ 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("q) Exit \r\n");
+ pc.printf("Please make a choice => \r\n");
+}
+
+
+
//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()
{
- //analyze emg (= velocity, averages?)
+
+ //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);
+
+ //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y);
- //calculate reference position based on the average emg (integrate)
+
+ //pc.printf("X is %f and Y is %f \r\n",x,y);
+
+ //calculate error (refpos-currentpos) currentpos = forward kinematics
+ x_error = x - current_x;
+ y_error = y - current_y;
+
+ //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error);
+
+ //inverse kinematics (refpos to refangle)
- //calculate error (refpos-currentpos) currentpos = forward kinematics
+ costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
+ sintheta2 = sqrt( 1 - pow(costheta2,2) );
+
+ //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2);
+
+ dtheta2 = atan2(sintheta2,costheta2);
+
+ costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sostheta2 ) / ( pow(x,2) + pow(y,2) );
+ sintheta1 = sqrt( 1 - pow(costheta1,2) );
- //inverse kinematics (pos error to angle error)
+ //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1);
+
+ dtheta1 = atan2(sintheta1,costheta1);
+
+
+ //Angle error
+
+ m1_error = dtheta1-theta1;
+ m2_error = dtheta2-theta2;
+
+ //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error);
//PID controller
@@ -470,13 +349,18 @@
keep_in_range(&u2,-1,1);
//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.
+ dir_motor1 = u1>0 ? 0 : 1; //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 ? 1 : 0; //conditional statement, same as if else below
pwm_motor2.write(abs(u2));
+
+ // } //if
+ //} //while
+
+
/*if(u1 > 0)
{
dir_motor1 = 0;
@@ -522,30 +406,12 @@
}
void caliMenu(){};
-//Start sampling
-void start_sampling(void)
-{
- sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
-
- //Debug LED will be green when sampling is active
- green=0;
- pc.printf("||- started sampling -|| \r\n");
-}
-//stop sampling
-void stop_sampling(void)
-{
- sample_timer.detach();
-
- //Debug LED will be turned off when sampling stops
- green=1;
- pc.printf("||- stopped sampling -|| \r\n");
-}
//Start control
void start_control(void)
{
- control_timer.attach(&control,SAMPLE_RATE); //100 Hz control
+ control_timer.attach(&control,0.01); //100 Hz control
//Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
blue=0;