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@74:23064681b4f3, 2022-10-25 (annotated)

Committer:
magnusmland
Date:
Tue Oct 25 08:42:38 2022 +0000
Revision:
74:23064681b4f3
Parent:
73:0646bad028c5
Child:
83:bc6d7abc1a72
Child:
85:204026c91369
speed update;

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