Richell Booyink / Mbed 2 deprecated TotalCodegr13V2

Filters hebben de goede waarden. Safety stop werkt nog niet.

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Total_code_v1_Gr13 by Richell Booyink

main.cpp@13:4f4cc1a5a79a, 2015-10-28 (annotated)

Committer:
RichellBooyink
Date:
Wed Oct 28 14:29:18 2015 +0000
Revision:
13:4f4cc1a5a79a
Parent:
12:146ba6f95fe0 
Filters goed. ; Safety stop werkt nog niet

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
RichellBooyink 12:146ba6f95fe0 40 enum state {HOME, MOVE_MOTORS, 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 12:146ba6f95fe0 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;
RichellBooyink 13:4f4cc1a5a79a 237 // Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
RichellBooyink 13:4f4cc1a5a79a 238 volatile bool safety_stop;
arunr 0:65ab9f79a4cc 239
riannebulthuis 8:b219ca30967f 240 void move_motor1()
riannebulthuis 8:b219ca30967f 241 {
RichellBooyink 13:4f4cc1a5a79a 242 if (safety_stop == true && (final_filter1 > 0.02 || button_1 == pressed)) {
arunr 10:34ccb2fed2ef 243 pc.printf("motor1 cw \n\r");
arunr 10:34ccb2fed2ef 244 motor1_direction = 0; //counterclockwise
RichellBooyink 13:4f4cc1a5a79a 245 motor1_speed = 0.1;
RichellBooyink 13:4f4cc1a5a79a 246 } else if (safety_stop == true && (final_filter2 > 0.02 || button_2 == pressed)) {
arunr 10:34ccb2fed2ef 247 pc.printf("motor1 ccw \n\r");
arunr 10:34ccb2fed2ef 248 motor1_direction = 1; //clockwise
RichellBooyink 13:4f4cc1a5a79a 249 motor1_speed = 0.1 ;
arunr 10:34ccb2fed2ef 250 } else {
RichellBooyink 13:4f4cc1a5a79a 251 pc.printf("Not moving1 \n\r");
arunr 10:34ccb2fed2ef 252 motor1_speed = 0;
arunr 10:34ccb2fed2ef 253 }
arunr 0:65ab9f79a4cc 254 }
arunr 0:65ab9f79a4cc 255
arunr 10:34ccb2fed2ef 256 void move_motor2()
arunr 10:34ccb2fed2ef 257 {
RichellBooyink 13:4f4cc1a5a79a 258 if (safety_stop == true && (final_filter3 > 0.08 || button_1 == pressed)) {
arunr 10:34ccb2fed2ef 259 pc.printf("motor2 cw \n\r");
arunr 10:34ccb2fed2ef 260 motor2_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 261 motor2_speed = 0.4;
RichellBooyink 13:4f4cc1a5a79a 262 } else if (safety_stop == true && (final_filter4 > 0.08 || button_2 == pressed)) {
RichellBooyink 11:b2dec8f3e75c 263 pc.printf("motor2 ccw \n\r");
arunr 10:34ccb2fed2ef 264 motor2_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 265 motor2_speed = 0.4;
RichellBooyink 13:4f4cc1a5a79a 266 }
RichellBooyink 13:4f4cc1a5a79a 267 else if (P2 > 500) {
RichellBooyink 13:4f4cc1a5a79a 268 safety_stop = false;
RichellBooyink 13:4f4cc1a5a79a 269 pc.printf("Stop! /n/r");
RichellBooyink 13:4f4cc1a5a79a 270 motor2_direction = 1;
RichellBooyink 13:4f4cc1a5a79a 271 motor2_speed = 0.1;
RichellBooyink 13:4f4cc1a5a79a 272 wait(0.2);
RichellBooyink 13:4f4cc1a5a79a 273 safety_stop = true;
RichellBooyink 13:4f4cc1a5a79a 274 pc.printf("En door /n/r");
RichellBooyink 13:4f4cc1a5a79a 275 } else if (P2 < -500) {
RichellBooyink 13:4f4cc1a5a79a 276 safety_stop = false;
RichellBooyink 13:4f4cc1a5a79a 277 pc.printf("Stop! /n/r");
RichellBooyink 13:4f4cc1a5a79a 278 motor2_direction = 0;
RichellBooyink 13:4f4cc1a5a79a 279 motor2_speed = 0.1;
RichellBooyink 13:4f4cc1a5a79a 280 wait(0.2);
RichellBooyink 13:4f4cc1a5a79a 281 safety_stop = true;
RichellBooyink 13:4f4cc1a5a79a 282 pc.printf("En gaan /n/r");
arunr 10:34ccb2fed2ef 283 } else {
RichellBooyink 13:4f4cc1a5a79a 284 pc.printf("Not moving2 \n\r");
arunr 10:34ccb2fed2ef 285 motor2_speed = 0;
arunr 10:34ccb2fed2ef 286 }
RichellBooyink 13:4f4cc1a5a79a 287
arunr 10:34ccb2fed2ef 288 }
arunr 10:34ccb2fed2ef 289
arunr 10:34ccb2fed2ef 290 void movetohome()
arunr 10:34ccb2fed2ef 291 {
arunr 10:34ccb2fed2ef 292 P1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 293 P2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 294
arunr 10:34ccb2fed2ef 295 if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
arunr 0:65ab9f79a4cc 296 motor1_speed = 0;
arunr 10:34ccb2fed2ef 297 } else if (P1 > 24) {
riannebulthuis 3:5f59cbe53d7d 298 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 299 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 300 } else if (P1 < -24) {
riannebulthuis 3:5f59cbe53d7d 301 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 302 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 303 } else if (P2 > 24) {
arunr 10:34ccb2fed2ef 304 motor2_direction = 1;
arunr 10:34ccb2fed2ef 305 motor2_speed = 0.1;
arunr 10:34ccb2fed2ef 306 } else if (P2 < -24) {
arunr 10:34ccb2fed2ef 307 motor2_direction = 0;
arunr 10:34ccb2fed2ef 308 motor2_speed = 0.1;
arunr 0:65ab9f79a4cc 309 }
arunr 0:65ab9f79a4cc 310 }
arunr 0:65ab9f79a4cc 311
arunr 10:34ccb2fed2ef 312 void HIDScope ()
arunr 10:34ccb2fed2ef 313 {
arunr 10:34ccb2fed2ef 314 scope.set (0, final_filter1);
arunr 10:34ccb2fed2ef 315 scope.set (1, final_filter2);
arunr 10:34ccb2fed2ef 316 scope.set (2, final_filter3);
arunr 10:34ccb2fed2ef 317 scope.set (3, final_filter4);
arunr 10:34ccb2fed2ef 318 scope.set (4, motor1.getPosition());
arunr 10:34ccb2fed2ef 319 scope.set (5, motor2.getPosition());
arunr 6:1597888c9a56 320 scope.send ();
arunr 10:34ccb2fed2ef 321 }
arunr 10:34ccb2fed2ef 322
arunr 0:65ab9f79a4cc 323 int main()
arunr 10:34ccb2fed2ef 324 {
RichellBooyink 13:4f4cc1a5a79a 325 safety_stop = true;
arunr 0:65ab9f79a4cc 326 while (true) {
riannebulthuis 8:b219ca30967f 327 pc.baud(9600); //pc baud rate van de computer
arunr 10:34ccb2fed2ef 328 EMG_Filter.attach_us(Filters, 5e4); //filters uitvoeren
arunr 10:34ccb2fed2ef 329 Scope.attach_us(HIDScope, 5e4); //EMG en encoder signaal naar de hidscope sturen
arunr 10:34ccb2fed2ef 330
arunr 10:34ccb2fed2ef 331 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
arunr 10:34ccb2fed2ef 332
arunr 10:34ccb2fed2ef 333 case HOME: { //positie op 0 zetten voor arm 1
arunr 10:34ccb2fed2ef 334 pc.printf("home\n\r");
arunr 10:34ccb2fed2ef 335 H1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 336 H2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 337 pc.printf("Home-position is %f\n\r", H1);
arunr 10:34ccb2fed2ef 338 pc.printf("Home-pso is %f\n\r", H2);
RichellBooyink 12:146ba6f95fe0 339 state = MOVE_MOTORS;
arunr 10:34ccb2fed2ef 340 wait(2);
arunr 10:34ccb2fed2ef 341 break;
riannebulthuis 2:866a8a9f2b93 342 }
arunr 10:34ccb2fed2ef 343
RichellBooyink 12:146ba6f95fe0 344 case MOVE_MOTORS: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 345 pc.printf("move_motor\n\r");
RichellBooyink 12:146ba6f95fe0 346 while (state == MOVE_MOTORS) {
arunr 10:34ccb2fed2ef 347 move_motor1();
RichellBooyink 12:146ba6f95fe0 348 move_motor2();
arunr 10:34ccb2fed2ef 349 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 350 motor1_speed = 0;
arunr 10:34ccb2fed2ef 351 motor2_speed = 0;
arunr 10:34ccb2fed2ef 352 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 353 }
arunr 10:34ccb2fed2ef 354 }
arunr 10:34ccb2fed2ef 355 wait (1);
riannebulthuis 3:5f59cbe53d7d 356 break;
RichellBooyink 12:146ba6f95fe0 357 }
arunr 10:34ccb2fed2ef 358
arunr 10:34ccb2fed2ef 359
RichellBooyink 12:146ba6f95fe0 360 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
RichellBooyink 12:146ba6f95fe0 361 pc.printf("backhomeposition\n\r");
RichellBooyink 12:146ba6f95fe0 362 wait (1);
arunr 10:34ccb2fed2ef 363
RichellBooyink 12:146ba6f95fe0 364 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
RichellBooyink 12:146ba6f95fe0 365 while(state == BACKTOHOMEPOSITION) {
RichellBooyink 12:146ba6f95fe0 366 movetohome();
RichellBooyink 12:146ba6f95fe0 367 while(regelaar_ticker_flag != true);
RichellBooyink 12:146ba6f95fe0 368 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 369
RichellBooyink 12:146ba6f95fe0 370 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 371 pc.printf("referentie %f, %f \n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 372
RichellBooyink 12:146ba6f95fe0 373 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
RichellBooyink 12:146ba6f95fe0 374 pc.printf("motor1 pos %d", motor1.getPosition());
RichellBooyink 12:146ba6f95fe0 375 pc.printf("motor2 pos %d", motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 376 pc.printf("referentie %f %f\n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 377 state = STOP;
RichellBooyink 12:146ba6f95fe0 378 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 379 break;
arunr 10:34ccb2fed2ef 380 }
arunr 10:34ccb2fed2ef 381 }
RichellBooyink 12:146ba6f95fe0 382 }
RichellBooyink 12:146ba6f95fe0 383 case STOP: {
RichellBooyink 12:146ba6f95fe0 384 static int c;
RichellBooyink 12:146ba6f95fe0 385 while(state == STOP) {
RichellBooyink 12:146ba6f95fe0 386 motor1_speed = 0;
RichellBooyink 12:146ba6f95fe0 387 motor2_speed = 0;
RichellBooyink 12:146ba6f95fe0 388 if (c++ == 0) {
RichellBooyink 12:146ba6f95fe0 389 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 390 }
riannebulthuis 2:866a8a9f2b93 391 }
RichellBooyink 12:146ba6f95fe0 392 break;
riannebulthuis 3:5f59cbe53d7d 393 }
riannebulthuis 3:5f59cbe53d7d 394 }
arunr 10:34ccb2fed2ef 395 }
RichellBooyink 12:146ba6f95fe0 396 }