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@4:228fd78e44fd, 2013-12-06 (annotated)

Committer:
rabad1
Date:
Fri Dec 06 08:18:16 2013 +0000
Revision:
4:228fd78e44fd
Parent:
3:6bf3b80a8cd9
Child:
5:c15ad8a7eb8b
added virtual COM port to allow wiFily to be configured by the user rather than hardcoding into the source code.

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 0:34bcb9bae5ea 68 #define RANGE_FINDER_MEAS_INTERVAL (0.5)
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 0:34bcb9bae5ea 73 #define RANGE_COLLISION (0.15f) // 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 3:6bf3b80a8cd9 242 server->bind(WIFI_ECHO_SERVER_PORT);
rabad1 3:6bf3b80a8cd9 243 server->listen();
rabad1 0:34bcb9bae5ea 244
rabad1 0:34bcb9bae5ea 245 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 246 printf("\n\rWait for remote connection...\n\r");
rabad1 0:34bcb9bae5ea 247 #endif /* DEBUG */
rabad1 3:6bf3b80a8cd9 248 server->accept(*client); // wait until remote host connects
rabad1 3:6bf3b80a8cd9 249 client->set_blocking(false);
rabad1 0:34bcb9bae5ea 250 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 251 printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 252 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 253 lcdScreen.locate(0,10);
rabad1 0:34bcb9bae5ea 254 lcdScreen.printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 255 #endif /* ENABLE_WIFI */
rabad1 0:34bcb9bae5ea 256
rabad1 0:34bcb9bae5ea 257 // start Range Finder
rabad1 0:34bcb9bae5ea 258 #ifdef ENABLE_RANGE_FINDER
rabad1 0:34bcb9bae5ea 259 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 260 printf("Initialize Range Finder...");
rabad1 0:34bcb9bae5ea 261 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 262 rangeFinderTimer.attach(rangeFinderTimer_cb, RANGE_FINDER_MEAS_INTERVAL);
rabad1 0:34bcb9bae5ea 263 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 264 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 265 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 266 #endif /* ENABLE_RANGE_FINDER */
rabad1 0:34bcb9bae5ea 267
rabad1 0:34bcb9bae5ea 268 // start Motor Driver
rabad1 0:34bcb9bae5ea 269 #ifdef ENABLE_MOTOR_DRIVER
rabad1 0:34bcb9bae5ea 270 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 271 printf("Initialize Motor Driver...");
rabad1 0:34bcb9bae5ea 272 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 273 fPwmPeriod = TB6612FNG_PWM_PERIOD_DEFAULT;
rabad1 0:34bcb9bae5ea 274 fPwmPulsewidth = TB6612FNG_PWM_PULSEWIDTH_DEFAULT;
rabad1 0:34bcb9bae5ea 275 motorDriver.setPwmAperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 276 motorDriver.setPwmBperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 277 motorDriver.setPwmApulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 278 motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 279 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 280 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 281 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 282 #endif /* ENABLE_MOTOR_DRIVER */
rabad1 0:34bcb9bae5ea 283
rabad1 0:34bcb9bae5ea 284 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 285 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 286 printf("Initialization complete!\n\r");
rabad1 0:34bcb9bae5ea 287 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 288
rabad1 0:34bcb9bae5ea 289 strcpy( wiflyBuffer, "ROBot awaiting commands\n\r");
rabad1 3:6bf3b80a8cd9 290 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 291 }
rabad1 0:34bcb9bae5ea 292
rabad1 0:34bcb9bae5ea 293 /* Name: ROBot_sm()
rabad1 0:34bcb9bae5ea 294 * Desc: ROBot state machine. Processes all peripheral data and sets
rabad1 0:34bcb9bae5ea 295 * status accordingly
rabad1 0:34bcb9bae5ea 296 * Inputs: none
rabad1 0:34bcb9bae5ea 297 * Outputs: none
rabad1 0:34bcb9bae5ea 298 */
rabad1 0:34bcb9bae5ea 299 void ROBot_sm(void)
rabad1 0:34bcb9bae5ea 300 {
rabad1 0:34bcb9bae5ea 301 float range;
rabad1 0:34bcb9bae5ea 302
rabad1 0:34bcb9bae5ea 303 if ( rangeFinder.getMeas(range) != RANGE_MEAS_INVALID )
rabad1 0:34bcb9bae5ea 304 {
rabad1 0:34bcb9bae5ea 305 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 306 printf("range = %.3f m\n\r", range);
rabad1 0:34bcb9bae5ea 307 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 308 if ( range < RANGE_COLLISION )
rabad1 0:34bcb9bae5ea 309 {
rabad1 0:34bcb9bae5ea 310 strcpy( wiflyBuffer, "\n\r**** COLLISION AVOIDANCE ****\n\r");
rabad1 3:6bf3b80a8cd9 311 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 312 if ( ROBot_STATUS != STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 313 {
rabad1 0:34bcb9bae5ea 314 moveStop();
rabad1 0:34bcb9bae5ea 315 }
rabad1 0:34bcb9bae5ea 316 ROBot_STATUS = STATUS_NEAR_COLLISION;
rabad1 0:34bcb9bae5ea 317 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 318 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 319 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 320 }
rabad1 0:34bcb9bae5ea 321 else if ( range < RANGE_WARNING )
rabad1 0:34bcb9bae5ea 322 {
rabad1 0:34bcb9bae5ea 323 strcpy( wiflyBuffer, "\n\r**** COLLISION WARNING ****");
rabad1 0:34bcb9bae5ea 324 sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
rabad1 3:6bf3b80a8cd9 325 client->send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 326 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 327 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 328 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 329 if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 330 {
rabad1 0:34bcb9bae5ea 331 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 332 }
rabad1 0:34bcb9bae5ea 333 }
rabad1 0:34bcb9bae5ea 334 else if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 335 {
rabad1 0:34bcb9bae5ea 336 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 337 }
rabad1 0:34bcb9bae5ea 338 }
rabad1 0:34bcb9bae5ea 339
rabad1 0:34bcb9bae5ea 340 // receive input and process it
rabad1 0:34bcb9bae5ea 341 memset(wiflyBuffer, 0, sizeof(wiflyBuffer));
rabad1 3:6bf3b80a8cd9 342 int numCmds = client->receive(wiflyBuffer, sizeof(wiflyBuffer));
rabad1 0:34bcb9bae5ea 343 etROBot_CMD *pCmd = (etROBot_CMD *) wiflyBuffer;
rabad1 0:34bcb9bae5ea 344 while ( numCmds > 0 )
rabad1 0:34bcb9bae5ea 345 {
rabad1 0:34bcb9bae5ea 346 int cmdIdx = getCmdIdx(*pCmd);
rabad1 0:34bcb9bae5ea 347 if ( cmdIdx == CMD_MOVE_UNKNOWN )
rabad1 0:34bcb9bae5ea 348 {
rabad1 0:34bcb9bae5ea 349 pCmd++;
rabad1 0:34bcb9bae5ea 350 numCmds--;
rabad1 0:34bcb9bae5ea 351 continue;
rabad1 0:34bcb9bae5ea 352 }
rabad1 0:34bcb9bae5ea 353
rabad1 0:34bcb9bae5ea 354 switch ( ROBot_STATUS )
rabad1 0:34bcb9bae5ea 355 {
rabad1 0:34bcb9bae5ea 356 case STATUS_OK:
rabad1 0:34bcb9bae5ea 357 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 358 break;
rabad1 0:34bcb9bae5ea 359
rabad1 0:34bcb9bae5ea 360 case STATUS_NEAR_COLLISION:
rabad1 0:34bcb9bae5ea 361 if ( *pCmd != CMD_MOVE_FORWARD )
rabad1 0:34bcb9bae5ea 362 {
rabad1 0:34bcb9bae5ea 363 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 364 }
rabad1 0:34bcb9bae5ea 365 break;
rabad1 0:34bcb9bae5ea 366 }
rabad1 0:34bcb9bae5ea 367 pCmd++;
rabad1 0:34bcb9bae5ea 368 numCmds--;
rabad1 0:34bcb9bae5ea 369 }
rabad1 0:34bcb9bae5ea 370 }
rabad1 0:34bcb9bae5ea 371
rabad1 0:34bcb9bae5ea 372 /* Name: rangeFinderTimer_cb()
rabad1 0:34bcb9bae5ea 373 * Desc: callback function for range finder timer. Timer is used to
rabad1 0:34bcb9bae5ea 374 * initiate range finder measurements
rabad1 0:34bcb9bae5ea 375 * Inputs: none
rabad1 0:34bcb9bae5ea 376 * Outputs: none
rabad1 0:34bcb9bae5ea 377 */
rabad1 0:34bcb9bae5ea 378 void rangeFinderTimer_cb(void)
rabad1 0:34bcb9bae5ea 379 {
rabad1 0:34bcb9bae5ea 380 rangeFinder.startMeas();
rabad1 0:34bcb9bae5ea 381 }
rabad1 0:34bcb9bae5ea 382
rabad1 0:34bcb9bae5ea 383 /* Name: getCmdIdx()
rabad1 0:34bcb9bae5ea 384 * Desc: returns command[] index of the input command
rabad1 0:34bcb9bae5ea 385 * Inputs: none
rabad1 0:34bcb9bae5ea 386 * Outputs: none
rabad1 0:34bcb9bae5ea 387 */
rabad1 0:34bcb9bae5ea 388 int getCmdIdx(etROBot_CMD cmd)
rabad1 0:34bcb9bae5ea 389 {
rabad1 0:34bcb9bae5ea 390 int i;
rabad1 0:34bcb9bae5ea 391
rabad1 0:34bcb9bae5ea 392 for ( i = 0; i < NUM_CMDS; i++)
rabad1 0:34bcb9bae5ea 393 {
rabad1 0:34bcb9bae5ea 394 if ( cmd == command[i].cmd )
rabad1 0:34bcb9bae5ea 395 {
rabad1 0:34bcb9bae5ea 396 return i;
rabad1 0:34bcb9bae5ea 397 }
rabad1 0:34bcb9bae5ea 398 }
rabad1 0:34bcb9bae5ea 399 return CMD_MOVE_UNKNOWN;
rabad1 0:34bcb9bae5ea 400 }
rabad1 0:34bcb9bae5ea 401
rabad1 1:e918d1aa9c26 402 /* Name: moveStop()
rabad1 1:e918d1aa9c26 403 * Desc: ceases all motion
rabad1 1:e918d1aa9c26 404 * Inputs: none
rabad1 1:e918d1aa9c26 405 * Outputs: none
rabad1 1:e918d1aa9c26 406 */
rabad1 0:34bcb9bae5ea 407 void moveStop(void)
rabad1 0:34bcb9bae5ea 408 {
rabad1 0:34bcb9bae5ea 409 motorDriver.standby();
rabad1 0:34bcb9bae5ea 410 }
rabad1 0:34bcb9bae5ea 411
rabad1 1:e918d1aa9c26 412 /* Name: moveForward()
rabad1 1:e918d1aa9c26 413 * Desc: commands forward motion
rabad1 1:e918d1aa9c26 414 * Inputs: none
rabad1 1:e918d1aa9c26 415 * Outputs: none
rabad1 1:e918d1aa9c26 416 */
rabad1 0:34bcb9bae5ea 417 void moveForward(void)
rabad1 0:34bcb9bae5ea 418 {
rabad1 0:34bcb9bae5ea 419 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 420 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 421 }
rabad1 0:34bcb9bae5ea 422
rabad1 1:e918d1aa9c26 423 /* Name: moveBackward()
rabad1 1:e918d1aa9c26 424 * Desc: commands backward motion
rabad1 1:e918d1aa9c26 425 * Inputs: none
rabad1 1:e918d1aa9c26 426 * Outputs: none
rabad1 1:e918d1aa9c26 427 */
rabad1 0:34bcb9bae5ea 428 void moveBackward(void)
rabad1 0:34bcb9bae5ea 429 {
rabad1 0:34bcb9bae5ea 430 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 431 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 432 }
rabad1 0:34bcb9bae5ea 433
rabad1 1:e918d1aa9c26 434 /* Name: moveLeft()
rabad1 1:e918d1aa9c26 435 * Desc: commands left motion
rabad1 1:e918d1aa9c26 436 * Inputs: none
rabad1 1:e918d1aa9c26 437 * Outputs: none
rabad1 1:e918d1aa9c26 438 */
rabad1 0:34bcb9bae5ea 439 void moveLeft(void)
rabad1 0:34bcb9bae5ea 440 {
rabad1 0:34bcb9bae5ea 441 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 442 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 443 }
rabad1 0:34bcb9bae5ea 444
rabad1 1:e918d1aa9c26 445 /* Name: moveRight()
rabad1 1:e918d1aa9c26 446 * Desc: commands right motion
rabad1 1:e918d1aa9c26 447 * Inputs: none
rabad1 1:e918d1aa9c26 448 * Outputs: none
rabad1 1:e918d1aa9c26 449 */
rabad1 0:34bcb9bae5ea 450 void moveRight(void)
rabad1 0:34bcb9bae5ea 451 {
rabad1 0:34bcb9bae5ea 452 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 453 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 454 }