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