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

Committer:
aschut
Date:
Fri Apr 05 13:11:44 2019 +0000
Revision:
5:ef77da99d0d1
Parent:
4:babe09a69296
Child:
6:464d2fdfd7de
nu ook werkende

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 5:ef77da99d0d1 252 //Continues Read
aschut 5:ef77da99d0d1 253 Pot2 = pot.read();
aschut 5:ef77da99d0d1 254 Pot1 = pot0.read();
aschut 5:ef77da99d0d1 255 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 5:ef77da99d0d1 256 pwm1 =(Pot1*2)-1;
aschut 5:ef77da99d0d1 257 Input1 = pwm1;
aschut 5:ef77da99d0d1 258 Input2 = pwm2;
aschut 5:ef77da99d0d1 259
aschut 3:2aaf54ce090b 260 float Polshoek1 = Kinematics1(Input1);
aschut 3:2aaf54ce090b 261 float Polshoek4 = Limits1(Polshoek1);
aschut 3:2aaf54ce090b 262 PolsReference = Polshoek4;
aschut 3:2aaf54ce090b 263
aschut 2:7c9974f0947a 264 // Reference hoek berekenen, in graden
aschut 3:2aaf54ce090b 265 float Ellebooghoek1 = Kinematics2(Input2);
aschut 2:7c9974f0947a 266 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 2:7c9974f0947a 267 ElbowReference = Ellebooghoek4;
aschut 2:7c9974f0947a 268
aschut 2:7c9974f0947a 269 // Positie motor berekenen, in graden
aschut 2:7c9974f0947a 270 Pulses1 = encoder1.getPulses();
aschut 2:7c9974f0947a 271 motor_position1 = -(Pulses1/1200)*360;
aschut 2:7c9974f0947a 272 Pulses2 = encoder2.getPulses();
aschut 2:7c9974f0947a 273 motor_position2 = -(Pulses2/4800)*360;
aschut 2:7c9974f0947a 274
aschut 2:7c9974f0947a 275 double error1 = PolsReference - motor_position1;
aschut 2:7c9974f0947a 276 double u1 = PID_controller1(error1);
aschut 2:7c9974f0947a 277 moter1_control(u1);
aschut 2:7c9974f0947a 278
aschut 2:7c9974f0947a 279 double error2 = ElbowReference - motor_position2;
aschut 2:7c9974f0947a 280 double u2 = PID_controller2(error2);
aschut 2:7c9974f0947a 281 moter2_control(u2);
aschut 2:7c9974f0947a 282
aschut 2:7c9974f0947a 283 }
aschut 2:7c9974f0947a 284
aschut 2:7c9974f0947a 285 void MotorOn(void)
aschut 2:7c9974f0947a 286 {
aschut 2:7c9974f0947a 287 pwmpin1 = 0;
aschut 2:7c9974f0947a 288 pwmpin2 = 0;
aschut 2:7c9974f0947a 289 Pwm.attach (PwmMotor, Ts);
aschut 2:7c9974f0947a 290
aschut 2:7c9974f0947a 291 }
aschut 2:7c9974f0947a 292
aschut 4:babe09a69296 293 //Servo functie
aschut 4:babe09a69296 294 void ServoPeriod()
aschut 4:babe09a69296 295 {
aschut 4:babe09a69296 296 led1 = 1;
aschut 4:babe09a69296 297 led2 = 1;
aschut 4:babe09a69296 298 double Pulslength = 0.0005 + (servo_position * 0.002); //in seconden
aschut 4:babe09a69296 299 if (counts <= (Pulslength/Periodlength)) {
aschut 4:babe09a69296 300 myservo1.pulsewidth(0.00006);
aschut 4:babe09a69296 301 counts++;
aschut 4:babe09a69296 302 }
aschut 4:babe09a69296 303 else if (counts <= (0.02/Periodlength)){
aschut 4:babe09a69296 304 myservo1.pulsewidth(0);
aschut 4:babe09a69296 305 counts++;
aschut 4:babe09a69296 306 }
aschut 4:babe09a69296 307 else {
aschut 4:babe09a69296 308 counts = 0;
aschut 4:babe09a69296 309 }
aschut 4:babe09a69296 310 }
aschut 2:7c9974f0947a 311
aschut 2:7c9974f0947a 312 void ContinuousReader(void)
aschut 2:7c9974f0947a 313 {
aschut 2:7c9974f0947a 314 Pot2 = pot.read();
aschut 2:7c9974f0947a 315 Pot1 = pot0.read();
aschut 2:7c9974f0947a 316 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 2:7c9974f0947a 317 pwm1 =(Pot1*2)-1;
aschut 2:7c9974f0947a 318 }
aschut 2:7c9974f0947a 319
aschut 2:7c9974f0947a 320 // StateMachine
aschut 2:7c9974f0947a 321
aschut 2:7c9974f0947a 322 enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
aschut 0:90750f158475 323 int f = 1;
aschut 0:90750f158475 324 states currentState = MOTORS_OFF;
aschut 0:90750f158475 325 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 326
aschut 0:90750f158475 327 void ProcessStateMachine(void)
aschut 0:90750f158475 328 {
aschut 0:90750f158475 329 switch (currentState)
aschut 0:90750f158475 330 {
aschut 0:90750f158475 331 case MOTORS_OFF:
aschut 0:90750f158475 332 // Actions
aschut 0:90750f158475 333 if (stateChanged)
aschut 0:90750f158475 334 {
aschut 0:90750f158475 335 // state initialization: rood
aschut 2:7c9974f0947a 336
aschut 0:90750f158475 337 led1 = 1;
aschut 0:90750f158475 338 led2 = 0;
aschut 0:90750f158475 339 led3 = 1;
aschut 0:90750f158475 340 wait (1);
aschut 0:90750f158475 341 stateChanged = false;
aschut 0:90750f158475 342 }
aschut 0:90750f158475 343
aschut 0:90750f158475 344 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 2:7c9974f0947a 345 if (!button3)
aschut 0:90750f158475 346 {
aschut 2:7c9974f0947a 347 currentState = CALIBRATION ;
aschut 0:90750f158475 348 stateChanged = true;
aschut 0:90750f158475 349 }
aschut 0:90750f158475 350 else
aschut 0:90750f158475 351 {
aschut 0:90750f158475 352 currentState = MOTORS_OFF;
aschut 0:90750f158475 353 stateChanged = true;
aschut 0:90750f158475 354 }
aschut 0:90750f158475 355
aschut 0:90750f158475 356 break;
aschut 0:90750f158475 357
aschut 0:90750f158475 358 case CALIBRATION:
aschut 0:90750f158475 359 // Actions
aschut 0:90750f158475 360 if (stateChanged)
aschut 0:90750f158475 361 {
aschut 0:90750f158475 362 // state initialization: oranje
aschut 4:babe09a69296 363 led1 = 0;
aschut 4:babe09a69296 364 led2 = 0;
aschut 4:babe09a69296 365 led3 = 1;
aschut 4:babe09a69296 366
aschut 4:babe09a69296 367 servo_position = 0.5;
aschut 4:babe09a69296 368 ServoTick.attach(ServoPeriod, Periodlength);
aschut 4:babe09a69296 369 wait(1);
aschut 4:babe09a69296 370 servo_position = 0.9;
aschut 4:babe09a69296 371 wait(1);
aschut 4:babe09a69296 372 servo_position = 0.5;
aschut 4:babe09a69296 373 wait(1);
aschut 4:babe09a69296 374 servo_position = 0.1;
aschut 4:babe09a69296 375 wait (1);
aschut 4:babe09a69296 376 stateChanged = false;
aschut 5:ef77da99d0d1 377 ServoTick.detach();
aschut 0:90750f158475 378 }
aschut 0:90750f158475 379
aschut 0:90750f158475 380 // State transition logic: automatisch terug naar motors off.
aschut 0:90750f158475 381
aschut 2:7c9974f0947a 382 currentState = HOMING1;
aschut 2:7c9974f0947a 383 stateChanged = true;
aschut 0:90750f158475 384 break;
aschut 2:7c9974f0947a 385
aschut 2:7c9974f0947a 386 case HOMING1:
aschut 0:90750f158475 387 // Actions
aschut 0:90750f158475 388 if (stateChanged)
aschut 0:90750f158475 389 {
aschut 0:90750f158475 390 // state initialization: green
aschut 0:90750f158475 391 t.start();
aschut 0:90750f158475 392 led1 = 0;
aschut 0:90750f158475 393 led2 = 1;
aschut 0:90750f158475 394 led3 = 1;
aschut 2:7c9974f0947a 395
aschut 2:7c9974f0947a 396 if (!But1)
aschut 2:7c9974f0947a 397 {
aschut 2:7c9974f0947a 398 led1 = 1;
aschut 3:2aaf54ce090b 399 float H1 = 0.7f;
aschut 2:7c9974f0947a 400 moter1_control(H1);
aschut 2:7c9974f0947a 401 wait(0.001f);
aschut 2:7c9974f0947a 402 }
aschut 2:7c9974f0947a 403 else if (!But2)
aschut 2:7c9974f0947a 404 {
aschut 2:7c9974f0947a 405 led1 = 1;
aschut 3:2aaf54ce090b 406 float H1 = -0.7f;
aschut 2:7c9974f0947a 407 moter1_control(H1);
aschut 2:7c9974f0947a 408 wait(0.001f);
aschut 2:7c9974f0947a 409 }
aschut 3:2aaf54ce090b 410 encoder1.reset();
aschut 3:2aaf54ce090b 411 motor_position1 = 0;
aschut 2:7c9974f0947a 412 pwmpin1 = 0;
aschut 2:7c9974f0947a 413 pwmpin2 = 0;
aschut 2:7c9974f0947a 414 ;
aschut 0:90750f158475 415
aschut 0:90750f158475 416 stateChanged = false;
aschut 0:90750f158475 417 }
aschut 0:90750f158475 418
aschut 3:2aaf54ce090b 419 // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
aschut 2:7c9974f0947a 420
aschut 2:7c9974f0947a 421 if (!button3)
aschut 0:90750f158475 422 {
aschut 2:7c9974f0947a 423 currentState = HOMING2 ;
aschut 0:90750f158475 424 stateChanged = true;
aschut 2:7c9974f0947a 425 wait(1);
aschut 0:90750f158475 426 }
aschut 0:90750f158475 427 else if (t>300)
aschut 0:90750f158475 428 {
aschut 0:90750f158475 429 t.stop();
aschut 0:90750f158475 430 t.reset();
aschut 0:90750f158475 431 currentState = MOTORS_OFF ;
aschut 0:90750f158475 432 stateChanged = true;
aschut 0:90750f158475 433 }
aschut 0:90750f158475 434 else
aschut 0:90750f158475 435 {
aschut 2:7c9974f0947a 436 currentState = HOMING1 ;
aschut 2:7c9974f0947a 437 stateChanged = true;
aschut 2:7c9974f0947a 438 }
aschut 2:7c9974f0947a 439 break;
aschut 2:7c9974f0947a 440
aschut 2:7c9974f0947a 441 case HOMING2:
aschut 2:7c9974f0947a 442 // Actions
aschut 2:7c9974f0947a 443 if (stateChanged)
aschut 2:7c9974f0947a 444 {
aschut 3:2aaf54ce090b 445 // state initialization: white
aschut 2:7c9974f0947a 446 t.start();
aschut 2:7c9974f0947a 447 led1 = 0;
aschut 3:2aaf54ce090b 448 led2 = 0;
aschut 3:2aaf54ce090b 449 led3 = 0;
aschut 2:7c9974f0947a 450
aschut 2:7c9974f0947a 451 if (!But1)
aschut 2:7c9974f0947a 452 {
aschut 2:7c9974f0947a 453 led1 = 1;
aschut 2:7c9974f0947a 454 float H2 = 0.98f;
aschut 2:7c9974f0947a 455 moter2_control(H2);
aschut 2:7c9974f0947a 456 wait(0.001f);
aschut 2:7c9974f0947a 457 }
aschut 2:7c9974f0947a 458 else if (!But2)
aschut 2:7c9974f0947a 459 {
aschut 2:7c9974f0947a 460 led1 = 1;
aschut 2:7c9974f0947a 461 float H2 = -0.98f;
aschut 2:7c9974f0947a 462 moter2_control(H2);
aschut 2:7c9974f0947a 463 wait(0.001f);
aschut 2:7c9974f0947a 464 }
aschut 3:2aaf54ce090b 465 encoder2.reset();
aschut 3:2aaf54ce090b 466 motor_position2 = 0;
aschut 2:7c9974f0947a 467 pwmpin1 = 0;
aschut 2:7c9974f0947a 468 pwmpin2 = 0;
aschut 2:7c9974f0947a 469 ;
aschut 2:7c9974f0947a 470
aschut 2:7c9974f0947a 471 stateChanged = false;
aschut 2:7c9974f0947a 472 }
aschut 2:7c9974f0947a 473
aschut 2:7c9974f0947a 474 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
aschut 2:7c9974f0947a 475 if (!button2)
aschut 2:7c9974f0947a 476 {
aschut 2:7c9974f0947a 477 currentState = DEMO;
aschut 2:7c9974f0947a 478 stateChanged = true;
aschut 2:7c9974f0947a 479 }
aschut 2:7c9974f0947a 480 else if (!button3)
aschut 2:7c9974f0947a 481 {
aschut 2:7c9974f0947a 482 currentState = MOVEMENT ;
aschut 2:7c9974f0947a 483 stateChanged = true;
aschut 2:7c9974f0947a 484 led1 = 1;
aschut 2:7c9974f0947a 485 led2 = 0;
aschut 2:7c9974f0947a 486 led3 = 0;
aschut 2:7c9974f0947a 487 wait(1);
aschut 2:7c9974f0947a 488 }
aschut 2:7c9974f0947a 489 else if (t>300)
aschut 2:7c9974f0947a 490 {
aschut 2:7c9974f0947a 491 t.stop();
aschut 2:7c9974f0947a 492 t.reset();
aschut 2:7c9974f0947a 493 currentState = MOTORS_OFF ;
aschut 2:7c9974f0947a 494 stateChanged = true;
aschut 2:7c9974f0947a 495 }
aschut 2:7c9974f0947a 496 else
aschut 2:7c9974f0947a 497 {
aschut 2:7c9974f0947a 498 currentState = HOMING2 ;
aschut 0:90750f158475 499 stateChanged = true;
aschut 0:90750f158475 500 }
aschut 0:90750f158475 501 break;
aschut 0:90750f158475 502
aschut 0:90750f158475 503 case DEMO:
aschut 0:90750f158475 504 // Actions
aschut 0:90750f158475 505 if (stateChanged)
aschut 0:90750f158475 506 {
aschut 0:90750f158475 507 // state initialization: light blue
aschut 0:90750f158475 508 led1 = 0;
aschut 0:90750f158475 509 led2 = 1;
aschut 0:90750f158475 510 led3 = 0;
aschut 0:90750f158475 511 wait (1);
aschut 0:90750f158475 512
aschut 0:90750f158475 513 stateChanged = false;
aschut 0:90750f158475 514 }
aschut 0:90750f158475 515
aschut 0:90750f158475 516 // State transition logic: automatisch terug naar HOMING
aschut 2:7c9974f0947a 517 currentState = HOMING1;
aschut 0:90750f158475 518 stateChanged = true;
aschut 0:90750f158475 519 break;
aschut 0:90750f158475 520
aschut 0:90750f158475 521 case MOVEMENT:
aschut 0:90750f158475 522 // Actions
aschut 0:90750f158475 523 if (stateChanged)
aschut 0:90750f158475 524 {
aschut 0:90750f158475 525 // state initialization: purple
aschut 0:90750f158475 526 t.start();
aschut 3:2aaf54ce090b 527
aschut 2:7c9974f0947a 528 pwmpin1 = 0;
aschut 2:7c9974f0947a 529 pwmpin2 = 0;
aschut 3:2aaf54ce090b 530 Input1 = pwm1;
aschut 3:2aaf54ce090b 531 Input2 = pwm2;
aschut 3:2aaf54ce090b 532
aschut 3:2aaf54ce090b 533 // printen
aschut 3:2aaf54ce090b 534 if(count==500)
aschut 3:2aaf54ce090b 535 {
aschut 3:2aaf54ce090b 536 float tmp = t2.read();
aschut 3:2aaf54ce090b 537 pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference);
aschut 3:2aaf54ce090b 538 count = 0;
aschut 3:2aaf54ce090b 539 }
aschut 3:2aaf54ce090b 540 count++;
aschut 3:2aaf54ce090b 541
aschut 2:7c9974f0947a 542 Pwm.attach (PwmMotor, Ts);
aschut 0:90750f158475 543 led1 = 1;
aschut 0:90750f158475 544 led2 = 0;
aschut 0:90750f158475 545 led3 = 0;
aschut 2:7c9974f0947a 546
aschut 0:90750f158475 547 stateChanged = false;
aschut 0:90750f158475 548 }
aschut 0:90750f158475 549
aschut 2:7c9974f0947a 550 // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
aschut 2:7c9974f0947a 551 if (!button2)
aschut 0:90750f158475 552 {
aschut 2:7c9974f0947a 553 currentState = FREEZE;
aschut 1:070092564648 554 stateChanged = true;
aschut 0:90750f158475 555 }
aschut 2:7c9974f0947a 556 else if (!button3)
aschut 0:90750f158475 557 {
aschut 2:7c9974f0947a 558 Pwm.detach ();
aschut 2:7c9974f0947a 559 pwmpin2 = 0;
aschut 2:7c9974f0947a 560 pwmpin1 = 0;
aschut 0:90750f158475 561 currentState = MOTORS_OFF ;
aschut 0:90750f158475 562 stateChanged = true;
aschut 0:90750f158475 563 }
aschut 0:90750f158475 564 else if (t>300)
aschut 0:90750f158475 565 {
aschut 0:90750f158475 566 t.stop();
aschut 0:90750f158475 567 t.reset();
aschut 2:7c9974f0947a 568 currentState = HOMING1 ;
aschut 0:90750f158475 569 stateChanged = true;
aschut 0:90750f158475 570 }
aschut 0:90750f158475 571 else
aschut 0:90750f158475 572 {
aschut 0:90750f158475 573 currentState = MOVEMENT ;
aschut 0:90750f158475 574 stateChanged = true;
aschut 0:90750f158475 575 }
aschut 0:90750f158475 576 break;
aschut 0:90750f158475 577
aschut 2:7c9974f0947a 578 case FREEZE:
aschut 0:90750f158475 579 // Actions
aschut 0:90750f158475 580 if (stateChanged)
aschut 0:90750f158475 581 {
aschut 0:90750f158475 582 // state initialization: blue
aschut 0:90750f158475 583 led1 = 1;
aschut 0:90750f158475 584 led2 = 1;
aschut 0:90750f158475 585 led3 = 0;
aschut 0:90750f158475 586 wait (1);
aschut 0:90750f158475 587
aschut 0:90750f158475 588 stateChanged = false;
aschut 0:90750f158475 589 }
aschut 0:90750f158475 590
aschut 0:90750f158475 591 // State transition logic: automatisch terug naar MOVEMENT.
aschut 0:90750f158475 592
aschut 0:90750f158475 593 currentState = MOVEMENT;
aschut 0:90750f158475 594 stateChanged = true;
aschut 0:90750f158475 595 break;
aschut 0:90750f158475 596
aschut 0:90750f158475 597 }
aschut 0:90750f158475 598 }
aschut 0:90750f158475 599
aschut 0:90750f158475 600 int main()
aschut 0:90750f158475 601 {
aschut 2:7c9974f0947a 602
aschut 2:7c9974f0947a 603 t2.start();
aschut 2:7c9974f0947a 604 int counter = 0;
aschut 4:babe09a69296 605 myservo1.period_us(60);
aschut 4:babe09a69296 606 //PotRead.attach(ContinuousReader,Ts);
aschut 2:7c9974f0947a 607 pc.baud(115200);
aschut 2:7c9974f0947a 608
aschut 4:babe09a69296 609
aschut 2:7c9974f0947a 610 while(true)
aschut 0:90750f158475 611 {
aschut 0:90750f158475 612 led1 = 1;
aschut 2:7c9974f0947a 613 led2 =1;
aschut 2:7c9974f0947a 614 led3 =1;
aschut 3:2aaf54ce090b 615 /*
aschut 2:7c9974f0947a 616 if(counter==10)
aschut 2:7c9974f0947a 617 {
aschut 2:7c9974f0947a 618 float tmp = t2.read();
aschut 2:7c9974f0947a 619 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
aschut 2:7c9974f0947a 620 counter = 0;
aschut 2:7c9974f0947a 621 }
aschut 2:7c9974f0947a 622 counter++;
aschut 3:2aaf54ce090b 623 */
aschut 0:90750f158475 624 ProcessStateMachine();
aschut 2:7c9974f0947a 625
aschut 0:90750f158475 626
aschut 2:7c9974f0947a 627 wait(0.001);
aschut 2:7c9974f0947a 628 }
aschut 0:90750f158475 629 }
aschut 0:90750f158475 630
aschut 2:7c9974f0947a 631
aschut 2:7c9974f0947a 632
aschut 2:7c9974f0947a 633
aschut 2:7c9974f0947a 634
aschut 2:7c9974f0947a 635
aschut 0:90750f158475 636
aschut 0:90750f158475 637
aschut 0:90750f158475 638
aschut 0:90750f158475 639
aschut 0:90750f158475 640
aschut 0:90750f158475 641
aschut 2:7c9974f0947a 642