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@22:cce4dc5738af, 2019-10-30 (annotated)

Committer:
Floris_Hoek
Date:
Wed Oct 30 11:34:47 2019 +0000
Revision:
22:cce4dc5738af
Parent:
19:b3e224e0cb85
Child:
23:ff73ee119244
no control and START_GAME is not 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
Floris_Hoek 19:b3e224e0cb85 12 #include <cmath>
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 22:cce4dc5738af 17 DigitalIn button1(D13); // Button1 input to go to next state
Floris_Hoek 12:f4331640e3ad 18 InterruptIn button2(SW2); // Button2 input to activate failuremode()
Floris_Hoek 19:b3e224e0cb85 19 InterruptIn button3(SW3); // Button3 input to go to a specific state
Floris_Hoek 22:cce4dc5738af 20 DigitalOut ledg(LED_GREEN); // to test stuff
Floris_Hoek 12:f4331640e3ad 21 DigitalOut ledr(LED_RED); // Red LED output to show
Floris_Hoek 9:e8cc37a94fec 22
Floris_Hoek 22:cce4dc5738af 23 AnalogIn S0(A0), S1(A1), S2(A2),S3(A3); // Input EMG Shield 0,1,2,3
Floris_Hoek 22:cce4dc5738af 24 DigitalOut MD0(D7), MD1(D4); // MotorDirection of motors 0,1,2
Floris_Hoek 22:cce4dc5738af 25 FastPWM MV0(D6), MV1(D5), MV2(D12); // MotorVelocities of motors 0,1,2
Floris_Hoek 22:cce4dc5738af 26 QEI E0(D8,D9,NC,8400), E1(D10,D11,NC,8400); // Encoders of motors 0,1,2
Floris_Hoek 9:e8cc37a94fec 27
Floris_Hoek 12:f4331640e3ad 28 Ticker measurecontrol; // Ticker function for motor in- and output
Floris_Hoek 18:4a6be1893d7f 29
Floris_Hoek 18:4a6be1893d7f 30 // Make arrays for the different variables for the motors
Floris_Hoek 18:4a6be1893d7f 31 AnalogIn Shields[4] = {S0, S1, S2, S3};
Floris_Hoek 22:cce4dc5738af 32 DigitalOut MotorDirections[2] = {MD0, MD1};
Floris_Hoek 18:4a6be1893d7f 33 FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
Floris_Hoek 22:cce4dc5738af 34 QEI Encoders[2] = {E0, E1};
Floris_Hoek 18:4a6be1893d7f 35
Floris_Hoek 22:cce4dc5738af 36 Serial pc(USBTX, USBRX);
Floris_Hoek 8:7dab565a208e 37
Floris_Hoek 10:8c38a1a5b522 38 float PI = 3.1415926f;//535897932384626433832795028841971693993;
Floris_Hoek 12:f4331640e3ad 39 float timeinterval = 0.001f; // Time interval of the Ticker function
Floris_Hoek 9:e8cc37a94fec 40 bool whileloop = true; // Statement to keep the whileloop in the main function running
Floris_Hoek 22:cce4dc5738af 41 // While loop has to stop running when failuremode is activated
Floris_Hoek 9:e8cc37a94fec 42
Floris_Hoek 9:e8cc37a94fec 43 // Define the different states in which the robot can be
Floris_Hoek 22:cce4dc5738af 44 enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION, START_GAME,
Floris_Hoek 22:cce4dc5738af 45 DEMO_MODE, OPERATING_MODE, MOVE_GRIPPER, END_GAME, FAILURE_MODE
Floris_Hoek 22:cce4dc5738af 46 };
Floris_Hoek 19:b3e224e0cb85 47
Floris_Hoek 9:e8cc37a94fec 48
Floris_Hoek 9:e8cc37a94fec 49 // Default state is the state in which the motors are turned off
Floris_Hoek 9:e8cc37a94fec 50 States MyState = MOTORS_OFF;
paulstuiver 5:2ae500da8fe1 51
Floris_Hoek 14:1343966a79e8 52 // Initialise the functions
Floris_Hoek 9:e8cc37a94fec 53
Floris_Hoek 14:1343966a79e8 54 void motorsoff();
Floris_Hoek 14:1343966a79e8 55
Floris_Hoek 18:4a6be1893d7f 56 void measure_data(double &rms_0, double &rms_1, double &rms_2, double &rms_3);
Floris_Hoek 14:1343966a79e8 57
Floris_Hoek 18:4a6be1893d7f 58 void det_max(double y, double &max_y);
Floris_Hoek 14:1343966a79e8 59
Floris_Hoek 14:1343966a79e8 60 void emgcalibration();
Floris_Hoek 14:1343966a79e8 61
Floris_Hoek 14:1343966a79e8 62 double P_controller(double error);
Floris_Hoek 14:1343966a79e8 63
Floris_Hoek 22:cce4dc5738af 64 void nothing()
Floris_Hoek 22:cce4dc5738af 65 {
Floris_Hoek 14:1343966a79e8 66 // Do nothing
Floris_Hoek 14:1343966a79e8 67 }
Floris_Hoek 14:1343966a79e8 68
Floris_Hoek 18:4a6be1893d7f 69 void get_input(char c);
Floris_Hoek 18:4a6be1893d7f 70
Floris_Hoek 18:4a6be1893d7f 71 void startgame() ;
Floris_Hoek 18:4a6be1893d7f 72
Floris_Hoek 18:4a6be1893d7f 73 void operating_mode();
Floris_Hoek 18:4a6be1893d7f 74
Floris_Hoek 14:1343966a79e8 75 void New_State();
Floris_Hoek 14:1343966a79e8 76
Floris_Hoek 19:b3e224e0cb85 77 void Set_State();
Floris_Hoek 19:b3e224e0cb85 78
Floris_Hoek 14:1343966a79e8 79 void failuremode();
Floris_Hoek 14:1343966a79e8 80
Floris_Hoek 14:1343966a79e8 81
Floris_Hoek 18:4a6be1893d7f 82 //__________________________________________________________________________________________________________________________________
Floris_Hoek 18:4a6be1893d7f 83 //__________________________________________________________________________________________________________________________________
Floris_Hoek 14:1343966a79e8 84 int main()
Floris_Hoek 14:1343966a79e8 85 {
Floris_Hoek 22:cce4dc5738af 86 pc.baud(115200);
Floris_Hoek 19:b3e224e0cb85 87 ledr = 1;
Floris_Hoek 22:cce4dc5738af 88 ledg = 1;
Floris_Hoek 22:cce4dc5738af 89
Floris_Hoek 14:1343966a79e8 90 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 18:4a6be1893d7f 91 double frequency = 17000.0; // Set motorfrequency
Floris_Hoek 18:4a6be1893d7f 92 double period_signal = 1.0/frequency; // Convert to period of the signal
Floris_Hoek 14:1343966a79e8 93 pc.baud(115200);
Floris_Hoek 22:cce4dc5738af 94
Floris_Hoek 14:1343966a79e8 95 button2.fall(failuremode); // Function is always activated when the button is pressed
Floris_Hoek 19:b3e224e0cb85 96 button3.fall(Set_State); // Function is always activated when the button is pressed
Floris_Hoek 22:cce4dc5738af 97
Floris_Hoek 18:4a6be1893d7f 98 for (int i = 0; i < 3; i++) {
Floris_Hoek 18:4a6be1893d7f 99 MotorVelocities[i].period(period_signal); // Set the period of the PWMfunction
Floris_Hoek 18:4a6be1893d7f 100 }
Floris_Hoek 22:cce4dc5738af 101
Floris_Hoek 14:1343966a79e8 102 measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
Floris_Hoek 22:cce4dc5738af 103
Floris_Hoek 14:1343966a79e8 104 int previous_state_int = (int)MyState; // Save previous state to compare with current state and possibly execute New_State()
Floris_Hoek 22:cce4dc5738af 105 // in the while loop
Floris_Hoek 22:cce4dc5738af 106
Floris_Hoek 14:1343966a79e8 107 New_State(); // Execute the functions belonging to the current state
Floris_Hoek 22:cce4dc5738af 108
Floris_Hoek 22:cce4dc5738af 109 while(whileloop) {
Floris_Hoek 14:1343966a79e8 110 if ( (previous_state_int - (int)MyState) != 0 ) { // If current state is not previous state execute New_State()
Floris_Hoek 14:1343966a79e8 111 New_State();
Floris_Hoek 14:1343966a79e8 112 }
Floris_Hoek 14:1343966a79e8 113 previous_state_int = (int)MyState; // Change previous state to current state
Floris_Hoek 14:1343966a79e8 114 }
Floris_Hoek 14:1343966a79e8 115 // While has stopped running
Floris_Hoek 14:1343966a79e8 116 pc.printf("Programm stops running\r\n"); // So show that the programm is quiting
Floris_Hoek 18:4a6be1893d7f 117 sendtomotor(0.0); // Set the motor velocity to 0
Floris_Hoek 14:1343966a79e8 118 measurecontrol.attach(nothing,10000); // Attach empty function to Ticker (?? Appropriate solution ??)
Floris_Hoek 22:cce4dc5738af 119
Floris_Hoek 14:1343966a79e8 120 return 0;
Floris_Hoek 14:1343966a79e8 121 }
Floris_Hoek 18:4a6be1893d7f 122 //__________________________________________________________________________________________________________________________________
Floris_Hoek 18:4a6be1893d7f 123 //__________________________________________________________________________________________________________________________________
Floris_Hoek 9:e8cc37a94fec 124
Floris_Hoek 22:cce4dc5738af 125 void motorsoff()
Floris_Hoek 22:cce4dc5738af 126 {
Floris_Hoek 22:cce4dc5738af 127 // 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 128 // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
Floris_Hoek 22:cce4dc5738af 129
Floris_Hoek 12:f4331640e3ad 130 bool whileloop_boolean = true; // Boolean for the while loop
Floris_Hoek 18:4a6be1893d7f 131 sendtomotor(0.0); // Set motor velocity to 0
Floris_Hoek 22:cce4dc5738af 132
Floris_Hoek 12:f4331640e3ad 133 while (whileloop_boolean) {
Floris_Hoek 12:f4331640e3ad 134 if (button1.read() == 0) { // If button1 is pressed:
Floris_Hoek 12:f4331640e3ad 135 MyState = EMG_CALIBRATION; // set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 12:f4331640e3ad 136 whileloop_boolean = false; // by making whileloop_boolean equal to false
Floris_Hoek 22:cce4dc5738af 137 } else if (MyState != MOTORS_OFF) { // If the state is changed by keyboard input, exit the while loop
Floris_Hoek 22:cce4dc5738af 138 whileloop_boolean = false;
Floris_Hoek 9:e8cc37a94fec 139 }
Floris_Hoek 9:e8cc37a94fec 140 }
Floris_Hoek 9:e8cc37a94fec 141 }
Floris_Hoek 12:f4331640e3ad 142
Floris_Hoek 22:cce4dc5738af 143 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3)
Floris_Hoek 22:cce4dc5738af 144 {
Floris_Hoek 18:4a6be1893d7f 145 // High pass
Floris_Hoek 18:4a6be1893d7f 146 double hb0 = 0.9169; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 147 double hb1 = -1.8338; //
Floris_Hoek 18:4a6be1893d7f 148 double hb2 = 0.9169; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 149 double ha0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 150 double ha1 = -1.8268; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 151 double ha2 = 0.8407; //
Floris_Hoek 22:cce4dc5738af 152
Floris_Hoek 18:4a6be1893d7f 153 // Low pass
Floris_Hoek 18:4a6be1893d7f 154 double lb0 = 0.000083621; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 155 double lb1 = 0.0006724; //
Floris_Hoek 18:4a6be1893d7f 156 double lb2 = 0.000083621; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 157 double la0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 158 double la1 = -1.9740; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 159 double la2 = 0.9743; //
Floris_Hoek 22:cce4dc5738af 160
Floris_Hoek 19:b3e224e0cb85 161 static double max_y0 = 1;
Floris_Hoek 19:b3e224e0cb85 162 static double max_y1 = 1;
Floris_Hoek 19:b3e224e0cb85 163 static double max_y2 = 1;
Floris_Hoek 19:b3e224e0cb85 164 static double max_y3 = 1;
Floris_Hoek 22:cce4dc5738af 165
Floris_Hoek 18:4a6be1893d7f 166 static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 167 static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 168 static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 169 static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 22:cce4dc5738af 170
Floris_Hoek 18:4a6be1893d7f 171 static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 172 static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 173 static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 174 static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 22:cce4dc5738af 175
Floris_Hoek 18:4a6be1893d7f 176 f_y0 = hFilter0.step(S0); // Apply filters on the different EMG signals
Floris_Hoek 18:4a6be1893d7f 177 f_y1 = hFilter1.step(S1);
Floris_Hoek 18:4a6be1893d7f 178 f_y2 = hFilter2.step(S2);
Floris_Hoek 18:4a6be1893d7f 179 f_y3 = hFilter3.step(S3);
Floris_Hoek 22:cce4dc5738af 180
Floris_Hoek 19:b3e224e0cb85 181 f_y0 = abs(f_y0);
Floris_Hoek 19:b3e224e0cb85 182 f_y1 = abs(f_y1);
Floris_Hoek 19:b3e224e0cb85 183 f_y2 = abs(f_y2);
Floris_Hoek 19:b3e224e0cb85 184 f_y3 = abs(f_y3);
Floris_Hoek 22:cce4dc5738af 185
Floris_Hoek 18:4a6be1893d7f 186 f_y0 = lFilter0.step(f_y0);
Floris_Hoek 18:4a6be1893d7f 187 f_y1 = lFilter1.step(f_y1);
Floris_Hoek 18:4a6be1893d7f 188 f_y2 = lFilter2.step(f_y2);
Floris_Hoek 18:4a6be1893d7f 189 f_y3 = lFilter3.step(f_y3);
Floris_Hoek 18:4a6be1893d7f 190
Floris_Hoek 18:4a6be1893d7f 191
Floris_Hoek 15:5f9450964075 192 if (MyState == EMG_CALIBRATION) {
Floris_Hoek 22:cce4dc5738af 193
Floris_Hoek 18:4a6be1893d7f 194 det_max(f_y0, max_y0); // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
Floris_Hoek 18:4a6be1893d7f 195 det_max(f_y1, max_y1);
Floris_Hoek 18:4a6be1893d7f 196 det_max(f_y2, max_y2);
Floris_Hoek 18:4a6be1893d7f 197 det_max(f_y3, max_y3);
Floris_Hoek 22:cce4dc5738af 198
Floris_Hoek 22:cce4dc5738af 199 } else if ((int)MyState > 4) {
Floris_Hoek 18:4a6be1893d7f 200 f_y0 = f_y0/max_y0; // Normalise the RMS value by dividing by the maximum RMS value
Floris_Hoek 18:4a6be1893d7f 201 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 202 f_y2 = f_y2/max_y2;
Floris_Hoek 18:4a6be1893d7f 203 f_y3 = f_y3/max_y3;
Floris_Hoek 14:1343966a79e8 204 }
Floris_Hoek 22:cce4dc5738af 205
Floris_Hoek 10:8c38a1a5b522 206 }
Floris_Hoek 12:f4331640e3ad 207
Floris_Hoek 22:cce4dc5738af 208 void det_max(double y, double &max_y)
Floris_Hoek 22:cce4dc5738af 209 {
Floris_Hoek 18:4a6be1893d7f 210 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 211 }
Floris_Hoek 12:f4331640e3ad 212
Floris_Hoek 22:cce4dc5738af 213 void emgcalibration()
Floris_Hoek 22:cce4dc5738af 214 {
Floris_Hoek 18:4a6be1893d7f 215 double y0, y1, y2, y3; // RMS values of the different EMG signals
Floris_Hoek 14:1343966a79e8 216
Floris_Hoek 18:4a6be1893d7f 217 measure_data(y0, y1, y2, y3); // Calculate RMS values
Floris_Hoek 22:cce4dc5738af 218
Floris_Hoek 19:b3e224e0cb85 219 double duration = 20.0; // Duration of the emgcalibration function, in this case 10 seconds
Floris_Hoek 14:1343966a79e8 220 int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
Floris_Hoek 14:1343966a79e8 221 // rounds is an integer so the value of duration / timeinterval is floored
Floris_Hoek 22:cce4dc5738af 222
Floris_Hoek 14:1343966a79e8 223 static int counter = 0; // Counter which keeps track of the amount of times the function has executed
Floris_Hoek 14:1343966a79e8 224 if (counter >= rounds) {
Floris_Hoek 14:1343966a79e8 225 MyState = MOTOR_CALIBRATION; // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
Floris_Hoek 22:cce4dc5738af 226 } else {
Floris_Hoek 14:1343966a79e8 227 counter++; // Else increase counter by 1
Floris_Hoek 14:1343966a79e8 228 }
Floris_Hoek 22:cce4dc5738af 229
Floris_Hoek 14:1343966a79e8 230 }
Floris_Hoek 12:f4331640e3ad 231
paulstuiver 5:2ae500da8fe1 232 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 233 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 234 {
Floris_Hoek 8:7dab565a208e 235 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 236 //Proportional part:
paulstuiver 5:2ae500da8fe1 237 double u_k = Kp * error;
Floris_Hoek 22:cce4dc5738af 238
paulstuiver 5:2ae500da8fe1 239 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 240 return u_k;
paulstuiver 5:2ae500da8fe1 241 }
Floris_Hoek 22:cce4dc5738af 242 /*
Floris_Hoek 18:4a6be1893d7f 243 void get_input(char c)
Floris_Hoek 18:4a6be1893d7f 244 {
Floris_Hoek 18:4a6be1893d7f 245 if (c == 'd') {
Floris_Hoek 18:4a6be1893d7f 246 MyState = DEMO_MODE;
Floris_Hoek 22:cce4dc5738af 247 } else if (c == 'o') {
Floris_Hoek 18:4a6be1893d7f 248 MyState = OPERATING_MODE;
Floris_Hoek 22:cce4dc5738af 249 } else {
Floris_Hoek 22:cce4dc5738af 250 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 22:cce4dc5738af 251 get_input(pc.getc());
Floris_Hoek 18:4a6be1893d7f 252 }
Floris_Hoek 18:4a6be1893d7f 253 }
Floris_Hoek 22:cce4dc5738af 254 */
Floris_Hoek 22:cce4dc5738af 255 void startgame()
Floris_Hoek 22:cce4dc5738af 256 {
Floris_Hoek 18:4a6be1893d7f 257 pc.printf("Please choose which game you would like to start:\r\n");
Floris_Hoek 18:4a6be1893d7f 258
Floris_Hoek 18:4a6be1893d7f 259 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 260 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 261 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 262
Floris_Hoek 18:4a6be1893d7f 263 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 264 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 265
Floris_Hoek 22:cce4dc5738af 266 while (MyState == START_GAME) {
Floris_Hoek 22:cce4dc5738af 267 char c = pc.getc();
Floris_Hoek 22:cce4dc5738af 268 if (c == 'd') {
Floris_Hoek 22:cce4dc5738af 269 MyState = DEMO_MODE;
Floris_Hoek 22:cce4dc5738af 270 } else if (c == 'o') {
Floris_Hoek 22:cce4dc5738af 271 MyState = OPERATING_MODE;
Floris_Hoek 22:cce4dc5738af 272 }
Floris_Hoek 22:cce4dc5738af 273 else {
Floris_Hoek 22:cce4dc5738af 274 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 22:cce4dc5738af 275 }
Floris_Hoek 22:cce4dc5738af 276 }
Floris_Hoek 18:4a6be1893d7f 277 }
Floris_Hoek 18:4a6be1893d7f 278
Floris_Hoek 18:4a6be1893d7f 279 /*
Floris_Hoek 18:4a6be1893d7f 280 void demo_mode() {
Floris_Hoek 22:cce4dc5738af 281
Floris_Hoek 18:4a6be1893d7f 282 }
Floris_Hoek 18:4a6be1893d7f 283 */
Floris_Hoek 18:4a6be1893d7f 284
Floris_Hoek 22:cce4dc5738af 285 void operating_mode()
Floris_Hoek 22:cce4dc5738af 286 {
Floris_Hoek 22:cce4dc5738af 287 double y0,y1,y2,y3;
Floris_Hoek 22:cce4dc5738af 288 measure_data(y0,y1,y2,y3);
Floris_Hoek 22:cce4dc5738af 289 double F_Y[4] = {y0, y1, y2, y3};
Floris_Hoek 22:cce4dc5738af 290
Floris_Hoek 22:cce4dc5738af 291 double threshold = 0.3;
Floris_Hoek 22:cce4dc5738af 292 for (int i = 0; i < 4; i++) {
Floris_Hoek 22:cce4dc5738af 293 if (F_Y[i] > threshold) {
Floris_Hoek 22:cce4dc5738af 294 //The direction and velocity of the motors are determind here
Floris_Hoek 22:cce4dc5738af 295 }
Floris_Hoek 22:cce4dc5738af 296 }
Floris_Hoek 22:cce4dc5738af 297
Floris_Hoek 18:4a6be1893d7f 298 }
Floris_Hoek 18:4a6be1893d7f 299
Floris_Hoek 22:cce4dc5738af 300 void New_State()
Floris_Hoek 22:cce4dc5738af 301 {
Floris_Hoek 22:cce4dc5738af 302 switch (MyState) {
Floris_Hoek 9:e8cc37a94fec 303 case MOTORS_OFF :
Floris_Hoek 22:cce4dc5738af 304 pc.printf("\r\nState: Motors turned off\r\n");
Floris_Hoek 9:e8cc37a94fec 305 motorsoff();
Floris_Hoek 9:e8cc37a94fec 306 break;
Floris_Hoek 22:cce4dc5738af 307
Floris_Hoek 9:e8cc37a94fec 308 case EMG_CALIBRATION :
Floris_Hoek 22:cce4dc5738af 309 pc.printf("\r\nState: EMG Calibration\r\n");
Floris_Hoek 22:cce4dc5738af 310 pc.printf("Emg calibration mode will run for 20 seconds. Try to flex the muscles to which the electrodes are attached as hard as possible during this time.\r\n");
Floris_Hoek 14:1343966a79e8 311 measurecontrol.attach(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 312 break;
Floris_Hoek 22:cce4dc5738af 313
Floris_Hoek 9:e8cc37a94fec 314 case MOTOR_CALIBRATION :
Floris_Hoek 22:cce4dc5738af 315 pc.printf("\r\nState: Motor Calibration\r\n");
Floris_Hoek 9:e8cc37a94fec 316 break;
Floris_Hoek 22:cce4dc5738af 317
Floris_Hoek 9:e8cc37a94fec 318 case START_GAME :
Floris_Hoek 22:cce4dc5738af 319 pc.printf("\r\nState: Start game\r\n");
Floris_Hoek 18:4a6be1893d7f 320 startgame();
Floris_Hoek 9:e8cc37a94fec 321 break;
Floris_Hoek 22:cce4dc5738af 322
Floris_Hoek 9:e8cc37a94fec 323 case DEMO_MODE :
Floris_Hoek 22:cce4dc5738af 324 pc.printf("\r\nState: Demo mode\r\n");
Floris_Hoek 18:4a6be1893d7f 325 //demo_mode();
Floris_Hoek 9:e8cc37a94fec 326 break;
Floris_Hoek 22:cce4dc5738af 327
Floris_Hoek 18:4a6be1893d7f 328 case OPERATING_MODE :
Floris_Hoek 22:cce4dc5738af 329 pc.printf("\r\nState: Operating mode\r\n");
Floris_Hoek 18:4a6be1893d7f 330 measurecontrol.attach(operating_mode,timeinterval);
Floris_Hoek 9:e8cc37a94fec 331 break;
Floris_Hoek 22:cce4dc5738af 332
Floris_Hoek 9:e8cc37a94fec 333 case MOVE_GRIPPER :
Floris_Hoek 22:cce4dc5738af 334 pc.printf("\r\nState: Move the gripper\r\n");
Floris_Hoek 9:e8cc37a94fec 335 break;
Floris_Hoek 22:cce4dc5738af 336
Floris_Hoek 9:e8cc37a94fec 337 case END_GAME :
Floris_Hoek 22:cce4dc5738af 338 pc.printf("\r\nState: End of the game\r\n");
Floris_Hoek 9:e8cc37a94fec 339 break;
Floris_Hoek 22:cce4dc5738af 340
Floris_Hoek 9:e8cc37a94fec 341 case FAILURE_MODE :
Floris_Hoek 22:cce4dc5738af 342 pc.printf("\r\nFAILURE MODE!!\r\n"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 343 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 344 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 345 break;
Floris_Hoek 22:cce4dc5738af 346
Floris_Hoek 9:e8cc37a94fec 347 default :
Floris_Hoek 22:cce4dc5738af 348 pc.printf("\r\nDefault state: Motors are turned off\r\n");
Floris_Hoek 22:cce4dc5738af 349 measurecontrol.attach(nothing,10000);
Floris_Hoek 18:4a6be1893d7f 350 sendtomotor(0.0);
Floris_Hoek 9:e8cc37a94fec 351 break;
Floris_Hoek 9:e8cc37a94fec 352 }
Floris_Hoek 9:e8cc37a94fec 353 }
Floris_Hoek 9:e8cc37a94fec 354
Floris_Hoek 22:cce4dc5738af 355 void Set_State()
Floris_Hoek 22:cce4dc5738af 356 {
Floris_Hoek 22:cce4dc5738af 357 if (MyState == FAILURE_MODE) {
Floris_Hoek 22:cce4dc5738af 358 pc.printf("\r\nNot possible to set state because robot is in FAILURE_MODE. Please restart robot.\r\n");
Floris_Hoek 22:cce4dc5738af 359 } else {
Floris_Hoek 22:cce4dc5738af 360 sendtomotor(0.0); // Stop the motors
Floris_Hoek 22:cce4dc5738af 361 measurecontrol.attach(nothing,10000); // Stop the ticker function from running
Floris_Hoek 19:b3e224e0cb85 362
Floris_Hoek 22:cce4dc5738af 363 pc.printf("\r\nPress number: | To go to state:");
Floris_Hoek 22:cce4dc5738af 364 pc.printf("\r\n (0) | MOTORS_OFF: Set motorspeed just in case to 0 and wait till button1 is pressed");
Floris_Hoek 22:cce4dc5738af 365 pc.printf("\r\n (1) | EMG_CALIBRATION: Calibrate the maximum of the emg signals and normalise the emg signals with these maxima");
Floris_Hoek 22:cce4dc5738af 366 pc.printf("\r\n (2) | MOTOR_CALIBRATION: Calibrate the motors to determine the (0,0) position.");
Floris_Hoek 22:cce4dc5738af 367 pc.printf("\r\n (3) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
Floris_Hoek 22:cce4dc5738af 368 pc.printf("\r\n (4) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
Floris_Hoek 22:cce4dc5738af 369 pc.printf("\r\n (5) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
Floris_Hoek 22:cce4dc5738af 370 pc.printf("\r\n (6) | MOVE_GRIPPER: Control the movement of the gripper");
Floris_Hoek 22:cce4dc5738af 371 pc.printf("\r\n (7) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
Floris_Hoek 22:cce4dc5738af 372 pc.printf("\r\n (8) | FAILURE_MODE: Everything is stopped and the whole programm stops running.\r\n");
Floris_Hoek 22:cce4dc5738af 373
Floris_Hoek 22:cce4dc5738af 374 wait(0.5);
Floris_Hoek 22:cce4dc5738af 375
Floris_Hoek 22:cce4dc5738af 376 char a = '0';
Floris_Hoek 22:cce4dc5738af 377 char b = '8';
Floris_Hoek 22:cce4dc5738af 378 bool boolean = true;
Floris_Hoek 22:cce4dc5738af 379
Floris_Hoek 22:cce4dc5738af 380 while (boolean) {
Floris_Hoek 22:cce4dc5738af 381 char c = pc.getc();
Floris_Hoek 22:cce4dc5738af 382
Floris_Hoek 22:cce4dc5738af 383 if (c >= a && c <= b) {
Floris_Hoek 22:cce4dc5738af 384 MyState = (States)(c-'0');
Floris_Hoek 22:cce4dc5738af 385 boolean = false;
Floris_Hoek 22:cce4dc5738af 386
Floris_Hoek 22:cce4dc5738af 387 } else {
Floris_Hoek 22:cce4dc5738af 388 pc.printf("\r\nPlease enter a number between 0 and 8\r\n");
Floris_Hoek 22:cce4dc5738af 389 }
Floris_Hoek 19:b3e224e0cb85 390 }
Floris_Hoek 19:b3e224e0cb85 391 }
Floris_Hoek 19:b3e224e0cb85 392 }
Floris_Hoek 19:b3e224e0cb85 393
Floris_Hoek 22:cce4dc5738af 394 void failuremode()
Floris_Hoek 22:cce4dc5738af 395 {
Floris_Hoek 12:f4331640e3ad 396 MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
Floris_Hoek 12:f4331640e3ad 397 New_State(); // Execute actions coupled to FAILURE_MODE
Floris_Hoek 14:1343966a79e8 398 }