Matthew Maat / Mbed 2 deprecated template_biorobotics

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@19:fb3d570a115e, 2019-10-21 (annotated)

Committer:
MatthewMaat
Date:
Mon Oct 21 12:16:42 2019 +0000
Revision:
19:fb3d570a115e
Parent:
18:8002c75b8e20
Child:
21:a316452da8cd
Kalibratie en FSM

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 18:8002c75b8e20 9 QEI motor2_pos (D8, D9, NC, 32);
MatthewMaat 18:8002c75b8e20 10 QEI motor1_pos (D12, D13, NC, 32);
MatthewMaat 13:ec4708dab45d 11 AnalogIn ain2(A2);
MatthewMaat 13:ec4708dab45d 12 AnalogIn ain1(A3);
MatthewMaat 13:ec4708dab45d 13 DigitalOut dir2(D4);
MatthewMaat 13:ec4708dab45d 14 DigitalOut dir1(D7);
MatthewMaat 13:ec4708dab45d 15 //D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
MatthewMaat 13:ec4708dab45d 16 PwmOut motor2_pwm(D5);
MatthewMaat 13:ec4708dab45d 17 PwmOut motor1_pwm(D6);
MatthewMaat 13:ec4708dab45d 18 AnalogIn emg0( A0 );
MatthewMaat 13:ec4708dab45d 19 AnalogIn emg1( A1 );
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 18:8002c75b8e20 26 HIDScope scope( 4);
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 18:8002c75b8e20 43 const float angle2_offset=asin(0.2);
MatthewMaat 18:8002c75b8e20 44 const float angle1_offset=asin(3.8/35.0);
MatthewMaat 18:8002c75b8e20 45 const double pi=3.1415926535897932384626;
MatthewMaat 18:8002c75b8e20 46 volatile float theta1;
MatthewMaat 18:8002c75b8e20 47 volatile float theta2;
MatthewMaat 14:dc89250ebc52 48
MatthewMaat 14:dc89250ebc52 49 void read_emg()
MatthewMaat 13:ec4708dab45d 50 {
MatthewMaat 15:c4799ad02cdc 51 //EMG signal 0
MatthewMaat 15:c4799ad02cdc 52 static int count0=0;
MatthewMaat 15:c4799ad02cdc 53 static float RMS_value0=0;
MatthewMaat 15:c4799ad02cdc 54 static float HighPass_value0=0;
MatthewMaat 15:c4799ad02cdc 55 count0+=1;
MatthewMaat 15:c4799ad02cdc 56 static float RMS0[150];
MatthewMaat 15:c4799ad02cdc 57 static float HighPass0[30];
MatthewMaat 17:d1acb6888b82 58 static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 17:d1acb6888b82 59 static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 15:c4799ad02cdc 60 float I0;
MatthewMaat 15:c4799ad02cdc 61 float If0;
MatthewMaat 15:c4799ad02cdc 62 //signal 1
MatthewMaat 15:c4799ad02cdc 63 static int count1=0;
MatthewMaat 15:c4799ad02cdc 64 static float RMS_value1=0;
MatthewMaat 15:c4799ad02cdc 65 static float HighPass_value1=0;
MatthewMaat 15:c4799ad02cdc 66 count1+=1;
MatthewMaat 15:c4799ad02cdc 67 static float RMS1[150];
MatthewMaat 15:c4799ad02cdc 68 static float HighPass1[30];
MatthewMaat 13:ec4708dab45d 69 float I1;
MatthewMaat 15:c4799ad02cdc 70 float If1;
MatthewMaat 15:c4799ad02cdc 71 I0=emg0.read(); //read signal
MatthewMaat 17:d1acb6888b82 72 double notched0=Notch0.step(I0);
MatthewMaat 17:d1acb6888b82 73 HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
MatthewMaat 17:d1acb6888b82 74 HighPass0[count0%30]=notched0;
MatthewMaat 15:c4799ad02cdc 75 If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 76 RMS_value0+=(If0-RMS0[count0%150])/150.0;
MatthewMaat 15:c4799ad02cdc 77 RMS0[count0%150]=If0;
MatthewMaat 13:ec4708dab45d 78 /* 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 79 P0=sqrt(RMS_value0);
MatthewMaat 15:c4799ad02cdc 80 I1=emg1.read(); //read signal
MatthewMaat 17:d1acb6888b82 81 double notched1=Notch1.step(I1);
MatthewMaat 17:d1acb6888b82 82 HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
MatthewMaat 17:d1acb6888b82 83 HighPass1[count1%30]=notched1;
MatthewMaat 15:c4799ad02cdc 84 If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 85 RMS_value1+=(If1-RMS1[count1%150])/150.0;
MatthewMaat 15:c4799ad02cdc 86 RMS1[count1%150]=If1;
MatthewMaat 15:c4799ad02cdc 87 /* 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 88 P1=sqrt(RMS_value1);
MatthewMaat 13:ec4708dab45d 89 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 90 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 91 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 92 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 93 ledred=1;
MatthewMaat 14:dc89250ebc52 94 ledgreen=0;
MatthewMaat 14:dc89250ebc52 95 ledblue=1;
MatthewMaat 13:ec4708dab45d 96 }
MatthewMaat 14:dc89250ebc52 97
MatthewMaat 18:8002c75b8e20 98 void get_angles(void)
MatthewMaat 18:8002c75b8e20 99 {
MatthewMaat 18:8002c75b8e20 100 float pulses1=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 101 float pulses2=motor2_pos.getPulses();
MatthewMaat 19:fb3d570a115e 102 theta1=angle1_offset+pulses1*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 19:fb3d570a115e 103 theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 18:8002c75b8e20 104 }
MatthewMaat 18:8002c75b8e20 105
MatthewMaat 18:8002c75b8e20 106 void pos_cal(void)
MatthewMaat 18:8002c75b8e20 107 {
MatthewMaat 18:8002c75b8e20 108 float t=state_time.read();
MatthewMaat 18:8002c75b8e20 109 static int pos_time_counter=0;
MatthewMaat 18:8002c75b8e20 110 static int last_ticks=10000;
MatthewMaat 18:8002c75b8e20 111 static bool motor1_calibrated=false;
MatthewMaat 18:8002c75b8e20 112 float pulses;
MatthewMaat 18:8002c75b8e20 113 pos_time_counter+=1;
MatthewMaat 18:8002c75b8e20 114 if(!motor1_calibrated&&t>1.0f)
MatthewMaat 18:8002c75b8e20 115 {
MatthewMaat 18:8002c75b8e20 116 dir1=1; //???
MatthewMaat 18:8002c75b8e20 117 motor1_pwm.write(0.6f);
MatthewMaat 18:8002c75b8e20 118 pulses=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 119 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 120 {
MatthewMaat 18:8002c75b8e20 121 motor1_pos.reset();
MatthewMaat 18:8002c75b8e20 122 motor1_calibrated=true;
MatthewMaat 18:8002c75b8e20 123 last_ticks=10000;
MatthewMaat 18:8002c75b8e20 124 state_time.reset();
MatthewMaat 18:8002c75b8e20 125 dir1=!dir1;
MatthewMaat 18:8002c75b8e20 126 }
MatthewMaat 18:8002c75b8e20 127 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 128 {
MatthewMaat 18:8002c75b8e20 129 last_ticks=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 130 }
MatthewMaat 18:8002c75b8e20 131
MatthewMaat 18:8002c75b8e20 132 }
MatthewMaat 18:8002c75b8e20 133 else if(t>1.0f)
MatthewMaat 18:8002c75b8e20 134 {
MatthewMaat 18:8002c75b8e20 135 motor1_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 136 dir2=1; //???
MatthewMaat 18:8002c75b8e20 137 motor2_pwm.write(0.6f);
MatthewMaat 18:8002c75b8e20 138 pulses=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 139 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 140 {
MatthewMaat 18:8002c75b8e20 141 motor2_pos.reset();
MatthewMaat 18:8002c75b8e20 142 motor2_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 143 currentState=EMG_calibration;
MatthewMaat 18:8002c75b8e20 144 }
MatthewMaat 18:8002c75b8e20 145 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 146 {
MatthewMaat 18:8002c75b8e20 147 last_ticks=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 148 }
MatthewMaat 18:8002c75b8e20 149 }
MatthewMaat 18:8002c75b8e20 150
MatthewMaat 18:8002c75b8e20 151 }
MatthewMaat 18:8002c75b8e20 152
MatthewMaat 15:c4799ad02cdc 153 void record_min_max(void)
MatthewMaat 15:c4799ad02cdc 154 {
MatthewMaat 15:c4799ad02cdc 155 float t=state_time.read();
MatthewMaat 15:c4799ad02cdc 156 if(t>0.4)
MatthewMaat 15:c4799ad02cdc 157 {
MatthewMaat 15:c4799ad02cdc 158 if(P0<EMG_min0)
MatthewMaat 15:c4799ad02cdc 159 {
MatthewMaat 15:c4799ad02cdc 160 EMG_min0=P0;
MatthewMaat 15:c4799ad02cdc 161 }
MatthewMaat 15:c4799ad02cdc 162 else if(P0>EMG_max0)
MatthewMaat 15:c4799ad02cdc 163 {
MatthewMaat 15:c4799ad02cdc 164 EMG_max0=P0;
MatthewMaat 15:c4799ad02cdc 165 }
MatthewMaat 15:c4799ad02cdc 166 if(P1<EMG_min1)
MatthewMaat 15:c4799ad02cdc 167 {
MatthewMaat 15:c4799ad02cdc 168 EMG_min1=P1;
MatthewMaat 15:c4799ad02cdc 169 }
MatthewMaat 15:c4799ad02cdc 170 else if(P1>EMG_max1)
MatthewMaat 15:c4799ad02cdc 171 {
MatthewMaat 15:c4799ad02cdc 172 EMG_max1=P1;
MatthewMaat 15:c4799ad02cdc 173 }
MatthewMaat 15:c4799ad02cdc 174 }
MatthewMaat 15:c4799ad02cdc 175 }
MatthewMaat 15:c4799ad02cdc 176
MatthewMaat 15:c4799ad02cdc 177 void unignore_peaks(void)
MatthewMaat 15:c4799ad02cdc 178 {
MatthewMaat 15:c4799ad02cdc 179 ignore_peaks=false;
MatthewMaat 15:c4799ad02cdc 180 }
MatthewMaat 16:2d115aa2773e 181 void start_ignore_turn(void)
MatthewMaat 16:2d115aa2773e 182 {
MatthewMaat 16:2d115aa2773e 183 ignore_turn=true;
MatthewMaat 16:2d115aa2773e 184 }
MatthewMaat 15:c4799ad02cdc 185
MatthewMaat 14:dc89250ebc52 186 void set_PWM(void)
MatthewMaat 11:de4a85703169 187 {
MatthewMaat 15:c4799ad02cdc 188 static bool motor_on=false;
MatthewMaat 15:c4799ad02cdc 189 float Q0;
MatthewMaat 15:c4799ad02cdc 190 Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
MatthewMaat 17:d1acb6888b82 191 if (Q0>-0.2f && !ignore_peaks)
MatthewMaat 13:ec4708dab45d 192 {
MatthewMaat 17:d1acb6888b82 193 if (motor_on)
MatthewMaat 15:c4799ad02cdc 194 {
MatthewMaat 17:d1acb6888b82 195 motor1_pwm.write(0.0f);
MatthewMaat 17:d1acb6888b82 196 EMG_peak.attach(unignore_peaks,0.8);
MatthewMaat 17:d1acb6888b82 197 turn.attach(start_ignore_turn,1);
MatthewMaat 17:d1acb6888b82 198 ignore_turn=false;
MatthewMaat 15:c4799ad02cdc 199 ignore_peaks=true;
MatthewMaat 17:d1acb6888b82 200 motor_on=false;
MatthewMaat 15:c4799ad02cdc 201 }
MatthewMaat 16:2d115aa2773e 202 else if(ignore_turn)
MatthewMaat 15:c4799ad02cdc 203 {
MatthewMaat 17:d1acb6888b82 204 motor1_pwm.write(1.0f);
MatthewMaat 17:d1acb6888b82 205 EMG_peak.attach(unignore_peaks,0.8);
MatthewMaat 17:d1acb6888b82 206 turn.attach(start_ignore_turn,1);
MatthewMaat 16:2d115aa2773e 207 ignore_turn=false;
MatthewMaat 15:c4799ad02cdc 208 ignore_peaks=true;
MatthewMaat 17:d1acb6888b82 209 motor_on=true;
MatthewMaat 15:c4799ad02cdc 210 }
MatthewMaat 16:2d115aa2773e 211 else
MatthewMaat 16:2d115aa2773e 212 {
MatthewMaat 16:2d115aa2773e 213 motor1_pwm.write(1.0f);
MatthewMaat 16:2d115aa2773e 214 dir1=!dir1;
MatthewMaat 17:d1acb6888b82 215 EMG_peak.attach(unignore_peaks,1.5);
MatthewMaat 16:2d115aa2773e 216 ignore_peaks=true;
MatthewMaat 16:2d115aa2773e 217 motor_on=true;
MatthewMaat 16:2d115aa2773e 218 }
MatthewMaat 13:ec4708dab45d 219 }
MatthewMaat 13:ec4708dab45d 220 motor2_pwm.write(ain1.read());
MatthewMaat 11:de4a85703169 221 }
MatthewMaat 11:de4a85703169 222
MatthewMaat 14:dc89250ebc52 223 void sample()
MatthewMaat 14:dc89250ebc52 224 {
MatthewMaat 18:8002c75b8e20 225 get_angles();
MatthewMaat 18:8002c75b8e20 226 scope.set(0,P0);
MatthewMaat 18:8002c75b8e20 227 scope.set(1,P1);
MatthewMaat 18:8002c75b8e20 228 scope.set(2,theta1);
MatthewMaat 18:8002c75b8e20 229 scope.set(3,theta2);
MatthewMaat 18:8002c75b8e20 230 scope.send();
MatthewMaat 14:dc89250ebc52 231 switch(currentState)
MatthewMaat 14:dc89250ebc52 232 {
MatthewMaat 15:c4799ad02cdc 233 case Waiting:
MatthewMaat 15:c4799ad02cdc 234 ledred=0;
MatthewMaat 15:c4799ad02cdc 235 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 236 ledblue=1;
MatthewMaat 15:c4799ad02cdc 237 break;
MatthewMaat 15:c4799ad02cdc 238 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 239 ledred=1;
MatthewMaat 15:c4799ad02cdc 240 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 241 ledblue=0;
MatthewMaat 18:8002c75b8e20 242 pos_cal();
MatthewMaat 15:c4799ad02cdc 243 break;
MatthewMaat 15:c4799ad02cdc 244 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 245 ledred=1;
MatthewMaat 15:c4799ad02cdc 246 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 247 ledblue=0;
MatthewMaat 15:c4799ad02cdc 248 read_emg();
MatthewMaat 15:c4799ad02cdc 249 record_min_max();
MatthewMaat 15:c4799ad02cdc 250 break;
MatthewMaat 15:c4799ad02cdc 251 case Homing:
MatthewMaat 15:c4799ad02cdc 252 ledred=0;
MatthewMaat 15:c4799ad02cdc 253 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 254 ledblue=0;
MatthewMaat 15:c4799ad02cdc 255 break;
MatthewMaat 14:dc89250ebc52 256 case Operating:
MatthewMaat 14:dc89250ebc52 257 read_emg();
MatthewMaat 14:dc89250ebc52 258 set_PWM();
MatthewMaat 15:c4799ad02cdc 259 ledred=1;
MatthewMaat 15:c4799ad02cdc 260 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 261 ledblue=1;
MatthewMaat 15:c4799ad02cdc 262 break;
MatthewMaat 15:c4799ad02cdc 263 case Demo:
MatthewMaat 15:c4799ad02cdc 264 ledred=0;
MatthewMaat 15:c4799ad02cdc 265 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 266 ledblue=0;
MatthewMaat 14:dc89250ebc52 267 break;
MatthewMaat 14:dc89250ebc52 268 case Failure:
MatthewMaat 14:dc89250ebc52 269 ledred=0;
MatthewMaat 14:dc89250ebc52 270 ledgreen=1;
MatthewMaat 14:dc89250ebc52 271 ledblue=1;
MatthewMaat 15:c4799ad02cdc 272 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 273 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 274 break;
MatthewMaat 14:dc89250ebc52 275 }
MatthewMaat 14:dc89250ebc52 276 }
MatthewMaat 14:dc89250ebc52 277
MatthewMaat 14:dc89250ebc52 278 void error_occur()
MatthewMaat 14:dc89250ebc52 279 {
MatthewMaat 14:dc89250ebc52 280 currentState=Failure;
MatthewMaat 14:dc89250ebc52 281 }
MatthewMaat 14:dc89250ebc52 282
MatthewMaat 18:8002c75b8e20 283 void button_press(void)
MatthewMaat 15:c4799ad02cdc 284 //Press button to change state
MatthewMaat 15:c4799ad02cdc 285 {
MatthewMaat 18:8002c75b8e20 286 state_time.reset();
MatthewMaat 15:c4799ad02cdc 287 switch(currentState)
MatthewMaat 15:c4799ad02cdc 288 {
MatthewMaat 15:c4799ad02cdc 289 case Waiting:
MatthewMaat 15:c4799ad02cdc 290 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 291 wait(1);
MatthewMaat 15:c4799ad02cdc 292 break;
MatthewMaat 15:c4799ad02cdc 293 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 294 currentState=EMG_calibration;
MatthewMaat 15:c4799ad02cdc 295 wait(1);
MatthewMaat 15:c4799ad02cdc 296 break;
MatthewMaat 15:c4799ad02cdc 297 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 298 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 299 wait(1);
MatthewMaat 15:c4799ad02cdc 300 break;
MatthewMaat 15:c4799ad02cdc 301 case Homing:
MatthewMaat 15:c4799ad02cdc 302 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 303 wait(1);
MatthewMaat 15:c4799ad02cdc 304 break;
MatthewMaat 15:c4799ad02cdc 305 case Operating:
MatthewMaat 15:c4799ad02cdc 306 currentState=Demo;
MatthewMaat 15:c4799ad02cdc 307 wait(1);
MatthewMaat 15:c4799ad02cdc 308 break;
MatthewMaat 15:c4799ad02cdc 309 case Demo:
MatthewMaat 15:c4799ad02cdc 310 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 311 wait(1);
MatthewMaat 15:c4799ad02cdc 312 break;
MatthewMaat 15:c4799ad02cdc 313 }
MatthewMaat 15:c4799ad02cdc 314 }
MatthewMaat 15:c4799ad02cdc 315
MatthewMaat 8:ec3c634390c7 316 int main()
MatthewMaat 4:f988679bf9a1 317 {
MatthewMaat 14:dc89250ebc52 318 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 319 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 320 ledred=0;
MatthewMaat 13:ec4708dab45d 321 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 322 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 323 button.fall(button_press);
MatthewMaat 12:7f280a661e71 324 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 325 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 326 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 18:8002c75b8e20 327 state_time.start();
MatthewMaat 8:ec3c634390c7 328 while (true) {
MatthewMaat 12:7f280a661e71 329 wait(10);
MatthewMaat 4:f988679bf9a1 330 }
MatthewMaat 4:f988679bf9a1 331 }