Rianne Bulthuis / Mbed 2 deprecated Cases_homepos_picontrol_EMG_2

EMG

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of Cases_homepos_picontrol_EMG by Arun Raveenthiran

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?

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 "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 }