ROBot.cpp

00001 /* File: ROBot.cpp
00002  * Author: Robert Abad          Copyright (c) 2013
00003  *
00004  * Desc: ROBot application.  Motorized robot with the following features:
00005  *       - WiFly interface for remote operation
00006  *       - ultrasonic range finder
00007  *       - motor driver for all locomotion
00008  *
00009  *       The ROBot boots and connects to a known WiFi network.  (NOTE:
00010  *       Currently, the settings for the WiFi network must be hardcoded
00011  *       into the firmware.  So joining a different network will require
00012  *       recompilation and reloading of the firmware).  Upon successful
00013  *       connection, ROBot's IP address is shown on the LCD screen.  The
00014  *       user then connects via PC telnet session.  In the telnet console
00015  *       window, the operator can enter the following commands:
00016  *
00017  *          e - forward motion
00018  *          c - backward motion
00019  *          s - rotate left
00020  *          f - rotate right
00021  *          d - stop
00022  *
00023  *       All commands are case-sensitive.
00024  *
00025  *       ROBot is also equipped with an ultrasonic range finder.  This
00026  *       device is used to detect any obstacles in the direct forward
00027  *       path.  A warning will be sent to the telnet session when an
00028  *       obstacle is within 50cm.  If forward motion continues, a warning
00029  *       will continue to be displayed.  If the obstacle is within 15cm,
00030  *       all forward motion is disabled.  The operator will need to
00031  *       use the other commands to avoid the obstacle.
00032  */
00033 
00034 #include "mbed.h"
00035 
00036 #include "C12832_lcd.h"
00037 #include "WiflyInterface.h"
00038 #include "HCSR04.h"
00039 #include "TB6612FNG.h"
00040 #include "USBSerial.h"
00041  
00042 // uncomment to enable DEBUG
00043 // enabling DEBUG does the following:
00044 // - various debug statements
00045 // - show status of various state machines
00046 //#define DEBUG
00047 
00048 // uncomment to enable (or comment to disable)
00049 #define ENABLE_WIFI
00050 #define ENABLE_RANGE_FINDER
00051 #define ENABLE_MOTOR_DRIVER
00052 
00053 // WiFi related
00054 #define WIFLY_PIN_TX            (p9)
00055 #define WIFLY_PIN_RX            (p10)
00056 #define WIFLY_PIN_STATUS        (p29)
00057 #define WIFLY_PIN_RESET         (p30)
00058 #define WIFI_ECHO_SERVER_PORT   (7)
00059 #define WIFI_BUFFER_SIZE        (64)
00060 WiflyInterface *wiFly = NULL;
00061 TCPSocketServer *server = NULL;
00062 TCPSocketConnection *client = NULL;
00063 char wiflyBuffer[WIFI_BUFFER_SIZE];
00064 
00065 // Range Finder
00066 #define HCSR04_PIN_TRIGGER  (p14)
00067 #define HCSR04_PIN_ECHO     (p15)
00068 #define RANGE_FINDER_MEAS_INTERVAL  (0.065f)
00069 HCSR04 rangeFinder( HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO );
00070 void rangeFinderTimer_cb(void);
00071 Ticker rangeFinderTimer;
00072 #define RANGE_WARNING               (0.5f)      // meters
00073 #define RANGE_COLLISION             (0.30f)     // meters
00074 
00075 // MotorDriver
00076 #define TB6612FNG_PIN_PWMA      (p22)
00077 #define TB6612FNG_PIN_AIN1      (p17)
00078 #define TB6612FNG_PIN_AIN2      (p16)
00079 #define TB6612FNG_PIN_PWMB      (p21)
00080 #define TB6612FNG_PIN_BIN1      (p19)
00081 #define TB6612FNG_PIN_BIN2      (p20)
00082 #define TB6612FNG_PIN_NSTBY     (p18)
00083 TB6612FNG motorDriver( TB6612FNG_PIN_PWMA, TB6612FNG_PIN_AIN1, TB6612FNG_PIN_AIN2,
00084                        TB6612FNG_PIN_PWMB, TB6612FNG_PIN_BIN1, TB6612FNG_PIN_BIN2,
00085                        TB6612FNG_PIN_NSTBY );
00086 float fPwmPeriod;
00087 float fPwmPulsewidth;
00088 
00089 // ROBot State Machine status
00090 typedef enum
00091 {
00092     STATUS_OK,
00093     STATUS_NEAR_COLLISION
00094 } etROBot_STATUS;
00095 etROBot_STATUS ROBot_STATUS;
00096 
00097 // ROBot commands
00098 typedef enum
00099 {
00100     CMD_MOVE_STOP     = 'd',
00101     CMD_MOVE_FORWARD  = 'e',
00102     CMD_MOVE_BACKWARD = 'c',
00103     CMD_MOVE_LEFT     = 's',
00104     CMD_MOVE_RIGHT    = 'f',
00105     CMD_MOVE_UNKNOWN  = -1
00106 } etROBot_CMD;
00107 #define NUM_CMDS    (5)
00108 
00109 void moveStop(void);
00110 void moveForward(void);
00111 void moveBackward(void);
00112 void moveLeft(void);
00113 void moveRight(void);
00114 
00115 typedef struct
00116 {
00117     etROBot_CMD cmd;
00118     void (*cmd_func)(void);
00119 } etROBot_COMMAND;
00120 
00121 const etROBot_COMMAND command[] =
00122 {
00123     { CMD_MOVE_STOP, moveStop },
00124     { CMD_MOVE_FORWARD, moveForward },
00125     { CMD_MOVE_BACKWARD, moveBackward },
00126     { CMD_MOVE_LEFT, moveLeft },
00127     { CMD_MOVE_RIGHT, moveRight }
00128 };
00129 int getCmdIdx(etROBot_CMD cmd);
00130 
00131 
00132 C12832_LCD lcdScreen;           //status display
00133 
00134 //Virtual serial port over USB
00135 USBSerial console;
00136 #define STR_BUFFER_SZ       (64)
00137 
00138 // forward declarations
00139 void init(void);            // intialize
00140 void ROBot_sm(void);        // ROBot state machine
00141 
00142 /* Name: main()
00143  * Desc: entry point and main thread of execution
00144  * Inputs: none
00145  * Outputs: none
00146  */
00147 int main(void)
00148 {
00149     char wifi_ssid[STR_BUFFER_SZ];
00150     char wifi_passwd[STR_BUFFER_SZ];
00151     Security wifi_security;
00152 
00153 #ifdef DEBUG
00154     printf("starting ROBot\n\r");
00155 #endif /* DEBUG */
00156 
00157     // establish console connection to get WiFi parameters
00158     while (1)
00159     {
00160         console.printf("Press <ENTER> to initialize ROBot\n\r");
00161         wait(1);
00162         if ( console.readable() )
00163         {
00164             break;
00165         }
00166     }
00167     console.printf("\n\rEnter SSID\n\r");
00168     console.scanf("%s", wifi_ssid);
00169     console.printf("\n\rEnter password (enter 0 if no password is required)\n\r");
00170     console.scanf("%s", wifi_passwd);
00171     console.printf("\n\rEnter security type (0=NONE,1=WEP_128,3=WPA)\n\r");
00172     console.scanf("%d", (Security *)&wifi_security);
00173 #ifdef DEBUG
00174     printf("SSID = %s\n\r", wifi_ssid);
00175     printf("passwd = %s\n\r", wifi_passwd);
00176     printf("security type = %d\n\r", wifi_security);
00177 #endif /* DEBUG */
00178     wiFly = new WiflyInterface(WIFLY_PIN_TX,
00179                                WIFLY_PIN_RX,
00180                                WIFLY_PIN_RESET,
00181                                WIFLY_PIN_STATUS,
00182                                wifi_ssid,
00183                                ((wifi_passwd[0]=='0')&&(wifi_passwd[1]=='\0')) ? NULL : wifi_passwd,
00184                                wifi_security);;
00185     server = new TCPSocketServer;
00186     client = new TCPSocketConnection;
00187     
00188     while ( !wiFly || !server || !client )
00189     {
00190 #ifdef DEBUG
00191         printf("ERROR: unable to allocate memory\n\r");
00192 #endif /* DEBUG */
00193     }
00194 
00195     // initilize the app
00196     init();
00197 
00198     while (1)
00199     {
00200         ROBot_sm();
00201     }
00202     
00203     // the following are commented yield a compilation warning of unreachable code.
00204     // I acknowledge that this code is indeed unreachable but is placed here for the
00205     // sake of completeness.  Normally, there would be a memory leak if wiFly, server,
00206     // and client are dynamically allocated and not deallocated appropriately.  However,
00207     // since shutdown of this app will require either a reset or shutdown of power, memory
00208     // will get reset avoiding any memory leak.
00209     delete client;
00210     delete server;
00211     delete wiFly;
00212 }
00213 
00214 /* Name: init()
00215  * Desc: initialize the ROBot
00216  * Inputs: none
00217  * Outputs: none
00218  */
00219 void init(void)
00220 {
00221     // intialize peripherals
00222     lcdScreen.cls();
00223 
00224     // connect WiFi
00225 #ifdef ENABLE_WIFI
00226   #ifdef DEBUG
00227     printf("Connecting to WiFi...");
00228   #endif /* DEBUG */
00229     wiFly->init(); // use DHCP
00230   #ifdef DEBUG
00231     printf("DONE\n\r");
00232   #endif /* DEBUG */
00233     while (!wiFly->connect()) // join the network
00234     {
00235         printf("attempting to connect\n\r");
00236     }
00237   #ifdef DEBUG
00238     printf("IP Address is %s\n\r", wiFly->getIPAddress());
00239   #endif /* DEBUG */
00240     lcdScreen.locate(0,0);
00241     lcdScreen.printf("IP: %s", wiFly->getIPAddress());
00242     console.printf("\n\rIP: %s\n\r", wiFly->getIPAddress());
00243     server->bind(WIFI_ECHO_SERVER_PORT);
00244     server->listen();
00245 
00246   #ifdef DEBUG
00247     printf("\n\rWait for remote connection...\n\r");
00248   #endif /* DEBUG */    
00249     server->accept(*client);  // wait until remote host connects
00250     client->set_blocking(false);
00251   #ifdef DEBUG
00252     printf("remote host connected!\n\r");
00253   #endif /* DEBUG */
00254     lcdScreen.locate(0,10);
00255     lcdScreen.printf("remote host connected!\n\r");
00256 #endif /* ENABLE_WIFI */
00257 
00258     // start Range Finder
00259 #ifdef ENABLE_RANGE_FINDER
00260   #ifdef DEBUG
00261     printf("Initialize Range Finder...");
00262   #endif /* DEBUG */
00263     rangeFinderTimer.attach(rangeFinderTimer_cb, RANGE_FINDER_MEAS_INTERVAL);
00264   #ifdef DEBUG
00265     printf("DONE\n\r");
00266   #endif /* DEBUG */
00267 #endif /* ENABLE_RANGE_FINDER */
00268 
00269     // start Motor Driver
00270 #ifdef ENABLE_MOTOR_DRIVER    
00271   #ifdef DEBUG
00272     printf("Initialize Motor Driver...");
00273   #endif /* DEBUG */
00274     fPwmPeriod = TB6612FNG_PWM_PERIOD_DEFAULT;
00275     fPwmPulsewidth = TB6612FNG_PWM_PULSEWIDTH_DEFAULT;
00276     motorDriver.setPwmAperiod(fPwmPeriod);
00277     motorDriver.setPwmBperiod(fPwmPeriod);
00278     motorDriver.setPwmApulsewidth(fPwmPulsewidth);
00279     motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
00280   #ifdef DEBUG
00281     printf("DONE\n\r");
00282   #endif /* DEBUG */
00283 #endif /* ENABLE_MOTOR_DRIVER */
00284     
00285     ROBot_STATUS = STATUS_OK;
00286 #ifdef DEBUG
00287     printf("Initialization complete!\n\r");
00288 #endif /* DEBUG */
00289 
00290     strcpy( wiflyBuffer, "ROBot awaiting commands\n\r");
00291     client->send_all(wiflyBuffer, strlen(wiflyBuffer));
00292 }
00293 
00294 /* Name: ROBot_sm()
00295  * Desc: ROBot state machine.  Processes all peripheral data and sets
00296  *       status accordingly
00297  * Inputs: none
00298  * Outputs: none
00299  */
00300 void ROBot_sm(void)
00301 {
00302     float range;
00303     
00304     if ( rangeFinder.getMeas(range) != RANGE_MEAS_INVALID )
00305     {
00306 #ifdef DEBUG
00307         printf("range = %.3f m\n\r", range);
00308 #endif /* DEBUG */
00309         if ( range < RANGE_COLLISION )
00310         {
00311             if ( ROBot_STATUS != STATUS_NEAR_COLLISION )
00312             {
00313                 moveStop();
00314             }
00315             ROBot_STATUS = STATUS_NEAR_COLLISION;
00316             strcpy( wiflyBuffer, "\n\r**** COLLISION AVOIDANCE ****");
00317             sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
00318             client->send_all(wiflyBuffer, strlen(wiflyBuffer));
00319 #ifdef DEBUG
00320             printf("%s", wiflyBuffer);
00321 #endif /* DEBUG */
00322         }
00323         else if ( range < RANGE_WARNING )
00324         {
00325             strcpy( wiflyBuffer, "\n\r**** COLLISION WARNING ****");
00326             sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
00327             client->send_all(wiflyBuffer, strlen(wiflyBuffer));
00328 #ifdef DEBUG
00329             printf("%s", wiflyBuffer);
00330 #endif /* DEBUG */
00331             if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
00332             {
00333                 ROBot_STATUS = STATUS_OK;
00334             }
00335         }
00336         else if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
00337         {
00338             ROBot_STATUS = STATUS_OK;
00339         }
00340     }
00341 
00342     // receive input and process it
00343     memset(wiflyBuffer, 0, sizeof(wiflyBuffer));
00344     int numCmds = client->receive(wiflyBuffer, sizeof(wiflyBuffer));
00345     etROBot_CMD *pCmd = (etROBot_CMD *) wiflyBuffer;
00346     while ( numCmds > 0 )
00347     {
00348         int cmdIdx = getCmdIdx(*pCmd);
00349         if ( cmdIdx == CMD_MOVE_UNKNOWN )
00350         {
00351             pCmd++;
00352             numCmds--;
00353             continue;
00354         }
00355         
00356         switch ( ROBot_STATUS )
00357         {
00358             case STATUS_OK:
00359                 command[cmdIdx].cmd_func();
00360             break;
00361         
00362             case STATUS_NEAR_COLLISION:
00363                 if ( *pCmd != CMD_MOVE_FORWARD )
00364                 {
00365                     command[cmdIdx].cmd_func();
00366                 }
00367             break;
00368         }
00369         pCmd++;
00370         numCmds--;
00371     }
00372 }
00373 
00374 /* Name: rangeFinderTimer_cb()
00375  * Desc: callback function for range finder timer.  Timer is used to
00376  *       initiate range finder measurements
00377  * Inputs: none
00378  * Outputs: none
00379  */
00380 void rangeFinderTimer_cb(void)
00381 {
00382     rangeFinder.startMeas();
00383 }
00384 
00385 /* Name: getCmdIdx()
00386  * Desc: returns command[] index of the input command
00387  * Inputs: none
00388  * Outputs: none
00389  */
00390 int getCmdIdx(etROBot_CMD cmd)
00391 {
00392     int i;
00393     
00394     for ( i = 0; i < NUM_CMDS; i++)
00395     {
00396         if ( cmd == command[i].cmd )
00397         {
00398             return i;
00399         }
00400     }
00401     return CMD_MOVE_UNKNOWN;
00402 }
00403 
00404 /* Name: moveStop()
00405  * Desc: ceases all motion
00406  * Inputs: none
00407  * Outputs: none
00408  */
00409 void moveStop(void)
00410 {
00411     motorDriver.standby();
00412 }
00413 
00414 /* Name: moveForward()
00415  * Desc: commands forward motion
00416  * Inputs: none
00417  * Outputs: none
00418  */
00419 void moveForward(void)
00420 {
00421     motorDriver.motorA_ccw();
00422     motorDriver.motorB_ccw();
00423 }
00424 
00425 /* Name: moveBackward()
00426  * Desc: commands backward motion
00427  * Inputs: none
00428  * Outputs: none
00429  */
00430 void moveBackward(void)
00431 {
00432     motorDriver.motorA_cw();
00433     motorDriver.motorB_cw();
00434 }
00435 
00436 /* Name: moveLeft()
00437  * Desc: commands left motion
00438  * Inputs: none
00439  * Outputs: none
00440  */
00441 void moveLeft(void)
00442 {
00443     motorDriver.motorA_ccw();
00444     motorDriver.motorB_cw();
00445 }
00446 
00447 /* Name: moveRight()
00448  * Desc: commands right motion
00449  * Inputs: none
00450  * Outputs: none
00451  */
00452 void moveRight(void)
00453 {
00454     motorDriver.motorA_cw();
00455     motorDriver.motorB_ccw();
00456 }