Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@13:c087c0f64769, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 20:26:58 2018 +0000
Revision:
13:c087c0f64769
Parent:
12:b2b082e73ef1
Child:
14:abc125dcc246
emg calibration included, bugs resolved

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1725696 5:3581013d4505 1 #include "mbed.h" // Use revision 119!!
s1725696 5:3581013d4505 2 #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code
s1725696 5:3581013d4505 3 #include "QEI.h" // For reading the encoder of the motors
s1725696 9:d7a6a3619576 4 #include <ctime> // for the timer during the process (if needed)
s1725696 11:79311abb2bc2 5 #include "Servo.h" // For controlling the servo
s1725696 13:c087c0f64769 6 #include "BiQuad.h"
s1725696 0:cb8857cf3ea4 7
s1725696 5:3581013d4505 8 #define SERIAL_BAUD 115200
s1725696 5:3581013d4505 9
s1725696 5:3581013d4505 10 // In- en outputs
s1725696 4:8183e7b228f0 11 // -----------------------------------------------------------------------------
s1725696 4:8183e7b228f0 12
s1725696 0:cb8857cf3ea4 13 // EMG related
s1725696 13:c087c0f64769 14 // Constants EMG filter
s1725696 13:c087c0f64769 15 const double m1 = 0.5000;
s1725696 13:c087c0f64769 16 const double m2 = -0.8090;
s1725696 13:c087c0f64769 17 const double n0 = 0.5000;
s1725696 13:c087c0f64769 18 const double n1 = -0.8090;
s1725696 13:c087c0f64769 19 const double n2 = 0.0;
s1725696 13:c087c0f64769 20 const double a1 = 0.9565;
s1725696 13:c087c0f64769 21 const double a2 = -1.9131;
s1725696 13:c087c0f64769 22 const double b0 = 0.9565;
s1725696 13:c087c0f64769 23 const double b1 = -1.9112;
s1725696 13:c087c0f64769 24 const double b2 = 0.9150;
s1725696 13:c087c0f64769 25 const double c1 = 0.0675;
s1725696 13:c087c0f64769 26 const double c2 = 0.1349;
s1725696 13:c087c0f64769 27 const double d0 = 0.0675;
s1725696 13:c087c0f64769 28 const double d1 = -1.1430;
s1725696 13:c087c0f64769 29 const double d2 = 0.4128;
s1725696 13:c087c0f64769 30
s1725696 13:c087c0f64769 31 // Variables EMG
s1725696 13:c087c0f64769 32 double emg0;
s1725696 13:c087c0f64769 33 double emg1;
s1725696 13:c087c0f64769 34 double emg2;
s1725696 13:c087c0f64769 35 double notch0;
s1725696 13:c087c0f64769 36 double notch1;
s1725696 13:c087c0f64769 37 double notch2;
s1725696 13:c087c0f64769 38 double high0;
s1725696 13:c087c0f64769 39 double high1;
s1725696 13:c087c0f64769 40 double high2;
s1725696 13:c087c0f64769 41 double absolute0;
s1725696 13:c087c0f64769 42 double absolute1;
s1725696 13:c087c0f64769 43 double absolute2;
s1725696 13:c087c0f64769 44 double low0;
s1725696 13:c087c0f64769 45 double low1;
s1725696 13:c087c0f64769 46 double low2;
s1725696 13:c087c0f64769 47
s1725696 13:c087c0f64769 48 // BiQuad values
s1725696 13:c087c0f64769 49 BiQuadChain notch;
s1725696 13:c087c0f64769 50 BiQuad N1( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 51 BiQuad N2( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 52 BiQuad N3( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 53 BiQuadChain highpass;
s1725696 13:c087c0f64769 54 BiQuad H1( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 55 BiQuad H2( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 56 BiQuad H3( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 57 BiQuadChain lowpass;
s1725696 13:c087c0f64769 58 BiQuad L1( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 59 BiQuad L2( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 60 BiQuad L3( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 61
s1725696 13:c087c0f64769 62 const float T=0.001f;
s1725696 13:c087c0f64769 63
s1725696 13:c087c0f64769 64 // EMG
s1725696 13:c087c0f64769 65 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
s1725696 13:c087c0f64769 66 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
s1725696 13:c087c0f64769 67 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
s1725696 13:c087c0f64769 68
s1725696 13:c087c0f64769 69 //Empty arrays to calculate MovAgs
s1725696 13:c087c0f64769 70 double Average0, Average1, Average2; //Outcome of MovAg
s1725696 13:c087c0f64769 71 const int sizeCali = 500; //Size of array over which the Threshold will be calculated
s1725696 13:c087c0f64769 72 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
s1725696 13:c087c0f64769 73
s1725696 13:c087c0f64769 74 //Empty arrays to calculate means in calibration
s1725696 13:c087c0f64769 75 double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
s1725696 13:c087c0f64769 76 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
s1725696 13:c087c0f64769 77 int g = 0; //Part of the switch void, where the current state can be changed
s1725696 13:c087c0f64769 78 int emg_calib=0; //After calibration this value will be 1, enabling the
s1725696 13:c087c0f64769 79
s1725696 13:c087c0f64769 80 // EMG
s1725696 13:c087c0f64769 81 Ticker Filter_tick;
s1725696 13:c087c0f64769 82 Ticker MovAg_tick;
s1725696 13:c087c0f64769 83 AnalogIn emg0_in( A0 );
s1725696 13:c087c0f64769 84 AnalogIn emg1_in( A1 );
s1725696 13:c087c0f64769 85 AnalogIn emg2_in( A2 );
s1725696 10:56136a0da8c1 86
s1725696 0:cb8857cf3ea4 87 // Motor related
s1725696 12:b2b082e73ef1 88 DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
s1725696 0:cb8857cf3ea4 89 PwmOut pwmpin_1(D5); // PWM pin of motor 1
s1725696 12:b2b082e73ef1 90 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
s1725696 5:3581013d4505 91 PwmOut pwmpin_2(D6); // PWM pin of motor 2
s1725696 0:cb8857cf3ea4 92
s1725696 0:cb8857cf3ea4 93 // Extra stuff
s1725696 9:d7a6a3619576 94 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 8:5ad8a7892693 95 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
s1725696 9:d7a6a3619576 96 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 8:5ad8a7892693 97 DigitalIn button_demo(D6); // button for demo mode, on bioshield
s1725696 9:d7a6a3619576 98
s1725696 4:8183e7b228f0 99 DigitalIn led_red(LED_RED); // red led
s1725696 4:8183e7b228f0 100 DigitalIn led_green(LED_GREEN); // green led
s1725696 4:8183e7b228f0 101 DigitalIn led_blue(LED_BLUE); // blue led
s1725696 9:d7a6a3619576 102
s1725696 5:3581013d4505 103 AnalogIn pot_1(A1); // potmeter for simulating EMG input
s1725696 13:c087c0f64769 104 Servo myservo(D8); // Define the servo to control (in penholder), up to start with
s1725696 0:cb8857cf3ea4 105
s1725696 0:cb8857cf3ea4 106 // Other stuff
s1725696 4:8183e7b228f0 107 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 108 // Define stuff like tickers etc
s1725696 12:b2b082e73ef1 109
s1725696 10:56136a0da8c1 110 Ticker ticker; // Name a ticker, use each ticker only for 1 function!
s1725696 13:c087c0f64769 111 HIDScope scope(6); // Number of channels in HIDScope
s1725696 12:b2b082e73ef1 112 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
s1725696 12:b2b082e73ef1 113 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
s1725696 5:3581013d4505 114 Serial pc(USBTX,USBRX);
s1725696 6:f495a77c2c95 115 Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
s1725696 0:cb8857cf3ea4 116
s1725696 0:cb8857cf3ea4 117 // Variables
s1725696 4:8183e7b228f0 118 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 119 // Define here all variables needed throughout the whole code
s1725696 12:b2b082e73ef1 120 volatile double time_overall;
s1725696 12:b2b082e73ef1 121 volatile double time_in_state;
s1725696 12:b2b082e73ef1 122 volatile double motor_velocity = 0;
s1725696 12:b2b082e73ef1 123 volatile double EMG = 0;
s1725696 12:b2b082e73ef1 124 volatile double errors = 0;
s1725696 12:b2b082e73ef1 125 volatile int counts1_prev = 0;
s1725696 12:b2b082e73ef1 126 volatile int counts2_prev = 0;
s1725696 12:b2b082e73ef1 127 volatile int counts1;
s1725696 12:b2b082e73ef1 128 volatile int counts2;
s1725696 12:b2b082e73ef1 129
s1725696 12:b2b082e73ef1 130 // MOTOR_CAL
s1725696 12:b2b082e73ef1 131 volatile double tower_1_position = 0.1; // the tower which he reaches first
s1725696 12:b2b082e73ef1 132 volatile double tower_end_position = 0.1; // the tower which he reaches second
s1725696 12:b2b082e73ef1 133 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
s1725696 12:b2b082e73ef1 134 volatile double position;
s1725696 12:b2b082e73ef1 135 float speed = 0.70;
s1725696 12:b2b082e73ef1 136 int dir = 0;
s1725696 6:f495a77c2c95 137
s1725696 6:f495a77c2c95 138 // states
s1725696 9:d7a6a3619576 139 enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
s1725696 7:ec5add330cb3 140 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
s1725696 6:f495a77c2c95 141 bool StateChanged = true; // the state must be changed to go into the next state
s1725696 0:cb8857cf3ea4 142
s1725696 0:cb8857cf3ea4 143 // Functions
s1725696 4:8183e7b228f0 144 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 145
s1725696 4:8183e7b228f0 146 // Encoder
s1725696 4:8183e7b228f0 147 // Getting encoder information from motors
s1725696 12:b2b082e73ef1 148 int Counts1(volatile int& a) // a = counts1
s1725696 4:8183e7b228f0 149 {
s1725696 12:b2b082e73ef1 150 counts1_prev = a;
s1725696 12:b2b082e73ef1 151 a = Encoder2.getPulses();
s1725696 12:b2b082e73ef1 152 return a;
s1725696 12:b2b082e73ef1 153 }
s1725696 12:b2b082e73ef1 154
s1725696 12:b2b082e73ef1 155 int Counts2(volatile int& a) // a = counts2
s1725696 12:b2b082e73ef1 156 {
s1725696 12:b2b082e73ef1 157 counts2_prev = a;
s1725696 12:b2b082e73ef1 158 a = Encoder1.getPulses();
s1725696 12:b2b082e73ef1 159 return a;
s1725696 12:b2b082e73ef1 160 }
s1725696 5:3581013d4505 161
s1725696 11:79311abb2bc2 162 // Potmeter for controlling motor
s1725696 5:3581013d4505 163 float potmeter()
s1725696 5:3581013d4505 164 {
s1725696 5:3581013d4505 165 float out_1 = pot_1 * 2.0f;
s1725696 5:3581013d4505 166 float out_2 = out_1 - 1.0f;
s1725696 5:3581013d4505 167
s1725696 5:3581013d4505 168 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 169 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 170
s1725696 5:3581013d4505 171 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 172 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 173
s1725696 5:3581013d4505 174 return out_2;
s1725696 6:f495a77c2c95 175 }
s1725696 9:d7a6a3619576 176
s1725696 11:79311abb2bc2 177 // Servo control
s1725696 12:b2b082e73ef1 178 // To lift the pen up, with a push of button
s1725696 11:79311abb2bc2 179 void servocontrol()
s1725696 11:79311abb2bc2 180 {
s1725696 13:c087c0f64769 181 myservo = 0.1;
s1725696 12:b2b082e73ef1 182 if (button_motorcal == true) // this button is further only used in WAIT mode to go to MOTOR_CAL
s1725696 11:79311abb2bc2 183 {
s1725696 11:79311abb2bc2 184 myservo = 0.1;
s1725696 11:79311abb2bc2 185 }
s1725696 11:79311abb2bc2 186 else
s1725696 11:79311abb2bc2 187 {
s1725696 11:79311abb2bc2 188 myservo = 0.0;
s1725696 11:79311abb2bc2 189 }
s1725696 12:b2b082e73ef1 190 }
s1725696 11:79311abb2bc2 191
s1725696 9:d7a6a3619576 192 // Send information to HIDScope
s1725696 6:f495a77c2c95 193 void hidscope() // Attach this to a ticker!
s1725696 0:cb8857cf3ea4 194 {
s1725696 12:b2b082e73ef1 195 scope.set(0, emg1); // send EMG 1 to channel 1 (=0)
s1725696 12:b2b082e73ef1 196 scope.set(1, emg2); // sent EMG 2 to channel 2 (=1)
s1725696 5:3581013d4505 197
s1725696 1:1a8211e1f3f3 198 // Ensure that enough channels are available (HIDScope scope(2))
s1725696 0:cb8857cf3ea4 199 scope.send(); // Send all channels to the PC at once
s1725696 6:f495a77c2c95 200 }
s1725696 9:d7a6a3619576 201
s1725696 10:56136a0da8c1 202 // EMG filter
s1725696 10:56136a0da8c1 203 // To process the EMG signal before information can be caught from it
s1725696 13:c087c0f64769 204 //Filter of the first EMG signal
s1725696 13:c087c0f64769 205 void filtering()
s1725696 13:c087c0f64769 206 {
s1725696 13:c087c0f64769 207 // Reading the EMG signal
s1725696 13:c087c0f64769 208 emg0 = emg0_in.read();
s1725696 13:c087c0f64769 209 emg1 = emg1_in.read();
s1725696 13:c087c0f64769 210 emg2 = emg2_in.read();
s1725696 13:c087c0f64769 211
s1725696 13:c087c0f64769 212 // Applying a notch filter over the EMG data
s1725696 13:c087c0f64769 213 notch0 = N1.step(emg0);
s1725696 13:c087c0f64769 214 notch1 = N2.step(emg1);
s1725696 13:c087c0f64769 215 notch2 = N3.step(emg2);
s1725696 13:c087c0f64769 216
s1725696 13:c087c0f64769 217 // Applying a high pass filter
s1725696 13:c087c0f64769 218 high0 = H1.step(notch0);
s1725696 13:c087c0f64769 219 high1 = H2.step(notch1);
s1725696 13:c087c0f64769 220 high2 = H3.step(notch2);
s1725696 13:c087c0f64769 221
s1725696 13:c087c0f64769 222 // Rectifying the signal
s1725696 13:c087c0f64769 223 absolute0 = fabs(high0);
s1725696 13:c087c0f64769 224 absolute1 = fabs(high1);
s1725696 13:c087c0f64769 225 absolute2 = fabs(high2);
s1725696 13:c087c0f64769 226
s1725696 13:c087c0f64769 227 // Applying low pass filter
s1725696 13:c087c0f64769 228 low0 = L1.step(absolute0);
s1725696 13:c087c0f64769 229 low1 = L2.step(absolute1);
s1725696 13:c087c0f64769 230 low2 = L3.step(absolute2);
s1725696 13:c087c0f64769 231 }
s1725696 13:c087c0f64769 232
s1725696 13:c087c0f64769 233 // Moving average filter
s1725696 13:c087c0f64769 234 // To determine the moving average, apply after filtering
s1725696 13:c087c0f64769 235 void MovAg()
s1725696 13:c087c0f64769 236 {
s1725696 13:c087c0f64769 237 // For statement to make an array of the last datapoints of the filtered signal
s1725696 13:c087c0f64769 238 for (int i = sizeMovAg - 1; i >= 0; i--)
s1725696 13:c087c0f64769 239 {
s1725696 13:c087c0f64769 240 // Shifts the i'th element one place to the right
s1725696 13:c087c0f64769 241 StoreArray0[i] = StoreArray0[i-1];
s1725696 13:c087c0f64769 242 StoreArray1[i] = StoreArray1[i-1];
s1725696 13:c087c0f64769 243 StoreArray2[i] = StoreArray2[i-1];
s1725696 13:c087c0f64769 244 }
s1725696 13:c087c0f64769 245
s1725696 13:c087c0f64769 246 // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 247 StoreArray0[0] = low0;
s1725696 13:c087c0f64769 248 StoreArray1[0] = low1;
s1725696 13:c087c0f64769 249 StoreArray2[0] = low2;
s1725696 13:c087c0f64769 250 sum1 = 0.0;
s1725696 13:c087c0f64769 251 sum2 = 0.0;
s1725696 13:c087c0f64769 252 sum3 = 0.0;
s1725696 13:c087c0f64769 253
s1725696 13:c087c0f64769 254 // For statement to sum the elements in the array
s1725696 13:c087c0f64769 255 for (int a = 0; a<=sizeMovAg-1; a++)
s1725696 13:c087c0f64769 256 {
s1725696 13:c087c0f64769 257 sum1+=StoreArray0[a];
s1725696 13:c087c0f64769 258 sum2+=StoreArray1[a];
s1725696 13:c087c0f64769 259 sum3+=StoreArray2[a];
s1725696 13:c087c0f64769 260 }
s1725696 13:c087c0f64769 261
s1725696 13:c087c0f64769 262 // Calculates an average over the datapoints in the array
s1725696 13:c087c0f64769 263 Average0 = sum1/sizeMovAg;
s1725696 13:c087c0f64769 264 Average1 = sum2/sizeMovAg;
s1725696 13:c087c0f64769 265 Average2 = sum3/sizeMovAg;
s1725696 13:c087c0f64769 266
s1725696 13:c087c0f64769 267 scope.set( 0, emg0); // Sending the signal to the HIDScope
s1725696 13:c087c0f64769 268 scope.set( 1, low0); // Change the number of inputs on the top when necessary
s1725696 13:c087c0f64769 269 scope.set( 2, Average0);
s1725696 13:c087c0f64769 270 scope.set( 3, low1);
s1725696 13:c087c0f64769 271 scope.set( 4, emg2);
s1725696 13:c087c0f64769 272 scope.set( 5, low2);
s1725696 13:c087c0f64769 273 scope.send();
s1725696 13:c087c0f64769 274 }
s1725696 10:56136a0da8c1 275
s1725696 10:56136a0da8c1 276 // WAIT
s1725696 10:56136a0da8c1 277 // To do nothing
s1725696 10:56136a0da8c1 278 void wait_mode()
s1725696 10:56136a0da8c1 279 {
s1725696 10:56136a0da8c1 280 // go back to the initial values
s1725696 10:56136a0da8c1 281 // Copy here the variables list with initial values
s1725696 13:c087c0f64769 282 // all pwm's to zero
s1725696 13:c087c0f64769 283 // all counts to zero
s1725696 10:56136a0da8c1 284 }
s1725696 10:56136a0da8c1 285
s1725696 10:56136a0da8c1 286 // MOTOR_CAL
s1725696 10:56136a0da8c1 287 // To calibrate the motor angle to some mechanical boundaries
s1725696 10:56136a0da8c1 288 // Kenneth mee bezig
s1725696 12:b2b082e73ef1 289 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 12:b2b082e73ef1 290
s1725696 13:c087c0f64769 291 if (tower_1_position == 0.1)
s1725696 13:c087c0f64769 292 {
s1725696 12:b2b082e73ef1 293 tower_1_position = a;
s1725696 12:b2b082e73ef1 294 }
s1725696 13:c087c0f64769 295 else if (tower_end_position == 0.1)
s1725696 13:c087c0f64769 296 {
s1725696 12:b2b082e73ef1 297 tower_end_position = a;
s1725696 12:b2b082e73ef1 298 }
s1725696 13:c087c0f64769 299 else if (rotation_start_position == 0.1)
s1725696 13:c087c0f64769 300 {
s1725696 12:b2b082e73ef1 301 rotation_start_position = a;
s1725696 12:b2b082e73ef1 302 }
s1725696 12:b2b082e73ef1 303 }
s1725696 12:b2b082e73ef1 304
s1725696 12:b2b082e73ef1 305 // Start translation
s1725696 12:b2b082e73ef1 306 void translation_start(int a, float b) // a = dir , b = speed
s1725696 12:b2b082e73ef1 307 {
s1725696 12:b2b082e73ef1 308 dirpin_1.write(a);
s1725696 12:b2b082e73ef1 309 pwmpin_1 = b;
s1725696 12:b2b082e73ef1 310 }
s1725696 12:b2b082e73ef1 311
s1725696 12:b2b082e73ef1 312 // Stop translation
s1725696 12:b2b082e73ef1 313 void translation_stop()
s1725696 12:b2b082e73ef1 314 {
s1725696 12:b2b082e73ef1 315 pwmpin_1 = 0.0;
s1725696 12:b2b082e73ef1 316 }
s1725696 12:b2b082e73ef1 317
s1725696 12:b2b082e73ef1 318 // Start rotation
s1725696 12:b2b082e73ef1 319 void rotation_start(int a, float b)
s1725696 12:b2b082e73ef1 320 {
s1725696 12:b2b082e73ef1 321 dirpin_2.write(a);
s1725696 12:b2b082e73ef1 322 pwmpin_2 = b;
s1725696 12:b2b082e73ef1 323 }
s1725696 12:b2b082e73ef1 324
s1725696 12:b2b082e73ef1 325 // Stop rotation
s1725696 12:b2b082e73ef1 326 void rotation_stop()
s1725696 12:b2b082e73ef1 327 {
s1725696 12:b2b082e73ef1 328 pwmpin_2 = 0.0;
s1725696 12:b2b082e73ef1 329 }
s1725696 12:b2b082e73ef1 330
s1725696 12:b2b082e73ef1 331 // Calibration of translation
s1725696 12:b2b082e73ef1 332 void calibration_translation()
s1725696 12:b2b082e73ef1 333 {
s1725696 12:b2b082e73ef1 334 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 12:b2b082e73ef1 335 {
s1725696 12:b2b082e73ef1 336 pc.printf("\r\nTranslatie loop\r\n");
s1725696 12:b2b082e73ef1 337 translation_start(dir,speed);
s1725696 12:b2b082e73ef1 338 pc.printf("Direction = %i\r\n", dir);
s1725696 12:b2b082e73ef1 339
s1725696 12:b2b082e73ef1 340 bool g = true; // to make a condition for the while loop
s1725696 12:b2b082e73ef1 341 while (g == true)
s1725696 12:b2b082e73ef1 342 {
s1725696 12:b2b082e73ef1 343 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 12:b2b082e73ef1 344 {
s1725696 12:b2b082e73ef1 345 translation_stop();
s1725696 12:b2b082e73ef1 346 pos_store(Counts1(counts1));
s1725696 12:b2b082e73ef1 347 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 12:b2b082e73ef1 348 dir = dir + 1;
s1725696 12:b2b082e73ef1 349
s1725696 12:b2b082e73ef1 350 g = false; // to end the while loop
s1725696 12:b2b082e73ef1 351 }
s1725696 12:b2b082e73ef1 352
s1725696 12:b2b082e73ef1 353 wait(0.01);
s1725696 12:b2b082e73ef1 354 }
s1725696 12:b2b082e73ef1 355
s1725696 12:b2b082e73ef1 356 wait(1.5); // wait 3 seconds before next round of translation/rotation
s1725696 12:b2b082e73ef1 357 }
s1725696 12:b2b082e73ef1 358 }
s1725696 12:b2b082e73ef1 359
s1725696 12:b2b082e73ef1 360 void calibration_rotation()
s1725696 12:b2b082e73ef1 361 {
s1725696 12:b2b082e73ef1 362 rotation_start(dir, speed);
s1725696 12:b2b082e73ef1 363 pc.printf("\r\nRotatie start\r\n");
s1725696 12:b2b082e73ef1 364
s1725696 12:b2b082e73ef1 365 bool f = true; // condition for while loop
s1725696 12:b2b082e73ef1 366 while(f == true)
s1725696 12:b2b082e73ef1 367 {
s1725696 12:b2b082e73ef1 368 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 12:b2b082e73ef1 369 {
s1725696 12:b2b082e73ef1 370 rotation_stop();
s1725696 12:b2b082e73ef1 371
s1725696 12:b2b082e73ef1 372 f = false; // to end the while loop
s1725696 12:b2b082e73ef1 373 }
s1725696 12:b2b082e73ef1 374
s1725696 12:b2b082e73ef1 375 wait(0.01);
s1725696 12:b2b082e73ef1 376 }
s1725696 12:b2b082e73ef1 377
s1725696 12:b2b082e73ef1 378 pos_store(Counts2(counts2));
s1725696 12:b2b082e73ef1 379 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 12:b2b082e73ef1 380 }
s1725696 12:b2b082e73ef1 381
s1725696 10:56136a0da8c1 382 void motor_calibration()
s1725696 10:56136a0da8c1 383 {
s1725696 12:b2b082e73ef1 384 // translation
s1725696 12:b2b082e73ef1 385 calibration_translation();
s1725696 12:b2b082e73ef1 386
s1725696 12:b2b082e73ef1 387 pc.printf("before wait\r\n");
s1725696 12:b2b082e73ef1 388 wait(1.5);
s1725696 12:b2b082e73ef1 389
s1725696 12:b2b082e73ef1 390 // rotation
s1725696 12:b2b082e73ef1 391 calibration_rotation();
s1725696 12:b2b082e73ef1 392
s1725696 12:b2b082e73ef1 393 pc.printf("Motor calibration done");
s1725696 10:56136a0da8c1 394 }
s1725696 10:56136a0da8c1 395
s1725696 10:56136a0da8c1 396 // EMG_CAL
s1725696 10:56136a0da8c1 397 // To calibrate the EMG signal to some boundary values
s1725696 13:c087c0f64769 398
s1725696 13:c087c0f64769 399 // Void to switch between signals to calibrate
s1725696 13:c087c0f64769 400 void switch_to_calibrate()
s1725696 13:c087c0f64769 401 {
s1725696 13:c087c0f64769 402 g++;
s1725696 13:c087c0f64769 403 //If g = 0, led is blue
s1725696 13:c087c0f64769 404 if (g == 0)
s1725696 13:c087c0f64769 405 {
s1725696 13:c087c0f64769 406 led_blue==0;
s1725696 13:c087c0f64769 407 led_red==1;
s1725696 13:c087c0f64769 408 led_green==1;
s1725696 13:c087c0f64769 409 }
s1725696 13:c087c0f64769 410 //If g = 1, led is red
s1725696 13:c087c0f64769 411 else if(g == 1)
s1725696 13:c087c0f64769 412 {
s1725696 13:c087c0f64769 413 led_blue==1;
s1725696 13:c087c0f64769 414 led_red==0;
s1725696 13:c087c0f64769 415 led_green==1;
s1725696 13:c087c0f64769 416 }
s1725696 13:c087c0f64769 417 //If g = 2, led is green
s1725696 13:c087c0f64769 418 else if(g == 2)
s1725696 13:c087c0f64769 419 {
s1725696 13:c087c0f64769 420 led_blue==1;
s1725696 13:c087c0f64769 421 led_red==1;
s1725696 13:c087c0f64769 422 led_green==0;
s1725696 13:c087c0f64769 423 }
s1725696 13:c087c0f64769 424 //If g > 3, led is white
s1725696 13:c087c0f64769 425 else
s1725696 13:c087c0f64769 426 {
s1725696 13:c087c0f64769 427 led_blue==0;
s1725696 13:c087c0f64769 428 led_red==0;
s1725696 13:c087c0f64769 429 led_green==0;
s1725696 13:c087c0f64769 430 emg_calib = 0;
s1725696 13:c087c0f64769 431 g = 0;
s1725696 13:c087c0f64769 432 }
s1725696 13:c087c0f64769 433 }
s1725696 13:c087c0f64769 434
s1725696 13:c087c0f64769 435 // Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
s1725696 13:c087c0f64769 436 void calibrate(void)
s1725696 13:c087c0f64769 437 {
s1725696 13:c087c0f64769 438 switch (g)
s1725696 13:c087c0f64769 439 {
s1725696 13:c087c0f64769 440 case 0:
s1725696 13:c087c0f64769 441 { // Case zero, calibrate EMG signal of right biceps
s1725696 13:c087c0f64769 442 sum = 0.0;
s1725696 13:c087c0f64769 443
s1725696 13:c087c0f64769 444 //For statement to make an array of the latest datapoints of the filtered signal
s1725696 13:c087c0f64769 445 for (int j = 0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 446 {
s1725696 13:c087c0f64769 447
s1725696 13:c087c0f64769 448 StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 449 sum+=StoreCali0[j]; // Sums the elements in the array
s1725696 13:c087c0f64769 450 wait(0.001f);
s1725696 13:c087c0f64769 451 }
s1725696 13:c087c0f64769 452 Mean0 = sum/sizeCali; // Calculates the mean of the signal
s1725696 13:c087c0f64769 453 Threshold0 = Mean0; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 454 break;
s1725696 13:c087c0f64769 455 }
s1725696 13:c087c0f64769 456 case 1:
s1725696 13:c087c0f64769 457 { // Case one, calibrate EMG signal of left biceps
s1725696 13:c087c0f64769 458 sum = 0.0;
s1725696 13:c087c0f64769 459 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 460 {
s1725696 13:c087c0f64769 461 StoreCali1[j] = low1;
s1725696 13:c087c0f64769 462 sum+=StoreCali1[j];
s1725696 13:c087c0f64769 463 wait(0.001f);
s1725696 13:c087c0f64769 464 }
s1725696 13:c087c0f64769 465 Mean1 = sum/sizeCali;
s1725696 13:c087c0f64769 466 Threshold1 = Mean1; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 467 break;
s1725696 13:c087c0f64769 468 }
s1725696 13:c087c0f64769 469 case 2:
s1725696 13:c087c0f64769 470 { // Case two, calibrate EMG signal of calf
s1725696 13:c087c0f64769 471 sum = 0.0;
s1725696 13:c087c0f64769 472 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 473 {
s1725696 13:c087c0f64769 474 StoreCali2[j] = low2;
s1725696 13:c087c0f64769 475 sum+=StoreCali2[j];
s1725696 13:c087c0f64769 476 wait(0.001f);
s1725696 13:c087c0f64769 477 }
s1725696 13:c087c0f64769 478 Mean2 = sum/sizeCali;
s1725696 13:c087c0f64769 479 Threshold2 = Mean2; //Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 480 break;
s1725696 13:c087c0f64769 481 }
s1725696 13:c087c0f64769 482 case 3:
s1725696 13:c087c0f64769 483 { // Sets calibration value to 1; robot can be set to Home position
s1725696 13:c087c0f64769 484 emg_calib=1;
s1725696 13:c087c0f64769 485 wait(0.001f);
s1725696 13:c087c0f64769 486 break;
s1725696 13:c087c0f64769 487 }
s1725696 13:c087c0f64769 488 default:
s1725696 13:c087c0f64769 489 { // Ensures nothing happens if g is not equal to 0, 1 or 2.
s1725696 13:c087c0f64769 490 break;
s1725696 13:c087c0f64769 491 }
s1725696 13:c087c0f64769 492 }
s1725696 13:c087c0f64769 493 }
s1725696 13:c087c0f64769 494
s1725696 13:c087c0f64769 495 // Void to calibrate the EMG signal
s1725696 10:56136a0da8c1 496 void emg_calibration()
s1725696 10:56136a0da8c1 497 {
s1725696 13:c087c0f64769 498 for(int m = 1; m <= 4; m++)
s1725696 13:c087c0f64769 499 {
s1725696 13:c087c0f64769 500 led_blue == 0;
s1725696 13:c087c0f64769 501 led_red == 1;
s1725696 13:c087c0f64769 502 led_green == 1;
s1725696 13:c087c0f64769 503
s1725696 13:c087c0f64769 504 MovAg_tick.attach(&MovAg,T); // Moving average calculation every T sec.
s1725696 13:c087c0f64769 505
s1725696 13:c087c0f64769 506
s1725696 13:c087c0f64769 507 pc.printf("g is %i\n\r",g);
s1725696 13:c087c0f64769 508 pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
s1725696 13:c087c0f64769 509 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
s1725696 13:c087c0f64769 510
s1725696 13:c087c0f64769 511 bool k = true;
s1725696 13:c087c0f64769 512 while(k == true)
s1725696 13:c087c0f64769 513 {
s1725696 13:c087c0f64769 514 if(button_motorcal == false)
s1725696 13:c087c0f64769 515 {
s1725696 13:c087c0f64769 516 calibrate(); // Calibrate threshold for 3 muscles
s1725696 13:c087c0f64769 517 k = false;
s1725696 13:c087c0f64769 518 }
s1725696 13:c087c0f64769 519 wait(0.2f); // Wait to avoid bouncing of button
s1725696 13:c087c0f64769 520 }
s1725696 13:c087c0f64769 521
s1725696 13:c087c0f64769 522 bool h = true;
s1725696 13:c087c0f64769 523 while(h == true)
s1725696 13:c087c0f64769 524 {
s1725696 13:c087c0f64769 525 if (button_demo == false)
s1725696 13:c087c0f64769 526 {
s1725696 13:c087c0f64769 527 switch_to_calibrate(); // Switch state of calibration (which muscle)
s1725696 13:c087c0f64769 528 h = false;
s1725696 13:c087c0f64769 529 }
s1725696 13:c087c0f64769 530
s1725696 13:c087c0f64769 531 wait(0.2f); // Wait to avoid bouncing of button
s1725696 13:c087c0f64769 532 }
s1725696 13:c087c0f64769 533 }
s1725696 10:56136a0da8c1 534 }
s1725696 10:56136a0da8c1 535
s1725696 9:d7a6a3619576 536 // PID controller
s1725696 9:d7a6a3619576 537 // To control the input signal before it goes into the motor control
s1725696 9:d7a6a3619576 538 // Simon mee bezig
s1725696 10:56136a0da8c1 539 void pid_controller()
s1725696 10:56136a0da8c1 540 {
s1725696 10:56136a0da8c1 541 // Simons code here
s1725696 10:56136a0da8c1 542 }
s1725696 0:cb8857cf3ea4 543
s1725696 10:56136a0da8c1 544 // START
s1725696 0:cb8857cf3ea4 545 // To move the robot to the starting position: middle
s1725696 10:56136a0da8c1 546 void start_mode()
s1725696 4:8183e7b228f0 547 {
s1725696 12:b2b082e73ef1 548 // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
s1725696 4:8183e7b228f0 549 }
s1725696 0:cb8857cf3ea4 550
s1725696 10:56136a0da8c1 551 // OPERATING
s1725696 0:cb8857cf3ea4 552 // To control the robot with EMG signals
s1725696 11:79311abb2bc2 553 // Kenneth bezig met motor aansturen
s1725696 12:b2b082e73ef1 554 void operating()
s1725696 12:b2b082e73ef1 555 {
s1725696 12:b2b082e73ef1 556 servocontrol(); // make sure the servo is used
s1725696 12:b2b082e73ef1 557 // RKI fuction
s1725696 12:b2b082e73ef1 558 }
s1725696 0:cb8857cf3ea4 559
s1725696 10:56136a0da8c1 560 // DEMO
s1725696 0:cb8857cf3ea4 561 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 562 // Voor het laatst bewaren
s1725696 10:56136a0da8c1 563 void demo_mode()
s1725696 10:56136a0da8c1 564 {
s1725696 10:56136a0da8c1 565 // code here
s1725696 10:56136a0da8c1 566 }
s1725696 0:cb8857cf3ea4 567
s1725696 10:56136a0da8c1 568 // FAILURE
s1725696 0:cb8857cf3ea4 569 // To shut down the robot after an error etc
s1725696 13:c087c0f64769 570 void failure_mode()
s1725696 10:56136a0da8c1 571 {
s1725696 10:56136a0da8c1 572 // code here
s1725696 10:56136a0da8c1 573 }
s1725696 0:cb8857cf3ea4 574
s1725696 0:cb8857cf3ea4 575 // Main function
s1725696 4:8183e7b228f0 576 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 577 int main()
s1725696 0:cb8857cf3ea4 578 {
s1725696 0:cb8857cf3ea4 579 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 580 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 9:d7a6a3619576 581 pwmpin_1.period_us(60); // Setting period for PWM
s1725696 10:56136a0da8c1 582 ticker.attach(&hidscope,0.002f); // Send information to HIDScope
s1725696 13:c087c0f64769 583 Filter_tick.attach(&filtering,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
s1725696 1:1a8211e1f3f3 584
s1725696 1:1a8211e1f3f3 585 while(true){
s1725696 6:f495a77c2c95 586 // timer
s1725696 6:f495a77c2c95 587 clock_t start; // start the timer
s1725696 6:f495a77c2c95 588 start = clock();
s1725696 9:d7a6a3619576 589 time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 5:3581013d4505 590
s1725696 6:f495a77c2c95 591 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 592 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 593
s1725696 6:f495a77c2c95 594 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 9:d7a6a3619576 595 switch(CurrentState)
s1725696 6:f495a77c2c95 596 {
s1725696 6:f495a77c2c95 597 case WAIT:
s1725696 6:f495a77c2c95 598
s1725696 6:f495a77c2c95 599 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 600 {
s1725696 10:56136a0da8c1 601 pc.printf("State is WAIT\r\n");
s1725696 10:56136a0da8c1 602
s1725696 6:f495a77c2c95 603 // Execute WAIT mode
s1725696 10:56136a0da8c1 604 wait_mode();
s1725696 10:56136a0da8c1 605
s1725696 6:f495a77c2c95 606 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 607 }
s1725696 6:f495a77c2c95 608
s1725696 12:b2b082e73ef1 609 if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 610 {
s1725696 6:f495a77c2c95 611 CurrentState = MOTOR_CAL;
s1725696 9:d7a6a3619576 612 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 613 StateChanged = true;
s1725696 6:f495a77c2c95 614 }
s1725696 7:ec5add330cb3 615
s1725696 12:b2b082e73ef1 616 if (button_emergency == false) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 617 {
s1725696 7:ec5add330cb3 618 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 619 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 620 StateChanged = true;
s1725696 7:ec5add330cb3 621 }
s1725696 6:f495a77c2c95 622
s1725696 6:f495a77c2c95 623 break;
s1725696 6:f495a77c2c95 624
s1725696 6:f495a77c2c95 625 case MOTOR_CAL:
s1725696 6:f495a77c2c95 626
s1725696 6:f495a77c2c95 627 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 628 {
s1725696 9:d7a6a3619576 629 t.reset();
s1725696 6:f495a77c2c95 630 t.start();
s1725696 10:56136a0da8c1 631
s1725696 6:f495a77c2c95 632 // Execute MOTOR_CAL mode
s1725696 10:56136a0da8c1 633 motor_calibration();
s1725696 10:56136a0da8c1 634
s1725696 6:f495a77c2c95 635 t.stop();
s1725696 6:f495a77c2c95 636 time_in_state = t.read();
s1725696 9:d7a6a3619576 637 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 638
s1725696 6:f495a77c2c95 639 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 640 }
s1725696 6:f495a77c2c95 641
s1725696 12:b2b082e73ef1 642 if((time_in_state > 3.0) && (pwmpin_1 < 0.1) && (pwmpin_2 < 0.1)) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
s1725696 6:f495a77c2c95 643 {
s1725696 6:f495a77c2c95 644 CurrentState = EMG_CAL;
s1725696 9:d7a6a3619576 645 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 646 StateChanged = true;
s1725696 6:f495a77c2c95 647 }
s1725696 6:f495a77c2c95 648
s1725696 12:b2b082e73ef1 649 if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 650 {
s1725696 7:ec5add330cb3 651 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 652 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 653 StateChanged = true;
s1725696 7:ec5add330cb3 654 }
s1725696 7:ec5add330cb3 655
s1725696 6:f495a77c2c95 656 break;
s1725696 6:f495a77c2c95 657
s1725696 6:f495a77c2c95 658 case EMG_CAL:
s1725696 6:f495a77c2c95 659
s1725696 6:f495a77c2c95 660 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 661 {
s1725696 6:f495a77c2c95 662 t.reset();
s1725696 6:f495a77c2c95 663 t.start();
s1725696 10:56136a0da8c1 664
s1725696 6:f495a77c2c95 665 // Execute EMG_CAL mode
s1725696 10:56136a0da8c1 666 emg_calibration();
s1725696 10:56136a0da8c1 667
s1725696 6:f495a77c2c95 668 t.stop();
s1725696 6:f495a77c2c95 669 time_in_state = t.read();
s1725696 9:d7a6a3619576 670 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 671
s1725696 6:f495a77c2c95 672 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 673 }
s1725696 6:f495a77c2c95 674
s1725696 9:d7a6a3619576 675 if((time_in_state > 5) && (EMG < 0.01)) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 676 {
s1725696 6:f495a77c2c95 677 CurrentState = START;
s1725696 9:d7a6a3619576 678 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 679 StateChanged = true;
s1725696 6:f495a77c2c95 680 }
s1725696 6:f495a77c2c95 681
s1725696 12:b2b082e73ef1 682 if (button_emergency == false) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 683 {
s1725696 7:ec5add330cb3 684 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 685 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 686 StateChanged = true;
s1725696 7:ec5add330cb3 687 }
s1725696 7:ec5add330cb3 688
s1725696 6:f495a77c2c95 689 break;
s1725696 6:f495a77c2c95 690
s1725696 6:f495a77c2c95 691 case START:
s1725696 6:f495a77c2c95 692
s1725696 6:f495a77c2c95 693 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 694 {
s1725696 6:f495a77c2c95 695 t.reset();
s1725696 6:f495a77c2c95 696 t.start();
s1725696 10:56136a0da8c1 697
s1725696 6:f495a77c2c95 698 // Execute START mode
s1725696 10:56136a0da8c1 699 start_mode();
s1725696 10:56136a0da8c1 700
s1725696 6:f495a77c2c95 701 t.stop();
s1725696 6:f495a77c2c95 702 time_in_state = t.read();
s1725696 9:d7a6a3619576 703 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 704
s1725696 6:f495a77c2c95 705 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 706 }
s1725696 6:f495a77c2c95 707
s1725696 9:d7a6a3619576 708 if((time_in_state > 5) && (errors < 0.01)) // condition for START --> OPERATING; 5s and error is small
s1725696 6:f495a77c2c95 709 {
s1725696 6:f495a77c2c95 710 CurrentState = OPERATING;
s1725696 9:d7a6a3619576 711 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 712 StateChanged = true;
s1725696 6:f495a77c2c95 713 }
s1725696 6:f495a77c2c95 714
s1725696 12:b2b082e73ef1 715 if (button_emergency == false) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 716 {
s1725696 7:ec5add330cb3 717 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 718 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 719 StateChanged = true;
s1725696 7:ec5add330cb3 720 }
s1725696 7:ec5add330cb3 721
s1725696 6:f495a77c2c95 722 break;
s1725696 6:f495a77c2c95 723
s1725696 6:f495a77c2c95 724 case OPERATING:
s1725696 6:f495a77c2c95 725
s1725696 6:f495a77c2c95 726 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 727 {
s1725696 6:f495a77c2c95 728 // Execute OPERATING mode
s1725696 10:56136a0da8c1 729
s1725696 6:f495a77c2c95 730 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 731 }
s1725696 6:f495a77c2c95 732
s1725696 12:b2b082e73ef1 733 if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 734 {
s1725696 6:f495a77c2c95 735 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 736 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 737 StateChanged = true;
s1725696 6:f495a77c2c95 738 }
s1725696 6:f495a77c2c95 739
s1725696 12:b2b082e73ef1 740 if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 741 {
s1725696 6:f495a77c2c95 742 CurrentState = DEMO;
s1725696 9:d7a6a3619576 743 pc.printf("State is DEMO\r\n");
s1725696 9:d7a6a3619576 744 StateChanged = true;
s1725696 9:d7a6a3619576 745 }
s1725696 9:d7a6a3619576 746
s1725696 12:b2b082e73ef1 747 if(button_wait == false) // condition OPERATING --> WAIT; button_wait press
s1725696 9:d7a6a3619576 748 {
s1725696 9:d7a6a3619576 749 CurrentState = WAIT;
s1725696 9:d7a6a3619576 750 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 751 StateChanged = true;
s1725696 6:f495a77c2c95 752 }
s1725696 6:f495a77c2c95 753
s1725696 6:f495a77c2c95 754 break;
s1725696 6:f495a77c2c95 755
s1725696 6:f495a77c2c95 756 case FAILURE:
s1725696 6:f495a77c2c95 757
s1725696 6:f495a77c2c95 758 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 759 {
s1725696 6:f495a77c2c95 760 // Execute FAILURE mode
s1725696 10:56136a0da8c1 761 failure_mode();
s1725696 10:56136a0da8c1 762
s1725696 6:f495a77c2c95 763 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 764 }
s1725696 6:f495a77c2c95 765
s1725696 12:b2b082e73ef1 766 if(button_wait == false) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?)
s1725696 6:f495a77c2c95 767 {
s1725696 6:f495a77c2c95 768 CurrentState = WAIT;
s1725696 9:d7a6a3619576 769 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 770 StateChanged = true;
s1725696 6:f495a77c2c95 771 }
s1725696 6:f495a77c2c95 772
s1725696 6:f495a77c2c95 773 break;
s1725696 6:f495a77c2c95 774
s1725696 6:f495a77c2c95 775 case DEMO:
s1725696 6:f495a77c2c95 776
s1725696 6:f495a77c2c95 777 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 778 {
s1725696 6:f495a77c2c95 779 // Execute DEMO mode
s1725696 10:56136a0da8c1 780 demo_mode();
s1725696 10:56136a0da8c1 781
s1725696 6:f495a77c2c95 782 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 783 }
s1725696 6:f495a77c2c95 784
s1725696 12:b2b082e73ef1 785 if(button_wait == false) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 786 {
s1725696 6:f495a77c2c95 787 CurrentState = WAIT;
s1725696 9:d7a6a3619576 788 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 789 StateChanged = true;
s1725696 6:f495a77c2c95 790 }
s1725696 6:f495a77c2c95 791
s1725696 12:b2b082e73ef1 792 if (button_emergency == false) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 793 {
s1725696 6:f495a77c2c95 794 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 795 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 796 StateChanged = true;
s1725696 6:f495a77c2c95 797 }
s1725696 6:f495a77c2c95 798
s1725696 6:f495a77c2c95 799 break;
s1725696 6:f495a77c2c95 800
s1725696 6:f495a77c2c95 801 // no default
s1725696 4:8183e7b228f0 802 }
s1725696 6:f495a77c2c95 803
s1725696 6:f495a77c2c95 804 // while loop does not have to loop every time
s1725696 5:3581013d4505 805 }
s1725696 5:3581013d4505 806
s1725696 0:cb8857cf3ea4 807 }