mevlid yildirim / Mbed 2 deprecated BioRobotics_BMT

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@8:b4cf0a68e37f, 2019-10-22 (annotated)

Committer:
fb07
Date:
Tue Oct 22 14:52:28 2019 +0000
Revision:
8:b4cf0a68e37f
Parent:
7:a3e073b8dd29 
State-machine with hidscope working

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 6:79e42e1b87cb 27 enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Demo, Operating, EmergencyMode, Idle}; //All robot states
fb07 6:79e42e1b87cb 28 States State; //defines 'State' which can contain a state (i.e. StartWait) from 'enum States'.
RobertoO 0:67c50348f842 29
fb07 8:b4cf0a68e37f 30
fb07 2:45a85caaebfb 31 //*****************************************************************************
fb07 5:7e2c6d2235fe 32 // 3. (Global) Variables ***********************************************************
fb07 2:45a85caaebfb 33 //*****************************************************************************
fb07 5:7e2c6d2235fe 34 // 3.1 Tickers *****************************************************************
fb07 5:7e2c6d2235fe 35 Ticker ticker_mainloop; // The ticker which runs the mainloop
fb07 7:a3e073b8dd29 36 Ticker hidscope_ticker; // The ticker which sends data to the HIDScope server
fb07 5:7e2c6d2235fe 37
fb07 5:7e2c6d2235fe 38 // 3.2 General variables *******************************************************
fb07 5:7e2c6d2235fe 39
fb07 5:7e2c6d2235fe 40 MODSERIAL pc(USBTX, USBRX); // Serial communication with the board
fb07 5:7e2c6d2235fe 41 QEI encoder_motor1(D12,D13,NC,64); // Defines encoder for motor 1
fb07 5:7e2c6d2235fe 42 QEI encoder_motor2(D10,D11,NC,64); // Defines encoder for motor 1
fb07 5:7e2c6d2235fe 43 double f=1/100; // Frequency
fb07 6:79e42e1b87cb 44 const double Ts = f/1000; // Sampletime
fb07 7:a3e073b8dd29 45 HIDScope scope(1); // Amount of HIDScope servers
fb07 7:a3e073b8dd29 46 int test = 1;
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 6:79e42e1b87cb 57 // FastPWM led1(D8); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield (1=on, 0 = off)
fb07 6:79e42e1b87cb 58 FastPWM led2(D9); //Defines Led2 on the BioRobotics Shield (1=on, 0 = off)
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 5:7e2c6d2235fe 73
fb07 5:7e2c6d2235fe 74 // 3.5 PID variabelen ***************************************************************
fb07 5:7e2c6d2235fe 75 //3.5a PID-controller motor 1
fb07 7:a3e073b8dd29 76 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 77 static double error_integral_motor1 = 0;
fb07 5:7e2c6d2235fe 78 double Yref_motor1;
fb07 5:7e2c6d2235fe 79 double kp_motor1;
fb07 5:7e2c6d2235fe 80 double Ki_motor1;
fb07 5:7e2c6d2235fe 81 double Kd_motor1;
fb07 2:45a85caaebfb 82
fb07 5:7e2c6d2235fe 83 double positie_motor1; //counts encoder
fb07 7:a3e073b8dd29 84 double positie_verschil_motor1;
fb07 7:a3e073b8dd29 85 double positie_prev_motor1 = 200;
fb07 5:7e2c6d2235fe 86 double error1_motor1;
fb07 5:7e2c6d2235fe 87 double error1_prev_motor1;
fb07 5:7e2c6d2235fe 88 double error1_derivative_motor1;
fb07 5:7e2c6d2235fe 89 double error1_derivative_filtered_motor1;
fb07 5:7e2c6d2235fe 90 double P_motor1;
fb07 7:a3e073b8dd29 91
fb07 5:7e2c6d2235fe 92 //3.5b PID-controller motor 2
fb07 5:7e2c6d2235fe 93 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 94 static double error_integral_motor2 = 0;
fb07 5:7e2c6d2235fe 95 double Yref_motor2;
fb07 5:7e2c6d2235fe 96 double kp_motor2;
fb07 5:7e2c6d2235fe 97 double Ki_motor2;
fb07 5:7e2c6d2235fe 98 double Kd_motor2;
fb07 2:45a85caaebfb 99
fb07 5:7e2c6d2235fe 100 double positie_motor2; //counts encoder
fb07 7:a3e073b8dd29 101 double positie_verschil_motor2;
fb07 7:a3e073b8dd29 102 double positie_prev_motor2;
fb07 5:7e2c6d2235fe 103 double error1_motor2;
fb07 5:7e2c6d2235fe 104 double error1_prev_motor2;
fb07 5:7e2c6d2235fe 105 double error1_derivative_motor2;
fb07 5:7e2c6d2235fe 106 double error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 107 double P_motor2;
RobertoO 0:67c50348f842 108
fb07 2:45a85caaebfb 109 //******************************************************************************
fb07 5:7e2c6d2235fe 110 // 4. Functions ****************************************************************
fb07 5:7e2c6d2235fe 111 //******************************************************************************
fb07 5:7e2c6d2235fe 112
fb07 5:7e2c6d2235fe 113 // 4.1 Hidscope ****************************************************************
fb07 5:7e2c6d2235fe 114 void HIDScope() //voor HIDscope
fb07 5:7e2c6d2235fe 115 {
fb07 7:a3e073b8dd29 116 scope.set(0, test);
fb07 7:a3e073b8dd29 117 // scope.set(0, positie_motor1); //nog te definieren wat we willen weergeven
fb07 7:a3e073b8dd29 118 // scope.set(1, positie_verschil_motor1); //nog te definieren wat we willen weergeven
fb07 7:a3e073b8dd29 119 // scope.set(2, positie_prev_motor1);
fb07 5:7e2c6d2235fe 120 scope.send();
fb07 5:7e2c6d2235fe 121 }
fb07 5:7e2c6d2235fe 122
fb07 7:a3e073b8dd29 123 // 4.2 Encoders ****************************************************************
fb07 7:a3e073b8dd29 124 // 4.2a Encoder motor1 ****************************************************************
fb07 7:a3e073b8dd29 125 double Fun_encoder_motor1() // bepaalt de positie van de motor
fb07 7:a3e073b8dd29 126 {
fb07 7:a3e073b8dd29 127 positie_motor1 = encoder_motor1.getPulses(); // haalt encoder waardes op
fb07 7:a3e073b8dd29 128 positie_verschil_motor1 = (positie_motor1-positie_prev_motor1)/Ts;
fb07 7:a3e073b8dd29 129 positie_prev_motor1 = positie_motor1;
fb07 7:a3e073b8dd29 130
fb07 7:a3e073b8dd29 131 return positie_motor1; //geeft positie van motor
fb07 7:a3e073b8dd29 132 }
fb07 7:a3e073b8dd29 133 // 4.2b Encoder motor2 ****************************************************************
fb07 7:a3e073b8dd29 134
fb07 5:7e2c6d2235fe 135 // 4.2a PID-Controller motor 1**************************************************
fb07 5:7e2c6d2235fe 136 double PID_controller_motor1()
fb07 5:7e2c6d2235fe 137 {
fb07 5:7e2c6d2235fe 138 //Proportional part
fb07 5:7e2c6d2235fe 139 kp_motor1 = 0.01 ; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 140 double Up_motor1 = kp_motor1 * error1_motor1;
fb07 5:7e2c6d2235fe 141
fb07 5:7e2c6d2235fe 142 //Integral part
fb07 5:7e2c6d2235fe 143 Ki_motor1 = 0.0001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 144 error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1); // integrale fout + (de sample tijd * fout)
fb07 5:7e2c6d2235fe 145 double Ui_motor1 = Ki_motor1 * error_integral_motor1; // (fout * integrale fout)
fb07 5:7e2c6d2235fe 146
fb07 5:7e2c6d2235fe 147 //Derivative part
fb07 5:7e2c6d2235fe 148 Kd_motor1 = 0.01 ;// moet nog getweaked worden
fb07 5:7e2c6d2235fe 149 error1_derivative_motor1 = (error1_motor1 - error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
fb07 5:7e2c6d2235fe 150 error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
fb07 5:7e2c6d2235fe 151 double Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1; // (afgeleide gain) * (afgeleide gefilterde fout)
fb07 5:7e2c6d2235fe 152 error1_prev_motor1 = error1_motor1;
fb07 5:7e2c6d2235fe 153
fb07 5:7e2c6d2235fe 154 double P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1; //sommatie van de u's
fb07 5:7e2c6d2235fe 155
fb07 5:7e2c6d2235fe 156 return P_motor1;
fb07 5:7e2c6d2235fe 157 }
fb07 5:7e2c6d2235fe 158
fb07 5:7e2c6d2235fe 159 // 4.2b PID-Controller motor 2**************************************************
fb07 5:7e2c6d2235fe 160 double PID_controller_motor2()
fb07 5:7e2c6d2235fe 161 {
fb07 5:7e2c6d2235fe 162 //Proportional part
fb07 5:7e2c6d2235fe 163 kp_motor2 = 0.01 ; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 164 double Up_motor2 = kp_motor2 * error1_motor2;
fb07 5:7e2c6d2235fe 165
fb07 5:7e2c6d2235fe 166 //Integral part
fb07 5:7e2c6d2235fe 167 Ki_motor2 = 0.0001; // moet nog getweaked worden
fb07 5:7e2c6d2235fe 168 error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2); // integrale fout + (de sample tijd * fout)
fb07 5:7e2c6d2235fe 169 double Ui_motor2 = Ki_motor2 * error_integral_motor2; //de fout keer de integrale fout
fb07 5:7e2c6d2235fe 170
fb07 5:7e2c6d2235fe 171 //Derivative part
fb07 5:7e2c6d2235fe 172 Kd_motor2 = 0.01 ;// moet nog getweaked worden
fb07 5:7e2c6d2235fe 173 error1_derivative_motor2 = (error1_motor2 - error1_prev_motor2)/Ts;
fb07 5:7e2c6d2235fe 174 error1_derivative_filtered_motor2 = LowPassFilter.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
fb07 5:7e2c6d2235fe 175 double Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
fb07 5:7e2c6d2235fe 176 error1_prev_motor2 = error1_motor2;
fb07 5:7e2c6d2235fe 177
fb07 5:7e2c6d2235fe 178 double P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2; //sommatie van de u's
fb07 5:7e2c6d2235fe 179
fb07 5:7e2c6d2235fe 180 return P_motor2;
fb07 5:7e2c6d2235fe 181 }
fb07 7:a3e073b8dd29 182
fb07 7:a3e073b8dd29 183 // 4.4 motor_calibration *******************************************************
fb07 7:a3e073b8dd29 184 // 4.4a Motor1_calibration *******************************************************
fb07 7:a3e073b8dd29 185
fb07 7:a3e073b8dd29 186 void motor1_calibration()
fb07 7:a3e073b8dd29 187 {
fb07 7:a3e073b8dd29 188 motor1DirectionPin=0; //direction of the motor
fb07 7:a3e073b8dd29 189 motor1=0.2;
fb07 7:a3e073b8dd29 190 wait(0.5);
fb07 7:a3e073b8dd29 191 while (abs(positie_verschil_motor1)>100)
fb07 7:a3e073b8dd29 192 {
fb07 7:a3e073b8dd29 193 motor1=0.2 ;
fb07 7:a3e073b8dd29 194 pc.printf("\r\n While loop werkt");
fb07 7:a3e073b8dd29 195 }
fb07 7:a3e073b8dd29 196 motor1=0.0;
fb07 7:a3e073b8dd29 197 pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0");
fb07 7:a3e073b8dd29 198 //State=EMGCalibration;
fb07 7:a3e073b8dd29 199
fb07 7:a3e073b8dd29 200 }
fb07 8:b4cf0a68e37f 201
fb07 7:a3e073b8dd29 202 // 4.6 State-Machine *******************************************************
fb07 6:79e42e1b87cb 203 void state_machine()
fb07 6:79e42e1b87cb 204 {
fb07 6:79e42e1b87cb 205 if (sw2==0) {State = EmergencyMode;}
fb07 6:79e42e1b87cb 206 switch(State)
fb07 8:b4cf0a68e37f 207 {
fb07 8:b4cf0a68e37f 208
fb07 6:79e42e1b87cb 209 case StartWait:
fb07 8:b4cf0a68e37f 210 led_blue.write(0); //Turns Led blue on
fb07 8:b4cf0a68e37f 211 if(button1==0) {State=EMGCalibration;}//{State=MotorCalibration;}
fb07 6:79e42e1b87cb 212 if(button2==0) {State=Demo;}
fb07 6:79e42e1b87cb 213 break;
fb07 8:b4cf0a68e37f 214
fb07 8:b4cf0a68e37f 215 case MotorCalibration:
fb07 6:79e42e1b87cb 216 pc.printf("\r\n State: MotorCalibration");
fb07 7:a3e073b8dd29 217 motor1_calibration();
fb07 8:b4cf0a68e37f 218 //When motor1_calibration is done it defines State=EMGCalibration;
fb07 6:79e42e1b87cb 219 break;
fb07 8:b4cf0a68e37f 220
fb07 6:79e42e1b87cb 221 case EMGCalibration:
fb07 6:79e42e1b87cb 222 pc.printf("\r\n State: EMGCalibration");
fb07 6:79e42e1b87cb 223 led_blue.write(0);
fb07 6:79e42e1b87cb 224 led_red.write(1);
fb07 6:79e42e1b87cb 225 led_green.write(1);
fb07 6:79e42e1b87cb 226 // wait(2);
fb07 6:79e42e1b87cb 227 State=Homing;
fb07 6:79e42e1b87cb 228 break;
fb07 6:79e42e1b87cb 229
fb07 6:79e42e1b87cb 230 case Homing:
fb07 6:79e42e1b87cb 231 pc.printf("\r\n State: Homing");
fb07 6:79e42e1b87cb 232 State=Operating;
fb07 6:79e42e1b87cb 233 break;
fb07 6:79e42e1b87cb 234
fb07 6:79e42e1b87cb 235 case Demo:
fb07 6:79e42e1b87cb 236 pc.printf("\r\n State: Demo");
fb07 6:79e42e1b87cb 237 wait(2);
fb07 8:b4cf0a68e37f 238 led_red.write(1); //Turns Led red off
fb07 8:b4cf0a68e37f 239 led_blue.write(1); //Turns Led blue off
fb07 8:b4cf0a68e37f 240 led_green.write(0); //Turns Led green on
fb07 6:79e42e1b87cb 241 State=Idle;
fb07 6:79e42e1b87cb 242 break;
fb07 6:79e42e1b87cb 243
fb07 6:79e42e1b87cb 244 case Operating:
fb07 6:79e42e1b87cb 245 pc.printf("\r\n State: Operating");
fb07 7:a3e073b8dd29 246 //motor1=1;
fb07 6:79e42e1b87cb 247 break;
fb07 6:79e42e1b87cb 248 case Idle:
fb07 6:79e42e1b87cb 249 pc.printf("\r\n Idling...");
fb07 8:b4cf0a68e37f 250 break;
fb07 6:79e42e1b87cb 251
fb07 6:79e42e1b87cb 252 case EmergencyMode:
fb07 6:79e42e1b87cb 253 pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
fb07 6:79e42e1b87cb 254 motor1=0; //Turns motor1 off
fb07 6:79e42e1b87cb 255 motor2=0; //Turns motor2 off
fb07 6:79e42e1b87cb 256 //led1=0; //led1 is disabled //Turns Led1 off
fb07 6:79e42e1b87cb 257 led2=1; //Turns Led2 off
fb07 6:79e42e1b87cb 258
fb07 6:79e42e1b87cb 259 led_blue.write(1); //Turns Led blue off
fb07 6:79e42e1b87cb 260 led_green.write(1); //Turns Led green off
fb07 6:79e42e1b87cb 261
fb07 6:79e42e1b87cb 262
fb07 6:79e42e1b87cb 263 //SOS signal with Led Red start
fb07 6:79e42e1b87cb 264 led_red.write(0); // S
fb07 6:79e42e1b87cb 265 wait(0.5);
fb07 6:79e42e1b87cb 266 led_red.write(1); //pause
fb07 6:79e42e1b87cb 267 wait(0.25);
fb07 6:79e42e1b87cb 268 led_red.write(0); // O
fb07 6:79e42e1b87cb 269 wait(1.5);
fb07 6:79e42e1b87cb 270 led_red.write(1); // pause
fb07 6:79e42e1b87cb 271 wait(0.25);
fb07 6:79e42e1b87cb 272 led_red.write(0); // S
fb07 6:79e42e1b87cb 273 wait(0.5);
fb07 6:79e42e1b87cb 274 //SOS end
fb07 6:79e42e1b87cb 275 break;
fb07 6:79e42e1b87cb 276
fb07 6:79e42e1b87cb 277 }
fb07 6:79e42e1b87cb 278 }
fb07 7:a3e073b8dd29 279
fb07 5:7e2c6d2235fe 280 //******************************************************************************
fb07 5:7e2c6d2235fe 281 // 5. Main Loop ****************************************************************
fb07 2:45a85caaebfb 282 //******************************************************************************
fb07 2:45a85caaebfb 283
fb07 2:45a85caaebfb 284 void main_loop() { //Beginning of main_loop()
fb07 5:7e2c6d2235fe 285 // 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 7:a3e073b8dd29 286 Fun_encoder_motor1() ;
fb07 7:a3e073b8dd29 287
fb07 5:7e2c6d2235fe 288 // 5.1 Measure Analog and Digital input signals ********************************
fb07 5:7e2c6d2235fe 289 // 5.2 Run state-machine(s) ****************************************************
fb07 6:79e42e1b87cb 290 state_machine();
fb07 5:7e2c6d2235fe 291 // 5.3 Run controller(s) *******************************************************
fb07 5:7e2c6d2235fe 292 // 5.4 Send output signals to digital and PWM output pins **********************
fb07 2:45a85caaebfb 293
fb07 2:45a85caaebfb 294
fb07 2:45a85caaebfb 295 } //Ending of main_loop()
fb07 2:45a85caaebfb 296
fb07 2:45a85caaebfb 297 //******************************************************************************
fb07 5:7e2c6d2235fe 298 // 6. Main function ************************************************************
fb07 2:45a85caaebfb 299 //******************************************************************************
fb07 2:45a85caaebfb 300 int main()
fb07 5:7e2c6d2235fe 301 { //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 302 // 6.1 Initialization **********************************************************
RobertoO 0:67c50348f842 303 pc.baud(115200);
fb07 2:45a85caaebfb 304 pc.printf("\r\nStarting Project BioRobotics - Opening a Door " //print Project information
fb07 2:45a85caaebfb 305 "- Group 13 2019/2020 \r\n"
fb07 2:45a85caaebfb 306 "Dion ten Berge - s1864734 \r\n"
fb07 2:45a85caaebfb 307 "Bas Rutteman - s1854305 \r\n"
fb07 2:45a85caaebfb 308 "Nick in het Veld - s1915584 \r\n"
fb07 2:45a85caaebfb 309 "Marleen van der Weij - s1800078 \r\n"
fb07 2:45a85caaebfb 310 "Mevlid Yildirim - s2005735 \r\n");
fb07 6:79e42e1b87cb 311 led_green.write(1); //Led off
fb07 6:79e42e1b87cb 312 led_red.write(1); //Led off
fb07 6:79e42e1b87cb 313 led_blue.write(1); //Led off
fb07 8:b4cf0a68e37f 314 State = StartWait; //defines the starting State
fb07 8:b4cf0a68e37f 315 pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo");
RobertoO 0:67c50348f842 316
fb07 5:7e2c6d2235fe 317 ticker_mainloop.attach(&main_loop,0.001f); // change back to 0.001f //Run the function main_loop 1000 times per second
fb07 7:a3e073b8dd29 318 hidscope_ticker.attach(&HIDScope, 0.01f); //Ticker for Hidscope, different frequency compared to motors
fb07 2:45a85caaebfb 319
fb07 5:7e2c6d2235fe 320 // 6.2 While loop in main function**********************************************
fb07 2:45a85caaebfb 321 while (true) { } //Is not used but has to remain in the code!!
fb07 2:45a85caaebfb 322
fb07 2:45a85caaebfb 323 } //Ending of Main() Function