Robert Abad / Mbed 2 deprecated ROBot

Project ROBot

Dependencies:   C12832_lcd HCSR04 TB6612FNG WiflyInterface mbed USBDevice

Welcome to Project ROBot!

ROBot is a robot based on an mbed platform. It is remotely controlled from a PC via a WiFi connection. The vehicle is also equipped with an ultrasonic range finder to detect any obstacles in the direct path of the ROBot and stop any forward motion if a collision is deemed imminent.

/media/uploads/rabad1/20131124_140455.jpg

Components

The major components of the ROBot are:

  • mbed application board
  • RN-XV WiFly wireless board (datasheet)
  • Toshiba TB6612FNG motor driver (datasheet)
  • HC-SR04 ultrasonic range finder (datasheet)
  • Redbot kit (just the chassis, motors, and wheels)
  • 5V power source
  • 6V power source

Schematic

Here is a schematic of the ROBot: schematic

Software Block Diagram

A state machine processes all input and sets the output /media/uploads/rabad1/robot_-_sw_block2.jpg

Standard Operation

A wireless remote connection from a PC sends commands /media/uploads/rabad1/robot_-_standard_ops.jpg List of commands:

  • 'e' - move forward
  • 'c' - move backward
  • 'f' - move right
  • 's' - move left
  • 'd' - stop

commands are case-sensitive

Collision Warning/Avoidance

The range finder ranges every 65 millisecs /media/uploads/rabad1/robot_-_collision.jpg

When an obstacle is within 50cm, a warning is sent back to the remote host (PC)

When an obstacle is within 30cm, all forward motion ceases. The remote host operator must use other commands to move the ROBot away from the obstacle.

Video

  • ROBot and its parts
  • Using a terminal emulator (Tera Term) to connect to the ROBot via a USB connection for wireless configuration. Once connected, the ROBot's IP address is shown in the terminal and on the LCD screen.
  • Creating a telnet session to remotely control the ROBot. Connect to the IP address given in the previous step.
  • Remote host operation of the ROBot
  • Collision Warning/Avoidance

Things To Do

I'm not sure when I'll get to these but here are some things I need/would like to do eventually:

  • improve the logic for collision
  • incorporate the accelerometer on the mbed application board
  • develop an Android/iPhone app for remote operation
  • add video for true remote operation

Acknowledgements

This project would not have been possible without the following people (in no particular order)... Andrew, Bob, Morgan, Avnish. Thank you for your support! <3

ROBot.cpp@5:c15ad8a7eb8b, 2013-12-07 (annotated)

