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@29:55547603475e, 2019-11-01 (annotated)

Committer:
fb07
Date:
Fri Nov 01 14:13:20 2019 +0000
Revision:
29:55547603475e
Parent:
28:94da9cdc1f16 
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!

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