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@21:c3e256b18b96, 2022-10-11 (annotated)

Committer:
uld
Date:
Tue Oct 11 11:19:28 2022 +0000
Revision:
21:c3e256b18b96
Parent:
20:76f94dec91d1
Child:
22:5d3332fc4c5c
Smallfix

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