groep 16 / Mbed 2 deprecated Project_BioRobotics_12

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@10:3f93fdb90c29, 2018-10-31 (annotated)

Committer:
Duif
Date:
Wed Oct 31 14:34:21 2018 +0000
Revision:
10:3f93fdb90c29
Parent:
9:8b2d6ec577e3
Child:
12:3e084e1a699e
Child:
18:f36ac3ee081a
EMG calibration added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mirjam 0:46dbc9b620d8 1 #include "mbed.h"
arnouddomhof 3:dca57056e5cb 2 #include "MODSERIAL.h"
AppelSab 6:a02ad75f0333 3 #include "QEI.h"
AppelSab 6:a02ad75f0333 4 #include "FastPWM.h"
AppelSab 6:a02ad75f0333 5 #include "math.h"
Mirjam 0:46dbc9b620d8 6
Mirjam 7:d4090f334ce2 7 #include "mbed.h"
arnouddomhof 8:2afb66572fc4 8 //#include "HIDScope.h"
Mirjam 7:d4090f334ce2 9 #include "BiQuad.h"
Mirjam 7:d4090f334ce2 10 #include "BiQuad4.h"
Mirjam 7:d4090f334ce2 11 #include "FilterDesign.h"
Mirjam 7:d4090f334ce2 12 #include "FilterDesign2.h"
Mirjam 7:d4090f334ce2 13
AppelSab 6:a02ad75f0333 14 // LED's
arnouddomhof 3:dca57056e5cb 15 DigitalOut led_red(LED_RED);
arnouddomhof 3:dca57056e5cb 16 DigitalOut led_green(LED_GREEN);
arnouddomhof 3:dca57056e5cb 17 DigitalOut led_blue(LED_BLUE);
AppelSab 6:a02ad75f0333 18 // Buttons
arnouddomhof 9:8b2d6ec577e3 19 DigitalIn button_clbrt_home(SW2);
arnouddomhof 9:8b2d6ec577e3 20 DigitalIn button_Demo(D5);
arnouddomhof 9:8b2d6ec577e3 21 DigitalIn button_Emg(D6);
AppelSab 6:a02ad75f0333 22 DigitalIn Fail_button(SW3);
AppelSab 6:a02ad75f0333 23 // Modserial
arnouddomhof 3:dca57056e5cb 24 MODSERIAL pc(USBTX, USBRX);
AppelSab 6:a02ad75f0333 25 // Encoders
AppelSab 6:a02ad75f0333 26 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
AppelSab 6:a02ad75f0333 27 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
AppelSab 6:a02ad75f0333 28 // Motors (direction and PWM)
AppelSab 6:a02ad75f0333 29 DigitalOut directionM1(D4);
AppelSab 6:a02ad75f0333 30 DigitalOut directionM2(D7);
AppelSab 6:a02ad75f0333 31 FastPWM motor1_pwm(D5);
AppelSab 6:a02ad75f0333 32 FastPWM motor2_pwm(D6);
Mirjam 7:d4090f334ce2 33 // EMG input en start value of filtered EMG
Mirjam 7:d4090f334ce2 34 AnalogIn emg1_raw( A0 );
Mirjam 7:d4090f334ce2 35 AnalogIn emg2_raw( A1 );
Mirjam 7:d4090f334ce2 36 double emg1_filtered = 0.00;
Mirjam 7:d4090f334ce2 37 double emg2_filtered = 0.00;
Mirjam 7:d4090f334ce2 38 float threshold_EMG = 0.25; // Threshold on 25 percent of the maximum EMG
Mirjam 7:d4090f334ce2 39
AppelSab 6:a02ad75f0333 40 // Declare timers and Tickers
Mirjam 7:d4090f334ce2 41 Timer timer; // Timer for counting time in this state
Mirjam 7:d4090f334ce2 42 Ticker WriteValues; // Ticker to show values of velocity to screen
Mirjam 7:d4090f334ce2 43 Ticker StateMachine;
arnouddomhof 8:2afb66572fc4 44 //Ticker sample_EMGtoHIDscope; // Ticker to send the EMG signals to screen
arnouddomhof 8:2afb66572fc4 45 //HIDScope scope(4); //Number of channels which needs to be send to the HIDScope
arnouddomhof 3:dca57056e5cb 46
AppelSab 6:a02ad75f0333 47 // Set up ProcessStateMachine
arnouddomhof 5:07e401cb251d 48 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
arnouddomhof 3:dca57056e5cb 49 states currentState = WAITING;
AppelSab 6:a02ad75f0333 50 bool stateChanged = true;
AppelSab 6:a02ad75f0333 51 volatile bool writeVelocityFlag = false;
AppelSab 6:a02ad75f0333 52
AppelSab 6:a02ad75f0333 53 // Global variables
arnouddomhof 3:dca57056e5cb 54 char c;
AppelSab 6:a02ad75f0333 55 int counts1;
AppelSab 6:a02ad75f0333 56 int counts2;
AppelSab 6:a02ad75f0333 57 float theta1;
AppelSab 6:a02ad75f0333 58 float theta2;
AppelSab 6:a02ad75f0333 59 float vel_1;
AppelSab 6:a02ad75f0333 60 float vel_2;
AppelSab 6:a02ad75f0333 61 float theta1_prev = 0.0;
AppelSab 6:a02ad75f0333 62 float theta2_prev = 0.0;
AppelSab 6:a02ad75f0333 63 const float pi = 3.14159265359;
AppelSab 6:a02ad75f0333 64 float tijd = 0.005;
AppelSab 6:a02ad75f0333 65 float time_in_state;
AppelSab 6:a02ad75f0333 66
Mirjam 7:d4090f334ce2 67 int need_to_move_1; // Does the robot needs to move in the first direction?
Mirjam 7:d4090f334ce2 68 int need_to_move_2; // Does the robot needs to move in the second direction?
Mirjam 7:d4090f334ce2 69 double EMG_calibrated_max_1 = 2.00000; // Maximum value of the first EMG signal found in the calibration state.
Mirjam 7:d4090f334ce2 70 double EMG_calibrated_max_2 = 2.00000; // Maximum value of the second EMG signal found in the calibration state.
Duif 10:3f93fdb90c29 71 double emg1_cal = 0.00000; //measured value of the first emg
Duif 10:3f93fdb90c29 72 double emg2_cal = 0.00000; //measured value of the second emg
Mirjam 7:d4090f334ce2 73
AppelSab 6:a02ad75f0333 74 // ----------------------------------------------
AppelSab 6:a02ad75f0333 75 // ------- FUNCTIONS ----------------------------
AppelSab 6:a02ad75f0333 76 // ----------------------------------------------
AppelSab 6:a02ad75f0333 77
AppelSab 6:a02ad75f0333 78 float ReadEncoder1() // Read Encoder of motor 1.
AppelSab 6:a02ad75f0333 79 {
AppelSab 6:a02ad75f0333 80 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 6:a02ad75f0333 81 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 6:a02ad75f0333 82 vel_1 = (theta1 - theta1_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
AppelSab 6:a02ad75f0333 83 theta1_prev = theta1; // Define theta_prev
AppelSab 6:a02ad75f0333 84 return vel_1;
AppelSab 6:a02ad75f0333 85 }
AppelSab 6:a02ad75f0333 86 float ReadEncoder2() // Read encoder of motor 2.
AppelSab 6:a02ad75f0333 87 {
AppelSab 6:a02ad75f0333 88 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 6:a02ad75f0333 89 theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 6:a02ad75f0333 90 vel_2 = (theta2 - theta2_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
AppelSab 6:a02ad75f0333 91 theta2_prev = theta2; // Define theta_prev
AppelSab 6:a02ad75f0333 92 return vel_2;
AppelSab 6:a02ad75f0333 93 }
AppelSab 6:a02ad75f0333 94 void MotorAngleCalibrate() // Function that drives motor 1 and 2.
AppelSab 6:a02ad75f0333 95 {
AppelSab 6:a02ad75f0333 96 float U1 = -0.2; // Negative, so arm goes backwards.
AppelSab 6:a02ad75f0333 97 float U2 = -0.2; // Motor 2 is not taken into account yet.
AppelSab 6:a02ad75f0333 98
AppelSab 6:a02ad75f0333 99 motor1_pwm.write(fabs(U1)); // Send PWM values to motor
AppelSab 6:a02ad75f0333 100 motor2_pwm.write(fabs(U2));
AppelSab 6:a02ad75f0333 101
AppelSab 6:a02ad75f0333 102 directionM1 = U1 > 0.0f; // Either true or false, determines direction.
AppelSab 6:a02ad75f0333 103 directionM2 = U2 > 0.0f;
AppelSab 6:a02ad75f0333 104 }
Mirjam 7:d4090f334ce2 105 void sample()
Mirjam 7:d4090f334ce2 106 {
Mirjam 7:d4090f334ce2 107 emg1_filtered = FilterDesign(emg1_raw.read());
Mirjam 7:d4090f334ce2 108 emg2_filtered = FilterDesign2(emg2_raw.read());
Mirjam 7:d4090f334ce2 109
arnouddomhof 8:2afb66572fc4 110 /**
Mirjam 7:d4090f334ce2 111 scope.set(0, emg1_raw.read()); // Raw EMG 1 send to scope 0
Mirjam 7:d4090f334ce2 112 scope.set(1, emg1_filtered); // Filtered EMG 1 send to scope 1
Mirjam 7:d4090f334ce2 113 scope.set(2, emg2_raw.read()); // Raw EMG 2 send to scope 2
Mirjam 7:d4090f334ce2 114 scope.set(3, emg2_filtered); // Filtered EMG 2 send to scope 3
Mirjam 7:d4090f334ce2 115 scope.send(); // Send the data to the computer
arnouddomhof 8:2afb66572fc4 116 */
Mirjam 7:d4090f334ce2 117 }
AppelSab 6:a02ad75f0333 118 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 119 // --------STATEMACHINE-------------------------------
AppelSab 6:a02ad75f0333 120 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 121 void ProcessStateMachine(void)
AppelSab 6:a02ad75f0333 122 {
AppelSab 6:a02ad75f0333 123 switch (currentState)
AppelSab 6:a02ad75f0333 124 {
AppelSab 6:a02ad75f0333 125 case WAITING:
AppelSab 6:a02ad75f0333 126 // Description:
AppelSab 6:a02ad75f0333 127 // In this state we do nothing, and wait for a command
AppelSab 6:a02ad75f0333 128
AppelSab 6:a02ad75f0333 129 // Actions
AppelSab 6:a02ad75f0333 130 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
AppelSab 6:a02ad75f0333 131
AppelSab 6:a02ad75f0333 132 // State transition logic
arnouddomhof 9:8b2d6ec577e3 133 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 134 {
AppelSab 6:a02ad75f0333 135 currentState = MOTOR_ANGLE_CLBRT;
AppelSab 6:a02ad75f0333 136 stateChanged = true;
AppelSab 6:a02ad75f0333 137 pc.printf("Starting Calibration\n\r");
AppelSab 6:a02ad75f0333 138 }
arnouddomhof 9:8b2d6ec577e3 139 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 140 {
AppelSab 6:a02ad75f0333 141 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 142 stateChanged = true;
AppelSab 6:a02ad75f0333 143 }
AppelSab 6:a02ad75f0333 144 break;
AppelSab 6:a02ad75f0333 145
AppelSab 6:a02ad75f0333 146 case MOTOR_ANGLE_CLBRT:
AppelSab 6:a02ad75f0333 147 // Description:
AppelSab 6:a02ad75f0333 148 // In this state the robot moves with low motor PWM to some
AppelSab 6:a02ad75f0333 149 // mechanical limit of motion, in order to calibrate the motors.
AppelSab 6:a02ad75f0333 150
AppelSab 6:a02ad75f0333 151 // Actions
AppelSab 6:a02ad75f0333 152 led_red = 1; led_green = 0; led_blue = 0; // Colouring the led TURQUOISE
AppelSab 6:a02ad75f0333 153 timer.start(); //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT"
AppelSab 6:a02ad75f0333 154 if (stateChanged)
AppelSab 6:a02ad75f0333 155 {
AppelSab 6:a02ad75f0333 156 MotorAngleCalibrate(); // Actuate motor 1 and 2.
AppelSab 6:a02ad75f0333 157 vel_1 = ReadEncoder1(); // Get velocity of motor 1
AppelSab 6:a02ad75f0333 158 vel_2 = ReadEncoder2(); // Get velocity of motor 2
AppelSab 6:a02ad75f0333 159 stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
AppelSab 6:a02ad75f0333 160 }
AppelSab 6:a02ad75f0333 161
AppelSab 6:a02ad75f0333 162 // State transition logic
AppelSab 6:a02ad75f0333 163 time_in_state = timer.read(); // Determine if this state has run for long enough.
arnouddomhof 3:dca57056e5cb 164
AppelSab 6:a02ad75f0333 165 if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
AppelSab 6:a02ad75f0333 166 {
AppelSab 6:a02ad75f0333 167 //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);
AppelSab 6:a02ad75f0333 168 //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
arnouddomhof 8:2afb66572fc4 169 pc.printf("Motor calibration has ended. \n\r");
AppelSab 6:a02ad75f0333 170 timer.stop(); // Stop timer for this state
AppelSab 6:a02ad75f0333 171 timer.reset(); // Reset timer for this state
AppelSab 6:a02ad75f0333 172 motor1_pwm.write(fabs(0.0)); // Send PWM values to motor
AppelSab 6:a02ad75f0333 173 motor2_pwm.write(fabs(0.0));
AppelSab 6:a02ad75f0333 174 Encoder1.reset(); // Reset Encoders when arrived at zero-position
AppelSab 6:a02ad75f0333 175 Encoder2.reset();
AppelSab 6:a02ad75f0333 176
AppelSab 6:a02ad75f0333 177 currentState = EMG_CLBRT; // Switch to next state (EMG_CLRBRT).
arnouddomhof 8:2afb66572fc4 178 pc.printf("EMG calibration \r\n");
AppelSab 6:a02ad75f0333 179 stateChanged = true;
AppelSab 6:a02ad75f0333 180 }
AppelSab 6:a02ad75f0333 181 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 182 {
AppelSab 6:a02ad75f0333 183 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 184 stateChanged = true;
AppelSab 6:a02ad75f0333 185 }
AppelSab 6:a02ad75f0333 186 break;
AppelSab 6:a02ad75f0333 187
AppelSab 6:a02ad75f0333 188 case EMG_CLBRT:
AppelSab 6:a02ad75f0333 189 // In this state the person whom is connected to the robot needs
AppelSab 6:a02ad75f0333 190 // to flex his/her muscles as hard as possible, in order to
AppelSab 6:a02ad75f0333 191 // measure the maximum EMG-signal, which can be used to scale
AppelSab 6:a02ad75f0333 192 // the EMG-filter.
AppelSab 6:a02ad75f0333 193
AppelSab 6:a02ad75f0333 194 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led BLUE
AppelSab 6:a02ad75f0333 195
AppelSab 6:a02ad75f0333 196 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 197 // If enough time has passed (5 sec), and the EMG-signal drops below 10%
AppelSab 6:a02ad75f0333 198 // of the maximum measured value, we move to the Homing state.
AppelSab 6:a02ad75f0333 199
Duif 10:3f93fdb90c29 200 for (int i = 0; i <= 10; i++) //10 measuring points
Duif 10:3f93fdb90c29 201 {
Duif 10:3f93fdb90c29 202 if (emg1_cal > EMG_calibrated_max_1){
Duif 10:3f93fdb90c29 203 EMG_calibrated_max_1 = emg1_cal;}
Duif 10:3f93fdb90c29 204
Duif 10:3f93fdb90c29 205 if (emg2_cal > EMG_calibrated_max_2){
Duif 10:3f93fdb90c29 206 EMG_calibrated_max_2 = emg2_cal;}
Duif 10:3f93fdb90c29 207
Duif 10:3f93fdb90c29 208 //pc.printf("EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
Duif 10:3f93fdb90c29 209 wait(0.5f);
Duif 10:3f93fdb90c29 210 }
Duif 10:3f93fdb90c29 211
AppelSab 6:a02ad75f0333 212 currentState = HOMING;
AppelSab 6:a02ad75f0333 213 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 214 {
AppelSab 6:a02ad75f0333 215 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 216 stateChanged = true;
AppelSab 6:a02ad75f0333 217 }
AppelSab 6:a02ad75f0333 218 break;
AppelSab 6:a02ad75f0333 219
AppelSab 6:a02ad75f0333 220 case HOMING:
AppelSab 6:a02ad75f0333 221 // Description:
AppelSab 6:a02ad75f0333 222 // Robot moves to the home starting configuration
arnouddomhof 9:8b2d6ec577e3 223 pc.printf("HOMING \r\n");
AppelSab 6:a02ad75f0333 224
AppelSab 6:a02ad75f0333 225 led_red = 0; led_green = 1; led_red = 0; // Colouring the led PURPLE
AppelSab 6:a02ad75f0333 226
AppelSab 6:a02ad75f0333 227 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 228 // If we are in the right location, within some margin, we move to the Waiting for
AppelSab 6:a02ad75f0333 229 // signal state.
AppelSab 6:a02ad75f0333 230
AppelSab 6:a02ad75f0333 231 wait(5.0f); // time_in_this_state > 5.0f
AppelSab 6:a02ad75f0333 232 // INSERT MOVEMENT
AppelSab 6:a02ad75f0333 233 currentState = WAITING4SIGNAL;
AppelSab 6:a02ad75f0333 234 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 235 {
AppelSab 6:a02ad75f0333 236 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 237 stateChanged = true;
AppelSab 6:a02ad75f0333 238 }
AppelSab 6:a02ad75f0333 239 break;
AppelSab 6:a02ad75f0333 240
AppelSab 6:a02ad75f0333 241 case WAITING4SIGNAL:
AppelSab 6:a02ad75f0333 242 // Description:
AppelSab 6:a02ad75f0333 243 // In this state the robot waits for an action to occur.
arnouddomhof 9:8b2d6ec577e3 244
AppelSab 6:a02ad75f0333 245 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
AppelSab 6:a02ad75f0333 246
AppelSab 6:a02ad75f0333 247 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 248 // If a certain button is pressed we move to the corresponding
AppelSab 6:a02ad75f0333 249 // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
arnouddomhof 9:8b2d6ec577e3 250
arnouddomhof 9:8b2d6ec577e3 251 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 252 {
arnouddomhof 9:8b2d6ec577e3 253 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 9:8b2d6ec577e3 254 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 255 pc.printf("Starting Calibration \n\r");
AppelSab 6:a02ad75f0333 256 }
arnouddomhof 9:8b2d6ec577e3 257 else if (button_Demo == 1)
AppelSab 6:a02ad75f0333 258 {
arnouddomhof 9:8b2d6ec577e3 259 currentState = MOVE_W_DEMO;
arnouddomhof 9:8b2d6ec577e3 260 pc.printf("DEMO \r\n");
arnouddomhof 9:8b2d6ec577e3 261 wait(1.0f);
AppelSab 6:a02ad75f0333 262 }
arnouddomhof 9:8b2d6ec577e3 263 else if (button_Emg == 1)
AppelSab 6:a02ad75f0333 264 {
arnouddomhof 9:8b2d6ec577e3 265 currentState = MOVE_W_EMG;
arnouddomhof 9:8b2d6ec577e3 266 pc.printf("EMG \r\n");
arnouddomhof 9:8b2d6ec577e3 267 wait(1.0f);
AppelSab 6:a02ad75f0333 268 }
arnouddomhof 9:8b2d6ec577e3 269 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 270 {
AppelSab 6:a02ad75f0333 271 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 272 stateChanged = true;
AppelSab 6:a02ad75f0333 273 }
arnouddomhof 9:8b2d6ec577e3 274
AppelSab 6:a02ad75f0333 275 break;
AppelSab 6:a02ad75f0333 276
AppelSab 6:a02ad75f0333 277 case MOVE_W_DEMO:
AppelSab 6:a02ad75f0333 278 // Description:
AppelSab 6:a02ad75f0333 279 // In this state the robot follows a preprogrammed shape, e.g.
AppelSab 6:a02ad75f0333 280 // a square.
AppelSab 6:a02ad75f0333 281
AppelSab 6:a02ad75f0333 282 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led GREEN
AppelSab 6:a02ad75f0333 283
AppelSab 6:a02ad75f0333 284 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 285 // When the home button or the failure button is pressed, we
AppelSab 6:a02ad75f0333 286 // will the move to the corresponding state.
AppelSab 6:a02ad75f0333 287
AppelSab 6:a02ad75f0333 288 // BUILD DEMO MODE
arnouddomhof 9:8b2d6ec577e3 289
arnouddomhof 9:8b2d6ec577e3 290 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 291 {
arnouddomhof 9:8b2d6ec577e3 292 currentState = HOMING;
arnouddomhof 9:8b2d6ec577e3 293 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 294 pc.printf("Moving home\n\r");
AppelSab 6:a02ad75f0333 295 }
arnouddomhof 9:8b2d6ec577e3 296 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 297 {
AppelSab 6:a02ad75f0333 298 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 299 stateChanged = true;
AppelSab 6:a02ad75f0333 300 }
AppelSab 6:a02ad75f0333 301 break;
AppelSab 6:a02ad75f0333 302
AppelSab 6:a02ad75f0333 303 case MOVE_W_EMG:
AppelSab 6:a02ad75f0333 304 // Description:
AppelSab 6:a02ad75f0333 305 // In this state the robot will be controlled by use of
AppelSab 6:a02ad75f0333 306 // EMG-signals.
AppelSab 6:a02ad75f0333 307
AppelSab 6:a02ad75f0333 308 led_red = 1; led_green = 0; led_blue = 1; // Colouring the led GREEN
Mirjam 7:d4090f334ce2 309
Mirjam 7:d4090f334ce2 310 if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
Mirjam 7:d4090f334ce2 311 need_to_move_1 = 1; // The robot does have to move
Mirjam 7:d4090f334ce2 312 }
Mirjam 7:d4090f334ce2 313 else {
Mirjam 7:d4090f334ce2 314 need_to_move_1 = 0; // If the robot does not have to move
Mirjam 7:d4090f334ce2 315 }
Mirjam 7:d4090f334ce2 316
arnouddomhof 8:2afb66572fc4 317 if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
Mirjam 7:d4090f334ce2 318 need_to_move_2 = 1;
Mirjam 7:d4090f334ce2 319 }
Mirjam 7:d4090f334ce2 320 else {
Mirjam 7:d4090f334ce2 321 need_to_move_2 = 0;
Mirjam 7:d4090f334ce2 322 }
AppelSab 6:a02ad75f0333 323 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 324 // When the home button or the failure button is pressed, we
AppelSab 6:a02ad75f0333 325 // will the move to the corresponding state.
arnouddomhof 9:8b2d6ec577e3 326
arnouddomhof 9:8b2d6ec577e3 327 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 328 {
arnouddomhof 9:8b2d6ec577e3 329 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 9:8b2d6ec577e3 330 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 331 pc.printf("Starting Calibration \n\r");
arnouddomhof 9:8b2d6ec577e3 332 }
arnouddomhof 9:8b2d6ec577e3 333 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 334 {
AppelSab 6:a02ad75f0333 335 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 336 stateChanged = true;
AppelSab 6:a02ad75f0333 337 }
AppelSab 6:a02ad75f0333 338 break;
AppelSab 6:a02ad75f0333 339
AppelSab 6:a02ad75f0333 340 case FAILURE_MODE:
AppelSab 6:a02ad75f0333 341 // Description:
AppelSab 6:a02ad75f0333 342 // This state is reached when the failure button is reached.
AppelSab 6:a02ad75f0333 343 // In this state everything is turned off.
AppelSab 6:a02ad75f0333 344
AppelSab 6:a02ad75f0333 345 led_red = 0; led_green = 1; led_blue = 1; // Colouring the led RED
AppelSab 6:a02ad75f0333 346 // Actions
AppelSab 6:a02ad75f0333 347 if (stateChanged)
AppelSab 6:a02ad75f0333 348 {
AppelSab 6:a02ad75f0333 349 motor1_pwm.write(fabs(0.0)); // Stop all motors!
AppelSab 6:a02ad75f0333 350 motor2_pwm.write(fabs(0.0));
AppelSab 6:a02ad75f0333 351 pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
AppelSab 6:a02ad75f0333 352 stateChanged = false;
AppelSab 6:a02ad75f0333 353 }
AppelSab 6:a02ad75f0333 354 break;
AppelSab 6:a02ad75f0333 355
AppelSab 6:a02ad75f0333 356 // State transition logic
AppelSab 6:a02ad75f0333 357 // No state transition, you need to restart the robot.
AppelSab 6:a02ad75f0333 358
AppelSab 6:a02ad75f0333 359 default:
AppelSab 6:a02ad75f0333 360 // This state is a default state, this state is reached when
AppelSab 6:a02ad75f0333 361 // the program somehow defies all of the other states.
AppelSab 6:a02ad75f0333 362
AppelSab 6:a02ad75f0333 363 pc.printf("Unknown or unimplemented state reached!!! \n\r");
AppelSab 6:a02ad75f0333 364 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
AppelSab 6:a02ad75f0333 365 for (int n = 0; n < 50; n++) // Making an SOS signal with the RED led
AppelSab 6:a02ad75f0333 366 {
AppelSab 6:a02ad75f0333 367 for (int i = 0; i < 6; i++)
AppelSab 6:a02ad75f0333 368 {
AppelSab 6:a02ad75f0333 369 led_red = !led_red;
AppelSab 6:a02ad75f0333 370 wait(0.6f);
AppelSab 6:a02ad75f0333 371 }
AppelSab 6:a02ad75f0333 372 wait(0.4f);
AppelSab 6:a02ad75f0333 373 for (int i = 0 ; i < 6; i++)
AppelSab 6:a02ad75f0333 374 {
AppelSab 6:a02ad75f0333 375 led_red = !led_red;
AppelSab 6:a02ad75f0333 376 wait(0.2f);
AppelSab 6:a02ad75f0333 377 }
AppelSab 6:a02ad75f0333 378 wait(0.4f);
AppelSab 6:a02ad75f0333 379 }
arnouddomhof 3:dca57056e5cb 380 }
AppelSab 6:a02ad75f0333 381 }
AppelSab 6:a02ad75f0333 382
AppelSab 6:a02ad75f0333 383 // --------------------------------
AppelSab 6:a02ad75f0333 384 // ----- MAIN LOOP ----------------
AppelSab 6:a02ad75f0333 385 // --------------------------------
AppelSab 6:a02ad75f0333 386
Mirjam 0:46dbc9b620d8 387 int main()
Mirjam 0:46dbc9b620d8 388 {
Mirjam 4:a0c1c021026b 389 // Switch all LEDs off
arnouddomhof 3:dca57056e5cb 390 led_red = 1;
arnouddomhof 3:dca57056e5cb 391 led_green = 1;
arnouddomhof 3:dca57056e5cb 392 led_blue = 1;
AppelSab 6:a02ad75f0333 393
arnouddomhof 3:dca57056e5cb 394 pc.baud(115200);
arnouddomhof 8:2afb66572fc4 395
arnouddomhof 8:2afb66572fc4 396 pc.printf("\r\n _______________ INSERT ROBOT NAME HERE! _______________ \r\n");
arnouddomhof 8:2afb66572fc4 397 wait(0.5f);
arnouddomhof 8:2afb66572fc4 398 pc.printf("WAITING... \r\n");
arnouddomhof 8:2afb66572fc4 399
AppelSab 6:a02ad75f0333 400 StateMachine.attach(&ProcessStateMachine, 0.005f); // Run statemachine 200 times per second
arnouddomhof 8:2afb66572fc4 401 /**
Mirjam 7:d4090f334ce2 402 sample_EMGtoHIDscope.attach(&sample, 0.02f); // Display EMG values 50 times per second
arnouddomhof 8:2afb66572fc4 403 */
Mirjam 0:46dbc9b620d8 404 while (true) {
AppelSab 6:a02ad75f0333 405
AppelSab 6:a02ad75f0333 406 }
AppelSab 6:a02ad75f0333 407 }
AppelSab 6:a02ad75f0333 408
arnouddomhof 5:07e401cb251d 409