mevlid yildirim / Mbed 2 deprecated BioRobotics_BMT

Minor BioRobotics BMT Hierbij publish ik mijn code public ter inspiratie voor komende jaarlagen. Het gaat om een serial robot met twee links en een haak als end-effector. Veel plezier ermee!

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@20:682ac5b520a0, 2019-10-30 (annotated)

Committer:
fb07
Date:
Wed Oct 30 18:36:52 2019 +0000
Revision:
20:682ac5b520a0
Parent:
19:483fc61778f0
Child:
21:0d86a17f9974
Works anyway, cable was broken

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fb07 2:45a85caaebfb 1 // Project BioRobotics - Opening a Door - Group 13 2019/2020
fb07 2:45a85caaebfb 2 // Dion ten Berge - s1864734
fb07 2:45a85caaebfb 3 // Bas Rutteman - s1854305
fb07 2:45a85caaebfb 4 // Nick in het Veld - s1915584
fb07 2:45a85caaebfb 5 // Marleen van der Weij - s1800078
fb07 2:45a85caaebfb 6 // Mevlid Yildirim - s2005735
fb07 2:45a85caaebfb 7
fb07 5:7e2c6d2235fe 8 /* To-Do
fb07 5:7e2c6d2235fe 9 1. Kd, Ki, Kp waardes bepalen
fb07 5:7e2c6d2235fe 10 2. Filter cutoff frequentie bepalen, zie https://github.com/tomlankhorst/biquad
fb07 5:7e2c6d2235fe 11 3. Grenswaarde EMG signaal na het filteren
fb07 5:7e2c6d2235fe 12 */
fb07 2:45a85caaebfb 13
fb07 2:45a85caaebfb 14 //*****************************************************************************
fb07 2:45a85caaebfb 15 // 1. Libraries ******************************************************************
fb07 2:45a85caaebfb 16 //*****************************************************************************
RobertoO 0:67c50348f842 17 #include "mbed.h"
fb07 2:45a85caaebfb 18 #include "HIDScope.h"
fb07 2:45a85caaebfb 19 #include "QEI.h"
RobertoO 1:b862262a9d14 20 #include "MODSERIAL.h"
fb07 2:45a85caaebfb 21 #include "BiQuad.h"
fb07 2:45a85caaebfb 22 #include "FastPWM.h"
fb07 2:45a85caaebfb 23
fb07 2:45a85caaebfb 24 //*****************************************************************************
fb07 2:45a85caaebfb 25 // 2. States ******************************************************************
fb07 2:45a85caaebfb 26 //*****************************************************************************
fb07 12:f5dc65f1c27b 27 enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Demo, Operating, EmergencyMode, Idle}; //All robot states
fb07 12:f5dc65f1c27b 28 States State;
RobertoO 0:67c50348f842 29
fb07 2:45a85caaebfb 30 //*****************************************************************************
fb07 5:7e2c6d2235fe 31 // 3. (Global) Variables ***********************************************************
fb07 2:45a85caaebfb 32 //*****************************************************************************
fb07 5:7e2c6d2235fe 33 // 3.1 Tickers *****************************************************************
fb07 5:7e2c6d2235fe 34 Ticker ticker_mainloop; // The ticker which runs the mainloop
fb07 5:7e2c6d2235fe 35 Ticker ticker_hidscope; // The ticker which sends data to the HIDScope server
fb07 5:7e2c6d2235fe 36
fb07 5:7e2c6d2235fe 37 // 3.2 General variables *******************************************************
fb07 5:7e2c6d2235fe 38
fb07 5:7e2c6d2235fe 39 MODSERIAL pc(USBTX, USBRX); // Serial communication with the board
fb07 5:7e2c6d2235fe 40 QEI encoder_motor1(D12,D13,NC,64); // Defines encoder for motor 1
fb07 5:7e2c6d2235fe 41 QEI encoder_motor2(D10,D11,NC,64); // Defines encoder for motor 1
fb07 14:236ae2d7ec41 42 double f=1/100; // Frequency, currently unused
fb07 14:236ae2d7ec41 43 const double Ts = 0.001; // Sampletime
fb07 5:7e2c6d2235fe 44 HIDScope scope(2); // Amount of HIDScope servers
fb07 10:a60b369c1711 45
fb07 5:7e2c6d2235fe 46
fb07 17:f87e5d6c87f4 47 // 3.3 EMG Variables **********************************************************
fb07 18:8bfc1821d412 48 static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 18:8bfc1821d412 49 static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 18:8bfc1821d412 50
fb07 17:f87e5d6c87f4 51 static BiQuad highfilter0(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
fb07 17:f87e5d6c87f4 52 static BiQuad LowPassFilter0( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 17:f87e5d6c87f4 53
fb07 17:f87e5d6c87f4 54 static BiQuad highfilter1(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
fb07 17:f87e5d6c87f4 55 static BiQuad LowPassFilter1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 17:f87e5d6c87f4 56
fb07 17:f87e5d6c87f4 57 static BiQuad highfilter2(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
fb07 17:f87e5d6c87f4 58 static BiQuad LowPassFilter2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 17:f87e5d6c87f4 59
fb07 17:f87e5d6c87f4 60 static BiQuad highfilter3(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
fb07 17:f87e5d6c87f4 61 static BiQuad LowPassFilter3( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
fb07 17:f87e5d6c87f4 62
fb07 17:f87e5d6c87f4 63 double emg0_raw_signal ; double emg1_raw_signal ; double emg2_raw_signal ; double emg3_raw_signal ;
fb07 17:f87e5d6c87f4 64 double high_emg0_signal ; double high_emg1_signal ; double high_emg2_signal ; double high_emg3_signal ;
fb07 17:f87e5d6c87f4 65 double rec_emg0_signal ; double rec_emg1_signal ; double rec_emg2_signal ; double rec_emg3_signal ;
fb07 17:f87e5d6c87f4 66 double low_rec_high_emg0_signal ; double low_rec_high_emg1_signal ; double low_rec_high_emg2_signal ; double low_rec_high_emg3_signal ;
fb07 17:f87e5d6c87f4 67 double emg0_signal ; double emg1_signal ; double emg2_signal ; double emg3_signal ;
fb07 18:8bfc1821d412 68
fb07 18:8bfc1821d412 69
RobertoO 0:67c50348f842 70
fb07 5:7e2c6d2235fe 71 // 3.4 Hardware ***************************************************************
fb07 5:7e2c6d2235fe 72 //3.4a Leds
fb07 5:7e2c6d2235fe 73 DigitalOut led_red(LED_RED); // Defines the red led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 74 DigitalOut led_green(LED_GREEN); // Defines the green led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 75 DigitalOut led_blue(LED_BLUE); // Defines the blue led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 76 // FastPWM led1(D8); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 77 FastPWM led2(D9); //Defines Led2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 78
fb07 5:7e2c6d2235fe 79 //3.4b Potmeters and buttons
fb07 5:7e2c6d2235fe 80 AnalogIn pot1_links(A5); //Defines potmeter1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 81 AnalogIn pot2_rechts(A4); //Defines potmeter2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 82 DigitalIn button1(D2); //Defines button1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 83 DigitalIn button2(D3); //Defines button2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 84 DigitalIn sw2(SW2); //Defines button SW2 on the K64 board
fb07 5:7e2c6d2235fe 85 DigitalIn sw3(SW3); //Defines button SW3 on the K64 board
fb07 5:7e2c6d2235fe 86
fb07 5:7e2c6d2235fe 87 //3.4c Motors
fb07 5:7e2c6d2235fe 88 DigitalOut motor1DirectionPin(D7); // motor 1 direction control (1=cw, 0=ccw)
fb07 5:7e2c6d2235fe 89 FastPWM motor1(D6); // motor 1 velocity control (between 0-1)
fb07 5:7e2c6d2235fe 90 FastPWM motor2(D5); // motor 2 velocity control (between 0-1)
fb07 5:7e2c6d2235fe 91 DigitalOut motor2DirectionPin(D4); // motor 2 direction control (1=cw, 0=ccw)
fb07 11:a3fd9d5144bb 92 bool motor1_calibrated=false;
fb07 11:a3fd9d5144bb 93 bool motor2_calibrated=false;
fb07 5:7e2c6d2235fe 94
fb07 17:f87e5d6c87f4 95 //3.4d EMGs
fb07 17:f87e5d6c87f4 96 AnalogIn emg0(A0); //Rechterarm
fb07 17:f87e5d6c87f4 97 AnalogIn emg1(A1); //Linkerarm
fb07 17:f87e5d6c87f4 98 AnalogIn emg2(A2); //Rechterbeen
fb07 17:f87e5d6c87f4 99 AnalogIn emg3(A3); //Linkerbeen
fb07 17:f87e5d6c87f4 100
fb07 18:8bfc1821d412 101 double emg0_max=0.0;
fb07 18:8bfc1821d412 102 int emg_tijd=0;
fb07 18:8bfc1821d412 103 int homing_tijd=0;
fb07 10:a60b369c1711 104 // 3.5 Motor 1 variables ***************************************************************
fb07 5:7e2c6d2235fe 105 //3.5a PID-controller motor 1
fb07 5:7e2c6d2235fe 106 double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
fb07 5:7e2c6d2235fe 107 static double error_integral_motor1 = 0;
fb07 5:7e2c6d2235fe 108 double Yref_motor1;
fb07 5:7e2c6d2235fe 109 double kp_motor1;
fb07 5:7e2c6d2235fe 110 double Ki_motor1;
fb07 5:7e2c6d2235fe 111 double Kd_motor1;
fb07 14:236ae2d7ec41 112 double Up_motor1;
fb07 14:236ae2d7ec41 113 double Ui_motor1;
fb07 14:236ae2d7ec41 114 double Ud_motor1;
fb07 14:236ae2d7ec41 115
fb07 5:7e2c6d2235fe 116 double positie_motor1; //counts encoder
fb07 5:7e2c6d2235fe 117 double error1_motor1;
fb07 5:7e2c6d2235fe 118 double error1_prev_motor1;
fb07 5:7e2c6d2235fe 119 double error1_derivative_motor1;
fb07 5:7e2c6d2235fe 120 double error1_derivative_filtered_motor1;
fb07 5:7e2c6d2235fe 121 double P_motor1;
fb07 5:7e2c6d2235fe 122
fb07 10:a60b369c1711 123 double positie_verschil_motor1;
fb07 10:a60b369c1711 124 double positie_prev_motor1;
fb07 10:a60b369c1711 125
fb07 10:a60b369c1711 126 // 3.5 Motor 2 variables ***************************************************************
fb07 5:7e2c6d2235fe 127 //3.5b PID-controller motor 2
fb07 5:7e2c6d2235fe 128 double counts_per_rad_motor2 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
fb07 5:7e2c6d2235fe 129 static double error_integral_motor2 = 0;
fb07 5:7e2c6d2235fe 130 double Yref_motor2;
fb07 5:7e2c6d2235fe 131 double kp_motor2;
fb07 5:7e2c6d2235fe 132 double Ki_motor2;
fb07 5:7e2c6d2235fe 133 double Kd_motor2;
fb07 15:849e0fc5d3a8 134 double Up_motor2;
fb07 15:849e0fc5d3a8 135 double Ui_motor2;
fb07 15:849e0fc5d3a8 136 double Ud_motor2;
fb07 2:45a85caaebfb 137
fb07 5:7e2c6d2235fe 138 double positie_motor2; //counts encoder
fb07 5:7e2c6d2235fe 139 double error1_motor2;
fb07 5:7e2c6d2235fe 140 double error1_prev_motor2;
fb07 5:7e2c6d2235fe 141 double error1_derivative_motor2;
fb07 5:7e2c6d2235fe 142 double error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 143 double P_motor2;
fb07 10:a60b369c1711 144
fb07 10:a60b369c1711 145 double positie_verschil_motor2;
fb07 10:a60b369c1711 146 double positie_prev_motor2;
RobertoO 0:67c50348f842 147
fb07 2:45a85caaebfb 148 //******************************************************************************
fb07 5:7e2c6d2235fe 149 // 4. Functions ****************************************************************
fb07 5:7e2c6d2235fe 150 //******************************************************************************
fb07 5:7e2c6d2235fe 151
fb07 5:7e2c6d2235fe 152 // 4.1 Hidscope ****************************************************************
fb07 5:7e2c6d2235fe 153 void HIDScope() //voor HIDscope
fb07 5:7e2c6d2235fe 154 {
fb07 19:483fc61778f0 155 scope.set(0, 1);
fb07 19:483fc61778f0 156 // scope.set(1, error1_motor1);
fb07 19:483fc61778f0 157 // scope.set(2, emg_tijd);
fb07 18:8bfc1821d412 158 // scope.set(3, emg3_signal);
fb07 14:236ae2d7ec41 159 // scope.set(4, Ui_motor1);
fb07 14:236ae2d7ec41 160 // scope.set(5, Uk_motor1);
fb07 14:236ae2d7ec41 161
fb07 5:7e2c6d2235fe 162 scope.send();
fb07 5:7e2c6d2235fe 163 }
fb07 15:849e0fc5d3a8 164
fb07 10:a60b369c1711 165 // 4.x Encoder motor1 ****************************************************************
fb07 9:c4fa72ffa1c2 166 double fencoder_motor1() // bepaalt de positie van de motor
fb07 9:c4fa72ffa1c2 167 {
fb07 9:c4fa72ffa1c2 168 positie_motor1 = encoder_motor1.getPulses(); // haalt encoder waardes op
fb07 9:c4fa72ffa1c2 169 positie_verschil_motor1 = (positie_motor1-positie_prev_motor1)/Ts;
fb07 9:c4fa72ffa1c2 170 positie_prev_motor1 = positie_motor1;
fb07 9:c4fa72ffa1c2 171
fb07 9:c4fa72ffa1c2 172 return positie_motor1; //geeft positie van motor
fb07 9:c4fa72ffa1c2 173 }
fb07 10:a60b369c1711 174 // 4.x Encoder motor2 ****************************************************************
fb07 10:a60b369c1711 175 double fencoder_motor2() // bepaalt de positie van de motor
fb07 10:a60b369c1711 176 {
fb07 10:a60b369c1711 177 positie_motor2 = encoder_motor2.getPulses(); // haalt encoder waardes op
fb07 10:a60b369c1711 178 positie_verschil_motor2 = (positie_motor2-positie_prev_motor2)/Ts;
fb07 10:a60b369c1711 179 positie_prev_motor2 = positie_motor2;
fb07 10:a60b369c1711 180
fb07 10:a60b369c1711 181 return positie_motor2; //geeft positie van motor
fb07 10:a60b369c1711 182 }
fb07 10:a60b369c1711 183
fb07 11:a3fd9d5144bb 184 // 4.xa Calibration motors
fb07 11:a3fd9d5144bb 185
fb07 11:a3fd9d5144bb 186 void motor_calibration()
fb07 10:a60b369c1711 187 {
fb07 11:a3fd9d5144bb 188 // Calibration motor 2
fb07 10:a60b369c1711 189 motor1DirectionPin=0; //direction of the motor
fb07 10:a60b369c1711 190 motor1=1.0;
fb07 13:db1a8b51706b 191 wait(0.1);
fb07 10:a60b369c1711 192 while (abs(positie_verschil_motor1)>5)
fb07 10:a60b369c1711 193 {
fb07 10:a60b369c1711 194 motor1=0.2 ;
fb07 14:236ae2d7ec41 195 // pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
fb07 10:a60b369c1711 196 }
fb07 10:a60b369c1711 197 motor1=0.0;
fb07 11:a3fd9d5144bb 198 motor1_calibrated=true;
fb07 14:236ae2d7ec41 199 // pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
fb07 10:a60b369c1711 200
fb07 11:a3fd9d5144bb 201
fb07 10:a60b369c1711 202
fb07 11:a3fd9d5144bb 203 // Calibration motor 2
fb07 10:a60b369c1711 204 motor2DirectionPin=0; //direction of the motor
fb07 10:a60b369c1711 205 motor2=1.0;
fb07 13:db1a8b51706b 206 wait(0.1);
fb07 10:a60b369c1711 207 while (abs(positie_verschil_motor2)>5)
fb07 10:a60b369c1711 208 {
fb07 10:a60b369c1711 209 motor2=0.2 ;
fb07 14:236ae2d7ec41 210 // pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
fb07 13:db1a8b51706b 211 led2=1;
fb07 10:a60b369c1711 212 }
fb07 10:a60b369c1711 213 motor2=0.0;
fb07 13:db1a8b51706b 214 led2=0;
fb07 11:a3fd9d5144bb 215 motor2_calibrated=true;
fb07 14:236ae2d7ec41 216 // pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
fb07 13:db1a8b51706b 217
fb07 12:f5dc65f1c27b 218
fb07 10:a60b369c1711 219 }
fb07 10:a60b369c1711 220
fb07 5:7e2c6d2235fe 221 // 4.2a PID-Controller motor 1**************************************************
fb07 14:236ae2d7ec41 222 double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
fb07 5:7e2c6d2235fe 223 {
fb07 5:7e2c6d2235fe 224 //Proportional part
fb07 5:7e2c6d2235fe 225 kp_motor1 = 0.01 ; // moet nog getweaked worden
fb07 14:236ae2d7ec41 226 Up_motor1 = kp_motor1 * error1_motor1;
fb07 5:7e2c6d2235fe 227
fb07 5:7e2c6d2235fe 228 //Integral part
fb07 15:849e0fc5d3a8 229 Ki_motor1 = 0.001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 230 error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1); // integrale fout + (de sample tijd * fout)
fb07 14:236ae2d7ec41 231 Ui_motor1 = Ki_motor1 * error_integral_motor1; // (fout * integrale fout)
fb07 5:7e2c6d2235fe 232
fb07 5:7e2c6d2235fe 233 //Derivative part
fb07 15:849e0fc5d3a8 234 Kd_motor1 = 0.001 ;// moet nog getweaked worden
fb07 14:236ae2d7ec41 235 error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
fb07 18:8bfc1821d412 236 error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
fb07 14:236ae2d7ec41 237 Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1; // (afgeleide gain) * (afgeleide gefilterde fout)
fb07 5:7e2c6d2235fe 238 error1_prev_motor1 = error1_motor1;
fb07 5:7e2c6d2235fe 239
fb07 14:236ae2d7ec41 240 P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1; //sommatie van de u's
fb07 14:236ae2d7ec41 241
fb07 14:236ae2d7ec41 242 return P_motor1;
fb07 14:236ae2d7ec41 243
fb07 5:7e2c6d2235fe 244
fb07 5:7e2c6d2235fe 245 }
fb07 5:7e2c6d2235fe 246
fb07 5:7e2c6d2235fe 247 // 4.2b PID-Controller motor 2**************************************************
fb07 15:849e0fc5d3a8 248 double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
fb07 5:7e2c6d2235fe 249 {
fb07 5:7e2c6d2235fe 250 //Proportional part
fb07 5:7e2c6d2235fe 251 kp_motor2 = 0.01 ; // moet nog getweaked worden
fb07 15:849e0fc5d3a8 252 Up_motor2 = kp_motor2 * error1_motor2;
fb07 5:7e2c6d2235fe 253
fb07 5:7e2c6d2235fe 254 //Integral part
fb07 15:849e0fc5d3a8 255 Ki_motor2 = 0.001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 256 error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2); // integrale fout + (de sample tijd * fout)
fb07 15:849e0fc5d3a8 257 Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
fb07 5:7e2c6d2235fe 258
fb07 5:7e2c6d2235fe 259 //Derivative part
fb07 15:849e0fc5d3a8 260 Kd_motor2 = 0.001 ;// moet nog getweaked worden
fb07 5:7e2c6d2235fe 261 error1_derivative_motor2 = (error1_motor2 - error1_prev_motor2)/Ts;
fb07 18:8bfc1821d412 262 error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
fb07 15:849e0fc5d3a8 263 Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 264 error1_prev_motor2 = error1_motor2;
fb07 5:7e2c6d2235fe 265
fb07 15:849e0fc5d3a8 266 P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
fb07 5:7e2c6d2235fe 267
fb07 5:7e2c6d2235fe 268 return P_motor2;
fb07 5:7e2c6d2235fe 269 }
fb07 10:a60b369c1711 270
fb07 14:236ae2d7ec41 271 double motor1_pwm()
fb07 14:236ae2d7ec41 272 {
fb07 14:236ae2d7ec41 273
fb07 14:236ae2d7ec41 274 if (P_motor1 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
fb07 14:236ae2d7ec41 275 {
fb07 14:236ae2d7ec41 276 motor1DirectionPin=1; // Clockwise rotation
fb07 14:236ae2d7ec41 277 }
fb07 14:236ae2d7ec41 278 else
fb07 14:236ae2d7ec41 279 {
fb07 14:236ae2d7ec41 280 motor1DirectionPin=0; // Counterclockwise rotation
fb07 14:236ae2d7ec41 281 }
fb07 14:236ae2d7ec41 282
fb07 14:236ae2d7ec41 283 if (fabs(P_motor1) > 0.99 ) // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
fb07 14:236ae2d7ec41 284 {
fb07 14:236ae2d7ec41 285 motor1 = 0.99 ;
fb07 14:236ae2d7ec41 286 }
fb07 14:236ae2d7ec41 287 else
fb07 14:236ae2d7ec41 288 {
fb07 14:236ae2d7ec41 289 motor1 = fabs(P_motor1);
fb07 14:236ae2d7ec41 290 }
fb07 15:849e0fc5d3a8 291 }
fb07 15:849e0fc5d3a8 292 double motor2_pwm()
fb07 15:849e0fc5d3a8 293 {
fb07 15:849e0fc5d3a8 294
fb07 15:849e0fc5d3a8 295 if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
fb07 15:849e0fc5d3a8 296 {
fb07 15:849e0fc5d3a8 297 motor2DirectionPin=2; // Clockwise rotation
fb07 15:849e0fc5d3a8 298 }
fb07 15:849e0fc5d3a8 299 else
fb07 15:849e0fc5d3a8 300 {
fb07 15:849e0fc5d3a8 301 motor2DirectionPin=0; // Counterclockwise rotation
fb07 15:849e0fc5d3a8 302 }
fb07 14:236ae2d7ec41 303
fb07 15:849e0fc5d3a8 304 if (fabs(P_motor2) > 0.99 ) // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
fb07 15:849e0fc5d3a8 305 {
fb07 15:849e0fc5d3a8 306 motor2 = 0.99 ;
fb07 15:849e0fc5d3a8 307 }
fb07 15:849e0fc5d3a8 308 else
fb07 15:849e0fc5d3a8 309 {
fb07 15:849e0fc5d3a8 310 motor2 = fabs(P_motor2);
fb07 15:849e0fc5d3a8 311 }
fb07 14:236ae2d7ec41 312 }
fb07 14:236ae2d7ec41 313
fb07 14:236ae2d7ec41 314 void motor1_controller(void)
fb07 14:236ae2d7ec41 315 {
fb07 14:236ae2d7ec41 316 error1_motor1 = (Yref_motor1 - positie_motor1);
fb07 19:483fc61778f0 317 motor1_pwm();
fb07 19:483fc61778f0 318 /*if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 16:1be144329f05 319 {
fb07 16:1be144329f05 320 motor1_pwm();
fb07 19:483fc61778f0 321 } */
fb07 16:1be144329f05 322
fb07 14:236ae2d7ec41 323 }
fb07 14:236ae2d7ec41 324
fb07 15:849e0fc5d3a8 325 void motor2_controller(void)
fb07 15:849e0fc5d3a8 326 {
fb07 15:849e0fc5d3a8 327 error1_motor2 = (Yref_motor2 - positie_motor2);
fb07 19:483fc61778f0 328 motor2_pwm();
fb07 19:483fc61778f0 329 /* if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 16:1be144329f05 330 {
fb07 16:1be144329f05 331 motor2_pwm();
fb07 19:483fc61778f0 332 } */
fb07 17:f87e5d6c87f4 333 }
fb07 17:f87e5d6c87f4 334
fb07 17:f87e5d6c87f4 335 void emg0_processing()
fb07 17:f87e5d6c87f4 336 {
fb07 17:f87e5d6c87f4 337 emg0_raw_signal=emg0.read();
fb07 17:f87e5d6c87f4 338
fb07 17:f87e5d6c87f4 339 high_emg0_signal = highfilter0.step(emg0_raw_signal);
fb07 17:f87e5d6c87f4 340 rec_emg0_signal = abs(high_emg0_signal);
fb07 17:f87e5d6c87f4 341 low_rec_high_emg0_signal = LowPassFilter0.step(rec_emg0_signal);
fb07 18:8bfc1821d412 342 emg0_signal = low_rec_high_emg0_signal;
fb07 17:f87e5d6c87f4 343
fb07 17:f87e5d6c87f4 344 }
fb07 17:f87e5d6c87f4 345
fb07 17:f87e5d6c87f4 346 void emg1_processing()
fb07 17:f87e5d6c87f4 347 {
fb07 17:f87e5d6c87f4 348 emg1_raw_signal=emg1.read();
fb07 17:f87e5d6c87f4 349
fb07 17:f87e5d6c87f4 350 high_emg1_signal = highfilter1.step(emg1_raw_signal);
fb07 17:f87e5d6c87f4 351 rec_emg1_signal = abs(high_emg1_signal);
fb07 17:f87e5d6c87f4 352 low_rec_high_emg1_signal = LowPassFilter1.step(rec_emg1_signal);
fb07 17:f87e5d6c87f4 353 emg1_signal = low_rec_high_emg1_signal;
fb07 17:f87e5d6c87f4 354
fb07 17:f87e5d6c87f4 355 }
fb07 17:f87e5d6c87f4 356
fb07 17:f87e5d6c87f4 357 void emg2_processing()
fb07 17:f87e5d6c87f4 358 {
fb07 17:f87e5d6c87f4 359 emg2_raw_signal=emg2.read();
fb07 17:f87e5d6c87f4 360
fb07 17:f87e5d6c87f4 361 high_emg2_signal = highfilter2.step(emg2_raw_signal);
fb07 17:f87e5d6c87f4 362 rec_emg2_signal = abs(high_emg2_signal);
fb07 17:f87e5d6c87f4 363 low_rec_high_emg2_signal = LowPassFilter2.step(rec_emg2_signal);
fb07 17:f87e5d6c87f4 364 emg2_signal = low_rec_high_emg2_signal;
fb07 17:f87e5d6c87f4 365
fb07 17:f87e5d6c87f4 366 }
fb07 17:f87e5d6c87f4 367
fb07 17:f87e5d6c87f4 368 void emg3_processing()
fb07 17:f87e5d6c87f4 369 {
fb07 17:f87e5d6c87f4 370 emg3_raw_signal=emg3.read();
fb07 17:f87e5d6c87f4 371
fb07 17:f87e5d6c87f4 372 high_emg3_signal = highfilter3.step(emg3_raw_signal);
fb07 17:f87e5d6c87f4 373 rec_emg3_signal = abs(high_emg3_signal);
fb07 17:f87e5d6c87f4 374 low_rec_high_emg3_signal = LowPassFilter3.step(rec_emg3_signal);
fb07 17:f87e5d6c87f4 375 emg3_signal = low_rec_high_emg3_signal;
fb07 17:f87e5d6c87f4 376
fb07 17:f87e5d6c87f4 377 }
fb07 13:db1a8b51706b 378
fb07 5:7e2c6d2235fe 379 // 4.3 State-Machine *******************************************************
fb07 10:a60b369c1711 380
fb07 12:f5dc65f1c27b 381 void state_machine()
fb07 12:f5dc65f1c27b 382 {
fb07 12:f5dc65f1c27b 383 if (sw2==0) {State = EmergencyMode;}
fb07 12:f5dc65f1c27b 384 switch(State)
fb07 12:f5dc65f1c27b 385 {
fb07 12:f5dc65f1c27b 386 case MotorCalibration:
fb07 12:f5dc65f1c27b 387 // pc.printf("\r\n State: MotorCalibration");
fb07 12:f5dc65f1c27b 388 led_blue.write(1);
fb07 12:f5dc65f1c27b 389 led_red.write(1);
fb07 12:f5dc65f1c27b 390 led_green.write(0); //Green Led on when in this state
fb07 12:f5dc65f1c27b 391
fb07 14:236ae2d7ec41 392 if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 12:f5dc65f1c27b 393 {
fb07 12:f5dc65f1c27b 394 pc.printf("\r\n Motor Calibration is done!");
fb07 14:236ae2d7ec41 395 encoder_motor1.reset();
fb07 14:236ae2d7ec41 396 encoder_motor2.reset();
fb07 15:849e0fc5d3a8 397
fb07 15:849e0fc5d3a8 398
fb07 12:f5dc65f1c27b 399 State=StartWait;
fb07 12:f5dc65f1c27b 400 }
fb07 12:f5dc65f1c27b 401 else {;} //pc.printf("\r\n Motor Calibration is not done!");}
fb07 12:f5dc65f1c27b 402
fb07 12:f5dc65f1c27b 403 break;
fb07 12:f5dc65f1c27b 404
fb07 12:f5dc65f1c27b 405 case StartWait:
fb07 12:f5dc65f1c27b 406 // pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo");
fb07 12:f5dc65f1c27b 407 led_blue.write(0);
fb07 12:f5dc65f1c27b 408 led_red.write(1);
fb07 12:f5dc65f1c27b 409 led_green.write(1);
fb07 15:849e0fc5d3a8 410
fb07 18:8bfc1821d412 411 emg_tijd =0;
fb07 16:1be144329f05 412 State=EMGCalibration;
fb07 16:1be144329f05 413 break;
fb07 16:1be144329f05 414
fb07 12:f5dc65f1c27b 415 case EMGCalibration:
fb07 12:f5dc65f1c27b 416 // pc.printf("\r\n State: EMGCalibration");
fb07 12:f5dc65f1c27b 417 led_blue.write(1);
fb07 12:f5dc65f1c27b 418 led_red.write(1);
fb07 18:8bfc1821d412 419 led_green.write(1);
fb07 18:8bfc1821d412 420
fb07 18:8bfc1821d412 421 emg_tijd++;
fb07 18:8bfc1821d412 422
fb07 18:8bfc1821d412 423 if (0<=emg_tijd&&emg_tijd<=5000)
fb07 18:8bfc1821d412 424 { led_green.write(0); }
fb07 18:8bfc1821d412 425 else if (5000<emg_tijd&&emg_tijd<8000)
fb07 18:8bfc1821d412 426 { led_green.write(1);
fb07 18:8bfc1821d412 427 if (emg0_signal > emg0_max)
fb07 18:8bfc1821d412 428 {
fb07 18:8bfc1821d412 429 emg0_max=emg0_signal;
fb07 18:8bfc1821d412 430 }
fb07 18:8bfc1821d412 431 }
fb07 18:8bfc1821d412 432 else if (8000<emg_tijd)
fb07 18:8bfc1821d412 433 {led_green.write(0);
fb07 18:8bfc1821d412 434 if(button1==0) {State=StartWait;}
fb07 18:8bfc1821d412 435 if(button2==0) {State=Homing;}
fb07 18:8bfc1821d412 436 }
fb07 18:8bfc1821d412 437
fb07 18:8bfc1821d412 438
fb07 18:8bfc1821d412 439
fb07 18:8bfc1821d412 440 // Yref_motor1=5000;
fb07 18:8bfc1821d412 441 // Yref_motor2=2000;
fb07 18:8bfc1821d412 442 // State=Homing;
fb07 12:f5dc65f1c27b 443 break;
fb07 12:f5dc65f1c27b 444 case Homing:
fb07 14:236ae2d7ec41 445 // pc.printf("\r\n State: Homing");
fb07 18:8bfc1821d412 446 led_blue.write(0);
fb07 18:8bfc1821d412 447 led_red.write(1);
fb07 16:1be144329f05 448 led_green.write(1);
fb07 14:236ae2d7ec41 449 led2=1;
fb07 16:1be144329f05 450
fb07 18:8bfc1821d412 451 homing_tijd++;
fb07 18:8bfc1821d412 452
fb07 18:8bfc1821d412 453 if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
fb07 18:8bfc1821d412 454 {
fb07 18:8bfc1821d412 455 if(button1==0) {State=Operating;}
fb07 18:8bfc1821d412 456 if(button2==0) {State=Demo;}
fb07 18:8bfc1821d412 457 }
fb07 18:8bfc1821d412 458
fb07 12:f5dc65f1c27b 459 break;
fb07 16:1be144329f05 460
fb07 12:f5dc65f1c27b 461 case Operating:
fb07 12:f5dc65f1c27b 462 /* pc.printf("\r\n State: Operating");
fb07 12:f5dc65f1c27b 463 led_blue.write(1);
fb07 12:f5dc65f1c27b 464 led_red.write(1);
fb07 12:f5dc65f1c27b 465 led_green.write(0);
fb07 12:f5dc65f1c27b 466 wait(0.5);
fb07 12:f5dc65f1c27b 467 led_green.write(1);
fb07 12:f5dc65f1c27b 468 wait(0.5); */
fb07 12:f5dc65f1c27b 469 break;
fb07 16:1be144329f05 470
fb07 16:1be144329f05 471 case Demo:
fb07 16:1be144329f05 472 pc.printf("\r\n State: Demo");
fb07 16:1be144329f05 473 led_blue.write(1);
fb07 16:1be144329f05 474 led_red.write(1);
fb07 16:1be144329f05 475 led_green.write(0);
fb07 16:1be144329f05 476
fb07 16:1be144329f05 477 break;
fb07 16:1be144329f05 478
fb07 12:f5dc65f1c27b 479 case EmergencyMode:
fb07 12:f5dc65f1c27b 480 pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
fb07 12:f5dc65f1c27b 481
fb07 12:f5dc65f1c27b 482 motor1=0;
fb07 12:f5dc65f1c27b 483 motor2=0;
fb07 12:f5dc65f1c27b 484
fb07 12:f5dc65f1c27b 485 led_blue.write(1);
fb07 12:f5dc65f1c27b 486 led_green.write(1);
fb07 12:f5dc65f1c27b 487 //SOS start
fb07 12:f5dc65f1c27b 488 led_red.write(0); // S
fb07 12:f5dc65f1c27b 489 wait(0.5);
fb07 12:f5dc65f1c27b 490 led_red.write(1); //pause
fb07 12:f5dc65f1c27b 491 wait(0.25);
fb07 12:f5dc65f1c27b 492 led_red.write(0); // O
fb07 12:f5dc65f1c27b 493 wait(1.5);
fb07 12:f5dc65f1c27b 494 led_red.write(1); // pause
fb07 12:f5dc65f1c27b 495 wait(0.25);
fb07 12:f5dc65f1c27b 496 led_red.write(0); // S
fb07 12:f5dc65f1c27b 497 wait(0.5);
fb07 12:f5dc65f1c27b 498 //SOS end
fb07 12:f5dc65f1c27b 499 break;
fb07 12:f5dc65f1c27b 500 case Idle:
fb07 12:f5dc65f1c27b 501 /* pc.printf("\r\n Idling..."); */
fb07 12:f5dc65f1c27b 502 break;
fb07 12:f5dc65f1c27b 503
fb07 12:f5dc65f1c27b 504 }
fb07 12:f5dc65f1c27b 505 }
fb07 9:c4fa72ffa1c2 506
fb07 5:7e2c6d2235fe 507 //******************************************************************************
fb07 5:7e2c6d2235fe 508 // 5. Main Loop ****************************************************************
fb07 2:45a85caaebfb 509 //******************************************************************************
fb07 2:45a85caaebfb 510
fb07 2:45a85caaebfb 511 void main_loop() { //Beginning of main_loop()
fb07 5:7e2c6d2235fe 512 // pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)
fb07 20:682ac5b520a0 513
fb07 20:682ac5b520a0 514
fb07 20:682ac5b520a0 515
fb07 20:682ac5b520a0 516
fb07 20:682ac5b520a0 517
fb07 20:682ac5b520a0 518
fb07 20:682ac5b520a0 519 //test stuff:
fb07 20:682ac5b520a0 520 //Yref_motor1= 4200*pot2_rechts.read();
fb07 20:682ac5b520a0 521 //Yref_motor2= 4200*pot1_links.read();
fb07 20:682ac5b520a0 522
fb07 20:682ac5b520a0 523 // 5.1 Measure Analog and Digital input signals ********************************
fb07 17:f87e5d6c87f4 524 emg0_processing();
fb07 17:f87e5d6c87f4 525 emg1_processing();
fb07 17:f87e5d6c87f4 526 emg2_processing();
fb07 17:f87e5d6c87f4 527 emg3_processing();
fb07 17:f87e5d6c87f4 528
fb07 9:c4fa72ffa1c2 529 fencoder_motor1() ;
fb07 10:a60b369c1711 530 fencoder_motor2() ;
fb07 20:682ac5b520a0 531 // 5.2 Run state-machine(s) ****************************************************
fb07 20:682ac5b520a0 532 state_machine() ;
fb07 20:682ac5b520a0 533 // 5.3 Run controller(s) *******************************************************
fb07 14:236ae2d7ec41 534 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
fb07 15:849e0fc5d3a8 535 PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
fb07 20:682ac5b520a0 536 // 5.4 Send output signals to digital and PWM output pins **********************
fb07 16:1be144329f05 537 motor1_controller();
fb07 16:1be144329f05 538 motor2_controller();
fb07 2:45a85caaebfb 539
fb07 2:45a85caaebfb 540
fb07 2:45a85caaebfb 541 } //Ending of main_loop()
fb07 2:45a85caaebfb 542
fb07 2:45a85caaebfb 543 //******************************************************************************
fb07 5:7e2c6d2235fe 544 // 6. Main function ************************************************************
fb07 2:45a85caaebfb 545 //******************************************************************************
fb07 2:45a85caaebfb 546 int main()
fb07 5:7e2c6d2235fe 547 { //Beginning of Main() Function //All the things we do only once, some relevant things are now missing here: set pwmperiod to 60 microsec. Set Serial comm. Etc. Etc.
fb07 5:7e2c6d2235fe 548 // 6.1 Initialization **********************************************************
RobertoO 0:67c50348f842 549 pc.baud(115200);
fb07 2:45a85caaebfb 550 pc.printf("\r\nStarting Project BioRobotics - Opening a Door " //print Project information
fb07 2:45a85caaebfb 551 "- Group 13 2019/2020 \r\n"
fb07 2:45a85caaebfb 552 "Dion ten Berge - s1864734 \r\n"
fb07 2:45a85caaebfb 553 "Bas Rutteman - s1854305 \r\n"
fb07 2:45a85caaebfb 554 "Nick in het Veld - s1915584 \r\n"
fb07 2:45a85caaebfb 555 "Marleen van der Weij - s1800078 \r\n"
fb07 2:45a85caaebfb 556 "Mevlid Yildirim - s2005735 \r\n");
fb07 5:7e2c6d2235fe 557 led_green.write(1);
fb07 5:7e2c6d2235fe 558 led_red.write(1);
fb07 14:236ae2d7ec41 559 led_blue.write(1);
fb07 18:8bfc1821d412 560 State = EMGCalibration ; // veranderen naar MotorCalibration;
fb07 14:236ae2d7ec41 561 ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
fb07 9:c4fa72ffa1c2 562 ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second
fb07 14:236ae2d7ec41 563
fb07 18:8bfc1821d412 564 // motor_calibration();
fb07 10:a60b369c1711 565
fb07 10:a60b369c1711 566
fb07 5:7e2c6d2235fe 567 // 6.2 While loop in main function**********************************************
fb07 18:8bfc1821d412 568 while (true) { pc.printf("\r\n emg_tijd = %d, homing_tijd= %d", emg_tijd, homing_tijd); } //Is not used but has to remain in the code!!
fb07 2:45a85caaebfb 569
fb07 2:45a85caaebfb 570 } //Ending of Main() Function