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@3:dca57056e5cb, 2018-10-22 (annotated)

Committer:
arnouddomhof
Date:
Mon Oct 22 14:45:37 2018 +0000
Revision:
3:dca57056e5cb
Parent:
2:b6b962d7db36
Child:
4:a0c1c021026b
State machine af en toegevoegd

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 3:dca57056e5cb 11 enum states {WAITING,MOTOR_ANGLE_CLBRT,EMG_CLBRT,HOMING,WAITING4SIGNAL,MOVE_W_EMG,MOVE_W_DEMO,FAILURE_MODE,SHUTDOWN};
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 {
arnouddomhof 3:dca57056e5cb 22 led_red = 1;
arnouddomhof 3:dca57056e5cb 23 led_green = 1;
arnouddomhof 3:dca57056e5cb 24 led_blue = 1;
arnouddomhof 3:dca57056e5cb 25 pc.baud(115200);
arnouddomhof 3:dca57056e5cb 26 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 27
arnouddomhof 3:dca57056e5cb 28 pc.printf("\r\n______________ STATE MACHINE ______________ \r\n\r\n");
arnouddomhof 3:dca57056e5cb 29
Mirjam 0:46dbc9b620d8 30 while (true) {
arnouddomhof 3:dca57056e5cb 31 switch (currentState)
arnouddomhof 3:dca57056e5cb 32 {
arnouddomhof 3:dca57056e5cb 33 case WAITING:
arnouddomhof 3:dca57056e5cb 34 pc.printf("WAITING... \r\n");
arnouddomhof 3:dca57056e5cb 35 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
arnouddomhof 3:dca57056e5cb 36 if (!button) {
arnouddomhof 3:dca57056e5cb 37 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 3:dca57056e5cb 38 }
arnouddomhof 3:dca57056e5cb 39 wait(1.0f);
arnouddomhof 3:dca57056e5cb 40 break;
arnouddomhof 3:dca57056e5cb 41 case MOTOR_ANGLE_CLBRT:
arnouddomhof 3:dca57056e5cb 42 pc.printf("Motor angle calibration \r\n");
arnouddomhof 3:dca57056e5cb 43 led_red = 1; led_green = 0; led_blue = 0; // Colouring the led TURQUOISE
arnouddomhof 3:dca57056e5cb 44 wait(3.0f); // time_in_this_state > 3.0f
arnouddomhof 3:dca57056e5cb 45 // if (fabs(motor_velocity) < 0.01f) {
arnouddomhof 3:dca57056e5cb 46 currentState = EMG_CLBRT;
arnouddomhof 3:dca57056e5cb 47 break;
arnouddomhof 3:dca57056e5cb 48 case EMG_CLBRT:
arnouddomhof 3:dca57056e5cb 49 pc.printf("EMG calibration \r\n");
arnouddomhof 3:dca57056e5cb 50 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led BLUE
arnouddomhof 3:dca57056e5cb 51 wait(5.0f); // time_in_this_state > 5.0f
arnouddomhof 3:dca57056e5cb 52 // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
arnouddomhof 3:dca57056e5cb 53 currentState = HOMING;
arnouddomhof 3:dca57056e5cb 54 break;
arnouddomhof 3:dca57056e5cb 55 case HOMING:
arnouddomhof 3:dca57056e5cb 56 pc.printf("HOMING, moving to the home starting configuration. \r\n");
arnouddomhof 3:dca57056e5cb 57 led_red = 0; led_green = 1; led_red = 0; // Colouring the led PURPLE
arnouddomhof 3:dca57056e5cb 58 wait(5.0f); // time_in_this_state > 5.0f
arnouddomhof 3:dca57056e5cb 59 // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
arnouddomhof 3:dca57056e5cb 60 currentState = WAITING4SIGNAL;
arnouddomhof 3:dca57056e5cb 61 break;
arnouddomhof 3:dca57056e5cb 62 case WAITING4SIGNAL:
arnouddomhof 3:dca57056e5cb 63 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
arnouddomhof 3:dca57056e5cb 64 pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate or Press s to shut the robot down. \r\n");
arnouddomhof 3:dca57056e5cb 65 c = pc.getc();
arnouddomhof 3:dca57056e5cb 66 if (c == 'd') {
arnouddomhof 3:dca57056e5cb 67 currentState = MOVE_W_DEMO;
arnouddomhof 3:dca57056e5cb 68 }
arnouddomhof 3:dca57056e5cb 69 else if (c == 'e') {
arnouddomhof 3:dca57056e5cb 70 currentState = MOVE_W_EMG;
arnouddomhof 3:dca57056e5cb 71 }
arnouddomhof 3:dca57056e5cb 72 else if (c == 'c') {
arnouddomhof 3:dca57056e5cb 73 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 3:dca57056e5cb 74 }
arnouddomhof 3:dca57056e5cb 75 else if (c == 's') {
arnouddomhof 3:dca57056e5cb 76 currentState = SHUTDOWN;
arnouddomhof 3:dca57056e5cb 77 }
arnouddomhof 3:dca57056e5cb 78 break;
arnouddomhof 3:dca57056e5cb 79 case MOVE_W_DEMO:
arnouddomhof 3:dca57056e5cb 80 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led GREEN
arnouddomhof 3:dca57056e5cb 81 pc.printf("MOVE_W_DEMO, Running the demo \r\n");
arnouddomhof 3:dca57056e5cb 82 c = pc.getcNb();
arnouddomhof 3:dca57056e5cb 83 if (c == 'h') {
arnouddomhof 3:dca57056e5cb 84 currentState = HOMING;
arnouddomhof 3:dca57056e5cb 85 }
arnouddomhof 3:dca57056e5cb 86 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 87 wait(1.0f);
arnouddomhof 3:dca57056e5cb 88 break;
arnouddomhof 3:dca57056e5cb 89 case MOVE_W_EMG:
arnouddomhof 3:dca57056e5cb 90 led_red = 1; led_green = 0; led_blue = 1; // Colouring the led GREEN
arnouddomhof 3:dca57056e5cb 91 pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
arnouddomhof 3:dca57056e5cb 92 wait(1);
arnouddomhof 3:dca57056e5cb 93 c = pc.getcNb();
arnouddomhof 3:dca57056e5cb 94 if (c == 'h') {
arnouddomhof 3:dca57056e5cb 95 currentState = HOMING;
arnouddomhof 3:dca57056e5cb 96 }
arnouddomhof 3:dca57056e5cb 97 Fail_button.fall(&Failing);
arnouddomhof 3:dca57056e5cb 98 wait(1.0f);
arnouddomhof 3:dca57056e5cb 99 break;
arnouddomhof 3:dca57056e5cb 100 case FAILURE_MODE:
arnouddomhof 3:dca57056e5cb 101 led_red = 0; led_green = 1; led_blue = 1; // Colouring the led RED
arnouddomhof 3:dca57056e5cb 102 pc.printf("FAILURE MODE \r\n");
arnouddomhof 3:dca57056e5cb 103 wait(2.0f);
arnouddomhof 3:dca57056e5cb 104 break;
arnouddomhof 3:dca57056e5cb 105 case SHUTDOWN:
arnouddomhof 3:dca57056e5cb 106 pc.printf("Shutting down \r\n");
arnouddomhof 3:dca57056e5cb 107 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
arnouddomhof 3:dca57056e5cb 108 wait(1.0f);
arnouddomhof 3:dca57056e5cb 109 for (int i = 0; i < 4; i++) { // Blinking WHITE 3x before turning BLACK
arnouddomhof 3:dca57056e5cb 110 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 111 led_green = !led_green;
arnouddomhof 3:dca57056e5cb 112 led_blue = !led_blue;
arnouddomhof 3:dca57056e5cb 113 wait(0.6f);
arnouddomhof 3:dca57056e5cb 114 }
arnouddomhof 3:dca57056e5cb 115 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 116 led_green = !led_green;
arnouddomhof 3:dca57056e5cb 117 led_blue = !led_blue;
arnouddomhof 3:dca57056e5cb 118 wait(1.0f);
arnouddomhof 3:dca57056e5cb 119 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 120 led_green = !led_green;
arnouddomhof 3:dca57056e5cb 121 led_blue = !led_blue;
arnouddomhof 3:dca57056e5cb 122 while (true) {
arnouddomhof 3:dca57056e5cb 123 }
arnouddomhof 3:dca57056e5cb 124
arnouddomhof 3:dca57056e5cb 125
arnouddomhof 3:dca57056e5cb 126 default:
arnouddomhof 3:dca57056e5cb 127 pc.printf("Unknown or unimplemented state reached!!! \n\r");
arnouddomhof 3:dca57056e5cb 128 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
arnouddomhof 3:dca57056e5cb 129 for (int n = 0; n < 50; n++) { // Making an SOS signal with the RED led
arnouddomhof 3:dca57056e5cb 130 for (int i = 0; i < 6; i++) {
arnouddomhof 3:dca57056e5cb 131 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 132 wait(0.6f);
arnouddomhof 3:dca57056e5cb 133 }
arnouddomhof 3:dca57056e5cb 134 wait(0.4f);
arnouddomhof 3:dca57056e5cb 135 for (int i = 0 ; i < 6; i++) {
arnouddomhof 3:dca57056e5cb 136 led_red = !led_red;
arnouddomhof 3:dca57056e5cb 137 wait(0.2f);
arnouddomhof 3:dca57056e5cb 138 }
arnouddomhof 3:dca57056e5cb 139 wait(0.4f);
arnouddomhof 3:dca57056e5cb 140 }
arnouddomhof 3:dca57056e5cb 141 } // end of switch
Mirjam 0:46dbc9b620d8 142 }
arnouddomhof 3:dca57056e5cb 143 }
arnouddomhof 3:dca57056e5cb 144