Richell Booyink / Mbed 2 deprecated Total_code_v1_Gr13

filters, homeposition and cases

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Cases_homepos_picontrol_EMG_2 by Arun Raveenthiran

main.cpp@11:b2dec8f3e75c, 2015-10-27 (annotated)

Committer:
RichellBooyink
Date:
Tue Oct 27 11:42:19 2015 +0000
Revision:
11:b2dec8f3e75c
Parent:
10:34ccb2fed2ef
Child:
12:146ba6f95fe0
Goede treshold waardes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arunr 0:65ab9f79a4cc 1 #include "mbed.h"
arunr 0:65ab9f79a4cc 2 #include "encoder.h"
arunr 0:65ab9f79a4cc 3 #include "HIDScope.h"
riannebulthuis 1:7d5e6bc2b314 4 #include "MODSERIAL.h"
arunr 0:65ab9f79a4cc 5
riannebulthuis 8:b219ca30967f 6
riannebulthuis 8:b219ca30967f 7 // pins
arunr 0:65ab9f79a4cc 8 DigitalOut motor1_direction(D4);
arunr 0:65ab9f79a4cc 9 PwmOut motor1_speed(D5);
arunr 10:34ccb2fed2ef 10 DigitalOut motor2_direction(D7);
arunr 10:34ccb2fed2ef 11 PwmOut motor2_speed(D6);
arunr 0:65ab9f79a4cc 12 DigitalIn button_1(PTC6); //counterclockwise
arunr 0:65ab9f79a4cc 13 DigitalIn button_2(PTA4); //clockwise
riannebulthuis 8:b219ca30967f 14 AnalogIn PotMeter1(A4);
arunr 10:34ccb2fed2ef 15 AnalogIn PotMeter2(A5);
arunr 10:34ccb2fed2ef 16 AnalogIn EMG_bicepsright(A0);
arunr 10:34ccb2fed2ef 17 AnalogIn EMG_bicepsleft(A1);
RichellBooyink 11:b2dec8f3e75c 18 AnalogIn EMG_legright(A2);
RichellBooyink 11:b2dec8f3e75c 19 AnalogIn EMG_legleft(A3);
arunr 5:b9d5d7311dac 20 Ticker controller;
arunr 5:b9d5d7311dac 21 Ticker ticker_regelaar;
arunr 6:1597888c9a56 22 Ticker EMG_Filter;
arunr 6:1597888c9a56 23 Ticker Scope;
arunr 0:65ab9f79a4cc 24 Encoder motor1(D12,D13);
arunr 10:34ccb2fed2ef 25 Encoder motor2(D10,D11);
arunr 10:34ccb2fed2ef 26 HIDScope scope(6);
riannebulthuis 8:b219ca30967f 27 MODSERIAL pc(USBTX, USBRX);
arunr 0:65ab9f79a4cc 28
riannebulthuis 8:b219ca30967f 29
riannebulthuis 8:b219ca30967f 30
riannebulthuis 8:b219ca30967f 31 // Regelaar homeposition
arunr 10:34ccb2fed2ef 32 #define SAMPLETIME_REGELAAR 0.005
riannebulthuis 2:866a8a9f2b93 33 volatile bool regelaar_ticker_flag;
arunr 10:34ccb2fed2ef 34 void setregelaar_ticker_flag()
arunr 10:34ccb2fed2ef 35 {
riannebulthuis 2:866a8a9f2b93 36 regelaar_ticker_flag = true;
riannebulthuis 2:866a8a9f2b93 37 }
riannebulthuis 2:866a8a9f2b93 38
riannebulthuis 2:866a8a9f2b93 39 //define states
arunr 10:34ccb2fed2ef 40 enum state {HOME, MOVE_MOTOR_1, MOVE_MOTOR_2, BACKTOHOMEPOSITION, STOP};
arunr 4:b4530fb376dd 41 uint8_t state = HOME;
arunr 0:65ab9f79a4cc 42
riannebulthuis 8:b219ca30967f 43 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
riannebulthuis 3:5f59cbe53d7d 44 const double g = 360; // Aantal graden 1 rotatie
riannebulthuis 3:5f59cbe53d7d 45 const double c = 4200; // Aantal counts 1 rotatie
riannebulthuis 3:5f59cbe53d7d 46 const double q = c/(g);
riannebulthuis 3:5f59cbe53d7d 47
riannebulthuis 3:5f59cbe53d7d 48 //PI-controller constante
riannebulthuis 3:5f59cbe53d7d 49 const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
riannebulthuis 3:5f59cbe53d7d 50 const double motor1_Ki = 0.002; // Integrating gain m1.
riannebulthuis 3:5f59cbe53d7d 51 const double motor1_Ts = 0.01; // Time step m1
arunr 10:34ccb2fed2ef 52 const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
arunr 10:34ccb2fed2ef 53 const double motor2_Ki = 0.002; // Integrating gain m1.
arunr 10:34ccb2fed2ef 54 const double motor2_Ts = 0.01; // Time step m1
riannebulthuis 3:5f59cbe53d7d 55 double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
arunr 10:34ccb2fed2ef 56 double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1
riannebulthuis 3:5f59cbe53d7d 57
riannebulthuis 3:5f59cbe53d7d 58 // Reusable P controller
arunr 10:34ccb2fed2ef 59 double Pc1 (double error1, const double motor1_Kp)
arunr 10:34ccb2fed2ef 60 {
arunr 10:34ccb2fed2ef 61 return motor1_Kp * err_int_m1;
arunr 10:34ccb2fed2ef 62 }
arunr 10:34ccb2fed2ef 63 double Pc2 (double error2, const double motor2_Kp)
arunr 10:34ccb2fed2ef 64 {
arunr 10:34ccb2fed2ef 65 return motor2_Kp * err_int_m2;
riannebulthuis 3:5f59cbe53d7d 66 }
riannebulthuis 3:5f59cbe53d7d 67
riannebulthuis 3:5f59cbe53d7d 68 // Measure the error and apply output to the plant
arunr 10:34ccb2fed2ef 69 void motor1_controlP()
arunr 10:34ccb2fed2ef 70 {
riannebulthuis 3:5f59cbe53d7d 71 double referenceP1 = PotMeter1.read();
riannebulthuis 3:5f59cbe53d7d 72 double positionP1 = q*motor1.getPosition();
arunr 10:34ccb2fed2ef 73 double motorP1 = Pc1(referenceP1 - positionP1, motor1_Kp);
arunr 10:34ccb2fed2ef 74 }
arunr 10:34ccb2fed2ef 75
arunr 10:34ccb2fed2ef 76 void motor2_controlP()
arunr 10:34ccb2fed2ef 77 {
arunr 10:34ccb2fed2ef 78 double referenceP2 = PotMeter2.read();
arunr 10:34ccb2fed2ef 79 double positionP2 = q*motor2.getPosition();
arunr 10:34ccb2fed2ef 80 double motorP2 = Pc2(referenceP2 - positionP2, motor2_Kp);
riannebulthuis 3:5f59cbe53d7d 81 }
riannebulthuis 3:5f59cbe53d7d 82
riannebulthuis 3:5f59cbe53d7d 83 // Reusable PI controller
arunr 10:34ccb2fed2ef 84 double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int_m1)
arunr 10:34ccb2fed2ef 85 {
arunr 10:34ccb2fed2ef 86 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
arunr 10:34ccb2fed2ef 87 return motor1_Kp*error + motor1_Ki*err_int_m1;
riannebulthuis 3:5f59cbe53d7d 88 } // De totale fout die wordt hersteld met behulp van PI control.
riannebulthuis 3:5f59cbe53d7d 89
arunr 10:34ccb2fed2ef 90 double PI2 (double error2, const double motor2_Kp, const double motor2_Ki, const double motor2_Ts, double &err_int_m2)
arunr 10:34ccb2fed2ef 91 {
arunr 10:34ccb2fed2ef 92 err_int_m2 = err_int_m2 * motor2_Ts*error2;
arunr 10:34ccb2fed2ef 93 return motor2_Kp*error2 + motor2_Ki*err_int_m2;
arunr 10:34ccb2fed2ef 94 }
arunr 10:34ccb2fed2ef 95
arunr 10:34ccb2fed2ef 96 void motor1_controlPI()
arunr 10:34ccb2fed2ef 97 {
arunr 10:34ccb2fed2ef 98 double referencePI1 = PotMeter1.read();
arunr 10:34ccb2fed2ef 99 double positionPI1 = q*motor1.getPosition();
arunr 10:34ccb2fed2ef 100 double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
arunr 10:34ccb2fed2ef 101 }
arunr 10:34ccb2fed2ef 102
arunr 10:34ccb2fed2ef 103 void motor2_controlPI()
arunr 10:34ccb2fed2ef 104 {
arunr 10:34ccb2fed2ef 105 double referencePI2 = PotMeter2.read();
arunr 10:34ccb2fed2ef 106 double positionPI2 = q*motor2.getPosition();
arunr 10:34ccb2fed2ef 107 double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
arunr 10:34ccb2fed2ef 108 }
arunr 5:b9d5d7311dac 109
arunr 5:b9d5d7311dac 110 // Filter1 = High pass filter tot 20 Hz
arunr 10:34ccb2fed2ef 111 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
arunr 10:34ccb2fed2ef 112 double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
arunr 10:34ccb2fed2ef 113 double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0;
arunr 10:34ccb2fed2ef 114 double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
arunr 10:34ccb2fed2ef 115 const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
arunr 10:34ccb2fed2ef 116 const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
arunr 5:b9d5d7311dac 117 // Filter2 = Low pass filter na 60 Hz
arunr 10:34ccb2fed2ef 118 double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
arunr 10:34ccb2fed2ef 119 double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
arunr 10:34ccb2fed2ef 120 double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
arunr 10:34ccb2fed2ef 121 double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
arunr 10:34ccb2fed2ef 122 const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
arunr 10:34ccb2fed2ef 123 const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
arunr 5:b9d5d7311dac 124 // Filter3 = Notch filter at 50 Hz
arunr 10:34ccb2fed2ef 125 double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
arunr 10:34ccb2fed2ef 126 double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
arunr 10:34ccb2fed2ef 127 double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
arunr 10:34ccb2fed2ef 128 double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
arunr 10:34ccb2fed2ef 129 const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
arunr 10:34ccb2fed2ef 130 const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
arunr 10:34ccb2fed2ef 131 const double fno3_a1 = -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
arunr 5:b9d5d7311dac 132 // Filter4 = Lowpass filter at 5 Hz
arunr 10:34ccb2fed2ef 133 double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
arunr 10:34ccb2fed2ef 134 double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
arunr 10:34ccb2fed2ef 135 double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
arunr 10:34ccb2fed2ef 136 double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
arunr 10:34ccb2fed2ef 137 const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
arunr 10:34ccb2fed2ef 138 const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
arunr 10:34ccb2fed2ef 139 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;
arunr 10:34ccb2fed2ef 140 double A, B, C, D;
arunr 10:34ccb2fed2ef 141 double final_filter1, final_filter2, final_filter3, final_filter4;
arunr 5:b9d5d7311dac 142
arunr 5:b9d5d7311dac 143 // Standaard formule voor het biquad filter
arunr 10:34ccb2fed2ef 144 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
arunr 10:34ccb2fed2ef 145 {
arunr 5:b9d5d7311dac 146 double v = u - a1*v1 - a2*v2;
arunr 5:b9d5d7311dac 147 double y = b0*v + b1*v1 + b2*v2;
arunr 5:b9d5d7311dac 148 v2=v1;
arunr 5:b9d5d7311dac 149 v1=v;
arunr 5:b9d5d7311dac 150 return y;
arunr 5:b9d5d7311dac 151 }
arunr 5:b9d5d7311dac 152
riannebulthuis 8:b219ca30967f 153 // script voor filters
arunr 10:34ccb2fed2ef 154 void Filters()
arunr 10:34ccb2fed2ef 155 {
RichellBooyink 11:b2dec8f3e75c 156 // Biceps right
arunr 10:34ccb2fed2ef 157 A = EMG_bicepsright.read();
arunr 5:b9d5d7311dac 158 //Highpass
arunr 10:34ccb2fed2ef 159 y1 = biquad (A, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 160 y2 = biquad (y1, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 5:b9d5d7311dac 161 //Lowpass
arunr 10:34ccb2fed2ef 162 y3 = biquad (y2, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 163 y4 = biquad (y3, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 164 // Notch
arunr 10:34ccb2fed2ef 165 y5 = biquad (y4, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 166 y6 = biquad (y5, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 167 y7 = biquad (y6, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 168 // Rectify sample
arunr 10:34ccb2fed2ef 169 y8 = fabs(y7);
arunr 10:34ccb2fed2ef 170 // Make it smooth
arunr 10:34ccb2fed2ef 171 y9 = biquad (y8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
arunr 10:34ccb2fed2ef 172 final_filter1 = biquad(y9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 173
RichellBooyink 11:b2dec8f3e75c 174 //Biceps left
arunr 10:34ccb2fed2ef 175 B = EMG_bicepsleft.read();
arunr 10:34ccb2fed2ef 176 //Highpass
arunr 10:34ccb2fed2ef 177 y10 = biquad (B, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 178 y11 = biquad (y10, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 179 //Lowpass
arunr 10:34ccb2fed2ef 180 y12 = biquad (y11, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 181 y13 = biquad (y12, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 5:b9d5d7311dac 182 // Notch
arunr 10:34ccb2fed2ef 183 y14 = biquad (y13, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 184 y15 = biquad (y14, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 185 y16 = biquad (y15, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 186 // Rectify sample
arunr 10:34ccb2fed2ef 187 y17 = fabs(y16);
arunr 10:34ccb2fed2ef 188 // Make it smooth
arunr 10:34ccb2fed2ef 189 y18 = biquad (y17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 190 final_filter2 = biquad(y18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 191
RichellBooyink 11:b2dec8f3e75c 192 /// EMG Filter right leg
RichellBooyink 11:b2dec8f3e75c 193 C = EMG_legright.read();
arunr 10:34ccb2fed2ef 194 //Highpass
arunr 10:34ccb2fed2ef 195 y19 = biquad (C, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 196 y20 = biquad (y19, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 197 //Lowpass
arunr 10:34ccb2fed2ef 198 y21 = biquad (y20, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 199 y22 = biquad (y21, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 200 // Notch
arunr 10:34ccb2fed2ef 201 y23 = biquad (y22, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 202 y24 = biquad (y23, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 203 y25 = biquad (y24, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 204 // Rectify sample
arunr 10:34ccb2fed2ef 205 y26 = fabs(y25);
arunr 10:34ccb2fed2ef 206 // Make it smooth
arunr 10:34ccb2fed2ef 207 y27 = biquad (y26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 208 final_filter3 = biquad(y27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 209
RichellBooyink 11:b2dec8f3e75c 210 // EMG filter left leg
RichellBooyink 11:b2dec8f3e75c 211 D = EMG_legleft.read();
arunr 10:34ccb2fed2ef 212 //Highpass
arunr 10:34ccb2fed2ef 213 y28 = biquad (D, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 214 y29 = biquad (y28, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 215 //Lowpass
arunr 10:34ccb2fed2ef 216 y30 = biquad (y29, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 217 y31 = biquad (y30, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 218 // Notch
arunr 10:34ccb2fed2ef 219 y32 = biquad (y31, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 220 y33 = biquad (y32, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 221 y34 = biquad (y33, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 222 // Rectify sample
arunr 10:34ccb2fed2ef 223 y35 = fabs(y34);
arunr 10:34ccb2fed2ef 224 // Make it smooth
arunr 10:34ccb2fed2ef 225 y36 = biquad (y35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 226 final_filter4 = biquad(y36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
riannebulthuis 9:888ed3c72795 227 }
arunr 5:b9d5d7311dac 228
riannebulthuis 3:5f59cbe53d7d 229
arunr 0:65ab9f79a4cc 230 const int pressed = 0;
arunr 0:65ab9f79a4cc 231
riannebulthuis 8:b219ca30967f 232 // constantes voor berekening homepositie
arunr 10:34ccb2fed2ef 233 double H1;
arunr 10:34ccb2fed2ef 234 double H2;
arunr 10:34ccb2fed2ef 235 double P1;
arunr 10:34ccb2fed2ef 236 double P2;
arunr 0:65ab9f79a4cc 237
riannebulthuis 8:b219ca30967f 238 void move_motor1()
riannebulthuis 8:b219ca30967f 239 {
RichellBooyink 11:b2dec8f3e75c 240 if (final_filter1 > 0.04 || button_1 == pressed) {
arunr 10:34ccb2fed2ef 241 pc.printf("motor1 cw \n\r");
arunr 10:34ccb2fed2ef 242 motor1_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 243 motor1_speed = 0.2;
RichellBooyink 11:b2dec8f3e75c 244 } else if (final_filter2 > 0.04 || button_2 == pressed){
arunr 10:34ccb2fed2ef 245 pc.printf("motor1 ccw \n\r");
arunr 10:34ccb2fed2ef 246 motor1_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 247 motor1_speed = 0.2;
arunr 10:34ccb2fed2ef 248 } else {
arunr 10:34ccb2fed2ef 249 pc.printf("Not moving \n\r");
arunr 10:34ccb2fed2ef 250 motor1_speed = 0;
arunr 10:34ccb2fed2ef 251 }
arunr 0:65ab9f79a4cc 252 }
arunr 0:65ab9f79a4cc 253
arunr 10:34ccb2fed2ef 254 void move_motor2()
arunr 10:34ccb2fed2ef 255 {
RichellBooyink 11:b2dec8f3e75c 256 if (final_filter3 > 0.05) {
arunr 10:34ccb2fed2ef 257 pc.printf("motor2 cw \n\r");
arunr 10:34ccb2fed2ef 258 motor2_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 259 motor2_speed = 0.4;
RichellBooyink 11:b2dec8f3e75c 260 } else if (final_filter4 > 0.05) {
RichellBooyink 11:b2dec8f3e75c 261 pc.printf("motor2 ccw \n\r");
arunr 10:34ccb2fed2ef 262 motor2_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 263 motor2_speed = 0.4;
arunr 10:34ccb2fed2ef 264 } else {
arunr 10:34ccb2fed2ef 265 pc.printf("Not moving \n\r");
arunr 10:34ccb2fed2ef 266 motor2_speed = 0;
arunr 10:34ccb2fed2ef 267 }
arunr 10:34ccb2fed2ef 268 }
arunr 10:34ccb2fed2ef 269
arunr 10:34ccb2fed2ef 270 void movetohome()
arunr 10:34ccb2fed2ef 271 {
arunr 10:34ccb2fed2ef 272 P1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 273 P2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 274
arunr 10:34ccb2fed2ef 275 if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
arunr 0:65ab9f79a4cc 276 motor1_speed = 0;
arunr 10:34ccb2fed2ef 277 } else if (P1 > 24) {
riannebulthuis 3:5f59cbe53d7d 278 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 279 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 280 } else if (P1 < -24) {
riannebulthuis 3:5f59cbe53d7d 281 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 282 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 283 } else if (P2 > 24) {
arunr 10:34ccb2fed2ef 284 motor2_direction = 1;
arunr 10:34ccb2fed2ef 285 motor2_speed = 0.1;
arunr 10:34ccb2fed2ef 286 } else if (P2 < -24) {
arunr 10:34ccb2fed2ef 287 motor2_direction = 0;
arunr 10:34ccb2fed2ef 288 motor2_speed = 0.1;
arunr 0:65ab9f79a4cc 289 }
arunr 0:65ab9f79a4cc 290 }
arunr 0:65ab9f79a4cc 291
arunr 10:34ccb2fed2ef 292 void HIDScope ()
arunr 10:34ccb2fed2ef 293 {
arunr 10:34ccb2fed2ef 294 scope.set (0, final_filter1);
arunr 10:34ccb2fed2ef 295 scope.set (1, final_filter2);
arunr 10:34ccb2fed2ef 296 scope.set (2, final_filter3);
arunr 10:34ccb2fed2ef 297 scope.set (3, final_filter4);
arunr 10:34ccb2fed2ef 298 scope.set (4, motor1.getPosition());
arunr 10:34ccb2fed2ef 299 scope.set (5, motor2.getPosition());
arunr 6:1597888c9a56 300 scope.send ();
arunr 10:34ccb2fed2ef 301 }
arunr 10:34ccb2fed2ef 302
arunr 0:65ab9f79a4cc 303 int main()
arunr 10:34ccb2fed2ef 304 {
arunr 0:65ab9f79a4cc 305 while (true) {
riannebulthuis 8:b219ca30967f 306 pc.baud(9600); //pc baud rate van de computer
arunr 10:34ccb2fed2ef 307 EMG_Filter.attach_us(Filters, 5e4); //filters uitvoeren
arunr 10:34ccb2fed2ef 308 Scope.attach_us(HIDScope, 5e4); //EMG en encoder signaal naar de hidscope sturen
arunr 10:34ccb2fed2ef 309
arunr 10:34ccb2fed2ef 310 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
arunr 10:34ccb2fed2ef 311
arunr 10:34ccb2fed2ef 312 case HOME: { //positie op 0 zetten voor arm 1
arunr 10:34ccb2fed2ef 313 pc.printf("home\n\r");
arunr 10:34ccb2fed2ef 314 H1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 315 H2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 316 pc.printf("Home-position is %f\n\r", H1);
arunr 10:34ccb2fed2ef 317 pc.printf("Home-pso is %f\n\r", H2);
arunr 10:34ccb2fed2ef 318 state = MOVE_MOTOR_1;
arunr 10:34ccb2fed2ef 319 wait(2);
arunr 10:34ccb2fed2ef 320 break;
riannebulthuis 2:866a8a9f2b93 321 }
arunr 10:34ccb2fed2ef 322
arunr 10:34ccb2fed2ef 323 case MOVE_MOTOR_1: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 324 pc.printf("move_motor\n\r");
arunr 10:34ccb2fed2ef 325 while (state == MOVE_MOTOR_1) {
arunr 10:34ccb2fed2ef 326 move_motor1();
arunr 10:34ccb2fed2ef 327 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 328 motor1_speed = 0;
arunr 10:34ccb2fed2ef 329 state = MOVE_MOTOR_2;
arunr 10:34ccb2fed2ef 330 }
arunr 10:34ccb2fed2ef 331 }
arunr 10:34ccb2fed2ef 332 wait (1);
arunr 10:34ccb2fed2ef 333 break;
riannebulthuis 2:866a8a9f2b93 334 }
arunr 10:34ccb2fed2ef 335
arunr 10:34ccb2fed2ef 336 case MOVE_MOTOR_2: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 337 pc.printf("move_motor\n\r");
arunr 10:34ccb2fed2ef 338 while (state == MOVE_MOTOR_2) {
arunr 10:34ccb2fed2ef 339 move_motor2();
arunr 10:34ccb2fed2ef 340 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 341 motor2_speed = 0;
arunr 10:34ccb2fed2ef 342 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 343 }
arunr 10:34ccb2fed2ef 344 }
arunr 10:34ccb2fed2ef 345 wait (1);
riannebulthuis 3:5f59cbe53d7d 346 break;
arunr 10:34ccb2fed2ef 347
arunr 10:34ccb2fed2ef 348 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
arunr 10:34ccb2fed2ef 349 pc.printf("backhomeposition\n\r");
arunr 10:34ccb2fed2ef 350 wait (1);
arunr 10:34ccb2fed2ef 351
arunr 10:34ccb2fed2ef 352 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
arunr 10:34ccb2fed2ef 353 while(state == BACKTOHOMEPOSITION) {
arunr 10:34ccb2fed2ef 354 movetohome();
arunr 10:34ccb2fed2ef 355 while(regelaar_ticker_flag != true);
arunr 10:34ccb2fed2ef 356 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 357
arunr 10:34ccb2fed2ef 358 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
arunr 10:34ccb2fed2ef 359 pc.printf("referentie %f, %f \n\r", H1, H2);
arunr 10:34ccb2fed2ef 360
arunr 10:34ccb2fed2ef 361 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
arunr 10:34ccb2fed2ef 362 pc.printf("motor1 pos %d", motor1.getPosition());
arunr 10:34ccb2fed2ef 363 pc.printf("motor2 pos %d", motor2.getPosition());
arunr 10:34ccb2fed2ef 364 pc.printf("referentie %f %f\n\r", H1, H2);
arunr 10:34ccb2fed2ef 365 state = STOP;
arunr 10:34ccb2fed2ef 366 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
arunr 10:34ccb2fed2ef 367 break;
arunr 10:34ccb2fed2ef 368 }
arunr 10:34ccb2fed2ef 369 }
arunr 10:34ccb2fed2ef 370 }
arunr 10:34ccb2fed2ef 371 case STOP: {
arunr 10:34ccb2fed2ef 372 static int c;
arunr 10:34ccb2fed2ef 373 while(state == STOP) {
arunr 10:34ccb2fed2ef 374 motor1_speed = 0;
arunr 10:34ccb2fed2ef 375 motor2_speed = 0;
arunr 10:34ccb2fed2ef 376 if (c++ == 0) {
arunr 10:34ccb2fed2ef 377 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 378 }
arunr 10:34ccb2fed2ef 379 }
arunr 10:34ccb2fed2ef 380 break;
riannebulthuis 2:866a8a9f2b93 381 }
riannebulthuis 3:5f59cbe53d7d 382 }
riannebulthuis 3:5f59cbe53d7d 383 }
arunr 10:34ccb2fed2ef 384 }
arunr 0:65ab9f79a4cc 385 }