BMT M9 groep 7
/
Motor_EMG_FinalV1_aangepastePIDwaarden
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Motor_EMG_FinalV1
by 
BMT M9 groep 7
Revision 1:3f49c8818619, committed 2015-10-22
- Comitter:
- Rvs94
- Date:
- Thu Oct 22 10:46:10 2015 +0000
- Parent:
- 0:5816557b2064
- Child:
- 2:7873c44b0568
- Commit message:
- Werkende PID controller incl EMG aansturing
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Oct 19 10:06:36 2015 +0000
+++ b/main.cpp Thu Oct 22 10:46:10 2015 +0000
@@ -15,7 +15,7 @@
MODSERIAL pc(USBTX, USBRX); // To/From PC
QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
- HIDScope scope(5); // Scope, 4 channels
+ HIDScope scope(6); // Scope, 4 channels
// LEDs
DigitalOut LedR(LED_RED);
@@ -50,10 +50,6 @@
int count = 0;
double Grens2 = 90, Grens1 = 90;
double Stapgrootte = 5;
-
- DigitalOut led(LED_RED);
- DigitalOut ledG(LED_GREEN);
- DigitalOut ledB(LED_BLUE);
// Declaring variables
double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
@@ -85,18 +81,13 @@
const double m2_Ts = 0.01, m1_Ts = 0.01;
//Controller gain Motor 2 & 1
- const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
- const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
+ const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57;
+ const double m1_Kp = 2.1/57,m1_Ki = 3.9/57, m1_Kd = 0.1/57;
double m2_err_int = 0, m2_prev_err = 0;
double m1_err_int = 0, m1_prev_err = 0;
-
-//Derivative filter coeffs Motor 2 & 1
- const double BiGain2 = 0.012, BiGain1 = 0.016955;
- const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
- const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
// coëfficiënten
-const double BiGainEMG_H1 = 0.796821;
+ const double BiGainEMG_H1 = 0.796821;
const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
const double BiGainEMG_L1= 0.001041;
@@ -104,10 +95,6 @@
const double BiGainEMG_N1 = 1.0;
const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
-
-// Filter variables
- double m2_f_v1 = 0, m2_f_v2 = 0;
- double m1_f_v1 = 0, m1_f_v2 = 0;
// Creating the filters
biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
@@ -126,12 +113,11 @@
//HIDScope
void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
{
- scope.set(0, reference2 - position2);
- scope.set(1, position2);
- scope.set(2, reference1 - position1);
- scope.set(3, position1);
- scope.set(4, EMG_left_MAF);
- scope.set(5, EMG_right_MAF);
+ scope.set(0, position2);
+ scope.set(1, position1);
+ scope.set(2, EMG_left_MAF);
+ scope.set(3, EMG_right_MAF);
+ scope.set(4, count);
scope.send();
}
@@ -198,7 +184,7 @@
{
// Setpoint motor 2
reference2 = m2_ref; // Reference in degrees
- position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
+ position2 = 360.0*Encoder2.getPulses()/(32.0*131.0); // Position in degrees
// Speed control
double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
double m2_P2 = m2_P1;
@@ -219,7 +205,7 @@
{
// Setpoint Motor 1
reference1 = m1_ref; // Reference in degrees
- position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
+ position1 = 360.0*Encoder1.getPulses()/(32.0*131.0); // Position in degrees
// Speed control
double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
double m1_P2 = m1_P1;
@@ -258,46 +244,51 @@
SampleEMGRight.attach(&EMGfilterRight, 0.01f);
MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f);
MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f);
-
- // Defining threshold
- ledG.write(1), led.write(1), ledB.write(1);
+//--------------------------------------------------------------------------------------------------------------------------//
+// Determing Threshold
+//--------------------------------------------------------------------------------------------------------------------------//
wait(20);
- ledG.write(1);
- wait(0.2);
- ledG.write(0);
- wait(0.2);
- ledG.write(1);
- wait(0.2);
- ledG.write(0);
- wait(0.2);
- ledG.write(1);
- wait(0.2);
- ledG.write(0);
- wait(2);
- Threshold1 = 0.5*EMG_left_MAF;
- Threshold2 = 0.2*EMG_left_MAF;
- ledG.write(1);
+ LedG.write(1);
+ wait(0.2);
+ LedG.write(0);
+ wait(0.2);
+ LedG.write(1);
+ wait(0.2);
+ LedG.write(0);
+ wait(0.2);
+ LedG.write(1);
+ wait(0.2);
+ LedG.write(0);
+ wait(2);
+ Threshold1 = 0.8*EMG_left_MAF;
+ Threshold2 = 0.2*EMG_left_MAF;
+ LedG.write(1);
+ LedR.write(0);
+ wait(2);
+ LedR.write(1);
+
+ wait(2);
+ LedB.write(1);
+ wait(0.2);
+ LedB.write(0);
+ wait(0.2);
+ LedB.write(1);
+ wait(0.2);
+ LedB.write(0);
+ wait(0.2);
+ LedB.write(1);
+ wait(0.2);
+ LedB.write(0);
+ wait(2);
+ Threshold3 = 0.8*EMG_right_MAF;
+ Threshold4 = 0.2*EMG_right_MAF;
+ LedB.write(1);
+ pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4);
+ LedR.write(0);
+ wait(2);
+ LedR.write(1);
- wait(2);
- ledB.write(1);
- wait(0.2);
- ledB.write(0);
- wait(0.2);
- ledB.write(1);
- wait(0.2);
- ledB.write(0);
- wait(0.2);
- ledB.write(1);
- wait(0.2);
- ledB.write(0);
- wait(2);
- Threshold3 = 0.5*EMG_right_MAF;
- Threshold4 = 0.2*EMG_right_MAF;
- ledB.write(1);
-
- pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4);
- ledG.write(1);
//--------------------------------------------------------------------------------------------------------------------------//
// Control Program
@@ -306,14 +297,14 @@
{
//char c = pc.getc();
// 1 Program UP
- if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
+ if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
{
count = count + 1;
if(count > 2)
{
count = 2;
}
-
+ wait(2);
}
// 1 Program DOWN
// if(c == 'd') // Hoe gaat dit aangestuurd worden?
@@ -339,6 +330,7 @@
m2_ref = Grens2;
m1_ref = -1*Grens1;
}
+ wait(0.1);
}
if((EMG_right_MAF < Threshold1) && (EMG_left_MAF > Threshold1)) //if (c == 'f') //
{
@@ -349,28 +341,33 @@
m2_ref = -1*Grens2;
m1_ref = Grens1;
}
+ wait(0.1);
}
-
+ }
// PROGRAM 1: Motor 1 control, Red LED
if(count == 1)
{
LedG = LedB = 1;
LedR = 0;
- if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF <= Threshold1)) // if(c == 't') //
+ if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) // if(c == 't') //
{
m1_ref = m1_ref + Stapgrootte;
+
if (m1_ref > Grens1)
{
m1_ref = Grens1;
}
+ wait(0.1);
}
- if ((EMG_left_MAF > Threshold1) && (EMG_right_MAF < Threshold1)) //if(c == 'g') //
+ if ((EMG_left_MAF > Threshold1) && (EMG_right_MAF < Threshold3)) //if(c == 'g') //
{
m1_ref = m1_ref - Stapgrootte;
+
if (m1_ref < -1*Grens1)
{
m1_ref = -1*Grens1;
}
+ wait(0.1);
}
}
// PROGRAM 2: Firing mechanism & Reset, Blue LED
@@ -385,6 +382,6 @@
m1_ref = 0;
count = 0;
}
-}
+
}
}
\ No newline at end of file