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@20:32e5cf8fe6dd, 2018-12-04 (annotated)

Committer:
janusboandersen
Date:
Tue Dec 04 15:26:28 2018 +0000
Revision:
20:32e5cf8fe6dd
Parent:
19:c0d59019de53
Child:
21:6dce9000da11
implement checks for car at pit and low battery.

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
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
janusboandersen 18:8b3efa4d4a36 6 //File name
janusboandersen 18:8b3efa4d4a36 7 #define FILESYS "robot"
janusboandersen 18:8b3efa4d4a36 8 #define FILENAME "/robot/data.txt"
janusboandersen 18:8b3efa4d4a36 9
janusboandersen 20:32e5cf8fe6dd 10 //Team calibrated parameters for car and battery
janusboandersen 20:32e5cf8fe6dd 11 #define LOW_BATTERY_VOLTAGE 4.2
janusboandersen 20:32e5cf8fe6dd 12 #define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
janusboandersen 20:32e5cf8fe6dd 13
janusboandersen 20:32e5cf8fe6dd 14 //Car sensors and LEDs
janusboandersen 20:32e5cf8fe6dd 15 #define PC0 0 //location of PC0 reflectance sensor
janusboandersen 20:32e5cf8fe6dd 16 #define PC4 4 //location of PC4 reflectance sensor
janusboandersen 20:32e5cf8fe6dd 17
chris 0:78f9794620a3 18 // Minimum and maximum motor speeds
janusboandersen 19:c0d59019de53 19 #define MAX 1.0
chris 0:78f9794620a3 20 #define MIN 0
janusboandersen 19:c0d59019de53 21 #define OPTIMAL_SPEED 0.8
janusboandersen 19:c0d59019de53 22 #define PIT_SPEED 0.3
chris 0:78f9794620a3 23
chris 0:78f9794620a3 24 // PID terms
chris 0:78f9794620a3 25 #define P_TERM 1
janusboandersen 19:c0d59019de53 26 #define D_TERM 8
chris 0:78f9794620a3 27 #define I_TERM 0
janusboandersen 19:c0d59019de53 28
janusboandersen 19:c0d59019de53 29 //Forward declare our type for storing car performance date
janusboandersen 19:c0d59019de53 30 //It can now be used as a type by the name performance_data
janusboandersen 19:c0d59019de53 31 typedef struct pd {
janusboandersen 19:c0d59019de53 32 float wh;
janusboandersen 19:c0d59019de53 33 int pitstops;
janusboandersen 19:c0d59019de53 34 } performance_data;
Blunsdon 1:47ea51d9e25d 35
janusboandersen 18:8b3efa4d4a36 36
janusboandersen 19:c0d59019de53 37 //Function to write car performance data to persistent file
janusboandersen 18:8b3efa4d4a36 38 void write_to_file(performance_data data) {
janusboandersen 18:8b3efa4d4a36 39
janusboandersen 18:8b3efa4d4a36 40 //Define file system
janusboandersen 18:8b3efa4d4a36 41 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 42
janusboandersen 19:c0d59019de53 43 //Open a file (or create a new) with write access
janusboandersen 19:c0d59019de53 44 //"w": write "r":read "a":append (add @end)
janusboandersen 19:c0d59019de53 45 //"w": will overwrite any existing data, so we just store one line.
janusboandersen 19:c0d59019de53 46 FILE *fp = fopen(FILENAME, "w");
janusboandersen 18:8b3efa4d4a36 47
janusboandersen 19:c0d59019de53 48 //write performance data to the file
janusboandersen 19:c0d59019de53 49 fprintf(fp, "%f %d", data.wh, data.pitstops);
janusboandersen 18:8b3efa4d4a36 50
janusboandersen 19:c0d59019de53 51 //close the file and release the memory when done.
janusboandersen 19:c0d59019de53 52 fclose(fp);
janusboandersen 18:8b3efa4d4a36 53
janusboandersen 18:8b3efa4d4a36 54 }
janusboandersen 18:8b3efa4d4a36 55
janusboandersen 19:c0d59019de53 56 //Function to read car performance data from persistent file
janusboandersen 18:8b3efa4d4a36 57 performance_data read_from_file() {
janusboandersen 18:8b3efa4d4a36 58
janusboandersen 19:c0d59019de53 59 //Declare the variable which will ultimately be returned
janusboandersen 18:8b3efa4d4a36 60 performance_data data;
janusboandersen 18:8b3efa4d4a36 61
janusboandersen 19:c0d59019de53 62 //Define file system on mbed LPC1768
janusboandersen 18:8b3efa4d4a36 63 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 64
janusboandersen 19:c0d59019de53 65 //Try to open the file as read (it might not exist)
janusboandersen 19:c0d59019de53 66 FILE *fp = fopen(FILENAME, "r");
janusboandersen 19:c0d59019de53 67
janusboandersen 19:c0d59019de53 68 //Test whether the file exists
janusboandersen 19:c0d59019de53 69 if (fp != NULL) {
janusboandersen 19:c0d59019de53 70
janusboandersen 19:c0d59019de53 71 //file exists, so read from it
janusboandersen 19:c0d59019de53 72
janusboandersen 19:c0d59019de53 73 //Read stored data from the file and put in struct
janusboandersen 19:c0d59019de53 74 // &(struct.member) and &struct.member yield same address
janusboandersen 19:c0d59019de53 75 fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) );
janusboandersen 18:8b3efa4d4a36 76
janusboandersen 19:c0d59019de53 77 //close file and release mem.
janusboandersen 19:c0d59019de53 78 fclose(fp);
janusboandersen 19:c0d59019de53 79
janusboandersen 19:c0d59019de53 80 } else {
janusboandersen 19:c0d59019de53 81
janusboandersen 19:c0d59019de53 82 //file doesn't exist, so just return zeros (no performance yet)
janusboandersen 19:c0d59019de53 83 data.wh = 0.0;
janusboandersen 19:c0d59019de53 84 data.pitstops = 0;
janusboandersen 19:c0d59019de53 85 }
janusboandersen 18:8b3efa4d4a36 86
janusboandersen 19:c0d59019de53 87 //return the data object to the caller
janusboandersen 18:8b3efa4d4a36 88 return data;
janusboandersen 19:c0d59019de53 89 }
janusboandersen 19:c0d59019de53 90
janusboandersen 20:32e5cf8fe6dd 91 //This function tests the car's battery state, and returs true if it needs
janusboandersen 20:32e5cf8fe6dd 92 //to charge. Otherwise false is returned.
janusboandersen 19:c0d59019de53 93 bool battery_low() {
janusboandersen 20:32e5cf8fe6dd 94
janusboandersen 20:32e5cf8fe6dd 95 //Get battery level from the 3pi robot
janusboandersen 20:32e5cf8fe6dd 96 float voltage = m3pi.battery();
janusboandersen 20:32e5cf8fe6dd 97
janusboandersen 20:32e5cf8fe6dd 98 //return battery state result
janusboandersen 20:32e5cf8fe6dd 99 return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
janusboandersen 18:8b3efa4d4a36 100 }
janusboandersen 18:8b3efa4d4a36 101
janusboandersen 18:8b3efa4d4a36 102
janusboandersen 19:c0d59019de53 103 //TO DO: CLEAN UP THIS FUNCTION
Blunsdon 12:dfc717775297 104 void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
Blunsdon 1:47ea51d9e25d 105 {
Blunsdon 12:dfc717775297 106 m3pi.putc(0x87); // send noget sensor data
Blunsdon 1:47ea51d9e25d 107
janusboandersen 4:5939f9ad88ca 108 for(int n=0; n < 5; n++) // forloop så sensor for data
Blunsdon 1:47ea51d9e25d 109 {
Blunsdon 1:47ea51d9e25d 110 char lowbyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 111 char hibyte = m3pi.getc();
Blunsdon 12:dfc717775297 112 arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
Blunsdon 1:47ea51d9e25d 113 }
Blunsdon 9:fa745d3afcac 114 m3pi.cls();
Blunsdon 9:fa745d3afcac 115 m3pi.locate(0,1);
Blunsdon 12:dfc717775297 116 m3pi.printf("%d", arr[0]);
Blunsdon 12:dfc717775297 117 m3pi.locate(0,0);
Blunsdon 12:dfc717775297 118 m3pi.printf("%d", arr[4]);
janusboandersen 20:32e5cf8fe6dd 119 }
janusboandersen 20:32e5cf8fe6dd 120
janusboandersen 20:32e5cf8fe6dd 121 //Function to check whether car is just at the pit
janusboandersen 20:32e5cf8fe6dd 122 bool car_at_pit_crossroad(int sensor_array[]) {
janusboandersen 20:32e5cf8fe6dd 123
janusboandersen 20:32e5cf8fe6dd 124 //Read the sensor values
janusboandersen 20:32e5cf8fe6dd 125 sensor_all(sensor_array);
janusboandersen 20:32e5cf8fe6dd 126
janusboandersen 20:32e5cf8fe6dd 127 //Check the PC0 and PC4 sensors vs thresholds
janusboandersen 20:32e5cf8fe6dd 128 if (sensor_array[PC0] > CROSSROAD_REFL_VAL &&
janusboandersen 20:32e5cf8fe6dd 129 sensor_array[PC4] > CROSSROAD_REFL_VAL) {
janusboandersen 20:32e5cf8fe6dd 130
janusboandersen 20:32e5cf8fe6dd 131 //Car is at the crossroad
janusboandersen 20:32e5cf8fe6dd 132 return true;
janusboandersen 20:32e5cf8fe6dd 133 } else {
janusboandersen 20:32e5cf8fe6dd 134 //Car is NOT at the crossroad
janusboandersen 20:32e5cf8fe6dd 135 return false;
janusboandersen 20:32e5cf8fe6dd 136 }
janusboandersen 20:32e5cf8fe6dd 137 }
janusboandersen 20:32e5cf8fe6dd 138
janusboandersen 20:32e5cf8fe6dd 139
Blunsdon 13:6a8689f601cd 140 int maze (void)
Blunsdon 13:6a8689f601cd 141 {
Blunsdon 13:6a8689f601cd 142
Blunsdon 13:6a8689f601cd 143 return 1;
Blunsdon 14:408d9d087d0f 144 }
Blunsdon 14:408d9d087d0f 145
Blunsdon 14:408d9d087d0f 146 void turn_left (void)
Blunsdon 14:408d9d087d0f 147 {
Blunsdon 14:408d9d087d0f 148 m3pi.left(0.3);
Blunsdon 14:408d9d087d0f 149 wait (0.22);
Blunsdon 14:408d9d087d0f 150 }
Blunsdon 14:408d9d087d0f 151
Blunsdon 13:6a8689f601cd 152 void turn_right (void)
Blunsdon 13:6a8689f601cd 153 {
Blunsdon 13:6a8689f601cd 154 m3pi.right (0.3);
Blunsdon 14:408d9d087d0f 155 wait (0.22);
Blunsdon 13:6a8689f601cd 156 }
chris 0:78f9794620a3 157
chris 0:78f9794620a3 158 int main() {
janusboandersen 18:8b3efa4d4a36 159
janusboandersen 19:c0d59019de53 160 /* Define all needed variables in this section */
janusboandersen 19:c0d59019de53 161 performance_data car_data; //Saves the car's Wh and pitstops data
janusboandersen 19:c0d59019de53 162
janusboandersen 19:c0d59019de53 163 float right; //right motor speed
janusboandersen 19:c0d59019de53 164 float left; //left motor speed
janusboandersen 19:c0d59019de53 165 float current_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 166 float previous_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 167 float derivative,proportional,integral = 0; //for PID
janusboandersen 19:c0d59019de53 168 float power; //differential speed
janusboandersen 19:c0d59019de53 169 float speed = OPTIMAL_SPEED; //our optimal speed setting
janusboandersen 19:c0d59019de53 170 int sensor_val[5]; //array for reflectance sensor values
janusboandersen 20:32e5cf8fe6dd 171 bool battery_low_flag = false;
janusboandersen 2:d22dcc2bfcc1 172
janusboandersen 19:c0d59019de53 173 /* When the car starts, read previous progress
janusboandersen 19:c0d59019de53 174 * if only connected to USB, do not perform this restore process
janusboandersen 19:c0d59019de53 175 */
janusboandersen 19:c0d59019de53 176
janusboandersen 19:c0d59019de53 177 // If robot is on this returns a value, otherwise it's just connected USB
janusboandersen 19:c0d59019de53 178 if (m3pi.battery() > 0) {
janusboandersen 19:c0d59019de53 179
janusboandersen 19:c0d59019de53 180 car_data = read_from_file(); //restore data from file
janusboandersen 19:c0d59019de53 181
janusboandersen 19:c0d59019de53 182 }
janusboandersen 19:c0d59019de53 183
janusboandersen 19:c0d59019de53 184 //Info to user
chris 0:78f9794620a3 185 m3pi.locate(0,1);
chris 0:78f9794620a3 186 m3pi.printf("Line PID");
chris 0:78f9794620a3 187
janusboandersen 19:c0d59019de53 188 //Calibrate before starting <- might be moved somewhere else
chris 0:78f9794620a3 189 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 190
janusboandersen 19:c0d59019de53 191 //Begin main event loop
chris 0:78f9794620a3 192 while (1) {
janusboandersen 17:c2e21d347ca5 193
janusboandersen 20:32e5cf8fe6dd 194 /* Check sensors
janusboandersen 20:32e5cf8fe6dd 195 * - might not be every loop: wrap this in a timer of sorts */
janusboandersen 20:32e5cf8fe6dd 196 bool battery_low_flag = battery_low();
janusboandersen 20:32e5cf8fe6dd 197
janusboandersen 20:32e5cf8fe6dd 198
janusboandersen 17:c2e21d347ca5 199 /*
janusboandersen 17:c2e21d347ca5 200 switch
janusboandersen 17:c2e21d347ca5 201 if nothing else: line following
janusboandersen 20:32e5cf8fe6dd 202
janusboandersen 20:32e5cf8fe6dd 203 */
janusboandersen 20:32e5cf8fe6dd 204
janusboandersen 20:32e5cf8fe6dd 205 //if low_batt: decelarate car (slow down) and continue to pit
janusboandersen 20:32e5cf8fe6dd 206 if ( battery_low_flag ) {
janusboandersen 20:32e5cf8fe6dd 207 speed = (speed > PIT_SPEED) ? speed-0.1 : PIT_SPEED;
janusboandersen 20:32e5cf8fe6dd 208 }
janusboandersen 19:c0d59019de53 209
janusboandersen 20:32e5cf8fe6dd 210 /*
janusboandersen 20:32e5cf8fe6dd 211 if atPitIntersection: increment internal lap counter
janusboandersen 20:32e5cf8fe6dd 212 */
janusboandersen 19:c0d59019de53 213
janusboandersen 20:32e5cf8fe6dd 214 //if low_batt && atPitIntersection: Stop && execute pit navigation program
janusboandersen 19:c0d59019de53 215
janusboandersen 20:32e5cf8fe6dd 216 /*
janusboandersen 17:c2e21d347ca5 217 if all_sensors == 1000 (lifted from track):
janusboandersen 17:c2e21d347ca5 218 show Wh and pitstops on display
janusboandersen 17:c2e21d347ca5 219 stop motors, reset PID errors, wait 20 secs, recalibrate sensors
janusboandersen 17:c2e21d347ca5 220
janusboandersen 17:c2e21d347ca5 221 if all_sensors < 90 (driven off track):
janusboandersen 17:c2e21d347ca5 222 stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
janusboandersen 17:c2e21d347ca5 223
janusboandersen 17:c2e21d347ca5 224 Periodically (not necessarily every lap - maybe every 2-5th lap):
janusboandersen 17:c2e21d347ca5 225 Save to file -> possibly only after pit
janusboandersen 17:c2e21d347ca5 226 Check battery voltage -> every couple of minutes
janusboandersen 17:c2e21d347ca5 227 Check light sensors for drift -> recalibrate (optional)
janusboandersen 17:c2e21d347ca5 228
janusboandersen 17:c2e21d347ca5 229 */
janusboandersen 17:c2e21d347ca5 230
janusboandersen 17:c2e21d347ca5 231
janusboandersen 17:c2e21d347ca5 232
janusboandersen 17:c2e21d347ca5 233 /* Separate functions
janusboandersen 17:c2e21d347ca5 234 pit_navigation (entering pit from T-intersection all the way to the charger)
janusboandersen 17:c2e21d347ca5 235 pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
janusboandersen 17:c2e21d347ca5 236 add 1 to number of pitstops
janusboandersen 17:c2e21d347ca5 237 charge and wait for signal from the external charging circuit
janusboandersen 17:c2e21d347ca5 238 during charging: integrate v(t) dt, add to cumulative sum
janusboandersen 17:c2e21d347ca5 239 when fully charged (or signal goes high):
janusboandersen 17:c2e21d347ca5 240 Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 )
janusboandersen 17:c2e21d347ca5 241 Calculate total_Wh
janusboandersen 17:c2e21d347ca5 242 save progress data to file
janusboandersen 17:c2e21d347ca5 243 leave the charger -> pass control to pit_exit
janusboandersen 17:c2e21d347ca5 244
janusboandersen 17:c2e21d347ca5 245 pit_exit
janusboandersen 17:c2e21d347ca5 246 reverse out of the pit back to T-intersection
janusboandersen 17:c2e21d347ca5 247 continue line following away from T-intersection, recal sensors after a few seconds
janusboandersen 17:c2e21d347ca5 248
janusboandersen 17:c2e21d347ca5 249
janusboandersen 17:c2e21d347ca5 250 AfterPitAdmin: Calibrate sensors, save state to file,
janusboandersen 17:c2e21d347ca5 251
janusboandersen 17:c2e21d347ca5 252 show_results_on_display:
janusboandersen 17:c2e21d347ca5 253
janusboandersen 17:c2e21d347ca5 254 check_sensors_for_errors:
janusboandersen 17:c2e21d347ca5 255
janusboandersen 17:c2e21d347ca5 256 check_battery_voltage -> return true false
janusboandersen 17:c2e21d347ca5 257 compare m3pi.battery() < V_CRITICAL
janusboandersen 17:c2e21d347ca5 258
janusboandersen 17:c2e21d347ca5 259
janusboandersen 17:c2e21d347ca5 260
janusboandersen 17:c2e21d347ca5 261 HUSK FOR POKKER KOLLES KODESTANDARD!
janusboandersen 17:c2e21d347ca5 262
janusboandersen 17:c2e21d347ca5 263 */
janusboandersen 17:c2e21d347ca5 264
janusboandersen 17:c2e21d347ca5 265
Blunsdon 11:394ab193971f 266 // Get the position of the line.
chris 0:78f9794620a3 267 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 268 proportional = current_pos_of_line;
chris 0:78f9794620a3 269
chris 0:78f9794620a3 270 // Compute the derivative
chris 0:78f9794620a3 271 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 272
chris 0:78f9794620a3 273 // Compute the integral
chris 0:78f9794620a3 274 integral += proportional;
chris 0:78f9794620a3 275
chris 0:78f9794620a3 276 // Remember the last position.
chris 0:78f9794620a3 277 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 278
chris 0:78f9794620a3 279 // Compute the power
chris 0:78f9794620a3 280 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 281
chris 0:78f9794620a3 282 // Compute new speeds
chris 0:78f9794620a3 283 right = speed+power;
chris 0:78f9794620a3 284 left = speed-power;
chris 0:78f9794620a3 285
chris 0:78f9794620a3 286 // limit checks
chris 0:78f9794620a3 287 if (right < MIN)
chris 0:78f9794620a3 288 right = MIN;
chris 0:78f9794620a3 289 else if (right > MAX)
chris 0:78f9794620a3 290 right = MAX;
chris 0:78f9794620a3 291
chris 0:78f9794620a3 292 if (left < MIN)
chris 0:78f9794620a3 293 left = MIN;
chris 0:78f9794620a3 294 else if (left > MAX)
chris 0:78f9794620a3 295 left = MAX;
chris 0:78f9794620a3 296
chris 0:78f9794620a3 297 // set speed
chris 0:78f9794620a3 298 m3pi.left_motor(left);
chris 0:78f9794620a3 299 m3pi.right_motor(right);
Blunsdon 11:394ab193971f 300
janusboandersen 2:d22dcc2bfcc1 301 //Marc's sensor test
Blunsdon 12:dfc717775297 302 sensor_all(sensor_val); // sensor_value gets fkt value
Blunsdon 13:6a8689f601cd 303 if (sensor_val[0] > 350 && sensor_val[4] > 350)
Blunsdon 12:dfc717775297 304 {
Blunsdon 13:6a8689f601cd 305
Blunsdon 13:6a8689f601cd 306 m3pi.stop();
Blunsdon 13:6a8689f601cd 307 turn_right();
Blunsdon 13:6a8689f601cd 308 m3pi.stop();
Blunsdon 13:6a8689f601cd 309 m3pi.forward(0.2);
Blunsdon 16:48b81a9246e6 310 wait (1.1);
Blunsdon 14:408d9d087d0f 311 m3pi.stop();
Blunsdon 16:48b81a9246e6 312 wait (5);
Blunsdon 14:408d9d087d0f 313 m3pi.backward(0.2);
Blunsdon 15:fd6886ab896a 314 wait (1);
Blunsdon 14:408d9d087d0f 315 m3pi.stop();
Blunsdon 14:408d9d087d0f 316 turn_left();
Blunsdon 13:6a8689f601cd 317 m3pi.stop();
Blunsdon 12:dfc717775297 318 }
janusboandersen 5:527c046c7b8a 319 /*
janusboandersen 2:d22dcc2bfcc1 320 sensor_value4 = sensor_all(4); // sensor_value gets fkt value
janusboandersen 6:3be33adda32d 321 */
janusboandersen 7:30c63d6460f6 322
janusboandersen 6:3be33adda32d 323 /*
janusboandersen 2:d22dcc2bfcc1 324 m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
janusboandersen 2:d22dcc2bfcc1 325 m3pi.locate(0,1);
janusboandersen 2:d22dcc2bfcc1 326 m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
janusboandersen 3:47968be0df37 327 */
chris 0:78f9794620a3 328
chris 0:78f9794620a3 329 }
chris 0:78f9794620a3 330 }