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@18:8b3efa4d4a36, 2018-12-03 (annotated)

Committer:
janusboandersen
Date:
Mon Dec 03 15:51:51 2018 +0000
Revision:
18:8b3efa4d4a36
Parent:
17:c2e21d347ca5
Child:
19:c0d59019de53
write and read from file system

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
janusboandersen 18:8b3efa4d4a36 6 //File name
janusboandersen 18:8b3efa4d4a36 7 #define FILESYS "robot"
janusboandersen 18:8b3efa4d4a36 8 #define FILENAME "/robot/data.txt"
janusboandersen 18:8b3efa4d4a36 9
chris 0:78f9794620a3 10 // Minimum and maximum motor speeds
Blunsdon 13:6a8689f601cd 11 #define MAX 0.3
chris 0:78f9794620a3 12 #define MIN 0
chris 0:78f9794620a3 13
chris 0:78f9794620a3 14 // PID terms
chris 0:78f9794620a3 15 #define P_TERM 1
chris 0:78f9794620a3 16 #define I_TERM 0
Blunsdon 1:47ea51d9e25d 17 #define D_TERM 8
Blunsdon 1:47ea51d9e25d 18
janusboandersen 18:8b3efa4d4a36 19
janusboandersen 18:8b3efa4d4a36 20 //typedef -> definer som type i C++
janusboandersen 18:8b3efa4d4a36 21 //uden typedef - det er ikke en type, så du skal skrive struct performance_data
janusboandersen 18:8b3efa4d4a36 22 typedef struct performance_data { //<- navn på structen
janusboandersen 18:8b3efa4d4a36 23 float wh;
janusboandersen 18:8b3efa4d4a36 24 int pitstops;
janusboandersen 18:8b3efa4d4a36 25 } performance_data; //<- navn på typen
janusboandersen 18:8b3efa4d4a36 26
janusboandersen 18:8b3efa4d4a36 27
janusboandersen 18:8b3efa4d4a36 28 void write_to_file(performance_data data) {
janusboandersen 18:8b3efa4d4a36 29
janusboandersen 18:8b3efa4d4a36 30 //Define file system
janusboandersen 18:8b3efa4d4a36 31 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 32
janusboandersen 18:8b3efa4d4a36 33 //Make a pointer to the file
janusboandersen 18:8b3efa4d4a36 34 FILE *fp = fopen(FILENAME, "w"); //"w" "r" "a"
janusboandersen 18:8b3efa4d4a36 35
janusboandersen 18:8b3efa4d4a36 36 fprintf(fp, "%0.2f %d", data.wh, data.pitstops); //write to the file
janusboandersen 18:8b3efa4d4a36 37
janusboandersen 18:8b3efa4d4a36 38 fclose(fp); //close when done.
janusboandersen 18:8b3efa4d4a36 39
janusboandersen 18:8b3efa4d4a36 40 }
janusboandersen 18:8b3efa4d4a36 41
janusboandersen 18:8b3efa4d4a36 42 performance_data read_from_file() {
janusboandersen 18:8b3efa4d4a36 43
janusboandersen 18:8b3efa4d4a36 44 performance_data data;
janusboandersen 18:8b3efa4d4a36 45
janusboandersen 18:8b3efa4d4a36 46
janusboandersen 18:8b3efa4d4a36 47 //Define file system
janusboandersen 18:8b3efa4d4a36 48 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 49
janusboandersen 18:8b3efa4d4a36 50 //Make a pointer to the file
janusboandersen 18:8b3efa4d4a36 51 FILE *fp = fopen(FILENAME, "r"); //"w" "r" "a"
janusboandersen 18:8b3efa4d4a36 52
janusboandersen 18:8b3efa4d4a36 53 //Make some error prevention later
janusboandersen 18:8b3efa4d4a36 54 fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) ); //write to the file
janusboandersen 18:8b3efa4d4a36 55
janusboandersen 18:8b3efa4d4a36 56 fclose(fp); //close when done.
janusboandersen 18:8b3efa4d4a36 57
janusboandersen 18:8b3efa4d4a36 58 return data;
janusboandersen 18:8b3efa4d4a36 59
janusboandersen 18:8b3efa4d4a36 60 }
janusboandersen 18:8b3efa4d4a36 61
janusboandersen 18:8b3efa4d4a36 62
Blunsdon 12:dfc717775297 63 void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
Blunsdon 1:47ea51d9e25d 64 {
Blunsdon 12:dfc717775297 65 m3pi.putc(0x87); // send noget sensor data
Blunsdon 1:47ea51d9e25d 66
janusboandersen 4:5939f9ad88ca 67 for(int n=0; n < 5; n++) // forloop så sensor for data
Blunsdon 1:47ea51d9e25d 68 {
Blunsdon 1:47ea51d9e25d 69 char lowbyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 70 char hibyte = m3pi.getc();
Blunsdon 12:dfc717775297 71 arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
Blunsdon 1:47ea51d9e25d 72 }
Blunsdon 9:fa745d3afcac 73 m3pi.cls();
Blunsdon 9:fa745d3afcac 74 m3pi.locate(0,1);
Blunsdon 12:dfc717775297 75 m3pi.printf("%d", arr[0]);
Blunsdon 12:dfc717775297 76 m3pi.locate(0,0);
Blunsdon 12:dfc717775297 77 m3pi.printf("%d", arr[4]);
Blunsdon 11:394ab193971f 78 }
Blunsdon 13:6a8689f601cd 79
Blunsdon 13:6a8689f601cd 80 int maze (void)
Blunsdon 13:6a8689f601cd 81 {
Blunsdon 13:6a8689f601cd 82
Blunsdon 13:6a8689f601cd 83 return 1;
Blunsdon 14:408d9d087d0f 84 }
Blunsdon 14:408d9d087d0f 85
Blunsdon 14:408d9d087d0f 86 void turn_left (void)
Blunsdon 14:408d9d087d0f 87 {
Blunsdon 14:408d9d087d0f 88 m3pi.left(0.3);
Blunsdon 14:408d9d087d0f 89 wait (0.22);
Blunsdon 14:408d9d087d0f 90 }
Blunsdon 14:408d9d087d0f 91
Blunsdon 13:6a8689f601cd 92 void turn_right (void)
Blunsdon 13:6a8689f601cd 93 {
Blunsdon 13:6a8689f601cd 94 m3pi.right (0.3);
Blunsdon 14:408d9d087d0f 95 wait (0.22);
Blunsdon 13:6a8689f601cd 96 }
chris 0:78f9794620a3 97
chris 0:78f9794620a3 98 int main() {
janusboandersen 18:8b3efa4d4a36 99
janusboandersen 18:8b3efa4d4a36 100 //test the function to write to a local file
janusboandersen 18:8b3efa4d4a36 101 performance_data robot_stats;
janusboandersen 18:8b3efa4d4a36 102 robot_stats.wh = 120.5;
janusboandersen 18:8b3efa4d4a36 103 robot_stats.pitstops = 200;
janusboandersen 18:8b3efa4d4a36 104
janusboandersen 18:8b3efa4d4a36 105 write_to_file(robot_stats);
janusboandersen 18:8b3efa4d4a36 106
janusboandersen 18:8b3efa4d4a36 107 performance_data new_data;
janusboandersen 18:8b3efa4d4a36 108 new_data = read_from_file();
janusboandersen 18:8b3efa4d4a36 109 //might get an error here due to timing
janusboandersen 18:8b3efa4d4a36 110
janusboandersen 18:8b3efa4d4a36 111
janusboandersen 18:8b3efa4d4a36 112 new_data.wh = new_data.wh * 2;
janusboandersen 18:8b3efa4d4a36 113 new_data.pitstops = new_data.pitstops * 2;
janusboandersen 18:8b3efa4d4a36 114
janusboandersen 18:8b3efa4d4a36 115 write_to_file(new_data);
janusboandersen 18:8b3efa4d4a36 116 //should get 247... and 246
janusboandersen 2:d22dcc2bfcc1 117
chris 0:78f9794620a3 118 m3pi.locate(0,1);
chris 0:78f9794620a3 119 m3pi.printf("Line PID");
chris 0:78f9794620a3 120
Blunsdon 1:47ea51d9e25d 121 wait(1.0);
chris 0:78f9794620a3 122
chris 0:78f9794620a3 123 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 124
chris 0:78f9794620a3 125 float right;
chris 0:78f9794620a3 126 float left;
chris 0:78f9794620a3 127 float current_pos_of_line = 0.0;
chris 0:78f9794620a3 128 float previous_pos_of_line = 0.0;
chris 0:78f9794620a3 129 float derivative,proportional,integral = 0;
chris 0:78f9794620a3 130 float power;
chris 0:78f9794620a3 131 float speed = MAX;
Blunsdon 12:dfc717775297 132 int sensor_val[5];
chris 0:78f9794620a3 133
janusboandersen 17:c2e21d347ca5 134
janusboandersen 17:c2e21d347ca5 135 /* if restartedFromOff: Load values from file, recalibrate and start.
janusboandersen 17:c2e21d347ca5 136 */
janusboandersen 17:c2e21d347ca5 137
chris 0:78f9794620a3 138 while (1) {
janusboandersen 17:c2e21d347ca5 139
janusboandersen 17:c2e21d347ca5 140 /*
janusboandersen 17:c2e21d347ca5 141 switch
janusboandersen 17:c2e21d347ca5 142 if nothing else: line following
janusboandersen 17:c2e21d347ca5 143 if low_batt: slow down and continue
janusboandersen 17:c2e21d347ca5 144 if atPitIntersection: increment internal lap counter
janusboandersen 17:c2e21d347ca5 145 if low_batt && atPitIntersection: Stop && execute pit navigation program
janusboandersen 17:c2e21d347ca5 146 if all_sensors == 1000 (lifted from track):
janusboandersen 17:c2e21d347ca5 147 show Wh and pitstops on display
janusboandersen 17:c2e21d347ca5 148 stop motors, reset PID errors, wait 20 secs, recalibrate sensors
janusboandersen 17:c2e21d347ca5 149
janusboandersen 17:c2e21d347ca5 150 if all_sensors < 90 (driven off track):
janusboandersen 17:c2e21d347ca5 151 stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
janusboandersen 17:c2e21d347ca5 152
janusboandersen 17:c2e21d347ca5 153 Periodically (not necessarily every lap - maybe every 2-5th lap):
janusboandersen 17:c2e21d347ca5 154 Save to file -> possibly only after pit
janusboandersen 17:c2e21d347ca5 155 Check battery voltage -> every couple of minutes
janusboandersen 17:c2e21d347ca5 156 Check light sensors for drift -> recalibrate (optional)
janusboandersen 17:c2e21d347ca5 157
janusboandersen 17:c2e21d347ca5 158 */
janusboandersen 17:c2e21d347ca5 159
janusboandersen 17:c2e21d347ca5 160
janusboandersen 17:c2e21d347ca5 161
janusboandersen 17:c2e21d347ca5 162 /* Separate functions
janusboandersen 17:c2e21d347ca5 163 pit_navigation (entering pit from T-intersection all the way to the charger)
janusboandersen 17:c2e21d347ca5 164 pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
janusboandersen 17:c2e21d347ca5 165 add 1 to number of pitstops
janusboandersen 17:c2e21d347ca5 166 charge and wait for signal from the external charging circuit
janusboandersen 17:c2e21d347ca5 167 during charging: integrate v(t) dt, add to cumulative sum
janusboandersen 17:c2e21d347ca5 168 when fully charged (or signal goes high):
janusboandersen 17:c2e21d347ca5 169 Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 )
janusboandersen 17:c2e21d347ca5 170 Calculate total_Wh
janusboandersen 17:c2e21d347ca5 171 save progress data to file
janusboandersen 17:c2e21d347ca5 172 leave the charger -> pass control to pit_exit
janusboandersen 17:c2e21d347ca5 173
janusboandersen 17:c2e21d347ca5 174 pit_exit
janusboandersen 17:c2e21d347ca5 175 reverse out of the pit back to T-intersection
janusboandersen 17:c2e21d347ca5 176 continue line following away from T-intersection, recal sensors after a few seconds
janusboandersen 17:c2e21d347ca5 177
janusboandersen 17:c2e21d347ca5 178
janusboandersen 17:c2e21d347ca5 179 AfterPitAdmin: Calibrate sensors, save state to file,
janusboandersen 17:c2e21d347ca5 180
janusboandersen 17:c2e21d347ca5 181 show_results_on_display:
janusboandersen 17:c2e21d347ca5 182
janusboandersen 17:c2e21d347ca5 183 check_sensors_for_errors:
janusboandersen 17:c2e21d347ca5 184
janusboandersen 17:c2e21d347ca5 185 check_battery_voltage -> return true false
janusboandersen 17:c2e21d347ca5 186 compare m3pi.battery() < V_CRITICAL
janusboandersen 17:c2e21d347ca5 187
janusboandersen 17:c2e21d347ca5 188
janusboandersen 17:c2e21d347ca5 189
janusboandersen 17:c2e21d347ca5 190 HUSK FOR POKKER KOLLES KODESTANDARD!
janusboandersen 17:c2e21d347ca5 191
janusboandersen 17:c2e21d347ca5 192 */
janusboandersen 17:c2e21d347ca5 193
janusboandersen 17:c2e21d347ca5 194
Blunsdon 11:394ab193971f 195 // Get the position of the line.
chris 0:78f9794620a3 196 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 197 proportional = current_pos_of_line;
chris 0:78f9794620a3 198
chris 0:78f9794620a3 199 // Compute the derivative
chris 0:78f9794620a3 200 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 201
chris 0:78f9794620a3 202 // Compute the integral
chris 0:78f9794620a3 203 integral += proportional;
chris 0:78f9794620a3 204
chris 0:78f9794620a3 205 // Remember the last position.
chris 0:78f9794620a3 206 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 207
chris 0:78f9794620a3 208 // Compute the power
chris 0:78f9794620a3 209 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 210
chris 0:78f9794620a3 211 // Compute new speeds
chris 0:78f9794620a3 212 right = speed+power;
chris 0:78f9794620a3 213 left = speed-power;
chris 0:78f9794620a3 214
chris 0:78f9794620a3 215 // limit checks
chris 0:78f9794620a3 216 if (right < MIN)
chris 0:78f9794620a3 217 right = MIN;
chris 0:78f9794620a3 218 else if (right > MAX)
chris 0:78f9794620a3 219 right = MAX;
chris 0:78f9794620a3 220
chris 0:78f9794620a3 221 if (left < MIN)
chris 0:78f9794620a3 222 left = MIN;
chris 0:78f9794620a3 223 else if (left > MAX)
chris 0:78f9794620a3 224 left = MAX;
chris 0:78f9794620a3 225
chris 0:78f9794620a3 226 // set speed
chris 0:78f9794620a3 227 m3pi.left_motor(left);
chris 0:78f9794620a3 228 m3pi.right_motor(right);
Blunsdon 11:394ab193971f 229
janusboandersen 2:d22dcc2bfcc1 230 //Marc's sensor test
Blunsdon 12:dfc717775297 231 sensor_all(sensor_val); // sensor_value gets fkt value
Blunsdon 13:6a8689f601cd 232 if (sensor_val[0] > 350 && sensor_val[4] > 350)
Blunsdon 12:dfc717775297 233 {
Blunsdon 13:6a8689f601cd 234
Blunsdon 13:6a8689f601cd 235 m3pi.stop();
Blunsdon 13:6a8689f601cd 236 turn_right();
Blunsdon 13:6a8689f601cd 237 m3pi.stop();
Blunsdon 13:6a8689f601cd 238 m3pi.forward(0.2);
Blunsdon 16:48b81a9246e6 239 wait (1.1);
Blunsdon 14:408d9d087d0f 240 m3pi.stop();
Blunsdon 16:48b81a9246e6 241 wait (5);
Blunsdon 14:408d9d087d0f 242 m3pi.backward(0.2);
Blunsdon 15:fd6886ab896a 243 wait (1);
Blunsdon 14:408d9d087d0f 244 m3pi.stop();
Blunsdon 14:408d9d087d0f 245 turn_left();
Blunsdon 13:6a8689f601cd 246 m3pi.stop();
Blunsdon 12:dfc717775297 247 }
janusboandersen 5:527c046c7b8a 248 /*
janusboandersen 2:d22dcc2bfcc1 249 sensor_value4 = sensor_all(4); // sensor_value gets fkt value
janusboandersen 6:3be33adda32d 250 */
janusboandersen 7:30c63d6460f6 251
janusboandersen 6:3be33adda32d 252 /*
janusboandersen 2:d22dcc2bfcc1 253 m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
janusboandersen 2:d22dcc2bfcc1 254 m3pi.locate(0,1);
janusboandersen 2:d22dcc2bfcc1 255 m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
janusboandersen 3:47968be0df37 256 */
chris 0:78f9794620a3 257
chris 0:78f9794620a3 258 }
chris 0:78f9794620a3 259 }