Janus Bo Andersen / Mbed 2 deprecated T2PRO1_master

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:   mbed m3pi

main.cpp@21:6dce9000da11, 2018-12-04 (annotated)

Committer:
janusboandersen
Date:
Tue Dec 04 15:47:02 2018 +0000
Revision:
21:6dce9000da11
Parent:
20:32e5cf8fe6dd
Child:
22:fe373ba68df3
pit detection and pit navigation implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
janusboandersen 18:8b3efa4d4a36 6 //File name
janusboandersen 18:8b3efa4d4a36 7 #define FILESYS "robot"
janusboandersen 18:8b3efa4d4a36 8 #define FILENAME "/robot/data.txt"
janusboandersen 18:8b3efa4d4a36 9
janusboandersen 20:32e5cf8fe6dd 10 //Team calibrated parameters for car and battery
janusboandersen 20:32e5cf8fe6dd 11 #define LOW_BATTERY_VOLTAGE 4.2
janusboandersen 20:32e5cf8fe6dd 12 #define CROSSROAD_REFL_VAL 350 //sensor reading on PC0 and PC4 at crossroad
janusboandersen 20:32e5cf8fe6dd 13
janusboandersen 21:6dce9000da11 14 //Pit control
janusboandersen 21:6dce9000da11 15 #define ENTER_PIT 1
janusboandersen 21:6dce9000da11 16 #define LEAVE_PIT -1
janusboandersen 21:6dce9000da11 17 #define SPEED_INSIDE_PIT 0.2
janusboandersen 21:6dce9000da11 18 #define PIT_TRANSPORT_TIME 1
janusboandersen 21:6dce9000da11 19
janusboandersen 20:32e5cf8fe6dd 20 //Car sensors and LEDs
janusboandersen 20:32e5cf8fe6dd 21 #define PC0 0 //location of PC0 reflectance sensor
janusboandersen 20:32e5cf8fe6dd 22 #define PC4 4 //location of PC4 reflectance sensor
janusboandersen 20:32e5cf8fe6dd 23
chris 0:78f9794620a3 24 // Minimum and maximum motor speeds
janusboandersen 19:c0d59019de53 25 #define MAX 1.0
chris 0:78f9794620a3 26 #define MIN 0
janusboandersen 19:c0d59019de53 27 #define OPTIMAL_SPEED 0.8
janusboandersen 19:c0d59019de53 28 #define PIT_SPEED 0.3
chris 0:78f9794620a3 29
chris 0:78f9794620a3 30 // PID terms
chris 0:78f9794620a3 31 #define P_TERM 1
janusboandersen 19:c0d59019de53 32 #define D_TERM 8
chris 0:78f9794620a3 33 #define I_TERM 0
janusboandersen 19:c0d59019de53 34
janusboandersen 19:c0d59019de53 35 //Forward declare our type for storing car performance date
janusboandersen 19:c0d59019de53 36 //It can now be used as a type by the name performance_data
janusboandersen 19:c0d59019de53 37 typedef struct pd {
janusboandersen 19:c0d59019de53 38 float wh;
janusboandersen 19:c0d59019de53 39 int pitstops;
janusboandersen 19:c0d59019de53 40 } performance_data;
Blunsdon 1:47ea51d9e25d 41
janusboandersen 18:8b3efa4d4a36 42
janusboandersen 19:c0d59019de53 43 //Function to write car performance data to persistent file
janusboandersen 18:8b3efa4d4a36 44 void write_to_file(performance_data data) {
janusboandersen 18:8b3efa4d4a36 45
janusboandersen 18:8b3efa4d4a36 46 //Define file system
janusboandersen 18:8b3efa4d4a36 47 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 48
janusboandersen 19:c0d59019de53 49 //Open a file (or create a new) with write access
janusboandersen 19:c0d59019de53 50 //"w": write "r":read "a":append (add @end)
janusboandersen 19:c0d59019de53 51 //"w": will overwrite any existing data, so we just store one line.
janusboandersen 19:c0d59019de53 52 FILE *fp = fopen(FILENAME, "w");
janusboandersen 18:8b3efa4d4a36 53
janusboandersen 19:c0d59019de53 54 //write performance data to the file
janusboandersen 19:c0d59019de53 55 fprintf(fp, "%f %d", data.wh, data.pitstops);
janusboandersen 18:8b3efa4d4a36 56
janusboandersen 19:c0d59019de53 57 //close the file and release the memory when done.
janusboandersen 19:c0d59019de53 58 fclose(fp);
janusboandersen 18:8b3efa4d4a36 59
janusboandersen 18:8b3efa4d4a36 60 }
janusboandersen 18:8b3efa4d4a36 61
janusboandersen 19:c0d59019de53 62 //Function to read car performance data from persistent file
janusboandersen 18:8b3efa4d4a36 63 performance_data read_from_file() {
janusboandersen 18:8b3efa4d4a36 64
janusboandersen 19:c0d59019de53 65 //Declare the variable which will ultimately be returned
janusboandersen 18:8b3efa4d4a36 66 performance_data data;
janusboandersen 18:8b3efa4d4a36 67
janusboandersen 19:c0d59019de53 68 //Define file system on mbed LPC1768
janusboandersen 18:8b3efa4d4a36 69 LocalFileSystem local(FILESYS);
janusboandersen 18:8b3efa4d4a36 70
janusboandersen 19:c0d59019de53 71 //Try to open the file as read (it might not exist)
janusboandersen 19:c0d59019de53 72 FILE *fp = fopen(FILENAME, "r");
janusboandersen 19:c0d59019de53 73
janusboandersen 19:c0d59019de53 74 //Test whether the file exists
janusboandersen 19:c0d59019de53 75 if (fp != NULL) {
janusboandersen 19:c0d59019de53 76
janusboandersen 19:c0d59019de53 77 //file exists, so read from it
janusboandersen 19:c0d59019de53 78
janusboandersen 19:c0d59019de53 79 //Read stored data from the file and put in struct
janusboandersen 19:c0d59019de53 80 // &(struct.member) and &struct.member yield same address
janusboandersen 19:c0d59019de53 81 fscanf(fp, "%f %d", &(data.wh), &(data.pitstops) );
janusboandersen 18:8b3efa4d4a36 82
janusboandersen 19:c0d59019de53 83 //close file and release mem.
janusboandersen 19:c0d59019de53 84 fclose(fp);
janusboandersen 19:c0d59019de53 85
janusboandersen 19:c0d59019de53 86 } else {
janusboandersen 19:c0d59019de53 87
janusboandersen 19:c0d59019de53 88 //file doesn't exist, so just return zeros (no performance yet)
janusboandersen 19:c0d59019de53 89 data.wh = 0.0;
janusboandersen 19:c0d59019de53 90 data.pitstops = 0;
janusboandersen 19:c0d59019de53 91 }
janusboandersen 18:8b3efa4d4a36 92
janusboandersen 19:c0d59019de53 93 //return the data object to the caller
janusboandersen 18:8b3efa4d4a36 94 return data;
janusboandersen 19:c0d59019de53 95 }
janusboandersen 19:c0d59019de53 96
janusboandersen 20:32e5cf8fe6dd 97 //This function tests the car's battery state, and returs true if it needs
janusboandersen 20:32e5cf8fe6dd 98 //to charge. Otherwise false is returned.
janusboandersen 19:c0d59019de53 99 bool battery_low() {
janusboandersen 20:32e5cf8fe6dd 100
janusboandersen 20:32e5cf8fe6dd 101 //Get battery level from the 3pi robot
janusboandersen 20:32e5cf8fe6dd 102 float voltage = m3pi.battery();
janusboandersen 20:32e5cf8fe6dd 103
janusboandersen 20:32e5cf8fe6dd 104 //return battery state result
janusboandersen 20:32e5cf8fe6dd 105 return (voltage <= LOW_BATTERY_VOLTAGE) ? true : false;
janusboandersen 18:8b3efa4d4a36 106 }
janusboandersen 18:8b3efa4d4a36 107
janusboandersen 18:8b3efa4d4a36 108
janusboandersen 19:c0d59019de53 109 //TO DO: CLEAN UP THIS FUNCTION
Blunsdon 12:dfc717775297 110 void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
Blunsdon 1:47ea51d9e25d 111 {
Blunsdon 12:dfc717775297 112 m3pi.putc(0x87); // send noget sensor data
Blunsdon 1:47ea51d9e25d 113
janusboandersen 4:5939f9ad88ca 114 for(int n=0; n < 5; n++) // forloop så sensor for data
Blunsdon 1:47ea51d9e25d 115 {
Blunsdon 1:47ea51d9e25d 116 char lowbyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 117 char hibyte = m3pi.getc();
Blunsdon 12:dfc717775297 118 arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
Blunsdon 1:47ea51d9e25d 119 }
Blunsdon 9:fa745d3afcac 120 m3pi.cls();
Blunsdon 9:fa745d3afcac 121 m3pi.locate(0,1);
Blunsdon 12:dfc717775297 122 m3pi.printf("%d", arr[0]);
Blunsdon 12:dfc717775297 123 m3pi.locate(0,0);
Blunsdon 12:dfc717775297 124 m3pi.printf("%d", arr[4]);
janusboandersen 20:32e5cf8fe6dd 125 }
janusboandersen 20:32e5cf8fe6dd 126
janusboandersen 20:32e5cf8fe6dd 127 //Function to check whether car is just at the pit
janusboandersen 20:32e5cf8fe6dd 128 bool car_at_pit_crossroad(int sensor_array[]) {
janusboandersen 20:32e5cf8fe6dd 129
janusboandersen 20:32e5cf8fe6dd 130 //Read the sensor values
janusboandersen 20:32e5cf8fe6dd 131 sensor_all(sensor_array);
janusboandersen 20:32e5cf8fe6dd 132
janusboandersen 20:32e5cf8fe6dd 133 //Check the PC0 and PC4 sensors vs thresholds
janusboandersen 20:32e5cf8fe6dd 134 if (sensor_array[PC0] > CROSSROAD_REFL_VAL &&
janusboandersen 20:32e5cf8fe6dd 135 sensor_array[PC4] > CROSSROAD_REFL_VAL) {
janusboandersen 20:32e5cf8fe6dd 136
janusboandersen 20:32e5cf8fe6dd 137 //Car is at the crossroad
janusboandersen 20:32e5cf8fe6dd 138 return true;
janusboandersen 20:32e5cf8fe6dd 139 } else {
janusboandersen 20:32e5cf8fe6dd 140 //Car is NOT at the crossroad
janusboandersen 20:32e5cf8fe6dd 141 return false;
janusboandersen 20:32e5cf8fe6dd 142 }
janusboandersen 20:32e5cf8fe6dd 143 }
janusboandersen 20:32e5cf8fe6dd 144
janusboandersen 21:6dce9000da11 145 //Turn the car 90 degrees left
janusboandersen 21:6dce9000da11 146 void turn_left (void)
janusboandersen 21:6dce9000da11 147 {
janusboandersen 21:6dce9000da11 148 m3pi.left(0.3);
janusboandersen 21:6dce9000da11 149 wait (0.22);
janusboandersen 21:6dce9000da11 150 }
janusboandersen 21:6dce9000da11 151
janusboandersen 21:6dce9000da11 152 //Turn the car 90 degrees right
janusboandersen 21:6dce9000da11 153 void turn_right (void)
janusboandersen 21:6dce9000da11 154 {
janusboandersen 21:6dce9000da11 155 m3pi.right (0.3);
janusboandersen 21:6dce9000da11 156 wait (0.22);
janusboandersen 21:6dce9000da11 157 }
janusboandersen 21:6dce9000da11 158
janusboandersen 21:6dce9000da11 159
janusboandersen 21:6dce9000da11 160 //Safely navigate the car from the pit crossroad and all the way to the charger.
janusboandersen 21:6dce9000da11 161 //direction = 1 to enter pit... direction = -1 to leave the pit.
janusboandersen 21:6dce9000da11 162 //takes control all the way from track to pit OR reverse.
janusboandersen 21:6dce9000da11 163 //Error handling: If a wrong code is passed, the function does nothing quietly.
janusboandersen 21:6dce9000da11 164 void pit_navigate(int direction) {
janusboandersen 21:6dce9000da11 165
janusboandersen 21:6dce9000da11 166 //Car must enter pit forward
janusboandersen 21:6dce9000da11 167 if (direction == ENTER_PIT) {
janusboandersen 21:6dce9000da11 168 m3pi.stop();
janusboandersen 21:6dce9000da11 169 turn_right();
janusboandersen 21:6dce9000da11 170 m3pi.stop();
janusboandersen 21:6dce9000da11 171 m3pi.forward(SPEED_INSIDE_PIT);
janusboandersen 21:6dce9000da11 172 wait(PIT_TRANSPORT_TIME);
janusboandersen 21:6dce9000da11 173 m3pi.stop();
janusboandersen 21:6dce9000da11 174 }
janusboandersen 21:6dce9000da11 175
janusboandersen 21:6dce9000da11 176 //Car must leave pit backward
janusboandersen 21:6dce9000da11 177 if (direction == LEAVE_PIT) {
janusboandersen 21:6dce9000da11 178 m3pi.backward(SPEED_INSIDE_PIT);
janusboandersen 21:6dce9000da11 179 wait(PIT_TRANSPORT_TIME);
janusboandersen 21:6dce9000da11 180 m3pi.stop();
janusboandersen 21:6dce9000da11 181 turn_left();
janusboandersen 21:6dce9000da11 182 m3pi.stop();
janusboandersen 21:6dce9000da11 183 }
janusboandersen 21:6dce9000da11 184 }
janusboandersen 20:32e5cf8fe6dd 185
Blunsdon 13:6a8689f601cd 186 int maze (void)
Blunsdon 13:6a8689f601cd 187 {
Blunsdon 13:6a8689f601cd 188
Blunsdon 13:6a8689f601cd 189 return 1;
Blunsdon 14:408d9d087d0f 190 }
Blunsdon 14:408d9d087d0f 191
chris 0:78f9794620a3 192
chris 0:78f9794620a3 193 int main() {
janusboandersen 18:8b3efa4d4a36 194
janusboandersen 19:c0d59019de53 195 /* Define all needed variables in this section */
janusboandersen 19:c0d59019de53 196 performance_data car_data; //Saves the car's Wh and pitstops data
janusboandersen 19:c0d59019de53 197
janusboandersen 19:c0d59019de53 198 float right; //right motor speed
janusboandersen 19:c0d59019de53 199 float left; //left motor speed
janusboandersen 19:c0d59019de53 200 float current_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 201 float previous_pos_of_line = 0.0; //for PID
janusboandersen 19:c0d59019de53 202 float derivative,proportional,integral = 0; //for PID
janusboandersen 19:c0d59019de53 203 float power; //differential speed
janusboandersen 19:c0d59019de53 204 float speed = OPTIMAL_SPEED; //our optimal speed setting
janusboandersen 21:6dce9000da11 205 int sensor_values[5]; //array for reflectance sensor values
janusboandersen 20:32e5cf8fe6dd 206 bool battery_low_flag = false;
janusboandersen 2:d22dcc2bfcc1 207
janusboandersen 19:c0d59019de53 208 /* When the car starts, read previous progress
janusboandersen 19:c0d59019de53 209 * if only connected to USB, do not perform this restore process
janusboandersen 19:c0d59019de53 210 */
janusboandersen 19:c0d59019de53 211
janusboandersen 19:c0d59019de53 212 // If robot is on this returns a value, otherwise it's just connected USB
janusboandersen 19:c0d59019de53 213 if (m3pi.battery() > 0) {
janusboandersen 19:c0d59019de53 214
janusboandersen 19:c0d59019de53 215 car_data = read_from_file(); //restore data from file
janusboandersen 19:c0d59019de53 216
janusboandersen 19:c0d59019de53 217 }
janusboandersen 19:c0d59019de53 218
janusboandersen 19:c0d59019de53 219 //Info to user
chris 0:78f9794620a3 220 m3pi.locate(0,1);
chris 0:78f9794620a3 221 m3pi.printf("Line PID");
chris 0:78f9794620a3 222
janusboandersen 19:c0d59019de53 223 //Calibrate before starting <- might be moved somewhere else
chris 0:78f9794620a3 224 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 225
janusboandersen 19:c0d59019de53 226 //Begin main event loop
chris 0:78f9794620a3 227 while (1) {
janusboandersen 17:c2e21d347ca5 228
janusboandersen 20:32e5cf8fe6dd 229 /* Check sensors
janusboandersen 20:32e5cf8fe6dd 230 * - might not be every loop: wrap this in a timer of sorts */
janusboandersen 21:6dce9000da11 231 battery_low_flag = battery_low();
janusboandersen 20:32e5cf8fe6dd 232
janusboandersen 20:32e5cf8fe6dd 233
janusboandersen 17:c2e21d347ca5 234 /*
janusboandersen 17:c2e21d347ca5 235 switch
janusboandersen 17:c2e21d347ca5 236 if nothing else: line following
janusboandersen 20:32e5cf8fe6dd 237
janusboandersen 20:32e5cf8fe6dd 238 */
janusboandersen 20:32e5cf8fe6dd 239
janusboandersen 21:6dce9000da11 240 //if low_batt: decellerate car (slow down) and continue to pit
janusboandersen 20:32e5cf8fe6dd 241 if ( battery_low_flag ) {
janusboandersen 20:32e5cf8fe6dd 242 speed = (speed > PIT_SPEED) ? speed-0.1 : PIT_SPEED;
janusboandersen 20:32e5cf8fe6dd 243 }
janusboandersen 19:c0d59019de53 244
janusboandersen 20:32e5cf8fe6dd 245 /*
janusboandersen 20:32e5cf8fe6dd 246 if atPitIntersection: increment internal lap counter
janusboandersen 20:32e5cf8fe6dd 247 */
janusboandersen 19:c0d59019de53 248
janusboandersen 20:32e5cf8fe6dd 249 //if low_batt && atPitIntersection: Stop && execute pit navigation program
janusboandersen 21:6dce9000da11 250 if ( battery_low_flag && car_at_pit_crossroad(sensor_values) ) {
janusboandersen 21:6dce9000da11 251 //We are at the pit, and the battery is low
janusboandersen 21:6dce9000da11 252
janusboandersen 21:6dce9000da11 253 }
janusboandersen 19:c0d59019de53 254
janusboandersen 20:32e5cf8fe6dd 255 /*
janusboandersen 17:c2e21d347ca5 256 if all_sensors == 1000 (lifted from track):
janusboandersen 17:c2e21d347ca5 257 show Wh and pitstops on display
janusboandersen 17:c2e21d347ca5 258 stop motors, reset PID errors, wait 20 secs, recalibrate sensors
janusboandersen 17:c2e21d347ca5 259
janusboandersen 17:c2e21d347ca5 260 if all_sensors < 90 (driven off track):
janusboandersen 17:c2e21d347ca5 261 stop and ask for help? Flash LEDs? turn back and continue? Reset PID errors
janusboandersen 17:c2e21d347ca5 262
janusboandersen 17:c2e21d347ca5 263 Periodically (not necessarily every lap - maybe every 2-5th lap):
janusboandersen 17:c2e21d347ca5 264 Save to file -> possibly only after pit
janusboandersen 17:c2e21d347ca5 265 Check battery voltage -> every couple of minutes
janusboandersen 17:c2e21d347ca5 266 Check light sensors for drift -> recalibrate (optional)
janusboandersen 17:c2e21d347ca5 267
janusboandersen 17:c2e21d347ca5 268 */
janusboandersen 17:c2e21d347ca5 269
janusboandersen 17:c2e21d347ca5 270
janusboandersen 17:c2e21d347ca5 271
janusboandersen 17:c2e21d347ca5 272 /* Separate functions
janusboandersen 17:c2e21d347ca5 273 pit_navigation (entering pit from T-intersection all the way to the charger)
janusboandersen 17:c2e21d347ca5 274 pit_execution (only when connected to charger - voltage > V_CHARGER_MIN
janusboandersen 17:c2e21d347ca5 275 add 1 to number of pitstops
janusboandersen 17:c2e21d347ca5 276 charge and wait for signal from the external charging circuit
janusboandersen 17:c2e21d347ca5 277 during charging: integrate v(t) dt, add to cumulative sum
janusboandersen 17:c2e21d347ca5 278 when fully charged (or signal goes high):
janusboandersen 17:c2e21d347ca5 279 Calculate delta_Wh ( integrate{ v(t) dt} * u * 1/3600 )
janusboandersen 17:c2e21d347ca5 280 Calculate total_Wh
janusboandersen 17:c2e21d347ca5 281 save progress data to file
janusboandersen 17:c2e21d347ca5 282 leave the charger -> pass control to pit_exit
janusboandersen 17:c2e21d347ca5 283
janusboandersen 17:c2e21d347ca5 284 pit_exit
janusboandersen 17:c2e21d347ca5 285 reverse out of the pit back to T-intersection
janusboandersen 17:c2e21d347ca5 286 continue line following away from T-intersection, recal sensors after a few seconds
janusboandersen 17:c2e21d347ca5 287
janusboandersen 17:c2e21d347ca5 288
janusboandersen 17:c2e21d347ca5 289 AfterPitAdmin: Calibrate sensors, save state to file,
janusboandersen 17:c2e21d347ca5 290
janusboandersen 17:c2e21d347ca5 291 show_results_on_display:
janusboandersen 17:c2e21d347ca5 292
janusboandersen 17:c2e21d347ca5 293 check_sensors_for_errors:
janusboandersen 17:c2e21d347ca5 294
janusboandersen 17:c2e21d347ca5 295 check_battery_voltage -> return true false
janusboandersen 17:c2e21d347ca5 296 compare m3pi.battery() < V_CRITICAL
janusboandersen 17:c2e21d347ca5 297
janusboandersen 17:c2e21d347ca5 298
janusboandersen 17:c2e21d347ca5 299
janusboandersen 17:c2e21d347ca5 300 HUSK FOR POKKER KOLLES KODESTANDARD!
janusboandersen 17:c2e21d347ca5 301
janusboandersen 17:c2e21d347ca5 302 */
janusboandersen 17:c2e21d347ca5 303
janusboandersen 17:c2e21d347ca5 304
Blunsdon 11:394ab193971f 305 // Get the position of the line.
chris 0:78f9794620a3 306 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 307 proportional = current_pos_of_line;
chris 0:78f9794620a3 308
chris 0:78f9794620a3 309 // Compute the derivative
chris 0:78f9794620a3 310 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 311
chris 0:78f9794620a3 312 // Compute the integral
chris 0:78f9794620a3 313 integral += proportional;
chris 0:78f9794620a3 314
chris 0:78f9794620a3 315 // Remember the last position.
chris 0:78f9794620a3 316 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 317
chris 0:78f9794620a3 318 // Compute the power
chris 0:78f9794620a3 319 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 320
chris 0:78f9794620a3 321 // Compute new speeds
chris 0:78f9794620a3 322 right = speed+power;
chris 0:78f9794620a3 323 left = speed-power;
chris 0:78f9794620a3 324
chris 0:78f9794620a3 325 // limit checks
chris 0:78f9794620a3 326 if (right < MIN)
chris 0:78f9794620a3 327 right = MIN;
chris 0:78f9794620a3 328 else if (right > MAX)
chris 0:78f9794620a3 329 right = MAX;
chris 0:78f9794620a3 330
chris 0:78f9794620a3 331 if (left < MIN)
chris 0:78f9794620a3 332 left = MIN;
chris 0:78f9794620a3 333 else if (left > MAX)
chris 0:78f9794620a3 334 left = MAX;
chris 0:78f9794620a3 335
chris 0:78f9794620a3 336 // set speed
chris 0:78f9794620a3 337 m3pi.left_motor(left);
chris 0:78f9794620a3 338 m3pi.right_motor(right);
Blunsdon 11:394ab193971f 339
chris 0:78f9794620a3 340
janusboandersen 21:6dce9000da11 341 } //end while
janusboandersen 21:6dce9000da11 342 } //end main