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@8:7fd9ac522ea9, 2019-04-17 (annotated)

Committer:
aschut
Date:
Wed Apr 17 14:57:37 2019 +0000
Revision:
8:7fd9ac522ea9
Parent:
6:464d2fdfd7de
Child:
9:ad4bd1b16eed
met PID waardes

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 4:babe09a69296 7
aschut 2:7c9974f0947a 8 // Algemeen
aschut 6:464d2fdfd7de 9 DigitalIn button3(SW3);
aschut 6:464d2fdfd7de 10 DigitalIn button2(SW2);
aschut 8:7fd9ac522ea9 11 DigitalIn But2(D12);
aschut 8:7fd9ac522ea9 12 DigitalIn But1(D13);
aschut 0:90750f158475 13
aschut 0:90750f158475 14 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 15 DigitalOut led2(LED_RED);
aschut 0:90750f158475 16 DigitalOut led3(LED_BLUE);
aschut 6:464d2fdfd7de 17 float counts = 0;
aschut 0:90750f158475 18
aschut 2:7c9974f0947a 19 MODSERIAL pc(USBTX, USBRX);
aschut 0:90750f158475 20 Timer t;
aschut 2:7c9974f0947a 21 Timer t2;
aschut 6:464d2fdfd7de 22
aschut 2:7c9974f0947a 23 //Motoren
aschut 2:7c9974f0947a 24 DigitalOut direction1(D4);
aschut 6:464d2fdfd7de 25 FastPWM pwmpin1(D5);
aschut 3:2aaf54ce090b 26 FastPWM pwmpin2(D6);
aschut 2:7c9974f0947a 27 DigitalOut direction2(D7);
aschut 2:7c9974f0947a 28 volatile float pwm1;
aschut 2:7c9974f0947a 29 volatile float pwm2;
aschut 2:7c9974f0947a 30
aschut 2:7c9974f0947a 31 //Encoder
aschut 8:7fd9ac522ea9 32 QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
aschut 8:7fd9ac522ea9 33 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
aschut 2:7c9974f0947a 34 double Pulses1;
aschut 2:7c9974f0947a 35 double motor_position1;
aschut 2:7c9974f0947a 36 double Pulses2;
aschut 2:7c9974f0947a 37 double motor_position2;
aschut 2:7c9974f0947a 38 double error1;
aschut 3:2aaf54ce090b 39 double u1;
aschut 2:7c9974f0947a 40
aschut 2:7c9974f0947a 41 //Pot meter
aschut 8:7fd9ac522ea9 42 AnalogIn pot(A5);
aschut 2:7c9974f0947a 43 AnalogIn pot0(A0);
aschut 2:7c9974f0947a 44 float Pot2;
aschut 2:7c9974f0947a 45 float Pot1;
aschut 2:7c9974f0947a 46
aschut 2:7c9974f0947a 47 //Ticker
aschut 2:7c9974f0947a 48 Ticker Pwm;
aschut 2:7c9974f0947a 49 Ticker PotRead;
aschut 2:7c9974f0947a 50 Ticker Kdc;
aschut 2:7c9974f0947a 51
aschut 4:babe09a69296 52 //Servo
aschut 4:babe09a69296 53 Ticker ServoTick;
aschut 6:464d2fdfd7de 54 DigitalOut myservo1(D8); //Duim
aschut 8:7fd9ac522ea9 55 DigitalOut myservo2(D11); //Pink tot Middel vinger
aschut 6:464d2fdfd7de 56 DigitalOut myservo3(D10); //wijsvinger
aschut 6:464d2fdfd7de 57
aschut 6:464d2fdfd7de 58 float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms
aschut 6:464d2fdfd7de 59 float servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
aschut 6:464d2fdfd7de 60 float servo_position2;
aschut 6:464d2fdfd7de 61 float servo_position3;
aschut 6:464d2fdfd7de 62
aschut 6:464d2fdfd7de 63 // Vinger posities
aschut 6:464d2fdfd7de 64 float Duim_krom = 0.05;
aschut 6:464d2fdfd7de 65 float Duim_recht = 0.85;
aschut 6:464d2fdfd7de 66 float MWP_krom = 0.06;
aschut 6:464d2fdfd7de 67 float MWP_recht = 0.89;
aschut 6:464d2fdfd7de 68 float Wijsvinger_krom = 0.15;
aschut 6:464d2fdfd7de 69 float Wijsvinger_recht = 0.93;
aschut 6:464d2fdfd7de 70
aschut 4:babe09a69296 71
aschut 3:2aaf54ce090b 72 // EMG
aschut 3:2aaf54ce090b 73 float EMG1; // Rotatie
aschut 3:2aaf54ce090b 74 float EMG2; // Elleboog
aschut 3:2aaf54ce090b 75 float EMG3; // Hand
aschut 3:2aaf54ce090b 76 float EMG4; // Reverse
aschut 3:2aaf54ce090b 77 float Input1; // Voor zonder EMG
aschut 3:2aaf54ce090b 78 float Input2;
aschut 3:2aaf54ce090b 79 int count = 0;
aschut 3:2aaf54ce090b 80
aschut 2:7c9974f0947a 81 //Kinematica
aschut 2:7c9974f0947a 82 double stap1;
aschut 2:7c9974f0947a 83 double stap2;
aschut 2:7c9974f0947a 84 double KPot;
aschut 2:7c9974f0947a 85
aschut 2:7c9974f0947a 86 float ElbowReference;
aschut 2:7c9974f0947a 87 float Ellebooghoek1;
aschut 2:7c9974f0947a 88 float Ellebooghoek2;
aschut 2:7c9974f0947a 89 float Ellebooghoek3;
aschut 2:7c9974f0947a 90 float Ellebooghoek4;
aschut 2:7c9974f0947a 91
aschut 2:7c9974f0947a 92 float PolsReference;
aschut 2:7c9974f0947a 93 float Polshoek1;
aschut 2:7c9974f0947a 94 float Polshoek2;
aschut 2:7c9974f0947a 95 float Polshoek3;
aschut 2:7c9974f0947a 96 float Polshoek4;
aschut 2:7c9974f0947a 97
aschut 2:7c9974f0947a 98 float Hoeknieuw1;
aschut 2:7c9974f0947a 99 float Hoeknieuw2;
aschut 2:7c9974f0947a 100
aschut 2:7c9974f0947a 101 //Limiet in graden
aschut 3:2aaf54ce090b 102 float lowerlim1 = -900;
aschut 3:2aaf54ce090b 103 float upperlim1 = 900;
aschut 6:464d2fdfd7de 104 float lowerlim2 = 0;
aschut 6:464d2fdfd7de 105 float upperlim2 = 1500;
aschut 2:7c9974f0947a 106
aschut 2:7c9974f0947a 107 // VARIABLES PID CONTROLLER
aschut 6:464d2fdfd7de 108 double Kp1 = 12.5;
aschut 6:464d2fdfd7de 109 double Ki1 = 0;
aschut 2:7c9974f0947a 110 double Kd1 = 1;
aschut 8:7fd9ac522ea9 111 double Kp2 = 20; // Zonder arm: 6,0,1, met rotatie: 10, 0, 1
aschut 6:464d2fdfd7de 112 double Ki2 = 0;
aschut 6:464d2fdfd7de 113 double Kd2 = 1;
aschut 2:7c9974f0947a 114 double Ts = 0.0005; // Sample time in seconds
aschut 2:7c9974f0947a 115
aschut 2:7c9974f0947a 116 // Functies Kinematica
aschut 3:2aaf54ce090b 117 float Kinematics1(float EMG1)
aschut 2:7c9974f0947a 118 {
aschut 2:7c9974f0947a 119
aschut 3:2aaf54ce090b 120 if (EMG1 > 0.45f) {
aschut 3:2aaf54ce090b 121 stap1 = EMG1*450*Ts;
aschut 2:7c9974f0947a 122 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 123 return Hoeknieuw1;
aschut 2:7c9974f0947a 124 }
aschut 2:7c9974f0947a 125
aschut 3:2aaf54ce090b 126 else if (EMG1 < -0.45f) {
aschut 3:2aaf54ce090b 127 stap1 = EMG1*450*Ts;
aschut 2:7c9974f0947a 128 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 129 return Hoeknieuw1;
aschut 2:7c9974f0947a 130 }
aschut 2:7c9974f0947a 131
aschut 2:7c9974f0947a 132 else {
aschut 2:7c9974f0947a 133 return PolsReference;
aschut 2:7c9974f0947a 134 }
aschut 2:7c9974f0947a 135 }
aschut 3:2aaf54ce090b 136 float Kinematics2(float EMG2)
aschut 2:7c9974f0947a 137 {
aschut 2:7c9974f0947a 138
aschut 3:2aaf54ce090b 139 if (EMG2 > 0.45f) {
aschut 6:464d2fdfd7de 140 stap2 = EMG2*300*Ts;
aschut 2:7c9974f0947a 141 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 142 return Hoeknieuw2;
aschut 2:7c9974f0947a 143 }
aschut 2:7c9974f0947a 144
aschut 3:2aaf54ce090b 145 else if (EMG2 < -0.45f) {
aschut 3:2aaf54ce090b 146 stap2 = EMG2*300*Ts;
aschut 2:7c9974f0947a 147 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 148 return Hoeknieuw2;
aschut 2:7c9974f0947a 149 }
aschut 2:7c9974f0947a 150
aschut 2:7c9974f0947a 151 else {
aschut 2:7c9974f0947a 152 return ElbowReference;
aschut 2:7c9974f0947a 153 }
aschut 2:7c9974f0947a 154 }
aschut 2:7c9974f0947a 155
aschut 2:7c9974f0947a 156 float Limits1(float Polshoek2)
aschut 2:7c9974f0947a 157 {
aschut 2:7c9974f0947a 158
aschut 2:7c9974f0947a 159 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 2:7c9974f0947a 160 Polshoek3 = Polshoek2;
aschut 2:7c9974f0947a 161 }
aschut 2:7c9974f0947a 162
aschut 2:7c9974f0947a 163 else {
aschut 2:7c9974f0947a 164 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 2:7c9974f0947a 165 Polshoek3 = upperlim1;
aschut 2:7c9974f0947a 166 } else { //Onder de limiet
aschut 2:7c9974f0947a 167 Polshoek3 = lowerlim1;
aschut 2:7c9974f0947a 168 }
aschut 2:7c9974f0947a 169 }
aschut 2:7c9974f0947a 170
aschut 2:7c9974f0947a 171 return Polshoek3;
aschut 2:7c9974f0947a 172 }
aschut 2:7c9974f0947a 173
aschut 2:7c9974f0947a 174
aschut 2:7c9974f0947a 175 float Limits2(float Ellebooghoek2)
aschut 2:7c9974f0947a 176 {
aschut 0:90750f158475 177
aschut 2:7c9974f0947a 178 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 2:7c9974f0947a 179 Ellebooghoek3 = Ellebooghoek2;
aschut 2:7c9974f0947a 180 }
aschut 2:7c9974f0947a 181
aschut 2:7c9974f0947a 182 else {
aschut 2:7c9974f0947a 183 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 2:7c9974f0947a 184 Ellebooghoek3 = upperlim2;
aschut 2:7c9974f0947a 185 } else { //Onder de limiet
aschut 2:7c9974f0947a 186 Ellebooghoek3 = lowerlim2;
aschut 2:7c9974f0947a 187 }
aschut 2:7c9974f0947a 188 }
aschut 2:7c9974f0947a 189
aschut 2:7c9974f0947a 190 return Ellebooghoek3;
aschut 2:7c9974f0947a 191 }
aschut 2:7c9974f0947a 192
aschut 2:7c9974f0947a 193
aschut 2:7c9974f0947a 194 // PID Controller
aschut 2:7c9974f0947a 195 double PID_controller1(double error1)
aschut 2:7c9974f0947a 196 {
aschut 2:7c9974f0947a 197 static double error1_integral = 0;
aschut 2:7c9974f0947a 198 static double error1_prev = error1; // initialization with this value only done once!
aschut 2:7c9974f0947a 199 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 6:464d2fdfd7de 200
aschut 2:7c9974f0947a 201 // Proportional part:
aschut 2:7c9974f0947a 202 double u_k1 = Kp1 * error1;
aschut 2:7c9974f0947a 203
aschut 2:7c9974f0947a 204 // Integral part
aschut 2:7c9974f0947a 205 error1_integral = error1_integral + error1 * Ts;
aschut 2:7c9974f0947a 206 double u_i1 = Ki1* error1_integral;
aschut 2:7c9974f0947a 207
aschut 2:7c9974f0947a 208 // Derivative part
aschut 2:7c9974f0947a 209 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 2:7c9974f0947a 210 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 2:7c9974f0947a 211 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 2:7c9974f0947a 212 error1_prev = error1;
aschut 2:7c9974f0947a 213
aschut 2:7c9974f0947a 214 // Sum all parts and return it
aschut 2:7c9974f0947a 215 return u_k1 + u_i1 + u_d1;
aschut 2:7c9974f0947a 216 }
aschut 2:7c9974f0947a 217 double PID_controller2(double error2)
aschut 2:7c9974f0947a 218 {
aschut 2:7c9974f0947a 219 static double error2_integral = 0;
aschut 2:7c9974f0947a 220 static double error2_prev = error2; // initialization with this value only done once!
aschut 2:7c9974f0947a 221 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 6:464d2fdfd7de 222
aschut 2:7c9974f0947a 223 // Proportional part:
aschut 2:7c9974f0947a 224 double u_k2 = Kp2 * error2;
aschut 2:7c9974f0947a 225
aschut 2:7c9974f0947a 226 // Integral part
aschut 2:7c9974f0947a 227 error2_integral = error2_integral + error2 * Ts;
aschut 2:7c9974f0947a 228 double u_i2 = Ki2 * error2_integral;
aschut 2:7c9974f0947a 229
aschut 2:7c9974f0947a 230 // Derivative part
aschut 2:7c9974f0947a 231 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 2:7c9974f0947a 232 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 2:7c9974f0947a 233 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 2:7c9974f0947a 234 error2_prev = error2;
aschut 2:7c9974f0947a 235
aschut 2:7c9974f0947a 236 // Sum all parts and return it
aschut 2:7c9974f0947a 237 return u_k2 + u_i2 + u_d2;
aschut 2:7c9974f0947a 238 }
aschut 2:7c9974f0947a 239
aschut 2:7c9974f0947a 240 // Functies Motor
aschut 2:7c9974f0947a 241 void moter1_control(double u1)
aschut 2:7c9974f0947a 242 {
aschut 2:7c9974f0947a 243 direction1= u1 > 0.0f; //positief = CW
aschut 6:464d2fdfd7de 244 if (fabs(u1)> 0.7f) {
aschut 6:464d2fdfd7de 245 u1 = 0.7f;
aschut 2:7c9974f0947a 246 } else {
aschut 2:7c9974f0947a 247 u1= u1;
aschut 2:7c9974f0947a 248 }
aschut 3:2aaf54ce090b 249 pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 250 }
aschut 2:7c9974f0947a 251
aschut 2:7c9974f0947a 252 void moter2_control(double u2)
aschut 2:7c9974f0947a 253 {
aschut 2:7c9974f0947a 254 direction2= u2 < 0.0f; //positief = CW
aschut 2:7c9974f0947a 255 if (fabs(u2)> 0.99f) {
aschut 2:7c9974f0947a 256 u2 = 0.99f;
aschut 2:7c9974f0947a 257 } else {
aschut 2:7c9974f0947a 258 u2= u2;
aschut 2:7c9974f0947a 259 }
aschut 3:2aaf54ce090b 260 pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 261 }
aschut 2:7c9974f0947a 262
aschut 2:7c9974f0947a 263 void PwmMotor(void)
aschut 2:7c9974f0947a 264 {
aschut 5:ef77da99d0d1 265 //Continues Read
aschut 5:ef77da99d0d1 266 Pot2 = pot.read();
aschut 5:ef77da99d0d1 267 Pot1 = pot0.read();
aschut 5:ef77da99d0d1 268 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 5:ef77da99d0d1 269 pwm1 =(Pot1*2)-1;
aschut 5:ef77da99d0d1 270 Input1 = pwm1;
aschut 5:ef77da99d0d1 271 Input2 = pwm2;
aschut 6:464d2fdfd7de 272
aschut 3:2aaf54ce090b 273 float Polshoek1 = Kinematics1(Input1);
aschut 3:2aaf54ce090b 274 float Polshoek4 = Limits1(Polshoek1);
aschut 3:2aaf54ce090b 275 PolsReference = Polshoek4;
aschut 6:464d2fdfd7de 276
aschut 2:7c9974f0947a 277 // Reference hoek berekenen, in graden
aschut 3:2aaf54ce090b 278 float Ellebooghoek1 = Kinematics2(Input2);
aschut 2:7c9974f0947a 279 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 2:7c9974f0947a 280 ElbowReference = Ellebooghoek4;
aschut 6:464d2fdfd7de 281
aschut 2:7c9974f0947a 282 // Positie motor berekenen, in graden
aschut 2:7c9974f0947a 283 Pulses1 = encoder1.getPulses();
aschut 2:7c9974f0947a 284 motor_position1 = -(Pulses1/1200)*360;
aschut 2:7c9974f0947a 285 Pulses2 = encoder2.getPulses();
aschut 2:7c9974f0947a 286 motor_position2 = -(Pulses2/4800)*360;
aschut 2:7c9974f0947a 287
aschut 2:7c9974f0947a 288 double error1 = PolsReference - motor_position1;
aschut 2:7c9974f0947a 289 double u1 = PID_controller1(error1);
aschut 2:7c9974f0947a 290 moter1_control(u1);
aschut 6:464d2fdfd7de 291
aschut 2:7c9974f0947a 292 double error2 = ElbowReference - motor_position2;
aschut 2:7c9974f0947a 293 double u2 = PID_controller2(error2);
aschut 2:7c9974f0947a 294 moter2_control(u2);
aschut 2:7c9974f0947a 295
aschut 2:7c9974f0947a 296 }
aschut 2:7c9974f0947a 297
aschut 2:7c9974f0947a 298 void MotorOn(void)
aschut 2:7c9974f0947a 299 {
aschut 2:7c9974f0947a 300 pwmpin1 = 0;
aschut 2:7c9974f0947a 301 pwmpin2 = 0;
aschut 2:7c9974f0947a 302 Pwm.attach (PwmMotor, Ts);
aschut 2:7c9974f0947a 303
aschut 2:7c9974f0947a 304 }
aschut 2:7c9974f0947a 305
aschut 6:464d2fdfd7de 306 //Servo functies
aschut 6:464d2fdfd7de 307 void servowait1(void)
aschut 6:464d2fdfd7de 308 {
aschut 6:464d2fdfd7de 309 double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in
aschut 6:464d2fdfd7de 310 myservo1 = true;
aschut 6:464d2fdfd7de 311 wait(Pulslength1);
aschut 6:464d2fdfd7de 312 myservo1 = false;
aschut 6:464d2fdfd7de 313 }
aschut 6:464d2fdfd7de 314
aschut 6:464d2fdfd7de 315 void servowait2(void)
aschut 6:464d2fdfd7de 316 {
aschut 6:464d2fdfd7de 317 double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden
aschut 6:464d2fdfd7de 318 myservo2 = true;
aschut 6:464d2fdfd7de 319 wait(Pulslength2);
aschut 6:464d2fdfd7de 320 myservo2 = false;
aschut 6:464d2fdfd7de 321 }
aschut 6:464d2fdfd7de 322
aschut 6:464d2fdfd7de 323 void servowait3(void)
aschut 6:464d2fdfd7de 324 {
aschut 6:464d2fdfd7de 325 double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden
aschut 6:464d2fdfd7de 326 myservo3 = true;
aschut 6:464d2fdfd7de 327 wait(Pulslength3);
aschut 6:464d2fdfd7de 328 myservo3 = false;
aschut 6:464d2fdfd7de 329 }
aschut 6:464d2fdfd7de 330
aschut 4:babe09a69296 331 void ServoPeriod()
aschut 4:babe09a69296 332 {
aschut 6:464d2fdfd7de 333 servowait1();
aschut 6:464d2fdfd7de 334 servowait2();
aschut 6:464d2fdfd7de 335 servowait3();
aschut 4:babe09a69296 336 }
aschut 2:7c9974f0947a 337
aschut 6:464d2fdfd7de 338
aschut 6:464d2fdfd7de 339
aschut 2:7c9974f0947a 340 void ContinuousReader(void)
aschut 2:7c9974f0947a 341 {
aschut 2:7c9974f0947a 342 Pot2 = pot.read();
aschut 2:7c9974f0947a 343 Pot1 = pot0.read();
aschut 2:7c9974f0947a 344 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 2:7c9974f0947a 345 pwm1 =(Pot1*2)-1;
aschut 2:7c9974f0947a 346 }
aschut 2:7c9974f0947a 347
aschut 2:7c9974f0947a 348 // StateMachine
aschut 2:7c9974f0947a 349
aschut 6:464d2fdfd7de 350 enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
aschut 0:90750f158475 351 int f = 1;
aschut 6:464d2fdfd7de 352 states currentState = MOTORS_OFF;
aschut 0:90750f158475 353 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 354
aschut 0:90750f158475 355 void ProcessStateMachine(void)
aschut 0:90750f158475 356 {
aschut 6:464d2fdfd7de 357 switch (currentState) {
aschut 6:464d2fdfd7de 358 case MOTORS_OFF:
aschut 6:464d2fdfd7de 359 // Actions
aschut 6:464d2fdfd7de 360 if (stateChanged) {
aschut 6:464d2fdfd7de 361 // state initialization: rood
aschut 6:464d2fdfd7de 362
aschut 6:464d2fdfd7de 363 led1 = 1;
aschut 6:464d2fdfd7de 364 led2 = 0;
aschut 6:464d2fdfd7de 365 led3 = 1;
aschut 6:464d2fdfd7de 366 wait (1);
aschut 6:464d2fdfd7de 367 stateChanged = false;
aschut 6:464d2fdfd7de 368 }
aschut 2:7c9974f0947a 369
aschut 6:464d2fdfd7de 370 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 6:464d2fdfd7de 371 if (!button3) {
aschut 6:464d2fdfd7de 372 currentState = CALIBRATION ;
aschut 6:464d2fdfd7de 373 stateChanged = true;
aschut 6:464d2fdfd7de 374 } else {
aschut 6:464d2fdfd7de 375 currentState = MOTORS_OFF;
aschut 6:464d2fdfd7de 376 stateChanged = true;
aschut 6:464d2fdfd7de 377 }
aschut 6:464d2fdfd7de 378
aschut 6:464d2fdfd7de 379 break;
aschut 6:464d2fdfd7de 380
aschut 6:464d2fdfd7de 381 case CALIBRATION:
aschut 6:464d2fdfd7de 382 // Actions
aschut 6:464d2fdfd7de 383 if (stateChanged) {
aschut 6:464d2fdfd7de 384 // state initialization: oranje
aschut 4:babe09a69296 385 led1 = 0;
aschut 4:babe09a69296 386 led2 = 0;
aschut 4:babe09a69296 387 led3 = 1;
aschut 6:464d2fdfd7de 388
aschut 4:babe09a69296 389 wait(1);
aschut 4:babe09a69296 390 stateChanged = false;
aschut 6:464d2fdfd7de 391
aschut 6:464d2fdfd7de 392 }
aschut 6:464d2fdfd7de 393
aschut 6:464d2fdfd7de 394 // State transition logic: automatisch terug naar motors off.
aschut 6:464d2fdfd7de 395
aschut 6:464d2fdfd7de 396 currentState = HOMING1;
aschut 6:464d2fdfd7de 397 stateChanged = true;
aschut 6:464d2fdfd7de 398 break;
aschut 6:464d2fdfd7de 399
aschut 6:464d2fdfd7de 400 case HOMING1:
aschut 6:464d2fdfd7de 401 // Actions
aschut 6:464d2fdfd7de 402 if (stateChanged) {
aschut 6:464d2fdfd7de 403 // state initialization: green
aschut 6:464d2fdfd7de 404 t.start();
aschut 6:464d2fdfd7de 405 led1 = 0;
aschut 6:464d2fdfd7de 406 led2 = 1;
aschut 6:464d2fdfd7de 407 led3 = 1;
aschut 0:90750f158475 408
aschut 6:464d2fdfd7de 409 if (!But1) {
aschut 6:464d2fdfd7de 410 led1 = 1;
aschut 6:464d2fdfd7de 411 float H1 = 0.99f;
aschut 6:464d2fdfd7de 412 moter1_control(H1);
aschut 6:464d2fdfd7de 413 wait(0.001f);
aschut 6:464d2fdfd7de 414 } else if (!But2) {
aschut 6:464d2fdfd7de 415 led1 = 1;
aschut 6:464d2fdfd7de 416 float H1 = -0.99f;
aschut 6:464d2fdfd7de 417 moter1_control(H1);
aschut 6:464d2fdfd7de 418 wait(0.001f);
aschut 6:464d2fdfd7de 419 }
aschut 6:464d2fdfd7de 420 encoder1.reset();
aschut 6:464d2fdfd7de 421 motor_position1 = 0;
aschut 6:464d2fdfd7de 422 pwmpin1 = 0;
aschut 6:464d2fdfd7de 423 pwmpin2 = 0;
aschut 6:464d2fdfd7de 424 ;
aschut 6:464d2fdfd7de 425
aschut 6:464d2fdfd7de 426 stateChanged = false;
aschut 6:464d2fdfd7de 427 }
aschut 6:464d2fdfd7de 428
aschut 6:464d2fdfd7de 429 // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
aschut 6:464d2fdfd7de 430
aschut 6:464d2fdfd7de 431 if (!button3) {
aschut 6:464d2fdfd7de 432 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 433 stateChanged = true;
aschut 6:464d2fdfd7de 434 wait(1);
aschut 6:464d2fdfd7de 435 } else if (t>300) {
aschut 6:464d2fdfd7de 436 t.stop();
aschut 6:464d2fdfd7de 437 t.reset();
aschut 6:464d2fdfd7de 438 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 439 stateChanged = true;
aschut 6:464d2fdfd7de 440 } else {
aschut 6:464d2fdfd7de 441 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 442 stateChanged = true;
aschut 6:464d2fdfd7de 443 }
aschut 6:464d2fdfd7de 444 break;
aschut 2:7c9974f0947a 445
aschut 6:464d2fdfd7de 446 case HOMING2:
aschut 6:464d2fdfd7de 447 // Actions
aschut 6:464d2fdfd7de 448 if (stateChanged) {
aschut 6:464d2fdfd7de 449 // state initialization: white
aschut 6:464d2fdfd7de 450 t.start();
aschut 6:464d2fdfd7de 451 led1 = 0;
aschut 6:464d2fdfd7de 452 led2 = 0;
aschut 6:464d2fdfd7de 453 led3 = 0;
aschut 6:464d2fdfd7de 454
aschut 6:464d2fdfd7de 455 if (!But1) {
aschut 6:464d2fdfd7de 456 led1 = 1;
aschut 6:464d2fdfd7de 457 float H2 = 0.98f;
aschut 6:464d2fdfd7de 458 moter2_control(H2);
aschut 6:464d2fdfd7de 459 wait(0.001f);
aschut 6:464d2fdfd7de 460 } else if (!But2) {
aschut 6:464d2fdfd7de 461 led1 = 1;
aschut 6:464d2fdfd7de 462 float H2 = -0.98f;
aschut 6:464d2fdfd7de 463 moter2_control(H2);
aschut 6:464d2fdfd7de 464 wait(0.001f);
aschut 6:464d2fdfd7de 465 }
aschut 6:464d2fdfd7de 466 encoder2.reset();
aschut 6:464d2fdfd7de 467 motor_position2 = 0;
aschut 6:464d2fdfd7de 468 pwmpin1 = 0;
aschut 6:464d2fdfd7de 469 pwmpin2 = 0;
aschut 6:464d2fdfd7de 470 ;
aschut 6:464d2fdfd7de 471
aschut 6:464d2fdfd7de 472 stateChanged = false;
aschut 6:464d2fdfd7de 473 }
aschut 6:464d2fdfd7de 474
aschut 6:464d2fdfd7de 475 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
aschut 6:464d2fdfd7de 476 if (!button2) {
aschut 6:464d2fdfd7de 477 currentState = DEMO;
aschut 6:464d2fdfd7de 478 stateChanged = true;
aschut 6:464d2fdfd7de 479 } else if (!button3) {
aschut 6:464d2fdfd7de 480 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 481 stateChanged = true;
aschut 6:464d2fdfd7de 482
aschut 6:464d2fdfd7de 483 } else if (t>300) {
aschut 6:464d2fdfd7de 484 t.stop();
aschut 6:464d2fdfd7de 485 t.reset();
aschut 6:464d2fdfd7de 486 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 487 stateChanged = true;
aschut 6:464d2fdfd7de 488 } else {
aschut 6:464d2fdfd7de 489 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 490 stateChanged = true;
aschut 6:464d2fdfd7de 491 }
aschut 6:464d2fdfd7de 492 break;
aschut 6:464d2fdfd7de 493
aschut 6:464d2fdfd7de 494 case DEMO:
aschut 6:464d2fdfd7de 495 // Actions
aschut 6:464d2fdfd7de 496 if (stateChanged) {
aschut 6:464d2fdfd7de 497 // state initialization: light blue
aschut 6:464d2fdfd7de 498 led1 = 0;
aschut 6:464d2fdfd7de 499 led2 = 1;
aschut 6:464d2fdfd7de 500 led3 = 0;
aschut 6:464d2fdfd7de 501
aschut 6:464d2fdfd7de 502 servo_position1 = 0.5;
aschut 6:464d2fdfd7de 503 servo_position2 = 0.5;
aschut 6:464d2fdfd7de 504 servo_position3 = 0.5;
aschut 6:464d2fdfd7de 505 ServoTick.attach(&ServoPeriod, Periodlength);
aschut 2:7c9974f0947a 506
aschut 6:464d2fdfd7de 507 wait(1.5 );
aschut 6:464d2fdfd7de 508 servo_position1 = 0.9;
aschut 6:464d2fdfd7de 509 servo_position2 = 0.9;
aschut 6:464d2fdfd7de 510 servo_position3 = 0.9;
aschut 6:464d2fdfd7de 511 wait(1.5);
aschut 6:464d2fdfd7de 512 servo_position1 = 0.1;
aschut 6:464d2fdfd7de 513 servo_position2 = 0.1;
aschut 6:464d2fdfd7de 514 servo_position3 = 0.1;
aschut 6:464d2fdfd7de 515 wait(1);
aschut 6:464d2fdfd7de 516
aschut 6:464d2fdfd7de 517 ServoTick.detach();
aschut 6:464d2fdfd7de 518
aschut 6:464d2fdfd7de 519
aschut 6:464d2fdfd7de 520 wait (1);
aschut 6:464d2fdfd7de 521
aschut 6:464d2fdfd7de 522 stateChanged = false;
aschut 6:464d2fdfd7de 523 }
aschut 6:464d2fdfd7de 524
aschut 6:464d2fdfd7de 525 // State transition logic: automatisch terug naar HOMING
aschut 6:464d2fdfd7de 526 currentState = HOMING2;
aschut 6:464d2fdfd7de 527 stateChanged = true;
aschut 6:464d2fdfd7de 528 break;
aschut 6:464d2fdfd7de 529
aschut 6:464d2fdfd7de 530 case MOVEMENT:
aschut 6:464d2fdfd7de 531 // Actions
aschut 6:464d2fdfd7de 532
aschut 6:464d2fdfd7de 533 if (stateChanged) {
aschut 6:464d2fdfd7de 534 // state initialization: purple
aschut 6:464d2fdfd7de 535 t.start(); // na 5 minuten terug naar Homing
aschut 6:464d2fdfd7de 536 led1 = 1;
aschut 6:464d2fdfd7de 537 led2 = 0;
aschut 6:464d2fdfd7de 538 led3 = 0;
aschut 6:464d2fdfd7de 539
aschut 6:464d2fdfd7de 540 // Tickers aan
aschut 6:464d2fdfd7de 541 if (counts == 0) {
aschut 6:464d2fdfd7de 542 pwmpin1 = 0;
aschut 6:464d2fdfd7de 543 pwmpin2 = 0;
aschut 6:464d2fdfd7de 544 Input1 = pwm1;
aschut 6:464d2fdfd7de 545 Input2 = pwm2;
aschut 6:464d2fdfd7de 546 Pwm.attach (PwmMotor, Ts);
aschut 6:464d2fdfd7de 547
aschut 6:464d2fdfd7de 548 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 549 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 550 servo_position3 = Wijsvinger_krom;
aschut 6:464d2fdfd7de 551
aschut 6:464d2fdfd7de 552 ServoTick.attach(&ServoPeriod, Periodlength);
aschut 6:464d2fdfd7de 553 counts++;
aschut 6:464d2fdfd7de 554 wait(1);
aschut 6:464d2fdfd7de 555 }
aschut 6:464d2fdfd7de 556
aschut 6:464d2fdfd7de 557 // Servo positie
aschut 6:464d2fdfd7de 558
aschut 6:464d2fdfd7de 559 if (!But1) {
aschut 6:464d2fdfd7de 560 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 561 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 562 servo_position3 = Wijsvinger_krom;
aschut 6:464d2fdfd7de 563 led1 = !led1;
aschut 6:464d2fdfd7de 564 }
aschut 6:464d2fdfd7de 565 if (!But2) {
aschut 6:464d2fdfd7de 566 servo_position1 = Duim_recht;
aschut 6:464d2fdfd7de 567 servo_position2 = MWP_recht;
aschut 6:464d2fdfd7de 568 servo_position3 = Wijsvinger_recht;
aschut 6:464d2fdfd7de 569 led1 = !led1;
aschut 6:464d2fdfd7de 570 }
aschut 3:2aaf54ce090b 571
aschut 6:464d2fdfd7de 572 /*
aschut 6:464d2fdfd7de 573
aschut 6:464d2fdfd7de 574 // printen
aschut 6:464d2fdfd7de 575 if(count==500)
aschut 6:464d2fdfd7de 576 {
aschut 6:464d2fdfd7de 577 float tmp = t2.read();
aschut 6:464d2fdfd7de 578 pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference);
aschut 6:464d2fdfd7de 579 count = 0;
aschut 6:464d2fdfd7de 580 }
aschut 6:464d2fdfd7de 581 count++;
aschut 6:464d2fdfd7de 582
aschut 6:464d2fdfd7de 583
aschut 6:464d2fdfd7de 584 */
aschut 3:2aaf54ce090b 585
aschut 6:464d2fdfd7de 586 stateChanged = false;
aschut 6:464d2fdfd7de 587 }
aschut 6:464d2fdfd7de 588
aschut 6:464d2fdfd7de 589 // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
aschut 6:464d2fdfd7de 590 if (!button2) {
aschut 6:464d2fdfd7de 591 currentState = FREEZE;
aschut 6:464d2fdfd7de 592 stateChanged = true;
aschut 6:464d2fdfd7de 593 } else if (!button3) {
aschut 6:464d2fdfd7de 594 Pwm.detach ();
aschut 6:464d2fdfd7de 595 ServoTick.detach();
aschut 6:464d2fdfd7de 596 pwmpin2 = 0;
aschut 6:464d2fdfd7de 597 pwmpin1 = 0;
aschut 6:464d2fdfd7de 598 counts = 0;
aschut 6:464d2fdfd7de 599 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 600 stateChanged = true;
aschut 6:464d2fdfd7de 601 } else if (t>300) {
aschut 6:464d2fdfd7de 602 t.stop();
aschut 6:464d2fdfd7de 603 t.reset();
aschut 6:464d2fdfd7de 604 Pwm.detach ();
aschut 6:464d2fdfd7de 605 ServoTick.detach();
aschut 6:464d2fdfd7de 606 counts = 0;
aschut 6:464d2fdfd7de 607 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 608 stateChanged = true;
aschut 6:464d2fdfd7de 609 } else {
aschut 6:464d2fdfd7de 610 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 611 stateChanged = true;
aschut 6:464d2fdfd7de 612 }
aschut 6:464d2fdfd7de 613 break;
aschut 6:464d2fdfd7de 614
aschut 2:7c9974f0947a 615 case FREEZE:
aschut 6:464d2fdfd7de 616 // Actions
aschut 6:464d2fdfd7de 617 if (stateChanged) {
aschut 6:464d2fdfd7de 618 // state initialization: blue
aschut 6:464d2fdfd7de 619 led1 = 1;
aschut 6:464d2fdfd7de 620 led2 = 1;
aschut 6:464d2fdfd7de 621 led3 = 0;
aschut 6:464d2fdfd7de 622
aschut 6:464d2fdfd7de 623 wait (1);
aschut 0:90750f158475 624
aschut 6:464d2fdfd7de 625 stateChanged = false;
aschut 6:464d2fdfd7de 626 }
aschut 6:464d2fdfd7de 627
aschut 6:464d2fdfd7de 628 // State transition logic: automatisch terug naar MOVEMENT.
aschut 6:464d2fdfd7de 629
aschut 6:464d2fdfd7de 630 currentState = MOVEMENT;
aschut 6:464d2fdfd7de 631 stateChanged = true;
aschut 6:464d2fdfd7de 632 break;
aschut 6:464d2fdfd7de 633
aschut 6:464d2fdfd7de 634 }
aschut 0:90750f158475 635 }
aschut 6:464d2fdfd7de 636
aschut 0:90750f158475 637 int main()
aschut 0:90750f158475 638 {
aschut 6:464d2fdfd7de 639
aschut 2:7c9974f0947a 640 t2.start();
aschut 2:7c9974f0947a 641 int counter = 0;
aschut 6:464d2fdfd7de 642 pwmpin1.period_us(60);
aschut 4:babe09a69296 643 //PotRead.attach(ContinuousReader,Ts);
aschut 2:7c9974f0947a 644 pc.baud(115200);
aschut 2:7c9974f0947a 645
aschut 6:464d2fdfd7de 646 while(true) {
aschut 6:464d2fdfd7de 647 led1 = 1;
aschut 6:464d2fdfd7de 648 led2 =1;
aschut 6:464d2fdfd7de 649 led3 =1;
aschut 6:464d2fdfd7de 650 /*
aschut 6:464d2fdfd7de 651 if(counter==10)
aschut 6:464d2fdfd7de 652 {
aschut 6:464d2fdfd7de 653 float tmp = t2.read();
aschut 6:464d2fdfd7de 654 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
aschut 6:464d2fdfd7de 655 counter = 0;
aschut 6:464d2fdfd7de 656 }
aschut 6:464d2fdfd7de 657 counter++;
aschut 6:464d2fdfd7de 658 */
aschut 6:464d2fdfd7de 659 ProcessStateMachine();
aschut 6:464d2fdfd7de 660
aschut 6:464d2fdfd7de 661
aschut 6:464d2fdfd7de 662 wait(0.001);
aschut 0:90750f158475 663 }
aschut 6:464d2fdfd7de 664 }
aschut 0:90750f158475 665
aschut 0:90750f158475 666
aschut 0:90750f158475 667
aschut 0:90750f158475 668
aschut 0:90750f158475 669
aschut 0:90750f158475 670
aschut 2:7c9974f0947a 671
aschut 6:464d2fdfd7de 672
aschut 6:464d2fdfd7de 673
aschut 6:464d2fdfd7de 674
aschut 6:464d2fdfd7de 675
aschut 6:464d2fdfd7de 676
aschut 6:464d2fdfd7de 677