Welcome to Project ROBot!

File content as of revision 5:c15ad8a7eb8b:

/* File: ROBot.cpp
 * Author: Robert Abad          Copyright (c) 2013
 *
 * Desc: ROBot application.  Motorized robot with the following features:
 *       - WiFly interface for remote operation
 *       - ultrasonic range finder
 *       - motor driver for all locomotion
 *
 *       The ROBot boots and connects to a known WiFi network.  (NOTE:
 *       Currently, the settings for the WiFi network must be hardcoded
 *       into the firmware.  So joining a different network will require
 *       recompilation and reloading of the firmware).  Upon successful
 *       connection, ROBot's IP address is shown on the LCD screen.  The
 *       user then connects via PC telnet session.  In the telnet console
 *       window, the operator can enter the following commands:
 *
 *          e - forward motion
 *          c - backward motion
 *          s - rotate left
 *          f - rotate right
 *          d - stop
 *
 *       All commands are case-sensitive.
 *
 *       ROBot is also equipped with an ultrasonic range finder.  This
 *       device is used to detect any obstacles in the direct forward
 *       path.  A warning will be sent to the telnet session when an
 *       obstacle is within 50cm.  If forward motion continues, a warning
 *       will continue to be displayed.  If the obstacle is within 15cm,
 *       all forward motion is disabled.  The operator will need to
 *       use the other commands to avoid the obstacle.
 */

#include "mbed.h"

#include "C12832_lcd.h"
#include "WiflyInterface.h"
#include "HCSR04.h"
#include "TB6612FNG.h"
#include "USBSerial.h"
 
// uncomment to enable DEBUG
// enabling DEBUG does the following:
// - various debug statements
// - show status of various state machines
//#define DEBUG

// uncomment to enable (or comment to disable)
#define ENABLE_WIFI
#define ENABLE_RANGE_FINDER
#define ENABLE_MOTOR_DRIVER

// WiFi related
#define WIFLY_PIN_TX            (p9)
#define WIFLY_PIN_RX            (p10)
#define WIFLY_PIN_STATUS        (p29)
#define WIFLY_PIN_RESET         (p30)
#define WIFI_ECHO_SERVER_PORT   (7)
#define WIFI_BUFFER_SIZE        (64)
WiflyInterface *wiFly = NULL;
TCPSocketServer *server = NULL;
TCPSocketConnection *client = NULL;
char wiflyBuffer[WIFI_BUFFER_SIZE];

// Range Finder
#define HCSR04_PIN_TRIGGER  (p14)
#define HCSR04_PIN_ECHO     (p15)
#define RANGE_FINDER_MEAS_INTERVAL  (0.065f)
HCSR04 rangeFinder( HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO );
void rangeFinderTimer_cb(void);
Ticker rangeFinderTimer;
#define RANGE_WARNING               (0.5f)      // meters
#define RANGE_COLLISION             (0.30f)     // meters

// MotorDriver
#define TB6612FNG_PIN_PWMA      (p22)
#define TB6612FNG_PIN_AIN1      (p17)
#define TB6612FNG_PIN_AIN2      (p16)
#define TB6612FNG_PIN_PWMB      (p21)
#define TB6612FNG_PIN_BIN1      (p19)
#define TB6612FNG_PIN_BIN2      (p20)
#define TB6612FNG_PIN_NSTBY     (p18)
TB6612FNG motorDriver( TB6612FNG_PIN_PWMA, TB6612FNG_PIN_AIN1, TB6612FNG_PIN_AIN2,
                       TB6612FNG_PIN_PWMB, TB6612FNG_PIN_BIN1, TB6612FNG_PIN_BIN2,
                       TB6612FNG_PIN_NSTBY );
float fPwmPeriod;
float fPwmPulsewidth;

// ROBot State Machine status
typedef enum
{
    STATUS_OK,
    STATUS_NEAR_COLLISION
} etROBot_STATUS;
etROBot_STATUS ROBot_STATUS;

// ROBot commands
typedef enum
{
    CMD_MOVE_STOP     = 'd',
    CMD_MOVE_FORWARD  = 'e',
    CMD_MOVE_BACKWARD = 'c',
    CMD_MOVE_LEFT     = 's',
    CMD_MOVE_RIGHT    = 'f',
    CMD_MOVE_UNKNOWN  = -1
} etROBot_CMD;
#define NUM_CMDS    (5)

void moveStop(void);
void moveForward(void);
void moveBackward(void);
void moveLeft(void);
void moveRight(void);

typedef struct
{
    etROBot_CMD cmd;
    void (*cmd_func)(void);
} etROBot_COMMAND;

const etROBot_COMMAND command[] =
{
    { CMD_MOVE_STOP, moveStop },
    { CMD_MOVE_FORWARD, moveForward },
    { CMD_MOVE_BACKWARD, moveBackward },
    { CMD_MOVE_LEFT, moveLeft },
    { CMD_MOVE_RIGHT, moveRight }
};
int getCmdIdx(etROBot_CMD cmd);


