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@27:a19b4916169b, 2018-12-05 (annotated)

Committer:
janusboandersen
Date:
Wed Dec 05 13:13:43 2018 +0000
Revision:
27:a19b4916169b
Parent:
26:857158816262
Child:
28:216a26d99858
Test to Marc

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