mevlid yildirim / Mbed 2 deprecated BioRobotics_BMT

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@25:d94275968963, 2019-10-31 (annotated)

Committer:
fb07
Date:
Thu Oct 31 15:00:30 2019 +0000
Revision:
25:d94275968963
Parent:
24:f8482c47a385 
Niet werkende versie

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