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@29:7475bcdce4e6, 2018-12-05 (annotated)

Committer:
janusboandersen
Date:
Wed Dec 05 14:29:51 2018 +0000
Revision:
29:7475bcdce4e6
Parent:
28:216a26d99858
Child:
30:ad77e3ee288b
Implement waiting at pit, then go into charging

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