C12832_LCD lcdScreen;           //status display

//Virtual serial port over USB
USBSerial console;
#define STR_BUFFER_SZ       (64)

// forward declarations
void init(void);            // intialize
void ROBot_sm(void);        // ROBot state machine

/* Name: main()
 * Desc: entry point and main thread of execution
 * Inputs: none
 * Outputs: none
 */
int main(void)
{
    char wifi_ssid[STR_BUFFER_SZ];
    char wifi_passwd[STR_BUFFER_SZ];
    Security wifi_security;

#ifdef DEBUG
    printf("starting ROBot\n\r");
#endif /* DEBUG */

    // establish console connection to get WiFi parameters
    while (1)
    {
        console.printf("Press <ENTER> to initialize ROBot\n\r");
        wait(1);
        if ( console.readable() )
        {
            break;
        }
    }
    console.printf("\n\rEnter SSID\n\r");
    console.scanf("%s", wifi_ssid);
    console.printf("\n\rEnter password (enter 0 if no password is required)\n\r");
    console.scanf("%s", wifi_passwd);
    console.printf("\n\rEnter security type (0=NONE,1=WEP_128,3=WPA)\n\r");
    console.scanf("%d", (Security *)&wifi_security);
#ifdef DEBUG
    printf("SSID = %s\n\r", wifi_ssid);
    printf("passwd = %s\n\r", wifi_passwd);
    printf("security type = %d\n\r", wifi_security);
#endif /* DEBUG */
    wiFly = new WiflyInterface(WIFLY_PIN_TX,
                               WIFLY_PIN_RX,
                               WIFLY_PIN_RESET,
                               WIFLY_PIN_STATUS,
                               wifi_ssid,
                               ((wifi_passwd[0]=='0')&&(wifi_passwd[1]=='\0')) ? NULL : wifi_passwd,
                               wifi_security);;
    server = new TCPSocketServer;
    client = new TCPSocketConnection;
    
    while ( !wiFly || !server || !client )
    {
#ifdef DEBUG
        printf("ERROR: unable to allocate memory\n\r");
#endif /* DEBUG */
    }

    // initilize the app
    init();

    while (1)
    {
        ROBot_sm();
    }
    
    // the following are commented yield a compilation warning of unreachable code.
    // I acknowledge that this code is indeed unreachable but is placed here for the
    // sake of completeness.  Normally, there would be a memory leak if wiFly, server,
    // and client are dynamically allocated and not deallocated appropriately.  However,
    // since shutdown of this app will require either a reset or shutdown of power, memory
    // will get reset avoiding any memory leak.
    delete client;
    delete server;
    delete wiFly;
}

/* Name: init()
 * Desc: initialize the ROBot
 * Inputs: none
 * Outputs: none
 */
void init(void)
{
    // intialize peripherals
    lcdScreen.cls();

    // connect WiFi
#ifdef ENABLE_WIFI
  #ifdef DEBUG
    printf("Connecting to WiFi...");
  #endif /* DEBUG */
    wiFly->init(); // use DHCP
  #ifdef DEBUG
    printf("DONE\n\r");
  #endif /* DEBUG */
    while (!wiFly->connect()) // join the network
    {
        printf("attempting to connect\n\r");
    }
  #ifdef DEBUG
    printf("IP Address is %s\n\r", wiFly->getIPAddress());
  #endif /* DEBUG */
    lcdScreen.locate(0,0);
    lcdScreen.printf("IP: %s", wiFly->getIPAddress());
    console.printf("\n\rIP: %s\n\r", wiFly->getIPAddress());
    server->bind(WIFI_ECHO_SERVER_PORT);
    server->listen();

  #ifdef DEBUG
    printf("\n\rWait for remote connection...\n\r");
  #endif /* DEBUG */    
    server->accept(*client);  // wait until remote host connects
    client->set_blocking(false);
  #ifdef DEBUG
    printf("remote host connected!\n\r");
  #endif /* DEBUG */
    lcdScreen.locate(0,10);
    lcdScreen.printf("remote host connected!\n\r");
#endif /* ENABLE_WIFI */

    // start Range Finder
#ifdef ENABLE_RANGE_FINDER
  #ifdef DEBUG
    printf("Initialize Range Finder...");
  #endif /* DEBUG */
    rangeFinderTimer.attach(rangeFinderTimer_cb, RANGE_FINDER_MEAS_INTERVAL);
  #ifdef DEBUG
    printf("DONE\n\r");
  #endif /* DEBUG */
#endif /* ENABLE_RANGE_FINDER */

    // start Motor Driver
#ifdef ENABLE_MOTOR_DRIVER    
  #ifdef DEBUG
    printf("Initialize Motor Driver...");
  #endif /* DEBUG */
    fPwmPeriod = TB6612FNG_PWM_PERIOD_DEFAULT;
    fPwmPulsewidth = TB6612FNG_PWM_PULSEWIDTH_DEFAULT;
    motorDriver.setPwmAperiod(fPwmPeriod);
    motorDriver.setPwmBperiod(fPwmPeriod);
    motorDriver.setPwmApulsewidth(fPwmPulsewidth);
    motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
  #ifdef DEBUG
    printf("DONE\n\r");
  #endif /* DEBUG */
#endif /* ENABLE_MOTOR_DRIVER */
    
    ROBot_STATUS = STATUS_OK;
#ifdef DEBUG
    printf("Initialization complete!\n\r");
#endif /* DEBUG */

    strcpy( wiflyBuffer, "ROBot awaiting commands\n\r");
    client->send_all(wiflyBuffer, strlen(wiflyBuffer));
}

