Arun Raveenthiran / Mbed 2 deprecated Cases_homepos_picontrol_EMG_2

bewegen met emg en terug naar home

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Cases_homepos_picontrol_EMG_2 by Rianne Bulthuis

main.cpp@10:34ccb2fed2ef, 2015-10-27 (annotated)

Committer:
arunr
Date:
Tue Oct 27 10:23:31 2015 +0000
Revision:
10:34ccb2fed2ef
Parent:
9:888ed3c72795 
cases bewegen met emg;

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);
arunr 10:34ccb2fed2ef 18 AnalogIn EMG_tricepsright(A2);
arunr 10:34ccb2fed2ef 19 AnalogIn EMG_tricepsleft(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 {
arunr 10:34ccb2fed2ef 156 A = EMG_bicepsright.read();
arunr 5:b9d5d7311dac 157 //Highpass
arunr 10:34ccb2fed2ef 158 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 159 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 160 //Lowpass
arunr 10:34ccb2fed2ef 161 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 162 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 163 // Notch
arunr 10:34ccb2fed2ef 164 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 165 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 166 y7 = biquad (y6, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 167 // Rectify sample
arunr 10:34ccb2fed2ef 168 y8 = fabs(y7);
arunr 10:34ccb2fed2ef 169 // Make it smooth
arunr 10:34ccb2fed2ef 170 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 171 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 172
arunr 10:34ccb2fed2ef 173 //Biceps right
arunr 10:34ccb2fed2ef 174 B = EMG_bicepsleft.read();
arunr 10:34ccb2fed2ef 175 //Highpass
arunr 10:34ccb2fed2ef 176 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 177 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 178 //Lowpass
arunr 10:34ccb2fed2ef 179 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 180 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 181 // Notch
arunr 10:34ccb2fed2ef 182 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 183 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 184 y16 = biquad (y15, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 185 // Rectify sample
arunr 10:34ccb2fed2ef 186 y17 = fabs(y16);
arunr 10:34ccb2fed2ef 187 // Make it smooth
arunr 10:34ccb2fed2ef 188 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 189 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 190
arunr 10:34ccb2fed2ef 191 /// EMG Filter left leg
arunr 10:34ccb2fed2ef 192 C = EMG_tricepsright.read();
arunr 10:34ccb2fed2ef 193 //Highpass
arunr 10:34ccb2fed2ef 194 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 195 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 196 //Lowpass
arunr 10:34ccb2fed2ef 197 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 198 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 199 // Notch
arunr 10:34ccb2fed2ef 200 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 201 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 202 y25 = biquad (y24, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 203 // Rectify sample
arunr 10:34ccb2fed2ef 204 y26 = fabs(y25);
arunr 10:34ccb2fed2ef 205 // Make it smooth
arunr 10:34ccb2fed2ef 206 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 207 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 208
arunr 10:34ccb2fed2ef 209 // EMG filter right leg
arunr 10:34ccb2fed2ef 210 D = EMG_tricepsleft.read();
arunr 10:34ccb2fed2ef 211 //Highpass
arunr 10:34ccb2fed2ef 212 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 213 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 214 //Lowpass
arunr 10:34ccb2fed2ef 215 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 216 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 217 // Notch
arunr 10:34ccb2fed2ef 218 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 219 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 220 y34 = biquad (y33, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 221 // Rectify sample
arunr 10:34ccb2fed2ef 222 y35 = fabs(y34);
arunr 10:34ccb2fed2ef 223 // Make it smooth
arunr 10:34ccb2fed2ef 224 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 225 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 226 }
arunr 5:b9d5d7311dac 227
riannebulthuis 3:5f59cbe53d7d 228
arunr 0:65ab9f79a4cc 229 const int pressed = 0;
arunr 0:65ab9f79a4cc 230
riannebulthuis 8:b219ca30967f 231 // constantes voor berekening homepositie
arunr 10:34ccb2fed2ef 232 double H1;
arunr 10:34ccb2fed2ef 233 double H2;
arunr 10:34ccb2fed2ef 234 double P1;
arunr 10:34ccb2fed2ef 235 double P2;
arunr 0:65ab9f79a4cc 236
riannebulthuis 8:b219ca30967f 237 void move_motor1()
riannebulthuis 8:b219ca30967f 238 {
arunr 10:34ccb2fed2ef 239 if (final_filter1 > 0.06 || button_1 == pressed) {
arunr 10:34ccb2fed2ef 240 pc.printf("motor1 cw \n\r");
arunr 10:34ccb2fed2ef 241 motor1_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 242 motor1_speed = 0.2;
arunr 10:34ccb2fed2ef 243 } else if (final_filter3 > 0.07 || button_2 == pressed){
arunr 10:34ccb2fed2ef 244 pc.printf("motor1 ccw \n\r");
arunr 10:34ccb2fed2ef 245 motor1_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 246 motor1_speed = 0.2;
arunr 10:34ccb2fed2ef 247 } else {
arunr 10:34ccb2fed2ef 248 pc.printf("Not moving \n\r");
arunr 10:34ccb2fed2ef 249 motor1_speed = 0;
arunr 10:34ccb2fed2ef 250 }
arunr 0:65ab9f79a4cc 251 }
arunr 0:65ab9f79a4cc 252
arunr 10:34ccb2fed2ef 253 void move_motor2()
arunr 10:34ccb2fed2ef 254 {
arunr 10:34ccb2fed2ef 255 if (button_1 == pressed) {
arunr 10:34ccb2fed2ef 256 pc.printf("motor2 cw \n\r");
arunr 10:34ccb2fed2ef 257 motor2_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 258 motor2_speed = 0.4;
arunr 10:34ccb2fed2ef 259 } else if (button_2 == pressed) {
arunr 10:34ccb2fed2ef 260 pc.printf("Moving 2 counterclockwise \n\r");
arunr 10:34ccb2fed2ef 261 motor2_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 262 motor2_speed = 0.4;
arunr 10:34ccb2fed2ef 263 } else {
arunr 10:34ccb2fed2ef 264 pc.printf("Not moving \n\r");
arunr 10:34ccb2fed2ef 265 motor2_speed = 0;
arunr 10:34ccb2fed2ef 266 }
arunr 10:34ccb2fed2ef 267 }
arunr 10:34ccb2fed2ef 268
arunr 10:34ccb2fed2ef 269 void movetohome()
arunr 10:34ccb2fed2ef 270 {
arunr 10:34ccb2fed2ef 271 P1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 272 P2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 273
arunr 10:34ccb2fed2ef 274 if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
arunr 0:65ab9f79a4cc 275 motor1_speed = 0;
arunr 10:34ccb2fed2ef 276 } else if (P1 > 24) {
riannebulthuis 3:5f59cbe53d7d 277 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 278 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 279 } else if (P1 < -24) {
riannebulthuis 3:5f59cbe53d7d 280 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 281 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 282 } else if (P2 > 24) {
arunr 10:34ccb2fed2ef 283 motor2_direction = 1;
arunr 10:34ccb2fed2ef 284 motor2_speed = 0.1;
arunr 10:34ccb2fed2ef 285 } else if (P2 < -24) {
arunr 10:34ccb2fed2ef 286 motor2_direction = 0;
arunr 10:34ccb2fed2ef 287 motor2_speed = 0.1;
arunr 0:65ab9f79a4cc 288 }
arunr 0:65ab9f79a4cc 289 }
arunr 0:65ab9f79a4cc 290
arunr 10:34ccb2fed2ef 291 void HIDScope ()
arunr 10:34ccb2fed2ef 292 {
arunr 10:34ccb2fed2ef 293 scope.set (0, final_filter1);
arunr 10:34ccb2fed2ef 294 scope.set (1, final_filter2);
arunr 10:34ccb2fed2ef 295 scope.set (2, final_filter3);
arunr 10:34ccb2fed2ef 296 scope.set (3, final_filter4);
arunr 10:34ccb2fed2ef 297 scope.set (4, motor1.getPosition());
arunr 10:34ccb2fed2ef 298 scope.set (5, motor2.getPosition());
arunr 6:1597888c9a56 299 scope.send ();
arunr 10:34ccb2fed2ef 300 }
arunr 10:34ccb2fed2ef 301
arunr 0:65ab9f79a4cc 302 int main()
arunr 10:34ccb2fed2ef 303 {
arunr 0:65ab9f79a4cc 304 while (true) {
riannebulthuis 8:b219ca30967f 305 pc.baud(9600); //pc baud rate van de computer
arunr 10:34ccb2fed2ef 306 EMG_Filter.attach_us(Filters, 5e4); //filters uitvoeren
arunr 10:34ccb2fed2ef 307 Scope.attach_us(HIDScope, 5e4); //EMG en encoder signaal naar de hidscope sturen
arunr 10:34ccb2fed2ef 308
arunr 10:34ccb2fed2ef 309 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
arunr 10:34ccb2fed2ef 310
arunr 10:34ccb2fed2ef 311 case HOME: { //positie op 0 zetten voor arm 1
arunr 10:34ccb2fed2ef 312 pc.printf("home\n\r");
arunr 10:34ccb2fed2ef 313 H1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 314 H2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 315 pc.printf("Home-position is %f\n\r", H1);
arunr 10:34ccb2fed2ef 316 pc.printf("Home-pso is %f\n\r", H2);
arunr 10:34ccb2fed2ef 317 state = MOVE_MOTOR_1;
arunr 10:34ccb2fed2ef 318 wait(2);
arunr 10:34ccb2fed2ef 319 break;
riannebulthuis 2:866a8a9f2b93 320 }
arunr 10:34ccb2fed2ef 321
arunr 10:34ccb2fed2ef 322 case MOVE_MOTOR_1: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 323 pc.printf("move_motor\n\r");
arunr 10:34ccb2fed2ef 324 while (state == MOVE_MOTOR_1) {
arunr 10:34ccb2fed2ef 325 move_motor1();
arunr 10:34ccb2fed2ef 326 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 327 motor1_speed = 0;
arunr 10:34ccb2fed2ef 328 state = MOVE_MOTOR_2;
arunr 10:34ccb2fed2ef 329 }
arunr 10:34ccb2fed2ef 330 }
arunr 10:34ccb2fed2ef 331 wait (1);
arunr 10:34ccb2fed2ef 332 break;
riannebulthuis 2:866a8a9f2b93 333 }
arunr 10:34ccb2fed2ef 334
arunr 10:34ccb2fed2ef 335 case MOVE_MOTOR_2: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 336 pc.printf("move_motor\n\r");
arunr 10:34ccb2fed2ef 337 while (state == MOVE_MOTOR_2) {
arunr 10:34ccb2fed2ef 338 move_motor2();
arunr 10:34ccb2fed2ef 339 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 340 motor2_speed = 0;
arunr 10:34ccb2fed2ef 341 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 342 }
arunr 10:34ccb2fed2ef 343 }
arunr 10:34ccb2fed2ef 344 wait (1);
riannebulthuis 3:5f59cbe53d7d 345 break;
arunr 10:34ccb2fed2ef 346
arunr 10:34ccb2fed2ef 347 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
arunr 10:34ccb2fed2ef 348 pc.printf("backhomeposition\n\r");
arunr 10:34ccb2fed2ef 349 wait (1);
arunr 10:34ccb2fed2ef 350
arunr 10:34ccb2fed2ef 351 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
arunr 10:34ccb2fed2ef 352 while(state == BACKTOHOMEPOSITION) {
arunr 10:34ccb2fed2ef 353 movetohome();
arunr 10:34ccb2fed2ef 354 while(regelaar_ticker_flag != true);
arunr 10:34ccb2fed2ef 355 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 356
arunr 10:34ccb2fed2ef 357 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
arunr 10:34ccb2fed2ef 358 pc.printf("referentie %f, %f \n\r", H1, H2);
arunr 10:34ccb2fed2ef 359
arunr 10:34ccb2fed2ef 360 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
arunr 10:34ccb2fed2ef 361 pc.printf("motor1 pos %d", motor1.getPosition());
arunr 10:34ccb2fed2ef 362 pc.printf("motor2 pos %d", motor2.getPosition());
arunr 10:34ccb2fed2ef 363 pc.printf("referentie %f %f\n\r", H1, H2);
arunr 10:34ccb2fed2ef 364 state = STOP;
arunr 10:34ccb2fed2ef 365 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
arunr 10:34ccb2fed2ef 366 break;
arunr 10:34ccb2fed2ef 367 }
arunr 10:34ccb2fed2ef 368 }
arunr 10:34ccb2fed2ef 369 }
arunr 10:34ccb2fed2ef 370 case STOP: {
arunr 10:34ccb2fed2ef 371 static int c;
arunr 10:34ccb2fed2ef 372 while(state == STOP) {
arunr 10:34ccb2fed2ef 373 motor1_speed = 0;
arunr 10:34ccb2fed2ef 374 motor2_speed = 0;
arunr 10:34ccb2fed2ef 375 if (c++ == 0) {
arunr 10:34ccb2fed2ef 376 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 377 }
arunr 10:34ccb2fed2ef 378 }
arunr 10:34ccb2fed2ef 379 break;
riannebulthuis 2:866a8a9f2b93 380 }
riannebulthuis 3:5f59cbe53d7d 381 }
riannebulthuis 3:5f59cbe53d7d 382 }
arunr 10:34ccb2fed2ef 383 }
arunr 0:65ab9f79a4cc 384 }