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@26:418f025a30c6, 2019-11-01 (annotated)

Committer:
Floris_Hoek
Date:
Fri Nov 01 17:18:39 2019 +0000
Revision:
26:418f025a30c6
Parent:
25:45c131af2dba
Child:
27:e704fdc41e87
FINAL VERSION

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Floris_Hoek 23:ff73ee119244 1 // Operating mode might not go to next state when SW2 is pressed
Floris_Hoek 12:f4331640e3ad 2
RobertoO 0:67c50348f842 3 #include "mbed.h"
Floris_Hoek 8:7dab565a208e 4 #include "HIDScope.h"
Floris_Hoek 8:7dab565a208e 5 #include "BiQuad.h"
RobertoO 1:b862262a9d14 6 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 7 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 8 #include "QEI.h"
Floris_Hoek 12:f4331640e3ad 9
Floris_Hoek 23:ff73ee119244 10 #include <cmath> // Included to use different math operations
paulstuiver 2:75b2f713161c 11
Floris_Hoek 23:ff73ee119244 12 #include "Servo.h" // Included to control the servo motor
Floris_Hoek 8:7dab565a208e 13
Floris_Hoek 23:ff73ee119244 14 Servo myservo(D13); // To control the servo motor
Floris_Hoek 23:ff73ee119244 15 DigitalIn but3(SW2); // To go to the next state or to choose one of two game modes when in state START_GAME
Floris_Hoek 23:ff73ee119244 16 DigitalIn but4(SW3); // To choose one of two game modes when in state START_GAME or to move the gripper
Floris_Hoek 26:418f025a30c6 17 DigitalOut ledr(LED_RED);
Floris_Hoek 26:418f025a30c6 18 DigitalOut ledg(LED_GREEN);
Floris_Hoek 26:418f025a30c6 19 DigitalOut ledb(LED_BLUE);
Floris_Hoek 9:e8cc37a94fec 20
paulstuiver 24:e87e4fcf6226 21 AnalogIn S0(A0);
paulstuiver 24:e87e4fcf6226 22 AnalogIn S1(A1);
paulstuiver 24:e87e4fcf6226 23 AnalogIn S2(A2);
paulstuiver 24:e87e4fcf6226 24 AnalogIn S3(A3);
paulstuiver 24:e87e4fcf6226 25
paulstuiver 24:e87e4fcf6226 26 DigitalOut motor1Direction(D7);
paulstuiver 24:e87e4fcf6226 27 FastPWM motor1Velocity(D6);
paulstuiver 24:e87e4fcf6226 28 DigitalOut motor2Direction(D4);
paulstuiver 24:e87e4fcf6226 29 FastPWM motor2Velocity(D5);
paulstuiver 24:e87e4fcf6226 30
paulstuiver 24:e87e4fcf6226 31 // Encoders 1 and 2 respectively
paulstuiver 24:e87e4fcf6226 32 QEI Encoder1(D8,D9,NC,8400);
paulstuiver 24:e87e4fcf6226 33 QEI Encoder2(D11,D10,NC,8400);
Floris_Hoek 9:e8cc37a94fec 34
Floris_Hoek 23:ff73ee119244 35 Ticker measurecontrol; // Ticker function for the measurements
Floris_Hoek 18:4a6be1893d7f 36
Floris_Hoek 18:4a6be1893d7f 37 // Make arrays for the different variables for the motors
paulstuiver 24:e87e4fcf6226 38 //AnalogIn Shields[4] = {S0, S1, S2, S3};
paulstuiver 24:e87e4fcf6226 39 //DigitalOut MotorDirections[2] = {MD0, MD1};
paulstuiver 24:e87e4fcf6226 40 //FastPWM MotorVelocities[2] = {MV0, MV1};
paulstuiver 24:e87e4fcf6226 41 //QEI Encoders[2] = {E0, E1};
Floris_Hoek 18:4a6be1893d7f 42
Floris_Hoek 22:cce4dc5738af 43 Serial pc(USBTX, USBRX);
Floris_Hoek 8:7dab565a208e 44
Floris_Hoek 23:ff73ee119244 45 double PI = 3.14159265358;//97932384626433832795028841971693993;
Floris_Hoek 23:ff73ee119244 46 volatile double timeinterval = 0.001f; // Time interval of the Ticker function
Floris_Hoek 23:ff73ee119244 47 volatile double frequency = 17000.0; // Set motorfrequency
Floris_Hoek 23:ff73ee119244 48 double period_signal = 1.0/frequency; // Convert to period of the signal
Floris_Hoek 23:ff73ee119244 49
Floris_Hoek 23:ff73ee119244 50 double yendeffector = 10.6159;
Floris_Hoek 23:ff73ee119244 51 double xendeffector = 0;
Floris_Hoek 23:ff73ee119244 52 int ingedrukt = 0;
Floris_Hoek 23:ff73ee119244 53 int state_int;
paulstuiver 24:e87e4fcf6226 54 int previous_state_int;
paulstuiver 25:45c131af2dba 55 double motorvalue1;
paulstuiver 25:45c131af2dba 56 double motorvalue2;
Floris_Hoek 9:e8cc37a94fec 57 // Define the different states in which the robot can be
Floris_Hoek 23:ff73ee119244 58 enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
Floris_Hoek 23:ff73ee119244 59 DEMO_MODE, OPERATING_MODE, END_GAME
Floris_Hoek 22:cce4dc5738af 60 };
Floris_Hoek 19:b3e224e0cb85 61
Floris_Hoek 9:e8cc37a94fec 62 // Default state is the state in which the motors are turned off
Floris_Hoek 9:e8cc37a94fec 63 States MyState = MOTORS_OFF;
paulstuiver 5:2ae500da8fe1 64
Floris_Hoek 23:ff73ee119244 65
Floris_Hoek 14:1343966a79e8 66 // Initialise the functions
Floris_Hoek 9:e8cc37a94fec 67
Floris_Hoek 14:1343966a79e8 68 void motorsoff();
Floris_Hoek 14:1343966a79e8 69
Floris_Hoek 23:ff73ee119244 70 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3);
Floris_Hoek 14:1343966a79e8 71
Floris_Hoek 18:4a6be1893d7f 72 void det_max(double y, double &max_y);
Floris_Hoek 14:1343966a79e8 73
Floris_Hoek 14:1343966a79e8 74 void emgcalibration();
Floris_Hoek 14:1343966a79e8 75
Floris_Hoek 22:cce4dc5738af 76 void nothing()
Floris_Hoek 22:cce4dc5738af 77 {
Floris_Hoek 14:1343966a79e8 78 // Do nothing
Floris_Hoek 14:1343966a79e8 79 }
Floris_Hoek 14:1343966a79e8 80
Floris_Hoek 23:ff73ee119244 81 void startgame() ;
Floris_Hoek 18:4a6be1893d7f 82
Floris_Hoek 23:ff73ee119244 83 void demo_mode();
Floris_Hoek 18:4a6be1893d7f 84
Floris_Hoek 18:4a6be1893d7f 85 void operating_mode();
Floris_Hoek 18:4a6be1893d7f 86
Floris_Hoek 14:1343966a79e8 87 void New_State();
Floris_Hoek 14:1343966a79e8 88
Floris_Hoek 19:b3e224e0cb85 89 void Set_State();
Floris_Hoek 19:b3e224e0cb85 90
Floris_Hoek 23:ff73ee119244 91 double PID_controller1(double error1);
Floris_Hoek 23:ff73ee119244 92
Floris_Hoek 23:ff73ee119244 93 double PID_controller2(double error2);
Floris_Hoek 23:ff73ee119244 94
Floris_Hoek 23:ff73ee119244 95 void getmeasuredposition(double & measuredposition1, double & measuredposition2);
Floris_Hoek 23:ff73ee119244 96
Floris_Hoek 23:ff73ee119244 97 void getreferenceposition(double & referenceposition1, double & referenceposition2);
Floris_Hoek 23:ff73ee119244 98
Floris_Hoek 23:ff73ee119244 99 void sendtomotor(double motorvalue1, double motorvalue2);
Floris_Hoek 23:ff73ee119244 100
Floris_Hoek 23:ff73ee119244 101 void measureandcontrol();
Floris_Hoek 14:1343966a79e8 102
Floris_Hoek 14:1343966a79e8 103
Floris_Hoek 23:ff73ee119244 104
Floris_Hoek 14:1343966a79e8 105 int main()
Floris_Hoek 14:1343966a79e8 106 {
Floris_Hoek 26:418f025a30c6 107 ledr = 1;
Floris_Hoek 26:418f025a30c6 108 ledg = 1;
Floris_Hoek 26:418f025a30c6 109 ledb = 1;
Floris_Hoek 22:cce4dc5738af 110 pc.baud(115200);
Floris_Hoek 23:ff73ee119244 111 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 22:cce4dc5738af 112
paulstuiver 24:e87e4fcf6226 113 motor1Velocity.period(period_signal); // Set the period of the PWMfunction
paulstuiver 24:e87e4fcf6226 114 motor2Velocity.period(period_signal); // Set the period of the PWMfunction
Floris_Hoek 22:cce4dc5738af 115
paulstuiver 24:e87e4fcf6226 116 previous_state_int = (int)MyState; // Save previous state to compare with current state and possibly execute New_State()
Floris_Hoek 22:cce4dc5738af 117 // in the while loop
Floris_Hoek 22:cce4dc5738af 118
Floris_Hoek 14:1343966a79e8 119 New_State(); // Execute the functions belonging to the current state
Floris_Hoek 22:cce4dc5738af 120
Floris_Hoek 23:ff73ee119244 121 while(true) {
Floris_Hoek 14:1343966a79e8 122 if ( (previous_state_int - (int)MyState) != 0 ) { // If current state is not previous state execute New_State()
Floris_Hoek 14:1343966a79e8 123 New_State();
Floris_Hoek 14:1343966a79e8 124 }
Floris_Hoek 14:1343966a79e8 125 }
Floris_Hoek 23:ff73ee119244 126 }
Floris_Hoek 23:ff73ee119244 127
Floris_Hoek 23:ff73ee119244 128
Floris_Hoek 23:ff73ee119244 129
Floris_Hoek 23:ff73ee119244 130 double PID_controller1(double error1)
Floris_Hoek 23:ff73ee119244 131 {
Floris_Hoek 23:ff73ee119244 132 // Define errors for motor 1 and 2
Floris_Hoek 23:ff73ee119244 133 static double error_integral1 = 0;
Floris_Hoek 23:ff73ee119244 134 static double error_prev1 = error1;
Floris_Hoek 23:ff73ee119244 135
Floris_Hoek 23:ff73ee119244 136 // Low-pass filter
Floris_Hoek 23:ff73ee119244 137 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Floris_Hoek 23:ff73ee119244 138
Floris_Hoek 23:ff73ee119244 139 // PID variables: we assume them to be the same for both motors
paulstuiver 24:e87e4fcf6226 140 double Kp = 63;
Floris_Hoek 23:ff73ee119244 141 double Ki = 3.64;
Floris_Hoek 23:ff73ee119244 142 double Kd = 5;
Floris_Hoek 23:ff73ee119244 143
Floris_Hoek 23:ff73ee119244 144 //Proportional part:
Floris_Hoek 23:ff73ee119244 145 double u_k1 = Kp * error1;
Floris_Hoek 23:ff73ee119244 146
Floris_Hoek 23:ff73ee119244 147 // Integreal part
Floris_Hoek 23:ff73ee119244 148 error_integral1 = error_integral1 + error1 * timeinterval;
Floris_Hoek 23:ff73ee119244 149 double u_i1 = Ki*error_integral1;
Floris_Hoek 23:ff73ee119244 150
Floris_Hoek 23:ff73ee119244 151 // Derivate part
Floris_Hoek 23:ff73ee119244 152 double error_derivative1 = (error1 - error_prev1)/timeinterval;
Floris_Hoek 23:ff73ee119244 153 double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
Floris_Hoek 23:ff73ee119244 154 double u_d1 = Kd * filtered_error_derivative1;
Floris_Hoek 23:ff73ee119244 155 error_prev1 = error1;
Floris_Hoek 23:ff73ee119244 156
Floris_Hoek 23:ff73ee119244 157 //sum all parts and return it
Floris_Hoek 23:ff73ee119244 158 return u_k1 + u_i1 + u_d1;
Floris_Hoek 23:ff73ee119244 159 }
Floris_Hoek 23:ff73ee119244 160
Floris_Hoek 23:ff73ee119244 161
Floris_Hoek 23:ff73ee119244 162 double PID_controller2(double error2)
Floris_Hoek 23:ff73ee119244 163 {
Floris_Hoek 23:ff73ee119244 164 // Define errors for motor 1 and 2
Floris_Hoek 23:ff73ee119244 165 static double error_integral2 = 0;
Floris_Hoek 23:ff73ee119244 166 static double error_prev2 = error2;
Floris_Hoek 23:ff73ee119244 167
Floris_Hoek 23:ff73ee119244 168 // Low-pass filter
Floris_Hoek 23:ff73ee119244 169 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Floris_Hoek 23:ff73ee119244 170
Floris_Hoek 23:ff73ee119244 171 // PID variables: we assume them to be the same for both motors
paulstuiver 24:e87e4fcf6226 172 double Kp = 63;
Floris_Hoek 23:ff73ee119244 173 double Ki = 3.64;
Floris_Hoek 23:ff73ee119244 174 double Kd = 5;
Floris_Hoek 23:ff73ee119244 175
Floris_Hoek 23:ff73ee119244 176 //Proportional part:
Floris_Hoek 23:ff73ee119244 177 double u_k2 = Kp * error2;
Floris_Hoek 23:ff73ee119244 178
Floris_Hoek 23:ff73ee119244 179 // Integreal part
Floris_Hoek 23:ff73ee119244 180 error_integral2 = error_integral2 + error2 * timeinterval;
Floris_Hoek 23:ff73ee119244 181 double u_i2 = Ki*error_integral2;
Floris_Hoek 23:ff73ee119244 182
Floris_Hoek 23:ff73ee119244 183 // Derivate part
Floris_Hoek 23:ff73ee119244 184 double error_derivative2 = (error2 - error_prev2)/timeinterval;
Floris_Hoek 23:ff73ee119244 185 double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
Floris_Hoek 23:ff73ee119244 186 double u_d2 = Kd * filtered_error_derivative2;
Floris_Hoek 23:ff73ee119244 187 error_prev2 = error2;
Floris_Hoek 23:ff73ee119244 188
Floris_Hoek 23:ff73ee119244 189 //sum all parts and return it
Floris_Hoek 23:ff73ee119244 190 return u_k2 + u_i2 + u_d2;
Floris_Hoek 23:ff73ee119244 191 }
Floris_Hoek 23:ff73ee119244 192
Floris_Hoek 22:cce4dc5738af 193
Floris_Hoek 23:ff73ee119244 194 //get the measured position
Floris_Hoek 23:ff73ee119244 195 void getmeasuredposition(double & measuredposition1, double & measuredposition2)
Floris_Hoek 23:ff73ee119244 196 {
Floris_Hoek 23:ff73ee119244 197 // Obtain the counts of motors 1 and 2 from the encoder
Floris_Hoek 23:ff73ee119244 198 int countsmotor1;
Floris_Hoek 23:ff73ee119244 199 int countsmotor2;
paulstuiver 24:e87e4fcf6226 200 countsmotor1 = Encoder1.getPulses();
paulstuiver 24:e87e4fcf6226 201 countsmotor2 = Encoder2.getPulses();
Floris_Hoek 23:ff73ee119244 202
Floris_Hoek 23:ff73ee119244 203 // Obtain the measured position for motor 1 and 2
Floris_Hoek 23:ff73ee119244 204 measuredposition1 = ((double)countsmotor1) / 8400.0f * 2.0f;
Floris_Hoek 23:ff73ee119244 205 measuredposition2 = ((double)countsmotor2) / 8400.0f * 2.0f;
Floris_Hoek 23:ff73ee119244 206 }
Floris_Hoek 23:ff73ee119244 207
Floris_Hoek 23:ff73ee119244 208
Floris_Hoek 23:ff73ee119244 209 //get the reference of the
Floris_Hoek 23:ff73ee119244 210 void getreferenceposition(double & referenceposition1, double & referenceposition2)
Floris_Hoek 23:ff73ee119244 211 {
Floris_Hoek 23:ff73ee119244 212 //Measurements of the arm
Floris_Hoek 23:ff73ee119244 213 double L0=1.95;
Floris_Hoek 23:ff73ee119244 214 double L1=15;
Floris_Hoek 23:ff73ee119244 215 double L2=20;
Floris_Hoek 23:ff73ee119244 216
Floris_Hoek 23:ff73ee119244 217 //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
Floris_Hoek 23:ff73ee119244 218 double desiredmotorangle1, desiredmotorangle2;
Floris_Hoek 23:ff73ee119244 219 desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;
Floris_Hoek 23:ff73ee119244 220 desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;
Floris_Hoek 23:ff73ee119244 221
Floris_Hoek 23:ff73ee119244 222 //Convert motor angles to counts
Floris_Hoek 23:ff73ee119244 223 double desiredmotorrounds1;
Floris_Hoek 23:ff73ee119244 224 double desiredmotorrounds2;
Floris_Hoek 23:ff73ee119244 225 desiredmotorrounds1 = (desiredmotorangle1)/360.0;
Floris_Hoek 23:ff73ee119244 226 desiredmotorrounds2 = (desiredmotorangle2)/360.0;
Floris_Hoek 23:ff73ee119244 227
Floris_Hoek 23:ff73ee119244 228 //Assign this to new variables because otherwise it doesn't work
Floris_Hoek 23:ff73ee119244 229 referenceposition1 = desiredmotorrounds1;
Floris_Hoek 23:ff73ee119244 230 referenceposition2 = desiredmotorrounds2;
Floris_Hoek 14:1343966a79e8 231 }
Floris_Hoek 23:ff73ee119244 232
Floris_Hoek 23:ff73ee119244 233
Floris_Hoek 23:ff73ee119244 234 //send value to motor
Floris_Hoek 23:ff73ee119244 235 // IT WAS "void sendtomotor(float & motorvalue1, float & motorvalue2)" BUT I REMOVED THE REFERENCE, BECAUSE I THOUGHT IT WAS NOT NEEDED
Floris_Hoek 23:ff73ee119244 236 void sendtomotor(double motorvalue1, double motorvalue2)
Floris_Hoek 23:ff73ee119244 237 {
Floris_Hoek 23:ff73ee119244 238 // Define the absolute motor values
Floris_Hoek 23:ff73ee119244 239 double absolutemotorvalue1;
Floris_Hoek 23:ff73ee119244 240 double absolutemotorvalue2;
Floris_Hoek 23:ff73ee119244 241 absolutemotorvalue1 = fabs(motorvalue1);
Floris_Hoek 23:ff73ee119244 242 absolutemotorvalue2 = fabs(motorvalue2);
Floris_Hoek 23:ff73ee119244 243
Floris_Hoek 23:ff73ee119244 244 // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
Floris_Hoek 23:ff73ee119244 245 absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;
Floris_Hoek 23:ff73ee119244 246 absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
Floris_Hoek 23:ff73ee119244 247
Floris_Hoek 23:ff73ee119244 248 // Send the absolutemotorvalue to the motors
paulstuiver 24:e87e4fcf6226 249 motor1Velocity = absolutemotorvalue1;
paulstuiver 24:e87e4fcf6226 250 motor2Velocity = absolutemotorvalue2;
Floris_Hoek 23:ff73ee119244 251
Floris_Hoek 23:ff73ee119244 252 // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 24:e87e4fcf6226 253 motor1Direction = (motorvalue1 > 0.0f);
paulstuiver 24:e87e4fcf6226 254 motor2Direction = (motorvalue2 > 0.0f);
Floris_Hoek 23:ff73ee119244 255 }
Floris_Hoek 23:ff73ee119244 256
Floris_Hoek 23:ff73ee119244 257 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
Floris_Hoek 23:ff73ee119244 258 void measureandcontrol()
Floris_Hoek 23:ff73ee119244 259 {
Floris_Hoek 23:ff73ee119244 260 // Get the reference positions of motor 1 and 2
Floris_Hoek 23:ff73ee119244 261 double reference1, reference2;
Floris_Hoek 23:ff73ee119244 262 getreferenceposition(reference1, reference2);
Floris_Hoek 23:ff73ee119244 263
Floris_Hoek 23:ff73ee119244 264 // Get the measured positions of motor 1 and 2
Floris_Hoek 23:ff73ee119244 265 double measured1, measured2;
Floris_Hoek 23:ff73ee119244 266 getmeasuredposition(measured1, measured2);
Floris_Hoek 23:ff73ee119244 267
Floris_Hoek 23:ff73ee119244 268 // Calculate the motor values
paulstuiver 25:45c131af2dba 269 //double motorvalue1, motorvalue2;
Floris_Hoek 23:ff73ee119244 270 motorvalue1 = PID_controller1(reference1 - measured1);
Floris_Hoek 23:ff73ee119244 271 motorvalue2 = PID_controller2(reference2 - measured2);
Floris_Hoek 23:ff73ee119244 272 sendtomotor(motorvalue1, motorvalue2);
Floris_Hoek 23:ff73ee119244 273 }
Floris_Hoek 23:ff73ee119244 274
Floris_Hoek 9:e8cc37a94fec 275
Floris_Hoek 22:cce4dc5738af 276 void motorsoff()
Floris_Hoek 22:cce4dc5738af 277 {
Floris_Hoek 23:ff73ee119244 278 // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button SW2 is pressed.
Floris_Hoek 12:f4331640e3ad 279 // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
Floris_Hoek 23:ff73ee119244 280 sendtomotor(0.0,0.0); // Set motor velocity to 0
Floris_Hoek 23:ff73ee119244 281
Floris_Hoek 23:ff73ee119244 282 state_int = 10;
Floris_Hoek 22:cce4dc5738af 283
Floris_Hoek 12:f4331640e3ad 284 bool whileloop_boolean = true; // Boolean for the while loop
Floris_Hoek 22:cce4dc5738af 285
Floris_Hoek 12:f4331640e3ad 286 while (whileloop_boolean) {
Floris_Hoek 23:ff73ee119244 287 if (but3.read() == 0) { // If button SW2 is pressed:
Floris_Hoek 23:ff73ee119244 288 MyState = (States)((int)MyState+1); // set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 23:ff73ee119244 289 whileloop_boolean = false; // by making whileloop_boolean equal to false
Floris_Hoek 23:ff73ee119244 290 wait(0.5f);
Floris_Hoek 9:e8cc37a94fec 291 }
Floris_Hoek 9:e8cc37a94fec 292 }
Floris_Hoek 9:e8cc37a94fec 293 }
Floris_Hoek 12:f4331640e3ad 294
Floris_Hoek 22:cce4dc5738af 295 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3)
Floris_Hoek 22:cce4dc5738af 296 {
Floris_Hoek 18:4a6be1893d7f 297 // High pass
Floris_Hoek 18:4a6be1893d7f 298 double hb0 = 0.9169; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 299 double hb1 = -1.8338; //
Floris_Hoek 18:4a6be1893d7f 300 double hb2 = 0.9169; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 301 double ha0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 302 double ha1 = -1.8268; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 303 double ha2 = 0.8407; //
Floris_Hoek 22:cce4dc5738af 304
Floris_Hoek 18:4a6be1893d7f 305 // Low pass
Floris_Hoek 18:4a6be1893d7f 306 double lb0 = 0.000083621; // Coefficients from the following formula:
Floris_Hoek 18:4a6be1893d7f 307 double lb1 = 0.0006724; //
Floris_Hoek 18:4a6be1893d7f 308 double lb2 = 0.000083621; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 18:4a6be1893d7f 309 double la0 = 1.0; // H(z) = ----------------------
Floris_Hoek 18:4a6be1893d7f 310 double la1 = -1.9740; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 18:4a6be1893d7f 311 double la2 = 0.9743; //
Floris_Hoek 22:cce4dc5738af 312
Floris_Hoek 23:ff73ee119244 313 static double max_y0 = 0.001;
Floris_Hoek 23:ff73ee119244 314 static double max_y1 = 0.001;
Floris_Hoek 23:ff73ee119244 315 static double max_y2 = 0.001;
Floris_Hoek 23:ff73ee119244 316 static double max_y3 = 0.001;
Floris_Hoek 22:cce4dc5738af 317
Floris_Hoek 18:4a6be1893d7f 318 static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 319 static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 320 static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 18:4a6be1893d7f 321 static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
Floris_Hoek 22:cce4dc5738af 322
Floris_Hoek 18:4a6be1893d7f 323 static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 18:4a6be1893d7f 324 static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 325 static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 18:4a6be1893d7f 326 static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
Floris_Hoek 22:cce4dc5738af 327
paulstuiver 24:e87e4fcf6226 328 f_y0 = hFilter0.step(S0); // Apply filters on the different EMG signals
paulstuiver 24:e87e4fcf6226 329 f_y1 = hFilter1.step(S1);
paulstuiver 24:e87e4fcf6226 330 f_y2 = hFilter2.step(S2);
paulstuiver 24:e87e4fcf6226 331 f_y3 = hFilter3.step(S3);
Floris_Hoek 22:cce4dc5738af 332
Floris_Hoek 19:b3e224e0cb85 333 f_y0 = abs(f_y0);
Floris_Hoek 19:b3e224e0cb85 334 f_y1 = abs(f_y1);
Floris_Hoek 19:b3e224e0cb85 335 f_y2 = abs(f_y2);
Floris_Hoek 19:b3e224e0cb85 336 f_y3 = abs(f_y3);
Floris_Hoek 22:cce4dc5738af 337
Floris_Hoek 18:4a6be1893d7f 338 f_y0 = lFilter0.step(f_y0);
Floris_Hoek 18:4a6be1893d7f 339 f_y1 = lFilter1.step(f_y1);
Floris_Hoek 18:4a6be1893d7f 340 f_y2 = lFilter2.step(f_y2);
Floris_Hoek 18:4a6be1893d7f 341 f_y3 = lFilter3.step(f_y3);
Floris_Hoek 18:4a6be1893d7f 342
Floris_Hoek 18:4a6be1893d7f 343
Floris_Hoek 15:5f9450964075 344 if (MyState == EMG_CALIBRATION) {
Floris_Hoek 22:cce4dc5738af 345
Floris_Hoek 18:4a6be1893d7f 346 det_max(f_y0, max_y0); // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
Floris_Hoek 18:4a6be1893d7f 347 det_max(f_y1, max_y1);
Floris_Hoek 18:4a6be1893d7f 348 det_max(f_y2, max_y2);
Floris_Hoek 18:4a6be1893d7f 349 det_max(f_y3, max_y3);
Floris_Hoek 22:cce4dc5738af 350
Floris_Hoek 23:ff73ee119244 351 } else if ((int)MyState > 3) {
Floris_Hoek 18:4a6be1893d7f 352 f_y0 = f_y0/max_y0; // Normalise the RMS value by dividing by the maximum RMS value
Floris_Hoek 23:ff73ee119244 353 f_y1 = f_y1/max_y1; // This is done during the states with a value higher than 3, as this is when you start the operating mode
Floris_Hoek 18:4a6be1893d7f 354 f_y2 = f_y2/max_y2;
Floris_Hoek 18:4a6be1893d7f 355 f_y3 = f_y3/max_y3;
Floris_Hoek 14:1343966a79e8 356 }
Floris_Hoek 22:cce4dc5738af 357
Floris_Hoek 10:8c38a1a5b522 358 }
Floris_Hoek 12:f4331640e3ad 359
Floris_Hoek 22:cce4dc5738af 360 void det_max(double y, double &max_y)
Floris_Hoek 22:cce4dc5738af 361 {
Floris_Hoek 18:4a6be1893d7f 362 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 363 }
Floris_Hoek 12:f4331640e3ad 364
Floris_Hoek 22:cce4dc5738af 365 void emgcalibration()
Floris_Hoek 22:cce4dc5738af 366 {
Floris_Hoek 18:4a6be1893d7f 367 double y0, y1, y2, y3; // RMS values of the different EMG signals
Floris_Hoek 14:1343966a79e8 368
Floris_Hoek 18:4a6be1893d7f 369 measure_data(y0, y1, y2, y3); // Calculate RMS values
Floris_Hoek 22:cce4dc5738af 370
Floris_Hoek 26:418f025a30c6 371 double duration = 20.0; // Duration of the emgcalibration function, in this case 10 seconds
Floris_Hoek 14:1343966a79e8 372 int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
Floris_Hoek 14:1343966a79e8 373 // rounds is an integer so the value of duration / timeinterval is floored
Floris_Hoek 22:cce4dc5738af 374
Floris_Hoek 14:1343966a79e8 375 static int counter = 0; // Counter which keeps track of the amount of times the function has executed
Floris_Hoek 14:1343966a79e8 376 if (counter >= rounds) {
paulstuiver 24:e87e4fcf6226 377 MyState = START_GAME; // If counter is larger than rounds, change MyState to the next state
Floris_Hoek 26:418f025a30c6 378 ledb = 1;
paulstuiver 24:e87e4fcf6226 379 measurecontrol.detach();
Floris_Hoek 22:cce4dc5738af 380 } else {
Floris_Hoek 14:1343966a79e8 381 counter++; // Else increase counter by 1
Floris_Hoek 14:1343966a79e8 382 }
Floris_Hoek 26:418f025a30c6 383
Floris_Hoek 26:418f025a30c6 384 int duration_led = 0.1 / timeinterval;
Floris_Hoek 26:418f025a30c6 385 if (counter % duration_led == 0) {
Floris_Hoek 26:418f025a30c6 386 ledb = !ledb;
Floris_Hoek 26:418f025a30c6 387 }
Floris_Hoek 14:1343966a79e8 388 }
Floris_Hoek 12:f4331640e3ad 389
Floris_Hoek 22:cce4dc5738af 390 void startgame()
Floris_Hoek 22:cce4dc5738af 391 {
Floris_Hoek 18:4a6be1893d7f 392 pc.printf("Please choose which game you would like to start:\r\n");
Floris_Hoek 18:4a6be1893d7f 393
Floris_Hoek 23:ff73ee119244 394 pc.printf("- Press button SW2 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 23:ff73ee119244 395 pc.printf(" It will move in straight lines to show that the robot meets the requirements.\r\n");
Floris_Hoek 18:4a6be1893d7f 396 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 397
Floris_Hoek 23:ff73ee119244 398 pc.printf("- Press button SW3 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 399 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 400
Floris_Hoek 22:cce4dc5738af 401 while (MyState == START_GAME) {
Floris_Hoek 23:ff73ee119244 402
Floris_Hoek 23:ff73ee119244 403 if (but3.read() == 0) {
Floris_Hoek 23:ff73ee119244 404 MyState = (States)((int)MyState+1);
Floris_Hoek 23:ff73ee119244 405 wait(0.5f);
Floris_Hoek 23:ff73ee119244 406 } else if (but4.read() == 0) {
Floris_Hoek 23:ff73ee119244 407 MyState = (States)((int)MyState+2);
Floris_Hoek 23:ff73ee119244 408 wait(0.5f);
Floris_Hoek 22:cce4dc5738af 409 }
Floris_Hoek 23:ff73ee119244 410
Floris_Hoek 22:cce4dc5738af 411 }
Floris_Hoek 18:4a6be1893d7f 412 }
Floris_Hoek 18:4a6be1893d7f 413
Floris_Hoek 23:ff73ee119244 414
Floris_Hoek 23:ff73ee119244 415 void demo_mode()
Floris_Hoek 23:ff73ee119244 416 {
Floris_Hoek 23:ff73ee119244 417
Floris_Hoek 23:ff73ee119244 418 // 5 pre determined positions of the end effector to show it can move in straight lines
Floris_Hoek 23:ff73ee119244 419 xendeffector=-5;
Floris_Hoek 23:ff73ee119244 420 yendeffector=10.6159;
Floris_Hoek 23:ff73ee119244 421 wait(0.5);
Floris_Hoek 23:ff73ee119244 422
Floris_Hoek 23:ff73ee119244 423 xendeffector=10;
Floris_Hoek 23:ff73ee119244 424 yendeffector=25.6159;
Floris_Hoek 23:ff73ee119244 425 wait(0.5);
Floris_Hoek 22:cce4dc5738af 426
Floris_Hoek 23:ff73ee119244 427 xendeffector=-14;
Floris_Hoek 23:ff73ee119244 428 yendeffector=21.6159;
Floris_Hoek 23:ff73ee119244 429 wait(0.5);
Floris_Hoek 23:ff73ee119244 430
Floris_Hoek 23:ff73ee119244 431 xendeffector=10;
Floris_Hoek 23:ff73ee119244 432 yendeffector=11.6159;
Floris_Hoek 23:ff73ee119244 433 wait(0.5);
Floris_Hoek 23:ff73ee119244 434
Floris_Hoek 23:ff73ee119244 435 // Last position is the start position
Floris_Hoek 23:ff73ee119244 436 xendeffector=0;
Floris_Hoek 23:ff73ee119244 437 yendeffector=10.6159;
Floris_Hoek 23:ff73ee119244 438 wait(0.5);
Floris_Hoek 23:ff73ee119244 439
Floris_Hoek 23:ff73ee119244 440 MyState = START_GAME; // Go back to START_GAME mode
Floris_Hoek 18:4a6be1893d7f 441 }
Floris_Hoek 23:ff73ee119244 442
Floris_Hoek 18:4a6be1893d7f 443
Floris_Hoek 22:cce4dc5738af 444 void operating_mode()
Floris_Hoek 22:cce4dc5738af 445 {
Floris_Hoek 26:418f025a30c6 446 // green turns on and off while running this function
Floris_Hoek 26:418f025a30c6 447 static int counter = 0;
Floris_Hoek 26:418f025a30c6 448 int duration_led = 0.1 / timeinterval;
Floris_Hoek 26:418f025a30c6 449
Floris_Hoek 26:418f025a30c6 450 if (counter % duration_led == 0) {
Floris_Hoek 26:418f025a30c6 451 ledg = !ledg;
Floris_Hoek 26:418f025a30c6 452 counter = 0;
Floris_Hoek 26:418f025a30c6 453 }
Floris_Hoek 26:418f025a30c6 454 counter++;
Floris_Hoek 26:418f025a30c6 455
Floris_Hoek 26:418f025a30c6 456
Floris_Hoek 22:cce4dc5738af 457 double y0,y1,y2,y3;
Floris_Hoek 22:cce4dc5738af 458 measure_data(y0,y1,y2,y3);
paulstuiver 24:e87e4fcf6226 459
Floris_Hoek 26:418f025a30c6 460 //servo besturing
paulstuiver 24:e87e4fcf6226 461
Floris_Hoek 26:418f025a30c6 462 if(but4.read() == 0 && ingedrukt == 0) {
Floris_Hoek 26:418f025a30c6 463 for(int i=0; i<100; i++) {
Floris_Hoek 26:418f025a30c6 464 myservo = i/100.0;
Floris_Hoek 26:418f025a30c6 465 }
Floris_Hoek 26:418f025a30c6 466 ingedrukt = 1;
Floris_Hoek 26:418f025a30c6 467 y0 = 0;
Floris_Hoek 26:418f025a30c6 468 y1 = 0;
Floris_Hoek 26:418f025a30c6 469 y2 = 0;
Floris_Hoek 26:418f025a30c6 470 y3 = 0;
Floris_Hoek 26:418f025a30c6 471 }
Floris_Hoek 26:418f025a30c6 472 else if(but4.read() == 0 && ingedrukt == 1) {
Floris_Hoek 26:418f025a30c6 473 for(int i=100; i>0; i--) {
Floris_Hoek 26:418f025a30c6 474 myservo = i/100.0;
Floris_Hoek 26:418f025a30c6 475 }
Floris_Hoek 26:418f025a30c6 476 y0 = 0;
Floris_Hoek 26:418f025a30c6 477 y1 = 0;
Floris_Hoek 26:418f025a30c6 478 y2 = 0;
Floris_Hoek 26:418f025a30c6 479 y3 = 0;
Floris_Hoek 26:418f025a30c6 480 ingedrukt = 0;
Floris_Hoek 26:418f025a30c6 481 }
Floris_Hoek 26:418f025a30c6 482
Floris_Hoek 26:418f025a30c6 483
Floris_Hoek 26:418f025a30c6 484 double threshold = 0.4; // When the analysed signal is above this threshold, the position of the end effector is changed
paulstuiver 24:e87e4fcf6226 485 double dr = 0.01; // The change in position with each step
Floris_Hoek 23:ff73ee119244 486
Floris_Hoek 23:ff73ee119244 487 if(y0 > threshold && xendeffector < 16) {
Floris_Hoek 23:ff73ee119244 488 xendeffector=xendeffector+dr;
Floris_Hoek 23:ff73ee119244 489 }
Floris_Hoek 23:ff73ee119244 490 else if(y1 > threshold && xendeffector > -16) {
Floris_Hoek 23:ff73ee119244 491 xendeffector=xendeffector-dr;
Floris_Hoek 23:ff73ee119244 492 }
Floris_Hoek 26:418f025a30c6 493 if(y2 > threshold+0.2 && yendeffector < 28) {
Floris_Hoek 23:ff73ee119244 494 yendeffector=yendeffector+dr;
Floris_Hoek 23:ff73ee119244 495 }
Floris_Hoek 26:418f025a30c6 496 else if(y3 > threshold+0.2 && yendeffector > 8) {
Floris_Hoek 23:ff73ee119244 497 yendeffector=yendeffector-dr;
Floris_Hoek 22:cce4dc5738af 498 }
Floris_Hoek 22:cce4dc5738af 499
paulstuiver 24:e87e4fcf6226 500
Floris_Hoek 23:ff73ee119244 501 if (but3.read() == 0) {
paulstuiver 25:45c131af2dba 502 bool buttonss = true;
paulstuiver 25:45c131af2dba 503 while (buttonss) {
paulstuiver 25:45c131af2dba 504 if (but3.read() == 1) {
paulstuiver 25:45c131af2dba 505 buttonss = false;
paulstuiver 25:45c131af2dba 506 }
paulstuiver 25:45c131af2dba 507 }
paulstuiver 24:e87e4fcf6226 508 pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
paulstuiver 24:e87e4fcf6226 509 MyState = END_GAME;
Floris_Hoek 26:418f025a30c6 510 measurecontrol.detach();
paulstuiver 25:45c131af2dba 511 }
Floris_Hoek 26:418f025a30c6 512
Floris_Hoek 23:ff73ee119244 513 measureandcontrol();
Floris_Hoek 23:ff73ee119244 514 }
Floris_Hoek 23:ff73ee119244 515
Floris_Hoek 23:ff73ee119244 516 void endgame()
Floris_Hoek 23:ff73ee119244 517 {
Floris_Hoek 26:418f025a30c6 518 measurecontrol.attach(measureandcontrol,timeinterval);
paulstuiver 25:45c131af2dba 519
Floris_Hoek 26:418f025a30c6 520 double tempx = xendeffector;
Floris_Hoek 26:418f025a30c6 521 double tempy = yendeffector-10.6159;
Floris_Hoek 26:418f025a30c6 522 double steps = 20;
Floris_Hoek 26:418f025a30c6 523
Floris_Hoek 26:418f025a30c6 524 for (int i = 0; i < steps; i++) {
Floris_Hoek 26:418f025a30c6 525 xendeffector = xendeffector - tempx/steps;
Floris_Hoek 26:418f025a30c6 526 yendeffector = yendeffector - tempy/steps;
Floris_Hoek 26:418f025a30c6 527 wait(0.1);
paulstuiver 25:45c131af2dba 528 }
Floris_Hoek 26:418f025a30c6 529
paulstuiver 24:e87e4fcf6226 530 measurecontrol.detach();
Floris_Hoek 23:ff73ee119244 531 MyState = START_GAME;
Floris_Hoek 18:4a6be1893d7f 532 }
Floris_Hoek 18:4a6be1893d7f 533
Floris_Hoek 22:cce4dc5738af 534 void New_State()
Floris_Hoek 22:cce4dc5738af 535 {
paulstuiver 24:e87e4fcf6226 536 previous_state_int = (int)MyState; // Change previous state to current state
paulstuiver 24:e87e4fcf6226 537
Floris_Hoek 22:cce4dc5738af 538 switch (MyState) {
Floris_Hoek 9:e8cc37a94fec 539 case MOTORS_OFF :
Floris_Hoek 22:cce4dc5738af 540 pc.printf("\r\nState: Motors turned off\r\n");
Floris_Hoek 26:418f025a30c6 541 ledb = 0; // blue led turns on when you enter MOTORS_OFF state
Floris_Hoek 9:e8cc37a94fec 542 motorsoff();
Floris_Hoek 26:418f025a30c6 543 ledb = 1; // blue led turns off when you exit MOTORS_OFF state
Floris_Hoek 9:e8cc37a94fec 544 break;
Floris_Hoek 22:cce4dc5738af 545
Floris_Hoek 9:e8cc37a94fec 546 case EMG_CALIBRATION :
Floris_Hoek 22:cce4dc5738af 547 pc.printf("\r\nState: EMG Calibration\r\n");
Floris_Hoek 22:cce4dc5738af 548 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 549 measurecontrol.attach(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 550 break;
Floris_Hoek 22:cce4dc5738af 551
Floris_Hoek 9:e8cc37a94fec 552 case START_GAME :
Floris_Hoek 22:cce4dc5738af 553 pc.printf("\r\nState: Start game\r\n");
Floris_Hoek 26:418f025a30c6 554 ledr = 0; // red led is on when you enter START_GAME state
Floris_Hoek 26:418f025a30c6 555 ledb = 0; // blue led is on when you enter START_GAME state
Floris_Hoek 26:418f025a30c6 556 ledg = 0; // green led is on when you enter START_GAME state
Floris_Hoek 18:4a6be1893d7f 557 startgame();
Floris_Hoek 9:e8cc37a94fec 558 break;
Floris_Hoek 22:cce4dc5738af 559
Floris_Hoek 9:e8cc37a94fec 560 case DEMO_MODE :
Floris_Hoek 22:cce4dc5738af 561 pc.printf("\r\nState: Demo mode\r\n");
Floris_Hoek 23:ff73ee119244 562 measurecontrol.attach(measureandcontrol,timeinterval);
Floris_Hoek 26:418f025a30c6 563 ledr = 1; // red led is on when you enter START_GAME state
Floris_Hoek 26:418f025a30c6 564 ledb = 0; // blue led is on when you enter START_GAME state
Floris_Hoek 26:418f025a30c6 565 ledg = 0; // green led is on when you enter START_GAME state
Floris_Hoek 23:ff73ee119244 566 demo_mode();
paulstuiver 24:e87e4fcf6226 567 measurecontrol.detach();
Floris_Hoek 9:e8cc37a94fec 568 break;
Floris_Hoek 22:cce4dc5738af 569
Floris_Hoek 18:4a6be1893d7f 570 case OPERATING_MODE :
Floris_Hoek 22:cce4dc5738af 571 pc.printf("\r\nState: Operating mode\r\n");
Floris_Hoek 26:418f025a30c6 572 ledr = 1; // red led is off when you enter OPERATING_MODE state
Floris_Hoek 26:418f025a30c6 573 ledb = 1; // blue led is off when you enter OPERATING_MODE state
Floris_Hoek 26:418f025a30c6 574 ledg = 0; // green led is on when you enter OPERATING_MODE state
Floris_Hoek 18:4a6be1893d7f 575 measurecontrol.attach(operating_mode,timeinterval);
Floris_Hoek 9:e8cc37a94fec 576 break;
Floris_Hoek 22:cce4dc5738af 577
Floris_Hoek 9:e8cc37a94fec 578 case END_GAME :
Floris_Hoek 22:cce4dc5738af 579 pc.printf("\r\nState: End of the game\r\n");
Floris_Hoek 26:418f025a30c6 580 ledr = 0; // red led is on when you enter OPERATING_MODE state
Floris_Hoek 26:418f025a30c6 581 ledb = 1; // blue led is off when you enter OPERATING_MODE state
Floris_Hoek 26:418f025a30c6 582 ledg = 1; // green led is off when you enter OPERATING_MODE state
Floris_Hoek 23:ff73ee119244 583 endgame();
Floris_Hoek 9:e8cc37a94fec 584 break;
Floris_Hoek 22:cce4dc5738af 585
Floris_Hoek 9:e8cc37a94fec 586 default :
Floris_Hoek 22:cce4dc5738af 587 pc.printf("\r\nDefault state: Motors are turned off\r\n");
paulstuiver 24:e87e4fcf6226 588
Floris_Hoek 23:ff73ee119244 589 sendtomotor(0.0,0.0);
Floris_Hoek 9:e8cc37a94fec 590 break;
Floris_Hoek 9:e8cc37a94fec 591 }
Floris_Hoek 9:e8cc37a94fec 592 }
Floris_Hoek 9:e8cc37a94fec 593
Floris_Hoek 22:cce4dc5738af 594 void Set_State()
Floris_Hoek 22:cce4dc5738af 595 {
Floris_Hoek 23:ff73ee119244 596 xendeffector=0.0;
Floris_Hoek 23:ff73ee119244 597 yendeffector=10.6159;
Floris_Hoek 23:ff73ee119244 598 wait(0.3f);
Floris_Hoek 23:ff73ee119244 599 sendtomotor(0.0,0.0); // Stop the motors
paulstuiver 24:e87e4fcf6226 600 // Stop the ticker function from running
Floris_Hoek 19:b3e224e0cb85 601
Floris_Hoek 23:ff73ee119244 602 pc.printf("\r\nPress number: | To go to state:");
Floris_Hoek 23:ff73ee119244 603 pc.printf("\r\n (0) | MOTORS_OFF: Set motorspeed just in case to 0 and wait till button SW2 is pressed");
Floris_Hoek 23:ff73ee119244 604 pc.printf("\r\n (1) | EMG_CALIBRATION: Calibrate the maximum of the emg signals and normalise the emg signals with these maxima");
Floris_Hoek 23:ff73ee119244 605 pc.printf("\r\n (2) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
Floris_Hoek 23:ff73ee119244 606 pc.printf("\r\n (3) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
Floris_Hoek 23:ff73ee119244 607 pc.printf("\r\n (4) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
Floris_Hoek 23:ff73ee119244 608 pc.printf("\r\n (5) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
Floris_Hoek 22:cce4dc5738af 609
Floris_Hoek 23:ff73ee119244 610 wait(0.5f);
Floris_Hoek 22:cce4dc5738af 611
Floris_Hoek 23:ff73ee119244 612 char a = '0';
Floris_Hoek 23:ff73ee119244 613 char b = '5';
Floris_Hoek 23:ff73ee119244 614 bool boolean = true;
Floris_Hoek 22:cce4dc5738af 615
Floris_Hoek 23:ff73ee119244 616 while (boolean) {
Floris_Hoek 23:ff73ee119244 617 char c = pc.getc();
Floris_Hoek 22:cce4dc5738af 618
Floris_Hoek 23:ff73ee119244 619 if (c >= a && c <= b) {
Floris_Hoek 23:ff73ee119244 620 MyState = (States)(c-'0');
Floris_Hoek 23:ff73ee119244 621 boolean = false;
Floris_Hoek 22:cce4dc5738af 622
Floris_Hoek 23:ff73ee119244 623 } else {
Floris_Hoek 23:ff73ee119244 624 pc.printf("\r\nPlease enter a number between 0 and 5\r\n");
Floris_Hoek 19:b3e224e0cb85 625 }
Floris_Hoek 19:b3e224e0cb85 626 }
Floris_Hoek 19:b3e224e0cb85 627 }