Janus Bo Andersen / Mbed 2 deprecated T2PRO1_master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed m3pi

main.cpp@24:7fa7383e1b07, 2018-12-05 (annotated)

Committer:
janusboandersen
Date:
Wed Dec 05 11:46:47 2018 +0000
Revision:
24:7fa7383e1b07
Parent:
23:e1e6411e5221
Child:
25:17e9e410795c
Implented Wh and pitstops in display, cleaned up sensor_all function, cleaned up turning speed and definitions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
janusboandersen 22:fe373ba68df3 4 m3pi m3pi; //declare a new car object
chris 0:78f9794620a3 5
janusboandersen 22:fe373ba68df3 6 //Filename settings
janusboandersen 22:fe373ba68df3 7 #define FILESYS "robot" //name of the virtual file system on the LPC1768
janusboandersen 22:fe373ba68df3 8 #define FILENAME "/robot/data.txt" //filename to write backups to
janusboandersen 18:8b3efa4d4a36 9
janusboandersen 20:32e5cf8fe6dd 10 //Team calibrated parameters for car and battery
janusboandersen 23:e1e6411e5221 11 #define LOW_BATTERY_VOLTAGE 4.75 //measured voltage where charging is required
janusboandersen 22:fe373ba68df3 12 #define CHARGE_TIME 20 //in seconds. 1200 seconds = 20 minutes
janusboandersen 20:32e5cf8fe6dd 13 #define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
janusboandersen 20:32e5cf8fe6dd 14
janusboandersen 21:6dce9000da11 15 //Pit control
janusboandersen 22:fe373ba68df3 16 #define ENTER_PIT 1 //direction constant to mark driven forward out of pit
janusboandersen 22:fe373ba68df3 17 #define LEAVE_PIT -1 //direction constant to mark backing out of pit
janusboandersen 22:fe373ba68df3 18 #define SPEED_INSIDE_PIT 0.2 //speed to drive inside the pit
janusboandersen 22:fe373ba68df3 19 #define PIT_TRANSPORT_TIME 1 //seconds to drive pit stretch at interal pit spd
janusboandersen 21:6dce9000da11 20
janusboandersen 20:32e5cf8fe6dd 21 //Car sensors and LEDs
janusboandersen 20:32e5cf8fe6dd 22 #define PC0 0 //location of PC0 reflectance sensor
janusboandersen 20:32e5cf8fe6dd 23 #define PC4 4 //location of PC4 reflectance sensor
janusboandersen 24:7fa7383e1b07 24 #define CALIBRATED_SENS_VALS 0x87 //command to 3pi to get calibrated values
janusboandersen 20:32e5cf8fe6dd 25
janusboandersen 22:fe373ba68df3 26 //Various motor speeds
janusboandersen 19:c0d59019de53 27 #define MAX 1.0
chris 0:78f9794620a3 28 #define MIN 0
janusboandersen 22:fe373ba68df3 29 #define OPTIMAL_SPEED 0.8 //Current targeted optimal speed
janusboandersen 23:e1e6411e5221 30 #define PIT_SPEED 0.5 //Speed on a pit lap to avoid overshooting the pit
janusboandersen 22:fe373ba68df3 31 #define DECELLERATION 0.1 //Rate of decelleration to avoid bumping
chris 0:78f9794620a3 32
janusboandersen 24:7fa7383e1b07 33 //Calibrated values for performing 90 degree turns
janusboandersen 24:7fa7383e1b07 34 #define TURN_SPEED 0.3
janusboandersen 24:7fa7383e1b07 35 #define TURN_TIME 0.22
janusboandersen 24:7fa7383e1b07 36
chris 0:78f9794620a3 37 // PID terms
chris 0:78f9794620a3 38 #define P_TERM 1
janusboandersen 19:c0d59019de53 39 #define D_TERM 8
chris 0:78f9794620a3 40 #define I_TERM 0
janusboandersen 19:c0d59019de53 41
janusboandersen 19:c0d59019de53 42 //Forward declare our type for storing car performance date
janusboandersen 19:c0d59019de53 43 //It can now be used as a type by the name performance_data
janusboandersen 19:c0d59019de53 44 typedef struct pd {
janusboandersen 19:c0d59019de53 45 float wh;
janusboandersen 19:c0d59019de53 46 int pitstops;
janusboandersen 19:c0d59019de53 47 } performance_data;
Blunsdon 1:47ea51d9e25d 48
janusboandersen 18:8b3efa4d4a36 49
janusboandersen 19:c0d59019de53 50 //Function to write car performance data to persistent file
janusboandersen 18:8b3efa4d4a36 51 void write_to_file(performance_data data) {
janusboandersen 18:8b3efa4d4a36 52
janusboandersen 18:8b3efa4d4a36 53 //Define file system
janusboandersen 18:8b3efa4d4a36 54 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 55
janusboandersen 19:c0d59019de53 56 //Open a file (or create a new) with write access
janusboandersen 19:c0d59019de53 57 //"w": write "r":read "a":append (add @end)
janusboandersen 19:c0d59019de53 58 //"w": will overwrite any existing data, so we just store one line.
janusboandersen 19:c0d59019de53 59 FILE *fp = fopen(FILENAME, "w");
janusboandersen 18:8b3efa4d4a36 60
janusboandersen 19:c0d59019de53 61 //write performance data to the file
janusboandersen 19:c0d59019de53 62 fprintf(fp, "%f %d", data.wh, data.pitstops);
janusboandersen 18:8b3efa4d4a36 63
janusboandersen 19:c0d59019de53 64 //close the file and release the memory when done.
janusboandersen 19:c0d59019de53 65 fclose(fp);
janusboandersen 18:8b3efa4d4a36 66
janusboandersen 18:8b3efa4d4a36 67 }
janusboandersen 18:8b3efa4d4a36 68
janusboandersen 19:c0d59019de53 69 //Function to read car performance data from persistent file
janusboandersen 18:8b3efa4d4a36 70 performance_data read_from_file() {
janusboandersen 18:8b3efa4d4a36 71
janusboandersen 19:c0d59019de53 72 //Declare the variable which will ultimately be returned
janusboandersen 18:8b3efa4d4a36 73 performance_data data;
janusboandersen 18:8b3efa4d4a36 74
janusboandersen 19:c0d59019de53 75 //Define file system on mbed LPC1768
janusboandersen 18:8b3efa4d4a36 76 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 77
janusboandersen 19:c0d59019de53 78 //Try to open the file as read (it might not exist)
janusboandersen 19:c0d59019de53 79 FILE *fp = fopen(FILENAME, "r");
janusboandersen 19:c0d59019de53 80
janusboandersen 19:c0d59019de53 81 //Test whether the file exists
janusboandersen 19:c0d59019de53 82 if (fp != NULL) {
janusboandersen 19:c0d59019de53 83
janusboandersen 19:c0d59019de53 84 //file exists, so read from it
janusboandersen 19:c0d59019de53 85
janusboandersen 19:c0d59019de53 86 //Read stored data from the file and put in struct
janusboandersen 19:c0d59019de53 87 // &(struct.member) and &struct.member yield same address
janusboandersen 19:c0d59019de53 88 fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) );
janusboandersen 18:8b3efa4d4a36 89
janusboandersen 19:c0d59019de53 90 //close file and release mem.
janusboandersen 19:c0d59019de53 91 fclose(fp);
janusboandersen 19:c0d59019de53 92
janusboandersen 19:c0d59019de53 93 } else {
janusboandersen 19:c0d59019de53 94
janusboandersen 19:c0d59019de53 95 //file doesn't exist, so just return zeros (no performance yet)
janusboandersen 19:c0d59019de53 96 data.wh = 0.0;
janusboandersen 19:c0d59019de53 97 data.pitstops = 0;
janusboandersen 19:c0d59019de53 98 }
janusboandersen 18:8b3efa4d4a36 99
janusboandersen 19:c0d59019de53 100 //return the data object to the caller
janusboandersen 18:8b3efa4d4a36 101 return data;
janusboandersen 19:c0d59019de53 102 }
janusboandersen 19:c0d59019de53 103
janusboandersen 20:32e5cf8fe6dd 104 //This function tests the car's battery state, and returs true if it needs
janusboandersen 20:32e5cf8fe6dd 105 //to charge. Otherwise false is returned.
janusboandersen 19:c0d59019de53 106 bool battery_low() {
janusboandersen 20:32e5cf8fe6dd 107
janusboandersen 20:32e5cf8fe6dd 108 //Get battery level from the 3pi robot
janusboandersen 20:32e5cf8fe6dd 109 float voltage = m3pi.battery();
janusboandersen 20:32e5cf8fe6dd 110
janusboandersen 20:32e5cf8fe6dd 111 //return battery state result
janusboandersen 20:32e5cf8fe6dd 112 return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
janusboandersen 18:8b3efa4d4a36 113 }
janusboandersen 18:8b3efa4d4a36 114
janusboandersen 18:8b3efa4d4a36 115
janusboandersen 24:7fa7383e1b07 116 //Get the values from 3pi read from reflectance sensors PC0-PC4
janusboandersen 24:7fa7383e1b07 117 //The function returns in the array that is provided as function argument
janusboandersen 24:7fa7383e1b07 118 void sensor_all(int arr[]) {
Blunsdon 1:47ea51d9e25d 119
janusboandersen 24:7fa7383e1b07 120 m3pi.putc(CALIBRATED_SENS_VALS); // request 3pi for calibrated values
janusboandersen 24:7fa7383e1b07 121
janusboandersen 24:7fa7383e1b07 122 //Pick up the received data
janusboandersen 24:7fa7383e1b07 123 for(int n = PC0; n <= PC4; n++) { // loop through all sensors
janusboandersen 24:7fa7383e1b07 124 char lowbyte = m3pi.getc(); //LSB: we receive the data "little endian"
janusboandersen 24:7fa7383e1b07 125 char hibyte = m3pi.getc(); //get the MSB
janusboandersen 24:7fa7383e1b07 126 arr[n] = ((lowbyte + (hibyte << 8))); // make 2 byte int (8 bits / byte)
janusboandersen 24:7fa7383e1b07 127 }
janusboandersen 20:32e5cf8fe6dd 128 }
janusboandersen 20:32e5cf8fe6dd 129
janusboandersen 20:32e5cf8fe6dd 130 //Function to check whether car is just at the pit
janusboandersen 20:32e5cf8fe6dd 131 bool car_at_pit_crossroad(int sensor_array[]) {
janusboandersen 20:32e5cf8fe6dd 132
janusboandersen 20:32e5cf8fe6dd 133 //Read the sensor values
janusboandersen 20:32e5cf8fe6dd 134 sensor_all(sensor_array);
janusboandersen 20:32e5cf8fe6dd 135
janusboandersen 20:32e5cf8fe6dd 136 //Check the PC0 and PC4 sensors vs thresholds
janusboandersen 20:32e5cf8fe6dd 137 if (sensor_array[PC0] > CROSSROAD_REFL_VAL &&
janusboandersen 20:32e5cf8fe6dd 138 sensor_array[PC4] > CROSSROAD_REFL_VAL) {
janusboandersen 20:32e5cf8fe6dd 139
janusboandersen 20:32e5cf8fe6dd 140 //Car is at the crossroad
janusboandersen 20:32e5cf8fe6dd 141 return true;
janusboandersen 20:32e5cf8fe6dd 142 } else {
janusboandersen 20:32e5cf8fe6dd 143 //Car is NOT at the crossroad
janusboandersen 20:32e5cf8fe6dd 144 return false;
janusboandersen 20:32e5cf8fe6dd 145 }
janusboandersen 20:32e5cf8fe6dd 146 }
janusboandersen 20:32e5cf8fe6dd 147
janusboandersen 24:7fa7383e1b07 148
janusboandersen 21:6dce9000da11 149 //Turn the car 90 degrees left
janusboandersen 21:6dce9000da11 150 void turn_left (void)
janusboandersen 21:6dce9000da11 151 {
janusboandersen 24:7fa7383e1b07 152 m3pi.left(TURN_SPEED);
janusboandersen 24:7fa7383e1b07 153 wait(TURN_TIME);
janusboandersen 21:6dce9000da11 154 }
janusboandersen 21:6dce9000da11 155
janusboandersen 21:6dce9000da11 156 //Turn the car 90 degrees right
janusboandersen 21:6dce9000da11 157 void turn_right (void)
janusboandersen 21:6dce9000da11 158 {
janusboandersen 24:7fa7383e1b07 159 m3pi.right (TURN_SPEED);
janusboandersen 24:7fa7383e1b07 160 wait(TURN_TIME);
janusboandersen 21:6dce9000da11 161 }
janusboandersen 21:6dce9000da11 162
janusboandersen 21:6dce9000da11 163
janusboandersen 21:6dce9000da11 164 //Safely navigate the car from the pit crossroad and all the way to the charger.
janusboandersen 21:6dce9000da11 165 //direction = 1 to enter pit... direction = -1 to leave the pit.
janusboandersen 21:6dce9000da11 166 //takes control all the way from track to pit OR reverse.
janusboandersen 21:6dce9000da11 167 //Error handling: If a wrong code is passed, the function does nothing quietly.
janusboandersen 21:6dce9000da11 168 void pit_navigate(int direction) {
janusboandersen 21:6dce9000da11 169
janusboandersen 21:6dce9000da11 170 //Car must enter pit forward
janusboandersen 21:6dce9000da11 171 if (direction == ENTER_PIT) {
janusboandersen 21:6dce9000da11 172 m3pi.stop();
janusboandersen 21:6dce9000da11 173 turn_right();
janusboandersen 21:6dce9000da11 174 m3pi.stop();
janusboandersen 21:6dce9000da11 175 m3pi.forward(SPEED_INSIDE_PIT);
janusboandersen 21:6dce9000da11 176 wait(PIT_TRANSPORT_TIME);
janusboandersen 21:6dce9000da11 177 m3pi.stop();
janusboandersen 21:6dce9000da11 178 }
janusboandersen 21:6dce9000da11 179
janusboandersen 21:6dce9000da11 180 //Car must leave pit backward
janusboandersen 21:6dce9000da11 181 if (direction == LEAVE_PIT) {
janusboandersen 21:6dce9000da11 182 m3pi.backward(SPEED_INSIDE_PIT);
janusboandersen 21:6dce9000da11 183 wait(PIT_TRANSPORT_TIME);
janusboandersen 21:6dce9000da11 184 m3pi.stop();
janusboandersen 21:6dce9000da11 185 turn_left();
janusboandersen 21:6dce9000da11 186 m3pi.stop();
janusboandersen 21:6dce9000da11 187 }
janusboandersen 21:6dce9000da11 188 }
janusboandersen 20:32e5cf8fe6dd 189
janusboandersen 22:fe373ba68df3 190 //TO DO: IMPLEMENT A WH CALCULATION
janusboandersen 22:fe373ba68df3 191 /* Begin the charging by calling when parked in pit
janusboandersen 22:fe373ba68df3 192 * Charges for a called number of seconds (int charge_time)
janusboandersen 22:fe373ba68df3 193 * Return the amount of Wh charged during each charging session
janusboandersen 22:fe373ba68df3 194 * Function keeps control as long as charging
janusboandersen 22:fe373ba68df3 195 */
janusboandersen 22:fe373ba68df3 196 float charge(int charge_time) {
janusboandersen 22:fe373ba68df3 197
janusboandersen 22:fe373ba68df3 198 float wh_this_session = 0;
janusboandersen 22:fe373ba68df3 199
janusboandersen 22:fe373ba68df3 200 for (int i = 0; i < charge_time; i++) {
janusboandersen 22:fe373ba68df3 201
janusboandersen 22:fe373ba68df3 202 // PERFORM SOME WEIRD CALCULATION HERE
janusboandersen 22:fe373ba68df3 203 wait(1.0);
janusboandersen 22:fe373ba68df3 204
janusboandersen 22:fe373ba68df3 205 } //end for
janusboandersen 22:fe373ba68df3 206
janusboandersen 22:fe373ba68df3 207 //return the amount of Wh charged
janusboandersen 22:fe373ba68df3 208 return wh_this_session;
janusboandersen 22:fe373ba68df3 209 }
janusboandersen 22:fe373ba68df3 210
janusboandersen 22:fe373ba68df3 211
janusboandersen 24:7fa7383e1b07 212 //display values from the performance data struct in the display
janusboandersen 24:7fa7383e1b07 213 void show_car_data(performance_data data) {
janusboandersen 24:7fa7383e1b07 214
janusboandersen 24:7fa7383e1b07 215 /* Format:
janusboandersen 24:7fa7383e1b07 216 xxx.xxWh
janusboandersen 24:7fa7383e1b07 217 0000xxPs
Blunsdon 13:6a8689f601cd 218
janusboandersen 24:7fa7383e1b07 219 Formats are specified according to guesstimates with the following calcs
janusboandersen 24:7fa7383e1b07 220 Wh = u * i * t = 6 V * 0.5 A * 1/3h * 20 times = 20 Wh
janusboandersen 24:7fa7383e1b07 221 Bilforbrug: ca. 800 mA -> over 20 timer -> 16000 mAh -> 16 Ah
janusboandersen 24:7fa7383e1b07 222 16 Ah * 5 V = 80 Wh.
janusboandersen 24:7fa7383e1b07 223 -> go for something between 20 - 80 Wh, with two decimals
janusboandersen 24:7fa7383e1b07 224 */
janusboandersen 24:7fa7383e1b07 225
janusboandersen 24:7fa7383e1b07 226
janusboandersen 24:7fa7383e1b07 227 //clear screen
janusboandersen 24:7fa7383e1b07 228 m3pi.cls();
janusboandersen 24:7fa7383e1b07 229
janusboandersen 24:7fa7383e1b07 230 //print Wh in first line
janusboandersen 24:7fa7383e1b07 231 m3pi.locate(0,0);
janusboandersen 24:7fa7383e1b07 232 printf("%06.2fWh", data.wh); //6 wide in total, 2 after comma, pad with zero
janusboandersen 24:7fa7383e1b07 233
janusboandersen 24:7fa7383e1b07 234 //print Pitstops in second line
janusboandersen 24:7fa7383e1b07 235 m3pi.locate(0,1);
janusboandersen 24:7fa7383e1b07 236 printf("%06dPs", data.pitstops); //6 wide in total, padded with zeros
janusboandersen 24:7fa7383e1b07 237
janusboandersen 24:7fa7383e1b07 238 }
Blunsdon 14:408d9d087d0f 239
chris 0:78f9794620a3 240
chris 0:78f9794620a3 241 int main() {
janusboandersen 18:8b3efa4d4a36 242
janusboandersen 19:c0d59019de53 243 /* Define all needed variables in this section */
janusboandersen 19:c0d59019de53 244 performance_data car_data; //Saves the car's Wh and pitstops data
janusboandersen 19:c0d59019de53 245
janusboandersen 22:fe373ba68df3 246 int laps = 0; //experimantal lap counter
janusboandersen 19:c0d59019de53 247 float right; //right motor speed
janusboandersen 19:c0d59019de53 248 float left; //left motor speed
janusboandersen 19:c0d59019de53 249 float current_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 250 float previous_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 251 float derivative,proportional,integral = 0; //for PID
janusboandersen 19:c0d59019de53 252 float power; //differential speed
janusboandersen 19:c0d59019de53 253 float speed = OPTIMAL_SPEED; //our optimal speed setting
janusboandersen 21:6dce9000da11 254 int sensor_values[5]; //array for reflectance sensor values
janusboandersen 22:fe373ba68df3 255 bool battery_low_flag = false; //goes true when battery needs charging
janusboandersen 22:fe373ba68df3 256 bool pit_crossroad_flag = false; //goes true when car is at the pit crossrd
janusboandersen 24:7fa7383e1b07 257 bool button_pressed_flag = false; //goes true when the button has been prs
janusboandersen 22:fe373ba68df3 258
janusboandersen 24:7fa7383e1b07 259
janusboandersen 24:7fa7383e1b07 260 //define the button: user push button on p21
janusboandersen 24:7fa7383e1b07 261 DigitalIn button(p21);
janusboandersen 24:7fa7383e1b07 262 button.mode(PullUp); //1 if not pressed, 0 if pressed
janusboandersen 2:d22dcc2bfcc1 263
janusboandersen 19:c0d59019de53 264 /* When the car starts, read previous progress
janusboandersen 19:c0d59019de53 265 * if only connected to USB, do not perform this restore process
janusboandersen 19:c0d59019de53 266 */
janusboandersen 19:c0d59019de53 267
janusboandersen 19:c0d59019de53 268 // If robot is on this returns a value, otherwise it's just connected USB
janusboandersen 19:c0d59019de53 269 if (m3pi.battery() > 0) {
janusboandersen 19:c0d59019de53 270
janusboandersen 19:c0d59019de53 271 car_data = read_from_file(); //restore data from file
janusboandersen 19:c0d59019de53 272
janusboandersen 19:c0d59019de53 273 }
janusboandersen 19:c0d59019de53 274
janusboandersen 19:c0d59019de53 275 //Info to user
chris 0:78f9794620a3 276 m3pi.locate(0,1);
chris 0:78f9794620a3 277 m3pi.printf("Line PID");
chris 0:78f9794620a3 278
janusboandersen 19:c0d59019de53 279 //Calibrate before starting <- might be moved somewhere else
chris 0:78f9794620a3 280 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 281
janusboandersen 19:c0d59019de53 282 //Begin main event loop
chris 0:78f9794620a3 283 while (1) {
janusboandersen 17:c2e21d347ca5 284
janusboandersen 22:fe373ba68df3 285 /* Check sensors and update sensor-based flags
janusboandersen 20:32e5cf8fe6dd 286 * - might not be every loop: wrap this in a timer of sorts */
janusboandersen 21:6dce9000da11 287 battery_low_flag = battery_low();
janusboandersen 22:fe373ba68df3 288
janusboandersen 22:fe373ba68df3 289 pit_crossroad_flag = car_at_pit_crossroad(sensor_values); //anden frekvens?
janusboandersen 20:32e5cf8fe6dd 290
janusboandersen 24:7fa7383e1b07 291 //debounce the button
janusboandersen 24:7fa7383e1b07 292 if (button == 0) {
janusboandersen 24:7fa7383e1b07 293 button_pressed_flag = true;
janusboandersen 24:7fa7383e1b07 294 }
janusboandersen 24:7fa7383e1b07 295
janusboandersen 24:7fa7383e1b07 296
janusboandersen 17:c2e21d347ca5 297 /*
janusboandersen 17:c2e21d347ca5 298 switch
janusboandersen 17:c2e21d347ca5 299 if nothing else: line following
janusboandersen 20:32e5cf8fe6dd 300
janusboandersen 20:32e5cf8fe6dd 301 */
janusboandersen 20:32e5cf8fe6dd 302
janusboandersen 24:7fa7383e1b07 303 //if the button is pressed, show some stats on the display
janusboandersen 24:7fa7383e1b07 304 if ( button_pressed_flag ) {
janusboandersen 24:7fa7383e1b07 305
janusboandersen 24:7fa7383e1b07 306 //call the function to print car data
janusboandersen 24:7fa7383e1b07 307 show_car_data(car_data);
janusboandersen 24:7fa7383e1b07 308
janusboandersen 24:7fa7383e1b07 309 //reset the flag
janusboandersen 24:7fa7383e1b07 310 button_pressed_flag = false;
janusboandersen 24:7fa7383e1b07 311 }
janusboandersen 24:7fa7383e1b07 312
janusboandersen 21:6dce9000da11 313 //if low_batt: decellerate car (slow down) and continue to pit
janusboandersen 20:32e5cf8fe6dd 314 if ( battery_low_flag ) {
janusboandersen 22:fe373ba68df3 315 speed = (speed > PIT_SPEED) ? speed-DECELLERATION : PIT_SPEED;
janusboandersen 22:fe373ba68df3 316 }
janusboandersen 22:fe373ba68df3 317
janusboandersen 22:fe373ba68df3 318 //if @pitcrossrd, not going to charge: increment internal lap counter
janusboandersen 22:fe373ba68df3 319 if ( pit_crossroad_flag ) {
janusboandersen 22:fe373ba68df3 320 laps++; //might count multiple times if the loop is too fast
janusboandersen 20:32e5cf8fe6dd 321 }
janusboandersen 19:c0d59019de53 322
janusboandersen 20:32e5cf8fe6dd 323 //if low_batt && atPitIntersection: Stop && execute pit navigation program
janusboandersen 22:fe373ba68df3 324 if ( battery_low_flag && pit_crossroad_flag ) {
janusboandersen 21:6dce9000da11 325 //We are at the pit, and the battery is low
janusboandersen 21:6dce9000da11 326
janusboandersen 22:fe373ba68df3 327 //Enter the pit
janusboandersen 22:fe373ba68df3 328 pit_navigate(ENTER_PIT);
janusboandersen 22:fe373ba68df3 329
janusboandersen 22:fe373ba68df3 330 //Call charging function
janusboandersen 22:fe373ba68df3 331 //When done, it will return the amount of Wh charged
janusboandersen 22:fe373ba68df3 332 car_data.wh += charge(CHARGE_TIME);
janusboandersen 22:fe373ba68df3 333
janusboandersen 22:fe373ba68df3 334 //increment the number of pitstops
janusboandersen 22:fe373ba68df3 335 car_data.pitstops += 1;
janusboandersen 22:fe373ba68df3 336
janusboandersen 22:fe373ba68df3 337 //backup data to persistent file
janusboandersen 22:fe373ba68df3 338 write_to_file(car_data);
janusboandersen 22:fe373ba68df3 339
janusboandersen 22:fe373ba68df3 340 //Leave the pit
janusboandersen 22:fe373ba68df3 341 pit_navigate(LEAVE_PIT);
janusboandersen 22:fe373ba68df3 342
janusboandersen 22:fe373ba68df3 343 //Reset pit-related flags
janusboandersen 22:fe373ba68df3 344 battery_low_flag = false;
janusboandersen 22:fe373ba68df3 345 pit_crossroad_flag = false;
janusboandersen 21:6dce9000da11 346 }
janusboandersen 19:c0d59019de53 347
janusboandersen 20:32e5cf8fe6dd 348 /*
janusboandersen 17:c2e21d347ca5 349 if all_sensors == 1000 (lifted from track):
janusboandersen 17:c2e21d347ca5 350 show Wh and pitstops on display
janusboandersen 17:c2e21d347ca5 351 stop motors, reset PID errors, wait 20 secs, recalibrate sensors
janusboandersen 17:c2e21d347ca5 352
janusboandersen 17:c2e21d347ca5 353 if all_sensors < 90 (driven off track):
janusboandersen 17:c2e21d347ca5 354 stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
janusboandersen 17:c2e21d347ca5 355
janusboandersen 17:c2e21d347ca5 356 Periodically (not necessarily every lap - maybe every 2-5th lap):
janusboandersen 17:c2e21d347ca5 357 Save to file -> possibly only after pit
janusboandersen 17:c2e21d347ca5 358 Check battery voltage -> every couple of minutes
janusboandersen 17:c2e21d347ca5 359 Check light sensors for drift -> recalibrate (optional)
janusboandersen 17:c2e21d347ca5 360
janusboandersen 17:c2e21d347ca5 361 */
janusboandersen 17:c2e21d347ca5 362
janusboandersen 17:c2e21d347ca5 363
janusboandersen 17:c2e21d347ca5 364
janusboandersen 17:c2e21d347ca5 365 /* Separate functions
janusboandersen 17:c2e21d347ca5 366 pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
janusboandersen 17:c2e21d347ca5 367 charge and wait for signal from the external charging circuit
janusboandersen 17:c2e21d347ca5 368 during charging: integrate v(t) dt, add to cumulative sum
janusboandersen 17:c2e21d347ca5 369 when fully charged (or signal goes high):
janusboandersen 17:c2e21d347ca5 370 Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 )
janusboandersen 17:c2e21d347ca5 371 Calculate total_Wh
janusboandersen 23:e1e6411e5221 372
janusboandersen 17:c2e21d347ca5 373
janusboandersen 23:e1e6411e5221 374 AfterPitAdmin: Calibrate sensors
janusboandersen 23:e1e6411e5221 375 show_results_on_display
janusboandersen 23:e1e6411e5221 376 check_sensors_for_errors
janusboandersen 17:c2e21d347ca5 377 */
janusboandersen 17:c2e21d347ca5 378
janusboandersen 17:c2e21d347ca5 379
Blunsdon 11:394ab193971f 380 // Get the position of the line.
chris 0:78f9794620a3 381 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 382 proportional = current_pos_of_line;
chris 0:78f9794620a3 383
chris 0:78f9794620a3 384 // Compute the derivative
chris 0:78f9794620a3 385 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 386
chris 0:78f9794620a3 387 // Compute the integral
chris 0:78f9794620a3 388 integral += proportional;
chris 0:78f9794620a3 389
chris 0:78f9794620a3 390 // Remember the last position.
chris 0:78f9794620a3 391 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 392
chris 0:78f9794620a3 393 // Compute the power
chris 0:78f9794620a3 394 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 395
chris 0:78f9794620a3 396 // Compute new speeds
chris 0:78f9794620a3 397 right = speed+power;
chris 0:78f9794620a3 398 left = speed-power;
chris 0:78f9794620a3 399
chris 0:78f9794620a3 400 // limit checks
chris 0:78f9794620a3 401 if (right < MIN)
chris 0:78f9794620a3 402 right = MIN;
chris 0:78f9794620a3 403 else if (right > MAX)
chris 0:78f9794620a3 404 right = MAX;
chris 0:78f9794620a3 405
chris 0:78f9794620a3 406 if (left < MIN)
chris 0:78f9794620a3 407 left = MIN;
chris 0:78f9794620a3 408 else if (left > MAX)
chris 0:78f9794620a3 409 left = MAX;
chris 0:78f9794620a3 410
chris 0:78f9794620a3 411 // set speed
chris 0:78f9794620a3 412 m3pi.left_motor(left);
chris 0:78f9794620a3 413 m3pi.right_motor(right);
chris 0:78f9794620a3 414
janusboandersen 21:6dce9000da11 415 } //end while
janusboandersen 21:6dce9000da11 416 } //end main