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@79:9d46b0d532e1, 2022-10-25 (annotated)

Committer:
uld
Date:
Tue Oct 25 09:22:42 2022 +0000
Revision:
79:9d46b0d532e1
Parent:
78:0d905b656132
Parent:
12:6fe33785b0f4
Child:
80:dd2134edd58b
tryto fix merge issues

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