/* Name: ROBot_sm()
 * Desc: ROBot state machine.  Processes all peripheral data and sets
 *       status accordingly
 * Inputs: none
 * Outputs: none
 */
void ROBot_sm(void)
{
    float range;
    
    if ( rangeFinder.getMeas(range) != RANGE_MEAS_INVALID )
    {
#ifdef DEBUG
        printf("range = %.3f m\n\r", range);
#endif /* DEBUG */
        if ( range < RANGE_COLLISION )
        {
            if ( ROBot_STATUS != STATUS_NEAR_COLLISION )
            {
                moveStop();
            }
            ROBot_STATUS = STATUS_NEAR_COLLISION;
            strcpy( wiflyBuffer, "\n\r**** COLLISION AVOIDANCE ****");
            sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
            client->send_all(wiflyBuffer, strlen(wiflyBuffer));
#ifdef DEBUG
            printf("%s", wiflyBuffer);
#endif /* DEBUG */
        }
        else if ( range < RANGE_WARNING )
        {
            strcpy( wiflyBuffer, "\n\r**** COLLISION WARNING ****");
            sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
            client->send_all(wiflyBuffer, strlen(wiflyBuffer));
#ifdef DEBUG
            printf("%s", wiflyBuffer);
#endif /* DEBUG */
            if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
            {
                ROBot_STATUS = STATUS_OK;
            }
        }
        else if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
        {
            ROBot_STATUS = STATUS_OK;
        }
    }

    // receive input and process it
    memset(wiflyBuffer, 0, sizeof(wiflyBuffer));
    int numCmds = client->receive(wiflyBuffer, sizeof(wiflyBuffer));
    etROBot_CMD *pCmd = (etROBot_CMD *) wiflyBuffer;
    while ( numCmds > 0 )
    {
        int cmdIdx = getCmdIdx(*pCmd);
        if ( cmdIdx == CMD_MOVE_UNKNOWN )
        {
            pCmd++;
            numCmds--;
            continue;
        }
        
        switch ( ROBot_STATUS )
        {
            case STATUS_OK:
                command[cmdIdx].cmd_func();
            break;
        
            case STATUS_NEAR_COLLISION:
                if ( *pCmd != CMD_MOVE_FORWARD )
                {
                    command[cmdIdx].cmd_func();
                }
            break;
        }
        pCmd++;
        numCmds--;
    }
}

/* Name: rangeFinderTimer_cb()
 * Desc: callback function for range finder timer.  Timer is used to
 *       initiate range finder measurements
 * Inputs: none
 * Outputs: none
 */
void rangeFinderTimer_cb(void)
{
    rangeFinder.startMeas();
}

/* Name: getCmdIdx()
 * Desc: returns command[] index of the input command
 * Inputs: none
 * Outputs: none
 */
int getCmdIdx(etROBot_CMD cmd)
{
    int i;
    
    for ( i = 0; i < NUM_CMDS; i++)
    {
        if ( cmd == command[i].cmd )
        {
            return i;
        }
    }
    return CMD_MOVE_UNKNOWN;
}

/* Name: moveStop()
 * Desc: ceases all motion
 * Inputs: none
 * Outputs: none
 */
void moveStop(void)
{
    motorDriver.standby();
}

/* Name: moveForward()
 * Desc: commands forward motion
 * Inputs: none
 * Outputs: none
 */
void moveForward(void)
{
    motorDriver.motorA_ccw();
    motorDriver.motorB_ccw();
}

/* Name: moveBackward()
 * Desc: commands backward motion
 * Inputs: none
 * Outputs: none
 */
void moveBackward(void)
{
    motorDriver.motorA_cw();
    motorDriver.motorB_cw();
}

/* Name: moveLeft()
 * Desc: commands left motion
 * Inputs: none
 * Outputs: none
 */
void moveLeft(void)
{
    motorDriver.motorA_ccw();
    motorDriver.motorB_cw();
}

/* Name: moveRight()
 * Desc: commands right motion
 * Inputs: none
 * Outputs: none
 */
void moveRight(void)
{
    motorDriver.motorA_cw();
    motorDriver.motorB_ccw();
}