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

Committer:
fb07
Date:
Thu Oct 31 18:54:29 2019 +0000
Revision:
27:f5b33cbd3c22
Parent:
26:9e21ce046d4e
Child:
28:94da9cdc1f16
EIND VERSIE VOOR OPTIMALISATIE

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 26:9e21ce046d4e 53 double x_max= 100; //30.5;
fb07 26:9e21ce046d4e 54 double x_min= -100; //8.5;
fb07 23:d1a3d537460f 55 double x_home=8.5;
fb07 27:f5b33cbd3c22 56 double x_vergroting=0.02;
fb07 23:d1a3d537460f 57
fb07 23:d1a3d537460f 58 double y = 0; // 7.5 < y < 27
fb07 26:9e21ce046d4e 59 double y_max=100; //27;
fb07 26:9e21ce046d4e 60 double y_min=-100; //7.5;
fb07 23:d1a3d537460f 61 double y_home=7.5;
fb07 27:f5b33cbd3c22 62 double y_vergroting=0.02;
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 26:9e21ce046d4e 102 FastPWM led1(D9); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield
fb07 26:9e21ce046d4e 103 FastPWM led2(A5); //Defines Led2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 104
fb07 5:7e2c6d2235fe 105 //3.4b Potmeters and buttons
fb07 26:9e21ce046d4e 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 26:9e21ce046d4e 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 26:9e21ce046d4e 189 scope.set(0, emg2_signal);
fb07 26:9e21ce046d4e 190 scope.set(1, emg3_signal);
fb07 26:9e21ce046d4e 191 // scope.set(2, emg2_raw_signal);
fb07 26:9e21ce046d4e 192 // scope.set(3, emg3_raw_signal);
fb07 26:9e21ce046d4e 193
fb07 26:9e21ce046d4e 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 26:9e21ce046d4e 224 // Calibration motor 1
fb07 10:a60b369c1711 225 motor1DirectionPin=0; //direction of the motor
fb07 10:a60b369c1711 226 motor1=1.0;
fb07 26:9e21ce046d4e 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 26:9e21ce046d4e 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 26:9e21ce046d4e 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 26:9e21ce046d4e 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 26:9e21ce046d4e 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 26:9e21ce046d4e 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 5:7e2c6d2235fe 279 }
fb07 5:7e2c6d2235fe 280
fb07 5:7e2c6d2235fe 281 // 4.2b PID-Controller motor 2**************************************************
fb07 15:849e0fc5d3a8 282 double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
fb07 5:7e2c6d2235fe 283 {
fb07 5:7e2c6d2235fe 284 //Proportional part
fb07 26:9e21ce046d4e 285 kp_motor2 = 4.9801 ; // moet nog getweaked worden
fb07 15:849e0fc5d3a8 286 Up_motor2 = kp_motor2 * error1_motor2;
fb07 5:7e2c6d2235fe 287
fb07 5:7e2c6d2235fe 288 //Integral part
fb07 26:9e21ce046d4e 289 Ki_motor2 = 22.6073; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 290 error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2); // integrale fout + (de sample tijd * fout)
fb07 15:849e0fc5d3a8 291 Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
fb07 5:7e2c6d2235fe 292
fb07 5:7e2c6d2235fe 293 //Derivative part
fb07 26:9e21ce046d4e 294 Kd_motor2 = 0.012757 ;// moet nog getweaked worden
fb07 5:7e2c6d2235fe 295 error1_derivative_motor2 = (error1_motor2 - error1_prev_motor2)/Ts;
fb07 18:8bfc1821d412 296 error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
fb07 15:849e0fc5d3a8 297 Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 298 error1_prev_motor2 = error1_motor2;
fb07 5:7e2c6d2235fe 299
fb07 15:849e0fc5d3a8 300 P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
fb07 5:7e2c6d2235fe 301
fb07 5:7e2c6d2235fe 302 return P_motor2;
fb07 5:7e2c6d2235fe 303 }
fb07 10:a60b369c1711 304
fb07 14:236ae2d7ec41 305 double motor1_pwm()
fb07 14:236ae2d7ec41 306 {
fb07 14:236ae2d7ec41 307
fb07 14:236ae2d7ec41 308 if (P_motor1 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
fb07 14:236ae2d7ec41 309 {
fb07 14:236ae2d7ec41 310 motor1DirectionPin=1; // Clockwise rotation
fb07 14:236ae2d7ec41 311 }
fb07 14:236ae2d7ec41 312 else
fb07 14:236ae2d7ec41 313 {
fb07 14:236ae2d7ec41 314 motor1DirectionPin=0; // Counterclockwise rotation
fb07 14:236ae2d7ec41 315 }
fb07 14:236ae2d7ec41 316
fb07 14:236ae2d7ec41 317 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 318 {
fb07 14:236ae2d7ec41 319 motor1 = 0.99 ;
fb07 14:236ae2d7ec41 320 }
fb07 14:236ae2d7ec41 321 else
fb07 14:236ae2d7ec41 322 {
fb07 14:236ae2d7ec41 323 motor1 = fabs(P_motor1);
fb07 14:236ae2d7ec41 324 }
fb07 15:849e0fc5d3a8 325 }
fb07 15:849e0fc5d3a8 326 double motor2_pwm()
fb07 15:849e0fc5d3a8 327 {
fb07 15:849e0fc5d3a8 328
fb07 15:849e0fc5d3a8 329 if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
fb07 15:849e0fc5d3a8 330 {
fb07 26:9e21ce046d4e 331 motor2DirectionPin=1; // Clockwise rotation
fb07 15:849e0fc5d3a8 332 }
fb07 15:849e0fc5d3a8 333 else
fb07 15:849e0fc5d3a8 334 {
fb07 15:849e0fc5d3a8 335 motor2DirectionPin=0; // Counterclockwise rotation
fb07 15:849e0fc5d3a8 336 }
fb07 14:236ae2d7ec41 337
fb07 15:849e0fc5d3a8 338 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 339 {
fb07 15:849e0fc5d3a8 340 motor2 = 0.99 ;
fb07 15:849e0fc5d3a8 341 }
fb07 15:849e0fc5d3a8 342 else
fb07 15:849e0fc5d3a8 343 {
fb07 15:849e0fc5d3a8 344 motor2 = fabs(P_motor2);
fb07 15:849e0fc5d3a8 345 }
fb07 14:236ae2d7ec41 346 }
fb07 23:d1a3d537460f 347 void emg0_processing()
fb07 23:d1a3d537460f 348 {
fb07 23:d1a3d537460f 349 emg0_raw_signal=emg0.read();
fb07 23:d1a3d537460f 350
fb07 23:d1a3d537460f 351 high_emg0_signal = highfilter0.step(emg0_raw_signal);
fb07 23:d1a3d537460f 352 rec_emg0_signal = abs(high_emg0_signal);
fb07 23:d1a3d537460f 353 low_rec_high_emg0_signal = LowPassFilter0.step(rec_emg0_signal);
fb07 23:d1a3d537460f 354 emg0_signal = low_rec_high_emg0_signal;
fb07 14:236ae2d7ec41 355
fb07 23:d1a3d537460f 356 emg0_threshhold = 0.5*emg0_max; //
fb07 23:d1a3d537460f 357
fb07 23:d1a3d537460f 358 if (calibration_done && (emg0_signal>emg0_threshhold))
fb07 23:d1a3d537460f 359 { emg0_active=true;}
fb07 23:d1a3d537460f 360 else {emg0_active=false;}
fb07 23:d1a3d537460f 361 }
fb07 23:d1a3d537460f 362
fb07 23:d1a3d537460f 363 void emg1_processing()
fb07 23:d1a3d537460f 364 {
fb07 23:d1a3d537460f 365 emg1_raw_signal=emg1.read();
fb07 23:d1a3d537460f 366
fb07 23:d1a3d537460f 367 high_emg1_signal = highfilter1.step(emg1_raw_signal);
fb07 23:d1a3d537460f 368 rec_emg1_signal = abs(high_emg1_signal);
fb07 23:d1a3d537460f 369 low_rec_high_emg1_signal = LowPassFilter1.step(rec_emg1_signal);
fb07 23:d1a3d537460f 370 emg1_signal = low_rec_high_emg1_signal;
fb07 23:d1a3d537460f 371
fb07 23:d1a3d537460f 372 emg1_threshhold = 0.5*emg1_max; //
fb07 23:d1a3d537460f 373
fb07 23:d1a3d537460f 374 if (calibration_done && (emg1_signal>emg1_threshhold))
fb07 23:d1a3d537460f 375 { emg1_active=true;}
fb07 23:d1a3d537460f 376 else {emg1_active=false;}
fb07 23:d1a3d537460f 377
fb07 23:d1a3d537460f 378 }
fb07 23:d1a3d537460f 379
fb07 23:d1a3d537460f 380 void emg2_processing()
fb07 23:d1a3d537460f 381 {
fb07 23:d1a3d537460f 382 emg2_raw_signal=emg2.read();
fb07 14:236ae2d7ec41 383
fb07 23:d1a3d537460f 384 high_emg2_signal = highfilter2.step(emg2_raw_signal);
fb07 23:d1a3d537460f 385 rec_emg2_signal = abs(high_emg2_signal);
fb07 23:d1a3d537460f 386 low_rec_high_emg2_signal = LowPassFilter2.step(rec_emg2_signal);
fb07 23:d1a3d537460f 387 emg2_signal = low_rec_high_emg2_signal;
fb07 23:d1a3d537460f 388
fb07 23:d1a3d537460f 389 emg2_threshhold = 0.5*emg2_max; //
fb07 23:d1a3d537460f 390
fb07 23:d1a3d537460f 391 if (calibration_done && (emg2_signal>emg2_threshhold))
fb07 23:d1a3d537460f 392 { emg2_active=true;}
fb07 23:d1a3d537460f 393 else {emg2_active=false;}
fb07 23:d1a3d537460f 394 }
fb07 23:d1a3d537460f 395
fb07 23:d1a3d537460f 396 void emg3_processing()
fb07 23:d1a3d537460f 397 {
fb07 23:d1a3d537460f 398 emg3_raw_signal=emg3.read();
fb07 23:d1a3d537460f 399
fb07 23:d1a3d537460f 400 high_emg3_signal = highfilter3.step(emg3_raw_signal);
fb07 23:d1a3d537460f 401 rec_emg3_signal = abs(high_emg3_signal);
fb07 23:d1a3d537460f 402 low_rec_high_emg3_signal = LowPassFilter3.step(rec_emg3_signal);
fb07 23:d1a3d537460f 403 emg3_signal = low_rec_high_emg3_signal;
fb07 23:d1a3d537460f 404
fb07 23:d1a3d537460f 405 emg3_threshhold = 0.5*emg3_max; //
fb07 23:d1a3d537460f 406
fb07 23:d1a3d537460f 407 if (calibration_done && (emg3_signal>emg3_threshhold))
fb07 23:d1a3d537460f 408 { emg3_active=true;}
fb07 23:d1a3d537460f 409 else {emg3_active=false;}
fb07 23:d1a3d537460f 410 }
fb07 23:d1a3d537460f 411
fb07 17:f87e5d6c87f4 412
fb07 22:239075a92d33 413 double Angle_motor1()
fb07 22:239075a92d33 414 {
fb07 26:9e21ce046d4e 415 a = atan2(y , x);
fb07 22:239075a92d33 416 b = asin((length_link2 * sin(theta_motor2)) / (sqrt(pow(x, 2.0) + pow(y, 2.0))));
fb07 22:239075a92d33 417 theta_motor1 = b + a;
fb07 22:239075a92d33 418 return theta_motor1;
fb07 22:239075a92d33 419 }
fb07 22:239075a92d33 420
fb07 22:239075a92d33 421 double Angle_motor2()
fb07 22:239075a92d33 422 {
fb07 22:239075a92d33 423 c = (pow(x, 2.0) + pow(y, 2.0) - pow(length_link1, 2.0) - pow(length_link2, 2.0));
fb07 22:239075a92d33 424 d = (2 * length_link1 * length_link2);
fb07 22:239075a92d33 425 theta_motor2 = acos(c / d);
fb07 22:239075a92d33 426 return theta_motor2;
fb07 22:239075a92d33 427 }
fb07 22:239075a92d33 428
fb07 22:239075a92d33 429 double RKI()
fb07 22:239075a92d33 430 {
fb07 22:239075a92d33 431 if (emg0_active==true)
fb07 26:9e21ce046d4e 432 {
fb07 26:9e21ce046d4e 433 led1=1;
fb07 26:9e21ce046d4e 434 if (x>x_max) {x=x_max;}
fb07 26:9e21ce046d4e 435 else { x=x+ x_vergroting ; }
fb07 22:239075a92d33 436 }
fb07 22:239075a92d33 437
fb07 22:239075a92d33 438 if (emg1_active==true)
fb07 26:9e21ce046d4e 439 {
fb07 26:9e21ce046d4e 440 led2=1;
fb07 26:9e21ce046d4e 441 if (y>y_max) {y=y_max;}
fb07 26:9e21ce046d4e 442 else {y=y + y_vergroting ; }
fb07 22:239075a92d33 443 }
fb07 22:239075a92d33 444
fb07 22:239075a92d33 445 if (emg2_active==true)
fb07 26:9e21ce046d4e 446 {
fb07 26:9e21ce046d4e 447 led1=0;
fb07 26:9e21ce046d4e 448 if (x<x_min) {x=x_min;}
fb07 22:239075a92d33 449 else {x = x - x_vergroting; }
fb07 22:239075a92d33 450 }
fb07 22:239075a92d33 451
fb07 22:239075a92d33 452 if (emg3_active==true)
fb07 26:9e21ce046d4e 453 {
fb07 26:9e21ce046d4e 454 led2=1;
fb07 26:9e21ce046d4e 455 if (y<y_min) {y=y_min;}
fb07 22:239075a92d33 456 else {y=y - y_vergroting;}
fb07 22:239075a92d33 457 }
fb07 22:239075a92d33 458
fb07 22:239075a92d33 459 Angle_motor1();
fb07 22:239075a92d33 460 Angle_motor2();
fb07 22:239075a92d33 461 }
fb07 17:f87e5d6c87f4 462
fb07 23:d1a3d537460f 463 void motor1_controller(void)
fb07 23:d1a3d537460f 464 {
fb07 23:d1a3d537460f 465 error1_motor1 = (theta_motor1 - positie_motor1);
fb07 26:9e21ce046d4e 466
fb07 23:d1a3d537460f 467 if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 23:d1a3d537460f 468 {
fb07 26:9e21ce046d4e 469 motor1_pwm();
fb07 23:d1a3d537460f 470 }
fb07 23:d1a3d537460f 471
fb07 23:d1a3d537460f 472 }
fb07 17:f87e5d6c87f4 473
fb07 23:d1a3d537460f 474 void motor2_controller(void)
fb07 23:d1a3d537460f 475 {
fb07 23:d1a3d537460f 476 error1_motor2 = (theta_motor2 - positie_motor2);
fb07 23:d1a3d537460f 477 if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 23:d1a3d537460f 478 {
fb07 23:d1a3d537460f 479 motor2_pwm();
fb07 23:d1a3d537460f 480 }
fb07 23:d1a3d537460f 481 }
fb07 13:db1a8b51706b 482
fb07 5:7e2c6d2235fe 483 // 4.3 State-Machine *******************************************************
fb07 10:a60b369c1711 484
fb07 12:f5dc65f1c27b 485 void state_machine()
fb07 12:f5dc65f1c27b 486 {
fb07 12:f5dc65f1c27b 487 if (sw2==0) {State = EmergencyMode;}
fb07 12:f5dc65f1c27b 488 switch(State)
fb07 12:f5dc65f1c27b 489 {
fb07 12:f5dc65f1c27b 490 case MotorCalibration:
fb07 12:f5dc65f1c27b 491 // pc.printf("\r\n State: MotorCalibration");
fb07 26:9e21ce046d4e 492 led_blue.write(0);
fb07 12:f5dc65f1c27b 493 led_red.write(1);
fb07 26:9e21ce046d4e 494 led_green.write(1); //Green Led on when in this state
fb07 12:f5dc65f1c27b 495
fb07 14:236ae2d7ec41 496 if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 12:f5dc65f1c27b 497 {
fb07 12:f5dc65f1c27b 498 pc.printf("\r\n Motor Calibration is done!");
fb07 26:9e21ce046d4e 499 // x=28; aangeven wat de huidige positie is
fb07 26:9e21ce046d4e 500 // y=11;
fb07 14:236ae2d7ec41 501 encoder_motor1.reset();
fb07 14:236ae2d7ec41 502 encoder_motor2.reset();
fb07 15:849e0fc5d3a8 503
fb07 15:849e0fc5d3a8 504
fb07 12:f5dc65f1c27b 505 State=StartWait;
fb07 12:f5dc65f1c27b 506 }
fb07 12:f5dc65f1c27b 507 else {;} //pc.printf("\r\n Motor Calibration is not done!");}
fb07 12:f5dc65f1c27b 508
fb07 12:f5dc65f1c27b 509 break;
fb07 12:f5dc65f1c27b 510
fb07 12:f5dc65f1c27b 511 case StartWait:
fb07 12:f5dc65f1c27b 512 // pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo");
fb07 12:f5dc65f1c27b 513 led_blue.write(0);
fb07 12:f5dc65f1c27b 514 led_red.write(1);
fb07 26:9e21ce046d4e 515 led_green.write(0);
fb07 15:849e0fc5d3a8 516
fb07 18:8bfc1821d412 517 emg_tijd =0;
fb07 26:9e21ce046d4e 518 calibration_done=false;
fb07 16:1be144329f05 519 State=EMGCalibration;
fb07 16:1be144329f05 520 break;
fb07 16:1be144329f05 521
fb07 12:f5dc65f1c27b 522 case EMGCalibration:
fb07 12:f5dc65f1c27b 523 // pc.printf("\r\n State: EMGCalibration");
fb07 12:f5dc65f1c27b 524 led_blue.write(1);
fb07 12:f5dc65f1c27b 525 led_red.write(1);
fb07 18:8bfc1821d412 526 led_green.write(1);
fb07 18:8bfc1821d412 527
fb07 18:8bfc1821d412 528 emg_tijd++;
fb07 18:8bfc1821d412 529
fb07 26:9e21ce046d4e 530 if (0<=emg_tijd&&emg_tijd<=3000)
fb07 26:9e21ce046d4e 531 { led_green.write(1); }
fb07 26:9e21ce046d4e 532 else if (3000<emg_tijd&&emg_tijd<6000)
fb07 26:9e21ce046d4e 533 { led_green.write(0);
fb07 18:8bfc1821d412 534 if (emg0_signal > emg0_max)
fb07 18:8bfc1821d412 535 {
fb07 18:8bfc1821d412 536 emg0_max=emg0_signal;
fb07 23:d1a3d537460f 537 }
fb07 23:d1a3d537460f 538
fb07 23:d1a3d537460f 539 if (emg1_signal > emg1_max)
fb07 23:d1a3d537460f 540 {
fb07 23:d1a3d537460f 541 emg1_max=emg1_signal;
fb07 23:d1a3d537460f 542 }
fb07 23:d1a3d537460f 543
fb07 23:d1a3d537460f 544 if (emg2_signal > emg2_max)
fb07 23:d1a3d537460f 545 {
fb07 23:d1a3d537460f 546 emg2_max=emg2_signal;
fb07 23:d1a3d537460f 547 }
fb07 23:d1a3d537460f 548
fb07 23:d1a3d537460f 549 if (emg3_signal > emg3_max)
fb07 23:d1a3d537460f 550 {
fb07 23:d1a3d537460f 551 emg3_max=emg3_signal;
fb07 23:d1a3d537460f 552 }
fb07 18:8bfc1821d412 553 }
fb07 26:9e21ce046d4e 554 else if (6000<emg_tijd)
fb07 26:9e21ce046d4e 555 {led_green.write(1);
fb07 26:9e21ce046d4e 556
fb07 23:d1a3d537460f 557 calibration_done=true; // later verplaatsen
fb07 18:8bfc1821d412 558 if(button1==0) {State=StartWait;}
fb07 18:8bfc1821d412 559 if(button2==0) {State=Homing;}
fb07 18:8bfc1821d412 560 }
fb07 18:8bfc1821d412 561
fb07 12:f5dc65f1c27b 562 break;
fb07 12:f5dc65f1c27b 563 case Homing:
fb07 14:236ae2d7ec41 564 // pc.printf("\r\n State: Homing");
fb07 18:8bfc1821d412 565 led_blue.write(0);
fb07 18:8bfc1821d412 566 led_red.write(1);
fb07 16:1be144329f05 567 led_green.write(1);
fb07 14:236ae2d7ec41 568 led2=1;
fb07 16:1be144329f05 569
fb07 18:8bfc1821d412 570 homing_tijd++;
fb07 23:d1a3d537460f 571 x= x_home;
fb07 23:d1a3d537460f 572 y= y_home;
fb07 18:8bfc1821d412 573 if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
fb07 18:8bfc1821d412 574 {
fb07 26:9e21ce046d4e 575 if(button1==0) {State=Demo;}
fb07 26:9e21ce046d4e 576 if(button2==0) {State=Operating;}
fb07 26:9e21ce046d4e 577
fb07 18:8bfc1821d412 578 }
fb07 18:8bfc1821d412 579
fb07 12:f5dc65f1c27b 580 break;
fb07 16:1be144329f05 581
fb07 12:f5dc65f1c27b 582 case Operating:
fb07 22:239075a92d33 583 // pc.printf("\r\n State: Operating");
fb07 12:f5dc65f1c27b 584 led_blue.write(1);
fb07 12:f5dc65f1c27b 585 led_red.write(1);
fb07 12:f5dc65f1c27b 586 led_green.write(0);
fb07 23:d1a3d537460f 587 led2=0;
fb07 12:f5dc65f1c27b 588 break;
fb07 16:1be144329f05 589
fb07 16:1be144329f05 590 case Demo:
fb07 26:9e21ce046d4e 591 // pc.printf("\r\n State: Demo");
fb07 16:1be144329f05 592 led_blue.write(1);
fb07 26:9e21ce046d4e 593 led_red.write(0);
fb07 26:9e21ce046d4e 594 led_green.write(1);
fb07 26:9e21ce046d4e 595
fb07 27:f5b33cbd3c22 596 // motor1_calibrated=true;
fb07 27:f5b33cbd3c22 597 // motor2_calibrated=true;
fb07 27:f5b33cbd3c22 598 // emg0_threshhold=100;
fb07 27:f5b33cbd3c22 599 // emg1_threshhold=100;
fb07 27:f5b33cbd3c22 600 // emg2_threshhold=100;
fb07 27:f5b33cbd3c22 601 // emg3_threshhold=100;
fb07 26:9e21ce046d4e 602
fb07 26:9e21ce046d4e 603
fb07 26:9e21ce046d4e 604 if (button1==0)
fb07 26:9e21ce046d4e 605 { led1=1;
fb07 26:9e21ce046d4e 606 if (y>y_max) {y=y_max;}
fb07 26:9e21ce046d4e 607 else { y=y+ y_vergroting ;}
fb07 26:9e21ce046d4e 608 }
fb07 26:9e21ce046d4e 609 if (button2==0)
fb07 26:9e21ce046d4e 610 { led1=0;
fb07 26:9e21ce046d4e 611 if (y<y_min) {y=y_min;}
fb07 26:9e21ce046d4e 612 else { y=y - y_vergroting ;}
fb07 26:9e21ce046d4e 613 }
fb07 26:9e21ce046d4e 614
fb07 26:9e21ce046d4e 615
fb07 26:9e21ce046d4e 616 x = 30*pot2_rechts.read();
fb07 16:1be144329f05 617
fb07 16:1be144329f05 618 break;
fb07 16:1be144329f05 619
fb07 12:f5dc65f1c27b 620 case EmergencyMode:
fb07 12:f5dc65f1c27b 621 pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
fb07 12:f5dc65f1c27b 622
fb07 12:f5dc65f1c27b 623 motor1=0;
fb07 12:f5dc65f1c27b 624 motor2=0;
fb07 12:f5dc65f1c27b 625
fb07 12:f5dc65f1c27b 626 led_blue.write(1);
fb07 12:f5dc65f1c27b 627 led_green.write(1);
fb07 12:f5dc65f1c27b 628 //SOS start
fb07 12:f5dc65f1c27b 629 led_red.write(0); // S
fb07 12:f5dc65f1c27b 630 wait(0.5);
fb07 12:f5dc65f1c27b 631 led_red.write(1); //pause
fb07 12:f5dc65f1c27b 632 wait(0.25);
fb07 12:f5dc65f1c27b 633 led_red.write(0); // O
fb07 12:f5dc65f1c27b 634 wait(1.5);
fb07 12:f5dc65f1c27b 635 led_red.write(1); // pause
fb07 12:f5dc65f1c27b 636 wait(0.25);
fb07 12:f5dc65f1c27b 637 led_red.write(0); // S
fb07 12:f5dc65f1c27b 638 wait(0.5);
fb07 12:f5dc65f1c27b 639 //SOS end
fb07 12:f5dc65f1c27b 640 break;
fb07 12:f5dc65f1c27b 641 case Idle:
fb07 12:f5dc65f1c27b 642 /* pc.printf("\r\n Idling..."); */
fb07 12:f5dc65f1c27b 643 break;
fb07 12:f5dc65f1c27b 644
fb07 12:f5dc65f1c27b 645 }
fb07 12:f5dc65f1c27b 646 }
fb07 9:c4fa72ffa1c2 647
fb07 5:7e2c6d2235fe 648 //******************************************************************************
fb07 5:7e2c6d2235fe 649 // 5. Main Loop ****************************************************************
fb07 2:45a85caaebfb 650 //******************************************************************************
fb07 2:45a85caaebfb 651
fb07 2:45a85caaebfb 652 void main_loop() { //Beginning of main_loop()
fb07 23:d1a3d537460f 653
fb07 23:d1a3d537460f 654
fb07 23:d1a3d537460f 655
fb07 23:d1a3d537460f 656
fb07 5:7e2c6d2235fe 657 // 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 658
fb07 20:682ac5b520a0 659
fb07 20:682ac5b520a0 660
fb07 20:682ac5b520a0 661
fb07 20:682ac5b520a0 662
fb07 20:682ac5b520a0 663
fb07 20:682ac5b520a0 664 //test stuff:
fb07 20:682ac5b520a0 665 //Yref_motor1= 4200*pot2_rechts.read();
fb07 20:682ac5b520a0 666 //Yref_motor2= 4200*pot1_links.read();
fb07 20:682ac5b520a0 667
fb07 20:682ac5b520a0 668 // 5.1 Measure Analog and Digital input signals ********************************
fb07 17:f87e5d6c87f4 669 emg0_processing();
fb07 17:f87e5d6c87f4 670 emg1_processing();
fb07 17:f87e5d6c87f4 671 emg2_processing();
fb07 17:f87e5d6c87f4 672 emg3_processing();
fb07 17:f87e5d6c87f4 673
fb07 9:c4fa72ffa1c2 674 fencoder_motor1() ;
fb07 10:a60b369c1711 675 fencoder_motor2() ;
fb07 20:682ac5b520a0 676 // 5.2 Run state-machine(s) ****************************************************
fb07 20:682ac5b520a0 677 state_machine() ;
fb07 20:682ac5b520a0 678 // 5.3 Run controller(s) *******************************************************
fb07 26:9e21ce046d4e 679
fb07 22:239075a92d33 680 RKI();
fb07 22:239075a92d33 681
fb07 14:236ae2d7ec41 682 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
fb07 15:849e0fc5d3a8 683 PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
fb07 20:682ac5b520a0 684 // 5.4 Send output signals to digital and PWM output pins **********************
fb07 16:1be144329f05 685 motor1_controller();
fb07 16:1be144329f05 686 motor2_controller();
fb07 2:45a85caaebfb 687
fb07 2:45a85caaebfb 688
fb07 2:45a85caaebfb 689 } //Ending of main_loop()
fb07 2:45a85caaebfb 690
fb07 2:45a85caaebfb 691 //******************************************************************************
fb07 5:7e2c6d2235fe 692 // 6. Main function ************************************************************
fb07 2:45a85caaebfb 693 //******************************************************************************
fb07 2:45a85caaebfb 694 int main()
fb07 5:7e2c6d2235fe 695 { //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 696 // 6.1 Initialization **********************************************************
RobertoO 0:67c50348f842 697 pc.baud(115200);
fb07 2:45a85caaebfb 698 pc.printf("\r\nStarting Project BioRobotics - Opening a Door " //print Project information
fb07 2:45a85caaebfb 699 "- Group 13 2019/2020 \r\n"
fb07 2:45a85caaebfb 700 "Dion ten Berge - s1864734 \r\n"
fb07 2:45a85caaebfb 701 "Bas Rutteman - s1854305 \r\n"
fb07 2:45a85caaebfb 702 "Nick in het Veld - s1915584 \r\n"
fb07 2:45a85caaebfb 703 "Marleen van der Weij - s1800078 \r\n"
fb07 2:45a85caaebfb 704 "Mevlid Yildirim - s2005735 \r\n");
fb07 5:7e2c6d2235fe 705 led_green.write(1);
fb07 5:7e2c6d2235fe 706 led_red.write(1);
fb07 14:236ae2d7ec41 707 led_blue.write(1);
fb07 27:f5b33cbd3c22 708 State = MotorCalibration ; // veranderen naar MotorCalibration;
fb07 14:236ae2d7ec41 709 ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
fb07 9:c4fa72ffa1c2 710 ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second
fb07 14:236ae2d7ec41 711
fb07 26:9e21ce046d4e 712 motor_calibration();
fb07 10:a60b369c1711 713
fb07 10:a60b369c1711 714
fb07 5:7e2c6d2235fe 715 // 6.2 While loop in main function**********************************************
fb07 26:9e21ce046d4e 716 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 717 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 718
fb07 2:45a85caaebfb 719 } //Ending of Main() Function