Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@20:0f6a88f29a71, 2019-10-21 (annotated)

Committer:
jobjansen
Date:
Mon Oct 21 12:09:28 2019 +0000
Revision:
20:0f6a88f29a71
Parent:
17:d1acb6888b82
Child:
21:a316452da8cd
PID controller and reference signal added;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
MatthewMaat 13:ec4708dab45d 2 #include "HIDScope.h"
MatthewMaat 17:d1acb6888b82 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
MatthewMaat 17:d1acb6888b82 5 #include "BiQuad.h"
MatthewMaat 8:ec3c634390c7 6 #include "FastPWM.h"
MatthewMaat 2:626688c21b6f 7 #include <iostream>
MatthewMaat 5:cee5f898b350 8 MODSERIAL pc(USBTX, USBRX);
MatthewMaat 13:ec4708dab45d 9 AnalogIn ain2(A2);
MatthewMaat 13:ec4708dab45d 10 AnalogIn ain1(A3);
MatthewMaat 13:ec4708dab45d 11 DigitalOut dir2(D4);
MatthewMaat 13:ec4708dab45d 12 DigitalOut dir1(D7);
MatthewMaat 13:ec4708dab45d 13 //D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
MatthewMaat 13:ec4708dab45d 14 PwmOut motor2_pwm(D5);
MatthewMaat 13:ec4708dab45d 15 PwmOut motor1_pwm(D6);
MatthewMaat 13:ec4708dab45d 16 AnalogIn emg0( A0 );
MatthewMaat 13:ec4708dab45d 17 AnalogIn emg1( A1 );
jobjansen 20:0f6a88f29a71 18 QEI motor1 (D8, D9, NC, 32);
jobjansen 20:0f6a88f29a71 19 QEI motor2 (D12, D13, NC, 32);
MatthewMaat 13:ec4708dab45d 20
MatthewMaat 15:c4799ad02cdc 21 Ticker ticktick;
MatthewMaat 15:c4799ad02cdc 22 Timer state_time;
MatthewMaat 15:c4799ad02cdc 23 Timeout EMG_peak;
MatthewMaat 17:d1acb6888b82 24 Timeout turn;
MatthewMaat 13:ec4708dab45d 25 Ticker sample_timer;
MatthewMaat 17:d1acb6888b82 26 HIDScope scope( 2);
MatthewMaat 14:dc89250ebc52 27 DigitalOut ledred(LED_RED);
MatthewMaat 14:dc89250ebc52 28 DigitalOut ledblue(LED_BLUE);
MatthewMaat 14:dc89250ebc52 29 DigitalOut ledgreen(LED_GREEN);
MatthewMaat 15:c4799ad02cdc 30 InterruptIn err(SW2);
MatthewMaat 15:c4799ad02cdc 31 InterruptIn button(SW3);
MatthewMaat 15:c4799ad02cdc 32
MatthewMaat 15:c4799ad02cdc 33 volatile float P0;
MatthewMaat 15:c4799ad02cdc 34 volatile float P1;
MatthewMaat 15:c4799ad02cdc 35 volatile float EMG_min0=1;
MatthewMaat 15:c4799ad02cdc 36 volatile float EMG_max0=0;
MatthewMaat 15:c4799ad02cdc 37 volatile float EMG_min1=1;
MatthewMaat 15:c4799ad02cdc 38 volatile float EMG_max1=0;
MatthewMaat 15:c4799ad02cdc 39 volatile bool ignore_peaks=false;
MatthewMaat 16:2d115aa2773e 40 volatile bool ignore_turn=true;
MatthewMaat 14:dc89250ebc52 41 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
MatthewMaat 15:c4799ad02cdc 42 states currentState=Waiting;
MatthewMaat 10:e1eb73e19540 43
jobjansen 20:0f6a88f29a71 44 static float v_refx; //reference speeds
jobjansen 20:0f6a88f29a71 45 static float v_refy;
jobjansen 20:0f6a88f29a71 46 static float T = 0.002; //measuring period
jobjansen 20:0f6a88f29a71 47 static float L = 0.35; //distance between the motor axles, has to be checked
jobjansen 20:0f6a88f29a71 48 volatile float theta1_old = 0.0; //feedback variables
jobjansen 20:0f6a88f29a71 49 volatile float theta2_old = 0.0;
jobjansen 20:0f6a88f29a71 50
jobjansen 20:0f6a88f29a71 51 int m1_count;
jobjansen 20:0f6a88f29a71 52 float m1_angle;
jobjansen 20:0f6a88f29a71 53 int m2_count;
jobjansen 20:0f6a88f29a71 54 float m2_angle;
jobjansen 20:0f6a88f29a71 55 float pi = 3.14;
jobjansen 20:0f6a88f29a71 56
jobjansen 20:0f6a88f29a71 57 float error_x;
jobjansen 20:0f6a88f29a71 58 float error_y;
jobjansen 20:0f6a88f29a71 59
jobjansen 20:0f6a88f29a71 60 //PID controller variables
jobjansen 20:0f6a88f29a71 61 bool q = true;
jobjansen 20:0f6a88f29a71 62 float error_prev;
jobjansen 20:0f6a88f29a71 63 static float Kp = 8;
jobjansen 20:0f6a88f29a71 64 static float Ki = 1.02;
jobjansen 20:0f6a88f29a71 65 static float Kd = 0.1;
jobjansen 20:0f6a88f29a71 66 float u;
jobjansen 20:0f6a88f29a71 67 float u_p;
jobjansen 20:0f6a88f29a71 68 float u_i;
jobjansen 20:0f6a88f29a71 69 float u_d;
jobjansen 20:0f6a88f29a71 70 float error_integral = 0.0;
jobjansen 20:0f6a88f29a71 71
jobjansen 20:0f6a88f29a71 72 float u_1;
jobjansen 20:0f6a88f29a71 73 float u_2;
MatthewMaat 14:dc89250ebc52 74
MatthewMaat 14:dc89250ebc52 75 void read_emg()
MatthewMaat 13:ec4708dab45d 76 {
MatthewMaat 15:c4799ad02cdc 77 //EMG signal 0
MatthewMaat 15:c4799ad02cdc 78 static int count0=0;
MatthewMaat 15:c4799ad02cdc 79 static float RMS_value0=0;
MatthewMaat 15:c4799ad02cdc 80 static float HighPass_value0=0;
MatthewMaat 15:c4799ad02cdc 81 count0+=1;
MatthewMaat 15:c4799ad02cdc 82 static float RMS0[150];
MatthewMaat 15:c4799ad02cdc 83 static float HighPass0[30];
MatthewMaat 17:d1acb6888b82 84 static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 17:d1acb6888b82 85 static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 15:c4799ad02cdc 86 float I0;
MatthewMaat 15:c4799ad02cdc 87 float If0;
MatthewMaat 15:c4799ad02cdc 88 //signal 1
MatthewMaat 15:c4799ad02cdc 89 static int count1=0;
MatthewMaat 15:c4799ad02cdc 90 static float RMS_value1=0;
MatthewMaat 15:c4799ad02cdc 91 static float HighPass_value1=0;
MatthewMaat 15:c4799ad02cdc 92 count1+=1;
MatthewMaat 15:c4799ad02cdc 93 static float RMS1[150];
MatthewMaat 15:c4799ad02cdc 94 static float HighPass1[30];
MatthewMaat 13:ec4708dab45d 95 float I1;
MatthewMaat 15:c4799ad02cdc 96 float If1;
MatthewMaat 15:c4799ad02cdc 97 I0=emg0.read(); //read signal
MatthewMaat 17:d1acb6888b82 98 double notched0=Notch0.step(I0);
MatthewMaat 17:d1acb6888b82 99 HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
MatthewMaat 17:d1acb6888b82 100 HighPass0[count0%30]=notched0;
MatthewMaat 15:c4799ad02cdc 101 If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 102 RMS_value0+=(If0-RMS0[count0%150])/150.0;
MatthewMaat 15:c4799ad02cdc 103 RMS0[count0%150]=If0;
MatthewMaat 13:ec4708dab45d 104 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 105 P0=sqrt(RMS_value0);
MatthewMaat 15:c4799ad02cdc 106 I1=emg1.read(); //read signal
MatthewMaat 17:d1acb6888b82 107 double notched1=Notch1.step(I1);
MatthewMaat 17:d1acb6888b82 108 HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
MatthewMaat 17:d1acb6888b82 109 HighPass1[count1%30]=notched1;
MatthewMaat 15:c4799ad02cdc 110 If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 111 RMS_value1+=(If1-RMS1[count1%150])/150.0;
MatthewMaat 15:c4799ad02cdc 112 RMS1[count1%150]=If1;
MatthewMaat 15:c4799ad02cdc 113 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 114 P1=sqrt(RMS_value1);
MatthewMaat 15:c4799ad02cdc 115 scope.set(0, P0 ); // send root mean squared
MatthewMaat 17:d1acb6888b82 116 scope.set(1, notched0);
MatthewMaat 13:ec4708dab45d 117 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 118 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 119 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 120 scope.send();
MatthewMaat 13:ec4708dab45d 121 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 122 ledred=1;
MatthewMaat 14:dc89250ebc52 123 ledgreen=0;
MatthewMaat 14:dc89250ebc52 124 ledblue=1;
MatthewMaat 13:ec4708dab45d 125 }
MatthewMaat 14:dc89250ebc52 126
MatthewMaat 15:c4799ad02cdc 127 void record_min_max(void)
MatthewMaat 15:c4799ad02cdc 128 {
MatthewMaat 15:c4799ad02cdc 129 float t=state_time.read();
MatthewMaat 15:c4799ad02cdc 130 if(t>0.4)
MatthewMaat 15:c4799ad02cdc 131 {
MatthewMaat 15:c4799ad02cdc 132 if(P0<EMG_min0)
MatthewMaat 15:c4799ad02cdc 133 {
MatthewMaat 15:c4799ad02cdc 134 EMG_min0=P0;
MatthewMaat 15:c4799ad02cdc 135 }
MatthewMaat 15:c4799ad02cdc 136 else if(P0>EMG_max0)
MatthewMaat 15:c4799ad02cdc 137 {
MatthewMaat 15:c4799ad02cdc 138 EMG_max0=P0;
MatthewMaat 15:c4799ad02cdc 139 }
MatthewMaat 15:c4799ad02cdc 140 if(P1<EMG_min1)
MatthewMaat 15:c4799ad02cdc 141 {
MatthewMaat 15:c4799ad02cdc 142 EMG_min1=P1;
MatthewMaat 15:c4799ad02cdc 143 }
MatthewMaat 15:c4799ad02cdc 144 else if(P1>EMG_max1)
MatthewMaat 15:c4799ad02cdc 145 {
MatthewMaat 15:c4799ad02cdc 146 EMG_max1=P1;
MatthewMaat 15:c4799ad02cdc 147 }
MatthewMaat 15:c4799ad02cdc 148 }
MatthewMaat 15:c4799ad02cdc 149 }
MatthewMaat 15:c4799ad02cdc 150
MatthewMaat 15:c4799ad02cdc 151 void unignore_peaks(void)
MatthewMaat 15:c4799ad02cdc 152 {
MatthewMaat 15:c4799ad02cdc 153 ignore_peaks=false;
MatthewMaat 15:c4799ad02cdc 154 }
MatthewMaat 16:2d115aa2773e 155 void start_ignore_turn(void)
MatthewMaat 16:2d115aa2773e 156 {
MatthewMaat 16:2d115aa2773e 157 ignore_turn=true;
MatthewMaat 16:2d115aa2773e 158 }
MatthewMaat 15:c4799ad02cdc 159
MatthewMaat 14:dc89250ebc52 160 void set_PWM(void)
MatthewMaat 11:de4a85703169 161 {
MatthewMaat 15:c4799ad02cdc 162 static bool motor_on=false;
MatthewMaat 15:c4799ad02cdc 163 float Q0;
MatthewMaat 15:c4799ad02cdc 164 Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
MatthewMaat 17:d1acb6888b82 165 if (Q0>-0.2f && !ignore_peaks)
MatthewMaat 13:ec4708dab45d 166 {
MatthewMaat 17:d1acb6888b82 167 if (motor_on)
MatthewMaat 15:c4799ad02cdc 168 {
MatthewMaat 17:d1acb6888b82 169 motor1_pwm.write(0.0f);
MatthewMaat 17:d1acb6888b82 170 EMG_peak.attach(unignore_peaks,0.8);
MatthewMaat 17:d1acb6888b82 171 turn.attach(start_ignore_turn,1);
MatthewMaat 17:d1acb6888b82 172 ignore_turn=false;
MatthewMaat 15:c4799ad02cdc 173 ignore_peaks=true;
MatthewMaat 17:d1acb6888b82 174 motor_on=false;
MatthewMaat 15:c4799ad02cdc 175 }
MatthewMaat 16:2d115aa2773e 176 else if(ignore_turn)
MatthewMaat 15:c4799ad02cdc 177 {
MatthewMaat 17:d1acb6888b82 178 motor1_pwm.write(1.0f);
MatthewMaat 17:d1acb6888b82 179 EMG_peak.attach(unignore_peaks,0.8);
MatthewMaat 17:d1acb6888b82 180 turn.attach(start_ignore_turn,1);
MatthewMaat 16:2d115aa2773e 181 ignore_turn=false;
MatthewMaat 15:c4799ad02cdc 182 ignore_peaks=true;
MatthewMaat 17:d1acb6888b82 183 motor_on=true;
MatthewMaat 15:c4799ad02cdc 184 }
MatthewMaat 16:2d115aa2773e 185 else
MatthewMaat 16:2d115aa2773e 186 {
MatthewMaat 16:2d115aa2773e 187 motor1_pwm.write(1.0f);
MatthewMaat 16:2d115aa2773e 188 dir1=!dir1;
MatthewMaat 17:d1acb6888b82 189 EMG_peak.attach(unignore_peaks,1.5);
MatthewMaat 16:2d115aa2773e 190 ignore_peaks=true;
MatthewMaat 16:2d115aa2773e 191 motor_on=true;
MatthewMaat 16:2d115aa2773e 192 }
MatthewMaat 13:ec4708dab45d 193 }
MatthewMaat 13:ec4708dab45d 194 motor2_pwm.write(ain1.read());
MatthewMaat 11:de4a85703169 195 }
MatthewMaat 11:de4a85703169 196
jobjansen 20:0f6a88f29a71 197 void error(void){
jobjansen 20:0f6a88f29a71 198 m1_count = motor1.getPulses(); // Read out both motor counts and convert them to rads.
jobjansen 20:0f6a88f29a71 199 m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
jobjansen 20:0f6a88f29a71 200 m2_count = motor2.getPulses();
jobjansen 20:0f6a88f29a71 201 m2_angle = (float)m1_count / 4200.0f * (2.0f * pi);
jobjansen 20:0f6a88f29a71 202
jobjansen 20:0f6a88f29a71 203 float dvd=L * (tan(m2_angle) * pow(tan(m1_angle),2) - tan(m1_angle) * pow(tan(m2_angle),2));
jobjansen 20:0f6a88f29a71 204
jobjansen 20:0f6a88f29a71 205 float a = ( -pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m1_angle),2) ) / dvd; // inverse matrix components of differential x and differential y
jobjansen 20:0f6a88f29a71 206 float b = ( pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * tan(m1_angle) ) / dvd;
jobjansen 20:0f6a88f29a71 207 float c = ( pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2) ) / dvd;
jobjansen 20:0f6a88f29a71 208 float d = ( pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2) ) / dvd;
jobjansen 20:0f6a88f29a71 209
jobjansen 20:0f6a88f29a71 210 float theta1_dot = a*v_refx + b*v_refy;
jobjansen 20:0f6a88f29a71 211 float theta2_dot = c*v_refx + d*v_refy;
jobjansen 20:0f6a88f29a71 212
jobjansen 20:0f6a88f29a71 213 float theta1_ref = theta1_old+theta1_dot*T;
jobjansen 20:0f6a88f29a71 214 float theta2_ref = theta2_old+theta2_dot*T;
jobjansen 20:0f6a88f29a71 215
jobjansen 20:0f6a88f29a71 216 error_x = m1_angle - theta1_ref;
jobjansen 20:0f6a88f29a71 217 error_y = m2_angle - theta2_ref;
jobjansen 20:0f6a88f29a71 218
jobjansen 20:0f6a88f29a71 219 theta1_old = theta1_ref;
jobjansen 20:0f6a88f29a71 220 theta2_old = theta2_ref;
jobjansen 20:0f6a88f29a71 221
jobjansen 20:0f6a88f29a71 222 }
jobjansen 20:0f6a88f29a71 223
jobjansen 20:0f6a88f29a71 224 float PID(float err){
jobjansen 20:0f6a88f29a71 225 //P action
jobjansen 20:0f6a88f29a71 226 u_p = Kp * fabs(err);
jobjansen 20:0f6a88f29a71 227
jobjansen 20:0f6a88f29a71 228 //I action
jobjansen 20:0f6a88f29a71 229 error_integral = error_integral + err * T;
jobjansen 20:0f6a88f29a71 230 u_i = Ki * error_integral;
jobjansen 20:0f6a88f29a71 231
jobjansen 20:0f6a88f29a71 232 /*
jobjansen 20:0f6a88f29a71 233 //D action
jobjansen 20:0f6a88f29a71 234 if(q){
jobjansen 20:0f6a88f29a71 235 error_prev = err;
jobjansen 20:0f6a88f29a71 236 q=false;
jobjansen 20:0f6a88f29a71 237 }
jobjansen 20:0f6a88f29a71 238
jobjansen 20:0f6a88f29a71 239 float error_derivative = (err - error_prev)/T;
jobjansen 20:0f6a88f29a71 240 float filtered_error_derivative = LowPassFilter.step(error_derivative);
jobjansen 20:0f6a88f29a71 241 u_d = Kd * filtered_error_derivative;
jobjansen 20:0f6a88f29a71 242 error_prev = err;
jobjansen 20:0f6a88f29a71 243
jobjansen 20:0f6a88f29a71 244 */
jobjansen 20:0f6a88f29a71 245
jobjansen 20:0f6a88f29a71 246 u = u_p + u_i;
jobjansen 20:0f6a88f29a71 247
jobjansen 20:0f6a88f29a71 248 return u;
jobjansen 20:0f6a88f29a71 249 }
jobjansen 20:0f6a88f29a71 250
jobjansen 20:0f6a88f29a71 251 void setPWM_controller(void){
jobjansen 20:0f6a88f29a71 252 u_1 = PID(error_x);
jobjansen 20:0f6a88f29a71 253 u_2 = PID(error_y);
jobjansen 20:0f6a88f29a71 254
jobjansen 20:0f6a88f29a71 255 if(u_1 < 0.0f){
jobjansen 20:0f6a88f29a71 256 dir1 = 1;
jobjansen 20:0f6a88f29a71 257 }else{
jobjansen 20:0f6a88f29a71 258 dir1 = 0;
jobjansen 20:0f6a88f29a71 259 }
jobjansen 20:0f6a88f29a71 260 motor1_pwm.write(fabs(u_1));
jobjansen 20:0f6a88f29a71 261
jobjansen 20:0f6a88f29a71 262 if(u_2 < 0.0f){
jobjansen 20:0f6a88f29a71 263 dir2 = 1;
jobjansen 20:0f6a88f29a71 264 }else{
jobjansen 20:0f6a88f29a71 265 dir2 = 0;
jobjansen 20:0f6a88f29a71 266 }
jobjansen 20:0f6a88f29a71 267 motor2_pwm.write(fabs(u_1));
jobjansen 20:0f6a88f29a71 268
jobjansen 20:0f6a88f29a71 269 }
jobjansen 20:0f6a88f29a71 270
MatthewMaat 14:dc89250ebc52 271 void sample()
MatthewMaat 14:dc89250ebc52 272 {
MatthewMaat 14:dc89250ebc52 273 switch(currentState)
MatthewMaat 14:dc89250ebc52 274 {
MatthewMaat 15:c4799ad02cdc 275 case Waiting:
MatthewMaat 15:c4799ad02cdc 276 ledred=0;
MatthewMaat 15:c4799ad02cdc 277 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 278 ledblue=1;
MatthewMaat 15:c4799ad02cdc 279 break;
MatthewMaat 15:c4799ad02cdc 280 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 281 ledred=1;
MatthewMaat 15:c4799ad02cdc 282 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 283 ledblue=0;
MatthewMaat 15:c4799ad02cdc 284 break;
MatthewMaat 15:c4799ad02cdc 285 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 286 ledred=1;
MatthewMaat 15:c4799ad02cdc 287 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 288 ledblue=0;
MatthewMaat 15:c4799ad02cdc 289 read_emg();
MatthewMaat 15:c4799ad02cdc 290 record_min_max();
MatthewMaat 15:c4799ad02cdc 291 break;
MatthewMaat 15:c4799ad02cdc 292 case Homing:
MatthewMaat 15:c4799ad02cdc 293 ledred=0;
MatthewMaat 15:c4799ad02cdc 294 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 295 ledblue=0;
MatthewMaat 15:c4799ad02cdc 296 break;
MatthewMaat 14:dc89250ebc52 297 case Operating:
MatthewMaat 14:dc89250ebc52 298 read_emg();
MatthewMaat 14:dc89250ebc52 299 set_PWM();
MatthewMaat 15:c4799ad02cdc 300 ledred=1;
MatthewMaat 15:c4799ad02cdc 301 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 302 ledblue=1;
MatthewMaat 15:c4799ad02cdc 303 break;
MatthewMaat 15:c4799ad02cdc 304 case Demo:
MatthewMaat 15:c4799ad02cdc 305 ledred=0;
MatthewMaat 15:c4799ad02cdc 306 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 307 ledblue=0;
MatthewMaat 14:dc89250ebc52 308 break;
MatthewMaat 14:dc89250ebc52 309 case Failure:
MatthewMaat 14:dc89250ebc52 310 ledred=0;
MatthewMaat 14:dc89250ebc52 311 ledgreen=1;
MatthewMaat 14:dc89250ebc52 312 ledblue=1;
MatthewMaat 15:c4799ad02cdc 313 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 314 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 315 break;
MatthewMaat 14:dc89250ebc52 316 }
MatthewMaat 14:dc89250ebc52 317 }
MatthewMaat 14:dc89250ebc52 318
MatthewMaat 14:dc89250ebc52 319 void error_occur()
MatthewMaat 14:dc89250ebc52 320 {
MatthewMaat 14:dc89250ebc52 321 currentState=Failure;
MatthewMaat 14:dc89250ebc52 322 }
MatthewMaat 14:dc89250ebc52 323
MatthewMaat 15:c4799ad02cdc 324 void button_press()
MatthewMaat 15:c4799ad02cdc 325 //Press button to change state
MatthewMaat 15:c4799ad02cdc 326 {
MatthewMaat 15:c4799ad02cdc 327 state_time.start();
MatthewMaat 15:c4799ad02cdc 328 switch(currentState)
MatthewMaat 15:c4799ad02cdc 329 {
MatthewMaat 15:c4799ad02cdc 330 case Waiting:
MatthewMaat 15:c4799ad02cdc 331 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 332 wait(1);
MatthewMaat 15:c4799ad02cdc 333 break;
MatthewMaat 15:c4799ad02cdc 334 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 335 currentState=EMG_calibration;
MatthewMaat 15:c4799ad02cdc 336 wait(1);
MatthewMaat 15:c4799ad02cdc 337 break;
MatthewMaat 15:c4799ad02cdc 338 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 339 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 340 wait(1);
MatthewMaat 15:c4799ad02cdc 341 break;
MatthewMaat 15:c4799ad02cdc 342 case Homing:
MatthewMaat 15:c4799ad02cdc 343 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 344 wait(1);
MatthewMaat 15:c4799ad02cdc 345 break;
MatthewMaat 15:c4799ad02cdc 346 case Operating:
MatthewMaat 15:c4799ad02cdc 347 currentState=Demo;
MatthewMaat 15:c4799ad02cdc 348 wait(1);
MatthewMaat 15:c4799ad02cdc 349 break;
MatthewMaat 15:c4799ad02cdc 350 case Demo:
MatthewMaat 15:c4799ad02cdc 351 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 352 wait(1);
MatthewMaat 15:c4799ad02cdc 353 break;
MatthewMaat 15:c4799ad02cdc 354 }
MatthewMaat 15:c4799ad02cdc 355 }
MatthewMaat 15:c4799ad02cdc 356
MatthewMaat 8:ec3c634390c7 357 int main()
MatthewMaat 4:f988679bf9a1 358 {
MatthewMaat 14:dc89250ebc52 359 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 360 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 361 ledred=0;
MatthewMaat 13:ec4708dab45d 362 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 363 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 364 button.fall(button_press);
MatthewMaat 12:7f280a661e71 365 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 366 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 367 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 11:de4a85703169 368
MatthewMaat 8:ec3c634390c7 369 while (true) {
MatthewMaat 12:7f280a661e71 370 wait(10);
MatthewMaat 4:f988679bf9a1 371 }
jobjansen 20:0f6a88f29a71 372
jobjansen 20:0f6a88f29a71 373 }