main.cpp

00001 /**
00002  * @author Aaron Berk
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  *
00028  * Rover example.
00029  * 
00030  * Reads and executes a set of commands from a simple command script.
00031  */
00032 
00033 /**
00034  * Includes
00035  */
00036 #include "Rover.h"
00037 
00038 //Debugging.
00039 Serial pc(USBTX, USBRX);
00040 DigitalOut led1(LED1);
00041 
00042 //Globals.
00043 LocalFileSystem local("local");
00044 SDFileSystem sd(p11, p12, p13, p14, "sd");
00045 
00046 int main() {
00047 
00048     //left motor pwm, left motor brake, left motor direction,
00049     //right motor pwm, right motor brake, right motor direction,
00050     //left qei A, left qei B, left qei index, left pulses per rev,
00051     //right qei A, right qei B, right qei index, right pulses per rev.
00052     Rover myRover(p23, p19, p28, p26, p22, p24,
00053                   p29, p30, NC, 624,
00054                   p18, p21, NC, 624);
00055 
00056     //-----------------------
00057     // Simple command parser
00058     //-----------------------
00059     char  cmd0[16]; //{"move", "turn"}
00060     char  cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
00061     float cmd2 = 0; //{distance in METRES, angle in DEGREES}
00062 
00063     pc.printf("Starting...\n");
00064 
00065     //Wait a bit before we start moving.
00066     wait(3);
00067 
00068     //Open the command script.
00069     FILE *fp = fopen("/local/commands.txt", "r");
00070 
00071     //Check if we were successful in opening the command script.
00072     if (fp == NULL) {
00073         pc.printf("Opening file failed...\n");
00074     } else {
00075         pc.printf("Opening file succeeded!\n");
00076     }
00077 
00078     //While there's another line to read from the command script.
00079     while (fscanf(fp, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {
00080     
00081         led1 = 1;
00082     
00083         pc.printf("Start of new command\n");
00084 
00085         wait(1);
00086 
00087         pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);
00088 
00089         //move command.
00090         if (strcmp(cmd0, "move") == 0) {
00091             if (strcmp(cmd1, "forward") == 0) {
00092                 myRover.move(cmd2);
00093                 while (myRover.getState() != Rover::STATE_STATIONARY) {
00094                 
00095                 }
00096                 pc.printf("Finished!\n");
00097             } else if (strcmp(cmd1, "backward") == 0) {
00098                 myRover.move(-cmd2);
00099                 while (myRover.getState() != Rover::STATE_STATIONARY) {
00100                 }
00101             }
00102         }
00103         //turn command.
00104         else if (strcmp(cmd0, "turn") == 0) {
00105             if (strcmp(cmd1, "left") == 0) {
00106                 myRover.turn(-cmd2);
00107                 while (myRover.getState() != Rover::STATE_STATIONARY) {
00108                 }
00109             } else if (strcmp(cmd1, "right") == 0) {
00110                 myRover.turn(cmd2);
00111                 while (myRover.getState() != Rover::STATE_STATIONARY) {
00112                 }
00113             }
00114         }
00115         
00116         pc.printf("End of command\n");
00117         led1 = 0;
00118 
00119     }
00120 
00121     wait(1);
00122 
00123     myRover.stopLogging();
00124     fclose(fp);
00125 
00126 }