Janus Bo Andersen
/
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.
main.cpp@30:ad77e3ee288b, 2018-12-05 (annotated)
- Committer:
- janusboandersen
- Date:
- Wed Dec 05 19:01:53 2018 +0000
- Revision:
- 30:ad77e3ee288b
- Parent:
- 29:7475bcdce4e6
Initial commit as master (mainline) branch.
Who changed what in which revision?
| User | Revision | Line number | New 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 | 30:ad77e3ee288b | 219 | |
| janusboandersen | 30:ad77e3ee288b | 220 | //reset PID variables |
| janusboandersen | 30:ad77e3ee288b | 221 | proportional = 0; |
| janusboandersen | 30:ad77e3ee288b | 222 | derivative = 0; |
| janusboandersen | 30:ad77e3ee288b | 223 | integral = 0; |
| janusboandersen | 30:ad77e3ee288b | 224 | previous_pos_of_line = 0; //denne skal også nulstilles |
| janusboandersen | 30:ad77e3ee288b | 225 | |
| janusboandersen | 30:ad77e3ee288b | 226 | // m3pi.forward(0.1); |
| janusboandersen | 30:ad77e3ee288b | 227 | // wait(0.3); |
| janusboandersen | 30:ad77e3ee288b | 228 | // m3pi.stop(); |
| janusboandersen | 30:ad77e3ee288b | 229 | // m3pi.sensor_auto_calibrate(); |
| janusboandersen | 30:ad77e3ee288b | 230 | |
| janusboandersen | 25:17e9e410795c | 231 | pit_light = 0; |
| janusboandersen | 25:17e9e410795c | 232 | |
| janusboandersen | 21:6dce9000da11 | 233 | } |
| janusboandersen | 19:c0d59019de53 | 234 | |
| janusboandersen | 20:32e5cf8fe6dd | 235 | /* |
| janusboandersen | 17:c2e21d347ca5 | 236 | if all_sensors == 1000 (lifted from track): |
| janusboandersen | 17:c2e21d347ca5 | 237 | show Wh and pitstops on display |
| janusboandersen | 17:c2e21d347ca5 | 238 | stop motors, reset PID errors, wait 20 secs, recalibrate sensors |
| janusboandersen | 17:c2e21d347ca5 | 239 | |
| janusboandersen | 17:c2e21d347ca5 | 240 | if all_sensors < 90 (driven off track): |
| janusboandersen | 17:c2e21d347ca5 | 241 | stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors |
| janusboandersen | 17:c2e21d347ca5 | 242 | |
| janusboandersen | 17:c2e21d347ca5 | 243 | Periodically (not necessarily every lap - maybe every 2-5th lap): |
| janusboandersen | 17:c2e21d347ca5 | 244 | Save to file -> possibly only after pit |
| janusboandersen | 17:c2e21d347ca5 | 245 | Check battery voltage -> every couple of minutes |
| janusboandersen | 17:c2e21d347ca5 | 246 | Check light sensors for drift -> recalibrate (optional) |
| janusboandersen | 17:c2e21d347ca5 | 247 | |
| janusboandersen | 17:c2e21d347ca5 | 248 | */ |
| janusboandersen | 17:c2e21d347ca5 | 249 | |
| janusboandersen | 17:c2e21d347ca5 | 250 | |
| janusboandersen | 17:c2e21d347ca5 | 251 | |
| janusboandersen | 17:c2e21d347ca5 | 252 | /* Separate functions |
| janusboandersen | 17:c2e21d347ca5 | 253 | pit_execution (only when connected to charger - voltage > V_CHARGER_MIN |
| janusboandersen | 17:c2e21d347ca5 | 254 | charge and wait for signal from the external charging circuit |
| janusboandersen | 17:c2e21d347ca5 | 255 | during charging: integrate v(t) dt, add to cumulative sum |
| janusboandersen | 17:c2e21d347ca5 | 256 | when fully charged (or signal goes high): |
| janusboandersen | 17:c2e21d347ca5 | 257 | Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 ) |
| janusboandersen | 17:c2e21d347ca5 | 258 | Calculate total_Wh |
| janusboandersen | 23:e1e6411e5221 | 259 | |
| janusboandersen | 17:c2e21d347ca5 | 260 | |
| janusboandersen | 23:e1e6411e5221 | 261 | AfterPitAdmin: Calibrate sensors |
| janusboandersen | 23:e1e6411e5221 | 262 | show_results_on_display |
| janusboandersen | 23:e1e6411e5221 | 263 | check_sensors_for_errors |
| janusboandersen | 17:c2e21d347ca5 | 264 | */ |
| janusboandersen | 17:c2e21d347ca5 | 265 | |
| janusboandersen | 17:c2e21d347ca5 | 266 | |
| Blunsdon | 11:394ab193971f | 267 | // Get the position of the line. |
| chris | 0:78f9794620a3 | 268 | current_pos_of_line = m3pi.line_position(); |
| chris | 0:78f9794620a3 | 269 | proportional = current_pos_of_line; |
| chris | 0:78f9794620a3 | 270 | |
| chris | 0:78f9794620a3 | 271 | // Compute the derivative |
| chris | 0:78f9794620a3 | 272 | derivative = current_pos_of_line - previous_pos_of_line; |
| chris | 0:78f9794620a3 | 273 | |
| chris | 0:78f9794620a3 | 274 | // Compute the integral |
| chris | 0:78f9794620a3 | 275 | integral += proportional; |
| chris | 0:78f9794620a3 | 276 | |
| chris | 0:78f9794620a3 | 277 | // Remember the last position. |
| chris | 0:78f9794620a3 | 278 | previous_pos_of_line = current_pos_of_line; |
| chris | 0:78f9794620a3 | 279 | |
| chris | 0:78f9794620a3 | 280 | // Compute the power |
| chris | 0:78f9794620a3 | 281 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| chris | 0:78f9794620a3 | 282 | |
| chris | 0:78f9794620a3 | 283 | // Compute new speeds |
| chris | 0:78f9794620a3 | 284 | right = speed+power; |
| chris | 0:78f9794620a3 | 285 | left = speed-power; |
| chris | 0:78f9794620a3 | 286 | |
| chris | 0:78f9794620a3 | 287 | // limit checks |
| chris | 0:78f9794620a3 | 288 | if (right < MIN) |
| chris | 0:78f9794620a3 | 289 | right = MIN; |
| chris | 0:78f9794620a3 | 290 | else if (right > MAX) |
| chris | 0:78f9794620a3 | 291 | right = MAX; |
| chris | 0:78f9794620a3 | 292 | |
| chris | 0:78f9794620a3 | 293 | if (left < MIN) |
| chris | 0:78f9794620a3 | 294 | left = MIN; |
| chris | 0:78f9794620a3 | 295 | else if (left > MAX) |
| chris | 0:78f9794620a3 | 296 | left = MAX; |
| chris | 0:78f9794620a3 | 297 | |
| chris | 0:78f9794620a3 | 298 | // set speed |
| chris | 0:78f9794620a3 | 299 | m3pi.left_motor(left); |
| chris | 0:78f9794620a3 | 300 | m3pi.right_motor(right); |
| chris | 0:78f9794620a3 | 301 | |
| janusboandersen | 21:6dce9000da11 | 302 | } //end while |
| janusboandersen | 26:857158816262 | 303 | } //end main |
| janusboandersen | 26:857158816262 | 304 | |
| janusboandersen | 26:857158816262 | 305 | //Function to write car performance data to persistent file |
| janusboandersen | 26:857158816262 | 306 | void write_to_file(performance_data data) { |
| janusboandersen | 26:857158816262 | 307 | |
| janusboandersen | 26:857158816262 | 308 | //Define file system |
| janusboandersen | 26:857158816262 | 309 | LocalFileSystem local(FILESYS); |
| janusboandersen | 26:857158816262 | 310 | |
| janusboandersen | 26:857158816262 | 311 | //Open a file (or create a new) with write access |
| janusboandersen | 26:857158816262 | 312 | //"w": write "r":read "a":append (add @end) |
| janusboandersen | 26:857158816262 | 313 | //"w": will overwrite any existing data, so we just store one line. |
| janusboandersen | 26:857158816262 | 314 | FILE *fp = fopen(FILENAME, "w"); |
| janusboandersen | 26:857158816262 | 315 | |
| janusboandersen | 26:857158816262 | 316 | //write performance data to the file |
| janusboandersen | 26:857158816262 | 317 | fprintf(fp, "%f %d", data.wh, data.pitstops); |
| janusboandersen | 26:857158816262 | 318 | |
| janusboandersen | 26:857158816262 | 319 | //close the file and release the memory when done. |
| janusboandersen | 26:857158816262 | 320 | fclose(fp); |
| janusboandersen | 26:857158816262 | 321 | |
| janusboandersen | 26:857158816262 | 322 | } |
| janusboandersen | 26:857158816262 | 323 | |
| janusboandersen | 26:857158816262 | 324 | //Function to read car performance data from persistent file |
| janusboandersen | 26:857158816262 | 325 | performance_data read_from_file() { |
| janusboandersen | 26:857158816262 | 326 | |
| janusboandersen | 26:857158816262 | 327 | //Declare the variable which will ultimately be returned |
| janusboandersen | 26:857158816262 | 328 | performance_data data; |
| janusboandersen | 26:857158816262 | 329 | |
| janusboandersen | 26:857158816262 | 330 | //Define file system on mbed LPC1768 |
| janusboandersen | 26:857158816262 | 331 | LocalFileSystem local(FILESYS); |
| janusboandersen | 26:857158816262 | 332 | |
| janusboandersen | 26:857158816262 | 333 | //Try to open the file as read (it might not exist) |
| janusboandersen | 26:857158816262 | 334 | FILE *fp = fopen(FILENAME, "r"); |
| janusboandersen | 26:857158816262 | 335 | |
| janusboandersen | 26:857158816262 | 336 | //Test whether the file exists |
| janusboandersen | 26:857158816262 | 337 | if (fp != NULL) { |
| janusboandersen | 26:857158816262 | 338 | |
| janusboandersen | 26:857158816262 | 339 | //file exists, so read from it |
| janusboandersen | 26:857158816262 | 340 | |
| janusboandersen | 26:857158816262 | 341 | //Read stored data from the file and put in struct |
| janusboandersen | 26:857158816262 | 342 | // &(struct.member) and &struct.member yield same address |
| janusboandersen | 26:857158816262 | 343 | fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) ); |
| janusboandersen | 26:857158816262 | 344 | |
| janusboandersen | 26:857158816262 | 345 | //close file and release mem. |
| janusboandersen | 26:857158816262 | 346 | fclose(fp); |
| janusboandersen | 26:857158816262 | 347 | |
| janusboandersen | 26:857158816262 | 348 | } else { |
| janusboandersen | 26:857158816262 | 349 | |
| janusboandersen | 26:857158816262 | 350 | //file doesn't exist, so just return zeros (no performance yet) |
| janusboandersen | 26:857158816262 | 351 | data.wh = 0.0; |
| janusboandersen | 26:857158816262 | 352 | data.pitstops = 0; |
| janusboandersen | 26:857158816262 | 353 | } |
| janusboandersen | 26:857158816262 | 354 | |
| janusboandersen | 26:857158816262 | 355 | //return the data object to the caller |
| janusboandersen | 26:857158816262 | 356 | return data; |
| janusboandersen | 26:857158816262 | 357 | } |
| janusboandersen | 26:857158816262 | 358 | |
| janusboandersen | 26:857158816262 | 359 | |
| janusboandersen | 26:857158816262 | 360 | |
| janusboandersen | 26:857158816262 | 361 | //This function tests the car's battery state, and returs true if it needs |
| janusboandersen | 26:857158816262 | 362 | //to charge. Otherwise false is returned. |
| janusboandersen | 26:857158816262 | 363 | bool battery_low() { |
| janusboandersen | 26:857158816262 | 364 | |
| janusboandersen | 26:857158816262 | 365 | //Get battery level from the 3pi robot |
| janusboandersen | 26:857158816262 | 366 | float voltage = m3pi.battery(); |
| janusboandersen | 26:857158816262 | 367 | |
| janusboandersen | 26:857158816262 | 368 | //return battery state result |
| janusboandersen | 26:857158816262 | 369 | return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false; |
| janusboandersen | 26:857158816262 | 370 | } |
| janusboandersen | 26:857158816262 | 371 | |
| janusboandersen | 26:857158816262 | 372 | |
| janusboandersen | 26:857158816262 | 373 | //Get the values from 3pi read from reflectance sensors PC0-PC4 |
| janusboandersen | 26:857158816262 | 374 | //The function returns in the array that is provided as function argument |
| janusboandersen | 26:857158816262 | 375 | void sensor_all(int arr[]) { |
| janusboandersen | 26:857158816262 | 376 | |
| janusboandersen | 26:857158816262 | 377 | m3pi.putc(CALIBRATED_SENS_VALS); // request 3pi for calibrated values |
| janusboandersen | 26:857158816262 | 378 | |
| janusboandersen | 26:857158816262 | 379 | //Pick up the received data |
| janusboandersen | 26:857158816262 | 380 | for(int n = PC0; n < NUM_REFL_SENSORS; n++) { // loop through all sensors |
| janusboandersen | 26:857158816262 | 381 | char lowbyte = m3pi.getc(); //LSB: we receive the data "little endian" |
| janusboandersen | 26:857158816262 | 382 | char hibyte = m3pi.getc(); //get the MSB |
| janusboandersen | 26:857158816262 | 383 | arr[n] = ((lowbyte + (hibyte << 8))); // make 2 byte int (8 bits / byte) |
| janusboandersen | 26:857158816262 | 384 | } |
| janusboandersen | 26:857158816262 | 385 | } |
| janusboandersen | 26:857158816262 | 386 | |
| janusboandersen | 26:857158816262 | 387 | //Function to check whether car is just at the pit |
| janusboandersen | 26:857158816262 | 388 | bool car_at_pit_crossroad(int sensor_array[]) { |
| janusboandersen | 26:857158816262 | 389 | |
| janusboandersen | 26:857158816262 | 390 | //Read the sensor values |
| janusboandersen | 26:857158816262 | 391 | sensor_all(sensor_array); |
| janusboandersen | 26:857158816262 | 392 | |
| janusboandersen | 26:857158816262 | 393 | //Check the PC0 and PC4 sensors vs thresholds |
| janusboandersen | 26:857158816262 | 394 | if (sensor_array[PC0] > CROSSROAD_REFL_VAL && |
| janusboandersen | 26:857158816262 | 395 | sensor_array[PC4] > CROSSROAD_REFL_VAL) { |
| janusboandersen | 26:857158816262 | 396 | |
| janusboandersen | 26:857158816262 | 397 | //Car is at the crossroad |
| janusboandersen | 26:857158816262 | 398 | return true; |
| janusboandersen | 26:857158816262 | 399 | } else { |
| janusboandersen | 26:857158816262 | 400 | //Car is NOT at the crossroad |
| janusboandersen | 26:857158816262 | 401 | return false; |
| janusboandersen | 26:857158816262 | 402 | } |
| janusboandersen | 26:857158816262 | 403 | } |
| janusboandersen | 26:857158816262 | 404 | |
| janusboandersen | 26:857158816262 | 405 | //Turn the car 90 degrees left |
| janusboandersen | 26:857158816262 | 406 | void turn_left(void) { |
| janusboandersen | 26:857158816262 | 407 | m3pi.left(TURN_SPEED); |
| janusboandersen | 26:857158816262 | 408 | wait(TURN_TIME); |
| janusboandersen | 26:857158816262 | 409 | } |
| janusboandersen | 26:857158816262 | 410 | |
| janusboandersen | 26:857158816262 | 411 | //Turn the car 90 degrees right |
| janusboandersen | 26:857158816262 | 412 | void turn_right(void) { |
| janusboandersen | 26:857158816262 | 413 | m3pi.right (TURN_SPEED); |
| janusboandersen | 26:857158816262 | 414 | wait(TURN_TIME); |
| janusboandersen | 26:857158816262 | 415 | } |
| janusboandersen | 26:857158816262 | 416 | |
| janusboandersen | 26:857158816262 | 417 | |
| janusboandersen | 26:857158816262 | 418 | //Safely navigate the car from the pit crossroad and all the way to the charger. |
| janusboandersen | 26:857158816262 | 419 | //direction = 1 to enter pit... direction = -1 to leave the pit. |
| janusboandersen | 26:857158816262 | 420 | //takes control all the way from track to pit OR reverse. |
| janusboandersen | 26:857158816262 | 421 | //Error handling: If a wrong code is passed, the function does nothing quietly. |
| janusboandersen | 26:857158816262 | 422 | void pit_navigate(int direction) { |
| janusboandersen | 26:857158816262 | 423 | |
| janusboandersen | 26:857158816262 | 424 | //Car must enter pit forward |
| janusboandersen | 26:857158816262 | 425 | if (direction == ENTER_PIT) { |
| janusboandersen | 26:857158816262 | 426 | m3pi.stop(); |
| janusboandersen | 26:857158816262 | 427 | turn_right(); |
| janusboandersen | 26:857158816262 | 428 | m3pi.stop(); |
| janusboandersen | 26:857158816262 | 429 | m3pi.forward(SPEED_INSIDE_PIT); |
| janusboandersen | 26:857158816262 | 430 | wait(PIT_TRANSPORT_TIME); |
| janusboandersen | 26:857158816262 | 431 | m3pi.stop(); |
| janusboandersen | 26:857158816262 | 432 | } |
| janusboandersen | 26:857158816262 | 433 | |
| janusboandersen | 26:857158816262 | 434 | //Car must leave pit backward |
| janusboandersen | 26:857158816262 | 435 | if (direction == LEAVE_PIT) { |
| janusboandersen | 26:857158816262 | 436 | m3pi.backward(SPEED_INSIDE_PIT); |
| janusboandersen | 26:857158816262 | 437 | wait(PIT_TRANSPORT_TIME); |
| janusboandersen | 26:857158816262 | 438 | m3pi.stop(); |
| janusboandersen | 26:857158816262 | 439 | turn_left(); |
| janusboandersen | 26:857158816262 | 440 | m3pi.stop(); |
| janusboandersen | 26:857158816262 | 441 | } |
| janusboandersen | 26:857158816262 | 442 | } |
| janusboandersen | 26:857158816262 | 443 | |
| janusboandersen | 26:857158816262 | 444 | //TO DO: IMPLEMENT A WH CALCULATION |
| janusboandersen | 26:857158816262 | 445 | /* Begin the charging by calling when parked in pit |
| janusboandersen | 26:857158816262 | 446 | * Charges for a called number of seconds (int charge_time) |
| janusboandersen | 26:857158816262 | 447 | * Return the amount of Wh charged during each charging session |
| janusboandersen | 26:857158816262 | 448 | * Function keeps control as long as charging |
| janusboandersen | 26:857158816262 | 449 | */ |
| janusboandersen | 26:857158816262 | 450 | float charge(int charge_time) { |
| janusboandersen | 26:857158816262 | 451 | |
| janusboandersen | 26:857158816262 | 452 | float wh_this_session = 0; |
| janusboandersen | 26:857158816262 | 453 | float timestep = CHARGE_DELTA_T; //length of time between polls and integral |
| janusboandersen | 26:857158816262 | 454 | int timesteps = (int) (charge_time / timestep); |
| janusboandersen | 26:857158816262 | 455 | |
| janusboandersen | 26:857158816262 | 456 | for (int i = 0; i < timesteps; i++) { |
| janusboandersen | 26:857158816262 | 457 | |
| janusboandersen | 29:7475bcdce4e6 | 458 | show_voltage("Charging"); |
| janusboandersen | 26:857158816262 | 459 | |
| janusboandersen | 26:857158816262 | 460 | // PERFORM SOME WEIRD CALCULATION HERE |
| janusboandersen | 26:857158816262 | 461 | |
| janusboandersen | 26:857158816262 | 462 | //For now just put in a dummy number |
| janusboandersen | 26:857158816262 | 463 | wh_this_session += timestep; |
| janusboandersen | 26:857158816262 | 464 | |
| janusboandersen | 26:857158816262 | 465 | wait(timestep); //wait one second |
| janusboandersen | 26:857158816262 | 466 | |
| janusboandersen | 26:857158816262 | 467 | } //end for |
| janusboandersen | 26:857158816262 | 468 | |
| janusboandersen | 26:857158816262 | 469 | //return the amount of Wh charged |
| janusboandersen | 26:857158816262 | 470 | return wh_this_session; |
| janusboandersen | 26:857158816262 | 471 | } |
| janusboandersen | 26:857158816262 | 472 | |
| janusboandersen | 26:857158816262 | 473 | |
| janusboandersen | 26:857158816262 | 474 | //Show the battery voltage in the display |
| janusboandersen | 29:7475bcdce4e6 | 475 | void show_voltage(char* message) { |
| janusboandersen | 29:7475bcdce4e6 | 476 | m3pi.cls(); |
| janusboandersen | 29:7475bcdce4e6 | 477 | m3pi.locate(0,0); |
| janusboandersen | 29:7475bcdce4e6 | 478 | m3pi.printf("%s", message); |
| janusboandersen | 29:7475bcdce4e6 | 479 | m3pi.locate(0,1); |
| janusboandersen | 26:857158816262 | 480 | m3pi.printf("%07.2fV", m3pi.battery() ); |
| janusboandersen | 26:857158816262 | 481 | } |
| janusboandersen | 26:857158816262 | 482 | |
| janusboandersen | 26:857158816262 | 483 | |
| janusboandersen | 26:857158816262 | 484 | //display values from the performance data struct in the display |
| janusboandersen | 26:857158816262 | 485 | void show_car_data(performance_data data) { |
| janusboandersen | 26:857158816262 | 486 | |
| janusboandersen | 26:857158816262 | 487 | /* Format: |
| janusboandersen | 26:857158816262 | 488 | xxx.xxWh |
| janusboandersen | 26:857158816262 | 489 | 0000xxPs |
| janusboandersen | 26:857158816262 | 490 | |
| janusboandersen | 26:857158816262 | 491 | Formats are specified according to guesstimates with the following calcs |
| janusboandersen | 26:857158816262 | 492 | Wh = u * i * t = 6 V * 0.5 A * 1/3h * 20 times = 20 Wh |
| janusboandersen | 26:857158816262 | 493 | Bilforbrug: ca. 800 mA -> over 20 timer -> 16000 mAh -> 16 Ah |
| janusboandersen | 26:857158816262 | 494 | 16 Ah * 5 V = 80 Wh. |
| janusboandersen | 26:857158816262 | 495 | -> go for something between 20 - 80 Wh, with two decimals |
| janusboandersen | 26:857158816262 | 496 | */ |
| janusboandersen | 26:857158816262 | 497 | |
| janusboandersen | 26:857158816262 | 498 | |
| janusboandersen | 26:857158816262 | 499 | //clear screen |
| janusboandersen | 26:857158816262 | 500 | m3pi.cls(); |
| janusboandersen | 26:857158816262 | 501 | |
| janusboandersen | 26:857158816262 | 502 | //print Wh in first line |
| janusboandersen | 26:857158816262 | 503 | m3pi.locate(0,0); |
| janusboandersen | 26:857158816262 | 504 | m3pi.printf("%06.2fWh", data.wh); //6 wide in total, 2 after comma, pad with zero |
| janusboandersen | 26:857158816262 | 505 | |
| janusboandersen | 26:857158816262 | 506 | //print Pitstops in second line |
| janusboandersen | 26:857158816262 | 507 | m3pi.locate(0,1); |
| janusboandersen | 26:857158816262 | 508 | m3pi.printf("%06dPs", data.pitstops); //6 wide in total, padded with zeros |
| janusboandersen | 26:857158816262 | 509 | |
| janusboandersen | 26:857158816262 | 510 | } |
