Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

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 biquadFilter MODSERIAL FastPWM

main.cpp@18:4a6be1893d7f, 2019-10-29 (annotated)

Committer:
Floris_Hoek
Date:
Tue Oct 29 11:17:48 2019 +0000
Revision:
18:4a6be1893d7f
Parent:
15:5f9450964075
Child:
19:b3e224e0cb85
busy with operating mode, but it is working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Floris_Hoek 12:f4331640e3ad 1 // MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
Floris_Hoek 12:f4331640e3ad 2 // reference velocity has to be fixed? idk? --> wait for file from bram en paul
Floris_Hoek 18:4a6be1893d7f 3 // Coefficients for the filters have to be adjusted --> Is 1 filter enough?
Floris_Hoek 12:f4331640e3ad 4
RobertoO 0:67c50348f842 5 #include "mbed.h"
Floris_Hoek 8:7dab565a208e 6 #include "HIDScope.h"
Floris_Hoek 8:7dab565a208e 7 #include "BiQuad.h"
RobertoO 1:b862262a9d14 8 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 9 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 10 #include "QEI.h"
Floris_Hoek 12:f4331640e3ad 11
paulstuiver 5:2ae500da8fe1 12 #include <math.h>
Floris_Hoek 12:f4331640e3ad 13 #include <deque>
paulstuiver 2:75b2f713161c 14
Floris_Hoek 8:7dab565a208e 15 #include "Motor_Control.h"
Floris_Hoek 8:7dab565a208e 16
Floris_Hoek 12:f4331640e3ad 17 DigitalIn button1(D12); // Button1 input to go to next state
Floris_Hoek 12:f4331640e3ad 18 InterruptIn button2(SW2); // Button2 input to activate failuremode()
Floris_Hoek 12:f4331640e3ad 19 DigitalOut ledr(LED_RED); // Red LED output to show
Floris_Hoek 9:e8cc37a94fec 20
Floris_Hoek 18:4a6be1893d7f 21 AnalogIn S0(A0), S1(A1), S2(A2) ,S3(A3); // Input EMG Shield 0,1,2,3
Floris_Hoek 18:4a6be1893d7f 22 DigitalOut MD0(D1), MD1(D2), MD2(D3); // MotorDirection of motors 0,1,2
Floris_Hoek 18:4a6be1893d7f 23 FastPWM MV0(D4), MV1(D5), MV2(D6); // MotorVelocities of motors 0,1,2
Floris_Hoek 18:4a6be1893d7f 24 QEI E0(D7,D8,NC,8400), E1(D9,D10,NC,8400), E3(D11,D13,NC,8400); // Encoders of motors 0,1,2
Floris_Hoek 9:e8cc37a94fec 25
Floris_Hoek 12:f4331640e3ad 26 Ticker measurecontrol; // Ticker function for motor in- and output
Floris_Hoek 18:4a6be1893d7f 27
Floris_Hoek 18:4a6be1893d7f 28 // Make arrays for the different variables for the motors
Floris_Hoek 18:4a6be1893d7f 29 AnalogIn Shields[4] = {S0, S1, S2, S3};
Floris_Hoek 18:4a6be1893d7f 30 DigitalOut MotorDirections[3] = {MD0, MD1, MD2};
Floris_Hoek 18:4a6be1893d7f 31 FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
Floris_Hoek 18:4a6be1893d7f 32 QEI Encoders[3] = {E0, E1, E3};
Floris_Hoek 18:4a6be1893d7f 33
Floris_Hoek 8:7dab565a208e 34 MODSERIAL pc(USBTX, USBRX);
Floris_Hoek 8:7dab565a208e 35
Floris_Hoek 10:8c38a1a5b522 36 float PI = 3.1415926f;//535897932384626433832795028841971693993;
Floris_Hoek 12:f4331640e3ad 37 float timeinterval = 0.001f; // Time interval of the Ticker function
Floris_Hoek 9:e8cc37a94fec 38 bool whileloop = true; // Statement to keep the whileloop in the main function running
Floris_Hoek 9:e8cc37a94fec 39 // While loop has to stop running when failuremode is activated
Floris_Hoek 9:e8cc37a94fec 40
Floris_Hoek 9:e8cc37a94fec 41 // Define the different states in which the robot can be
Floris_Hoek 9:e8cc37a94fec 42 enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
Floris_Hoek 18:4a6be1893d7f 43 START_GAME, DEMO_MODE, OPERATING_MODE, MOVE_END_EFFECTOR,
Floris_Hoek 9:e8cc37a94fec 44 MOVE_GRIPPER, END_GAME, FAILURE_MODE};
Floris_Hoek 9:e8cc37a94fec 45
Floris_Hoek 9:e8cc37a94fec 46 // Default state is the state in which the motors are turned off
Floris_Hoek 9:e8cc37a94fec 47 States MyState = MOTORS_OFF;
paulstuiver 5:2ae500da8fe1 48
Floris_Hoek 14:1343966a79e8 49 // Initialise the functions
Floris_Hoek 9:e8cc37a94fec 50
Floris_Hoek 14:1343966a79e8 51 void motorsoff();
Floris_Hoek 14:1343966a79e8 52
Floris_Hoek 18:4a6be1893d7f 53 void measure_data(double &rms_0, double &rms_1, double &rms_2, double &rms_3);
Floris_Hoek 14:1343966a79e8 54
Floris_Hoek 18:4a6be1893d7f 55 void det_max(double y, double &max_y);
Floris_Hoek 14:1343966a79e8 56
Floris_Hoek 14:1343966a79e8 57 void emgcalibration();
Floris_Hoek 14:1343966a79e8 58
Floris_Hoek 14:1343966a79e8 59 double P_controller(double error);
Floris_Hoek 14:1343966a79e8 60
Floris_Hoek 14:1343966a79e8 61 void nothing(){
Floris_Hoek 14:1343966a79e8 62 // Do nothing
Floris_Hoek 14:1343966a79e8 63 }
Floris_Hoek 14:1343966a79e8 64
Floris_Hoek 18:4a6be1893d7f 65 void get_input(char c);
Floris_Hoek 18:4a6be1893d7f 66
Floris_Hoek 18:4a6be1893d7f 67 void startgame() ;
Floris_Hoek 18:4a6be1893d7f 68
Floris_Hoek 18:4a6be1893d7f 69 void operating_mode();
Floris_Hoek 18:4a6be1893d7f 70
Floris_Hoek 14:1343966a79e8 71 void New_State();
Floris_Hoek 14:1343966a79e8 72
Floris_Hoek 14:1343966a79e8 73 void failuremode();
Floris_Hoek 14:1343966a79e8 74
Floris_Hoek 14:1343966a79e8 75
Floris_Hoek 14:1343966a79e8 76
Floris_Hoek 14:1343966a79e8 77
Floris_Hoek 18:4a6be1893d7f 78 //__________________________________________________________________________________________________________________________________
Floris_Hoek 18:4a6be1893d7f 79 //__________________________________________________________________________________________________________________________________
Floris_Hoek 14:1343966a79e8 80 int main()
Floris_Hoek 14:1343966a79e8 81 {
Floris_Hoek 14:1343966a79e8 82 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 18:4a6be1893d7f 83 double frequency = 17000.0; // Set motorfrequency
Floris_Hoek 18:4a6be1893d7f 84 double period_signal = 1.0/frequency; // Convert to period of the signal
Floris_Hoek 14:1343966a79e8 85 pc.baud(115200);
Floris_Hoek 14:1343966a79e8 86
Floris_Hoek 14:1343966a79e8 87 button2.fall(failuremode); // Function is always activated when the button is pressed
Floris_Hoek 18:4a6be1893d7f 88
Floris_Hoek 18:4a6be1893d7f 89 for (int i = 0; i < 3; i++) {
Floris_Hoek 18:4a6be1893d7f 90 MotorVelocities[i].period(period_signal); // Set the period of the PWMfunction
Floris_Hoek 18:4a6be1893d7f 91 }
Floris_Hoek 14:1343966a79e8 92 measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
Floris_Hoek 14:1343966a79e8 93
Floris_Hoek 14:1343966a79e8 94 int previous_state_int = (int)MyState; // Save previous state to compare with current state and possibly execute New_State()
Floris_Hoek 14:1343966a79e8 95 // in the while loop
Floris_Hoek 14:1343966a79e8 96 New_State(); // Execute the functions belonging to the current state
Floris_Hoek 14:1343966a79e8 97
Floris_Hoek 14:1343966a79e8 98 while(whileloop)
Floris_Hoek 14:1343966a79e8 99 {
Floris_Hoek 14:1343966a79e8 100 if ( (previous_state_int - (int)MyState) != 0 ) { // If current state is not previous state execute New_State()
Floris_Hoek 14:1343966a79e8 101 New_State();
Floris_Hoek 14:1343966a79e8 102 }
Floris_Hoek 14:1343966a79e8 103 previous_state_int = (int)MyState; // Change previous state to current state
Floris_Hoek 14:1343966a79e8 104 }
Floris_Hoek 14:1343966a79e8 105 // While has stopped running
Floris_Hoek 14:1343966a79e8 106 pc.printf("Programm stops running\r\n"); // So show that the programm is quiting
Floris_Hoek 18:4a6be1893d7f 107 sendtomotor(0.0); // Set the motor velocity to 0
Floris_Hoek 14:1343966a79e8 108 measurecontrol.attach(nothing,10000); // Attach empty function to Ticker (?? Appropriate solution ??)
Floris_Hoek 14:1343966a79e8 109 return 0;
Floris_Hoek 14:1343966a79e8 110 }
Floris_Hoek 18:4a6be1893d7f 111 //__________________________________________________________________________________________________________________________________
Floris_Hoek 18:4a6be1893d7f 112 //__________________________________________________________________________________________________________________________________
Floris_Hoek 9:e8cc37a94fec 113
Floris_Hoek 9:e8cc37a94fec 114 void motorsoff() {
Floris_Hoek 12:f4331640e3ad 115 // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed.
Floris_Hoek 12:f4331640e3ad 116 // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
Floris_Hoek 12:f4331640e3ad 117
Floris_Hoek 12:f4331640e3ad 118 bool whileloop_boolean = true; // Boolean for the while loop
Floris_Hoek 18:4a6be1893d7f 119 sendtomotor(0.0); // Set motor velocity to 0
Floris_Hoek 12:f4331640e3ad 120
Floris_Hoek 12:f4331640e3ad 121 while (whileloop_boolean) {
Floris_Hoek 12:f4331640e3ad 122 if (button1.read() == 0) { // If button1 is pressed:
Floris_Hoek 12:f4331640e3ad 123 MyState = EMG_CALIBRATION; // set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 12:f4331640e3ad 124 whileloop_boolean = false; // by making whileloop_boolean equal to false
Floris_Hoek 9:e8cc37a94fec 125 }
Floris_Hoek 9:e8cc37a94fec 126 }
Floris_Hoek 9:e8cc37a94fec 127 }
Floris_Hoek 12:f4331640e3ad 128
Floris_Hoek 18:4a6be1893d7f 129 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3) {
Floris_Hoek 18:4a6be1893d7f 130 // High pass
Floris_Hoek 18:4a6be1893d7f 131 double hb0 = 0.9169; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 132 double hb1 = -1.8338; //
Floris_Hoek 18:4a6be1893d7f 133 double hb2 = 0.9169; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 134 double ha0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 135 double ha1 = -1.8268; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 136 double ha2 = 0.8407; //
Floris_Hoek 12:f4331640e3ad 137
Floris_Hoek 18:4a6be1893d7f 138 // Low pass
Floris_Hoek 18:4a6be1893d7f 139 double lb0 = 0.000083621; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 140 double lb1 = 0.0006724; //
Floris_Hoek 18:4a6be1893d7f 141 double lb2 = 0.000083621; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 142 double la0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 143 double la1 = -1.9740; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 144 double la2 = 0.9743; //
Floris_Hoek 14:1343966a79e8 145
Floris_Hoek 18:4a6be1893d7f 146 static double max_y0 = 0;
Floris_Hoek 18:4a6be1893d7f 147 static double max_y1 = 0;
Floris_Hoek 18:4a6be1893d7f 148 static double max_y2 = 0;
Floris_Hoek 18:4a6be1893d7f 149 static double max_y3 = 0;
Floris_Hoek 12:f4331640e3ad 150
Floris_Hoek 18:4a6be1893d7f 151 static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 152 static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 153 static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 154 static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 12:f4331640e3ad 155
Floris_Hoek 18:4a6be1893d7f 156 static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 157 static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 158 static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 159 static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 12:f4331640e3ad 160
Floris_Hoek 18:4a6be1893d7f 161 f_y0 = hFilter0.step(S0); // Apply filters on the different EMG signals
Floris_Hoek 18:4a6be1893d7f 162 f_y1 = hFilter1.step(S1);
Floris_Hoek 18:4a6be1893d7f 163 f_y2 = hFilter2.step(S2);
Floris_Hoek 18:4a6be1893d7f 164 f_y3 = hFilter3.step(S3);
Floris_Hoek 12:f4331640e3ad 165
Floris_Hoek 18:4a6be1893d7f 166 f_y0 = lFilter0.step(f_y0);
Floris_Hoek 18:4a6be1893d7f 167 f_y1 = lFilter1.step(f_y1);
Floris_Hoek 18:4a6be1893d7f 168 f_y2 = lFilter2.step(f_y2);
Floris_Hoek 18:4a6be1893d7f 169 f_y3 = lFilter3.step(f_y3);
Floris_Hoek 18:4a6be1893d7f 170
Floris_Hoek 18:4a6be1893d7f 171
Floris_Hoek 15:5f9450964075 172 if (MyState == EMG_CALIBRATION) {
Floris_Hoek 14:1343966a79e8 173
Floris_Hoek 18:4a6be1893d7f 174 det_max(f_y0, max_y0); // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
Floris_Hoek 18:4a6be1893d7f 175 det_max(f_y1, max_y1);
Floris_Hoek 18:4a6be1893d7f 176 det_max(f_y2, max_y2);
Floris_Hoek 18:4a6be1893d7f 177 det_max(f_y3, max_y3);
Floris_Hoek 14:1343966a79e8 178
Floris_Hoek 14:1343966a79e8 179 }
Floris_Hoek 15:5f9450964075 180 else if ((int)MyState > 4) {
Floris_Hoek 18:4a6be1893d7f 181 f_y0 = f_y0/max_y0; // Normalise the RMS value by dividing by the maximum RMS value
Floris_Hoek 18:4a6be1893d7f 182 f_y1 = f_y1/max_y1; // This is done during the states with a value higher than 4, as this is when you start the operating mode
Floris_Hoek 18:4a6be1893d7f 183 f_y2 = f_y2/max_y2;
Floris_Hoek 18:4a6be1893d7f 184 f_y3 = f_y3/max_y3;
Floris_Hoek 14:1343966a79e8 185 }
Floris_Hoek 12:f4331640e3ad 186
Floris_Hoek 10:8c38a1a5b522 187 }
Floris_Hoek 12:f4331640e3ad 188
Floris_Hoek 18:4a6be1893d7f 189 void det_max(double y, double &max_y) {
Floris_Hoek 18:4a6be1893d7f 190 max_y = max_y < y ? y : max_y; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
Floris_Hoek 14:1343966a79e8 191 }
Floris_Hoek 12:f4331640e3ad 192
Floris_Hoek 14:1343966a79e8 193 void emgcalibration() {
Floris_Hoek 18:4a6be1893d7f 194 double y0, y1, y2, y3; // RMS values of the different EMG signals
Floris_Hoek 14:1343966a79e8 195
Floris_Hoek 18:4a6be1893d7f 196 measure_data(y0, y1, y2, y3); // Calculate RMS values
Floris_Hoek 14:1343966a79e8 197
Floris_Hoek 18:4a6be1893d7f 198 double duration = 10.0; // Duration of the emgcalibration function, in this case 10 seconds
Floris_Hoek 14:1343966a79e8 199 int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
Floris_Hoek 14:1343966a79e8 200 // rounds is an integer so the value of duration / timeinterval is floored
Floris_Hoek 14:1343966a79e8 201
Floris_Hoek 14:1343966a79e8 202 static int counter = 0; // Counter which keeps track of the amount of times the function has executed
Floris_Hoek 14:1343966a79e8 203 if (counter >= rounds) {
Floris_Hoek 14:1343966a79e8 204 MyState = MOTOR_CALIBRATION; // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
Floris_Hoek 14:1343966a79e8 205 }
Floris_Hoek 14:1343966a79e8 206 else {
Floris_Hoek 14:1343966a79e8 207 counter++; // Else increase counter by 1
Floris_Hoek 14:1343966a79e8 208 }
Floris_Hoek 14:1343966a79e8 209
Floris_Hoek 14:1343966a79e8 210 }
Floris_Hoek 12:f4331640e3ad 211
paulstuiver 5:2ae500da8fe1 212 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 213 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 214 {
Floris_Hoek 8:7dab565a208e 215 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 216 //Proportional part:
paulstuiver 5:2ae500da8fe1 217 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 218
paulstuiver 5:2ae500da8fe1 219 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 220 return u_k;
paulstuiver 5:2ae500da8fe1 221 }
paulstuiver 2:75b2f713161c 222
Floris_Hoek 18:4a6be1893d7f 223 void get_input(char c)
Floris_Hoek 18:4a6be1893d7f 224 {
Floris_Hoek 18:4a6be1893d7f 225 if (c == 'd') {
Floris_Hoek 18:4a6be1893d7f 226 MyState = DEMO_MODE;
Floris_Hoek 18:4a6be1893d7f 227 }
Floris_Hoek 18:4a6be1893d7f 228 else if (c == 'o') {
Floris_Hoek 18:4a6be1893d7f 229 MyState = OPERATING_MODE;
Floris_Hoek 18:4a6be1893d7f 230 }
Floris_Hoek 18:4a6be1893d7f 231 else {
Floris_Hoek 18:4a6be1893d7f 232 pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
Floris_Hoek 18:4a6be1893d7f 233 get_input(pc.getc());
Floris_Hoek 18:4a6be1893d7f 234 }
Floris_Hoek 18:4a6be1893d7f 235 }
Floris_Hoek 18:4a6be1893d7f 236
Floris_Hoek 18:4a6be1893d7f 237 void startgame() {
Floris_Hoek 18:4a6be1893d7f 238 pc.printf("Please choose which game you would like to start:\r\n");
Floris_Hoek 18:4a6be1893d7f 239
Floris_Hoek 18:4a6be1893d7f 240 pc.printf("- Press 'd' to start the demo mode\r\n Demo mode is a mode in which the different movements of the robot are shown.\r\n");
Floris_Hoek 18:4a6be1893d7f 241 pc.printf(" It will show the edges of the space that the end effector is allowed to go to and will also move in straight lines to show that the robot meets the requirements.\r\n");
Floris_Hoek 18:4a6be1893d7f 242 pc.printf(" It will also show how it is able to grab and lift objects which are on the board.\r\n\r\n");
Floris_Hoek 18:4a6be1893d7f 243
Floris_Hoek 18:4a6be1893d7f 244 pc.printf("- Press 'o' to start the operating mode\r\n The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
Floris_Hoek 18:4a6be1893d7f 245 pc.printf(" You will be able to move the arm and use the gripper to try and grab and lift objects from the board to the container.\r\n\r\n");
Floris_Hoek 18:4a6be1893d7f 246
Floris_Hoek 18:4a6be1893d7f 247 get_input(pc.getc());
Floris_Hoek 18:4a6be1893d7f 248 }
Floris_Hoek 18:4a6be1893d7f 249
Floris_Hoek 18:4a6be1893d7f 250 /*
Floris_Hoek 18:4a6be1893d7f 251 void demo_mode() {
Floris_Hoek 18:4a6be1893d7f 252
Floris_Hoek 18:4a6be1893d7f 253 }
Floris_Hoek 18:4a6be1893d7f 254 */
Floris_Hoek 18:4a6be1893d7f 255
Floris_Hoek 18:4a6be1893d7f 256 void operating_mode() {
Floris_Hoek 18:4a6be1893d7f 257 double y0,y1,y2,y3;
Floris_Hoek 18:4a6be1893d7f 258 measure_data(y0,y1,y2,y3);
Floris_Hoek 18:4a6be1893d7f 259 double F_Y[4] = {y0, y1, y2, y3};
Floris_Hoek 18:4a6be1893d7f 260
Floris_Hoek 18:4a6be1893d7f 261 double threshold = 0.5;
Floris_Hoek 18:4a6be1893d7f 262 for (int i = 0; i < 4; i++) {
Floris_Hoek 18:4a6be1893d7f 263 if (F_Y[i] > threshold) {
Floris_Hoek 18:4a6be1893d7f 264 //hey
Floris_Hoek 18:4a6be1893d7f 265 }
Floris_Hoek 18:4a6be1893d7f 266 }
Floris_Hoek 18:4a6be1893d7f 267
Floris_Hoek 18:4a6be1893d7f 268 }
Floris_Hoek 18:4a6be1893d7f 269
Floris_Hoek 9:e8cc37a94fec 270 void New_State() {
Floris_Hoek 9:e8cc37a94fec 271 switch (MyState)
Floris_Hoek 9:e8cc37a94fec 272 {
Floris_Hoek 9:e8cc37a94fec 273 case MOTORS_OFF :
Floris_Hoek 9:e8cc37a94fec 274 pc.printf("State: Motors turned off");
Floris_Hoek 9:e8cc37a94fec 275 motorsoff();
Floris_Hoek 9:e8cc37a94fec 276 break;
Floris_Hoek 9:e8cc37a94fec 277
Floris_Hoek 9:e8cc37a94fec 278 case EMG_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 279 pc.printf("State: EMG Calibration");
Floris_Hoek 14:1343966a79e8 280 measurecontrol.attach(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 281 break;
Floris_Hoek 9:e8cc37a94fec 282
Floris_Hoek 9:e8cc37a94fec 283 case MOTOR_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 284 pc.printf("State: Motor Calibration");
Floris_Hoek 9:e8cc37a94fec 285 break;
Floris_Hoek 9:e8cc37a94fec 286
Floris_Hoek 9:e8cc37a94fec 287 case START_GAME :
Floris_Hoek 9:e8cc37a94fec 288 pc.printf("State: Start game");
Floris_Hoek 18:4a6be1893d7f 289 startgame();
Floris_Hoek 9:e8cc37a94fec 290 break;
Floris_Hoek 9:e8cc37a94fec 291
Floris_Hoek 9:e8cc37a94fec 292 case DEMO_MODE :
Floris_Hoek 9:e8cc37a94fec 293 pc.printf("State: Demo mode");
Floris_Hoek 18:4a6be1893d7f 294 //demo_mode();
Floris_Hoek 9:e8cc37a94fec 295 break;
Floris_Hoek 9:e8cc37a94fec 296
Floris_Hoek 18:4a6be1893d7f 297 case OPERATING_MODE :
Floris_Hoek 18:4a6be1893d7f 298 pc.printf("State: Operating mode");
Floris_Hoek 18:4a6be1893d7f 299 measurecontrol.attach(operating_mode,timeinterval);
Floris_Hoek 9:e8cc37a94fec 300 break;
Floris_Hoek 9:e8cc37a94fec 301
Floris_Hoek 9:e8cc37a94fec 302 case MOVE_END_EFFECTOR :
Floris_Hoek 9:e8cc37a94fec 303 pc.printf("State: Move end effector");
Floris_Hoek 9:e8cc37a94fec 304 break;
Floris_Hoek 9:e8cc37a94fec 305
Floris_Hoek 9:e8cc37a94fec 306 case MOVE_GRIPPER :
Floris_Hoek 9:e8cc37a94fec 307 pc.printf("State: Move the gripper");
Floris_Hoek 9:e8cc37a94fec 308 break;
Floris_Hoek 9:e8cc37a94fec 309
Floris_Hoek 9:e8cc37a94fec 310 case END_GAME :
Floris_Hoek 9:e8cc37a94fec 311 pc.printf("State: End of the game");
Floris_Hoek 9:e8cc37a94fec 312 break;
Floris_Hoek 9:e8cc37a94fec 313
Floris_Hoek 9:e8cc37a94fec 314 case FAILURE_MODE :
Floris_Hoek 9:e8cc37a94fec 315 pc.printf("FAILURE MODE!!"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 316 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 317 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 318 break;
Floris_Hoek 9:e8cc37a94fec 319
Floris_Hoek 9:e8cc37a94fec 320 default :
Floris_Hoek 9:e8cc37a94fec 321 pc.printf("Default state: Motors are turned off");
Floris_Hoek 18:4a6be1893d7f 322 sendtomotor(0.0);
Floris_Hoek 9:e8cc37a94fec 323 break;
Floris_Hoek 9:e8cc37a94fec 324 }
Floris_Hoek 9:e8cc37a94fec 325 }
Floris_Hoek 9:e8cc37a94fec 326
Floris_Hoek 9:e8cc37a94fec 327 void failuremode() {
Floris_Hoek 12:f4331640e3ad 328 MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
Floris_Hoek 12:f4331640e3ad 329 New_State(); // Execute actions coupled to FAILURE_MODE
Floris_Hoek 14:1343966a79e8 330 }