AutoDrive.cpp

00001 #include "mbed.h"
00002 #include "MotorDriver.h"
00003 #include "ReadFinalLine.h"
00004 #include "ReadSensor.h"
00005 #include "AutoDrive.h"
00006 #include "RouteCalculation.h" 
00007 
00008     void autoDrive(char route[]){
00009         
00010     int action = 0;
00011     int speed = 3;
00012     int placeholder = 1;
00013     
00014     while(route[action] != 5){
00015         
00016         switch(route[action]){
00017             case 1: // Fulldrive
00018                 driveOne(speed,route[action+1]);
00019                 break;
00020                 
00021             case 2: // turn Right
00022                 turnRight(placeholder);
00023                 break;
00024                 
00025             case 3: // turn Left
00026                 turnLeft(placeholder);
00027                 break;
00028                 
00029             case 4: // turn right on spot
00030                 turnRight(placeholder);
00031                 break;
00032                 
00033             default: printf("Error: route could not be red!");
00034         }
00035         
00036     action = action + 2;    
00037     }
00038     printf("The target destination has been sucessfully reached!");
00039     
00040     }