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@23:d1a3d537460f, 2019-10-31 (annotated)

Committer:
fb07
Date:
Thu Oct 31 12:11:54 2019 +0000
Revision:
23:d1a3d537460f
Parent:
22:239075a92d33
Child:
24:f8482c47a385
Child:
26:9e21ce046d4e
Tot zo ver werkend met alles erin

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