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@40:b7d615920395, 2022-10-12 (annotated)

Committer:
mikkelbredholt
Date:
Wed Oct 12 07:25:16 2022 +0000
Revision:
40:b7d615920395
Parent:
39:e3ec05251d74
Child:
42:4f200484f1f9
Alle er med

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"
magnusmland 0:f562e4f9c29f 3
magnusmland 0:f562e4f9c29f 4 m3pi m3pi;
uld 32:570b94fe2c19 5
mikkelbredholt 33:f035445a4604 6 //test11
mikkelbredholt 35:be40a5d57023 7 //test12
mikkelbredholt 36:17854202990b 8 //test13
mikkelbredholt 33:f035445a4604 9
uld 32:570b94fe2c19 10 // DigitalOuts
vehus 15:8b76add42254 11 DigitalOut led1(LED1);
vehus 15:8b76add42254 12 DigitalOut led2(LED2);
vehus 15:8b76add42254 13 DigitalOut led3(LED3);
vehus 15:8b76add42254 14 DigitalOut led4(LED4);
magnusmland 0:f562e4f9c29f 15
magnusmland 0:f562e4f9c29f 16 // Minimum and maximum motor speeds
uld 30:ccf5fa970bd2 17 #define MAX 0.80
magnusmland 0:f562e4f9c29f 18 #define MIN 0
magnusmland 0:f562e4f9c29f 19
magnusmland 0:f562e4f9c29f 20 // PID terms
magnusmland 0:f562e4f9c29f 21 #define P_TERM 1
magnusmland 0:f562e4f9c29f 22 #define I_TERM 0
magnusmland 0:f562e4f9c29f 23 #define D_TERM 20
magnusmland 0:f562e4f9c29f 24
uld 31:434a78739a59 25 // Ccount before test
uld 31:434a78739a59 26 #define CYCLEBEFORETEST 1500
uld 20:76f94dec91d1 27
uld 20:76f94dec91d1 28 // Textfile paths
uld 23:7e9505da7f48 29 #define PITLOGPATH "/local/pitlog.txt"
uld 23:7e9505da7f48 30 #define VOLTAGELOGPATH "/local/voltage.txt"
uld 20:76f94dec91d1 31
uld 20:76f94dec91d1 32
uld 7:ac88c8e35048 33 // Prototypes
uld 8:5640c8c5088e 34 int PitTest(void); // Test if to robot needs to goto pit
vehus 15:8b76add42254 35 void InitialMessages(void); // Prints initial message to the LCD
uld 28:2c93dff934b1 36 void LED_Control(int ledNumber, int state); //Turn ledNumber to 1=on, 0 = off
uld 28:2c93dff934b1 37 void LED_Blink(int ledNumber); // Make ledNumber blinik
uld 28:2c93dff934b1 38 void LCDCountDown(int num); //LCD Coundown function
uld 26:e6d82a8ba556 39
mikkelbredholt 13:ddff4bb7c24f 40
uld 20:76f94dec91d1 41 void PS_PitStop(void); //
uld 20:76f94dec91d1 42 void PS_CreateLog(void); // create a log file or resets it (WIP
uld 20:76f94dec91d1 43 void PS_AddStopToLog(void); // Add one to the log
uld 20:76f94dec91d1 44 // void PS_DisplayNumberofPS(void); // Display the final number on screen WIP
uld 20:76f94dec91d1 45
uld 20:76f94dec91d1 46 void TE_CreateVoltageLog(void); //
uld 21:c3e256b18b96 47 void TE_LogVoltage(int count); // test funktion that write the woltage each time the battry is checked
uld 20:76f94dec91d1 48
magnusmland 0:f562e4f9c29f 49 int main() {
uld 22:5d3332fc4c5c 50 LocalFileSystem local("local");
uld 27:8561eeb0bd1d 51
uld 5:dbd32cb3650a 52
uld 6:6865930c1135 53 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 54 float right;
magnusmland 0:f562e4f9c29f 55 float left;
magnusmland 0:f562e4f9c29f 56 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 57 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 58 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 59 float power;
magnusmland 0:f562e4f9c29f 60 float speed = MAX;
uld 6:6865930c1135 61
uld 6:6865930c1135 62 /*Team 7 Variabels*/
uld 7:ac88c8e35048 63 int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false.
vehus 15:8b76add42254 64 int ccount = 0; //used to count cycles
uld 6:6865930c1135 65
uld 23:7e9505da7f48 66 /*Printing secret cat mission*/
uld 23:7e9505da7f48 67 InitialMessages();
uld 27:8561eeb0bd1d 68 m3pi.sensor_auto_calibrate();
uld 27:8561eeb0bd1d 69
uld 23:7e9505da7f48 70
uld 20:76f94dec91d1 71 /*Create pitlog used to log the number of pitstop */
uld 20:76f94dec91d1 72 PS_CreateLog();
uld 22:5d3332fc4c5c 73 TE_CreateVoltageLog();
uld 7:ac88c8e35048 74
magnusmland 0:f562e4f9c29f 75 while (1) {
uld 8:5640c8c5088e 76 /* If cycle count divided by 100 does not have a rest. test if pit */
uld 31:434a78739a59 77 if (ccount % CYCLEBEFORETEST == 0 && gotoPit == 0)
vehus 15:8b76add42254 78 {
uld 21:c3e256b18b96 79 TE_LogVoltage(ccount);
vehus 15:8b76add42254 80 gotoPit = PitTest();
vehus 15:8b76add42254 81 }
vehus 15:8b76add42254 82 if (gotoPit == 1)
vehus 15:8b76add42254 83 {
uld 20:76f94dec91d1 84 /*Add one to the nummber allready in the pitlog*/
uld 20:76f94dec91d1 85 PS_AddStopToLog();
uld 20:76f94dec91d1 86 /*Run the pitstop function*/
uld 28:2c93dff934b1 87 PS_PitStop();
vehus 15:8b76add42254 88 }
vehus 15:8b76add42254 89
magnusmland 0:f562e4f9c29f 90 // Get the position of the line.
magnusmland 0:f562e4f9c29f 91 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 92 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 93
magnusmland 0:f562e4f9c29f 94 // Compute the derivative
magnusmland 0:f562e4f9c29f 95 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 96
magnusmland 0:f562e4f9c29f 97 // Compute the integral
magnusmland 0:f562e4f9c29f 98 integral += proportional;
magnusmland 0:f562e4f9c29f 99
magnusmland 0:f562e4f9c29f 100 // Remember the last position.
magnusmland 0:f562e4f9c29f 101 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 102
magnusmland 0:f562e4f9c29f 103 // Compute the power
magnusmland 0:f562e4f9c29f 104 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 105
magnusmland 0:f562e4f9c29f 106 // Compute new speeds
magnusmland 0:f562e4f9c29f 107 right = speed+power;
magnusmland 0:f562e4f9c29f 108 left = speed-power;
magnusmland 0:f562e4f9c29f 109
magnusmland 0:f562e4f9c29f 110 // limit checks
magnusmland 0:f562e4f9c29f 111 if (right < MIN)
magnusmland 0:f562e4f9c29f 112 right = MIN;
magnusmland 0:f562e4f9c29f 113 else if (right > MAX)
magnusmland 0:f562e4f9c29f 114 right = MAX;
magnusmland 0:f562e4f9c29f 115
magnusmland 0:f562e4f9c29f 116 if (left < MIN)
magnusmland 0:f562e4f9c29f 117 left = MIN;
magnusmland 0:f562e4f9c29f 118 else if (left > MAX)
magnusmland 0:f562e4f9c29f 119 left = MAX;
magnusmland 0:f562e4f9c29f 120
magnusmland 0:f562e4f9c29f 121 // set speed
magnusmland 0:f562e4f9c29f 122 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 123 m3pi.right_motor(right);
uld 6:6865930c1135 124
uld 8:5640c8c5088e 125 ccount++;
uld 7:ac88c8e35048 126 }
uld 20:76f94dec91d1 127 // PS_DisplayNumberofPS();
vehus 15:8b76add42254 128
uld 7:ac88c8e35048 129 }
uld 7:ac88c8e35048 130
vehus 15:8b76add42254 131 void InitialMessages(void){
uld 9:7b9094864268 132 /*Prints iniatl secret mission*/
uld 9:7b9094864268 133
uld 23:7e9505da7f48 134
uld 23:7e9505da7f48 135
uld 9:7b9094864268 136 m3pi.cls();
uld 9:7b9094864268 137 m3pi.locate(0,0);
uld 27:8561eeb0bd1d 138 m3pi.printf("DESTROY");
uld 9:7b9094864268 139 m3pi.locate(0,1);
uld 27:8561eeb0bd1d 140 m3pi.printf("**CATS**");
uld 20:76f94dec91d1 141 wait(5.0);
uld 9:7b9094864268 142
uld 9:7b9094864268 143 m3pi.cls();
uld 9:7b9094864268 144 m3pi.locate(0,0);
uld 20:76f94dec91d1 145 m3pi.printf("%4.4f ",m3pi.battery());
uld 9:7b9094864268 146 m3pi.locate(0,1);
uld 20:76f94dec91d1 147 m3pi.printf("%4.4f ",m3pi.pot_voltage());
uld 20:76f94dec91d1 148 wait(10.0);
uld 28:2c93dff934b1 149 m3pi.cls();
uld 27:8561eeb0bd1d 150 m3pi.locate(0,0);
uld 28:2c93dff934b1 151 m3pi.printf("ROBOT ON");
uld 27:8561eeb0bd1d 152 m3pi.locate(0,1);
uld 28:2c93dff934b1 153 m3pi.printf("TRACK!!");
uld 27:8561eeb0bd1d 154 wait(4.0);
uld 26:e6d82a8ba556 155 LCDCountDown(3);
uld 24:6427b144b17c 156 m3pi.cls();
uld 26:e6d82a8ba556 157 m3pi.locate(0,0);
uld 26:e6d82a8ba556 158 m3pi.printf("** GO **");
uld 26:e6d82a8ba556 159 }
uld 23:7e9505da7f48 160
uld 26:e6d82a8ba556 161 void LCDCountDown(int num){
uld 26:e6d82a8ba556 162 for (int i=0; i<num; i++)
uld 26:e6d82a8ba556 163 {
uld 26:e6d82a8ba556 164 m3pi.cls();
uld 26:e6d82a8ba556 165 m3pi.locate(0,0);
uld 26:e6d82a8ba556 166 m3pi.printf("** %d **", i );
uld 26:e6d82a8ba556 167 wait(1.0);
uld 26:e6d82a8ba556 168 }
uld 23:7e9505da7f48 169
uld 26:e6d82a8ba556 170 }
uld 26:e6d82a8ba556 171
uld 8:5640c8c5088e 172 int PitTest(void){
uld 7:ac88c8e35048 173 /* Test the batteri voltage if the robot is not headed for pit */
uld 7:ac88c8e35048 174
uld 30:ccf5fa970bd2 175 const float BATVOLTTRESHOLD = 0.05; // Treshold i volt
vehus 15:8b76add42254 176 int result = 0;
uld 7:ac88c8e35048 177
mikkelbredholt 13:ddff4bb7c24f 178 /*Test if the voltage is below the threshold if so turn on go to pit mode*/
vehus 15:8b76add42254 179 if (m3pi.battery() <= BATVOLTTRESHOLD ){
uld 9:7b9094864268 180 result = 1; // Set goto pit condition
vehus 15:8b76add42254 181 LED_Control(1, 1);
uld 7:ac88c8e35048 182 m3pi.cls();
uld 7:ac88c8e35048 183 m3pi.locate(0,0);
uld 7:ac88c8e35048 184 m3pi.printf("Going to");
uld 7:ac88c8e35048 185 m3pi.locate(0,1);
uld 27:8561eeb0bd1d 186 m3pi.printf("**PIT**");
magnusmland 0:f562e4f9c29f 187 }
uld 8:5640c8c5088e 188 return result;
mikkelbredholt 13:ddff4bb7c24f 189 }
mikkelbredholt 13:ddff4bb7c24f 190
mikkelbredholt 13:ddff4bb7c24f 191 void LED_Control(int ledNumber, int state){
mikkelbredholt 13:ddff4bb7c24f 192 //LED1 on if robot is looking for pit
uld 20:76f94dec91d1 193 if (ledNumber == 1) {
vehus 15:8b76add42254 194 led1 = state;
vehus 15:8b76add42254 195 }
uld 20:76f94dec91d1 196 if (ledNumber == 2){
vehus 15:8b76add42254 197 led2 = state;
mikkelbredholt 13:ddff4bb7c24f 198 }
uld 20:76f94dec91d1 199 if (ledNumber == 3){
vehus 15:8b76add42254 200 led3 = state;
vehus 15:8b76add42254 201 }
uld 20:76f94dec91d1 202 if (ledNumber == 4){
vehus 15:8b76add42254 203 led4 = state;
mikkelbredholt 13:ddff4bb7c24f 204 }
vehus 15:8b76add42254 205 }
vehus 15:8b76add42254 206
uld 28:2c93dff934b1 207 void LED_Blink(int ledNumber)
vehus 15:8b76add42254 208 {
vehus 15:8b76add42254 209 int a = 2;
vehus 15:8b76add42254 210 LED_Control (ledNumber , 0);
vehus 15:8b76add42254 211 wait(a);
vehus 15:8b76add42254 212 LED_Control (ledNumber , 1);
vehus 15:8b76add42254 213 wait(a);
mikkelbredholt 13:ddff4bb7c24f 214 }
vehus 15:8b76add42254 215
uld 20:76f94dec91d1 216 void PS_PitStop(void)
vehus 15:8b76add42254 217 {
vehus 15:8b76add42254 218 /* Testing alternative stop function
vehus 15:8b76add42254 219 m3pi.left_motor(0);
vehus 15:8b76add42254 220 m3pi.right_motor(0);
vehus 15:8b76add42254 221 */
vehus 15:8b76add42254 222 m3pi.stop(); // stop all engine
vehus 15:8b76add42254 223
vehus 15:8b76add42254 224 // increase counter with one
vehus 15:8b76add42254 225 while (1)
vehus 15:8b76add42254 226 {
uld 28:2c93dff934b1 227 LED_Blink (1); // signal in pit
vehus 15:8b76add42254 228
vehus 15:8b76add42254 229 /* missing input to stop blink. */
vehus 15:8b76add42254 230
mikkelbredholt 13:ddff4bb7c24f 231 }
uld 20:76f94dec91d1 232 }
uld 20:76f94dec91d1 233
uld 20:76f94dec91d1 234 void PS_CreateLog(void){
uld 20:76f94dec91d1 235 /* Create a pitlog file and test if it can open*/
uld 20:76f94dec91d1 236 FILE *fptr;
uld 20:76f94dec91d1 237 fptr = fopen(PITLOGPATH,"w");
uld 20:76f94dec91d1 238
uld 20:76f94dec91d1 239 if(fptr == NULL)
uld 20:76f94dec91d1 240 {
uld 20:76f94dec91d1 241 printf("Error creating log file ");
uld 20:76f94dec91d1 242 exit(1);
uld 20:76f94dec91d1 243 }
uld 20:76f94dec91d1 244 fprintf(fptr,"%d", 0);
uld 20:76f94dec91d1 245 fclose(fptr);
uld 20:76f94dec91d1 246 }
uld 20:76f94dec91d1 247
uld 20:76f94dec91d1 248 void PS_AddStopToLog(void){
uld 20:76f94dec91d1 249 /*Opens the pit log and read the number.
uld 20:76f94dec91d1 250 * Then adds one to that number at write it into the pitlog */
uld 20:76f94dec91d1 251
uld 20:76f94dec91d1 252 FILE *fptr;
uld 20:76f94dec91d1 253 int x, y;
uld 20:76f94dec91d1 254 if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
uld 20:76f94dec91d1 255 printf("Error! opening file");
uld 20:76f94dec91d1 256 // Program exits if the file pointer returns NULL.
uld 20:76f94dec91d1 257 exit(1);
uld 20:76f94dec91d1 258 }
uld 20:76f94dec91d1 259
uld 20:76f94dec91d1 260 fscanf(fptr,"%d", &x);
uld 20:76f94dec91d1 261 fclose(fptr);
uld 20:76f94dec91d1 262
uld 20:76f94dec91d1 263 y = x+1;
uld 20:76f94dec91d1 264 fptr = fopen(PITLOGPATH,"w");
uld 20:76f94dec91d1 265
uld 20:76f94dec91d1 266 if(fptr == NULL)
uld 20:76f94dec91d1 267 {
uld 20:76f94dec91d1 268 printf("Error creating log file ");
uld 20:76f94dec91d1 269 exit(1);
uld 20:76f94dec91d1 270 }
uld 20:76f94dec91d1 271 fprintf(fptr,"%d", y);
uld 20:76f94dec91d1 272 fclose(fptr);
uld 20:76f94dec91d1 273 }
uld 20:76f94dec91d1 274
uld 20:76f94dec91d1 275 /*
uld 20:76f94dec91d1 276 void PS_DisplayNumberofPS(void){
uld 27:8561eeb0bd1d 277 // Display the number i the pitstop recorded in the logfile
uld 20:76f94dec91d1 278 FILE *fptr;
uld 20:76f94dec91d1 279 int x;
uld 20:76f94dec91d1 280 if ((fptr = fopen(PITLOGPATH,"r")) == NULL){
uld 20:76f94dec91d1 281 printf("Error! opening file");
uld 20:76f94dec91d1 282 // Program exits if the file pointer returns NULL.
uld 20:76f94dec91d1 283 exit(1);
uld 20:76f94dec91d1 284 }
uld 20:76f94dec91d1 285 fscanf(fptr,"%d", &x);
uld 20:76f94dec91d1 286 printf("Final number of pits stops %d", x);
uld 20:76f94dec91d1 287 fclose(fptr);
uld 20:76f94dec91d1 288 }
uld 20:76f94dec91d1 289 */
uld 20:76f94dec91d1 290
uld 20:76f94dec91d1 291 void TE_CreateVoltageLog(void){
uld 20:76f94dec91d1 292 /* Create a voltagelog file and test if it can open*/
uld 20:76f94dec91d1 293 FILE *fptr;
uld 20:76f94dec91d1 294 fptr = fopen(VOLTAGELOGPATH,"w");
uld 20:76f94dec91d1 295
uld 20:76f94dec91d1 296 if(fptr == NULL)
uld 20:76f94dec91d1 297 {
uld 20:76f94dec91d1 298 printf("Error creating log file ");
uld 20:76f94dec91d1 299 exit(1);
uld 20:76f94dec91d1 300 }
uld 20:76f94dec91d1 301
uld 20:76f94dec91d1 302 fclose(fptr);
uld 20:76f94dec91d1 303 }
uld 20:76f94dec91d1 304
uld 21:c3e256b18b96 305 void TE_LogVoltage(int count){
uld 20:76f94dec91d1 306 /* Create a pitlog file and test if it can open*/
uld 20:76f94dec91d1 307 FILE *fptr; /* voltagelog adres */
uld 20:76f94dec91d1 308 fptr = fopen(VOLTAGELOGPATH,"a");
uld 20:76f94dec91d1 309
uld 26:e6d82a8ba556 310 fprintf(fptr," %8d %4.4f %4.4f \n" ,count, m3pi.battery(),m3pi.pot_voltage() );
uld 20:76f94dec91d1 311 fclose(fptr);
uld 32:570b94fe2c19 312 }
uld 32:570b94fe2c19 313
vehus 37:f73c49c23183 314 // Julesnaps smager godt og Andreas skal drikke hvergang en anden drikker
vehus 37:f73c49c23183 315
vehus 38:fff744f551e3 316 // Det bliver hårdt :)
mikkelbredholt 40:b7d615920395 317 // Ingen sang - tak.
mikkelbredholt 40:b7d615920395 318 // Nu er alle med - forhåbentligt.