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

Committer:
Floris_Hoek
Date:
Tue Oct 22 22:01:16 2019 +0000
Revision:
12:f4331640e3ad
Parent:
10:8c38a1a5b522
Child:
13:a243388e1790
Child:
14:1343966a79e8
'measure data' function added

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 9:e8cc37a94fec 44
Floris_Hoek 9:e8cc37a94fec 45
Floris_Hoek 9:e8cc37a94fec 46 void motorsoff() {
Floris_Hoek 12:f4331640e3ad 47 // 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 48 // 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 49
Floris_Hoek 12:f4331640e3ad 50 bool whileloop_boolean = true; // Boolean for the while loop
Floris_Hoek 12:f4331640e3ad 51 sendtomotor(0.0f); // Set motor velocity to 0
Floris_Hoek 12:f4331640e3ad 52
Floris_Hoek 12:f4331640e3ad 53 while (whileloop_boolean) {
Floris_Hoek 12:f4331640e3ad 54 if (button1.read() == 0) { // If button1 is pressed:
Floris_Hoek 12:f4331640e3ad 55 MyState = EMG_CALIBRATION; // set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 12:f4331640e3ad 56 whileloop_boolean = false; // by making whileloop_boolean equal to false
Floris_Hoek 9:e8cc37a94fec 57 }
Floris_Hoek 9:e8cc37a94fec 58 }
Floris_Hoek 9:e8cc37a94fec 59 }
Floris_Hoek 12:f4331640e3ad 60
Floris_Hoek 12:f4331640e3ad 61 float rms_deque(std::deque<float> deque) {
Floris_Hoek 12:f4331640e3ad 62 float sum = 0;
Floris_Hoek 12:f4331640e3ad 63 for (int i = 0; i < deque.size(); i++) {
Floris_Hoek 12:f4331640e3ad 64 sum = sum + pow(deque[i],2);
Floris_Hoek 12:f4331640e3ad 65 }
Floris_Hoek 12:f4331640e3ad 66 return pow(sum,0.5f);
Floris_Hoek 12:f4331640e3ad 67 }
Floris_Hoek 12:f4331640e3ad 68
Floris_Hoek 12:f4331640e3ad 69
Floris_Hoek 12:f4331640e3ad 70 void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3) {
Floris_Hoek 12:f4331640e3ad 71 float b0 = 0.0f; // Coefficients from the following formula:
Floris_Hoek 12:f4331640e3ad 72 float b1 = 0.0f; //
Floris_Hoek 12:f4331640e3ad 73 float b2 = 0.0f; // b0 + b1 z^-1 + b2 z^-2
Floris_Hoek 12:f4331640e3ad 74 float a0 = 0.0f; // H(z) = ----------------------
Floris_Hoek 12:f4331640e3ad 75 float a1 = 0.0f; // a0 + a1 z^-1 + a2 z^-2
Floris_Hoek 12:f4331640e3ad 76
Floris_Hoek 12:f4331640e3ad 77 static BiQuad Filter0(b0,b1,b2,a0,a1); // Create 4 equal filters used for the different EMG signals
Floris_Hoek 12:f4331640e3ad 78 static BiQuad Filter1(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 79 static BiQuad Filter2(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 80 static BiQuad Filter3(b0,b1,b2,a0,a1);
Floris_Hoek 12:f4331640e3ad 81
Floris_Hoek 12:f4331640e3ad 82 float f_y0 = Filter0.step(shield0); // Apply filters on the different EMG signals
Floris_Hoek 12:f4331640e3ad 83 float f_y1 = Filter1.step(shield1);
Floris_Hoek 12:f4331640e3ad 84 float f_y2 = Filter2.step(shield2);
Floris_Hoek 12:f4331640e3ad 85 float f_y3 = Filter3.step(shield3);
Floris_Hoek 12:f4331640e3ad 86
Floris_Hoek 12:f4331640e3ad 87 int rms_length = 6; // Set the amount of points of which the RMS signal is calculated
Floris_Hoek 12:f4331640e3ad 88 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 89 static std::deque<float> deque_f_y1 (rms_length);
Floris_Hoek 12:f4331640e3ad 90 static std::deque<float> deque_f_y2 (rms_length);
Floris_Hoek 12:f4331640e3ad 91 static std::deque<float> deque_f_y3 (rms_length);
Floris_Hoek 12:f4331640e3ad 92
Floris_Hoek 12:f4331640e3ad 93 deque_f_y0.push_front(f_y0); // Take new data point and put it in front of the deque values
Floris_Hoek 12:f4331640e3ad 94 deque_f_y1.push_front(f_y1);
Floris_Hoek 12:f4331640e3ad 95 deque_f_y2.push_front(f_y2);
Floris_Hoek 12:f4331640e3ad 96 deque_f_y3.push_front(f_y3);
Floris_Hoek 12:f4331640e3ad 97
Floris_Hoek 12:f4331640e3ad 98 deque_f_y0.pop_back(); // Remove latest element in deque to keep the deque the same length
Floris_Hoek 12:f4331640e3ad 99 deque_f_y1.pop_back();
Floris_Hoek 12:f4331640e3ad 100 deque_f_y2.pop_back();
Floris_Hoek 12:f4331640e3ad 101 deque_f_y3.pop_back();
Floris_Hoek 12:f4331640e3ad 102
Floris_Hoek 12:f4331640e3ad 103 rms_0 = rms_deque(deque_f_y0); // Calculate the RMS for the different deques
Floris_Hoek 12:f4331640e3ad 104 rms_1 = rms_deque(deque_f_y1); // and give this value to rms_1 which is a reference
Floris_Hoek 12:f4331640e3ad 105 rms_2 = rms_deque(deque_f_y2); //
Floris_Hoek 12:f4331640e3ad 106 rms_3 = rms_deque(deque_f_y3);
Floris_Hoek 12:f4331640e3ad 107
Floris_Hoek 12:f4331640e3ad 108 }
Floris_Hoek 12:f4331640e3ad 109
Floris_Hoek 12:f4331640e3ad 110
Floris_Hoek 10:8c38a1a5b522 111 void emgcalibration() {
Floris_Hoek 12:f4331640e3ad 112 float rms0, rms1, rms2, rms3;
Floris_Hoek 12:f4331640e3ad 113 measure_data(rms0, rms1, rms2, rms3);
Floris_Hoek 12:f4331640e3ad 114
Floris_Hoek 10:8c38a1a5b522 115 }
Floris_Hoek 12:f4331640e3ad 116
Floris_Hoek 12:f4331640e3ad 117
Floris_Hoek 12:f4331640e3ad 118
paulstuiver 5:2ae500da8fe1 119 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 120 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 121 {
Floris_Hoek 8:7dab565a208e 122 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 123 //Proportional part:
paulstuiver 5:2ae500da8fe1 124 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 125
paulstuiver 5:2ae500da8fe1 126 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 127 return u_k;
paulstuiver 5:2ae500da8fe1 128 }
paulstuiver 2:75b2f713161c 129
Floris_Hoek 8:7dab565a208e 130 void nothing(){// Do nothing
paulstuiver 2:75b2f713161c 131 }
paulstuiver 2:75b2f713161c 132
Floris_Hoek 9:e8cc37a94fec 133
Floris_Hoek 9:e8cc37a94fec 134 void New_State() {
Floris_Hoek 9:e8cc37a94fec 135 switch (MyState)
Floris_Hoek 9:e8cc37a94fec 136 {
Floris_Hoek 9:e8cc37a94fec 137 case MOTORS_OFF :
Floris_Hoek 9:e8cc37a94fec 138 pc.printf("State: Motors turned off");
Floris_Hoek 9:e8cc37a94fec 139 motorsoff();
Floris_Hoek 9:e8cc37a94fec 140 break;
Floris_Hoek 9:e8cc37a94fec 141
Floris_Hoek 9:e8cc37a94fec 142 case EMG_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 143 pc.printf("State: EMG Calibration");
Floris_Hoek 10:8c38a1a5b522 144 //measureandcontrol(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 145 break;
Floris_Hoek 9:e8cc37a94fec 146
Floris_Hoek 9:e8cc37a94fec 147 case MOTOR_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 148 pc.printf("State: Motor Calibration");
Floris_Hoek 9:e8cc37a94fec 149 break;
Floris_Hoek 9:e8cc37a94fec 150
Floris_Hoek 9:e8cc37a94fec 151 case START_GAME :
Floris_Hoek 9:e8cc37a94fec 152 pc.printf("State: Start game");
Floris_Hoek 9:e8cc37a94fec 153 break;
Floris_Hoek 9:e8cc37a94fec 154
Floris_Hoek 9:e8cc37a94fec 155 case DEMO_MODE :
Floris_Hoek 9:e8cc37a94fec 156 pc.printf("State: Demo mode");
Floris_Hoek 9:e8cc37a94fec 157 break;
Floris_Hoek 9:e8cc37a94fec 158
Floris_Hoek 9:e8cc37a94fec 159 case GAME_MODE :
Floris_Hoek 9:e8cc37a94fec 160 pc.printf("State: Game mode");
Floris_Hoek 9:e8cc37a94fec 161 break;
Floris_Hoek 9:e8cc37a94fec 162
Floris_Hoek 9:e8cc37a94fec 163 case MOVE_END_EFFECTOR :
Floris_Hoek 9:e8cc37a94fec 164 pc.printf("State: Move end effector");
Floris_Hoek 9:e8cc37a94fec 165 break;
Floris_Hoek 9:e8cc37a94fec 166
Floris_Hoek 9:e8cc37a94fec 167 case MOVE_GRIPPER :
Floris_Hoek 9:e8cc37a94fec 168 pc.printf("State: Move the gripper");
Floris_Hoek 9:e8cc37a94fec 169 break;
Floris_Hoek 9:e8cc37a94fec 170
Floris_Hoek 9:e8cc37a94fec 171 case END_GAME :
Floris_Hoek 9:e8cc37a94fec 172 pc.printf("State: End of the game");
Floris_Hoek 9:e8cc37a94fec 173 break;
Floris_Hoek 9:e8cc37a94fec 174
Floris_Hoek 9:e8cc37a94fec 175 case FAILURE_MODE :
Floris_Hoek 9:e8cc37a94fec 176 pc.printf("FAILURE MODE!!"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 177 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 178 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 179 break;
Floris_Hoek 9:e8cc37a94fec 180
Floris_Hoek 9:e8cc37a94fec 181 default :
Floris_Hoek 9:e8cc37a94fec 182 pc.printf("Default state: Motors are turned off");
Floris_Hoek 10:8c38a1a5b522 183 sendtomotor(0.0f);
Floris_Hoek 9:e8cc37a94fec 184 break;
Floris_Hoek 9:e8cc37a94fec 185 }
Floris_Hoek 9:e8cc37a94fec 186 }
Floris_Hoek 9:e8cc37a94fec 187
Floris_Hoek 9:e8cc37a94fec 188 void failuremode() {
Floris_Hoek 12:f4331640e3ad 189 MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
Floris_Hoek 12:f4331640e3ad 190 New_State(); // Execute actions coupled to FAILURE_MODE
Floris_Hoek 9:e8cc37a94fec 191 }
Floris_Hoek 9:e8cc37a94fec 192
RobertoO 0:67c50348f842 193 int main()
RobertoO 0:67c50348f842 194 {
Floris_Hoek 9:e8cc37a94fec 195 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 12:f4331640e3ad 196 double frequency = 17000.0f; // Set motorfrequency
Floris_Hoek 12:f4331640e3ad 197 double period_signal = 1.0f/frequency; // Convert to period of the signal
RobertoO 0:67c50348f842 198 pc.baud(115200);
Floris_Hoek 9:e8cc37a94fec 199
Floris_Hoek 12:f4331640e3ad 200 button2.fall(failuremode); // Function is always activated when the button is pressed
Floris_Hoek 12:f4331640e3ad 201 motor1Velocity.period(period_signal); // Set the period of the PWMfunction
Floris_Hoek 12:f4331640e3ad 202 measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
Floris_Hoek 9:e8cc37a94fec 203
Floris_Hoek 12:f4331640e3ad 204 int previous_state_int = (int)MyState; // Save previous state to compare with current state and possibly execute New_State()
Floris_Hoek 12:f4331640e3ad 205 // in the while loop
Floris_Hoek 12:f4331640e3ad 206 New_State(); // Execute the functions belonging to the current state
Floris_Hoek 9:e8cc37a94fec 207
Floris_Hoek 9:e8cc37a94fec 208 while(whileloop)
paulstuiver 2:75b2f713161c 209 {
Floris_Hoek 12:f4331640e3ad 210 if ( (previous_state_int - (int)MyState) != 0 ) { // If current state is not previous state execute New_State()
Floris_Hoek 9:e8cc37a94fec 211 New_State();
Floris_Hoek 9:e8cc37a94fec 212 }
Floris_Hoek 12:f4331640e3ad 213 previous_state_int = (int)MyState; // Change previous state to current state
RobertoO 0:67c50348f842 214 }
Floris_Hoek 12:f4331640e3ad 215 // While has stopped running
Floris_Hoek 12:f4331640e3ad 216 pc.printf("Programm stops running\r\n"); // So show that the programm is quiting
Floris_Hoek 12:f4331640e3ad 217 sendtomotor(0.0f); // Set the motor velocity to 0
Floris_Hoek 12:f4331640e3ad 218 measurecontrol.attach(nothing,10000); // Attach empty function to Ticker (?? Appropriate solution ??)
Floris_Hoek 8:7dab565a208e 219 return 0;
Floris_Hoek 8:7dab565a208e 220 }