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@17:3b463660aa42, 2019-10-29 (annotated)

Committer:
Floris_Hoek
Date:
Tue Oct 29 10:50:37 2019 +0000
Revision:
17:3b463660aa42
Parent:
15:5f9450964075 
fcked up file

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