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@5:07e401cb251d, 2018-10-22 (annotated)

Committer:
arnouddomhof
Date:
Mon Oct 22 16:18:40 2018 +0000
Revision:
5:07e401cb251d
Parent:
4:a0c1c021026b
Child:
6:a02ad75f0333
Added some necessary comments and removed the SHUTDOWN state, as it is redundant, as we do not really need to shut it down.

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"
Mirjam 0:46dbc9b620d8 3
arnouddomhof 3:dca57056e5cb 4 DigitalOut led_red(LED_RED);
arnouddomhof 3:dca57056e5cb 5 DigitalOut led_green(LED_GREEN);
arnouddomhof 3:dca57056e5cb 6 DigitalOut led_blue(LED_BLUE);
arnouddomhof 3:dca57056e5cb 7 DigitalIn button(SW2);
arnouddomhof 3:dca57056e5cb 8 InterruptIn Fail_button(SW3);
arnouddomhof 3:dca57056e5cb 9 MODSERIAL pc(USBTX, USBRX);
arnouddomhof 3:dca57056e5cb 10
arnouddomhof 5:07e401cb251d 11 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
arnouddomhof 3:dca57056e5cb 12 states currentState = WAITING;
arnouddomhof 3:dca57056e5cb 13 char c;
arnouddomhof 3:dca57056e5cb 14
arnouddomhof 3:dca57056e5cb 15 void Failing() {
arnouddomhof 3:dca57056e5cb 16 currentState = FAILURE_MODE;
arnouddomhof 3:dca57056e5cb 17 wait(1.0f);
arnouddomhof 3:dca57056e5cb 18 }
Mirjam 0:46dbc9b620d8 19
Mirjam 0:46dbc9b620d8 20 int main()
Mirjam 0:46dbc9b620d8 21 {
Mirjam 4:a0c1c021026b 22 // Switch all LEDs off
arnouddomhof 3:dca57056e5cb 23 led_red = 1;
arnouddomhof 3:dca57056e5cb 24 led_green = 1;
arnouddomhof 3:dca57056e5cb 25 led_blue = 1;
arnouddomhof 3:dca57056e5cb 26 pc.baud(115200);
arnouddomhof 3:dca57056e5cb 27 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 28
arnouddomhof 3:dca57056e5cb 29 pc.printf("\r\n______________ STATE MACHINE ______________ \r\n\r\n");
arnouddomhof 3:dca57056e5cb 30
Mirjam 0:46dbc9b620d8 31 while (true) {
arnouddomhof 3:dca57056e5cb 32 switch (currentState)
arnouddomhof 3:dca57056e5cb 33 {
arnouddomhof 3:dca57056e5cb 34 case WAITING:
arnouddomhof 5:07e401cb251d 35 // Description:
arnouddomhof 5:07e401cb251d 36 // In this state we do nothing
arnouddomhof 5:07e401cb251d 37
arnouddomhof 3:dca57056e5cb 38 pc.printf("WAITING... \r\n");
arnouddomhof 3:dca57056e5cb 39 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
arnouddomhof 3:dca57056e5cb 40 if (!button) {
arnouddomhof 3:dca57056e5cb 41 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 3:dca57056e5cb 42 }
arnouddomhof 3:dca57056e5cb 43 wait(1.0f);
arnouddomhof 3:dca57056e5cb 44 break;
Mirjam 4:a0c1c021026b 45
arnouddomhof 3:dca57056e5cb 46 case MOTOR_ANGLE_CLBRT:
arnouddomhof 5:07e401cb251d 47 // Description:
arnouddomhof 5:07e401cb251d 48 // In this state the robot moves with low motor PWM to some
arnouddomhof 5:07e401cb251d 49 // mechanical limit of motion.
arnouddomhof 5:07e401cb251d 50
arnouddomhof 3:dca57056e5cb 51 pc.printf("Motor angle calibration \r\n");
arnouddomhof 3:dca57056e5cb 52 led_red = 1; led_green = 0; led_blue = 0; // Colouring the led TURQUOISE
arnouddomhof 5:07e401cb251d 53
arnouddomhof 5:07e401cb251d 54 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 55 // If enough time has passed in this state (3 sec) and the
arnouddomhof 5:07e401cb251d 56 // motors stopped moving, we move to the EMG-calibration state.
arnouddomhof 5:07e401cb251d 57
Mirjam 4:a0c1c021026b 58 wait(3.0f); // time_in_this_state > 3.0f
arnouddomhof 3:dca57056e5cb 59 // if (fabs(motor_velocity) < 0.01f) {
Mirjam 4:a0c1c021026b 60 // INSERT CALIBRATING
arnouddomhof 3:dca57056e5cb 61 currentState = EMG_CLBRT;
arnouddomhof 3:dca57056e5cb 62 break;
Mirjam 4:a0c1c021026b 63
arnouddomhof 3:dca57056e5cb 64 case EMG_CLBRT:
arnouddomhof 5:07e401cb251d 65 // In this state the person whom is connected to the robot needs
arnouddomhof 5:07e401cb251d 66 // to flex his/her muscles as hard as possible, in order to
arnouddomhof 5:07e401cb251d 67 // measure the maximum EMG-signal, which can be used to scale
arnouddomhof 5:07e401cb251d 68 // the EMG-filter.
arnouddomhof 5:07e401cb251d 69
arnouddomhof 3:dca57056e5cb 70 pc.printf("EMG calibration \r\n");
arnouddomhof 3:dca57056e5cb 71 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led BLUE
arnouddomhof 5:07e401cb251d 72
arnouddomhof 5:07e401cb251d 73 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 74 // If enough time has passed (5 sec), and the EMG-signal drops below 10%
arnouddomhof 5:07e401cb251d 75 // of the maximum measured value, we move to the Homing state.
arnouddomhof 5:07e401cb251d 76
arnouddomhof 3:dca57056e5cb 77 wait(5.0f); // time_in_this_state > 5.0f
arnouddomhof 3:dca57056e5cb 78 // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
Mirjam 4:a0c1c021026b 79 // INSERT CALIBRATING
arnouddomhof 3:dca57056e5cb 80 currentState = HOMING;
Mirjam 4:a0c1c021026b 81 break;
Mirjam 4:a0c1c021026b 82
arnouddomhof 3:dca57056e5cb 83 case HOMING:
arnouddomhof 5:07e401cb251d 84 // Description:
arnouddomhof 5:07e401cb251d 85 // Robot moves to the home starting configuration
arnouddomhof 5:07e401cb251d 86
arnouddomhof 3:dca57056e5cb 87 pc.printf("HOMING, moving to the home starting configuration. \r\n");
arnouddomhof 3:dca57056e5cb 88 led_red = 0; led_green = 1; led_red = 0; // Colouring the led PURPLE
arnouddomhof 5:07e401cb251d 89
arnouddomhof 5:07e401cb251d 90 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 91 // If we are in the right location, within some margin, we move to the Waiting for
arnouddomhof 5:07e401cb251d 92 // signal state.
arnouddomhof 5:07e401cb251d 93
arnouddomhof 3:dca57056e5cb 94 wait(5.0f); // time_in_this_state > 5.0f
arnouddomhof 3:dca57056e5cb 95 // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
Mirjam 4:a0c1c021026b 96 // INSERT MOVEMENT
arnouddomhof 3:dca57056e5cb 97 currentState = WAITING4SIGNAL;
arnouddomhof 3:dca57056e5cb 98 break;
Mirjam 4:a0c1c021026b 99
arnouddomhof 3:dca57056e5cb 100 case WAITING4SIGNAL:
arnouddomhof 5:07e401cb251d 101 // Description:
arnouddomhof 5:07e401cb251d 102 // In this state the robot waits for an action to occur.
arnouddomhof 5:07e401cb251d 103
arnouddomhof 3:dca57056e5cb 104 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
arnouddomhof 5:07e401cb251d 105
arnouddomhof 5:07e401cb251d 106 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 107 // If a certain button is pressed we move to the corresponding
arnouddomhof 5:07e401cb251d 108 // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
arnouddomhof 5:07e401cb251d 109
Mirjam 4:a0c1c021026b 110 // CANNOT USE KEYBOARD. SO LOOK FOR ALTERNATIVE
arnouddomhof 5:07e401cb251d 111 pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate. \r\n");
arnouddomhof 3:dca57056e5cb 112 c = pc.getc();
arnouddomhof 3:dca57056e5cb 113 if (c == 'd') {
arnouddomhof 3:dca57056e5cb 114 currentState = MOVE_W_DEMO;
arnouddomhof 3:dca57056e5cb 115 }
arnouddomhof 3:dca57056e5cb 116 else if (c == 'e') {
arnouddomhof 3:dca57056e5cb 117 currentState = MOVE_W_EMG;
arnouddomhof 3:dca57056e5cb 118 }
arnouddomhof 3:dca57056e5cb 119 else if (c == 'c') {
arnouddomhof 3:dca57056e5cb 120 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 3:dca57056e5cb 121 }
arnouddomhof 5:07e401cb251d 122
arnouddomhof 3:dca57056e5cb 123 break;
Mirjam 4:a0c1c021026b 124
Mirjam 4:a0c1c021026b 125
arnouddomhof 3:dca57056e5cb 126 case MOVE_W_DEMO:
arnouddomhof 5:07e401cb251d 127 // Description:
arnouddomhof 5:07e401cb251d 128 // In this state the robot follows a preprogrammed shape, e.g.
arnouddomhof 5:07e401cb251d 129 // a square.
arnouddomhof 5:07e401cb251d 130
arnouddomhof 3:dca57056e5cb 131 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led GREEN
arnouddomhof 3:dca57056e5cb 132 pc.printf("MOVE_W_DEMO, Running the demo \r\n");
arnouddomhof 5:07e401cb251d 133
arnouddomhof 5:07e401cb251d 134 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 135 // When the home button or the failure button is pressed, we
arnouddomhof 5:07e401cb251d 136 // will the move to the corresponding state.
arnouddomhof 5:07e401cb251d 137
Mirjam 4:a0c1c021026b 138 // BUILD DEMO MODE
Mirjam 4:a0c1c021026b 139 // FIND ALTERNATIVE FOR KEYBOARD
arnouddomhof 3:dca57056e5cb 140 c = pc.getcNb();
arnouddomhof 3:dca57056e5cb 141 if (c == 'h') {
arnouddomhof 3:dca57056e5cb 142 currentState = HOMING;
arnouddomhof 3:dca57056e5cb 143 }
arnouddomhof 3:dca57056e5cb 144 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 145 wait(1.0f);
arnouddomhof 3:dca57056e5cb 146 break;
Mirjam 4:a0c1c021026b 147
arnouddomhof 3:dca57056e5cb 148 case MOVE_W_EMG:
arnouddomhof 5:07e401cb251d 149 // Description:
arnouddomhof 5:07e401cb251d 150 // In this state the robot will be controlled by use of
arnouddomhof 5:07e401cb251d 151 // EMG-signals.
arnouddomhof 5:07e401cb251d 152
arnouddomhof 3:dca57056e5cb 153 led_red = 1; led_green = 0; led_blue = 1; // Colouring the led GREEN
arnouddomhof 3:dca57056e5cb 154 pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
arnouddomhof 5:07e401cb251d 155
arnouddomhof 5:07e401cb251d 156 // Requirements to move to the next state:
arnouddomhof 5:07e401cb251d 157 // When the home button or the failure button is pressed, we
arnouddomhof 5:07e401cb251d 158 // will the move to the corresponding state.
arnouddomhof 5:07e401cb251d 159
Mirjam 4:a0c1c021026b 160 // BUILD THE MOVEMENT WITH EMG
arnouddomhof 3:dca57056e5cb 161 wait(1);
arnouddomhof 3:dca57056e5cb 162 c = pc.getcNb();
arnouddomhof 3:dca57056e5cb 163 if (c == 'h') {
arnouddomhof 3:dca57056e5cb 164 currentState = HOMING;
arnouddomhof 3:dca57056e5cb 165 }
arnouddomhof 3:dca57056e5cb 166 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 167 wait(1.0f);
Mirjam 4:a0c1c021026b 168 break;
Mirjam 4:a0c1c021026b 169
arnouddomhof 3:dca57056e5cb 170 case FAILURE_MODE:
arnouddomhof 5:07e401cb251d 171 // Description:
arnouddomhof 5:07e401cb251d 172 // This state is reached when the failure button is reached.
arnouddomhof 5:07e401cb251d 173 // In this state everything is turned off.
arnouddomhof 5:07e401cb251d 174
arnouddomhof 3:dca57056e5cb 175 led_red = 0; led_green = 1; led_blue = 1; // Colouring the led RED
Mirjam 4:a0c1c021026b 176 // WHAT NEEDS TO HAPPEN IN FAILURE MODE?
arnouddomhof 3:dca57056e5cb 177 pc.printf("FAILURE MODE \r\n");
arnouddomhof 3:dca57056e5cb 178 wait(2.0f);
arnouddomhof 3:dca57056e5cb 179 break;
arnouddomhof 3:dca57056e5cb 180
arnouddomhof 3:dca57056e5cb 181
arnouddomhof 3:dca57056e5cb 182 default:
arnouddomhof 5:07e401cb251d 183 // This state is a default state, this state is reached when
arnouddomhof 5:07e401cb251d 184 // the program somehow defies all of the other states.
arnouddomhof 5:07e401cb251d 185
arnouddomhof 3:dca57056e5cb 186 pc.printf("Unknown or unimplemented state reached!!! \n\r");
arnouddomhof 3:dca57056e5cb 187 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
arnouddomhof 3:dca57056e5cb 188 for (int n = 0; n < 50; n++) { // Making an SOS signal with the RED led
arnouddomhof 3:dca57056e5cb 189 for (int i = 0; i < 6; i++) {
arnouddomhof 3:dca57056e5cb 190 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 191 wait(0.6f);
arnouddomhof 3:dca57056e5cb 192 }
arnouddomhof 3:dca57056e5cb 193 wait(0.4f);
arnouddomhof 3:dca57056e5cb 194 for (int i = 0 ; i < 6; i++) {
arnouddomhof 3:dca57056e5cb 195 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 196 wait(0.2f);
arnouddomhof 3:dca57056e5cb 197 }
arnouddomhof 3:dca57056e5cb 198 wait(0.4f);
arnouddomhof 3:dca57056e5cb 199 }
arnouddomhof 3:dca57056e5cb 200 } // end of switch
Mirjam 0:46dbc9b620d8 201 }
arnouddomhof 3:dca57056e5cb 202 }
arnouddomhof 3:dca57056e5cb 203