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

Committer:
vehus
Date:
Wed Oct 12 10:43:12 2022 +0000
Branch:
Andreas
Revision:
65:b6dad21c9e0d
Parent:
52:a2f53d9d2bdc
Child:
66:6403ec154b77
#define CYCLES... to LOOPS...

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