Astrid Schut / Mbed 2 deprecated StateMachineMyoArm

Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@2:7c9974f0947a, 2019-03-27 (annotated)

Committer:
aschut
Date:
Wed Mar 27 17:33:56 2019 +0000
Revision:
2:7c9974f0947a
Parent:
1:070092564648
Child:
3:2aaf54ce090b
State machine met motor off, homing en movement, geen EMG

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:90750f158475 1 //Voor het toevoegen van een button:
aschut 0:90750f158475 2 #include "mbed.h"
aschut 2:7c9974f0947a 3 #include "MODSERIAL.h"
aschut 2:7c9974f0947a 4 #include "QEI.h"
aschut 2:7c9974f0947a 5 #include "BiQuad.h"
aschut 0:90750f158475 6
aschut 2:7c9974f0947a 7 // Algemeen
aschut 2:7c9974f0947a 8 DigitalIn button3(SW3);
aschut 2:7c9974f0947a 9 DigitalIn button2(SW2);
aschut 2:7c9974f0947a 10 AnalogIn But2(A5);
aschut 2:7c9974f0947a 11 AnalogIn But1(A3);
aschut 0:90750f158475 12
aschut 0:90750f158475 13 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 14 DigitalOut led2(LED_RED);
aschut 0:90750f158475 15 DigitalOut led3(LED_BLUE);
aschut 0:90750f158475 16
aschut 2:7c9974f0947a 17 MODSERIAL pc(USBTX, USBRX);
aschut 0:90750f158475 18 Timer t;
aschut 2:7c9974f0947a 19 Timer t2;
aschut 2:7c9974f0947a 20 //Motoren
aschut 2:7c9974f0947a 21 DigitalOut direction1(D4);
aschut 2:7c9974f0947a 22 PwmOut pwmpin1(D5);
aschut 2:7c9974f0947a 23 PwmOut pwmpin2(D6);
aschut 2:7c9974f0947a 24 DigitalOut direction2(D7);
aschut 2:7c9974f0947a 25 volatile float pwm1;
aschut 2:7c9974f0947a 26 volatile float pwm2;
aschut 2:7c9974f0947a 27
aschut 2:7c9974f0947a 28 //Encoder
aschut 2:7c9974f0947a 29 QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
aschut 2:7c9974f0947a 30 QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING);
aschut 2:7c9974f0947a 31 double Pulses1;
aschut 2:7c9974f0947a 32 double motor_position1;
aschut 2:7c9974f0947a 33 double Pulses2;
aschut 2:7c9974f0947a 34 double motor_position2;
aschut 2:7c9974f0947a 35 double error1;
aschut 2:7c9974f0947a 36
aschut 2:7c9974f0947a 37 //Pot meter
aschut 2:7c9974f0947a 38 AnalogIn pot(A1);
aschut 2:7c9974f0947a 39 AnalogIn pot0(A0);
aschut 2:7c9974f0947a 40 float Pot2;
aschut 2:7c9974f0947a 41 float Pot1;
aschut 2:7c9974f0947a 42
aschut 2:7c9974f0947a 43 //Ticker
aschut 2:7c9974f0947a 44 Ticker Pwm;
aschut 2:7c9974f0947a 45 Ticker PotRead;
aschut 2:7c9974f0947a 46 Ticker Kdc;
aschut 2:7c9974f0947a 47
aschut 2:7c9974f0947a 48 //Kinematica
aschut 2:7c9974f0947a 49 double stap1;
aschut 2:7c9974f0947a 50 double stap2;
aschut 2:7c9974f0947a 51 double KPot;
aschut 2:7c9974f0947a 52
aschut 2:7c9974f0947a 53 float ElbowReference;
aschut 2:7c9974f0947a 54 float Ellebooghoek1;
aschut 2:7c9974f0947a 55 float Ellebooghoek2;
aschut 2:7c9974f0947a 56 float Ellebooghoek3;
aschut 2:7c9974f0947a 57 float Ellebooghoek4;
aschut 2:7c9974f0947a 58
aschut 2:7c9974f0947a 59 float PolsReference;
aschut 2:7c9974f0947a 60 float Polshoek1;
aschut 2:7c9974f0947a 61 float Polshoek2;
aschut 2:7c9974f0947a 62 float Polshoek3;
aschut 2:7c9974f0947a 63 float Polshoek4;
aschut 2:7c9974f0947a 64
aschut 2:7c9974f0947a 65 float Hoeknieuw1;
aschut 2:7c9974f0947a 66 float Hoeknieuw2;
aschut 2:7c9974f0947a 67
aschut 2:7c9974f0947a 68 //Limiet in graden
aschut 2:7c9974f0947a 69 float lowerlim1 = 0;
aschut 2:7c9974f0947a 70 float upperlim1 = 748.8;
aschut 2:7c9974f0947a 71 float lowerlim2 = 0;
aschut 2:7c9974f0947a 72 float upperlim2 = 1300;
aschut 2:7c9974f0947a 73
aschut 2:7c9974f0947a 74 // VARIABLES PID CONTROLLER
aschut 2:7c9974f0947a 75 double Kp1 = 5;
aschut 2:7c9974f0947a 76 double Ki1 = 0;
aschut 2:7c9974f0947a 77 double Kd1 = 1;
aschut 2:7c9974f0947a 78 double Kp2 = 6; // Zonder arm: 6,0,1
aschut 2:7c9974f0947a 79 double Ki2 = 0;
aschut 2:7c9974f0947a 80 double Kd2 = 1;
aschut 2:7c9974f0947a 81 double Ts = 0.0005; // Sample time in seconds
aschut 2:7c9974f0947a 82
aschut 2:7c9974f0947a 83 // Functies Kinematica
aschut 2:7c9974f0947a 84 float Kinematics1(float KPot)
aschut 2:7c9974f0947a 85 {
aschut 2:7c9974f0947a 86
aschut 2:7c9974f0947a 87 if (KPot > 0.45f) {
aschut 2:7c9974f0947a 88 stap1 = KPot*150*Ts;
aschut 2:7c9974f0947a 89 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 90 return Hoeknieuw1;
aschut 2:7c9974f0947a 91 }
aschut 2:7c9974f0947a 92
aschut 2:7c9974f0947a 93 else if (KPot < -0.45f) {
aschut 2:7c9974f0947a 94 stap1 = KPot*150*Ts;
aschut 2:7c9974f0947a 95 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 96 return Hoeknieuw1;
aschut 2:7c9974f0947a 97 }
aschut 2:7c9974f0947a 98
aschut 2:7c9974f0947a 99 else {
aschut 2:7c9974f0947a 100 return PolsReference;
aschut 2:7c9974f0947a 101 }
aschut 2:7c9974f0947a 102 }
aschut 2:7c9974f0947a 103 float Kinematics2(float KPot)
aschut 2:7c9974f0947a 104 {
aschut 2:7c9974f0947a 105
aschut 2:7c9974f0947a 106 if (KPot > 0.45f) {
aschut 2:7c9974f0947a 107 stap2 = KPot*300*Ts;
aschut 2:7c9974f0947a 108 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 109 return Hoeknieuw2;
aschut 2:7c9974f0947a 110 }
aschut 2:7c9974f0947a 111
aschut 2:7c9974f0947a 112 else if (KPot < -0.45f) {
aschut 2:7c9974f0947a 113 stap2 = KPot*300*Ts;
aschut 2:7c9974f0947a 114 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 115 return Hoeknieuw2;
aschut 2:7c9974f0947a 116 }
aschut 2:7c9974f0947a 117
aschut 2:7c9974f0947a 118 else {
aschut 2:7c9974f0947a 119 return ElbowReference;
aschut 2:7c9974f0947a 120 }
aschut 2:7c9974f0947a 121 }
aschut 2:7c9974f0947a 122
aschut 2:7c9974f0947a 123 float Limits1(float Polshoek2)
aschut 2:7c9974f0947a 124 {
aschut 2:7c9974f0947a 125
aschut 2:7c9974f0947a 126 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 2:7c9974f0947a 127 Polshoek3 = Polshoek2;
aschut 2:7c9974f0947a 128 }
aschut 2:7c9974f0947a 129
aschut 2:7c9974f0947a 130 else {
aschut 2:7c9974f0947a 131 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 2:7c9974f0947a 132 Polshoek3 = upperlim1;
aschut 2:7c9974f0947a 133 } else { //Onder de limiet
aschut 2:7c9974f0947a 134 Polshoek3 = lowerlim1;
aschut 2:7c9974f0947a 135 }
aschut 2:7c9974f0947a 136 }
aschut 2:7c9974f0947a 137
aschut 2:7c9974f0947a 138 return Polshoek3;
aschut 2:7c9974f0947a 139 }
aschut 2:7c9974f0947a 140
aschut 2:7c9974f0947a 141
aschut 2:7c9974f0947a 142 float Limits2(float Ellebooghoek2)
aschut 2:7c9974f0947a 143 {
aschut 0:90750f158475 144
aschut 2:7c9974f0947a 145 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 2:7c9974f0947a 146 Ellebooghoek3 = Ellebooghoek2;
aschut 2:7c9974f0947a 147 }
aschut 2:7c9974f0947a 148
aschut 2:7c9974f0947a 149 else {
aschut 2:7c9974f0947a 150 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 2:7c9974f0947a 151 Ellebooghoek3 = upperlim2;
aschut 2:7c9974f0947a 152 } else { //Onder de limiet
aschut 2:7c9974f0947a 153 Ellebooghoek3 = lowerlim2;
aschut 2:7c9974f0947a 154 }
aschut 2:7c9974f0947a 155 }
aschut 2:7c9974f0947a 156
aschut 2:7c9974f0947a 157 return Ellebooghoek3;
aschut 2:7c9974f0947a 158 }
aschut 2:7c9974f0947a 159
aschut 2:7c9974f0947a 160
aschut 2:7c9974f0947a 161 // PID Controller
aschut 2:7c9974f0947a 162 double PID_controller1(double error1)
aschut 2:7c9974f0947a 163 {
aschut 2:7c9974f0947a 164 static double error1_integral = 0;
aschut 2:7c9974f0947a 165 static double error1_prev = error1; // initialization with this value only done once!
aschut 2:7c9974f0947a 166 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 2:7c9974f0947a 167
aschut 2:7c9974f0947a 168 // Proportional part:
aschut 2:7c9974f0947a 169 double u_k1 = Kp1 * error1;
aschut 2:7c9974f0947a 170
aschut 2:7c9974f0947a 171 // Integral part
aschut 2:7c9974f0947a 172 error1_integral = error1_integral + error1 * Ts;
aschut 2:7c9974f0947a 173 double u_i1 = Ki1* error1_integral;
aschut 2:7c9974f0947a 174
aschut 2:7c9974f0947a 175 // Derivative part
aschut 2:7c9974f0947a 176 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 2:7c9974f0947a 177 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 2:7c9974f0947a 178 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 2:7c9974f0947a 179 error1_prev = error1;
aschut 2:7c9974f0947a 180
aschut 2:7c9974f0947a 181 // Sum all parts and return it
aschut 2:7c9974f0947a 182 return u_k1 + u_i1 + u_d1;
aschut 2:7c9974f0947a 183 }
aschut 2:7c9974f0947a 184 double PID_controller2(double error2)
aschut 2:7c9974f0947a 185 {
aschut 2:7c9974f0947a 186 static double error2_integral = 0;
aschut 2:7c9974f0947a 187 static double error2_prev = error2; // initialization with this value only done once!
aschut 2:7c9974f0947a 188 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 2:7c9974f0947a 189
aschut 2:7c9974f0947a 190 // Proportional part:
aschut 2:7c9974f0947a 191 double u_k2 = Kp2 * error2;
aschut 2:7c9974f0947a 192
aschut 2:7c9974f0947a 193 // Integral part
aschut 2:7c9974f0947a 194 error2_integral = error2_integral + error2 * Ts;
aschut 2:7c9974f0947a 195 double u_i2 = Ki2 * error2_integral;
aschut 2:7c9974f0947a 196
aschut 2:7c9974f0947a 197 // Derivative part
aschut 2:7c9974f0947a 198 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 2:7c9974f0947a 199 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 2:7c9974f0947a 200 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 2:7c9974f0947a 201 error2_prev = error2;
aschut 2:7c9974f0947a 202
aschut 2:7c9974f0947a 203 // Sum all parts and return it
aschut 2:7c9974f0947a 204 return u_k2 + u_i2 + u_d2;
aschut 2:7c9974f0947a 205 }
aschut 2:7c9974f0947a 206
aschut 2:7c9974f0947a 207 // Functies Motor
aschut 2:7c9974f0947a 208 void moter1_control(double u1)
aschut 2:7c9974f0947a 209 {
aschut 2:7c9974f0947a 210 direction1= u1 > 0.0f; //positief = CW
aschut 2:7c9974f0947a 211 if (fabs(u1)> 0.5f) {
aschut 2:7c9974f0947a 212 u1 = 0.5f;
aschut 2:7c9974f0947a 213 } else {
aschut 2:7c9974f0947a 214 u1= u1;
aschut 2:7c9974f0947a 215 }
aschut 2:7c9974f0947a 216 pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 217 }
aschut 2:7c9974f0947a 218
aschut 2:7c9974f0947a 219 void moter2_control(double u2)
aschut 2:7c9974f0947a 220 {
aschut 2:7c9974f0947a 221 direction2= u2 < 0.0f; //positief = CW
aschut 2:7c9974f0947a 222 if (fabs(u2)> 0.99f) {
aschut 2:7c9974f0947a 223 u2 = 0.99f;
aschut 2:7c9974f0947a 224 } else {
aschut 2:7c9974f0947a 225 u2= u2;
aschut 2:7c9974f0947a 226 }
aschut 2:7c9974f0947a 227 pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 228 }
aschut 2:7c9974f0947a 229
aschut 2:7c9974f0947a 230 void PwmMotor(void)
aschut 2:7c9974f0947a 231 {
aschut 2:7c9974f0947a 232 // Reference hoek berekenen, in graden
aschut 2:7c9974f0947a 233 float Ellebooghoek1 = Kinematics2(pwm2);
aschut 2:7c9974f0947a 234 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 2:7c9974f0947a 235 ElbowReference = Ellebooghoek4;
aschut 2:7c9974f0947a 236
aschut 2:7c9974f0947a 237 float Polshoek1 = Kinematics1(pwm2);
aschut 2:7c9974f0947a 238 float Polshoek4 = Limits1(Polshoek1);
aschut 2:7c9974f0947a 239 PolsReference = Polshoek4;
aschut 2:7c9974f0947a 240
aschut 2:7c9974f0947a 241 // Positie motor berekenen, in graden
aschut 2:7c9974f0947a 242 Pulses1 = encoder1.getPulses();
aschut 2:7c9974f0947a 243 motor_position1 = -(Pulses1/1200)*360;
aschut 2:7c9974f0947a 244 Pulses2 = encoder2.getPulses();
aschut 2:7c9974f0947a 245 motor_position2 = -(Pulses2/4800)*360;
aschut 2:7c9974f0947a 246
aschut 2:7c9974f0947a 247 double error1 = PolsReference - motor_position1;
aschut 2:7c9974f0947a 248 double u1 = PID_controller1(error1);
aschut 2:7c9974f0947a 249 moter1_control(u1);
aschut 2:7c9974f0947a 250
aschut 2:7c9974f0947a 251 double error2 = ElbowReference - motor_position2;
aschut 2:7c9974f0947a 252 double u2 = PID_controller2(error2);
aschut 2:7c9974f0947a 253 moter2_control(u2);
aschut 2:7c9974f0947a 254
aschut 2:7c9974f0947a 255 }
aschut 2:7c9974f0947a 256
aschut 2:7c9974f0947a 257 void MotorOn(void)
aschut 2:7c9974f0947a 258 {
aschut 2:7c9974f0947a 259 pwmpin1 = 0;
aschut 2:7c9974f0947a 260 pwmpin2 = 0;
aschut 2:7c9974f0947a 261 Pwm.attach (PwmMotor, Ts);
aschut 2:7c9974f0947a 262
aschut 2:7c9974f0947a 263 }
aschut 2:7c9974f0947a 264
aschut 2:7c9974f0947a 265
aschut 2:7c9974f0947a 266 void ContinuousReader(void)
aschut 2:7c9974f0947a 267 {
aschut 2:7c9974f0947a 268 Pot2 = pot.read();
aschut 2:7c9974f0947a 269 Pot1 = pot0.read();
aschut 2:7c9974f0947a 270 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 2:7c9974f0947a 271 pwm1 =(Pot1*2)-1;
aschut 2:7c9974f0947a 272 }
aschut 2:7c9974f0947a 273
aschut 2:7c9974f0947a 274 // StateMachine
aschut 2:7c9974f0947a 275
aschut 2:7c9974f0947a 276 enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
aschut 0:90750f158475 277 int f = 1;
aschut 0:90750f158475 278 states currentState = MOTORS_OFF;
aschut 0:90750f158475 279 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 280
aschut 0:90750f158475 281 void ProcessStateMachine(void)
aschut 0:90750f158475 282 {
aschut 0:90750f158475 283 switch (currentState)
aschut 0:90750f158475 284 {
aschut 0:90750f158475 285 case MOTORS_OFF:
aschut 0:90750f158475 286 // Actions
aschut 0:90750f158475 287 if (stateChanged)
aschut 0:90750f158475 288 {
aschut 0:90750f158475 289 // state initialization: rood
aschut 2:7c9974f0947a 290
aschut 0:90750f158475 291 led1 = 1;
aschut 0:90750f158475 292 led2 = 0;
aschut 0:90750f158475 293 led3 = 1;
aschut 0:90750f158475 294 wait (1);
aschut 0:90750f158475 295 stateChanged = false;
aschut 0:90750f158475 296 }
aschut 0:90750f158475 297
aschut 0:90750f158475 298 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 2:7c9974f0947a 299 if (!button3)
aschut 0:90750f158475 300 {
aschut 2:7c9974f0947a 301 currentState = CALIBRATION ;
aschut 0:90750f158475 302 stateChanged = true;
aschut 0:90750f158475 303 }
aschut 0:90750f158475 304 else
aschut 0:90750f158475 305 {
aschut 0:90750f158475 306 currentState = MOTORS_OFF;
aschut 0:90750f158475 307 stateChanged = true;
aschut 0:90750f158475 308 }
aschut 0:90750f158475 309
aschut 0:90750f158475 310 break;
aschut 0:90750f158475 311
aschut 0:90750f158475 312 case CALIBRATION:
aschut 0:90750f158475 313 // Actions
aschut 0:90750f158475 314 if (stateChanged)
aschut 0:90750f158475 315 {
aschut 0:90750f158475 316 // state initialization: oranje
aschut 0:90750f158475 317 led1 = 0;
aschut 0:90750f158475 318 led2 = 0;
aschut 0:90750f158475 319 led3 = 1;
aschut 0:90750f158475 320 wait (1);
aschut 0:90750f158475 321
aschut 0:90750f158475 322 stateChanged = false;
aschut 0:90750f158475 323 }
aschut 0:90750f158475 324
aschut 0:90750f158475 325 // State transition logic: automatisch terug naar motors off.
aschut 0:90750f158475 326
aschut 2:7c9974f0947a 327 currentState = HOMING1;
aschut 2:7c9974f0947a 328 stateChanged = true;
aschut 0:90750f158475 329 break;
aschut 2:7c9974f0947a 330
aschut 2:7c9974f0947a 331 case HOMING1:
aschut 0:90750f158475 332 // Actions
aschut 0:90750f158475 333 if (stateChanged)
aschut 0:90750f158475 334 {
aschut 0:90750f158475 335 // state initialization: green
aschut 0:90750f158475 336 t.start();
aschut 0:90750f158475 337 led1 = 0;
aschut 0:90750f158475 338 led2 = 1;
aschut 0:90750f158475 339 led3 = 1;
aschut 2:7c9974f0947a 340
aschut 2:7c9974f0947a 341 if (!But1)
aschut 2:7c9974f0947a 342 {
aschut 2:7c9974f0947a 343 led1 = 1;
aschut 2:7c9974f0947a 344 float H1 = 0.98f;
aschut 2:7c9974f0947a 345 moter1_control(H1);
aschut 2:7c9974f0947a 346 wait(0.001f);
aschut 2:7c9974f0947a 347 }
aschut 2:7c9974f0947a 348 else if (!But2)
aschut 2:7c9974f0947a 349 {
aschut 2:7c9974f0947a 350 led1 = 1;
aschut 2:7c9974f0947a 351 float H1 = -0.98f;
aschut 2:7c9974f0947a 352 moter1_control(H1);
aschut 2:7c9974f0947a 353 wait(0.001f);
aschut 2:7c9974f0947a 354 }
aschut 2:7c9974f0947a 355 Pulses1 = 0;
aschut 2:7c9974f0947a 356 pwmpin1 = 0;
aschut 2:7c9974f0947a 357 pwmpin2 = 0;
aschut 2:7c9974f0947a 358 ;
aschut 0:90750f158475 359
aschut 0:90750f158475 360 stateChanged = false;
aschut 0:90750f158475 361 }
aschut 0:90750f158475 362
aschut 2:7c9974f0947a 363 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
aschut 2:7c9974f0947a 364
aschut 2:7c9974f0947a 365 if (!button3)
aschut 0:90750f158475 366 {
aschut 2:7c9974f0947a 367 currentState = HOMING2 ;
aschut 0:90750f158475 368 stateChanged = true;
aschut 2:7c9974f0947a 369 wait(1);
aschut 0:90750f158475 370 }
aschut 0:90750f158475 371 else if (t>300)
aschut 0:90750f158475 372 {
aschut 0:90750f158475 373 t.stop();
aschut 0:90750f158475 374 t.reset();
aschut 0:90750f158475 375 currentState = MOTORS_OFF ;
aschut 0:90750f158475 376 stateChanged = true;
aschut 0:90750f158475 377 }
aschut 0:90750f158475 378 else
aschut 0:90750f158475 379 {
aschut 2:7c9974f0947a 380 currentState = HOMING1 ;
aschut 2:7c9974f0947a 381 stateChanged = true;
aschut 2:7c9974f0947a 382 }
aschut 2:7c9974f0947a 383 break;
aschut 2:7c9974f0947a 384
aschut 2:7c9974f0947a 385 case HOMING2:
aschut 2:7c9974f0947a 386 // Actions
aschut 2:7c9974f0947a 387 if (stateChanged)
aschut 2:7c9974f0947a 388 {
aschut 2:7c9974f0947a 389 // state initialization: green
aschut 2:7c9974f0947a 390 t.start();
aschut 2:7c9974f0947a 391 led1 = 0;
aschut 2:7c9974f0947a 392 led2 = 1;
aschut 2:7c9974f0947a 393 led3 = 1;
aschut 2:7c9974f0947a 394
aschut 2:7c9974f0947a 395 if (!But1)
aschut 2:7c9974f0947a 396 {
aschut 2:7c9974f0947a 397 led1 = 1;
aschut 2:7c9974f0947a 398 float H2 = 0.98f;
aschut 2:7c9974f0947a 399 moter2_control(H2);
aschut 2:7c9974f0947a 400 wait(0.001f);
aschut 2:7c9974f0947a 401 }
aschut 2:7c9974f0947a 402 else if (!But2)
aschut 2:7c9974f0947a 403 {
aschut 2:7c9974f0947a 404 led1 = 1;
aschut 2:7c9974f0947a 405 float H2 = -0.98f;
aschut 2:7c9974f0947a 406 moter2_control(H2);
aschut 2:7c9974f0947a 407 wait(0.001f);
aschut 2:7c9974f0947a 408 }
aschut 2:7c9974f0947a 409 Pulses2 = 0;
aschut 2:7c9974f0947a 410 pwmpin1 = 0;
aschut 2:7c9974f0947a 411 pwmpin2 = 0;
aschut 2:7c9974f0947a 412 ;
aschut 2:7c9974f0947a 413
aschut 2:7c9974f0947a 414 stateChanged = false;
aschut 2:7c9974f0947a 415 }
aschut 2:7c9974f0947a 416
aschut 2:7c9974f0947a 417 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
aschut 2:7c9974f0947a 418 if (!button2)
aschut 2:7c9974f0947a 419 {
aschut 2:7c9974f0947a 420 currentState = DEMO;
aschut 2:7c9974f0947a 421 stateChanged = true;
aschut 2:7c9974f0947a 422 }
aschut 2:7c9974f0947a 423 else if (!button3)
aschut 2:7c9974f0947a 424 {
aschut 2:7c9974f0947a 425 currentState = MOVEMENT ;
aschut 2:7c9974f0947a 426 stateChanged = true;
aschut 2:7c9974f0947a 427 led1 = 1;
aschut 2:7c9974f0947a 428 led2 = 0;
aschut 2:7c9974f0947a 429 led3 = 0;
aschut 2:7c9974f0947a 430 wait(1);
aschut 2:7c9974f0947a 431 }
aschut 2:7c9974f0947a 432 else if (t>300)
aschut 2:7c9974f0947a 433 {
aschut 2:7c9974f0947a 434 t.stop();
aschut 2:7c9974f0947a 435 t.reset();
aschut 2:7c9974f0947a 436 currentState = MOTORS_OFF ;
aschut 2:7c9974f0947a 437 stateChanged = true;
aschut 2:7c9974f0947a 438 }
aschut 2:7c9974f0947a 439 else
aschut 2:7c9974f0947a 440 {
aschut 2:7c9974f0947a 441 currentState = HOMING2 ;
aschut 0:90750f158475 442 stateChanged = true;
aschut 0:90750f158475 443 }
aschut 0:90750f158475 444 break;
aschut 0:90750f158475 445
aschut 0:90750f158475 446 case DEMO:
aschut 0:90750f158475 447 // Actions
aschut 0:90750f158475 448 if (stateChanged)
aschut 0:90750f158475 449 {
aschut 0:90750f158475 450 // state initialization: light blue
aschut 0:90750f158475 451 led1 = 0;
aschut 0:90750f158475 452 led2 = 1;
aschut 0:90750f158475 453 led3 = 0;
aschut 0:90750f158475 454 wait (1);
aschut 0:90750f158475 455
aschut 0:90750f158475 456 stateChanged = false;
aschut 0:90750f158475 457 }
aschut 0:90750f158475 458
aschut 0:90750f158475 459 // State transition logic: automatisch terug naar HOMING
aschut 2:7c9974f0947a 460 currentState = HOMING1;
aschut 0:90750f158475 461 stateChanged = true;
aschut 0:90750f158475 462 break;
aschut 0:90750f158475 463
aschut 0:90750f158475 464 case MOVEMENT:
aschut 0:90750f158475 465 // Actions
aschut 0:90750f158475 466 if (stateChanged)
aschut 0:90750f158475 467 {
aschut 0:90750f158475 468 // state initialization: purple
aschut 0:90750f158475 469 t.start();
aschut 2:7c9974f0947a 470 pwmpin1 = 0;
aschut 2:7c9974f0947a 471 pwmpin2 = 0;
aschut 2:7c9974f0947a 472 Pwm.attach (PwmMotor, Ts);
aschut 0:90750f158475 473 led1 = 1;
aschut 0:90750f158475 474 led2 = 0;
aschut 0:90750f158475 475 led3 = 0;
aschut 2:7c9974f0947a 476
aschut 0:90750f158475 477 stateChanged = false;
aschut 0:90750f158475 478 }
aschut 0:90750f158475 479
aschut 2:7c9974f0947a 480 // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
aschut 2:7c9974f0947a 481 if (!button2)
aschut 0:90750f158475 482 {
aschut 2:7c9974f0947a 483 currentState = FREEZE;
aschut 1:070092564648 484 stateChanged = true;
aschut 0:90750f158475 485 }
aschut 2:7c9974f0947a 486 else if (!button3)
aschut 0:90750f158475 487 {
aschut 2:7c9974f0947a 488 Pwm.detach ();
aschut 2:7c9974f0947a 489 pwmpin2 = 0;
aschut 2:7c9974f0947a 490 pwmpin1 = 0;
aschut 0:90750f158475 491 currentState = MOTORS_OFF ;
aschut 0:90750f158475 492 stateChanged = true;
aschut 0:90750f158475 493 }
aschut 0:90750f158475 494 else if (t>300)
aschut 0:90750f158475 495 {
aschut 0:90750f158475 496 t.stop();
aschut 0:90750f158475 497 t.reset();
aschut 2:7c9974f0947a 498 currentState = HOMING1 ;
aschut 0:90750f158475 499 stateChanged = true;
aschut 0:90750f158475 500 }
aschut 0:90750f158475 501 else
aschut 0:90750f158475 502 {
aschut 0:90750f158475 503 currentState = MOVEMENT ;
aschut 0:90750f158475 504 stateChanged = true;
aschut 0:90750f158475 505 }
aschut 0:90750f158475 506 break;
aschut 0:90750f158475 507
aschut 2:7c9974f0947a 508 case FREEZE:
aschut 0:90750f158475 509 // Actions
aschut 0:90750f158475 510 if (stateChanged)
aschut 0:90750f158475 511 {
aschut 0:90750f158475 512 // state initialization: blue
aschut 0:90750f158475 513 led1 = 1;
aschut 0:90750f158475 514 led2 = 1;
aschut 0:90750f158475 515 led3 = 0;
aschut 0:90750f158475 516 wait (1);
aschut 0:90750f158475 517
aschut 0:90750f158475 518 stateChanged = false;
aschut 0:90750f158475 519 }
aschut 0:90750f158475 520
aschut 0:90750f158475 521 // State transition logic: automatisch terug naar MOVEMENT.
aschut 0:90750f158475 522
aschut 0:90750f158475 523 currentState = MOVEMENT;
aschut 0:90750f158475 524 stateChanged = true;
aschut 0:90750f158475 525 break;
aschut 0:90750f158475 526
aschut 0:90750f158475 527 }
aschut 0:90750f158475 528 }
aschut 0:90750f158475 529
aschut 0:90750f158475 530 int main()
aschut 0:90750f158475 531 {
aschut 2:7c9974f0947a 532
aschut 2:7c9974f0947a 533 t2.start();
aschut 2:7c9974f0947a 534 int counter = 0;
aschut 2:7c9974f0947a 535 pwmpin2.period_us(60);
aschut 2:7c9974f0947a 536 PotRead.attach(ContinuousReader,Ts);
aschut 2:7c9974f0947a 537 pc.baud(115200);
aschut 2:7c9974f0947a 538
aschut 2:7c9974f0947a 539 while(true)
aschut 0:90750f158475 540 {
aschut 0:90750f158475 541 led1 = 1;
aschut 2:7c9974f0947a 542 led2 =1;
aschut 2:7c9974f0947a 543 led3 =1;
aschut 2:7c9974f0947a 544 if(counter==10)
aschut 2:7c9974f0947a 545 {
aschut 2:7c9974f0947a 546 float tmp = t2.read();
aschut 2:7c9974f0947a 547 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
aschut 2:7c9974f0947a 548 counter = 0;
aschut 2:7c9974f0947a 549 }
aschut 2:7c9974f0947a 550 counter++;
aschut 2:7c9974f0947a 551
aschut 0:90750f158475 552 ProcessStateMachine();
aschut 2:7c9974f0947a 553
aschut 0:90750f158475 554
aschut 2:7c9974f0947a 555 wait(0.001);
aschut 2:7c9974f0947a 556 }
aschut 0:90750f158475 557 }
aschut 0:90750f158475 558
aschut 2:7c9974f0947a 559
aschut 2:7c9974f0947a 560
aschut 2:7c9974f0947a 561
aschut 2:7c9974f0947a 562
aschut 2:7c9974f0947a 563
aschut 0:90750f158475 564
aschut 0:90750f158475 565
aschut 0:90750f158475 566
aschut 0:90750f158475 567
aschut 0:90750f158475 568
aschut 0:90750f158475 569
aschut 2:7c9974f0947a 570