Arun Raveenthiran
bewegen met emg en terug naar home
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of
Cases_homepos_picontrol_EMG_2
by 
Rianne Bulthuis
Diff: main.cpp
- Revision:
- 10:34ccb2fed2ef
- Parent:
- 9:888ed3c72795
--- a/main.cpp Fri Oct 23 09:32:09 2015 +0000
+++ b/main.cpp Tue Oct 27 10:23:31 2015 +0000
@@ -7,29 +7,37 @@
// pins
DigitalOut motor1_direction(D4);
PwmOut motor1_speed(D5);
+DigitalOut motor2_direction(D7);
+PwmOut motor2_speed(D6);
DigitalIn button_1(PTC6); //counterclockwise
DigitalIn button_2(PTA4); //clockwise
AnalogIn PotMeter1(A4);
-AnalogIn EMG_bicepsright (A0);
+AnalogIn PotMeter2(A5);
+AnalogIn EMG_bicepsright(A0);
+AnalogIn EMG_bicepsleft(A1);
+AnalogIn EMG_tricepsright(A2);
+AnalogIn EMG_tricepsleft(A3);
Ticker controller;
Ticker ticker_regelaar;
Ticker EMG_Filter;
Ticker Scope;
Encoder motor1(D12,D13);
-HIDScope scope(2);
+Encoder motor2(D10,D11);
+HIDScope scope(6);
MODSERIAL pc(USBTX, USBRX);
// Regelaar homeposition
-#define SAMPLETIME_REGELAAR 0.005
+#define SAMPLETIME_REGELAAR 0.005
volatile bool regelaar_ticker_flag;
-void setregelaar_ticker_flag(){
+void setregelaar_ticker_flag()
+{
regelaar_ticker_flag = true;
}
//define states
-enum state {HOME, MOVE_MOTOR_1, BACKTOHOMEPOSITION, STOP};
+enum state {HOME, MOVE_MOTOR_1, MOVE_MOTOR_2, BACKTOHOMEPOSITION, STOP};
uint8_t state = HOME;
// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
@@ -41,54 +49,100 @@
const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
const double motor1_Ki = 0.002; // Integrating gain m1.
const double motor1_Ts = 0.01; // Time step m1
+const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
+const double motor2_Ki = 0.002; // Integrating gain m1.
+const double motor2_Ts = 0.01; // Time step m1
double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
+double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1
// Reusable P controller
-double Pc (double error, const double Kp){
- return motor1_Kp * error;
+double Pc1 (double error1, const double motor1_Kp)
+{
+ return motor1_Kp * err_int_m1;
+}
+double Pc2 (double error2, const double motor2_Kp)
+{
+ return motor2_Kp * err_int_m2;
}
// Measure the error and apply output to the plant
-void motor1_controlP(){
+void motor1_controlP()
+{
double referenceP1 = PotMeter1.read();
double positionP1 = q*motor1.getPosition();
- double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
+ double motorP1 = Pc1(referenceP1 - positionP1, motor1_Kp);
+}
+
+void motor2_controlP()
+{
+ double referenceP2 = PotMeter2.read();
+ double positionP2 = q*motor2.getPosition();
+ double motorP2 = Pc2(referenceP2 - positionP2, motor2_Kp);
}
// Reusable PI controller
-double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int){
- err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
- return motor1_Kp*error + motor1_Ki*err_int;
+double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int_m1)
+{
+ err_int_m1 = err_int_m1 * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
+ return motor1_Kp*error + motor1_Ki*err_int_m1;
} // De totale fout die wordt hersteld met behulp van PI control.
+double PI2 (double error2, const double motor2_Kp, const double motor2_Ki, const double motor2_Ts, double &err_int_m2)
+{
+ err_int_m2 = err_int_m2 * motor2_Ts*error2;
+ return motor2_Kp*error2 + motor2_Ki*err_int_m2;
+}
+
+void motor1_controlPI()
+{
+ double referencePI1 = PotMeter1.read();
+ double positionPI1 = q*motor1.getPosition();
+ double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
+}
+
+void motor2_controlPI()
+{
+ double referencePI2 = PotMeter2.read();
+ double positionPI2 = q*motor2.getPosition();
+ double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
+}
// Filter1 = High pass filter tot 20 Hz
-double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
-const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
-const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
-
+double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
+double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
+double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0;
+double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
+const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
+const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
// Filter2 = Low pass filter na 60 Hz
-double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
-const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
-const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
-
+double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
+double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
+double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
+double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
+const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
+const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
// Filter3 = Notch filter at 50 Hz
-double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
-const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
-const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
-const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
-
+double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
+double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
+double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
+double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
+const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
+const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
+const double fno3_a1 = -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
// Filter4 = Lowpass filter at 5 Hz
-double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
-const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
-const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
-
-double y1, y2, y3, y4, y5, y6, y7, y8, y9;
-double u1, u2, u3, u4, u5, u6, u7, u8, u9;
-double filter_br, filter_bl;
+double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
+double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
+double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
+double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
+const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
+const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
+double y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20, y21, y22, y23, y24, y25, y26, y27, y28, y29, y30, y31, y32, y33, y34, y35, y36;
+double A, B, C, D;
+double final_filter1, final_filter2, final_filter3, final_filter4;
// Standaard formule voor het biquad filter
-double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){
+double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+{
double v = u - a1*v1 - a2*v2;
double y = b0*v + b1*v1 + b2*v2;
v2=v1;
@@ -97,153 +151,234 @@
}
// script voor filters
-void Filters(){
- u1 = EMG_bicepsright.read();
+void Filters()
+{
+ A = EMG_bicepsright.read();
//Highpass
- y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
- u2 = y1;
- y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
+ y1 = biquad (A, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
+ y2 = biquad (y1, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
//Lowpass
- u3 = y2;
- y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
- u4 = y3;
- y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
+ y3 = biquad (y2, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
+ y4 = biquad (y3, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
+ // Notch
+ y5 = biquad (y4, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
+ y6 = biquad (y5, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
+ y7 = biquad (y6, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
+ // Rectify sample
+ y8 = fabs(y7);
+ // Make it smooth
+ y9 = biquad (y8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
+ final_filter1 = biquad(y9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
+
+ //Biceps right
+ B = EMG_bicepsleft.read();
+ //Highpass
+ y10 = biquad (B, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
+ y11 = biquad (y10, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
+ //Lowpass
+ y12 = biquad (y11, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
+ y13 = biquad (y12, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
// Notch
- u5 = y4;
- y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
- u6 = y5;
- y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
- u7 = y6;
- y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
- y8 = fabs (y7);
- //smooth
- u8 = y8;
- y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
- u9 = y9;
- filter_br = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+ y14 = biquad (y13, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
+ y15 = biquad (y14, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
+ y16 = biquad (y15, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
+ // Rectify sample
+ y17 = fabs(y16);
+ // Make it smooth
+ y18 = biquad (y17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
+ final_filter2 = biquad(y18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
+
+ /// EMG Filter left leg
+ C = EMG_tricepsright.read();
+ //Highpass
+ y19 = biquad (C, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
+ y20 = biquad (y19, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
+ //Lowpass
+ y21 = biquad (y20, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
+ y22 = biquad (y21, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
+ // Notch
+ y23 = biquad (y22, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
+ y24 = biquad (y23, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
+ y25 = biquad (y24, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
+ // Rectify sample
+ y26 = fabs(y25);
+ // Make it smooth
+ y27 = biquad (y26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
+ final_filter3 = biquad(y27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
+
+ // EMG filter right leg
+ D = EMG_tricepsleft.read();
+ //Highpass
+ y28 = biquad (D, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
+ y29 = biquad (y28, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
+ //Lowpass
+ y30 = biquad (y29, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
+ y31 = biquad (y30, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
+ // Notch
+ y32 = biquad (y31, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
+ y33 = biquad (y32, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
+ y34 = biquad (y33, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
+ // Rectify sample
+ y35 = fabs(y34);
+ // Make it smooth
+ y36 = biquad (y35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
+ final_filter4 = biquad(y36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
}
-
-void motor1_controlPI(){
- double referencePI1 = PotMeter1.read();
- double positionPI1 = q*motor1.getPosition();
- double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
-}
const int pressed = 0;
// constantes voor berekening homepositie
-double H;
-double P;
+double H1;
+double H2;
+double P1;
+double P2;
void move_motor1()
{
- if (filter_br > 0.03 || button_1 == pressed){
- pc.printf("Moving clockwise \n\r");
- motor1_direction = 0; //counterclockwise
- motor1_speed = 0.2;
- }
- else if (button_2 == pressed){
- pc.printf("Moving counterclockwise \n\r");
- motor1_direction = 1; //clockwise
- motor1_speed = 0.2;
- }
- else {
- pc.printf("Not moving \n\r");
- motor1_speed = 0;
- }
+ if (final_filter1 > 0.06 || button_1 == pressed) {
+ pc.printf("motor1 cw \n\r");
+ motor1_direction = 0; //counterclockwise
+ motor1_speed = 0.2;
+ } else if (final_filter3 > 0.07 || button_2 == pressed){
+ pc.printf("motor1 ccw \n\r");
+ motor1_direction = 1; //clockwise
+ motor1_speed = 0.2;
+ } else {
+ pc.printf("Not moving \n\r");
+ motor1_speed = 0;
+ }
}
-void movetohome(){
- P = motor1.getPosition();
-
- if (P >= -28 && P <= 28){
+void move_motor2()
+{
+ if (button_1 == pressed) {
+ pc.printf("motor2 cw \n\r");
+ motor2_direction = 1; //clockwise
+ motor2_speed = 0.4;
+ } else if (button_2 == pressed) {
+ pc.printf("Moving 2 counterclockwise \n\r");
+ motor2_direction = 0; //counterclockwise
+ motor2_speed = 0.4;
+ } else {
+ pc.printf("Not moving \n\r");
+ motor2_speed = 0;
+ }
+}
+
+void movetohome()
+{
+ P1 = motor1.getPosition();
+ P2 = motor2.getPosition();
+
+ if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
motor1_speed = 0;
- }
- else if (P > 24){
+ } else if (P1 > 24) {
motor1_direction = 1;
motor1_speed = 0.1;
- }
- else if (P < -24){
+ } else if (P1 < -24) {
motor1_direction = 0;
motor1_speed = 0.1;
+ } else if (P2 > 24) {
+ motor2_direction = 1;
+ motor2_speed = 0.1;
+ } else if (P2 < -24) {
+ motor2_direction = 0;
+ motor2_speed = 0.1;
}
}
-void HIDScope (){
- scope.set (0, filter_br);
- //scope.set (1, filter_bl);
- scope.set(1, motor1.getPosition());
+void HIDScope ()
+{
+ scope.set (0, final_filter1);
+ scope.set (1, final_filter2);
+ scope.set (2, final_filter3);
+ scope.set (3, final_filter4);
+ scope.set (4, motor1.getPosition());
+ scope.set (5, motor2.getPosition());
scope.send ();
- }
-
+}
+
int main()
-{
+{
while (true) {
pc.baud(9600); //pc baud rate van de computer
- EMG_Filter.attach_us(Filters, 1e3); //filters uitvoeren
- Scope.attach_us(HIDScope, 1e3); //EMG en encoder signaal naar de hidscope sturen
-
- switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
-
- case HOME: //positie op 0 zetten voor arm 1
- {
- pc.printf("home\n\r");
- H = motor1.getPosition();
- pc.printf("Home-position is %f\n\r", H);
- state = MOVE_MOTOR_1;
- wait(2);
- break;
- }
-
- case MOVE_MOTOR_1: //motor kan cw en ccw bewegen a.d.h.v. buttons
- {
- pc.printf("move_motor\n\r");
- while (state == MOVE_MOTOR_1){
- move_motor1();
- if (button_1 == pressed && button_2 == pressed){
- motor1_speed = 0;
- state = BACKTOHOMEPOSITION;
+ EMG_Filter.attach_us(Filters, 5e4); //filters uitvoeren
+ Scope.attach_us(HIDScope, 5e4); //EMG en encoder signaal naar de hidscope sturen
+
+ switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
+
+ case HOME: { //positie op 0 zetten voor arm 1
+ pc.printf("home\n\r");
+ H1 = motor1.getPosition();
+ H2 = motor2.getPosition();
+ pc.printf("Home-position is %f\n\r", H1);
+ pc.printf("Home-pso is %f\n\r", H2);
+ state = MOVE_MOTOR_1;
+ wait(2);
+ break;
}
+
+ case MOVE_MOTOR_1: { //motor kan cw en ccw bewegen a.d.h.v. buttons
+ pc.printf("move_motor\n\r");
+ while (state == MOVE_MOTOR_1) {
+ move_motor1();
+ if (button_1 == pressed && button_2 == pressed) {
+ motor1_speed = 0;
+ state = MOVE_MOTOR_2;
+ }
+ }
+ wait (1);
+ break;
}
- wait (1);
- break;
- }
-
- case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition
- {
- pc.printf("backhomeposition\n\r");
- wait (1);
-
- ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
- while(state == BACKTOHOMEPOSITION){
- movetohome();
- while(regelaar_ticker_flag != true);
- regelaar_ticker_flag = false;
-
- pc.printf("pulsmotorposition1 %d", motor1.getPosition());
- pc.printf("referentie %f\n\r", H);
-
- if (P <=24 && P >= -24){
- pc.printf("pulsmotorposition1 %d", motor1.getPosition());
- pc.printf("referentie %f\n\r", H);
- state = STOP;
- pc.printf("Laatste positie %f\n\r", motor1.getPosition());
+
+ case MOVE_MOTOR_2: { //motor kan cw en ccw bewegen a.d.h.v. buttons
+ pc.printf("move_motor\n\r");
+ while (state == MOVE_MOTOR_2) {
+ move_motor2();
+ if (button_1 == pressed && button_2 == pressed) {
+ motor2_speed = 0;
+ state = BACKTOHOMEPOSITION;
+ }
+ }
+ wait (1);
break;
+
+ case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
+ pc.printf("backhomeposition\n\r");
+ wait (1);
+
+ ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
+ while(state == BACKTOHOMEPOSITION) {
+ movetohome();
+ while(regelaar_ticker_flag != true);
+ regelaar_ticker_flag = false;
+
+ pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
+ pc.printf("referentie %f, %f \n\r", H1, H2);
+
+ if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
+ pc.printf("motor1 pos %d", motor1.getPosition());
+ pc.printf("motor2 pos %d", motor2.getPosition());
+ pc.printf("referentie %f %f\n\r", H1, H2);
+ state = STOP;
+ pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
+ break;
+ }
+ }
+ }
+ case STOP: {
+ static int c;
+ while(state == STOP) {
+ motor1_speed = 0;
+ motor2_speed = 0;
+ if (c++ == 0) {
+ pc.printf("einde\n\r");
+ }
+ }
+ break;
}
}
}
- case STOP:
- {
- static int c;
- while(state == STOP){
- motor1_speed = 0;
- if (c++ == 0){
- pc.printf("einde\n\r");
- }
- }
- break;
- }
-}
-}
+ }
}
\ No newline at end of file