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@3:2aaf54ce090b, 2019-04-04 (annotated)

Committer:
aschut
Date:
Thu Apr 04 15:37:37 2019 +0000
Revision:
3:2aaf54ce090b
Parent:
2:7c9974f0947a
Child:
4:babe09a69296
pre servo

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