filters, homeposition and cases
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of Cases_homepos_picontrol_EMG_2 by
main.cpp@6:1597888c9a56, 2015-10-22 (annotated)
- Committer:
- arunr
- Date:
- Thu Oct 22 13:53:03 2015 +0000
- Revision:
- 6:1597888c9a56
- Parent:
- 5:b9d5d7311dac
- Child:
- 7:22126f285d69
Bewegen met emg mogelijk, homeposition draait weer door voordat hij terug gaat
Who changed what in which revision?
User | Revision | Line number | New 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 "QEI.h" |
riannebulthuis | 1:7d5e6bc2b314 | 5 | #include "MODSERIAL.h" |
arunr | 0:65ab9f79a4cc | 6 | |
arunr | 0:65ab9f79a4cc | 7 | DigitalOut motor1_direction(D4); |
arunr | 0:65ab9f79a4cc | 8 | PwmOut motor1_speed(D5); |
arunr | 0:65ab9f79a4cc | 9 | PwmOut led(D9); |
arunr | 0:65ab9f79a4cc | 10 | DigitalIn button_1(PTC6); //counterclockwise |
arunr | 0:65ab9f79a4cc | 11 | DigitalIn button_2(PTA4); //clockwise |
arunr | 5:b9d5d7311dac | 12 | AnalogIn PotMeter1(A5); |
arunr | 5:b9d5d7311dac | 13 | AnalogIn EMG(A0); |
arunr | 5:b9d5d7311dac | 14 | //AnalogIn EMG_bicepsright (A1); |
arunr | 5:b9d5d7311dac | 15 | //AnalogIn EMG_legleft (A2); |
arunr | 5:b9d5d7311dac | 16 | //AnalogIn EMG_legright (A3); |
arunr | 5:b9d5d7311dac | 17 | Ticker controller; |
arunr | 5:b9d5d7311dac | 18 | Ticker ticker_regelaar; |
arunr | 6:1597888c9a56 | 19 | Ticker EMG_Filter; |
arunr | 6:1597888c9a56 | 20 | Ticker Scope; |
arunr | 6:1597888c9a56 | 21 | //Ticker Encoder_Scope; |
arunr | 5:b9d5d7311dac | 22 | //Timer Timer_Calibration; |
arunr | 0:65ab9f79a4cc | 23 | Encoder motor1(D12,D13); |
arunr | 5:b9d5d7311dac | 24 | HIDScope scope(3); |
arunr | 0:65ab9f79a4cc | 25 | |
riannebulthuis | 2:866a8a9f2b93 | 26 | MODSERIAL pc(USBTX, USBRX); |
riannebulthuis | 2:866a8a9f2b93 | 27 | volatile bool regelaar_ticker_flag; |
riannebulthuis | 2:866a8a9f2b93 | 28 | |
riannebulthuis | 2:866a8a9f2b93 | 29 | void setregelaar_ticker_flag() |
riannebulthuis | 2:866a8a9f2b93 | 30 | { |
riannebulthuis | 2:866a8a9f2b93 | 31 | regelaar_ticker_flag = true; |
riannebulthuis | 2:866a8a9f2b93 | 32 | } |
riannebulthuis | 2:866a8a9f2b93 | 33 | |
riannebulthuis | 2:866a8a9f2b93 | 34 | #define SAMPLETIME_REGELAAR 0.005 |
riannebulthuis | 2:866a8a9f2b93 | 35 | |
riannebulthuis | 2:866a8a9f2b93 | 36 | //define states |
arunr | 6:1597888c9a56 | 37 | enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; |
arunr | 4:b4530fb376dd | 38 | uint8_t state = HOME; |
arunr | 0:65ab9f79a4cc | 39 | |
riannebulthuis | 3:5f59cbe53d7d | 40 | // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering |
riannebulthuis | 3:5f59cbe53d7d | 41 | const double g = 360; // Aantal graden 1 rotatie |
riannebulthuis | 3:5f59cbe53d7d | 42 | const double c = 4200; // Aantal counts 1 rotatie |
riannebulthuis | 3:5f59cbe53d7d | 43 | const double q = c/(g); |
riannebulthuis | 3:5f59cbe53d7d | 44 | |
riannebulthuis | 3:5f59cbe53d7d | 45 | //PI-controller constante |
riannebulthuis | 3:5f59cbe53d7d | 46 | const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1 |
riannebulthuis | 3:5f59cbe53d7d | 47 | const double motor1_Ki = 0.002; // Integrating gain m1. |
riannebulthuis | 3:5f59cbe53d7d | 48 | const double motor1_Ts = 0.01; // Time step m1 |
riannebulthuis | 3:5f59cbe53d7d | 49 | double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1 |
riannebulthuis | 3:5f59cbe53d7d | 50 | |
riannebulthuis | 3:5f59cbe53d7d | 51 | // Reusable P controller |
riannebulthuis | 3:5f59cbe53d7d | 52 | double Pc (double error, const double Kp) |
riannebulthuis | 3:5f59cbe53d7d | 53 | { |
riannebulthuis | 3:5f59cbe53d7d | 54 | return motor1_Kp * error; |
riannebulthuis | 3:5f59cbe53d7d | 55 | } |
riannebulthuis | 3:5f59cbe53d7d | 56 | |
riannebulthuis | 3:5f59cbe53d7d | 57 | // Measure the error and apply output to the plant |
riannebulthuis | 3:5f59cbe53d7d | 58 | void motor1_controlP() |
riannebulthuis | 3:5f59cbe53d7d | 59 | { |
riannebulthuis | 3:5f59cbe53d7d | 60 | double referenceP1 = PotMeter1.read(); |
riannebulthuis | 3:5f59cbe53d7d | 61 | double positionP1 = q*motor1.getPosition(); |
riannebulthuis | 3:5f59cbe53d7d | 62 | double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp); |
riannebulthuis | 3:5f59cbe53d7d | 63 | } |
riannebulthuis | 3:5f59cbe53d7d | 64 | |
riannebulthuis | 3:5f59cbe53d7d | 65 | // Reusable PI controller |
riannebulthuis | 3:5f59cbe53d7d | 66 | double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int) |
riannebulthuis | 3:5f59cbe53d7d | 67 | { |
riannebulthuis | 3:5f59cbe53d7d | 68 | err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken |
riannebulthuis | 3:5f59cbe53d7d | 69 | return motor1_Kp*error + motor1_Ki*err_int; |
riannebulthuis | 3:5f59cbe53d7d | 70 | } // De totale fout die wordt hersteld met behulp van PI control. |
riannebulthuis | 3:5f59cbe53d7d | 71 | |
arunr | 5:b9d5d7311dac | 72 | //bool Cali = false; |
arunr | 5:b9d5d7311dac | 73 | //double TimeCali = 5; |
arunr | 5:b9d5d7311dac | 74 | |
arunr | 5:b9d5d7311dac | 75 | // Filter1 = High pass filter tot 20 Hz |
arunr | 5:b9d5d7311dac | 76 | double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; |
arunr | 5:b9d5d7311dac | 77 | const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; |
arunr | 5:b9d5d7311dac | 78 | const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; |
arunr | 5:b9d5d7311dac | 79 | // Filter2 = Low pass filter na 60 Hz |
arunr | 5:b9d5d7311dac | 80 | double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; |
arunr | 5:b9d5d7311dac | 81 | const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; |
arunr | 5:b9d5d7311dac | 82 | const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; |
arunr | 5:b9d5d7311dac | 83 | // Filter3 = Notch filter at 50 Hz |
arunr | 5:b9d5d7311dac | 84 | double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; |
arunr | 5:b9d5d7311dac | 85 | const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; |
arunr | 5:b9d5d7311dac | 86 | const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1; |
arunr | 5:b9d5d7311dac | 87 | const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1; |
arunr | 5:b9d5d7311dac | 88 | |
arunr | 5:b9d5d7311dac | 89 | // Filter4 = Lowpass filter at 5 Hz |
arunr | 5:b9d5d7311dac | 90 | double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0; |
arunr | 5:b9d5d7311dac | 91 | const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0; |
arunr | 5:b9d5d7311dac | 92 | const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1; |
arunr | 5:b9d5d7311dac | 93 | |
arunr | 5:b9d5d7311dac | 94 | double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7, u8, u9; |
arunr | 5:b9d5d7311dac | 95 | double final_filter1; |
arunr | 5:b9d5d7311dac | 96 | |
arunr | 5:b9d5d7311dac | 97 | // Standaard formule voor het biquad filter |
arunr | 5:b9d5d7311dac | 98 | double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
arunr | 5:b9d5d7311dac | 99 | |
arunr | 5:b9d5d7311dac | 100 | { |
arunr | 5:b9d5d7311dac | 101 | double v = u - a1*v1 - a2*v2; |
arunr | 5:b9d5d7311dac | 102 | double y = b0*v + b1*v1 + b2*v2; |
arunr | 5:b9d5d7311dac | 103 | v2=v1; |
arunr | 5:b9d5d7311dac | 104 | v1=v; |
arunr | 5:b9d5d7311dac | 105 | return y; |
arunr | 5:b9d5d7311dac | 106 | } |
arunr | 5:b9d5d7311dac | 107 | |
arunr | 5:b9d5d7311dac | 108 | void Filters() |
arunr | 5:b9d5d7311dac | 109 | { |
arunr | 5:b9d5d7311dac | 110 | u1 = EMG.read(); |
arunr | 5:b9d5d7311dac | 111 | //Highpass |
arunr | 5:b9d5d7311dac | 112 | y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); |
arunr | 5:b9d5d7311dac | 113 | u2 = y1; |
arunr | 5:b9d5d7311dac | 114 | y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); |
arunr | 5:b9d5d7311dac | 115 | //Lowpass |
arunr | 5:b9d5d7311dac | 116 | u3 = y2; |
arunr | 5:b9d5d7311dac | 117 | y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); |
arunr | 5:b9d5d7311dac | 118 | u4 = y3; |
arunr | 5:b9d5d7311dac | 119 | y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); |
arunr | 5:b9d5d7311dac | 120 | // Notch |
arunr | 5:b9d5d7311dac | 121 | u5 = y4; |
arunr | 5:b9d5d7311dac | 122 | y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); |
arunr | 5:b9d5d7311dac | 123 | u6 = y5; |
arunr | 5:b9d5d7311dac | 124 | y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); |
arunr | 5:b9d5d7311dac | 125 | u7 = y6; |
arunr | 5:b9d5d7311dac | 126 | y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); |
arunr | 5:b9d5d7311dac | 127 | y8 = fabs (y7); |
arunr | 5:b9d5d7311dac | 128 | //smooth |
arunr | 5:b9d5d7311dac | 129 | u8 = y8; |
arunr | 5:b9d5d7311dac | 130 | y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); |
arunr | 5:b9d5d7311dac | 131 | u9 = y9; |
arunr | 5:b9d5d7311dac | 132 | final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); |
arunr | 6:1597888c9a56 | 133 | } |
arunr | 5:b9d5d7311dac | 134 | |
arunr | 5:b9d5d7311dac | 135 | |
riannebulthuis | 3:5f59cbe53d7d | 136 | void motor1_controlPI() |
riannebulthuis | 3:5f59cbe53d7d | 137 | { |
riannebulthuis | 3:5f59cbe53d7d | 138 | double referencePI1 = PotMeter1.read(); |
riannebulthuis | 3:5f59cbe53d7d | 139 | double positionPI1 = q*motor1.getPosition(); |
riannebulthuis | 3:5f59cbe53d7d | 140 | double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1); |
riannebulthuis | 3:5f59cbe53d7d | 141 | } |
riannebulthuis | 3:5f59cbe53d7d | 142 | |
arunr | 0:65ab9f79a4cc | 143 | const int pressed = 0; |
arunr | 0:65ab9f79a4cc | 144 | |
arunr | 0:65ab9f79a4cc | 145 | double H; |
arunr | 0:65ab9f79a4cc | 146 | double P; |
arunr | 0:65ab9f79a4cc | 147 | double D; |
arunr | 0:65ab9f79a4cc | 148 | |
arunr | 0:65ab9f79a4cc | 149 | |
arunr | 4:b4530fb376dd | 150 | void gethome(){ |
arunr | 0:65ab9f79a4cc | 151 | H = motor1.getPosition(); |
arunr | 0:65ab9f79a4cc | 152 | } |
arunr | 0:65ab9f79a4cc | 153 | |
arunr | 0:65ab9f79a4cc | 154 | void move_motor1_ccw (){ |
arunr | 0:65ab9f79a4cc | 155 | motor1_direction = 0; |
arunr | 6:1597888c9a56 | 156 | motor1_speed = 0.4; |
arunr | 0:65ab9f79a4cc | 157 | } |
arunr | 0:65ab9f79a4cc | 158 | |
arunr | 0:65ab9f79a4cc | 159 | void move_motor1_cw (){ |
arunr | 0:65ab9f79a4cc | 160 | motor1_direction = 1; |
arunr | 6:1597888c9a56 | 161 | motor1_speed = 0.4; |
arunr | 0:65ab9f79a4cc | 162 | } |
arunr | 0:65ab9f79a4cc | 163 | |
arunr | 0:65ab9f79a4cc | 164 | void movetohome(){ |
arunr | 0:65ab9f79a4cc | 165 | P = motor1.getPosition(); |
arunr | 4:b4530fb376dd | 166 | |
arunr | 4:b4530fb376dd | 167 | if (P >= -28 && P <= 28){ |
arunr | 0:65ab9f79a4cc | 168 | motor1_speed = 0; |
arunr | 0:65ab9f79a4cc | 169 | } |
arunr | 4:b4530fb376dd | 170 | else if (P > 24){ |
riannebulthuis | 3:5f59cbe53d7d | 171 | motor1_direction = 1; |
riannebulthuis | 3:5f59cbe53d7d | 172 | motor1_speed = 0.1; |
arunr | 0:65ab9f79a4cc | 173 | } |
arunr | 4:b4530fb376dd | 174 | else if (P < -24){ |
riannebulthuis | 3:5f59cbe53d7d | 175 | motor1_direction = 0; |
riannebulthuis | 3:5f59cbe53d7d | 176 | motor1_speed = 0.1; |
arunr | 0:65ab9f79a4cc | 177 | } |
arunr | 0:65ab9f79a4cc | 178 | } |
arunr | 0:65ab9f79a4cc | 179 | |
arunr | 0:65ab9f79a4cc | 180 | void move_motor1() |
arunr | 0:65ab9f79a4cc | 181 | { |
arunr | 5:b9d5d7311dac | 182 | if (final_filter1 > 0.03){ |
arunr | 5:b9d5d7311dac | 183 | pc.printf("Moving clockwise \n\r"); |
arunr | 5:b9d5d7311dac | 184 | move_motor1_cw (); |
arunr | 5:b9d5d7311dac | 185 | } |
arunr | 5:b9d5d7311dac | 186 | else if (button_2 == pressed){ |
arunr | 5:b9d5d7311dac | 187 | pc.printf("Moving counterclockwise \n\r"); |
arunr | 5:b9d5d7311dac | 188 | move_motor1_ccw (); |
arunr | 5:b9d5d7311dac | 189 | } |
arunr | 5:b9d5d7311dac | 190 | else { |
arunr | 5:b9d5d7311dac | 191 | pc.printf("Not moving \n\r"); |
arunr | 5:b9d5d7311dac | 192 | motor1_speed = 0; |
arunr | 5:b9d5d7311dac | 193 | } |
arunr | 0:65ab9f79a4cc | 194 | } |
arunr | 0:65ab9f79a4cc | 195 | |
arunr | 5:b9d5d7311dac | 196 | //void read_encoder1 () // aflezen van encoder via hidscope?? |
arunr | 5:b9d5d7311dac | 197 | //{ |
arunr | 5:b9d5d7311dac | 198 | // scope.set(0,motor1.getPosition()); |
arunr | 5:b9d5d7311dac | 199 | //led.write(motor1.getPosition()/100.0); |
arunr | 5:b9d5d7311dac | 200 | // scope.send(); |
arunr | 5:b9d5d7311dac | 201 | // wait(0.2f); |
arunr | 5:b9d5d7311dac | 202 | //} |
arunr | 0:65ab9f79a4cc | 203 | |
arunr | 6:1597888c9a56 | 204 | void HIDScope (){ |
arunr | 6:1597888c9a56 | 205 | scope.set (0, y8); |
arunr | 6:1597888c9a56 | 206 | // scope.set (1, y2); |
arunr | 6:1597888c9a56 | 207 | // scope.set (2, y4); |
arunr | 6:1597888c9a56 | 208 | // scope.set (3, y7); |
arunr | 6:1597888c9a56 | 209 | scope.set (1, final_filter1); |
arunr | 6:1597888c9a56 | 210 | //scope.set (2, final_filter1); |
arunr | 6:1597888c9a56 | 211 | scope.set(2, motor1.getPosition()); |
arunr | 6:1597888c9a56 | 212 | scope.send (); |
arunr | 4:b4530fb376dd | 213 | } |
arunr | 6:1597888c9a56 | 214 | |
arunr | 6:1597888c9a56 | 215 | |
arunr | 6:1597888c9a56 | 216 | |
arunr | 0:65ab9f79a4cc | 217 | int main() |
riannebulthuis | 2:866a8a9f2b93 | 218 | { |
arunr | 0:65ab9f79a4cc | 219 | while (true) { |
riannebulthuis | 1:7d5e6bc2b314 | 220 | pc.baud(9600); //pc baud rate van de computer |
arunr | 6:1597888c9a56 | 221 | EMG_Filter.attach_us(Filters, 1e3); |
arunr | 6:1597888c9a56 | 222 | Scope.attach_us(HIDScope, 1e3); |
riannebulthuis | 1:7d5e6bc2b314 | 223 | |
riannebulthuis | 1:7d5e6bc2b314 | 224 | switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases |
riannebulthuis | 1:7d5e6bc2b314 | 225 | |
arunr | 4:b4530fb376dd | 226 | case HOME: //positie op 0 zetten voor arm 1 |
riannebulthuis | 1:7d5e6bc2b314 | 227 | { |
arunr | 4:b4530fb376dd | 228 | pc.printf("home\n\r"); |
arunr | 5:b9d5d7311dac | 229 | //read_encoder1(); |
arunr | 4:b4530fb376dd | 230 | gethome(); |
arunr | 4:b4530fb376dd | 231 | pc.printf("Home-position is %f\n\r", H); |
arunr | 6:1597888c9a56 | 232 | state = MOVE_MOTOR; |
arunr | 4:b4530fb376dd | 233 | wait(5); |
riannebulthuis | 1:7d5e6bc2b314 | 234 | break; |
riannebulthuis | 1:7d5e6bc2b314 | 235 | } |
riannebulthuis | 1:7d5e6bc2b314 | 236 | |
arunr | 5:b9d5d7311dac | 237 | //case CALIBRATIE: |
arunr | 5:b9d5d7311dac | 238 | //{ |
arunr | 5:b9d5d7311dac | 239 | //pc.printf("Aanspannen in 10 \n\r"); |
arunr | 5:b9d5d7311dac | 240 | //wait(10); |
arunr | 5:b9d5d7311dac | 241 | //EMG_Control.attach_us(MyController,1e3); |
arunr | 5:b9d5d7311dac | 242 | //Timer_Calibration.start(); |
arunr | 5:b9d5d7311dac | 243 | //if (Timer_Calibration < TimeCali) { |
arunr | 5:b9d5d7311dac | 244 | // Cali = true; |
arunr | 5:b9d5d7311dac | 245 | // pc.printf("Aanspannen \n\r"); |
arunr | 5:b9d5d7311dac | 246 | //} |
arunr | 5:b9d5d7311dac | 247 | //else { |
arunr | 5:b9d5d7311dac | 248 | // Cali = false; |
arunr | 5:b9d5d7311dac | 249 | //} |
arunr | 5:b9d5d7311dac | 250 | //pc.printf("Stoppen \n\r"); |
arunr | 5:b9d5d7311dac | 251 | //Timer_Calibration.stop(); |
arunr | 5:b9d5d7311dac | 252 | //Timer_Calibration.reset(); |
arunr | 5:b9d5d7311dac | 253 | //state = MOVE_MOTOR; |
arunr | 5:b9d5d7311dac | 254 | // break; |
arunr | 5:b9d5d7311dac | 255 | //} |
arunr | 5:b9d5d7311dac | 256 | |
riannebulthuis | 2:866a8a9f2b93 | 257 | case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons |
riannebulthuis | 1:7d5e6bc2b314 | 258 | { |
riannebulthuis | 2:866a8a9f2b93 | 259 | pc.printf("move_motor\n\r"); |
riannebulthuis | 2:866a8a9f2b93 | 260 | while (state == MOVE_MOTOR){ |
riannebulthuis | 2:866a8a9f2b93 | 261 | move_motor1(); |
arunr | 6:1597888c9a56 | 262 | //print_position(); |
riannebulthuis | 2:866a8a9f2b93 | 263 | if (button_1 == pressed && button_2 == pressed){ |
riannebulthuis | 2:866a8a9f2b93 | 264 | state = BACKTOHOMEPOSITION; |
riannebulthuis | 2:866a8a9f2b93 | 265 | } |
riannebulthuis | 2:866a8a9f2b93 | 266 | } |
riannebulthuis | 2:866a8a9f2b93 | 267 | wait (1); |
riannebulthuis | 2:866a8a9f2b93 | 268 | break; |
riannebulthuis | 1:7d5e6bc2b314 | 269 | } |
riannebulthuis | 1:7d5e6bc2b314 | 270 | |
riannebulthuis | 2:866a8a9f2b93 | 271 | case BACKTOHOMEPOSITION: //motor gaat terug naar homeposition |
riannebulthuis | 1:7d5e6bc2b314 | 272 | { |
riannebulthuis | 2:866a8a9f2b93 | 273 | pc.printf("backhomeposition\n\r"); |
riannebulthuis | 2:866a8a9f2b93 | 274 | wait (1); |
riannebulthuis | 2:866a8a9f2b93 | 275 | |
riannebulthuis | 2:866a8a9f2b93 | 276 | ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); |
arunr | 5:b9d5d7311dac | 277 | //EMG_Control.attach_us(Filters,1e3); |
arunr | 5:b9d5d7311dac | 278 | |
riannebulthuis | 2:866a8a9f2b93 | 279 | while(state == BACKTOHOMEPOSITION){ |
riannebulthuis | 3:5f59cbe53d7d | 280 | movetohome(); |
riannebulthuis | 2:866a8a9f2b93 | 281 | while(regelaar_ticker_flag != true); |
riannebulthuis | 2:866a8a9f2b93 | 282 | regelaar_ticker_flag = false; |
riannebulthuis | 2:866a8a9f2b93 | 283 | |
riannebulthuis | 2:866a8a9f2b93 | 284 | pc.printf("pulsmotorposition1 %d", motor1.getPosition()); |
arunr | 4:b4530fb376dd | 285 | pc.printf("referentie %f\n\r", H); |
riannebulthuis | 2:866a8a9f2b93 | 286 | |
arunr | 4:b4530fb376dd | 287 | if (P <=24 && P >= -24){ |
arunr | 4:b4530fb376dd | 288 | pc.printf("pulsmotorposition1 %d", motor1.getPosition()); |
arunr | 4:b4530fb376dd | 289 | pc.printf("referentie %f\n\r", H); |
riannebulthuis | 3:5f59cbe53d7d | 290 | state = STOP; |
arunr | 4:b4530fb376dd | 291 | pc.printf("Laatste positie %f\n\r", motor1.getPosition()); |
riannebulthuis | 3:5f59cbe53d7d | 292 | break; |
riannebulthuis | 2:866a8a9f2b93 | 293 | } |
riannebulthuis | 3:5f59cbe53d7d | 294 | } |
riannebulthuis | 3:5f59cbe53d7d | 295 | } |
riannebulthuis | 3:5f59cbe53d7d | 296 | case STOP: |
riannebulthuis | 3:5f59cbe53d7d | 297 | { |
arunr | 4:b4530fb376dd | 298 | static int c; |
riannebulthuis | 3:5f59cbe53d7d | 299 | while(state == STOP){ |
riannebulthuis | 3:5f59cbe53d7d | 300 | motor1_speed = 0; |
arunr | 4:b4530fb376dd | 301 | if (c++ == 0){ |
riannebulthuis | 3:5f59cbe53d7d | 302 | pc.printf("einde\n\r"); |
riannebulthuis | 3:5f59cbe53d7d | 303 | } |
riannebulthuis | 3:5f59cbe53d7d | 304 | } |
riannebulthuis | 1:7d5e6bc2b314 | 305 | break; |
riannebulthuis | 3:5f59cbe53d7d | 306 | } |
riannebulthuis | 3:5f59cbe53d7d | 307 | } |
riannebulthuis | 2:866a8a9f2b93 | 308 | } |
arunr | 0:65ab9f79a4cc | 309 | } |