Julesnaps / Mbed 2 deprecated Linefollowproject

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:   m3pi mbed

main.cpp@87:88aebb4c7226, 2022-10-25 (annotated)

Committer:
uld
Date:
Tue Oct 25 10:05:16 2022 +0000
Revision:
87:88aebb4c7226
Parent:
86:ca4ecb8828ed
Child:
88:55cd0d863d4f
Fixed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
magnusmland 0:f562e4f9c29f 1 #include "mbed.h"
magnusmland 0:f562e4f9c29f 2 #include "m3pi.h"
uld 71:4747e63eb48c 3 #include <cstdio>
magnusmland 0:f562e4f9c29f 4
magnusmland 0:f562e4f9c29f 5 m3pi m3pi;
uld 77:e1ece5800d7a 6 Timer timer;
uld 32:570b94fe2c19 7
uld 77:e1ece5800d7a 8 // DigitalOuts & Global Variabels
vehus 15:8b76add42254 9 DigitalOut led1(LED1);
vehus 15:8b76add42254 10 DigitalOut led2(LED2);
vehus 15:8b76add42254 11 DigitalOut led3(LED3);
vehus 15:8b76add42254 12 DigitalOut led4(LED4);
uld 77:e1ece5800d7a 13 int startTime;
magnusmland 0:f562e4f9c29f 14
magnusmland 0:f562e4f9c29f 15 // Minimum and maximum motor speeds
magnusmland 74:23064681b4f3 16 #define MAX 0.65
magnusmland 0:f562e4f9c29f 17 #define MIN 0
magnusmland 0:f562e4f9c29f 18
magnusmland 86:ca4ecb8828ed 19 // PID terms Test
magnusmland 0:f562e4f9c29f 20 #define P_TERM 1
magnusmland 0:f562e4f9c29f 21 #define I_TERM 0
magnusmland 0:f562e4f9c29f 22 #define D_TERM 20
magnusmland 0:f562e4f9c29f 23
uld 67:990fbcfee16f 24 // Ccount before test
uld 67:990fbcfee16f 25 #define CYCLEBEFORETEST 4500
uld 20:76f94dec91d1 26
uld 20:76f94dec91d1 27 // Textfile paths
uld 23:7e9505da7f48 28 #define PITLOGPATH "/local/pitlog.txt"
uld 23:7e9505da7f48 29 #define VOLTAGELOGPATH "/local/voltage.txt"
uld 20:76f94dec91d1 30
uld 7:ac88c8e35048 31 // Prototypes
uld 28:2c93dff934b1 32 void LED_Blink(int ledNumber); // Make ledNumber blinik
uld 75:b3a43f70e44c 33
uld 53:cef682f8684b 34 void LCD_CountDown(int num); //LCD Coundown function
uld 53:cef682f8684b 35 void LCD_InitialMessages(void); // Prints initial message to the LCD
uld 26:e6d82a8ba556 36
uld 53:cef682f8684b 37 int PS_BatteryTest(void); // Test if to robot needs to goto pit
uld 20:76f94dec91d1 38 void PS_PitStop(void); //
uld 20:76f94dec91d1 39 void PS_CreateLog(void); // create a log file or resets it (WIP
uld 72:fad84371f431 40 void PS_AddStopToLog(void); // Add one to the log
uld 81:a7a44a0944b5 41 int PS_GetNumberofPS(void); // Get number form file
uld 81:a7a44a0944b5 42 void PS_DisplayPS(void);
uld 20:76f94dec91d1 43
uld 20:76f94dec91d1 44 void TE_CreateVoltageLog(void); //
uld 21:c3e256b18b96 45 void TE_LogVoltage(int count); // test funktion that write the woltage each time the battry is checked
uld 20:76f94dec91d1 46
uld 7:ac88c8e35048 47
magnusmland 0:f562e4f9c29f 48 int main() {
uld 22:5d3332fc4c5c 49 LocalFileSystem local("local");
uld 77:e1ece5800d7a 50 timer.start();
uld 77:e1ece5800d7a 51 startTime = timer.read();
uld 77:e1ece5800d7a 52
uld 77:e1ece5800d7a 53
uld 6:6865930c1135 54 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 55 float right;
magnusmland 0:f562e4f9c29f 56 float left;
magnusmland 0:f562e4f9c29f 57 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 58 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 59 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 60 float power;
magnusmland 0:f562e4f9c29f 61 float speed = MAX;
uld 6:6865930c1135 62
uld 6:6865930c1135 63 /*Team 7 Variabels*/
uld 7:ac88c8e35048 64 int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false.
uld 67:990fbcfee16f 65 int ccount = 0; //used to count cycles
uld 6:6865930c1135 66
uld 23:7e9505da7f48 67 /*Printing secret cat mission*/
uld 81:a7a44a0944b5 68 PS_DisplayPS();
uld 53:cef682f8684b 69 LCD_InitialMessages();
uld 27:8561eeb0bd1d 70 m3pi.sensor_auto_calibrate();
uld 27:8561eeb0bd1d 71
uld 72:fad84371f431 72
uld 72:fad84371f431 73 /*Create logs used to log the number of pitstop */
uld 68:bb2ee5b4f9dd 74 PS_CreateLog(); //
uld 22:5d3332fc4c5c 75 TE_CreateVoltageLog();
uld 7:ac88c8e35048 76
magnusmland 0:f562e4f9c29f 77 while (1) {
uld 5:dbd32cb3650a 78
uld 78:0d905b656132 79 /* If cycle count divided by a constant does not have a rest. test if pit */
uld 67:990fbcfee16f 80 if (ccount % CYCLEBEFORETEST == 0 && gotoPit == 0)
vehus 15:8b76add42254 81 {
uld 67:990fbcfee16f 82 TE_LogVoltage(ccount);
uld 53:cef682f8684b 83 gotoPit = PS_BatteryTest();
vehus 15:8b76add42254 84 }
vehus 15:8b76add42254 85 if (gotoPit == 1)
vehus 15:8b76add42254 86 {
uld 20:76f94dec91d1 87 /*Add one to the nummber allready in the pitlog*/
uld 20:76f94dec91d1 88 PS_AddStopToLog();
uld 69:d7125d5b5cc8 89
uld 20:76f94dec91d1 90 /*Run the pitstop function*/
uld 28:2c93dff934b1 91 PS_PitStop();
vehus 15:8b76add42254 92 }
vehus 15:8b76add42254 93
magnusmland 0:f562e4f9c29f 94 // Get the position of the line.
magnusmland 0:f562e4f9c29f 95 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 96 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 97
magnusmland 0:f562e4f9c29f 98 // Compute the derivative
magnusmland 0:f562e4f9c29f 99 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 100
magnusmland 0:f562e4f9c29f 101 // Compute the integral
magnusmland 0:f562e4f9c29f 102 integral += proportional;
magnusmland 0:f562e4f9c29f 103
magnusmland 0:f562e4f9c29f 104 // Remember the last position.
magnusmland 0:f562e4f9c29f 105 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 106
magnusmland 0:f562e4f9c29f 107 // Compute the power
magnusmland 0:f562e4f9c29f 108 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 109
magnusmland 0:f562e4f9c29f 110 // Compute new speeds
magnusmland 0:f562e4f9c29f 111 right = speed+power;
magnusmland 0:f562e4f9c29f 112 left = speed-power;
magnusmland 0:f562e4f9c29f 113
magnusmland 0:f562e4f9c29f 114 // limit checks
magnusmland 0:f562e4f9c29f 115 if (right < MIN)
magnusmland 0:f562e4f9c29f 116 right = MIN;
magnusmland 0:f562e4f9c29f 117 else if (right > MAX)
magnusmland 0:f562e4f9c29f 118 right = MAX;
magnusmland 0:f562e4f9c29f 119
magnusmland 0:f562e4f9c29f 120 if (left < MIN)
magnusmland 0:f562e4f9c29f 121 left = MIN;
magnusmland 0:f562e4f9c29f 122 else if (left > MAX)
magnusmland 0:f562e4f9c29f 123 left = MAX;
magnusmland 0:f562e4f9c29f 124
magnusmland 0:f562e4f9c29f 125 // set speed
magnusmland 0:f562e4f9c29f 126 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 127 m3pi.right_motor(right);
uld 6:6865930c1135 128
uld 67:990fbcfee16f 129 ccount++;
uld 7:ac88c8e35048 130 }
vehus 15:8b76add42254 131
uld 7:ac88c8e35048 132 }
uld 7:ac88c8e35048 133
uld 72:fad84371f431 134 /**
uld 72:fad84371f431 135 * LCD_InitialMessages -Prints iniatial secret mission
uld 72:fad84371f431 136 */
uld 53:cef682f8684b 137 void LCD_InitialMessages(void){
uld 73:0646bad028c5 138
uld 9:7b9094864268 139 m3pi.cls();
uld 9:7b9094864268 140 m3pi.locate(0,0);
uld 69:d7125d5b5cc8 141 m3pi.printf("DESTROY");
uld 9:7b9094864268 142 m3pi.locate(0,1);
uld 69:d7125d5b5cc8 143 m3pi.printf("**CATS**");
uld 20:76f94dec91d1 144 wait(5.0);
uld 9:7b9094864268 145 m3pi.cls();
uld 9:7b9094864268 146 m3pi.locate(0,0);
uld 69:d7125d5b5cc8 147 m3pi.printf("%4.4f ",m3pi.battery());
uld 9:7b9094864268 148 m3pi.locate(0,1);
uld 69:d7125d5b5cc8 149 m3pi.printf("%4.4f ",m3pi.pot_voltage());
uld 20:76f94dec91d1 150 wait(10.0);
uld 28:2c93dff934b1 151 m3pi.cls();
uld 27:8561eeb0bd1d 152 m3pi.locate(0,0);
uld 70:e6cc44d1487d 153 m3pi.printf("ROBOT ON");
uld 27:8561eeb0bd1d 154 m3pi.locate(0,1);
uld 69:d7125d5b5cc8 155 m3pi.printf("TRACK!!");
uld 27:8561eeb0bd1d 156 wait(4.0);
uld 53:cef682f8684b 157 LCD_CountDown(3);
uld 24:6427b144b17c 158 m3pi.cls();
uld 26:e6d82a8ba556 159 m3pi.locate(0,0);
uld 69:d7125d5b5cc8 160 m3pi.printf("** GO **");
uld 69:d7125d5b5cc8 161
uld 72:fad84371f431 162
uld 53:cef682f8684b 163 wait (1.0);
uld 26:e6d82a8ba556 164 }
uld 72:fad84371f431 165
uld 72:fad84371f431 166 /**
uld 72:fad84371f431 167 * PS_DisplayPS - Display the number of pidstops from the log
uld 72:fad84371f431 168 */
uld 72:fad84371f431 169
uld 72:fad84371f431 170 void PS_DisplayPS(void){
uld 72:fad84371f431 171 m3pi.cls();
uld 72:fad84371f431 172 m3pi.locate(0,0);
uld 72:fad84371f431 173 m3pi.printf("PITSTOP:");
uld 72:fad84371f431 174 m3pi.locate(0,1);
uld 73:0646bad028c5 175 m3pi.printf("%d", PS_GetNumberofPS() );
uld 72:fad84371f431 176
uld 72:fad84371f431 177 }
uld 73:0646bad028c5 178
uld 72:fad84371f431 179 /**
uld 72:fad84371f431 180 * LCD_CountDown - display a countdown
uld 72:fad84371f431 181 * @num The number to count down from-
uld 72:fad84371f431 182 */
uld 53:cef682f8684b 183 void LCD_CountDown(int num){
uld 56:ea9a5cfbcc6b 184
uld 72:fad84371f431 185 for (int i=num; i>0; i--)
uld 26:e6d82a8ba556 186 {
uld 26:e6d82a8ba556 187 m3pi.cls();
uld 26:e6d82a8ba556 188 m3pi.locate(0,0);
uld 70:e6cc44d1487d 189 m3pi.printf("** %d **", i);
uld 26:e6d82a8ba556 190 wait(1.0);
uld 26:e6d82a8ba556 191 }
uld 82:f7443adac0c7 192 }
uld 73:0646bad028c5 193
uld 72:fad84371f431 194 /**
uld 72:fad84371f431 195 * PS_BatteryTest - Test the batteri voltage if the robot is not headed for pi
uld 72:fad84371f431 196 *
uld 72:fad84371f431 197 *return: 0 if the battery is above the threshold- Return 1 if the robot needs to goto pit
uld 72:fad84371f431 198 */
uld 53:cef682f8684b 199 int PS_BatteryTest(void){
uld 7:ac88c8e35048 200
uld 53:cef682f8684b 201 const float BATVOLTTRESHOLD = 0.5; // Treshold i volt
vehus 15:8b76add42254 202 int result = 0;
uld 77:e1ece5800d7a 203
mikkelbredholt 13:ddff4bb7c24f 204 /*Test if the voltage is below the threshold if so turn on go to pit mode*/
vehus 15:8b76add42254 205 if (m3pi.battery() <= BATVOLTTRESHOLD ){
uld 9:7b9094864268 206 result = 1; // Set goto pit condition
uld 87:88aebb4c7226 207 LED_Blink(3);
uld 79:9d46b0d532e1 208
uld 7:ac88c8e35048 209 m3pi.cls();
uld 7:ac88c8e35048 210 m3pi.locate(0,0);
uld 69:d7125d5b5cc8 211 m3pi.printf("Going to");
uld 7:ac88c8e35048 212 m3pi.locate(0,1);
uld 69:d7125d5b5cc8 213 m3pi.printf("**PIT**");
magnusmland 0:f562e4f9c29f 214 }
uld 8:5640c8c5088e 215 return result;
mikkelbredholt 13:ddff4bb7c24f 216 }
mikkelbredholt 13:ddff4bb7c24f 217
uld 72:fad84371f431 218 /**
uld 72:fad84371f431 219 * LED_Blink - Make a LED blink
uld 72:fad84371f431 220 *@ledNumber - The number of the targeted LED
uld 72:fad84371f431 221 */
uld 28:2c93dff934b1 222 void LED_Blink(int ledNumber)
vehus 15:8b76add42254 223 {
vehus 15:8b76add42254 224 int a = 2;
uld 87:88aebb4c7226 225
uld 87:88aebb4c7226 226 switch (ledNumber){
uld 87:88aebb4c7226 227 case(1):
uld 87:88aebb4c7226 228 led1 = 0;
uld 87:88aebb4c7226 229 wait(a);
uld 87:88aebb4c7226 230 led1 = 1;
uld 87:88aebb4c7226 231 wait(a);
uld 87:88aebb4c7226 232 led1 = 0;
uld 87:88aebb4c7226 233 wait(a);
uld 87:88aebb4c7226 234 break;
uld 87:88aebb4c7226 235
uld 87:88aebb4c7226 236 case(2):
uld 87:88aebb4c7226 237
uld 87:88aebb4c7226 238 led2 = 0;
uld 87:88aebb4c7226 239 wait(a);
uld 87:88aebb4c7226 240 led2 = 1;
uld 87:88aebb4c7226 241 wait(a);
uld 87:88aebb4c7226 242 led2 = 0;
uld 87:88aebb4c7226 243 wait(a);
uld 87:88aebb4c7226 244 break;
uld 87:88aebb4c7226 245
uld 87:88aebb4c7226 246 case(3):
uld 87:88aebb4c7226 247 led3 = 0;
uld 87:88aebb4c7226 248 wait(a);
uld 87:88aebb4c7226 249 led3 = 1;
uld 87:88aebb4c7226 250 wait(a);
uld 87:88aebb4c7226 251 led3 = 0;
uld 87:88aebb4c7226 252 wait(a);
uld 87:88aebb4c7226 253 break;
uld 87:88aebb4c7226 254
uld 87:88aebb4c7226 255 case(4):
uld 87:88aebb4c7226 256 led4 = 0;
uld 87:88aebb4c7226 257 wait(a);
uld 87:88aebb4c7226 258 led4 = 1;
uld 87:88aebb4c7226 259 wait(a);
uld 87:88aebb4c7226 260 led4 = 0;
uld 87:88aebb4c7226 261 wait(a);
uld 87:88aebb4c7226 262 break;
uld 87:88aebb4c7226 263
uld 87:88aebb4c7226 264
uld 87:88aebb4c7226 265 default:
uld 87:88aebb4c7226 266 break;
uld 87:88aebb4c7226 267 }
uld 87:88aebb4c7226 268
uld 87:88aebb4c7226 269
uld 87:88aebb4c7226 270
mikkelbredholt 13:ddff4bb7c24f 271 }
vehus 15:8b76add42254 272
uld 72:fad84371f431 273 /**
uld 72:fad84371f431 274 * PS_PitStop - Stops the robot and starts the signal
uld 72:fad84371f431 275 */
uld 20:76f94dec91d1 276 void PS_PitStop(void)
vehus 15:8b76add42254 277 {
vehus 15:8b76add42254 278 m3pi.stop(); // stop all engine
vehus 15:8b76add42254 279 // increase counter with one
vehus 15:8b76add42254 280 while (1)
vehus 15:8b76add42254 281 {
uld 28:2c93dff934b1 282 LED_Blink (1); // signal in pit
mikkelbredholt 13:ddff4bb7c24f 283 }
uld 20:76f94dec91d1 284 }
uld 20:76f94dec91d1 285
uld 72:fad84371f431 286 /**
uld 72:fad84371f431 287 * PS_CreateLog - Create a pitstop log i none excist
uld 72:fad84371f431 288 *
uld 72:fad84371f431 289 * Return: The number of ppitstops as noted in the log
uld 72:fad84371f431 290 */
uld 20:76f94dec91d1 291 void PS_CreateLog(void){
uld 20:76f94dec91d1 292 FILE *fptr;
uld 68:bb2ee5b4f9dd 293
uld 68:bb2ee5b4f9dd 294 if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
uld 68:bb2ee5b4f9dd 295 fptr = fopen(PITLOGPATH,"w");
uld 68:bb2ee5b4f9dd 296 fprintf(fptr,"%d", 0);
uld 71:4747e63eb48c 297 fclose(fptr);
uld 68:bb2ee5b4f9dd 298 }
uld 68:bb2ee5b4f9dd 299
uld 20:76f94dec91d1 300 }
uld 20:76f94dec91d1 301
uld 75:b3a43f70e44c 302 /**
uld 75:b3a43f70e44c 303 * PS_AddStopToLog - Add one to the number in the pitstop log
uld 75:b3a43f70e44c 304 */
uld 20:76f94dec91d1 305 void PS_AddStopToLog(void){
uld 75:b3a43f70e44c 306
uld 20:76f94dec91d1 307 FILE *fptr;
uld 20:76f94dec91d1 308 int x, y;
uld 20:76f94dec91d1 309 if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
uld 20:76f94dec91d1 310 printf("Error! opening file");
uld 20:76f94dec91d1 311 // Program exits if the file pointer returns NULL.
uld 20:76f94dec91d1 312 exit(1);
uld 20:76f94dec91d1 313 }
uld 20:76f94dec91d1 314
uld 20:76f94dec91d1 315 fscanf(fptr,"%d", &x);
uld 20:76f94dec91d1 316 fclose(fptr);
uld 20:76f94dec91d1 317
uld 20:76f94dec91d1 318 y = x+1;
uld 20:76f94dec91d1 319 fptr = fopen(PITLOGPATH,"w");
uld 20:76f94dec91d1 320
uld 20:76f94dec91d1 321 if(fptr == NULL)
uld 20:76f94dec91d1 322 {
uld 20:76f94dec91d1 323 printf("Error creating log file ");
uld 20:76f94dec91d1 324 exit(1);
uld 20:76f94dec91d1 325 }
uld 20:76f94dec91d1 326 fprintf(fptr,"%d", y);
uld 20:76f94dec91d1 327 fclose(fptr);
uld 20:76f94dec91d1 328 }
uld 20:76f94dec91d1 329
uld 75:b3a43f70e44c 330 /** PS_GetNumberofPS - Return the number i the pitstop recorded in the logfile
uld 75:b3a43f70e44c 331 *
uld 75:b3a43f70e44c 332 *return: An interger from the the pitlog
uld 75:b3a43f70e44c 333 */
uld 72:fad84371f431 334 int PS_GetNumberofPS(void){
uld 75:b3a43f70e44c 335 //
uld 20:76f94dec91d1 336 FILE *fptr;
uld 20:76f94dec91d1 337 int x;
uld 20:76f94dec91d1 338 if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
uld 20:76f94dec91d1 339 printf("Error! opening file");
uld 20:76f94dec91d1 340 // Program exits if the file pointer returns NULL.
uld 20:76f94dec91d1 341 exit(1);
uld 20:76f94dec91d1 342 }
uld 20:76f94dec91d1 343 fscanf(fptr,"%d", &x);
uld 72:fad84371f431 344 //printf("Final number of pits stops %d", x);
uld 20:76f94dec91d1 345 fclose(fptr);
uld 72:fad84371f431 346 return x;
uld 20:76f94dec91d1 347 }
uld 72:fad84371f431 348
uld 75:b3a43f70e44c 349 /** TE_CreateVoltageLog - create a voltagelog
uld 75:b3a43f70e44c 350 */
uld 20:76f94dec91d1 351 void TE_CreateVoltageLog(void){
uld 20:76f94dec91d1 352 /* Create a voltagelog file and test if it can open*/
uld 20:76f94dec91d1 353 FILE *fptr;
uld 20:76f94dec91d1 354 fptr = fopen(VOLTAGELOGPATH,"w");
uld 20:76f94dec91d1 355
uld 20:76f94dec91d1 356 if(fptr == NULL)
uld 20:76f94dec91d1 357 {
uld 20:76f94dec91d1 358 printf("Error creating log file ");
uld 20:76f94dec91d1 359 exit(1);
uld 20:76f94dec91d1 360 }
uld 20:76f94dec91d1 361
uld 20:76f94dec91d1 362 fclose(fptr);
uld 20:76f94dec91d1 363 }
uld 20:76f94dec91d1 364
uld 75:b3a43f70e44c 365 /** TE_LogVoltage - Add an entry to the voltagelog
uld 75:b3a43f70e44c 366 *@count: The number the counter has reached in the main loop
uld 75:b3a43f70e44c 367 */
uld 21:c3e256b18b96 368 void TE_LogVoltage(int count){
uld 75:b3a43f70e44c 369
uld 20:76f94dec91d1 370 FILE *fptr; /* voltagelog adres */
uld 20:76f94dec91d1 371 fptr = fopen(VOLTAGELOGPATH,"a");
uld 20:76f94dec91d1 372
uld 78:0d905b656132 373 fprintf(fptr,"%8d,%8d,%4.4f,%4.4f\n" , timer.read()-startTime, count, m3pi.battery(),m3pi.pot_voltage() );
uld 20:76f94dec91d1 374 fclose(fptr);
uld 58:852b91920a44 375
uld 79:9d46b0d532e1 376 }