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@19:b3e224e0cb85, 2019-10-29 (annotated)

Committer:
Floris_Hoek
Date:
Tue Oct 29 14:47:59 2019 +0000
Revision:
19:b3e224e0cb85
Parent:
18:4a6be1893d7f
Child:
22:cce4dc5738af
States done, but no motor control, and gives error when executed

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