Group 9 BioRobotics / Mbed 2 deprecated the_code

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:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@12:b2b082e73ef1, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 18:41:58 2018 +0000
Revision:
12:b2b082e73ef1
Parent:
11:79311abb2bc2
Child:
13:c087c0f64769
with the motor calibration;

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 0:cb8857cf3ea4 6
s1725696 5:3581013d4505 7 #define SERIAL_BAUD 115200
s1725696 5:3581013d4505 8
s1725696 5:3581013d4505 9 // In- en outputs
s1725696 4:8183e7b228f0 10 // -----------------------------------------------------------------------------
s1725696 4:8183e7b228f0 11
s1725696 0:cb8857cf3ea4 12 // EMG related
s1725696 5:3581013d4505 13 AnalogIn emg1(); // EMG signal 1
s1725696 5:3581013d4505 14 AnalogIn emg2(); // EMG signal 2
s1725696 5:3581013d4505 15 // if needed
s1725696 5:3581013d4505 16 AnalogIn emg3(); // EMG signal 3
s1725696 5:3581013d4505 17 AnalogIn emg4(); // EMG signal 4
s1725696 10:56136a0da8c1 18
s1725696 0:cb8857cf3ea4 19 // Motor related
s1725696 12:b2b082e73ef1 20 DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
s1725696 0:cb8857cf3ea4 21 PwmOut pwmpin_1(D5); // PWM pin of motor 1
s1725696 12:b2b082e73ef1 22 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
s1725696 5:3581013d4505 23 PwmOut pwmpin_2(D6); // PWM pin of motor 2
s1725696 0:cb8857cf3ea4 24
s1725696 0:cb8857cf3ea4 25 // Extra stuff
s1725696 0:cb8857cf3ea4 26 // Like LED lights, buttons etc
s1725696 9:d7a6a3619576 27
s1725696 9:d7a6a3619576 28 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 8:5ad8a7892693 29 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
s1725696 9:d7a6a3619576 30 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 8:5ad8a7892693 31 DigitalIn button_demo(D6); // button for demo mode, on bioshield
s1725696 9:d7a6a3619576 32
s1725696 4:8183e7b228f0 33 DigitalIn led_red(LED_RED); // red led
s1725696 4:8183e7b228f0 34 DigitalIn led_green(LED_GREEN); // green led
s1725696 4:8183e7b228f0 35 DigitalIn led_blue(LED_BLUE); // blue led
s1725696 9:d7a6a3619576 36
s1725696 5:3581013d4505 37 AnalogIn pot_1(A1); // potmeter for simulating EMG input
s1725696 12:b2b082e73ef1 38 Servo myservo(D7) = 0.1; // Define the servo to control (in penholder), up to start with
s1725696 0:cb8857cf3ea4 39
s1725696 0:cb8857cf3ea4 40 // Other stuff
s1725696 4:8183e7b228f0 41 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 42 // Define stuff like tickers etc
s1725696 12:b2b082e73ef1 43
s1725696 10:56136a0da8c1 44 Ticker ticker; // Name a ticker, use each ticker only for 1 function!
s1725696 0:cb8857cf3ea4 45 HIDScope scope(2); // Number of channels in HIDScope
s1725696 12:b2b082e73ef1 46 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
s1725696 12:b2b082e73ef1 47 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
s1725696 5:3581013d4505 48 Serial pc(USBTX,USBRX);
s1725696 6:f495a77c2c95 49 Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
s1725696 0:cb8857cf3ea4 50
s1725696 0:cb8857cf3ea4 51 // Variables
s1725696 4:8183e7b228f0 52 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 53 // Define here all variables needed throughout the whole code
s1725696 12:b2b082e73ef1 54 volatile double time_overall;
s1725696 12:b2b082e73ef1 55 volatile double time_in_state;
s1725696 12:b2b082e73ef1 56 volatile double motor_velocity = 0;
s1725696 12:b2b082e73ef1 57 volatile double EMG = 0;
s1725696 12:b2b082e73ef1 58 volatile double errors = 0;
s1725696 12:b2b082e73ef1 59 volatile int counts1_prev = 0;
s1725696 12:b2b082e73ef1 60 volatile int counts2_prev = 0;
s1725696 12:b2b082e73ef1 61 volatile int counts1;
s1725696 12:b2b082e73ef1 62 volatile int counts2;
s1725696 12:b2b082e73ef1 63
s1725696 12:b2b082e73ef1 64 // MOTOR_CAL
s1725696 12:b2b082e73ef1 65 volatile double tower_1_position = 0.1; // the tower which he reaches first
s1725696 12:b2b082e73ef1 66 volatile double tower_end_position = 0.1; // the tower which he reaches second
s1725696 12:b2b082e73ef1 67 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
s1725696 12:b2b082e73ef1 68 volatile double position;
s1725696 12:b2b082e73ef1 69 float speed = 0.70;
s1725696 12:b2b082e73ef1 70 int dir = 0;
s1725696 6:f495a77c2c95 71
s1725696 6:f495a77c2c95 72 // states
s1725696 9:d7a6a3619576 73 enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
s1725696 7:ec5add330cb3 74 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
s1725696 6:f495a77c2c95 75 bool StateChanged = true; // the state must be changed to go into the next state
s1725696 0:cb8857cf3ea4 76
s1725696 0:cb8857cf3ea4 77 // Functions
s1725696 4:8183e7b228f0 78 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 79
s1725696 4:8183e7b228f0 80 // Encoder
s1725696 4:8183e7b228f0 81 // Getting encoder information from motors
s1725696 12:b2b082e73ef1 82 int Counts1(volatile int& a) // a = counts1
s1725696 4:8183e7b228f0 83 {
s1725696 12:b2b082e73ef1 84 counts1_prev = a;
s1725696 12:b2b082e73ef1 85 a = Encoder2.getPulses();
s1725696 12:b2b082e73ef1 86 return a;
s1725696 12:b2b082e73ef1 87 }
s1725696 12:b2b082e73ef1 88
s1725696 12:b2b082e73ef1 89 int Counts2(volatile int& a) // a = counts2
s1725696 12:b2b082e73ef1 90 {
s1725696 12:b2b082e73ef1 91 counts2_prev = a;
s1725696 12:b2b082e73ef1 92 a = Encoder1.getPulses();
s1725696 12:b2b082e73ef1 93 return a;
s1725696 12:b2b082e73ef1 94 }
s1725696 5:3581013d4505 95
s1725696 11:79311abb2bc2 96 // Potmeter for controlling motor
s1725696 5:3581013d4505 97 float potmeter()
s1725696 5:3581013d4505 98 {
s1725696 5:3581013d4505 99 float out_1 = pot_1 * 2.0f;
s1725696 5:3581013d4505 100 float out_2 = out_1 - 1.0f;
s1725696 5:3581013d4505 101
s1725696 5:3581013d4505 102 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 103 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 104
s1725696 5:3581013d4505 105 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 106 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 107
s1725696 5:3581013d4505 108 return out_2;
s1725696 6:f495a77c2c95 109 }
s1725696 9:d7a6a3619576 110
s1725696 11:79311abb2bc2 111 // Servo control
s1725696 12:b2b082e73ef1 112 // To lift the pen up, with a push of button
s1725696 11:79311abb2bc2 113 void servocontrol()
s1725696 11:79311abb2bc2 114 {
s1725696 12:b2b082e73ef1 115 if (button_motorcal == true) // this button is further only used in WAIT mode to go to MOTOR_CAL
s1725696 11:79311abb2bc2 116 {
s1725696 11:79311abb2bc2 117 myservo = 0.1;
s1725696 11:79311abb2bc2 118 }
s1725696 11:79311abb2bc2 119 else
s1725696 11:79311abb2bc2 120 {
s1725696 11:79311abb2bc2 121 myservo = 0.0;
s1725696 11:79311abb2bc2 122 }
s1725696 12:b2b082e73ef1 123 }
s1725696 11:79311abb2bc2 124
s1725696 9:d7a6a3619576 125 // Send information to HIDScope
s1725696 6:f495a77c2c95 126 void hidscope() // Attach this to a ticker!
s1725696 0:cb8857cf3ea4 127 {
s1725696 12:b2b082e73ef1 128 scope.set(0, emg1); // send EMG 1 to channel 1 (=0)
s1725696 12:b2b082e73ef1 129 scope.set(1, emg2); // sent EMG 2 to channel 2 (=1)
s1725696 5:3581013d4505 130
s1725696 1:1a8211e1f3f3 131 // Ensure that enough channels are available (HIDScope scope(2))
s1725696 0:cb8857cf3ea4 132 scope.send(); // Send all channels to the PC at once
s1725696 6:f495a77c2c95 133 }
s1725696 9:d7a6a3619576 134
s1725696 10:56136a0da8c1 135 // EMG filter
s1725696 10:56136a0da8c1 136 // To process the EMG signal before information can be caught from it
s1725696 10:56136a0da8c1 137 // Kees mee bezig
s1725696 10:56136a0da8c1 138
s1725696 10:56136a0da8c1 139 // WAIT
s1725696 10:56136a0da8c1 140 // To do nothing
s1725696 10:56136a0da8c1 141 void wait_mode()
s1725696 10:56136a0da8c1 142 {
s1725696 10:56136a0da8c1 143 // go back to the initial values
s1725696 10:56136a0da8c1 144 // Copy here the variables list with initial values
s1725696 10:56136a0da8c1 145 motor_velocity = 0;
s1725696 10:56136a0da8c1 146 }
s1725696 10:56136a0da8c1 147
s1725696 10:56136a0da8c1 148 // MOTOR_CAL
s1725696 10:56136a0da8c1 149 // To calibrate the motor angle to some mechanical boundaries
s1725696 10:56136a0da8c1 150 // Kenneth mee bezig
s1725696 12:b2b082e73ef1 151 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 12:b2b082e73ef1 152
s1725696 12:b2b082e73ef1 153 if (tower_1_position == 0.1){
s1725696 12:b2b082e73ef1 154 tower_1_position = a;
s1725696 12:b2b082e73ef1 155 }
s1725696 12:b2b082e73ef1 156 else if (tower_end_position == 0.1){
s1725696 12:b2b082e73ef1 157 tower_end_position = a;
s1725696 12:b2b082e73ef1 158 }
s1725696 12:b2b082e73ef1 159 else if (rotation_start_position == 0.1){
s1725696 12:b2b082e73ef1 160 rotation_start_position = a;
s1725696 12:b2b082e73ef1 161 }
s1725696 12:b2b082e73ef1 162 }
s1725696 12:b2b082e73ef1 163
s1725696 12:b2b082e73ef1 164 // Start translation
s1725696 12:b2b082e73ef1 165 void translation_start(int a, float b) // a = dir , b = speed
s1725696 12:b2b082e73ef1 166 {
s1725696 12:b2b082e73ef1 167 dirpin_1.write(a);
s1725696 12:b2b082e73ef1 168 pwmpin_1 = b;
s1725696 12:b2b082e73ef1 169 }
s1725696 12:b2b082e73ef1 170
s1725696 12:b2b082e73ef1 171 // Stop translation
s1725696 12:b2b082e73ef1 172 void translation_stop()
s1725696 12:b2b082e73ef1 173 {
s1725696 12:b2b082e73ef1 174 pwmpin_1 = 0.0;
s1725696 12:b2b082e73ef1 175 }
s1725696 12:b2b082e73ef1 176
s1725696 12:b2b082e73ef1 177 // Start rotation
s1725696 12:b2b082e73ef1 178 void rotation_start(int a, float b)
s1725696 12:b2b082e73ef1 179 {
s1725696 12:b2b082e73ef1 180 dirpin_2.write(a);
s1725696 12:b2b082e73ef1 181 pwmpin_2 = b;
s1725696 12:b2b082e73ef1 182 }
s1725696 12:b2b082e73ef1 183
s1725696 12:b2b082e73ef1 184 // Stop rotation
s1725696 12:b2b082e73ef1 185 void rotation_stop()
s1725696 12:b2b082e73ef1 186 {
s1725696 12:b2b082e73ef1 187 pwmpin_2 = 0.0;
s1725696 12:b2b082e73ef1 188 }
s1725696 12:b2b082e73ef1 189
s1725696 12:b2b082e73ef1 190 // Calibration of translation
s1725696 12:b2b082e73ef1 191 void calibration_translation()
s1725696 12:b2b082e73ef1 192 {
s1725696 12:b2b082e73ef1 193 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 12:b2b082e73ef1 194 {
s1725696 12:b2b082e73ef1 195 pc.printf("\r\nTranslatie loop\r\n");
s1725696 12:b2b082e73ef1 196 translation_start(dir,speed);
s1725696 12:b2b082e73ef1 197 pc.printf("Direction = %i\r\n", dir);
s1725696 12:b2b082e73ef1 198
s1725696 12:b2b082e73ef1 199 bool g = true; // to make a condition for the while loop
s1725696 12:b2b082e73ef1 200 while (g == true)
s1725696 12:b2b082e73ef1 201 {
s1725696 12:b2b082e73ef1 202 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 12:b2b082e73ef1 203 {
s1725696 12:b2b082e73ef1 204 translation_stop();
s1725696 12:b2b082e73ef1 205 pos_store(Counts1(counts1));
s1725696 12:b2b082e73ef1 206 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 207 dir = dir + 1;
s1725696 12:b2b082e73ef1 208
s1725696 12:b2b082e73ef1 209 g = false; // to end the while loop
s1725696 12:b2b082e73ef1 210 }
s1725696 12:b2b082e73ef1 211
s1725696 12:b2b082e73ef1 212 wait(0.01);
s1725696 12:b2b082e73ef1 213 }
s1725696 12:b2b082e73ef1 214
s1725696 12:b2b082e73ef1 215 wait(1.5); // wait 3 seconds before next round of translation/rotation
s1725696 12:b2b082e73ef1 216 }
s1725696 12:b2b082e73ef1 217 }
s1725696 12:b2b082e73ef1 218
s1725696 12:b2b082e73ef1 219 void calibration_rotation()
s1725696 12:b2b082e73ef1 220 {
s1725696 12:b2b082e73ef1 221 rotation_start(dir, speed);
s1725696 12:b2b082e73ef1 222 pc.printf("\r\nRotatie start\r\n");
s1725696 12:b2b082e73ef1 223
s1725696 12:b2b082e73ef1 224 bool f = true; // condition for while loop
s1725696 12:b2b082e73ef1 225 while(f == true)
s1725696 12:b2b082e73ef1 226 {
s1725696 12:b2b082e73ef1 227 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 12:b2b082e73ef1 228 {
s1725696 12:b2b082e73ef1 229 rotation_stop();
s1725696 12:b2b082e73ef1 230
s1725696 12:b2b082e73ef1 231 f = false; // to end the while loop
s1725696 12:b2b082e73ef1 232 }
s1725696 12:b2b082e73ef1 233
s1725696 12:b2b082e73ef1 234 wait(0.01);
s1725696 12:b2b082e73ef1 235 }
s1725696 12:b2b082e73ef1 236
s1725696 12:b2b082e73ef1 237 pos_store(Counts2(counts2));
s1725696 12:b2b082e73ef1 238 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 239 }
s1725696 12:b2b082e73ef1 240
s1725696 10:56136a0da8c1 241 void motor_calibration()
s1725696 10:56136a0da8c1 242 {
s1725696 12:b2b082e73ef1 243 // translation
s1725696 12:b2b082e73ef1 244 calibration_translation();
s1725696 12:b2b082e73ef1 245
s1725696 12:b2b082e73ef1 246 pc.printf("before wait\r\n");
s1725696 12:b2b082e73ef1 247 wait(1.5);
s1725696 12:b2b082e73ef1 248
s1725696 12:b2b082e73ef1 249 // rotation
s1725696 12:b2b082e73ef1 250 calibration_rotation();
s1725696 12:b2b082e73ef1 251
s1725696 12:b2b082e73ef1 252 pc.printf("Motor calibration done");
s1725696 10:56136a0da8c1 253 }
s1725696 10:56136a0da8c1 254
s1725696 10:56136a0da8c1 255 // EMG_CAL
s1725696 10:56136a0da8c1 256 // To calibrate the EMG signal to some boundary values
s1725696 10:56136a0da8c1 257 void emg_calibration()
s1725696 10:56136a0da8c1 258 {
s1725696 10:56136a0da8c1 259 // code
s1725696 10:56136a0da8c1 260 }
s1725696 10:56136a0da8c1 261
s1725696 9:d7a6a3619576 262 // PID controller
s1725696 9:d7a6a3619576 263 // To control the input signal before it goes into the motor control
s1725696 9:d7a6a3619576 264 // Simon mee bezig
s1725696 10:56136a0da8c1 265 void pid_controller()
s1725696 10:56136a0da8c1 266 {
s1725696 10:56136a0da8c1 267 // Simons code here
s1725696 10:56136a0da8c1 268 }
s1725696 0:cb8857cf3ea4 269
s1725696 10:56136a0da8c1 270 // START
s1725696 0:cb8857cf3ea4 271 // To move the robot to the starting position: middle
s1725696 10:56136a0da8c1 272 void start_mode()
s1725696 4:8183e7b228f0 273 {
s1725696 12:b2b082e73ef1 274 // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
s1725696 4:8183e7b228f0 275 }
s1725696 0:cb8857cf3ea4 276
s1725696 10:56136a0da8c1 277 // OPERATING
s1725696 0:cb8857cf3ea4 278 // To control the robot with EMG signals
s1725696 11:79311abb2bc2 279 // Kenneth bezig met motor aansturen
s1725696 12:b2b082e73ef1 280 void operating()
s1725696 12:b2b082e73ef1 281 {
s1725696 12:b2b082e73ef1 282 servocontrol(); // make sure the servo is used
s1725696 12:b2b082e73ef1 283 // RKI fuction
s1725696 12:b2b082e73ef1 284 }
s1725696 0:cb8857cf3ea4 285
s1725696 10:56136a0da8c1 286 // DEMO
s1725696 0:cb8857cf3ea4 287 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 288 // Voor het laatst bewaren
s1725696 10:56136a0da8c1 289 void demo_mode()
s1725696 10:56136a0da8c1 290 {
s1725696 10:56136a0da8c1 291 // code here
s1725696 10:56136a0da8c1 292 }
s1725696 0:cb8857cf3ea4 293
s1725696 10:56136a0da8c1 294 // FAILURE
s1725696 0:cb8857cf3ea4 295 // To shut down the robot after an error etc
s1725696 10:56136a0da8c1 296 void failure()
s1725696 10:56136a0da8c1 297 {
s1725696 10:56136a0da8c1 298 // code here
s1725696 10:56136a0da8c1 299 }
s1725696 0:cb8857cf3ea4 300
s1725696 0:cb8857cf3ea4 301 // Main function
s1725696 4:8183e7b228f0 302 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 303 int main()
s1725696 0:cb8857cf3ea4 304 {
s1725696 0:cb8857cf3ea4 305 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 306 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 9:d7a6a3619576 307 pwmpin_1.period_us(60); // Setting period for PWM
s1725696 10:56136a0da8c1 308 ticker.attach(&hidscope,0.002f); // Send information to HIDScope
s1725696 1:1a8211e1f3f3 309
s1725696 1:1a8211e1f3f3 310 while(true){
s1725696 6:f495a77c2c95 311 // timer
s1725696 6:f495a77c2c95 312 clock_t start; // start the timer
s1725696 6:f495a77c2c95 313 start = clock();
s1725696 9:d7a6a3619576 314 time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 5:3581013d4505 315
s1725696 5:3581013d4505 316 counts = encoder();
s1725696 6:f495a77c2c95 317 potmeter_value = potmeter();
s1725696 5:3581013d4505 318
s1725696 6:f495a77c2c95 319 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 320 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 321
s1725696 6:f495a77c2c95 322 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 9:d7a6a3619576 323 switch(CurrentState)
s1725696 6:f495a77c2c95 324 {
s1725696 6:f495a77c2c95 325 case WAIT:
s1725696 6:f495a77c2c95 326
s1725696 6:f495a77c2c95 327 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 328 {
s1725696 10:56136a0da8c1 329 pc.printf("State is WAIT\r\n");
s1725696 10:56136a0da8c1 330
s1725696 6:f495a77c2c95 331 // Execute WAIT mode
s1725696 10:56136a0da8c1 332 wait_mode();
s1725696 10:56136a0da8c1 333
s1725696 6:f495a77c2c95 334 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 335 }
s1725696 6:f495a77c2c95 336
s1725696 12:b2b082e73ef1 337 if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 338 {
s1725696 6:f495a77c2c95 339 CurrentState = MOTOR_CAL;
s1725696 9:d7a6a3619576 340 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 341 StateChanged = true;
s1725696 6:f495a77c2c95 342 }
s1725696 7:ec5add330cb3 343
s1725696 12:b2b082e73ef1 344 if (button_emergency == false) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 345 {
s1725696 7:ec5add330cb3 346 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 347 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 348 StateChanged = true;
s1725696 7:ec5add330cb3 349 }
s1725696 6:f495a77c2c95 350
s1725696 6:f495a77c2c95 351 break;
s1725696 6:f495a77c2c95 352
s1725696 6:f495a77c2c95 353 case MOTOR_CAL:
s1725696 6:f495a77c2c95 354
s1725696 6:f495a77c2c95 355 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 356 {
s1725696 9:d7a6a3619576 357 t.reset();
s1725696 6:f495a77c2c95 358 t.start();
s1725696 10:56136a0da8c1 359
s1725696 6:f495a77c2c95 360 // Execute MOTOR_CAL mode
s1725696 10:56136a0da8c1 361 motor_calibration();
s1725696 10:56136a0da8c1 362
s1725696 6:f495a77c2c95 363 t.stop();
s1725696 6:f495a77c2c95 364 time_in_state = t.read();
s1725696 9:d7a6a3619576 365 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 366
s1725696 6:f495a77c2c95 367 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 368 }
s1725696 6:f495a77c2c95 369
s1725696 12:b2b082e73ef1 370 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 371 {
s1725696 6:f495a77c2c95 372 CurrentState = EMG_CAL;
s1725696 9:d7a6a3619576 373 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 374 StateChanged = true;
s1725696 6:f495a77c2c95 375 }
s1725696 6:f495a77c2c95 376
s1725696 12:b2b082e73ef1 377 if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 378 {
s1725696 7:ec5add330cb3 379 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 380 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 381 StateChanged = true;
s1725696 7:ec5add330cb3 382 }
s1725696 7:ec5add330cb3 383
s1725696 6:f495a77c2c95 384 break;
s1725696 6:f495a77c2c95 385
s1725696 6:f495a77c2c95 386 case EMG_CAL:
s1725696 6:f495a77c2c95 387
s1725696 6:f495a77c2c95 388 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 389 {
s1725696 6:f495a77c2c95 390 t.reset();
s1725696 6:f495a77c2c95 391 t.start();
s1725696 10:56136a0da8c1 392
s1725696 6:f495a77c2c95 393 // Execute EMG_CAL mode
s1725696 10:56136a0da8c1 394 emg_calibration();
s1725696 10:56136a0da8c1 395
s1725696 6:f495a77c2c95 396 t.stop();
s1725696 6:f495a77c2c95 397 time_in_state = t.read();
s1725696 9:d7a6a3619576 398 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 399
s1725696 6:f495a77c2c95 400 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 401 }
s1725696 6:f495a77c2c95 402
s1725696 9:d7a6a3619576 403 if((time_in_state > 5) && (EMG < 0.01)) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 404 {
s1725696 6:f495a77c2c95 405 CurrentState = START;
s1725696 9:d7a6a3619576 406 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 407 StateChanged = true;
s1725696 6:f495a77c2c95 408 }
s1725696 6:f495a77c2c95 409
s1725696 12:b2b082e73ef1 410 if (button_emergency == false) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 411 {
s1725696 7:ec5add330cb3 412 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 413 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 414 StateChanged = true;
s1725696 7:ec5add330cb3 415 }
s1725696 7:ec5add330cb3 416
s1725696 6:f495a77c2c95 417 break;
s1725696 6:f495a77c2c95 418
s1725696 6:f495a77c2c95 419 case START:
s1725696 6:f495a77c2c95 420
s1725696 6:f495a77c2c95 421 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 422 {
s1725696 6:f495a77c2c95 423 t.reset();
s1725696 6:f495a77c2c95 424 t.start();
s1725696 10:56136a0da8c1 425
s1725696 6:f495a77c2c95 426 // Execute START mode
s1725696 10:56136a0da8c1 427 start_mode();
s1725696 10:56136a0da8c1 428
s1725696 6:f495a77c2c95 429 t.stop();
s1725696 6:f495a77c2c95 430 time_in_state = t.read();
s1725696 9:d7a6a3619576 431 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 432
s1725696 6:f495a77c2c95 433 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 434 }
s1725696 6:f495a77c2c95 435
s1725696 9:d7a6a3619576 436 if((time_in_state > 5) && (errors < 0.01)) // condition for START --> OPERATING; 5s and error is small
s1725696 6:f495a77c2c95 437 {
s1725696 6:f495a77c2c95 438 CurrentState = OPERATING;
s1725696 9:d7a6a3619576 439 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 440 StateChanged = true;
s1725696 6:f495a77c2c95 441 }
s1725696 6:f495a77c2c95 442
s1725696 12:b2b082e73ef1 443 if (button_emergency == false) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 444 {
s1725696 7:ec5add330cb3 445 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 446 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 447 StateChanged = true;
s1725696 7:ec5add330cb3 448 }
s1725696 7:ec5add330cb3 449
s1725696 6:f495a77c2c95 450 break;
s1725696 6:f495a77c2c95 451
s1725696 6:f495a77c2c95 452 case OPERATING:
s1725696 6:f495a77c2c95 453
s1725696 6:f495a77c2c95 454 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 455 {
s1725696 6:f495a77c2c95 456 // Execute OPERATING mode
s1725696 10:56136a0da8c1 457
s1725696 6:f495a77c2c95 458 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 459 }
s1725696 6:f495a77c2c95 460
s1725696 12:b2b082e73ef1 461 if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 462 {
s1725696 6:f495a77c2c95 463 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 464 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 465 StateChanged = true;
s1725696 6:f495a77c2c95 466 }
s1725696 6:f495a77c2c95 467
s1725696 12:b2b082e73ef1 468 if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 469 {
s1725696 6:f495a77c2c95 470 CurrentState = DEMO;
s1725696 9:d7a6a3619576 471 pc.printf("State is DEMO\r\n");
s1725696 9:d7a6a3619576 472 StateChanged = true;
s1725696 9:d7a6a3619576 473 }
s1725696 9:d7a6a3619576 474
s1725696 12:b2b082e73ef1 475 if(button_wait == false) // condition OPERATING --> WAIT; button_wait press
s1725696 9:d7a6a3619576 476 {
s1725696 9:d7a6a3619576 477 CurrentState = WAIT;
s1725696 9:d7a6a3619576 478 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 479 StateChanged = true;
s1725696 6:f495a77c2c95 480 }
s1725696 6:f495a77c2c95 481
s1725696 6:f495a77c2c95 482 break;
s1725696 6:f495a77c2c95 483
s1725696 6:f495a77c2c95 484 case FAILURE:
s1725696 6:f495a77c2c95 485
s1725696 6:f495a77c2c95 486 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 487 {
s1725696 6:f495a77c2c95 488 // Execute FAILURE mode
s1725696 10:56136a0da8c1 489 failure_mode();
s1725696 10:56136a0da8c1 490
s1725696 6:f495a77c2c95 491 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 492 }
s1725696 6:f495a77c2c95 493
s1725696 12:b2b082e73ef1 494 if(button_wait == false) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?)
s1725696 6:f495a77c2c95 495 {
s1725696 6:f495a77c2c95 496 CurrentState = WAIT;
s1725696 9:d7a6a3619576 497 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 498 StateChanged = true;
s1725696 6:f495a77c2c95 499 }
s1725696 6:f495a77c2c95 500
s1725696 6:f495a77c2c95 501 break;
s1725696 6:f495a77c2c95 502
s1725696 6:f495a77c2c95 503 case DEMO:
s1725696 6:f495a77c2c95 504
s1725696 6:f495a77c2c95 505 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 506 {
s1725696 6:f495a77c2c95 507 // Execute DEMO mode
s1725696 10:56136a0da8c1 508 demo_mode();
s1725696 10:56136a0da8c1 509
s1725696 6:f495a77c2c95 510 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 511 }
s1725696 6:f495a77c2c95 512
s1725696 12:b2b082e73ef1 513 if(button_wait == false) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 514 {
s1725696 6:f495a77c2c95 515 CurrentState = WAIT;
s1725696 9:d7a6a3619576 516 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 517 StateChanged = true;
s1725696 6:f495a77c2c95 518 }
s1725696 6:f495a77c2c95 519
s1725696 12:b2b082e73ef1 520 if (button_emergency == false) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 521 {
s1725696 6:f495a77c2c95 522 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 523 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 524 StateChanged = true;
s1725696 6:f495a77c2c95 525 }
s1725696 6:f495a77c2c95 526
s1725696 6:f495a77c2c95 527 break;
s1725696 6:f495a77c2c95 528
s1725696 6:f495a77c2c95 529 // no default
s1725696 4:8183e7b228f0 530 }
s1725696 6:f495a77c2c95 531
s1725696 6:f495a77c2c95 532 // while loop does not have to loop every time
s1725696 5:3581013d4505 533 }
s1725696 5:3581013d4505 534
s1725696 0:cb8857cf3ea4 535 }