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@15:5f9450964075, 2019-10-25 (annotated)

Committer:
Floris_Hoek
Date:
Fri Oct 25 08:44:57 2019 +0000
Revision:
15:5f9450964075
Parent:
14:1343966a79e8
Child:
17:3b463660aa42
Child:
18:4a6be1893d7f
EMG_CALIBRATION state done (Small adjustments and added comments)

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 12:f4331640e3ad 3
RobertoO 0:67c50348f842 4 #include "mbed.h"
Floris_Hoek 8:7dab565a208e 5 #include "HIDScope.h"
Floris_Hoek 8:7dab565a208e 6 #include "BiQuad.h"
RobertoO 1:b862262a9d14 7 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 8 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 9 #include "QEI.h"
Floris_Hoek 12:f4331640e3ad 10
paulstuiver 5:2ae500da8fe1 11 #include <math.h>
Floris_Hoek 12:f4331640e3ad 12 #include <deque>
paulstuiver 2:75b2f713161c 13
Floris_Hoek 8:7dab565a208e 14 #include "Motor_Control.h"
Floris_Hoek 8:7dab565a208e 15
Floris_Hoek 12:f4331640e3ad 16 DigitalIn button1(D12); // Button1 input to go to next state
Floris_Hoek 12:f4331640e3ad 17 InterruptIn button2(SW2); // Button2 input to activate failuremode()
Floris_Hoek 12:f4331640e3ad 18 DigitalOut ledr(LED_RED); // Red LED output to show
Floris_Hoek 9:e8cc37a94fec 19
Floris_Hoek 12:f4331640e3ad 20 AnalogIn shield0(A0); // Input EMG Shield 0
Floris_Hoek 12:f4331640e3ad 21 AnalogIn shield1(A1); // Input EMG Shield 1
Floris_Hoek 12:f4331640e3ad 22 AnalogIn shield2(A2); // Input EMG Shield 2
Floris_Hoek 12:f4331640e3ad 23 AnalogIn shield3(A3); // Input EMG Shield 3
Floris_Hoek 9:e8cc37a94fec 24
Floris_Hoek 12:f4331640e3ad 25 Ticker measurecontrol; // Ticker function for motor in- and output
Floris_Hoek 12:f4331640e3ad 26 DigitalOut motor1Direction(D7); // Direction of motor 1
Floris_Hoek 12:f4331640e3ad 27 FastPWM motor1Velocity(D6); // FastPWM class to set motor velocity of motor 1
Floris_Hoek 8:7dab565a208e 28 MODSERIAL pc(USBTX, USBRX);
Floris_Hoek 12:f4331640e3ad 29 QEI Encoder(D8,D9,NC,8400); // Input from the encoder to measure how much the motor has turned
Floris_Hoek 8:7dab565a208e 30
Floris_Hoek 10:8c38a1a5b522 31 float PI = 3.1415926f;//535897932384626433832795028841971693993;
Floris_Hoek 12:f4331640e3ad 32 float timeinterval = 0.001f; // Time interval of the Ticker function
Floris_Hoek 9:e8cc37a94fec 33 bool whileloop = true; // Statement to keep the whileloop in the main function running
Floris_Hoek 9:e8cc37a94fec 34 // While loop has to stop running when failuremode is activated
Floris_Hoek 9:e8cc37a94fec 35
Floris_Hoek 9:e8cc37a94fec 36 // Define the different states in which the robot can be
Floris_Hoek 9:e8cc37a94fec 37 enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
Floris_Hoek 9:e8cc37a94fec 38 START_GAME, DEMO_MODE, GAME_MODE, MOVE_END_EFFECTOR,
Floris_Hoek 9:e8cc37a94fec 39 MOVE_GRIPPER, END_GAME, FAILURE_MODE};
Floris_Hoek 9:e8cc37a94fec 40
Floris_Hoek 9:e8cc37a94fec 41 // Default state is the state in which the motors are turned off
Floris_Hoek 9:e8cc37a94fec 42 States MyState = MOTORS_OFF;
paulstuiver 5:2ae500da8fe1 43
Floris_Hoek 14:1343966a79e8 44 // Initialise the functions
Floris_Hoek 9:e8cc37a94fec 45
Floris_Hoek 14:1343966a79e8 46 void motorsoff();
Floris_Hoek 14:1343966a79e8 47
Floris_Hoek 14:1343966a79e8 48 float rms_deque(std::deque<float> deque);
Floris_Hoek 14:1343966a79e8 49
Floris_Hoek 14:1343966a79e8 50 void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3);
Floris_Hoek 14:1343966a79e8 51
Floris_Hoek 14:1343966a79e8 52 void det_max(float rms, float &max_rms);
Floris_Hoek 14:1343966a79e8 53
Floris_Hoek 14:1343966a79e8 54 void emgcalibration();
Floris_Hoek 14:1343966a79e8 55
Floris_Hoek 14:1343966a79e8 56 double P_controller(double error);
Floris_Hoek 14:1343966a79e8 57
Floris_Hoek 14:1343966a79e8 58 void nothing(){
Floris_Hoek 14:1343966a79e8 59 // Do nothing
Floris_Hoek 14:1343966a79e8 60 }
Floris_Hoek 14:1343966a79e8 61
Floris_Hoek 14:1343966a79e8 62 void New_State();
Floris_Hoek 14:1343966a79e8 63
Floris_Hoek 14:1343966a79e8 64 void failuremode();
Floris_Hoek 14:1343966a79e8 65
Floris_Hoek 14:1343966a79e8 66
Floris_Hoek 14:1343966a79e8 67
Floris_Hoek 14:1343966a79e8 68
Floris_Hoek 14:1343966a79e8 69 int main()
Floris_Hoek 14:1343966a79e8 70 {
Floris_Hoek 14:1343966a79e8 71 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 14:1343966a79e8 72 double frequency = 17000.0f; // Set motorfrequency
Floris_Hoek 14:1343966a79e8 73 double period_signal = 1.0f/frequency; // Convert to period of the signal
Floris_Hoek 14:1343966a79e8 74 pc.baud(115200);
Floris_Hoek 14:1343966a79e8 75
Floris_Hoek 14:1343966a79e8 76 button2.fall(failuremode); // Function is always activated when the button is pressed
Floris_Hoek 14:1343966a79e8 77 motor1Velocity.period(period_signal); // Set the period of the PWMfunction
Floris_Hoek 14:1343966a79e8 78 measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
Floris_Hoek 14:1343966a79e8 79
Floris_Hoek 14:1343966a79e8 80 int previous_state_int = (int)MyState; // Save previous state to compare with current state and possibly execute New_State()
Floris_Hoek 14:1343966a79e8 81 // in the while loop
Floris_Hoek 14:1343966a79e8 82 New_State(); // Execute the functions belonging to the current state
Floris_Hoek 14:1343966a79e8 83
Floris_Hoek 14:1343966a79e8 84 while(whileloop)
Floris_Hoek 14:1343966a79e8 85 {
Floris_Hoek 14:1343966a79e8 86 if ( (previous_state_int - (int)MyState) != 0 ) { // If current state is not previous state execute New_State()
Floris_Hoek 14:1343966a79e8 87 New_State();
Floris_Hoek 14:1343966a79e8 88 }
Floris_Hoek 14:1343966a79e8 89 previous_state_int = (int)MyState; // Change previous state to current state
Floris_Hoek 14:1343966a79e8 90 }
Floris_Hoek 14:1343966a79e8 91 // While has stopped running
Floris_Hoek 14:1343966a79e8 92 pc.printf("Programm stops running\r\n"); // So show that the programm is quiting
Floris_Hoek 14:1343966a79e8 93 sendtomotor(0.0f); // Set the motor velocity to 0
Floris_Hoek 14:1343966a79e8 94 measurecontrol.attach(nothing,10000); // Attach empty function to Ticker (?? Appropriate solution ??)
Floris_Hoek 14:1343966a79e8 95 return 0;
Floris_Hoek 14:1343966a79e8 96 }
Floris_Hoek 9:e8cc37a94fec 97
Floris_Hoek 9:e8cc37a94fec 98 void motorsoff() {
Floris_Hoek 12:f4331640e3ad 99 // 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 100 // 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 101
Floris_Hoek 12:f4331640e3ad 102 bool whileloop_boolean = true; // Boolean for the while loop
Floris_Hoek 12:f4331640e3ad 103 sendtomotor(0.0f); // Set motor velocity to 0
Floris_Hoek 12:f4331640e3ad 104
Floris_Hoek 12:f4331640e3ad 105 while (whileloop_boolean) {
Floris_Hoek 12:f4331640e3ad 106 if (button1.read() == 0) { // If button1 is pressed:
Floris_Hoek 12:f4331640e3ad 107 MyState = EMG_CALIBRATION; // set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 12:f4331640e3ad 108 whileloop_boolean = false; // by making whileloop_boolean equal to false
Floris_Hoek 9:e8cc37a94fec 109 }
Floris_Hoek 9:e8cc37a94fec 110 }
Floris_Hoek 9:e8cc37a94fec 111 }
Floris_Hoek 12:f4331640e3ad 112
Floris_Hoek 12:f4331640e3ad 113 float rms_deque(std::deque<float> deque) {
Floris_Hoek 12:f4331640e3ad 114 float sum = 0;
Floris_Hoek 12:f4331640e3ad 115 for (int i = 0; i < deque.size(); i++) {
Floris_Hoek 15:5f9450964075 116 sum = sum + pow(deque[i],2); // Square the entries of the deque and add them to sum
Floris_Hoek 12:f4331640e3ad 117 }
Floris_Hoek 15:5f9450964075 118 return pow(sum,0.5f); // Return the square root of the sum (and thus the RMS)
Floris_Hoek 12:f4331640e3ad 119 }
Floris_Hoek 12:f4331640e3ad 120
Floris_Hoek 15:5f9450964075 121 void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3) {
Floris_Hoek 12:f4331640e3ad 122 float b0 = 0.0f; // Coefficients from the following formula:
Floris_Hoek 12:f4331640e3ad 123 float b1 = 0.0f; //
Floris_Hoek 12:f4331640e3ad 124 float b2 = 0.0f; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 12:f4331640e3ad 125 float a0 = 0.0f; // H(z) = ----------------------
Floris_Hoek 12:f4331640e3ad 126 float a1 = 0.0f; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 12:f4331640e3ad 127
Floris_Hoek 14:1343966a79e8 128 static float max_rms0 = 0;
Floris_Hoek 14:1343966a79e8 129 static float max_rms1 = 0;
Floris_Hoek 14:1343966a79e8 130 static float max_rms2 = 0;
Floris_Hoek 14:1343966a79e8 131 static float max_rms3 = 0;
Floris_Hoek 14:1343966a79e8 132
Floris_Hoek 12:f4331640e3ad 133 static BiQuad Filter0(b0,b1,b2,a0,a1); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 12:f4331640e3ad 134 static BiQuad Filter1(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 135 static BiQuad Filter2(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 136 static BiQuad Filter3(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 137
Floris_Hoek 12:f4331640e3ad 138 float f_y0 = Filter0.step(shield0); // Apply filters on the different EMG signals
Floris_Hoek 12:f4331640e3ad 139 float f_y1 = Filter1.step(shield1);
Floris_Hoek 12:f4331640e3ad 140 float f_y2 = Filter2.step(shield2);
Floris_Hoek 12:f4331640e3ad 141 float f_y3 = Filter3.step(shield3);
Floris_Hoek 12:f4331640e3ad 142
Floris_Hoek 12:f4331640e3ad 143 int rms_length = 6; // Set the amount of points of which the RMS signal is calculated
Floris_Hoek 12:f4331640e3ad 144 static std::deque<float> deque_f_y0 (rms_length); // Create deque for the 4 signals in which data can be added and removed
Floris_Hoek 12:f4331640e3ad 145 static std::deque<float> deque_f_y1 (rms_length);
Floris_Hoek 12:f4331640e3ad 146 static std::deque<float> deque_f_y2 (rms_length);
Floris_Hoek 12:f4331640e3ad 147 static std::deque<float> deque_f_y3 (rms_length);
Floris_Hoek 12:f4331640e3ad 148
Floris_Hoek 12:f4331640e3ad 149 deque_f_y0.push_front(f_y0); // Take new data point and put it in front of the deque values
Floris_Hoek 12:f4331640e3ad 150 deque_f_y1.push_front(f_y1);
Floris_Hoek 12:f4331640e3ad 151 deque_f_y2.push_front(f_y2);
Floris_Hoek 12:f4331640e3ad 152 deque_f_y3.push_front(f_y3);
Floris_Hoek 12:f4331640e3ad 153
Floris_Hoek 12:f4331640e3ad 154 deque_f_y0.pop_back(); // Remove latest element in deque to keep the deque the same length
Floris_Hoek 12:f4331640e3ad 155 deque_f_y1.pop_back();
Floris_Hoek 12:f4331640e3ad 156 deque_f_y2.pop_back();
Floris_Hoek 12:f4331640e3ad 157 deque_f_y3.pop_back();
Floris_Hoek 12:f4331640e3ad 158
Floris_Hoek 12:f4331640e3ad 159 rms_0 = rms_deque(deque_f_y0); // Calculate the RMS for the different deques
Floris_Hoek 12:f4331640e3ad 160 rms_1 = rms_deque(deque_f_y1); // and give this value to rms_1 which is a reference
Floris_Hoek 12:f4331640e3ad 161 rms_2 = rms_deque(deque_f_y2); //
Floris_Hoek 12:f4331640e3ad 162 rms_3 = rms_deque(deque_f_y3);
Floris_Hoek 12:f4331640e3ad 163
Floris_Hoek 15:5f9450964075 164 if (MyState == EMG_CALIBRATION) {
Floris_Hoek 14:1343966a79e8 165
Floris_Hoek 15:5f9450964075 166 det_max(rms_0, max_rms0); // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
Floris_Hoek 14:1343966a79e8 167 det_max(rms_1, max_rms1);
Floris_Hoek 14:1343966a79e8 168 det_max(rms_2, max_rms2);
Floris_Hoek 14:1343966a79e8 169 det_max(rms_3, max_rms3);
Floris_Hoek 14:1343966a79e8 170
Floris_Hoek 14:1343966a79e8 171 }
Floris_Hoek 15:5f9450964075 172 else if ((int)MyState > 4) {
Floris_Hoek 15:5f9450964075 173 rms_0 = rms_0/max_rms0; // Normalise the RMS value by dividing by the maximum RMS value
Floris_Hoek 15:5f9450964075 174 rms_1 = rms_1/max_rms1; // This is done during the states with a value higher than 4, as this is when you start the playable game mode
Floris_Hoek 14:1343966a79e8 175 rms_2 = rms_2/max_rms2;
Floris_Hoek 14:1343966a79e8 176 rms_3 = rms_3/max_rms3;
Floris_Hoek 14:1343966a79e8 177 }
Floris_Hoek 12:f4331640e3ad 178
Floris_Hoek 10:8c38a1a5b522 179 }
Floris_Hoek 12:f4331640e3ad 180
Floris_Hoek 14:1343966a79e8 181 void det_max(float rms, float &max_rms) {
Floris_Hoek 15:5f9450964075 182 max_rms = max_rms < rms ? rms : max_rms; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
Floris_Hoek 14:1343966a79e8 183 }
Floris_Hoek 12:f4331640e3ad 184
Floris_Hoek 14:1343966a79e8 185 void emgcalibration() {
Floris_Hoek 14:1343966a79e8 186 float rms0, rms1, rms2, rms3; // RMS values of the different EMG signals
Floris_Hoek 14:1343966a79e8 187
Floris_Hoek 15:5f9450964075 188 measure_data(rms0, rms1, rms2, rms3); // Calculate RMS values
Floris_Hoek 14:1343966a79e8 189
Floris_Hoek 14:1343966a79e8 190 float duration = 10.0f; // Duration of the emgcalibration function, in this case 10 seconds
Floris_Hoek 14:1343966a79e8 191 int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
Floris_Hoek 14:1343966a79e8 192 // rounds is an integer so the value of duration / timeinterval is floored
Floris_Hoek 14:1343966a79e8 193
Floris_Hoek 14:1343966a79e8 194 static int counter = 0; // Counter which keeps track of the amount of times the function has executed
Floris_Hoek 14:1343966a79e8 195 if (counter >= rounds) {
Floris_Hoek 14:1343966a79e8 196 MyState = MOTOR_CALIBRATION; // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
Floris_Hoek 14:1343966a79e8 197 }
Floris_Hoek 14:1343966a79e8 198 else {
Floris_Hoek 14:1343966a79e8 199 counter++; // Else increase counter by 1
Floris_Hoek 14:1343966a79e8 200 }
Floris_Hoek 14:1343966a79e8 201
Floris_Hoek 14:1343966a79e8 202 }
Floris_Hoek 12:f4331640e3ad 203
paulstuiver 5:2ae500da8fe1 204 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 205 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 206 {
Floris_Hoek 8:7dab565a208e 207 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 208 //Proportional part:
paulstuiver 5:2ae500da8fe1 209 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 210
paulstuiver 5:2ae500da8fe1 211 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 212 return u_k;
paulstuiver 5:2ae500da8fe1 213 }
paulstuiver 2:75b2f713161c 214
Floris_Hoek 9:e8cc37a94fec 215 void New_State() {
Floris_Hoek 9:e8cc37a94fec 216 switch (MyState)
Floris_Hoek 9:e8cc37a94fec 217 {
Floris_Hoek 9:e8cc37a94fec 218 case MOTORS_OFF :
Floris_Hoek 9:e8cc37a94fec 219 pc.printf("State: Motors turned off");
Floris_Hoek 9:e8cc37a94fec 220 motorsoff();
Floris_Hoek 9:e8cc37a94fec 221 break;
Floris_Hoek 9:e8cc37a94fec 222
Floris_Hoek 9:e8cc37a94fec 223 case EMG_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 224 pc.printf("State: EMG Calibration");
Floris_Hoek 14:1343966a79e8 225 measurecontrol.attach(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 226 break;
Floris_Hoek 9:e8cc37a94fec 227
Floris_Hoek 9:e8cc37a94fec 228 case MOTOR_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 229 pc.printf("State: Motor Calibration");
Floris_Hoek 9:e8cc37a94fec 230 break;
Floris_Hoek 9:e8cc37a94fec 231
Floris_Hoek 9:e8cc37a94fec 232 case START_GAME :
Floris_Hoek 9:e8cc37a94fec 233 pc.printf("State: Start game");
Floris_Hoek 9:e8cc37a94fec 234 break;
Floris_Hoek 9:e8cc37a94fec 235
Floris_Hoek 9:e8cc37a94fec 236 case DEMO_MODE :
Floris_Hoek 9:e8cc37a94fec 237 pc.printf("State: Demo mode");
Floris_Hoek 9:e8cc37a94fec 238 break;
Floris_Hoek 9:e8cc37a94fec 239
Floris_Hoek 9:e8cc37a94fec 240 case GAME_MODE :
Floris_Hoek 9:e8cc37a94fec 241 pc.printf("State: Game mode");
Floris_Hoek 9:e8cc37a94fec 242 break;
Floris_Hoek 9:e8cc37a94fec 243
Floris_Hoek 9:e8cc37a94fec 244 case MOVE_END_EFFECTOR :
Floris_Hoek 9:e8cc37a94fec 245 pc.printf("State: Move end effector");
Floris_Hoek 9:e8cc37a94fec 246 break;
Floris_Hoek 9:e8cc37a94fec 247
Floris_Hoek 9:e8cc37a94fec 248 case MOVE_GRIPPER :
Floris_Hoek 9:e8cc37a94fec 249 pc.printf("State: Move the gripper");
Floris_Hoek 9:e8cc37a94fec 250 break;
Floris_Hoek 9:e8cc37a94fec 251
Floris_Hoek 9:e8cc37a94fec 252 case END_GAME :
Floris_Hoek 9:e8cc37a94fec 253 pc.printf("State: End of the game");
Floris_Hoek 9:e8cc37a94fec 254 break;
Floris_Hoek 9:e8cc37a94fec 255
Floris_Hoek 9:e8cc37a94fec 256 case FAILURE_MODE :
Floris_Hoek 9:e8cc37a94fec 257 pc.printf("FAILURE MODE!!"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 258 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 259 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 260 break;
Floris_Hoek 9:e8cc37a94fec 261
Floris_Hoek 9:e8cc37a94fec 262 default :
Floris_Hoek 9:e8cc37a94fec 263 pc.printf("Default state: Motors are turned off");
Floris_Hoek 10:8c38a1a5b522 264 sendtomotor(0.0f);
Floris_Hoek 9:e8cc37a94fec 265 break;
Floris_Hoek 9:e8cc37a94fec 266 }
Floris_Hoek 9:e8cc37a94fec 267 }
Floris_Hoek 9:e8cc37a94fec 268
Floris_Hoek 9:e8cc37a94fec 269 void failuremode() {
Floris_Hoek 12:f4331640e3ad 270 MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
Floris_Hoek 12:f4331640e3ad 271 New_State(); // Execute actions coupled to FAILURE_MODE
Floris_Hoek 14:1343966a79e8 272 }