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@90:4e47612bc593, 2022-10-27 (annotated)

Committer:
mikkelbredholt
Date:
Thu Oct 27 11:13:59 2022 +0000
Revision:
90:4e47612bc593
Parent:
89:5ea90b86fefb 
BatteriCheckMedOptimerede v?rdier1

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