Committer:
rabad1
Date:
Sat Dec 07 04:49:05 2013 +0000
Revision:
5:c15ad8a7eb8b
Parent:
4:228fd78e44fd 
updated ranging interval and collision threshold

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rabad1 0:34bcb9bae5ea 1 /* File: ROBot.cpp
rabad1 1:e918d1aa9c26 2 * Author: Robert Abad Copyright (c) 2013
rabad1 1:e918d1aa9c26 3 *
rabad1 0:34bcb9bae5ea 4 * Desc: ROBot application. Motorized robot with the following features:
rabad1 0:34bcb9bae5ea 5 * - WiFly interface for remote operation
rabad1 0:34bcb9bae5ea 6 * - ultrasonic range finder
rabad1 0:34bcb9bae5ea 7 * - motor driver for all locomotion
rabad1 2:d093f7687c85 8 *
rabad1 2:d093f7687c85 9 * The ROBot boots and connects to a known WiFi network. (NOTE:
rabad1 2:d093f7687c85 10 * Currently, the settings for the WiFi network must be hardcoded
rabad1 2:d093f7687c85 11 * into the firmware. So joining a different network will require
rabad1 2:d093f7687c85 12 * recompilation and reloading of the firmware). Upon successful
rabad1 2:d093f7687c85 13 * connection, ROBot's IP address is shown on the LCD screen. The
rabad1 2:d093f7687c85 14 * user then connects via PC telnet session. In the telnet console
rabad1 2:d093f7687c85 15 * window, the operator can enter the following commands:
rabad1 2:d093f7687c85 16 *
rabad1 2:d093f7687c85 17 * e - forward motion
rabad1 2:d093f7687c85 18 * c - backward motion
rabad1 2:d093f7687c85 19 * s - rotate left
rabad1 2:d093f7687c85 20 * f - rotate right
rabad1 2:d093f7687c85 21 * d - stop
rabad1 2:d093f7687c85 22 *
rabad1 2:d093f7687c85 23 * All commands are case-sensitive.
rabad1 2:d093f7687c85 24 *
rabad1 2:d093f7687c85 25 * ROBot is also equipped with an ultrasonic range finder. This
rabad1 2:d093f7687c85 26 * device is used to detect any obstacles in the direct forward
rabad1 2:d093f7687c85 27 * path. A warning will be sent to the telnet session when an
rabad1 2:d093f7687c85 28 * obstacle is within 50cm. If forward motion continues, a warning
rabad1 2:d093f7687c85 29 * will continue to be displayed. If the obstacle is within 15cm,
rabad1 2:d093f7687c85 30 * all forward motion is disabled. The operator will need to
rabad1 2:d093f7687c85 31 * use the other commands to avoid the obstacle.
rabad1 0:34bcb9bae5ea 32 */
rabad1 0:34bcb9bae5ea 33
rabad1 0:34bcb9bae5ea 34 #include "mbed.h"
rabad1 0:34bcb9bae5ea 35
rabad1 0:34bcb9bae5ea 36 #include "C12832_lcd.h"
rabad1 3:6bf3b80a8cd9 37 #include "WiflyInterface.h"
rabad1 3:6bf3b80a8cd9 38 #include "HCSR04.h"
rabad1 3:6bf3b80a8cd9 39 #include "TB6612FNG.h"
rabad1 3:6bf3b80a8cd9 40 #include "USBSerial.h"
rabad1 3:6bf3b80a8cd9 41
rabad1 0:34bcb9bae5ea 42 // uncomment to enable DEBUG
rabad1 0:34bcb9bae5ea 43 // enabling DEBUG does the following:
rabad1 3:6bf3b80a8cd9 44 // - various debug statements
rabad1 0:34bcb9bae5ea 45 // - show status of various state machines
rabad1 1:e918d1aa9c26 46 //#define DEBUG
rabad1 0:34bcb9bae5ea 47
rabad1 0:34bcb9bae5ea 48 // uncomment to enable (or comment to disable)
rabad1 0:34bcb9bae5ea 49 #define ENABLE_WIFI
rabad1 0:34bcb9bae5ea 50 #define ENABLE_RANGE_FINDER
rabad1 0:34bcb9bae5ea 51 #define ENABLE_MOTOR_DRIVER
rabad1 0:34bcb9bae5ea 52
rabad1 0:34bcb9bae5ea 53 // WiFi related
rabad1 0:34bcb9bae5ea 54 #define WIFLY_PIN_TX (p9)
rabad1 0:34bcb9bae5ea 55 #define WIFLY_PIN_RX (p10)
rabad1 0:34bcb9bae5ea 56 #define WIFLY_PIN_STATUS (p29)
rabad1 0:34bcb9bae5ea 57 #define WIFLY_PIN_RESET (p30)
rabad1 0:34bcb9bae5ea 58 #define WIFI_ECHO_SERVER_PORT (7)
rabad1 0:34bcb9bae5ea 59 #define WIFI_BUFFER_SIZE (64)
rabad1 3:6bf3b80a8cd9 60 WiflyInterface *wiFly = NULL;
rabad1 3:6bf3b80a8cd9 61 TCPSocketServer *server = NULL;
rabad1 3:6bf3b80a8cd9 62 TCPSocketConnection *client = NULL;
rabad1 0:34bcb9bae5ea 63 char wiflyBuffer[WIFI_BUFFER_SIZE];
rabad1 0:34bcb9bae5ea 64
rabad1 0:34bcb9bae5ea 65 // Range Finder
rabad1 0:34bcb9bae5ea 66 #define HCSR04_PIN_TRIGGER (p14)
rabad1 0:34bcb9bae5ea 67 #define HCSR04_PIN_ECHO (p15)
rabad1 5:c15ad8a7eb8b 68 #define RANGE_FINDER_MEAS_INTERVAL (0.065f)
rabad1 0:34bcb9bae5ea 69 HCSR04 rangeFinder( HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO );
rabad1 0:34bcb9bae5ea 70 void rangeFinderTimer_cb(void);
rabad1 0:34bcb9bae5ea 71 Ticker rangeFinderTimer;
rabad1 0:34bcb9bae5ea 72 #define RANGE_WARNING (0.5f) // meters
rabad1 5:c15ad8a7eb8b 73 #define RANGE_COLLISION (0.30f) // meters
rabad1 0:34bcb9bae5ea 74
rabad1 0:34bcb9bae5ea 75 // MotorDriver
rabad1 0:34bcb9bae5ea 76 #define TB6612FNG_PIN_PWMA (p22)
rabad1 0:34bcb9bae5ea 77 #define TB6612FNG_PIN_AIN1 (p17)
rabad1 0:34bcb9bae5ea 78 #define TB6612FNG_PIN_AIN2 (p16)
rabad1 0:34bcb9bae5ea 79 #define TB6612FNG_PIN_PWMB (p21)
rabad1 0:34bcb9bae5ea 80 #define TB6612FNG_PIN_BIN1 (p19)
rabad1 0:34bcb9bae5ea 81 #define TB6612FNG_PIN_BIN2 (p20)
rabad1 0:34bcb9bae5ea 82 #define TB6612FNG_PIN_NSTBY (p18)
rabad1 0:34bcb9bae5ea 83 TB6612FNG motorDriver( TB6612FNG_PIN_PWMA, TB6612FNG_PIN_AIN1, TB6612FNG_PIN_AIN2,
rabad1 0:34bcb9bae5ea 84 TB6612FNG_PIN_PWMB, TB6612FNG_PIN_BIN1, TB6612FNG_PIN_BIN2,
rabad1 0:34bcb9bae5ea 85 TB6612FNG_PIN_NSTBY );
rabad1 0:34bcb9bae5ea 86 float fPwmPeriod;
rabad1 0:34bcb9bae5ea 87 float fPwmPulsewidth;
rabad1 0:34bcb9bae5ea 88
rabad1 0:34bcb9bae5ea 89 // ROBot State Machine status
rabad1 0:34bcb9bae5ea 90 typedef enum
rabad1 0:34bcb9bae5ea 91 {
rabad1 0:34bcb9bae5ea 92 STATUS_OK,
rabad1 0:34bcb9bae5ea 93 STATUS_NEAR_COLLISION
rabad1 0:34bcb9bae5ea 94 } etROBot_STATUS;
rabad1 0:34bcb9bae5ea 95 etROBot_STATUS ROBot_STATUS;
rabad1 0:34bcb9bae5ea 96
rabad1 0:34bcb9bae5ea 97 // ROBot commands
rabad1 0:34bcb9bae5ea 98 typedef enum
rabad1 0:34bcb9bae5ea 99 {
rabad1 0:34bcb9bae5ea 100 CMD_MOVE_STOP = 'd',
rabad1 0:34bcb9bae5ea 101 CMD_MOVE_FORWARD = 'e',
rabad1 0:34bcb9bae5ea 102 CMD_MOVE_BACKWARD = 'c',
rabad1 0:34bcb9bae5ea 103 CMD_MOVE_LEFT = 's',
rabad1 0:34bcb9bae5ea 104 CMD_MOVE_RIGHT = 'f',
rabad1 0:34bcb9bae5ea 105 CMD_MOVE_UNKNOWN = -1
rabad1 0:34bcb9bae5ea 106 } etROBot_CMD;
rabad1 0:34bcb9bae5ea 107 #define NUM_CMDS (5)
rabad1 0:34bcb9bae5ea 108
rabad1 0:34bcb9bae5ea 109 void moveStop(void);
rabad1 0:34bcb9bae5ea 110 void moveForward(void);
rabad1 0:34bcb9bae5ea 111 void moveBackward(void);
rabad1 0:34bcb9bae5ea 112 void moveLeft(void);
rabad1 0:34bcb9bae5ea 113 void moveRight(void);
rabad1 0:34bcb9bae5ea 114
rabad1 0:34bcb9bae5ea 115 typedef struct
rabad1 0:34bcb9bae5ea 116 {
rabad1 0:34bcb9bae5ea 117 etROBot_CMD cmd;
rabad1 0:34bcb9bae5ea 118 void (*cmd_func)(void);
rabad1 0:34bcb9bae5ea 119 } etROBot_COMMAND;
rabad1 0:34bcb9bae5ea 120
rabad1 0:34bcb9bae5ea 121 const etROBot_COMMAND command[] =
rabad1 0:34bcb9bae5ea 122 {
rabad1 0:34bcb9bae5ea 123 { CMD_MOVE_STOP, moveStop },
rabad1 0:34bcb9bae5ea 124 { CMD_MOVE_FORWARD, moveForward },
rabad1 0:34bcb9bae5ea 125 { CMD_MOVE_BACKWARD, moveBackward },
rabad1 0:34bcb9bae5ea 126 { CMD_MOVE_LEFT, moveLeft },
rabad1 0:34bcb9bae5ea 127 { CMD_MOVE_RIGHT, moveRight }
rabad1 0:34bcb9bae5ea 128 };
rabad1 0:34bcb9bae5ea 129 int getCmdIdx(etROBot_CMD cmd);
rabad1 0:34bcb9bae5ea 130
rabad1 0:34bcb9bae5ea 131
rabad1 0:34bcb9bae5ea 132 C12832_LCD lcdScreen; //status display
rabad1 0:34bcb9bae5ea 133
rabad1 3:6bf3b80a8cd9 134 //Virtual serial port over USB
rabad1 3:6bf3b80a8cd9 135 USBSerial console;
rabad1 3:6bf3b80a8cd9 136 #define STR_BUFFER_SZ (64)
rabad1 3:6bf3b80a8cd9 137
rabad1 0:34bcb9bae5ea 138 // forward declarations
rabad1 3:6bf3b80a8cd9 139 void init(void); // intialize
rabad1 0:34bcb9bae5ea 140 void ROBot_sm(void); // ROBot state machine
rabad1 0:34bcb9bae5ea 141
rabad1 0:34bcb9bae5ea 142 /* Name: main()
rabad1 0:34bcb9bae5ea 143 * Desc: entry point and main thread of execution
rabad1 0:34bcb9bae5ea 144 * Inputs: none
rabad1 0:34bcb9bae5ea 145 * Outputs: none
rabad1 0:34bcb9bae5ea 146 */
rabad1 0:34bcb9bae5ea 147 int main(void)
rabad1 0:34bcb9bae5ea 148 {
rabad1 3:6bf3b80a8cd9 149 char wifi_ssid[STR_BUFFER_SZ];
rabad1 3:6bf3b80a8cd9 150 char wifi_passwd[STR_BUFFER_SZ];
rabad1 3:6bf3b80a8cd9 151 Security wifi_security;
rabad1 3:6bf3b80a8cd9 152
rabad1 3:6bf3b80a8cd9 153 #ifdef DEBUG
rabad1 3:6bf3b80a8cd9 154 printf("starting ROBot\n\r");
rabad1 3:6bf3b80a8cd9 155 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 156
rabad1 3:6bf3b80a8cd9 157 // establish console connection to get WiFi parameters
rabad1 3:6bf3b80a8cd9 158 while (1)
rabad1 3:6bf3b80a8cd9 159 {
rabad1 3:6bf3b80a8cd9 160 console.printf("Press <ENTER> to initialize ROBot\n\r");
rabad1 3:6bf3b80a8cd9 161 wait(1);
rabad1 3:6bf3b80a8cd9 162 if ( console.readable() )
rabad1 3:6bf3b80a8cd9 163 {
rabad1 3:6bf3b80a8cd9 164 break;
rabad1 3:6bf3b80a8cd9 165 }
rabad1 3:6bf3b80a8cd9 166 }
rabad1 3:6bf3b80a8cd9 167 console.printf("\n\rEnter SSID\n\r");
rabad1 3:6bf3b80a8cd9 168 console.scanf("%s", wifi_ssid);
rabad1 3:6bf3b80a8cd9 169 console.printf("\n\rEnter password (enter 0 if no password is required)\n\r");
rabad1 3:6bf3b80a8cd9 170 console.scanf("%s", wifi_passwd);
rabad1 3:6bf3b80a8cd9 171 console.printf("\n\rEnter security type (0=NONE,1=WEP_128,3=WPA)\n\r");
rabad1 3:6bf3b80a8cd9 172 console.scanf("%d", (Security *)&wifi_security);
rabad1 3:6bf3b80a8cd9 173 #ifdef DEBUG
rabad1 3:6bf3b80a8cd9 174 printf("SSID = %s\n\r", wifi_ssid);
rabad1 3:6bf3b80a8cd9 175 printf("passwd = %s\n\r", wifi_passwd);
rabad1 3:6bf3b80a8cd9 176 printf("security type = %d\n\r", wifi_security);
rabad1 3:6bf3b80a8cd9 177 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 178 wiFly = new WiflyInterface(WIFLY_PIN_TX,
rabad1 3:6bf3b80a8cd9 179 WIFLY_PIN_RX,
rabad1 3:6bf3b80a8cd9 180 WIFLY_PIN_RESET,
rabad1 3:6bf3b80a8cd9 181 WIFLY_PIN_STATUS,
rabad1 3:6bf3b80a8cd9 182 wifi_ssid,
rabad1 3:6bf3b80a8cd9 183 ((wifi_passwd[0]=='0')&&(wifi_passwd[1]=='\0')) ? NULL : wifi_passwd,
rabad1 3:6bf3b80a8cd9 184 wifi_security);;
rabad1 3:6bf3b80a8cd9 185 server = new TCPSocketServer;
rabad1 3:6bf3b80a8cd9 186 client = new TCPSocketConnection;
rabad1 3:6bf3b80a8cd9 187
rabad1 3:6bf3b80a8cd9 188 while ( !wiFly || !server || !client )
rabad1 3:6bf3b80a8cd9 189 {
rabad1 3:6bf3b80a8cd9 190 #ifdef DEBUG
rabad1 3:6bf3b80a8cd9 191 printf("ERROR: unable to allocate memory\n\r");
rabad1 3:6bf3b80a8cd9 192 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 193 }
rabad1 3:6bf3b80a8cd9 194
rabad1 3:6bf3b80a8cd9 195 // initilize the app
rabad1 0:34bcb9bae5ea 196 init();
rabad1 0:34bcb9bae5ea 197
rabad1 0:34bcb9bae5ea 198 while (1)
rabad1 0:34bcb9bae5ea 199 {
rabad1 0:34bcb9bae5ea 200 ROBot_sm();
rabad1 0:34bcb9bae5ea 201 }
rabad1 4:228fd78e44fd 202
rabad1 4:228fd78e44fd 203 // the following are commented yield a compilation warning of unreachable code.
rabad1 4:228fd78e44fd 204 // I acknowledge that this code is indeed unreachable but is placed here for the
rabad1 4:228fd78e44fd 205 // sake of completeness. Normally, there would be a memory leak if wiFly, server,
rabad1 4:228fd78e44fd 206 // and client are dynamically allocated and not deallocated appropriately. However,
rabad1 4:228fd78e44fd 207 // since shutdown of this app will require either a reset or shutdown of power, memory
rabad1 4:228fd78e44fd 208 // will get reset avoiding any memory leak.
rabad1 4:228fd78e44fd 209 delete client;
rabad1 4:228fd78e44fd 210 delete server;
rabad1 4:228fd78e44fd 211 delete wiFly;
rabad1 0:34bcb9bae5ea 212 }
rabad1 0:34bcb9bae5ea 213
rabad1 0:34bcb9bae5ea 214 /* Name: init()
rabad1 0:34bcb9bae5ea 215 * Desc: initialize the ROBot
rabad1 0:34bcb9bae5ea 216 * Inputs: none
rabad1 0:34bcb9bae5ea 217 * Outputs: none
rabad1 0:34bcb9bae5ea 218 */
rabad1 0:34bcb9bae5ea 219 void init(void)
rabad1 0:34bcb9bae5ea 220 {
rabad1 0:34bcb9bae5ea 221 // intialize peripherals
rabad1 0:34bcb9bae5ea 222 lcdScreen.cls();
rabad1 0:34bcb9bae5ea 223
rabad1 0:34bcb9bae5ea 224 // connect WiFi
rabad1 0:34bcb9bae5ea 225 #ifdef ENABLE_WIFI
rabad1 0:34bcb9bae5ea 226 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 227 printf("Connecting to WiFi...");
rabad1 0:34bcb9bae5ea 228 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 229 wiFly->init(); // use DHCP
rabad1 0:34bcb9bae5ea 230 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 231 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 232 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 233 while (!wiFly->connect()) // join the network
rabad1 0:34bcb9bae5ea 234 {
rabad1 0:34bcb9bae5ea 235 printf("attempting to connect\n\r");
rabad1 0:34bcb9bae5ea 236 }
rabad1 0:34bcb9bae5ea 237 #ifdef DEBUG
rabad1 3:6bf3b80a8cd9 238 printf("IP Address is %s\n\r", wiFly->getIPAddress());
rabad1 0:34bcb9bae5ea 239 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 240 lcdScreen.locate(0,0);
rabad1 3:6bf3b80a8cd9 241 lcdScreen.printf("IP: %s", wiFly->getIPAddress());
rabad1 5:c15ad8a7eb8b 242 console.printf("\n\rIP: %s\n\r", wiFly->getIPAddress());
rabad1 3:6bf3b80a8cd9 243 server->bind(WIFI_ECHO_SERVER_PORT);
rabad1 3:6bf3b80a8cd9 244 server->listen();
rabad1 0:34bcb9bae5ea 245
rabad1 0:34bcb9bae5ea 246 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 247 printf("\n\rWait for remote connection...\n\r");
rabad1 0:34bcb9bae5ea 248 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 249 server->accept(*client); // wait until remote host connects
rabad1 3:6bf3b80a8cd9 250 client->set_blocking(false);
rabad1 0:34bcb9bae5ea 251 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 252 printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 253 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 254 lcdScreen.locate(0,10);
rabad1 0:34bcb9bae5ea 255 lcdScreen.printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 256 #endif /* ENABLE_WIFI */
rabad1 0:34bcb9bae5ea 257
rabad1 0:34bcb9bae5ea 258 // start Range Finder
rabad1 0:34bcb9bae5ea 259 #ifdef ENABLE_RANGE_FINDER
rabad1 0:34bcb9bae5ea 260 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 261 printf("Initialize Range Finder...");
rabad1 0:34bcb9bae5ea 262 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 263 rangeFinderTimer.attach(rangeFinderTimer_cb, RANGE_FINDER_MEAS_INTERVAL);
rabad1 0:34bcb9bae5ea 264 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 265 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 266 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 267 #endif /* ENABLE_RANGE_FINDER */
rabad1 0:34bcb9bae5ea 268
rabad1 0:34bcb9bae5ea 269 // start Motor Driver
rabad1 0:34bcb9bae5ea 270 #ifdef ENABLE_MOTOR_DRIVER
rabad1 0:34bcb9bae5ea 271 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 272 printf("Initialize Motor Driver...");
rabad1 0:34bcb9bae5ea 273 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 274 fPwmPeriod = TB6612FNG_PWM_PERIOD_DEFAULT;
rabad1 0:34bcb9bae5ea 275 fPwmPulsewidth = TB6612FNG_PWM_PULSEWIDTH_DEFAULT;
rabad1 0:34bcb9bae5ea 276 motorDriver.setPwmAperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 277 motorDriver.setPwmBperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 278 motorDriver.setPwmApulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 279 motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 280 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 281 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 282 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 283 #endif /* ENABLE_MOTOR_DRIVER */
rabad1 0:34bcb9bae5ea 284
rabad1 0:34bcb9bae5ea 285 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 286 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 287 printf("Initialization complete!\n\r");
rabad1 0:34bcb9bae5ea 288 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 289
rabad1 0:34bcb9bae5ea 290 strcpy( wiflyBuffer, "ROBot awaiting commands\n\r");
rabad1 3:6bf3b80a8cd9 291 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 292 }
rabad1 0:34bcb9bae5ea 293
rabad1 0:34bcb9bae5ea 294 /* Name: ROBot_sm()
rabad1 0:34bcb9bae5ea 295 * Desc: ROBot state machine. Processes all peripheral data and sets
rabad1 0:34bcb9bae5ea 296 * status accordingly
rabad1 0:34bcb9bae5ea 297 * Inputs: none
rabad1 0:34bcb9bae5ea 298 * Outputs: none
rabad1 0:34bcb9bae5ea 299 */
rabad1 0:34bcb9bae5ea 300 void ROBot_sm(void)
rabad1 0:34bcb9bae5ea 301 {
rabad1 0:34bcb9bae5ea 302 float range;
rabad1 0:34bcb9bae5ea 303
rabad1 0:34bcb9bae5ea 304 if ( rangeFinder.getMeas(range) != RANGE_MEAS_INVALID )
rabad1 0:34bcb9bae5ea 305 {
rabad1 0:34bcb9bae5ea 306 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 307 printf("range = %.3f m\n\r", range);
rabad1 0:34bcb9bae5ea 308 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 309 if ( range < RANGE_COLLISION )
rabad1 0:34bcb9bae5ea 310 {
rabad1 0:34bcb9bae5ea 311 if ( ROBot_STATUS != STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 312 {
rabad1 0:34bcb9bae5ea 313 moveStop();
rabad1 0:34bcb9bae5ea 314 }
rabad1 0:34bcb9bae5ea 315 ROBot_STATUS = STATUS_NEAR_COLLISION;
rabad1 5:c15ad8a7eb8b 316 strcpy( wiflyBuffer, "\n\r**** COLLISION AVOIDANCE ****");
rabad1 5:c15ad8a7eb8b 317 sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
rabad1 5:c15ad8a7eb8b 318 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 319 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 320 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 321 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 322 }
rabad1 0:34bcb9bae5ea 323 else if ( range < RANGE_WARNING )
rabad1 0:34bcb9bae5ea 324 {
rabad1 0:34bcb9bae5ea 325 strcpy( wiflyBuffer, "\n\r**** COLLISION WARNING ****");
rabad1 0:34bcb9bae5ea 326 sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
rabad1 3:6bf3b80a8cd9 327 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 328 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 329 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 330 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 331 if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 332 {
rabad1 0:34bcb9bae5ea 333 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 334 }
rabad1 0:34bcb9bae5ea 335 }
rabad1 0:34bcb9bae5ea 336 else if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 337 {
rabad1 0:34bcb9bae5ea 338 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 339 }
rabad1 0:34bcb9bae5ea 340 }
rabad1 0:34bcb9bae5ea 341
rabad1 0:34bcb9bae5ea 342 // receive input and process it
rabad1 0:34bcb9bae5ea 343 memset(wiflyBuffer, 0, sizeof(wiflyBuffer));
rabad1 3:6bf3b80a8cd9 344 int numCmds = client->receive(wiflyBuffer, sizeof(wiflyBuffer));
rabad1 0:34bcb9bae5ea 345 etROBot_CMD *pCmd = (etROBot_CMD *) wiflyBuffer;
rabad1 0:34bcb9bae5ea 346 while ( numCmds > 0 )
rabad1 0:34bcb9bae5ea 347 {
rabad1 0:34bcb9bae5ea 348 int cmdIdx = getCmdIdx(*pCmd);
rabad1 0:34bcb9bae5ea 349 if ( cmdIdx == CMD_MOVE_UNKNOWN )
rabad1 0:34bcb9bae5ea 350 {
rabad1 0:34bcb9bae5ea 351 pCmd++;
rabad1 0:34bcb9bae5ea 352 numCmds--;
rabad1 0:34bcb9bae5ea 353 continue;
rabad1 0:34bcb9bae5ea 354 }
rabad1 0:34bcb9bae5ea 355
rabad1 0:34bcb9bae5ea 356 switch ( ROBot_STATUS )
rabad1 0:34bcb9bae5ea 357 {
rabad1 0:34bcb9bae5ea 358 case STATUS_OK:
rabad1 0:34bcb9bae5ea 359 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 360 break;
rabad1 0:34bcb9bae5ea 361
rabad1 0:34bcb9bae5ea 362 case STATUS_NEAR_COLLISION:
rabad1 0:34bcb9bae5ea 363 if ( *pCmd != CMD_MOVE_FORWARD )
rabad1 0:34bcb9bae5ea 364 {
rabad1 0:34bcb9bae5ea 365 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 366 }
rabad1 0:34bcb9bae5ea 367 break;
rabad1 0:34bcb9bae5ea 368 }
rabad1 0:34bcb9bae5ea 369 pCmd++;
rabad1 0:34bcb9bae5ea 370 numCmds--;
rabad1 0:34bcb9bae5ea 371 }
rabad1 0:34bcb9bae5ea 372 }
rabad1 0:34bcb9bae5ea 373
rabad1 0:34bcb9bae5ea 374 /* Name: rangeFinderTimer_cb()
rabad1 0:34bcb9bae5ea 375 * Desc: callback function for range finder timer. Timer is used to
rabad1 0:34bcb9bae5ea 376 * initiate range finder measurements
rabad1 0:34bcb9bae5ea 377 * Inputs: none
rabad1 0:34bcb9bae5ea 378 * Outputs: none
rabad1 0:34bcb9bae5ea 379 */
rabad1 0:34bcb9bae5ea 380 void rangeFinderTimer_cb(void)
rabad1 0:34bcb9bae5ea 381 {
rabad1 0:34bcb9bae5ea 382 rangeFinder.startMeas();
rabad1 0:34bcb9bae5ea 383 }
rabad1 0:34bcb9bae5ea 384
rabad1 0:34bcb9bae5ea 385 /* Name: getCmdIdx()
rabad1 0:34bcb9bae5ea 386 * Desc: returns command[] index of the input command
rabad1 0:34bcb9bae5ea 387 * Inputs: none
rabad1 0:34bcb9bae5ea 388 * Outputs: none
rabad1 0:34bcb9bae5ea 389 */
rabad1 0:34bcb9bae5ea 390 int getCmdIdx(etROBot_CMD cmd)
rabad1 0:34bcb9bae5ea 391 {
rabad1 0:34bcb9bae5ea 392 int i;
rabad1 0:34bcb9bae5ea 393
rabad1 0:34bcb9bae5ea 394 for ( i = 0; i < NUM_CMDS; i++)
rabad1 0:34bcb9bae5ea 395 {
rabad1 0:34bcb9bae5ea 396 if ( cmd == command[i].cmd )
rabad1 0:34bcb9bae5ea 397 {
rabad1 0:34bcb9bae5ea 398 return i;
rabad1 0:34bcb9bae5ea 399 }
rabad1 0:34bcb9bae5ea 400 }
rabad1 0:34bcb9bae5ea 401 return CMD_MOVE_UNKNOWN;
rabad1 0:34bcb9bae5ea 402 }
rabad1 0:34bcb9bae5ea 403
rabad1 1:e918d1aa9c26 404 /* Name: moveStop()
rabad1 1:e918d1aa9c26 405 * Desc: ceases all motion
rabad1 1:e918d1aa9c26 406 * Inputs: none
rabad1 1:e918d1aa9c26 407 * Outputs: none
rabad1 1:e918d1aa9c26 408 */
rabad1 0:34bcb9bae5ea 409 void moveStop(void)
rabad1 0:34bcb9bae5ea 410 {
rabad1 0:34bcb9bae5ea 411 motorDriver.standby();
rabad1 0:34bcb9bae5ea 412 }
rabad1 0:34bcb9bae5ea 413
rabad1 1:e918d1aa9c26 414 /* Name: moveForward()
rabad1 1:e918d1aa9c26 415 * Desc: commands forward motion
rabad1 1:e918d1aa9c26 416 * Inputs: none
rabad1 1:e918d1aa9c26 417 * Outputs: none
rabad1 1:e918d1aa9c26 418 */
rabad1 0:34bcb9bae5ea 419 void moveForward(void)
rabad1 0:34bcb9bae5ea 420 {
rabad1 0:34bcb9bae5ea 421 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 422 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 423 }
rabad1 0:34bcb9bae5ea 424
rabad1 1:e918d1aa9c26 425 /* Name: moveBackward()
rabad1 1:e918d1aa9c26 426 * Desc: commands backward motion
rabad1 1:e918d1aa9c26 427 * Inputs: none
rabad1 1:e918d1aa9c26 428 * Outputs: none
rabad1 1:e918d1aa9c26 429 */
rabad1 0:34bcb9bae5ea 430 void moveBackward(void)
rabad1 0:34bcb9bae5ea 431 {
rabad1 0:34bcb9bae5ea 432 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 433 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 434 }
rabad1 0:34bcb9bae5ea 435
rabad1 1:e918d1aa9c26 436 /* Name: moveLeft()
rabad1 1:e918d1aa9c26 437 * Desc: commands left motion
rabad1 1:e918d1aa9c26 438 * Inputs: none
rabad1 1:e918d1aa9c26 439 * Outputs: none
rabad1 1:e918d1aa9c26 440 */
rabad1 0:34bcb9bae5ea 441 void moveLeft(void)
rabad1 0:34bcb9bae5ea 442 {
rabad1 0:34bcb9bae5ea 443 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 444 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 445 }
rabad1 0:34bcb9bae5ea 446
rabad1 1:e918d1aa9c26 447 /* Name: moveRight()
rabad1 1:e918d1aa9c26 448 * Desc: commands right motion
rabad1 1:e918d1aa9c26 449 * Inputs: none
rabad1 1:e918d1aa9c26 450 * Outputs: none
rabad1 1:e918d1aa9c26 451 */
rabad1 0:34bcb9bae5ea 452 void moveRight(void)
rabad1 0:34bcb9bae5ea 453 {
rabad1 0:34bcb9bae5ea 454 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 455 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 456 }