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@12:146ba6f95fe0, 2015-10-27 (annotated)

Committer:
RichellBooyink
Date:
Tue Oct 27 11:56:57 2015 +0000
Revision:
12:146ba6f95fe0
Parent:
11:b2dec8f3e75c 
Changed the cases: 2 cases for 2 motors => 1 case for both motors

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;
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 12:146ba6f95fe0 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);
RichellBooyink 12:146ba6f95fe0 318 state = MOVE_MOTORS;
arunr 10:34ccb2fed2ef 319 wait(2);
arunr 10:34ccb2fed2ef 320 break;
riannebulthuis 2:866a8a9f2b93 321 }
arunr 10:34ccb2fed2ef 322
RichellBooyink 12:146ba6f95fe0 323 case MOVE_MOTORS: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 324 pc.printf("move_motor\n\r");
RichellBooyink 12:146ba6f95fe0 325 while (state == MOVE_MOTORS) {
arunr 10:34ccb2fed2ef 326 move_motor1();
RichellBooyink 12:146ba6f95fe0 327 move_motor2();
arunr 10:34ccb2fed2ef 328 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 329 motor1_speed = 0;
arunr 10:34ccb2fed2ef 330 motor2_speed = 0;
arunr 10:34ccb2fed2ef 331 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 332 }
arunr 10:34ccb2fed2ef 333 }
arunr 10:34ccb2fed2ef 334 wait (1);
riannebulthuis 3:5f59cbe53d7d 335 break;
RichellBooyink 12:146ba6f95fe0 336 }
arunr 10:34ccb2fed2ef 337
arunr 10:34ccb2fed2ef 338
RichellBooyink 12:146ba6f95fe0 339 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
RichellBooyink 12:146ba6f95fe0 340 pc.printf("backhomeposition\n\r");
RichellBooyink 12:146ba6f95fe0 341 wait (1);
arunr 10:34ccb2fed2ef 342
RichellBooyink 12:146ba6f95fe0 343 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
RichellBooyink 12:146ba6f95fe0 344 while(state == BACKTOHOMEPOSITION) {
RichellBooyink 12:146ba6f95fe0 345 movetohome();
RichellBooyink 12:146ba6f95fe0 346 while(regelaar_ticker_flag != true);
RichellBooyink 12:146ba6f95fe0 347 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 348
RichellBooyink 12:146ba6f95fe0 349 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 350 pc.printf("referentie %f, %f \n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 351
RichellBooyink 12:146ba6f95fe0 352 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
RichellBooyink 12:146ba6f95fe0 353 pc.printf("motor1 pos %d", motor1.getPosition());
RichellBooyink 12:146ba6f95fe0 354 pc.printf("motor2 pos %d", motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 355 pc.printf("referentie %f %f\n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 356 state = STOP;
RichellBooyink 12:146ba6f95fe0 357 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 358 break;
arunr 10:34ccb2fed2ef 359 }
arunr 10:34ccb2fed2ef 360 }
RichellBooyink 12:146ba6f95fe0 361 }
RichellBooyink 12:146ba6f95fe0 362 case STOP: {
RichellBooyink 12:146ba6f95fe0 363 static int c;
RichellBooyink 12:146ba6f95fe0 364 while(state == STOP) {
RichellBooyink 12:146ba6f95fe0 365 motor1_speed = 0;
RichellBooyink 12:146ba6f95fe0 366 motor2_speed = 0;
RichellBooyink 12:146ba6f95fe0 367 if (c++ == 0) {
RichellBooyink 12:146ba6f95fe0 368 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 369 }
riannebulthuis 2:866a8a9f2b93 370 }
RichellBooyink 12:146ba6f95fe0 371 break;
riannebulthuis 3:5f59cbe53d7d 372 }
riannebulthuis 3:5f59cbe53d7d 373 }
arunr 10:34ccb2fed2ef 374 }
RichellBooyink 12:146ba6f95fe0 375 }