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@22:239075a92d33, 2019-10-30 (annotated)

Committer:
fb07
Date:
Wed Oct 30 23:02:29 2019 +0000
Revision:
22:239075a92d33
Parent:
21:0d86a17f9974
Child:
23:d1a3d537460f
With RKI added, next step is to connect EMG with RKI

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