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@67:990fbcfee16f, 2022-10-12 (annotated)

Committer:
uld
Date:
Wed Oct 12 10:49:29 2022 +0000
Revision:
67:990fbcfee16f
Parent:
66:6403ec154b77
Child:
68:bb2ee5b4f9dd
?v

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