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@14:236ae2d7ec41, 2019-10-29 (annotated)

Committer:
fb07
Date:
Tue Oct 29 20:07:26 2019 +0000
Revision:
14:236ae2d7ec41
Parent:
13:db1a8b51706b
Child:
15:849e0fc5d3a8
Motor 1 now goes to the desired position in the homing state. Revision before adding motor 2

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 5:7e2c6d2235fe 47 // 3.3 BiQuad Filters **********************************************************
fb07 5:7e2c6d2235fe 48 static BiQuad notchfilter(9.97761e-01, -1.97095e+00, 9.97761e-01, -1.97095e+00, 9.95522e-01);
fb07 5:7e2c6d2235fe 49 static BiQuad highfilter(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
fb07 5:7e2c6d2235fe 50 static BiQuad LowPassFilter( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
RobertoO 0:67c50348f842 51
fb07 5:7e2c6d2235fe 52 // 3.4 Hardware ***************************************************************
fb07 5:7e2c6d2235fe 53 //3.4a Leds
fb07 5:7e2c6d2235fe 54 DigitalOut led_red(LED_RED); // Defines the red led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 55 DigitalOut led_green(LED_GREEN); // Defines the green led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 56 DigitalOut led_blue(LED_BLUE); // Defines the blue led on the K64 board (0=on, 1 = off)
fb07 5:7e2c6d2235fe 57 // FastPWM led1(D8); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 58 FastPWM led2(D9); //Defines Led2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 59
fb07 5:7e2c6d2235fe 60 //3.4b Potmeters and buttons
fb07 5:7e2c6d2235fe 61 AnalogIn pot1_links(A5); //Defines potmeter1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 62 AnalogIn pot2_rechts(A4); //Defines potmeter2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 63 DigitalIn button1(D2); //Defines button1 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 64 DigitalIn button2(D3); //Defines button2 on the BioRobotics Shield
fb07 5:7e2c6d2235fe 65 DigitalIn sw2(SW2); //Defines button SW2 on the K64 board
fb07 5:7e2c6d2235fe 66 DigitalIn sw3(SW3); //Defines button SW3 on the K64 board
fb07 5:7e2c6d2235fe 67
fb07 5:7e2c6d2235fe 68 //3.4c Motors
fb07 5:7e2c6d2235fe 69 DigitalOut motor1DirectionPin(D7); // motor 1 direction control (1=cw, 0=ccw)
fb07 5:7e2c6d2235fe 70 FastPWM motor1(D6); // motor 1 velocity control (between 0-1)
fb07 5:7e2c6d2235fe 71 FastPWM motor2(D5); // motor 2 velocity control (between 0-1)
fb07 5:7e2c6d2235fe 72 DigitalOut motor2DirectionPin(D4); // motor 2 direction control (1=cw, 0=ccw)
fb07 11:a3fd9d5144bb 73 bool motor1_calibrated=false;
fb07 11:a3fd9d5144bb 74 bool motor2_calibrated=false;
fb07 5:7e2c6d2235fe 75
fb07 10:a60b369c1711 76 // 3.5 Motor 1 variables ***************************************************************
fb07 5:7e2c6d2235fe 77 //3.5a PID-controller motor 1
fb07 5:7e2c6d2235fe 78 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 79 static double error_integral_motor1 = 0;
fb07 5:7e2c6d2235fe 80 double Yref_motor1;
fb07 5:7e2c6d2235fe 81 double kp_motor1;
fb07 5:7e2c6d2235fe 82 double Ki_motor1;
fb07 5:7e2c6d2235fe 83 double Kd_motor1;
fb07 14:236ae2d7ec41 84 double Up_motor1;
fb07 14:236ae2d7ec41 85 double Ui_motor1;
fb07 14:236ae2d7ec41 86 double Ud_motor1;
fb07 14:236ae2d7ec41 87
fb07 5:7e2c6d2235fe 88 double positie_motor1; //counts encoder
fb07 5:7e2c6d2235fe 89 double error1_motor1;
fb07 5:7e2c6d2235fe 90 double error1_prev_motor1;
fb07 5:7e2c6d2235fe 91 double error1_derivative_motor1;
fb07 5:7e2c6d2235fe 92 double error1_derivative_filtered_motor1;
fb07 5:7e2c6d2235fe 93 double P_motor1;
fb07 5:7e2c6d2235fe 94
fb07 10:a60b369c1711 95 double positie_verschil_motor1;
fb07 10:a60b369c1711 96 double positie_prev_motor1;
fb07 10:a60b369c1711 97
fb07 10:a60b369c1711 98 // 3.5 Motor 2 variables ***************************************************************
fb07 5:7e2c6d2235fe 99 //3.5b PID-controller motor 2
fb07 5:7e2c6d2235fe 100 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 101 static double error_integral_motor2 = 0;
fb07 5:7e2c6d2235fe 102 double Yref_motor2;
fb07 5:7e2c6d2235fe 103 double kp_motor2;
fb07 5:7e2c6d2235fe 104 double Ki_motor2;
fb07 5:7e2c6d2235fe 105 double Kd_motor2;
fb07 2:45a85caaebfb 106
fb07 5:7e2c6d2235fe 107 double positie_motor2; //counts encoder
fb07 5:7e2c6d2235fe 108 double error1_motor2;
fb07 5:7e2c6d2235fe 109 double error1_prev_motor2;
fb07 5:7e2c6d2235fe 110 double error1_derivative_motor2;
fb07 5:7e2c6d2235fe 111 double error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 112 double P_motor2;
fb07 10:a60b369c1711 113
fb07 10:a60b369c1711 114 double positie_verschil_motor2;
fb07 10:a60b369c1711 115 double positie_prev_motor2;
RobertoO 0:67c50348f842 116
fb07 2:45a85caaebfb 117 //******************************************************************************
fb07 5:7e2c6d2235fe 118 // 4. Functions ****************************************************************
fb07 5:7e2c6d2235fe 119 //******************************************************************************
fb07 5:7e2c6d2235fe 120
fb07 5:7e2c6d2235fe 121 // 4.1 Hidscope ****************************************************************
fb07 5:7e2c6d2235fe 122 void HIDScope() //voor HIDscope
fb07 5:7e2c6d2235fe 123 {
fb07 9:c4fa72ffa1c2 124 scope.set(0, positie_motor1);
fb07 14:236ae2d7ec41 125 scope.set(1, error1_motor1); //nog te definieren wat we willen weergeven
fb07 14:236ae2d7ec41 126 scope.set(2, P_motor1); //nog te definieren wat we willen weergeven
fb07 14:236ae2d7ec41 127 scope.set(3, Up_motor1);
fb07 14:236ae2d7ec41 128 // scope.set(4, Ui_motor1);
fb07 14:236ae2d7ec41 129 // scope.set(5, Uk_motor1);
fb07 14:236ae2d7ec41 130
fb07 9:c4fa72ffa1c2 131
fb07 5:7e2c6d2235fe 132 scope.send();
fb07 5:7e2c6d2235fe 133 }
fb07 10:a60b369c1711 134 // 4.x Encoder motor1 ****************************************************************
fb07 9:c4fa72ffa1c2 135 double fencoder_motor1() // bepaalt de positie van de motor
fb07 9:c4fa72ffa1c2 136 {
fb07 9:c4fa72ffa1c2 137 positie_motor1 = encoder_motor1.getPulses(); // haalt encoder waardes op
fb07 9:c4fa72ffa1c2 138 positie_verschil_motor1 = (positie_motor1-positie_prev_motor1)/Ts;
fb07 9:c4fa72ffa1c2 139 positie_prev_motor1 = positie_motor1;
fb07 9:c4fa72ffa1c2 140
fb07 9:c4fa72ffa1c2 141 return positie_motor1; //geeft positie van motor
fb07 9:c4fa72ffa1c2 142 }
fb07 10:a60b369c1711 143 // 4.x Encoder motor2 ****************************************************************
fb07 10:a60b369c1711 144 double fencoder_motor2() // bepaalt de positie van de motor
fb07 10:a60b369c1711 145 {
fb07 10:a60b369c1711 146 positie_motor2 = encoder_motor2.getPulses(); // haalt encoder waardes op
fb07 10:a60b369c1711 147 positie_verschil_motor2 = (positie_motor2-positie_prev_motor2)/Ts;
fb07 10:a60b369c1711 148 positie_prev_motor2 = positie_motor2;
fb07 10:a60b369c1711 149
fb07 10:a60b369c1711 150 return positie_motor2; //geeft positie van motor
fb07 10:a60b369c1711 151 }
fb07 10:a60b369c1711 152
fb07 11:a3fd9d5144bb 153 // 4.xa Calibration motors
fb07 11:a3fd9d5144bb 154
fb07 11:a3fd9d5144bb 155 void motor_calibration()
fb07 10:a60b369c1711 156 {
fb07 11:a3fd9d5144bb 157 // Calibration motor 2
fb07 10:a60b369c1711 158 motor1DirectionPin=0; //direction of the motor
fb07 10:a60b369c1711 159 motor1=1.0;
fb07 13:db1a8b51706b 160 wait(0.1);
fb07 10:a60b369c1711 161 while (abs(positie_verschil_motor1)>5)
fb07 10:a60b369c1711 162 {
fb07 10:a60b369c1711 163 motor1=0.2 ;
fb07 14:236ae2d7ec41 164 // pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
fb07 10:a60b369c1711 165 }
fb07 10:a60b369c1711 166 motor1=0.0;
fb07 11:a3fd9d5144bb 167 motor1_calibrated=true;
fb07 14:236ae2d7ec41 168 // pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
fb07 10:a60b369c1711 169
fb07 11:a3fd9d5144bb 170
fb07 10:a60b369c1711 171
fb07 11:a3fd9d5144bb 172 // Calibration motor 2
fb07 10:a60b369c1711 173 motor2DirectionPin=0; //direction of the motor
fb07 10:a60b369c1711 174 motor2=1.0;
fb07 13:db1a8b51706b 175 wait(0.1);
fb07 10:a60b369c1711 176 while (abs(positie_verschil_motor2)>5)
fb07 10:a60b369c1711 177 {
fb07 10:a60b369c1711 178 motor2=0.2 ;
fb07 14:236ae2d7ec41 179 // pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
fb07 13:db1a8b51706b 180 led2=1;
fb07 10:a60b369c1711 181 }
fb07 10:a60b369c1711 182 motor2=0.0;
fb07 13:db1a8b51706b 183 led2=0;
fb07 11:a3fd9d5144bb 184 motor2_calibrated=true;
fb07 14:236ae2d7ec41 185 // pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
fb07 13:db1a8b51706b 186
fb07 12:f5dc65f1c27b 187
fb07 10:a60b369c1711 188 }
fb07 10:a60b369c1711 189
fb07 5:7e2c6d2235fe 190 // 4.2a PID-Controller motor 1**************************************************
fb07 14:236ae2d7ec41 191 double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
fb07 5:7e2c6d2235fe 192 {
fb07 5:7e2c6d2235fe 193 //Proportional part
fb07 5:7e2c6d2235fe 194 kp_motor1 = 0.01 ; // moet nog getweaked worden
fb07 14:236ae2d7ec41 195 Up_motor1 = kp_motor1 * error1_motor1;
fb07 5:7e2c6d2235fe 196
fb07 5:7e2c6d2235fe 197 //Integral part
fb07 5:7e2c6d2235fe 198 Ki_motor1 = 0.0001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 199 error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1); // integrale fout + (de sample tijd * fout)
fb07 14:236ae2d7ec41 200 Ui_motor1 = Ki_motor1 * error_integral_motor1; // (fout * integrale fout)
fb07 5:7e2c6d2235fe 201
fb07 5:7e2c6d2235fe 202 //Derivative part
fb07 5:7e2c6d2235fe 203 Kd_motor1 = 0.01 ;// moet nog getweaked worden
fb07 14:236ae2d7ec41 204 error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
fb07 5:7e2c6d2235fe 205 error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
fb07 14:236ae2d7ec41 206 Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1; // (afgeleide gain) * (afgeleide gefilterde fout)
fb07 5:7e2c6d2235fe 207 error1_prev_motor1 = error1_motor1;
fb07 5:7e2c6d2235fe 208
fb07 14:236ae2d7ec41 209 P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1; //sommatie van de u's
fb07 14:236ae2d7ec41 210
fb07 14:236ae2d7ec41 211 return P_motor1;
fb07 14:236ae2d7ec41 212
fb07 5:7e2c6d2235fe 213
fb07 5:7e2c6d2235fe 214 }
fb07 5:7e2c6d2235fe 215
fb07 5:7e2c6d2235fe 216 // 4.2b PID-Controller motor 2**************************************************
fb07 5:7e2c6d2235fe 217 double PID_controller_motor2()
fb07 5:7e2c6d2235fe 218 {
fb07 5:7e2c6d2235fe 219 //Proportional part
fb07 5:7e2c6d2235fe 220 kp_motor2 = 0.01 ; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 221 double Up_motor2 = kp_motor2 * error1_motor2;
fb07 5:7e2c6d2235fe 222
fb07 5:7e2c6d2235fe 223 //Integral part
fb07 5:7e2c6d2235fe 224 Ki_motor2 = 0.0001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 225 error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2); // integrale fout + (de sample tijd * fout)
fb07 5:7e2c6d2235fe 226 double Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
fb07 5:7e2c6d2235fe 227
fb07 5:7e2c6d2235fe 228 //Derivative part
fb07 5:7e2c6d2235fe 229 Kd_motor2 = 0.01 ;// moet nog getweaked worden
fb07 5:7e2c6d2235fe 230 error1_derivative_motor2 = (error1_motor2 - error1_prev_motor2)/Ts;
fb07 5:7e2c6d2235fe 231 error1_derivative_filtered_motor2 = LowPassFilter.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
fb07 5:7e2c6d2235fe 232 double Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 233 error1_prev_motor2 = error1_motor2;
fb07 5:7e2c6d2235fe 234
fb07 5:7e2c6d2235fe 235 double P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
fb07 5:7e2c6d2235fe 236
fb07 5:7e2c6d2235fe 237 return P_motor2;
fb07 5:7e2c6d2235fe 238 }
fb07 10:a60b369c1711 239
fb07 14:236ae2d7ec41 240 double motor1_pwm()
fb07 14:236ae2d7ec41 241 {
fb07 14:236ae2d7ec41 242
fb07 14:236ae2d7ec41 243 if (P_motor1 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
fb07 14:236ae2d7ec41 244 {
fb07 14:236ae2d7ec41 245 motor1DirectionPin=1; // Clockwise rotation
fb07 14:236ae2d7ec41 246 }
fb07 14:236ae2d7ec41 247 else
fb07 14:236ae2d7ec41 248 {
fb07 14:236ae2d7ec41 249 motor1DirectionPin=0; // Counterclockwise rotation
fb07 14:236ae2d7ec41 250 }
fb07 14:236ae2d7ec41 251
fb07 14:236ae2d7ec41 252 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 253 {
fb07 14:236ae2d7ec41 254 motor1 = 0.99 ;
fb07 14:236ae2d7ec41 255 }
fb07 14:236ae2d7ec41 256 else
fb07 14:236ae2d7ec41 257 {
fb07 14:236ae2d7ec41 258 motor1 = fabs(P_motor1);
fb07 14:236ae2d7ec41 259 }
fb07 14:236ae2d7ec41 260
fb07 14:236ae2d7ec41 261 }
fb07 14:236ae2d7ec41 262
fb07 14:236ae2d7ec41 263 void motor1_controller(void)
fb07 14:236ae2d7ec41 264 {
fb07 14:236ae2d7ec41 265 error1_motor1 = (Yref_motor1 - positie_motor1);
fb07 14:236ae2d7ec41 266 motor1_pwm();
fb07 14:236ae2d7ec41 267 }
fb07 14:236ae2d7ec41 268
fb07 13:db1a8b51706b 269
fb07 13:db1a8b51706b 270
fb07 5:7e2c6d2235fe 271 // 4.3 State-Machine *******************************************************
fb07 10:a60b369c1711 272
fb07 12:f5dc65f1c27b 273 void state_machine()
fb07 12:f5dc65f1c27b 274 {
fb07 12:f5dc65f1c27b 275 if (sw2==0) {State = EmergencyMode;}
fb07 12:f5dc65f1c27b 276 switch(State)
fb07 12:f5dc65f1c27b 277 {
fb07 12:f5dc65f1c27b 278 case MotorCalibration:
fb07 12:f5dc65f1c27b 279 // pc.printf("\r\n State: MotorCalibration");
fb07 12:f5dc65f1c27b 280 led_blue.write(1);
fb07 12:f5dc65f1c27b 281 led_red.write(1);
fb07 12:f5dc65f1c27b 282 led_green.write(0); //Green Led on when in this state
fb07 12:f5dc65f1c27b 283
fb07 14:236ae2d7ec41 284 if (motor1_calibrated==true&&motor2_calibrated==true)
fb07 12:f5dc65f1c27b 285 {
fb07 12:f5dc65f1c27b 286 pc.printf("\r\n Motor Calibration is done!");
fb07 14:236ae2d7ec41 287 encoder_motor1.reset();
fb07 14:236ae2d7ec41 288 encoder_motor2.reset();
fb07 14:236ae2d7ec41 289 Yref_motor1=10000;
fb07 12:f5dc65f1c27b 290 State=StartWait;
fb07 12:f5dc65f1c27b 291 }
fb07 12:f5dc65f1c27b 292 else {;} //pc.printf("\r\n Motor Calibration is not done!");}
fb07 12:f5dc65f1c27b 293
fb07 12:f5dc65f1c27b 294 break;
fb07 12:f5dc65f1c27b 295
fb07 12:f5dc65f1c27b 296 case StartWait:
fb07 12:f5dc65f1c27b 297 // pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo");
fb07 12:f5dc65f1c27b 298 led_blue.write(0);
fb07 12:f5dc65f1c27b 299 led_red.write(1);
fb07 12:f5dc65f1c27b 300 led_green.write(1);
fb07 14:236ae2d7ec41 301 Yref_motor1=10000;
fb07 12:f5dc65f1c27b 302 if(button1==0) {State=EMGCalibration;}
fb07 12:f5dc65f1c27b 303 if(button2==0) {State=Demo;}
fb07 12:f5dc65f1c27b 304 break;
fb07 12:f5dc65f1c27b 305 case EMGCalibration:
fb07 12:f5dc65f1c27b 306 // pc.printf("\r\n State: EMGCalibration");
fb07 12:f5dc65f1c27b 307 led_blue.write(1);
fb07 12:f5dc65f1c27b 308 led_red.write(1);
fb07 12:f5dc65f1c27b 309 led_green.write(0);
fb07 12:f5dc65f1c27b 310
fb07 12:f5dc65f1c27b 311 State=Homing;
fb07 12:f5dc65f1c27b 312 break;
fb07 12:f5dc65f1c27b 313 case Homing:
fb07 14:236ae2d7ec41 314 // pc.printf("\r\n State: Homing");
fb07 14:236ae2d7ec41 315
fb07 14:236ae2d7ec41 316 led2=1;
fb07 14:236ae2d7ec41 317 motor1_controller();
fb07 14:236ae2d7ec41 318 // State=Operating;
fb07 12:f5dc65f1c27b 319 break;
fb07 12:f5dc65f1c27b 320 case Demo:
fb07 12:f5dc65f1c27b 321 pc.printf("\r\n State: Demo");
fb07 12:f5dc65f1c27b 322 led_blue.write(1);
fb07 12:f5dc65f1c27b 323 led_red.write(1);
fb07 13:db1a8b51706b 324 led_green.write(0);
fb07 12:f5dc65f1c27b 325
fb07 12:f5dc65f1c27b 326
fb07 12:f5dc65f1c27b 327 break;
fb07 12:f5dc65f1c27b 328 case Operating:
fb07 12:f5dc65f1c27b 329 /* pc.printf("\r\n State: Operating");
fb07 12:f5dc65f1c27b 330 led_blue.write(1);
fb07 12:f5dc65f1c27b 331 led_red.write(1);
fb07 12:f5dc65f1c27b 332 led_green.write(0);
fb07 12:f5dc65f1c27b 333 wait(0.5);
fb07 12:f5dc65f1c27b 334 led_green.write(1);
fb07 12:f5dc65f1c27b 335 wait(0.5); */
fb07 12:f5dc65f1c27b 336 break;
fb07 12:f5dc65f1c27b 337 case EmergencyMode:
fb07 12:f5dc65f1c27b 338 pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
fb07 12:f5dc65f1c27b 339
fb07 12:f5dc65f1c27b 340 motor1=0;
fb07 12:f5dc65f1c27b 341 motor2=0;
fb07 12:f5dc65f1c27b 342
fb07 12:f5dc65f1c27b 343 led_blue.write(1);
fb07 12:f5dc65f1c27b 344 led_green.write(1);
fb07 12:f5dc65f1c27b 345 //SOS start
fb07 12:f5dc65f1c27b 346 led_red.write(0); // S
fb07 12:f5dc65f1c27b 347 wait(0.5);
fb07 12:f5dc65f1c27b 348 led_red.write(1); //pause
fb07 12:f5dc65f1c27b 349 wait(0.25);
fb07 12:f5dc65f1c27b 350 led_red.write(0); // O
fb07 12:f5dc65f1c27b 351 wait(1.5);
fb07 12:f5dc65f1c27b 352 led_red.write(1); // pause
fb07 12:f5dc65f1c27b 353 wait(0.25);
fb07 12:f5dc65f1c27b 354 led_red.write(0); // S
fb07 12:f5dc65f1c27b 355 wait(0.5);
fb07 12:f5dc65f1c27b 356 //SOS end
fb07 12:f5dc65f1c27b 357 break;
fb07 12:f5dc65f1c27b 358 case Idle:
fb07 12:f5dc65f1c27b 359 /* pc.printf("\r\n Idling..."); */
fb07 12:f5dc65f1c27b 360 break;
fb07 12:f5dc65f1c27b 361
fb07 12:f5dc65f1c27b 362 }
fb07 12:f5dc65f1c27b 363 }
fb07 9:c4fa72ffa1c2 364
fb07 5:7e2c6d2235fe 365 //******************************************************************************
fb07 5:7e2c6d2235fe 366 // 5. Main Loop ****************************************************************
fb07 2:45a85caaebfb 367 //******************************************************************************
fb07 2:45a85caaebfb 368
fb07 2:45a85caaebfb 369 void main_loop() { //Beginning of main_loop()
fb07 5:7e2c6d2235fe 370 // 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 9:c4fa72ffa1c2 371 fencoder_motor1() ;
fb07 10:a60b369c1711 372 fencoder_motor2() ;
fb07 14:236ae2d7ec41 373 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
fb07 12:f5dc65f1c27b 374 state_machine() ;
fb07 9:c4fa72ffa1c2 375
fb07 5:7e2c6d2235fe 376 // 5.1 Measure Analog and Digital input signals ********************************
fb07 5:7e2c6d2235fe 377 // 5.2 Run state-machine(s) ****************************************************
fb07 5:7e2c6d2235fe 378 // 5.3 Run controller(s) *******************************************************
fb07 5:7e2c6d2235fe 379 // 5.4 Send output signals to digital and PWM output pins **********************
fb07 2:45a85caaebfb 380
fb07 2:45a85caaebfb 381
fb07 2:45a85caaebfb 382 } //Ending of main_loop()
fb07 2:45a85caaebfb 383
fb07 2:45a85caaebfb 384 //******************************************************************************
fb07 5:7e2c6d2235fe 385 // 6. Main function ************************************************************
fb07 2:45a85caaebfb 386 //******************************************************************************
fb07 2:45a85caaebfb 387 int main()
fb07 5:7e2c6d2235fe 388 { //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 389 // 6.1 Initialization **********************************************************
RobertoO 0:67c50348f842 390 pc.baud(115200);
fb07 2:45a85caaebfb 391 pc.printf("\r\nStarting Project BioRobotics - Opening a Door " //print Project information
fb07 2:45a85caaebfb 392 "- Group 13 2019/2020 \r\n"
fb07 2:45a85caaebfb 393 "Dion ten Berge - s1864734 \r\n"
fb07 2:45a85caaebfb 394 "Bas Rutteman - s1854305 \r\n"
fb07 2:45a85caaebfb 395 "Nick in het Veld - s1915584 \r\n"
fb07 2:45a85caaebfb 396 "Marleen van der Weij - s1800078 \r\n"
fb07 2:45a85caaebfb 397 "Mevlid Yildirim - s2005735 \r\n");
fb07 5:7e2c6d2235fe 398 led_green.write(1);
fb07 5:7e2c6d2235fe 399 led_red.write(1);
fb07 14:236ae2d7ec41 400 led_blue.write(1);
fb07 14:236ae2d7ec41 401 State = StartWait ; // veranderen inMotorCalibration;
fb07 14:236ae2d7ec41 402 ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
fb07 9:c4fa72ffa1c2 403 ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second
fb07 14:236ae2d7ec41 404
fb07 14:236ae2d7ec41 405 //motor_calibration();
fb07 10:a60b369c1711 406
fb07 10:a60b369c1711 407
fb07 5:7e2c6d2235fe 408 // 6.2 While loop in main function**********************************************
fb07 2:45a85caaebfb 409 while (true) { } //Is not used but has to remain in the code!!
fb07 2:45a85caaebfb 410
fb07 2:45a85caaebfb 411 } //Ending of Main() Function