group14 - k9
testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
testPID
by 
Martijn Kern
Diff: main.cpp
- Revision:
- 24:56db31267f10
- Parent:
- 23:e9b1b426cde6
- Child:
- 25:49ccdc98639a
--- a/main.cpp Tue Oct 13 12:57:33 2015 +0000
+++ b/main.cpp Wed Oct 14 13:04:54 2015 +0000
@@ -47,30 +47,95 @@
DigitalOut dir_motor1(D7); //Direction motor 1
DigitalOut dir_motor2(D4); //Direction motor 2
-//Filters
-//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
-biquadFilter highpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes DC and movement artefacts
-biquadFilter notch1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes 49-51 Hz power interference
-biquadFilter notch2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //
-biquadFilter lowpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // EMG envelope
-biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter
+//Limit Switches
+DigitalIn shoulder_limit(PTA4); //using FRDM buttons for now
+DigitalIn elbow_limit(PTC6); //using FRDM buttons for now
+
/*--------------------------------------------------------------------------------------------------------------------
---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
//EMG variables
-double biceps_power; double bicepsMVC = 0;
-double triceps_power; double tricepsMVC = 0;
-double flexor_power; double flexorMVC = 0;
-double extens_power; double extensorMVC = 0;
+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;
+
+//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.
+
+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
+
+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 - envelop
+const double low_b0 = 0.000119046743110057;
+const double low_b1 = 0.000238093486220118;
+const double low_b2 = 0.000119046743110057;
+const double low_a1 = -1.968902268531908;
+const double low_a2 = 0.9693784555043481;
/*--------------------------------------------------------------------------------------------------------------------
----- DECLARE FUNCTION NAMES-------------------------------------------------------------------------------------------
+---- Filters ---------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-void keep_in_range(float * in, float min, float max);
+
+//Using biquadFilter library
+//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
+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
+biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter
+
+/*--------------------------------------------------------------------------------------------------------------------
+---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
+void keep_in_range(double * in, double min, double max);
void sample_filter(void);
void control();
void calibrate_emg(int muscle);
@@ -90,12 +155,13 @@
int main()
{
- // make a menu, user has to initiated next step
+ pc.baud(115200);
+ // make a menu, user has to initiate next step
mainMenu();
caliMenu(); // Menu function
calibrate_arm(); //Start Calibration
start_sampling(); //500 Hz EMG
- calibrate_emg(1);
+ calibrate_emg(1); //calibrate muscle 1
start_control(); //100 Hz control
//maybe some stop commands when a button is pressed: detach from timers.
@@ -117,18 +183,6 @@
---- FUNCTIONS -------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-//Start sampling
-void start_sampling(void)
-{
- sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
-}
-
-//stop sampling
-void stop_sampling(void)
-{
- sample_timer.detach();
-}
-
//Sample and Filter
void sample_filter(void)
{
@@ -155,51 +209,72 @@
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)
{
+ bool hit = true;
dir_motor1=1; //ccw
dir_motor2=1; //ccw
- while(limitswitch != hit){
+ while(shoulder_limit != hit){
pwm_motor1.write(0.4);
+ }
+ Encoder1.reset();
+
+ while(elbow_limit != hit){
pwm_motor2.write(0.4);
}
- encoder1.reset();
- encoder2.reset();
+ Encoder2.reset();
}
//EMG Maximum Voluntary Contraction measurement
void calibrate_emg(int muscle)
{
- if(muscle==1){
start_sampling();
- wait(1);
+
+ //double sampletime=0;
+ //sampletime=+SAMPLE_RATE;
+ //
+ // if(sampletime<5)
for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
- if(biceps_power>bicepsMVC){
+ if(muscle==1){
+
+ if(biceps_power>bicepsMVC){
bicepsMVC=biceps_power;
- }
+ }
+ }
+
+ if(muscle==2){
+
+ if(triceps_power>tricepsMVC){
+ tricepsMVC=triceps_power;
+ }
+ }
+
+ if(muscle==3){
+
+ if(flexor_power>flexorMVC){
+ flexorMVC=flexor_power;
+ }
+ }
+
+ if(muscle==4){
+
+ if(extens_power>extensMVC){
+ extensMVC=extens_power;
+ }
+ }
}
stop_sampling();
- }
-
- if(muscle==2){
- start_sampling();
- wait(1);
-
- for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
- if(triceps_power>tricepsMVC){
- tricepsMVC=triceps_power;
- }
- }
- stop_sampling();
- }
+
}
@@ -207,11 +282,11 @@
double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
{
// Derivative
- double e_der = (error-e_prev)/CONTROL_RATE;
+ double e_der = (error-e_prev)/ CONTROL_RATE;
e_der = derfilter.step(e_der);
e_prev = error;
// Integral
- e_int = e_int+CONTROL_RATE*error;
+ e_int = e_int + CONTROL_RATE * error;
// PID
return kp*error + ki*e_int + kd * e_der;
@@ -229,9 +304,9 @@
//inverse kinematics (pos error to angle error)
//PID controller
- double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0;
- u1=pid(error,kp,ki,kd,e_int,e_prev); //motor 1
- u2=pid(error,kp,ki,kd,e_int,e_prev); //motor 2
+
+ 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,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1
keep_in_range(&u2,-1,1);
@@ -265,10 +340,25 @@
}
+void mainMenu(){};
+void caliMenu(){};
+
+//Start sampling
+void start_sampling(void)
+{
+ sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
+}
+
+//stop sampling
+void stop_sampling(void)
+{
+ sample_timer.detach();
+}
+
//Start control
void start_control(void)
{
- control_timer.attach(&control,SAMPLE_RATE); //100 Hz control
+ control_timer.attach(&control,CONTROL_RATE); //100 Hz control
}
//stop control
@@ -285,7 +375,7 @@
//keeps input limited between min max
-void keep_in_range(float * in, float min, float max)
+void keep_in_range(double * in, double min, double max)
{
*in > min ? *in < max? : *in = max: *in = min;
}
\ No newline at end of file