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@4:babe09a69296, 2019-04-05 (annotated)

Committer:
aschut
Date:
Fri Apr 05 12:28:22 2019 +0000
Revision:
4:babe09a69296
Parent:
3:2aaf54ce090b
Child:
5:ef77da99d0d1
Servo in de calibratie werkt, maar niet tegelijkertijd met readticker

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