Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@15:2772f8cbf382, 2018-11-01 (annotated)

Committer:
s1725696
Date:
Thu Nov 01 11:00:14 2018 +0000
Revision:
15:2772f8cbf382
Parent:
14:abc125dcc246
Child:
16:37b491eac34b
New conditions for each state;

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 14:abc125dcc246 162 // Potmeter for controlling motor, must be removed when EMG is used to control the motor
s1725696 5:3581013d4505 163 float potmeter()
s1725696 5:3581013d4505 164 {
s1725696 14:abc125dcc246 165 float out_2 = (pot_1 * 2.0f) - 1.0f;
s1725696 5:3581013d4505 166
s1725696 5:3581013d4505 167 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 168 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 169
s1725696 5:3581013d4505 170 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 171 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 172
s1725696 5:3581013d4505 173 return out_2;
s1725696 6:f495a77c2c95 174 }
s1725696 9:d7a6a3619576 175
s1725696 11:79311abb2bc2 176 // Servo control
s1725696 12:b2b082e73ef1 177 // To lift the pen up, with a push of button
s1725696 11:79311abb2bc2 178 void servocontrol()
s1725696 11:79311abb2bc2 179 {
s1725696 14:abc125dcc246 180 while (button_motorcal == false) // If button is pushed, pen should go up
s1725696 11:79311abb2bc2 181 {
s1725696 11:79311abb2bc2 182 myservo = 0.1;
s1725696 11:79311abb2bc2 183 }
s1725696 14:abc125dcc246 184 myservo = 0.0;
s1725696 6:f495a77c2c95 185 }
s1725696 9:d7a6a3619576 186
s1725696 10:56136a0da8c1 187 // EMG filter
s1725696 10:56136a0da8c1 188 // To process the EMG signal before information can be caught from it
s1725696 14:abc125dcc246 189
s1725696 14:abc125dcc246 190 // Filter of the first EMG signal
s1725696 13:c087c0f64769 191 void filtering()
s1725696 13:c087c0f64769 192 {
s1725696 13:c087c0f64769 193 // Reading the EMG signal
s1725696 13:c087c0f64769 194 emg0 = emg0_in.read();
s1725696 13:c087c0f64769 195 emg1 = emg1_in.read();
s1725696 13:c087c0f64769 196 emg2 = emg2_in.read();
s1725696 13:c087c0f64769 197
s1725696 13:c087c0f64769 198 // Applying a notch filter over the EMG data
s1725696 13:c087c0f64769 199 notch0 = N1.step(emg0);
s1725696 13:c087c0f64769 200 notch1 = N2.step(emg1);
s1725696 13:c087c0f64769 201 notch2 = N3.step(emg2);
s1725696 13:c087c0f64769 202
s1725696 13:c087c0f64769 203 // Applying a high pass filter
s1725696 13:c087c0f64769 204 high0 = H1.step(notch0);
s1725696 13:c087c0f64769 205 high1 = H2.step(notch1);
s1725696 13:c087c0f64769 206 high2 = H3.step(notch2);
s1725696 13:c087c0f64769 207
s1725696 13:c087c0f64769 208 // Rectifying the signal
s1725696 13:c087c0f64769 209 absolute0 = fabs(high0);
s1725696 13:c087c0f64769 210 absolute1 = fabs(high1);
s1725696 13:c087c0f64769 211 absolute2 = fabs(high2);
s1725696 13:c087c0f64769 212
s1725696 13:c087c0f64769 213 // Applying low pass filter
s1725696 13:c087c0f64769 214 low0 = L1.step(absolute0);
s1725696 13:c087c0f64769 215 low1 = L2.step(absolute1);
s1725696 13:c087c0f64769 216 low2 = L3.step(absolute2);
s1725696 13:c087c0f64769 217 }
s1725696 13:c087c0f64769 218
s1725696 13:c087c0f64769 219 // Moving average filter
s1725696 13:c087c0f64769 220 // To determine the moving average, apply after filtering
s1725696 13:c087c0f64769 221 void MovAg()
s1725696 13:c087c0f64769 222 {
s1725696 13:c087c0f64769 223 // For statement to make an array of the last datapoints of the filtered signal
s1725696 13:c087c0f64769 224 for (int i = sizeMovAg - 1; i >= 0; i--)
s1725696 13:c087c0f64769 225 {
s1725696 13:c087c0f64769 226 // Shifts the i'th element one place to the right
s1725696 13:c087c0f64769 227 StoreArray0[i] = StoreArray0[i-1];
s1725696 13:c087c0f64769 228 StoreArray1[i] = StoreArray1[i-1];
s1725696 13:c087c0f64769 229 StoreArray2[i] = StoreArray2[i-1];
s1725696 13:c087c0f64769 230 }
s1725696 13:c087c0f64769 231
s1725696 13:c087c0f64769 232 // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 233 StoreArray0[0] = low0;
s1725696 13:c087c0f64769 234 StoreArray1[0] = low1;
s1725696 13:c087c0f64769 235 StoreArray2[0] = low2;
s1725696 13:c087c0f64769 236 sum1 = 0.0;
s1725696 13:c087c0f64769 237 sum2 = 0.0;
s1725696 13:c087c0f64769 238 sum3 = 0.0;
s1725696 13:c087c0f64769 239
s1725696 13:c087c0f64769 240 // For statement to sum the elements in the array
s1725696 13:c087c0f64769 241 for (int a = 0; a<=sizeMovAg-1; a++)
s1725696 13:c087c0f64769 242 {
s1725696 13:c087c0f64769 243 sum1+=StoreArray0[a];
s1725696 13:c087c0f64769 244 sum2+=StoreArray1[a];
s1725696 13:c087c0f64769 245 sum3+=StoreArray2[a];
s1725696 13:c087c0f64769 246 }
s1725696 13:c087c0f64769 247
s1725696 13:c087c0f64769 248 // Calculates an average over the datapoints in the array
s1725696 13:c087c0f64769 249 Average0 = sum1/sizeMovAg;
s1725696 13:c087c0f64769 250 Average1 = sum2/sizeMovAg;
s1725696 13:c087c0f64769 251 Average2 = sum3/sizeMovAg;
s1725696 13:c087c0f64769 252
s1725696 14:abc125dcc246 253 // Sending the signal to the HIDScope
s1725696 14:abc125dcc246 254 // Change the number of channels in the beginning of the script when necessary
s1725696 14:abc125dcc246 255 scope.set( 0, emg0);
s1725696 14:abc125dcc246 256 scope.set( 1, low0);
s1725696 13:c087c0f64769 257 scope.set( 2, Average0);
s1725696 13:c087c0f64769 258 scope.set( 3, low1);
s1725696 13:c087c0f64769 259 scope.set( 4, emg2);
s1725696 13:c087c0f64769 260 scope.set( 5, low2);
s1725696 13:c087c0f64769 261 scope.send();
s1725696 14:abc125dcc246 262 }
s1725696 10:56136a0da8c1 263
s1725696 10:56136a0da8c1 264 // WAIT
s1725696 10:56136a0da8c1 265 // To do nothing
s1725696 10:56136a0da8c1 266 void wait_mode()
s1725696 10:56136a0da8c1 267 {
s1725696 10:56136a0da8c1 268 // go back to the initial values
s1725696 10:56136a0da8c1 269 // Copy here the variables list with initial values
s1725696 13:c087c0f64769 270 // all pwm's to zero
s1725696 13:c087c0f64769 271 // all counts to zero
s1725696 10:56136a0da8c1 272 }
s1725696 10:56136a0da8c1 273
s1725696 10:56136a0da8c1 274 // MOTOR_CAL
s1725696 10:56136a0da8c1 275 // To calibrate the motor angle to some mechanical boundaries
s1725696 10:56136a0da8c1 276 // Kenneth mee bezig
s1725696 12:b2b082e73ef1 277 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 12:b2b082e73ef1 278
s1725696 13:c087c0f64769 279 if (tower_1_position == 0.1)
s1725696 13:c087c0f64769 280 {
s1725696 12:b2b082e73ef1 281 tower_1_position = a;
s1725696 12:b2b082e73ef1 282 }
s1725696 13:c087c0f64769 283 else if (tower_end_position == 0.1)
s1725696 13:c087c0f64769 284 {
s1725696 12:b2b082e73ef1 285 tower_end_position = a;
s1725696 12:b2b082e73ef1 286 }
s1725696 13:c087c0f64769 287 else if (rotation_start_position == 0.1)
s1725696 13:c087c0f64769 288 {
s1725696 12:b2b082e73ef1 289 rotation_start_position = a;
s1725696 12:b2b082e73ef1 290 }
s1725696 12:b2b082e73ef1 291 }
s1725696 12:b2b082e73ef1 292
s1725696 12:b2b082e73ef1 293 // Start translation
s1725696 12:b2b082e73ef1 294 void translation_start(int a, float b) // a = dir , b = speed
s1725696 12:b2b082e73ef1 295 {
s1725696 12:b2b082e73ef1 296 dirpin_1.write(a);
s1725696 12:b2b082e73ef1 297 pwmpin_1 = b;
s1725696 12:b2b082e73ef1 298 }
s1725696 12:b2b082e73ef1 299
s1725696 12:b2b082e73ef1 300 // Stop translation
s1725696 12:b2b082e73ef1 301 void translation_stop()
s1725696 12:b2b082e73ef1 302 {
s1725696 12:b2b082e73ef1 303 pwmpin_1 = 0.0;
s1725696 12:b2b082e73ef1 304 }
s1725696 12:b2b082e73ef1 305
s1725696 12:b2b082e73ef1 306 // Start rotation
s1725696 12:b2b082e73ef1 307 void rotation_start(int a, float b)
s1725696 12:b2b082e73ef1 308 {
s1725696 12:b2b082e73ef1 309 dirpin_2.write(a);
s1725696 12:b2b082e73ef1 310 pwmpin_2 = b;
s1725696 12:b2b082e73ef1 311 }
s1725696 12:b2b082e73ef1 312
s1725696 12:b2b082e73ef1 313 // Stop rotation
s1725696 12:b2b082e73ef1 314 void rotation_stop()
s1725696 12:b2b082e73ef1 315 {
s1725696 12:b2b082e73ef1 316 pwmpin_2 = 0.0;
s1725696 12:b2b082e73ef1 317 }
s1725696 12:b2b082e73ef1 318
s1725696 12:b2b082e73ef1 319 // Calibration of translation
s1725696 12:b2b082e73ef1 320 void calibration_translation()
s1725696 12:b2b082e73ef1 321 {
s1725696 12:b2b082e73ef1 322 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 12:b2b082e73ef1 323 {
s1725696 14:abc125dcc246 324 // dir = 0, means that the pen moves to the translation motor, dir = 1 means it moves to the rotation motor
s1725696 12:b2b082e73ef1 325 pc.printf("\r\nTranslatie loop\r\n");
s1725696 12:b2b082e73ef1 326 translation_start(dir,speed);
s1725696 12:b2b082e73ef1 327 pc.printf("Direction = %i\r\n", dir);
s1725696 12:b2b082e73ef1 328
s1725696 12:b2b082e73ef1 329 bool g = true; // to make a condition for the while loop
s1725696 12:b2b082e73ef1 330 while (g == true)
s1725696 12:b2b082e73ef1 331 {
s1725696 12:b2b082e73ef1 332 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 12:b2b082e73ef1 333 {
s1725696 12:b2b082e73ef1 334 translation_stop();
s1725696 12:b2b082e73ef1 335 pos_store(Counts1(counts1));
s1725696 12:b2b082e73ef1 336 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 337 dir = dir + 1;
s1725696 12:b2b082e73ef1 338
s1725696 12:b2b082e73ef1 339 g = false; // to end the while loop
s1725696 12:b2b082e73ef1 340 }
s1725696 12:b2b082e73ef1 341
s1725696 12:b2b082e73ef1 342 wait(0.01);
s1725696 12:b2b082e73ef1 343 }
s1725696 12:b2b082e73ef1 344
s1725696 15:2772f8cbf382 345 wait(1.0); // wait 3 seconds before next round of translation/rotation
s1725696 12:b2b082e73ef1 346 }
s1725696 12:b2b082e73ef1 347 }
s1725696 12:b2b082e73ef1 348
s1725696 12:b2b082e73ef1 349 void calibration_rotation()
s1725696 12:b2b082e73ef1 350 {
s1725696 12:b2b082e73ef1 351 rotation_start(dir, speed);
s1725696 12:b2b082e73ef1 352 pc.printf("\r\nRotatie start\r\n");
s1725696 12:b2b082e73ef1 353
s1725696 12:b2b082e73ef1 354 bool f = true; // condition for while loop
s1725696 12:b2b082e73ef1 355 while(f == true)
s1725696 12:b2b082e73ef1 356 {
s1725696 12:b2b082e73ef1 357 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 12:b2b082e73ef1 358 {
s1725696 12:b2b082e73ef1 359 rotation_stop();
s1725696 12:b2b082e73ef1 360
s1725696 12:b2b082e73ef1 361 f = false; // to end the while loop
s1725696 12:b2b082e73ef1 362 }
s1725696 12:b2b082e73ef1 363
s1725696 12:b2b082e73ef1 364 wait(0.01);
s1725696 12:b2b082e73ef1 365 }
s1725696 14:abc125dcc246 366 int start_counts = 0;
s1725696 14:abc125dcc246 367 pos_store(Counts2(start_counts));
s1725696 12:b2b082e73ef1 368 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 369 }
s1725696 12:b2b082e73ef1 370
s1725696 10:56136a0da8c1 371 void motor_calibration()
s1725696 10:56136a0da8c1 372 {
s1725696 12:b2b082e73ef1 373 // translation
s1725696 12:b2b082e73ef1 374 calibration_translation();
s1725696 12:b2b082e73ef1 375
s1725696 12:b2b082e73ef1 376 pc.printf("before wait\r\n");
s1725696 12:b2b082e73ef1 377 wait(1.5);
s1725696 12:b2b082e73ef1 378
s1725696 12:b2b082e73ef1 379 // rotation
s1725696 12:b2b082e73ef1 380 calibration_rotation();
s1725696 12:b2b082e73ef1 381
s1725696 12:b2b082e73ef1 382 pc.printf("Motor calibration done");
s1725696 10:56136a0da8c1 383 }
s1725696 10:56136a0da8c1 384
s1725696 10:56136a0da8c1 385 // EMG_CAL
s1725696 10:56136a0da8c1 386 // To calibrate the EMG signal to some boundary values
s1725696 13:c087c0f64769 387 // Void to switch between signals to calibrate
s1725696 13:c087c0f64769 388 void switch_to_calibrate()
s1725696 13:c087c0f64769 389 {
s1725696 13:c087c0f64769 390 g++;
s1725696 13:c087c0f64769 391 //If g = 0, led is blue
s1725696 13:c087c0f64769 392 if (g == 0)
s1725696 13:c087c0f64769 393 {
s1725696 13:c087c0f64769 394 led_blue==0;
s1725696 13:c087c0f64769 395 led_red==1;
s1725696 13:c087c0f64769 396 led_green==1;
s1725696 13:c087c0f64769 397 }
s1725696 13:c087c0f64769 398 //If g = 1, led is red
s1725696 13:c087c0f64769 399 else if(g == 1)
s1725696 13:c087c0f64769 400 {
s1725696 13:c087c0f64769 401 led_blue==1;
s1725696 13:c087c0f64769 402 led_red==0;
s1725696 13:c087c0f64769 403 led_green==1;
s1725696 13:c087c0f64769 404 }
s1725696 13:c087c0f64769 405 //If g = 2, led is green
s1725696 13:c087c0f64769 406 else if(g == 2)
s1725696 13:c087c0f64769 407 {
s1725696 13:c087c0f64769 408 led_blue==1;
s1725696 13:c087c0f64769 409 led_red==1;
s1725696 13:c087c0f64769 410 led_green==0;
s1725696 13:c087c0f64769 411 }
s1725696 13:c087c0f64769 412 //If g > 3, led is white
s1725696 13:c087c0f64769 413 else
s1725696 13:c087c0f64769 414 {
s1725696 13:c087c0f64769 415 led_blue==0;
s1725696 13:c087c0f64769 416 led_red==0;
s1725696 13:c087c0f64769 417 led_green==0;
s1725696 13:c087c0f64769 418 emg_calib = 0;
s1725696 13:c087c0f64769 419 g = 0;
s1725696 13:c087c0f64769 420 }
s1725696 14:abc125dcc246 421 }
s1725696 13:c087c0f64769 422
s1725696 13:c087c0f64769 423 // Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
s1725696 14:abc125dcc246 424 void calibrate()
s1725696 13:c087c0f64769 425 {
s1725696 13:c087c0f64769 426 switch (g)
s1725696 13:c087c0f64769 427 {
s1725696 13:c087c0f64769 428 case 0:
s1725696 13:c087c0f64769 429 { // Case zero, calibrate EMG signal of right biceps
s1725696 13:c087c0f64769 430 sum = 0.0;
s1725696 13:c087c0f64769 431
s1725696 13:c087c0f64769 432 //For statement to make an array of the latest datapoints of the filtered signal
s1725696 13:c087c0f64769 433 for (int j = 0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 434 {
s1725696 13:c087c0f64769 435
s1725696 13:c087c0f64769 436 StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 437 sum+=StoreCali0[j]; // Sums the elements in the array
s1725696 13:c087c0f64769 438 wait(0.001f);
s1725696 13:c087c0f64769 439 }
s1725696 13:c087c0f64769 440 Mean0 = sum/sizeCali; // Calculates the mean of the signal
s1725696 13:c087c0f64769 441 Threshold0 = Mean0; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 442 break;
s1725696 13:c087c0f64769 443 }
s1725696 13:c087c0f64769 444 case 1:
s1725696 13:c087c0f64769 445 { // Case one, calibrate EMG signal of left biceps
s1725696 13:c087c0f64769 446 sum = 0.0;
s1725696 13:c087c0f64769 447 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 448 {
s1725696 13:c087c0f64769 449 StoreCali1[j] = low1;
s1725696 13:c087c0f64769 450 sum+=StoreCali1[j];
s1725696 13:c087c0f64769 451 wait(0.001f);
s1725696 13:c087c0f64769 452 }
s1725696 13:c087c0f64769 453 Mean1 = sum/sizeCali;
s1725696 13:c087c0f64769 454 Threshold1 = Mean1; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 455 break;
s1725696 13:c087c0f64769 456 }
s1725696 13:c087c0f64769 457 case 2:
s1725696 13:c087c0f64769 458 { // Case two, calibrate EMG signal of calf
s1725696 13:c087c0f64769 459 sum = 0.0;
s1725696 13:c087c0f64769 460 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 461 {
s1725696 13:c087c0f64769 462 StoreCali2[j] = low2;
s1725696 13:c087c0f64769 463 sum+=StoreCali2[j];
s1725696 13:c087c0f64769 464 wait(0.001f);
s1725696 13:c087c0f64769 465 }
s1725696 13:c087c0f64769 466 Mean2 = sum/sizeCali;
s1725696 13:c087c0f64769 467 Threshold2 = Mean2; //Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 468 break;
s1725696 13:c087c0f64769 469 }
s1725696 13:c087c0f64769 470 case 3:
s1725696 13:c087c0f64769 471 { // Sets calibration value to 1; robot can be set to Home position
s1725696 13:c087c0f64769 472 emg_calib=1;
s1725696 13:c087c0f64769 473 wait(0.001f);
s1725696 13:c087c0f64769 474 break;
s1725696 13:c087c0f64769 475 }
s1725696 13:c087c0f64769 476 default:
s1725696 13:c087c0f64769 477 { // Ensures nothing happens if g is not equal to 0, 1 or 2.
s1725696 13:c087c0f64769 478 break;
s1725696 13:c087c0f64769 479 }
s1725696 13:c087c0f64769 480 }
s1725696 13:c087c0f64769 481 }
s1725696 13:c087c0f64769 482
s1725696 13:c087c0f64769 483 // Void to calibrate the EMG signal
s1725696 10:56136a0da8c1 484 void emg_calibration()
s1725696 10:56136a0da8c1 485 {
s1725696 13:c087c0f64769 486 for(int m = 1; m <= 4; m++)
s1725696 13:c087c0f64769 487 {
s1725696 13:c087c0f64769 488 led_blue == 0;
s1725696 13:c087c0f64769 489 led_red == 1;
s1725696 13:c087c0f64769 490 led_green == 1;
s1725696 13:c087c0f64769 491
s1725696 13:c087c0f64769 492 MovAg_tick.attach(&MovAg,T); // Moving average calculation every T sec.
s1725696 13:c087c0f64769 493
s1725696 13:c087c0f64769 494
s1725696 13:c087c0f64769 495 pc.printf("g is %i\n\r",g);
s1725696 13:c087c0f64769 496 pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
s1725696 13:c087c0f64769 497 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
s1725696 13:c087c0f64769 498
s1725696 13:c087c0f64769 499 bool k = true;
s1725696 13:c087c0f64769 500 while(k == true)
s1725696 13:c087c0f64769 501 {
s1725696 13:c087c0f64769 502 if(button_motorcal == false)
s1725696 13:c087c0f64769 503 {
s1725696 13:c087c0f64769 504 calibrate(); // Calibrate threshold for 3 muscles
s1725696 13:c087c0f64769 505 k = false;
s1725696 13:c087c0f64769 506 }
s1725696 13:c087c0f64769 507 wait(0.2f); // Wait to avoid bouncing of button
s1725696 13:c087c0f64769 508 }
s1725696 13:c087c0f64769 509
s1725696 13:c087c0f64769 510 bool h = true;
s1725696 13:c087c0f64769 511 while(h == true)
s1725696 13:c087c0f64769 512 {
s1725696 13:c087c0f64769 513 if (button_demo == false)
s1725696 13:c087c0f64769 514 {
s1725696 13:c087c0f64769 515 switch_to_calibrate(); // Switch state of calibration (which muscle)
s1725696 13:c087c0f64769 516 h = false;
s1725696 13:c087c0f64769 517 }
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 14:abc125dcc246 522
s1725696 14:abc125dcc246 523 // Turning all leds off
s1725696 14:abc125dcc246 524 led_red == 1;
s1725696 14:abc125dcc246 525 led_blue == 1;
s1725696 14:abc125dcc246 526 led_green == 1;
s1725696 10:56136a0da8c1 527 }
s1725696 10:56136a0da8c1 528
s1725696 9:d7a6a3619576 529 // PID controller
s1725696 9:d7a6a3619576 530 // To control the input signal before it goes into the motor control
s1725696 9:d7a6a3619576 531 // Simon mee bezig
s1725696 10:56136a0da8c1 532 void pid_controller()
s1725696 10:56136a0da8c1 533 {
s1725696 10:56136a0da8c1 534 // Simons code here
s1725696 10:56136a0da8c1 535 }
s1725696 0:cb8857cf3ea4 536
s1725696 10:56136a0da8c1 537 // START
s1725696 0:cb8857cf3ea4 538 // To move the robot to the starting position: middle
s1725696 10:56136a0da8c1 539 void start_mode()
s1725696 4:8183e7b228f0 540 {
s1725696 12:b2b082e73ef1 541 // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
s1725696 14:abc125dcc246 542
s1725696 14:abc125dcc246 543 //translation home
s1725696 14:abc125dcc246 544 if (counts2 > ((tower_end_position - tower_1_position)/2))
s1725696 14:abc125dcc246 545 {
s1725696 14:abc125dcc246 546 translation_start(0,1.0);
s1725696 14:abc125dcc246 547 }
s1725696 14:abc125dcc246 548 else {
s1725696 14:abc125dcc246 549 translation_start(1,1.0);
s1725696 14:abc125dcc246 550 }
s1725696 14:abc125dcc246 551 if (counts2 > ((tower_end_position - tower_1_position)/2 - 100))
s1725696 14:abc125dcc246 552 {
s1725696 14:abc125dcc246 553 if (counts2 < ((tower_end_position - tower_1_position)/2 + 100))
s1725696 14:abc125dcc246 554 {
s1725696 14:abc125dcc246 555 translation_stop();
s1725696 14:abc125dcc246 556 }
s1725696 14:abc125dcc246 557 }
s1725696 4:8183e7b228f0 558 }
s1725696 0:cb8857cf3ea4 559
s1725696 10:56136a0da8c1 560 // OPERATING
s1725696 0:cb8857cf3ea4 561 // To control the robot with EMG signals
s1725696 11:79311abb2bc2 562 // Kenneth bezig met motor aansturen
s1725696 12:b2b082e73ef1 563 void operating()
s1725696 12:b2b082e73ef1 564 {
s1725696 14:abc125dcc246 565 servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
s1725696 12:b2b082e73ef1 566 // RKI fuction
s1725696 12:b2b082e73ef1 567 }
s1725696 0:cb8857cf3ea4 568
s1725696 10:56136a0da8c1 569 // DEMO
s1725696 0:cb8857cf3ea4 570 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 571 // Voor het laatst bewaren
s1725696 10:56136a0da8c1 572 void demo_mode()
s1725696 10:56136a0da8c1 573 {
s1725696 10:56136a0da8c1 574 // code here
s1725696 10:56136a0da8c1 575 }
s1725696 0:cb8857cf3ea4 576
s1725696 10:56136a0da8c1 577 // FAILURE
s1725696 0:cb8857cf3ea4 578 // To shut down the robot after an error etc
s1725696 13:c087c0f64769 579 void failure_mode()
s1725696 10:56136a0da8c1 580 {
s1725696 14:abc125dcc246 581 led_red == 0; // turning red led on to show emergency mode
s1725696 14:abc125dcc246 582
s1725696 14:abc125dcc246 583 // all pwmpins zero
s1725696 14:abc125dcc246 584 pwmpin_1 = 0.0;
s1725696 14:abc125dcc246 585 pwmpin_2 = 0.0;
s1725696 14:abc125dcc246 586
s1725696 14:abc125dcc246 587 // Servo up?
s1725696 14:abc125dcc246 588 // myservo = 0.1;
s1725696 14:abc125dcc246 589
s1725696 14:abc125dcc246 590 pc.printf("Emergency mode, reset system to continue\r\n");
s1725696 10:56136a0da8c1 591 }
s1725696 0:cb8857cf3ea4 592
s1725696 0:cb8857cf3ea4 593 // Main function
s1725696 4:8183e7b228f0 594 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 595 int main()
s1725696 0:cb8857cf3ea4 596 {
s1725696 0:cb8857cf3ea4 597 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 598 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 9:d7a6a3619576 599 pwmpin_1.period_us(60); // Setting period for PWM
s1725696 13:c087c0f64769 600 Filter_tick.attach(&filtering,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
s1725696 1:1a8211e1f3f3 601
s1725696 1:1a8211e1f3f3 602 while(true){
s1725696 6:f495a77c2c95 603 // timer
s1725696 6:f495a77c2c95 604 clock_t start; // start the timer
s1725696 6:f495a77c2c95 605 start = clock();
s1725696 9:d7a6a3619576 606 time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 14:abc125dcc246 607 myservo = 0.1; // Keep the pen lifted until servo function is called (operation mode)
s1725696 5:3581013d4505 608
s1725696 6:f495a77c2c95 609 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 610 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 611
s1725696 6:f495a77c2c95 612 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 9:d7a6a3619576 613 switch(CurrentState)
s1725696 6:f495a77c2c95 614 {
s1725696 6:f495a77c2c95 615 case WAIT:
s1725696 6:f495a77c2c95 616
s1725696 6:f495a77c2c95 617 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 618 {
s1725696 10:56136a0da8c1 619 pc.printf("State is WAIT\r\n");
s1725696 10:56136a0da8c1 620
s1725696 6:f495a77c2c95 621 // Execute WAIT mode
s1725696 10:56136a0da8c1 622 wait_mode();
s1725696 10:56136a0da8c1 623
s1725696 6:f495a77c2c95 624 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 625 }
s1725696 6:f495a77c2c95 626
s1725696 12:b2b082e73ef1 627 if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 628 {
s1725696 6:f495a77c2c95 629 CurrentState = MOTOR_CAL;
s1725696 9:d7a6a3619576 630 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 631 StateChanged = true;
s1725696 6:f495a77c2c95 632 }
s1725696 7:ec5add330cb3 633
s1725696 12:b2b082e73ef1 634 if (button_emergency == false) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 635 {
s1725696 7:ec5add330cb3 636 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 637 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 638 StateChanged = true;
s1725696 7:ec5add330cb3 639 }
s1725696 6:f495a77c2c95 640
s1725696 6:f495a77c2c95 641 break;
s1725696 6:f495a77c2c95 642
s1725696 6:f495a77c2c95 643 case MOTOR_CAL:
s1725696 6:f495a77c2c95 644
s1725696 6:f495a77c2c95 645 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 646 {
s1725696 9:d7a6a3619576 647 t.reset();
s1725696 6:f495a77c2c95 648 t.start();
s1725696 10:56136a0da8c1 649
s1725696 6:f495a77c2c95 650 // Execute MOTOR_CAL mode
s1725696 10:56136a0da8c1 651 motor_calibration();
s1725696 10:56136a0da8c1 652
s1725696 6:f495a77c2c95 653 t.stop();
s1725696 6:f495a77c2c95 654 time_in_state = t.read();
s1725696 9:d7a6a3619576 655 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 656
s1725696 6:f495a77c2c95 657 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 658 }
s1725696 6:f495a77c2c95 659
s1725696 15:2772f8cbf382 660 if((time_in_state > 10.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s and motors stopped moving
s1725696 6:f495a77c2c95 661 {
s1725696 6:f495a77c2c95 662 CurrentState = EMG_CAL;
s1725696 9:d7a6a3619576 663 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 664 StateChanged = true;
s1725696 6:f495a77c2c95 665 }
s1725696 6:f495a77c2c95 666
s1725696 12:b2b082e73ef1 667 if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 668 {
s1725696 7:ec5add330cb3 669 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 670 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 671 StateChanged = true;
s1725696 7:ec5add330cb3 672 }
s1725696 7:ec5add330cb3 673
s1725696 6:f495a77c2c95 674 break;
s1725696 6:f495a77c2c95 675
s1725696 6:f495a77c2c95 676 case EMG_CAL:
s1725696 6:f495a77c2c95 677
s1725696 6:f495a77c2c95 678 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 679 {
s1725696 6:f495a77c2c95 680 t.reset();
s1725696 6:f495a77c2c95 681 t.start();
s1725696 10:56136a0da8c1 682
s1725696 6:f495a77c2c95 683 // Execute EMG_CAL mode
s1725696 10:56136a0da8c1 684 emg_calibration();
s1725696 10:56136a0da8c1 685
s1725696 6:f495a77c2c95 686 t.stop();
s1725696 6:f495a77c2c95 687 time_in_state = t.read();
s1725696 9:d7a6a3619576 688 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 689
s1725696 6:f495a77c2c95 690 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 691 }
s1725696 6:f495a77c2c95 692
s1725696 15:2772f8cbf382 693 if((time_in_state > 10.0) && (Average0 < 0.04) && (Average1 < 0.04) && (Average2 < 0.04)) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 694 {
s1725696 6:f495a77c2c95 695 CurrentState = START;
s1725696 9:d7a6a3619576 696 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 697 StateChanged = true;
s1725696 6:f495a77c2c95 698 }
s1725696 6:f495a77c2c95 699
s1725696 12:b2b082e73ef1 700 if (button_emergency == false) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 701 {
s1725696 7:ec5add330cb3 702 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 703 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 704 StateChanged = true;
s1725696 7:ec5add330cb3 705 }
s1725696 7:ec5add330cb3 706
s1725696 6:f495a77c2c95 707 break;
s1725696 6:f495a77c2c95 708
s1725696 6:f495a77c2c95 709 case START:
s1725696 6:f495a77c2c95 710
s1725696 6:f495a77c2c95 711 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 712 {
s1725696 6:f495a77c2c95 713 t.reset();
s1725696 6:f495a77c2c95 714 t.start();
s1725696 10:56136a0da8c1 715
s1725696 6:f495a77c2c95 716 // Execute START mode
s1725696 10:56136a0da8c1 717 start_mode();
s1725696 10:56136a0da8c1 718
s1725696 6:f495a77c2c95 719 t.stop();
s1725696 6:f495a77c2c95 720 time_in_state = t.read();
s1725696 9:d7a6a3619576 721 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 722
s1725696 6:f495a77c2c95 723 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 724 }
s1725696 6:f495a77c2c95 725
s1725696 15:2772f8cbf382 726 if((time_in_state > 5.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for START --> OPERATING; 5s and motors stopped moving
s1725696 6:f495a77c2c95 727 {
s1725696 6:f495a77c2c95 728 CurrentState = OPERATING;
s1725696 9:d7a6a3619576 729 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 730 StateChanged = true;
s1725696 6:f495a77c2c95 731 }
s1725696 6:f495a77c2c95 732
s1725696 12:b2b082e73ef1 733 if (button_emergency == false) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 734 {
s1725696 7:ec5add330cb3 735 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 736 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 737 StateChanged = true;
s1725696 7:ec5add330cb3 738 }
s1725696 7:ec5add330cb3 739
s1725696 6:f495a77c2c95 740 break;
s1725696 6:f495a77c2c95 741
s1725696 6:f495a77c2c95 742 case OPERATING:
s1725696 6:f495a77c2c95 743
s1725696 6:f495a77c2c95 744 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 745 {
s1725696 6:f495a77c2c95 746 // Execute OPERATING mode
s1725696 10:56136a0da8c1 747
s1725696 6:f495a77c2c95 748 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 749 }
s1725696 6:f495a77c2c95 750
s1725696 12:b2b082e73ef1 751 if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 752 {
s1725696 6:f495a77c2c95 753 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 754 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 755 StateChanged = true;
s1725696 6:f495a77c2c95 756 }
s1725696 6:f495a77c2c95 757
s1725696 12:b2b082e73ef1 758 if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 759 {
s1725696 6:f495a77c2c95 760 CurrentState = DEMO;
s1725696 9:d7a6a3619576 761 pc.printf("State is DEMO\r\n");
s1725696 9:d7a6a3619576 762 StateChanged = true;
s1725696 9:d7a6a3619576 763 }
s1725696 9:d7a6a3619576 764
s1725696 12:b2b082e73ef1 765 if(button_wait == false) // condition OPERATING --> WAIT; button_wait press
s1725696 9:d7a6a3619576 766 {
s1725696 9:d7a6a3619576 767 CurrentState = WAIT;
s1725696 9:d7a6a3619576 768 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 769 StateChanged = true;
s1725696 6:f495a77c2c95 770 }
s1725696 6:f495a77c2c95 771
s1725696 6:f495a77c2c95 772 break;
s1725696 6:f495a77c2c95 773
s1725696 6:f495a77c2c95 774 case FAILURE:
s1725696 6:f495a77c2c95 775
s1725696 6:f495a77c2c95 776 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 777 {
s1725696 6:f495a77c2c95 778 // Execute FAILURE mode
s1725696 10:56136a0da8c1 779 failure_mode();
s1725696 14:abc125dcc246 780
s1725696 6:f495a77c2c95 781 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 782 }
s1725696 14:abc125dcc246 783 // no other states possible, a full reset (push on reset button) is necessary
s1725696 6:f495a77c2c95 784 break;
s1725696 6:f495a77c2c95 785
s1725696 6:f495a77c2c95 786 case DEMO:
s1725696 6:f495a77c2c95 787
s1725696 6:f495a77c2c95 788 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 789 {
s1725696 6:f495a77c2c95 790 // Execute DEMO mode
s1725696 10:56136a0da8c1 791 demo_mode();
s1725696 10:56136a0da8c1 792
s1725696 6:f495a77c2c95 793 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 794 }
s1725696 6:f495a77c2c95 795
s1725696 12:b2b082e73ef1 796 if(button_wait == false) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 797 {
s1725696 6:f495a77c2c95 798 CurrentState = WAIT;
s1725696 9:d7a6a3619576 799 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 800 StateChanged = true;
s1725696 6:f495a77c2c95 801 }
s1725696 6:f495a77c2c95 802
s1725696 12:b2b082e73ef1 803 if (button_emergency == false) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 804 {
s1725696 6:f495a77c2c95 805 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 806 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 807 StateChanged = true;
s1725696 6:f495a77c2c95 808 }
s1725696 6:f495a77c2c95 809
s1725696 6:f495a77c2c95 810 break;
s1725696 6:f495a77c2c95 811
s1725696 6:f495a77c2c95 812 // no default
s1725696 4:8183e7b228f0 813 }
s1725696 6:f495a77c2c95 814
s1725696 6:f495a77c2c95 815 // while loop does not have to loop every time
s1725696 5:3581013d4505 816 }
s1725696 5:3581013d4505 817
s1725696 0:cb8857cf3ea4 818 }