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@2:d093f7687c85, 2013-11-24 (annotated)

Committer:
rabad1
Date:
Sun Nov 24 23:20:32 2013 +0000
Revision:
2:d093f7687c85
Parent:
1:e918d1aa9c26
Child:
3:6bf3b80a8cd9
updated comments in ROBot.cpp

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 0:34bcb9bae5ea 37
rabad1 0:34bcb9bae5ea 38 // uncomment to enable DEBUG
rabad1 0:34bcb9bae5ea 39 // enabling DEBUG does the following:
rabad1 0:34bcb9bae5ea 40 // - show status of various state machines
rabad1 1:e918d1aa9c26 41 //#define DEBUG
rabad1 0:34bcb9bae5ea 42
rabad1 0:34bcb9bae5ea 43 // uncomment to enable (or comment to disable)
rabad1 0:34bcb9bae5ea 44 #define ENABLE_WIFI
rabad1 0:34bcb9bae5ea 45 #define ENABLE_RANGE_FINDER
rabad1 0:34bcb9bae5ea 46 #define ENABLE_MOTOR_DRIVER
rabad1 0:34bcb9bae5ea 47
rabad1 0:34bcb9bae5ea 48 #include "WiflyInterface.h"
rabad1 0:34bcb9bae5ea 49 #include "HCSR04.h"
rabad1 0:34bcb9bae5ea 50 #include "TB6612FNG.h"
rabad1 0:34bcb9bae5ea 51
rabad1 0:34bcb9bae5ea 52 // WiFi related
rabad1 0:34bcb9bae5ea 53 #define WIFLY_PIN_TX (p9)
rabad1 0:34bcb9bae5ea 54 #define WIFLY_PIN_RX (p10)
rabad1 0:34bcb9bae5ea 55 #define WIFLY_PIN_STATUS (p29)
rabad1 0:34bcb9bae5ea 56 #define WIFLY_PIN_RESET (p30)
rabad1 0:34bcb9bae5ea 57 #define WIFI_ECHO_SERVER_PORT (7)
rabad1 0:34bcb9bae5ea 58 #define WIFI_BUFFER_SIZE (64)
rabad1 0:34bcb9bae5ea 59 //WiflyInterface wifly(WIFLY_PIN_TX, WIFLY_PIN_RX, WIFLY_PIN_RESET, WIFLY_PIN_STATUS, "purpleMonkey", NULL, NONE);
rabad1 0:34bcb9bae5ea 60 WiflyInterface wifly(WIFLY_PIN_TX, WIFLY_PIN_RX, WIFLY_PIN_RESET, WIFLY_PIN_STATUS, "FishNet", "ffeeddccbbaa99887766554433", WEP_128);
rabad1 0:34bcb9bae5ea 61 //WiflyInterface wifly(WIFLY_PIN_TX, WIFLY_PIN_RX, WIFLY_PIN_RESET, WIFLY_PIN_STATUS, "Student", NULL, NONE);
rabad1 0:34bcb9bae5ea 62 TCPSocketServer server;
rabad1 0:34bcb9bae5ea 63 TCPSocketConnection client;
rabad1 0:34bcb9bae5ea 64 char wiflyBuffer[WIFI_BUFFER_SIZE];
rabad1 0:34bcb9bae5ea 65
rabad1 0:34bcb9bae5ea 66 // Range Finder
rabad1 0:34bcb9bae5ea 67 #define HCSR04_PIN_TRIGGER (p14)
rabad1 0:34bcb9bae5ea 68 #define HCSR04_PIN_ECHO (p15)
rabad1 0:34bcb9bae5ea 69 #define RANGE_FINDER_MEAS_INTERVAL (0.5)
rabad1 0:34bcb9bae5ea 70 HCSR04 rangeFinder( HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO );
rabad1 0:34bcb9bae5ea 71 void rangeFinderTimer_cb(void);
rabad1 0:34bcb9bae5ea 72 Ticker rangeFinderTimer;
rabad1 0:34bcb9bae5ea 73 #define RANGE_WARNING (0.5f) // meters
rabad1 0:34bcb9bae5ea 74 #define RANGE_COLLISION (0.15f) // meters
rabad1 0:34bcb9bae5ea 75
rabad1 0:34bcb9bae5ea 76 // MotorDriver
rabad1 0:34bcb9bae5ea 77 #define TB6612FNG_PIN_PWMA (p22)
rabad1 0:34bcb9bae5ea 78 #define TB6612FNG_PIN_AIN1 (p17)
rabad1 0:34bcb9bae5ea 79 #define TB6612FNG_PIN_AIN2 (p16)
rabad1 0:34bcb9bae5ea 80 #define TB6612FNG_PIN_PWMB (p21)
rabad1 0:34bcb9bae5ea 81 #define TB6612FNG_PIN_BIN1 (p19)
rabad1 0:34bcb9bae5ea 82 #define TB6612FNG_PIN_BIN2 (p20)
rabad1 0:34bcb9bae5ea 83 #define TB6612FNG_PIN_NSTBY (p18)
rabad1 0:34bcb9bae5ea 84 TB6612FNG motorDriver( TB6612FNG_PIN_PWMA, TB6612FNG_PIN_AIN1, TB6612FNG_PIN_AIN2,
rabad1 0:34bcb9bae5ea 85 TB6612FNG_PIN_PWMB, TB6612FNG_PIN_BIN1, TB6612FNG_PIN_BIN2,
rabad1 0:34bcb9bae5ea 86 TB6612FNG_PIN_NSTBY );
rabad1 0:34bcb9bae5ea 87 float fPwmPeriod;
rabad1 0:34bcb9bae5ea 88 float fPwmPulsewidth;
rabad1 0:34bcb9bae5ea 89
rabad1 0:34bcb9bae5ea 90 // ROBot State Machine status
rabad1 0:34bcb9bae5ea 91 typedef enum
rabad1 0:34bcb9bae5ea 92 {
rabad1 0:34bcb9bae5ea 93 STATUS_OK,
rabad1 0:34bcb9bae5ea 94 STATUS_NEAR_COLLISION
rabad1 0:34bcb9bae5ea 95 } etROBot_STATUS;
rabad1 0:34bcb9bae5ea 96 etROBot_STATUS ROBot_STATUS;
rabad1 0:34bcb9bae5ea 97
rabad1 0:34bcb9bae5ea 98 // ROBot commands
rabad1 0:34bcb9bae5ea 99 typedef enum
rabad1 0:34bcb9bae5ea 100 {
rabad1 0:34bcb9bae5ea 101 CMD_MOVE_STOP = 'd',
rabad1 0:34bcb9bae5ea 102 CMD_MOVE_FORWARD = 'e',
rabad1 0:34bcb9bae5ea 103 CMD_MOVE_BACKWARD = 'c',
rabad1 0:34bcb9bae5ea 104 CMD_MOVE_LEFT = 's',
rabad1 0:34bcb9bae5ea 105 CMD_MOVE_RIGHT = 'f',
rabad1 0:34bcb9bae5ea 106 CMD_MOVE_UNKNOWN = -1
rabad1 0:34bcb9bae5ea 107 } etROBot_CMD;
rabad1 0:34bcb9bae5ea 108 #define NUM_CMDS (5)
rabad1 0:34bcb9bae5ea 109
rabad1 0:34bcb9bae5ea 110 void moveStop(void);
rabad1 0:34bcb9bae5ea 111 void moveForward(void);
rabad1 0:34bcb9bae5ea 112 void moveBackward(void);
rabad1 0:34bcb9bae5ea 113 void moveLeft(void);
rabad1 0:34bcb9bae5ea 114 void moveRight(void);
rabad1 0:34bcb9bae5ea 115
rabad1 0:34bcb9bae5ea 116 typedef struct
rabad1 0:34bcb9bae5ea 117 {
rabad1 0:34bcb9bae5ea 118 etROBot_CMD cmd;
rabad1 0:34bcb9bae5ea 119 void (*cmd_func)(void);
rabad1 0:34bcb9bae5ea 120 } etROBot_COMMAND;
rabad1 0:34bcb9bae5ea 121
rabad1 0:34bcb9bae5ea 122 const etROBot_COMMAND command[] =
rabad1 0:34bcb9bae5ea 123 {
rabad1 0:34bcb9bae5ea 124 { CMD_MOVE_STOP, moveStop },
rabad1 0:34bcb9bae5ea 125 { CMD_MOVE_FORWARD, moveForward },
rabad1 0:34bcb9bae5ea 126 { CMD_MOVE_BACKWARD, moveBackward },
rabad1 0:34bcb9bae5ea 127 { CMD_MOVE_LEFT, moveLeft },
rabad1 0:34bcb9bae5ea 128 { CMD_MOVE_RIGHT, moveRight }
rabad1 0:34bcb9bae5ea 129 };
rabad1 0:34bcb9bae5ea 130 int getCmdIdx(etROBot_CMD cmd);
rabad1 0:34bcb9bae5ea 131
rabad1 0:34bcb9bae5ea 132
rabad1 0:34bcb9bae5ea 133 C12832_LCD lcdScreen; //status display
rabad1 0:34bcb9bae5ea 134
rabad1 0:34bcb9bae5ea 135 // forward declarations
rabad1 0:34bcb9bae5ea 136 void init(void); // intializie
rabad1 0:34bcb9bae5ea 137 void ROBot_sm(void); // ROBot state machine
rabad1 0:34bcb9bae5ea 138
rabad1 0:34bcb9bae5ea 139 /* Name: main()
rabad1 0:34bcb9bae5ea 140 * Desc: entry point and main thread of execution
rabad1 0:34bcb9bae5ea 141 * Inputs: none
rabad1 0:34bcb9bae5ea 142 * Outputs: none
rabad1 0:34bcb9bae5ea 143 */
rabad1 0:34bcb9bae5ea 144 int main(void)
rabad1 0:34bcb9bae5ea 145 {
rabad1 0:34bcb9bae5ea 146 init();
rabad1 0:34bcb9bae5ea 147
rabad1 0:34bcb9bae5ea 148 while (1)
rabad1 0:34bcb9bae5ea 149 {
rabad1 0:34bcb9bae5ea 150 ROBot_sm();
rabad1 0:34bcb9bae5ea 151 }
rabad1 0:34bcb9bae5ea 152 }
rabad1 0:34bcb9bae5ea 153
rabad1 0:34bcb9bae5ea 154 /* Name: init()
rabad1 0:34bcb9bae5ea 155 * Desc: initialize the ROBot
rabad1 0:34bcb9bae5ea 156 * Inputs: none
rabad1 0:34bcb9bae5ea 157 * Outputs: none
rabad1 0:34bcb9bae5ea 158 */
rabad1 0:34bcb9bae5ea 159 void init(void)
rabad1 0:34bcb9bae5ea 160 {
rabad1 0:34bcb9bae5ea 161 // intialize peripherals
rabad1 0:34bcb9bae5ea 162 lcdScreen.cls();
rabad1 0:34bcb9bae5ea 163
rabad1 0:34bcb9bae5ea 164 // connect WiFi
rabad1 0:34bcb9bae5ea 165 #ifdef ENABLE_WIFI
rabad1 0:34bcb9bae5ea 166 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 167 printf("Connecting to WiFi...");
rabad1 0:34bcb9bae5ea 168 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 169 wifly.init(); // use DHCP
rabad1 0:34bcb9bae5ea 170 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 171 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 172 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 173 while (!wifly.connect()) // join the network
rabad1 0:34bcb9bae5ea 174 {
rabad1 0:34bcb9bae5ea 175 printf("attempting to connect\n\r");
rabad1 0:34bcb9bae5ea 176 }
rabad1 0:34bcb9bae5ea 177 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 178 printf("IP Address is %s\n\r", wifly.getIPAddress());
rabad1 0:34bcb9bae5ea 179 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 180 lcdScreen.locate(0,0);
rabad1 0:34bcb9bae5ea 181 lcdScreen.printf("IP: %s", wifly.getIPAddress());
rabad1 0:34bcb9bae5ea 182 server.bind(WIFI_ECHO_SERVER_PORT);
rabad1 0:34bcb9bae5ea 183 server.listen();
rabad1 0:34bcb9bae5ea 184
rabad1 0:34bcb9bae5ea 185 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 186 printf("\n\rWait for remote connection...\n\r");
rabad1 0:34bcb9bae5ea 187 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 188 server.accept(client); // wait until remote host connects
rabad1 0:34bcb9bae5ea 189 client.set_blocking(false);
rabad1 0:34bcb9bae5ea 190 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 191 printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 192 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 193 lcdScreen.locate(0,10);
rabad1 0:34bcb9bae5ea 194 lcdScreen.printf("remote host connected!\n\r");
rabad1 0:34bcb9bae5ea 195 #endif /* ENABLE_WIFI */
rabad1 0:34bcb9bae5ea 196
rabad1 0:34bcb9bae5ea 197 // start Range Finder
rabad1 0:34bcb9bae5ea 198 #ifdef ENABLE_RANGE_FINDER
rabad1 0:34bcb9bae5ea 199 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 200 printf("Initialize Range Finder...");
rabad1 0:34bcb9bae5ea 201 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 202 rangeFinderTimer.attach(rangeFinderTimer_cb, RANGE_FINDER_MEAS_INTERVAL);
rabad1 0:34bcb9bae5ea 203 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 204 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 205 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 206 #endif /* ENABLE_RANGE_FINDER */
rabad1 0:34bcb9bae5ea 207
rabad1 0:34bcb9bae5ea 208 // start Motor Driver
rabad1 0:34bcb9bae5ea 209 #ifdef ENABLE_MOTOR_DRIVER
rabad1 0:34bcb9bae5ea 210 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 211 printf("Initialize Motor Driver...");
rabad1 0:34bcb9bae5ea 212 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 213 fPwmPeriod = TB6612FNG_PWM_PERIOD_DEFAULT;
rabad1 0:34bcb9bae5ea 214 fPwmPulsewidth = TB6612FNG_PWM_PULSEWIDTH_DEFAULT;
rabad1 0:34bcb9bae5ea 215 motorDriver.setPwmAperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 216 motorDriver.setPwmBperiod(fPwmPeriod);
rabad1 0:34bcb9bae5ea 217 motorDriver.setPwmApulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 218 motorDriver.setPwmBpulsewidth(fPwmPulsewidth);
rabad1 0:34bcb9bae5ea 219 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 220 printf("DONE\n\r");
rabad1 0:34bcb9bae5ea 221 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 222 #endif /* ENABLE_MOTOR_DRIVER */
rabad1 0:34bcb9bae5ea 223
rabad1 0:34bcb9bae5ea 224 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 225 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 226 printf("Initialization complete!\n\r");
rabad1 0:34bcb9bae5ea 227 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 228
rabad1 0:34bcb9bae5ea 229 strcpy( wiflyBuffer, "ROBot awaiting commands\n\r");
rabad1 0:34bcb9bae5ea 230 client.send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 231 }
rabad1 0:34bcb9bae5ea 232
rabad1 0:34bcb9bae5ea 233 /* Name: ROBot_sm()
rabad1 0:34bcb9bae5ea 234 * Desc: ROBot state machine. Processes all peripheral data and sets
rabad1 0:34bcb9bae5ea 235 * status accordingly
rabad1 0:34bcb9bae5ea 236 * Inputs: none
rabad1 0:34bcb9bae5ea 237 * Outputs: none
rabad1 0:34bcb9bae5ea 238 */
rabad1 0:34bcb9bae5ea 239 void ROBot_sm(void)
rabad1 0:34bcb9bae5ea 240 {
rabad1 0:34bcb9bae5ea 241 float range;
rabad1 0:34bcb9bae5ea 242
rabad1 0:34bcb9bae5ea 243 if ( rangeFinder.getMeas(range) != RANGE_MEAS_INVALID )
rabad1 0:34bcb9bae5ea 244 {
rabad1 0:34bcb9bae5ea 245 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 246 printf("range = %.3f m\n\r", range);
rabad1 0:34bcb9bae5ea 247 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 248 if ( range < RANGE_COLLISION )
rabad1 0:34bcb9bae5ea 249 {
rabad1 0:34bcb9bae5ea 250 strcpy( wiflyBuffer, "\n\r**** COLLISION AVOIDANCE ****\n\r");
rabad1 0:34bcb9bae5ea 251 client.send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 252 if ( ROBot_STATUS != STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 253 {
rabad1 0:34bcb9bae5ea 254 moveStop();
rabad1 0:34bcb9bae5ea 255 }
rabad1 0:34bcb9bae5ea 256 ROBot_STATUS = STATUS_NEAR_COLLISION;
rabad1 0:34bcb9bae5ea 257 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 258 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 259 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 260 }
rabad1 0:34bcb9bae5ea 261 else if ( range < RANGE_WARNING )
rabad1 0:34bcb9bae5ea 262 {
rabad1 0:34bcb9bae5ea 263 strcpy( wiflyBuffer, "\n\r**** COLLISION WARNING ****");
rabad1 0:34bcb9bae5ea 264 sprintf(wiflyBuffer, "%s %0.3f\n\r", wiflyBuffer, range);
rabad1 0:34bcb9bae5ea 265 client.send_all(wiflyBuffer, strlen(wiflyBuffer));
rabad1 0:34bcb9bae5ea 266 #ifdef DEBUG
rabad1 0:34bcb9bae5ea 267 printf("%s", wiflyBuffer);
rabad1 0:34bcb9bae5ea 268 #endif /* DEBUG */
rabad1 0:34bcb9bae5ea 269 if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 270 {
rabad1 0:34bcb9bae5ea 271 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 272 }
rabad1 0:34bcb9bae5ea 273 }
rabad1 0:34bcb9bae5ea 274 else if ( ROBot_STATUS == STATUS_NEAR_COLLISION )
rabad1 0:34bcb9bae5ea 275 {
rabad1 0:34bcb9bae5ea 276 ROBot_STATUS = STATUS_OK;
rabad1 0:34bcb9bae5ea 277 }
rabad1 0:34bcb9bae5ea 278 }
rabad1 0:34bcb9bae5ea 279
rabad1 0:34bcb9bae5ea 280 // receive input and process it
rabad1 0:34bcb9bae5ea 281 memset(wiflyBuffer, 0, sizeof(wiflyBuffer));
rabad1 0:34bcb9bae5ea 282 int numCmds = client.receive(wiflyBuffer, sizeof(wiflyBuffer));
rabad1 0:34bcb9bae5ea 283 etROBot_CMD *pCmd = (etROBot_CMD *) wiflyBuffer;
rabad1 0:34bcb9bae5ea 284 while ( numCmds > 0 )
rabad1 0:34bcb9bae5ea 285 {
rabad1 0:34bcb9bae5ea 286 int cmdIdx = getCmdIdx(*pCmd);
rabad1 0:34bcb9bae5ea 287 if ( cmdIdx == CMD_MOVE_UNKNOWN )
rabad1 0:34bcb9bae5ea 288 {
rabad1 0:34bcb9bae5ea 289 pCmd++;
rabad1 0:34bcb9bae5ea 290 numCmds--;
rabad1 0:34bcb9bae5ea 291 continue;
rabad1 0:34bcb9bae5ea 292 }
rabad1 0:34bcb9bae5ea 293
rabad1 0:34bcb9bae5ea 294 switch ( ROBot_STATUS )
rabad1 0:34bcb9bae5ea 295 {
rabad1 0:34bcb9bae5ea 296 case STATUS_OK:
rabad1 0:34bcb9bae5ea 297 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 298 break;
rabad1 0:34bcb9bae5ea 299
rabad1 0:34bcb9bae5ea 300 case STATUS_NEAR_COLLISION:
rabad1 0:34bcb9bae5ea 301 if ( *pCmd != CMD_MOVE_FORWARD )
rabad1 0:34bcb9bae5ea 302 {
rabad1 0:34bcb9bae5ea 303 command[cmdIdx].cmd_func();
rabad1 0:34bcb9bae5ea 304 }
rabad1 0:34bcb9bae5ea 305 break;
rabad1 0:34bcb9bae5ea 306 }
rabad1 0:34bcb9bae5ea 307 pCmd++;
rabad1 0:34bcb9bae5ea 308 numCmds--;
rabad1 0:34bcb9bae5ea 309 }
rabad1 0:34bcb9bae5ea 310 }
rabad1 0:34bcb9bae5ea 311
rabad1 0:34bcb9bae5ea 312 /* Name: rangeFinderTimer_cb()
rabad1 0:34bcb9bae5ea 313 * Desc: callback function for range finder timer. Timer is used to
rabad1 0:34bcb9bae5ea 314 * initiate range finder measurements
rabad1 0:34bcb9bae5ea 315 * Inputs: none
rabad1 0:34bcb9bae5ea 316 * Outputs: none
rabad1 0:34bcb9bae5ea 317 */
rabad1 0:34bcb9bae5ea 318 void rangeFinderTimer_cb(void)
rabad1 0:34bcb9bae5ea 319 {
rabad1 0:34bcb9bae5ea 320 rangeFinder.startMeas();
rabad1 0:34bcb9bae5ea 321 }
rabad1 0:34bcb9bae5ea 322
rabad1 0:34bcb9bae5ea 323 /* Name: getCmdIdx()
rabad1 0:34bcb9bae5ea 324 * Desc: returns command[] index of the input command
rabad1 0:34bcb9bae5ea 325 * Inputs: none
rabad1 0:34bcb9bae5ea 326 * Outputs: none
rabad1 0:34bcb9bae5ea 327 */
rabad1 0:34bcb9bae5ea 328 int getCmdIdx(etROBot_CMD cmd)
rabad1 0:34bcb9bae5ea 329 {
rabad1 0:34bcb9bae5ea 330 int i;
rabad1 0:34bcb9bae5ea 331
rabad1 0:34bcb9bae5ea 332 for ( i = 0; i < NUM_CMDS; i++)
rabad1 0:34bcb9bae5ea 333 {
rabad1 0:34bcb9bae5ea 334 if ( cmd == command[i].cmd )
rabad1 0:34bcb9bae5ea 335 {
rabad1 0:34bcb9bae5ea 336 return i;
rabad1 0:34bcb9bae5ea 337 }
rabad1 0:34bcb9bae5ea 338 }
rabad1 0:34bcb9bae5ea 339 return CMD_MOVE_UNKNOWN;
rabad1 0:34bcb9bae5ea 340 }
rabad1 0:34bcb9bae5ea 341
rabad1 1:e918d1aa9c26 342 /* Name: moveStop()
rabad1 1:e918d1aa9c26 343 * Desc: ceases all motion
rabad1 1:e918d1aa9c26 344 * Inputs: none
rabad1 1:e918d1aa9c26 345 * Outputs: none
rabad1 1:e918d1aa9c26 346 */
rabad1 0:34bcb9bae5ea 347 void moveStop(void)
rabad1 0:34bcb9bae5ea 348 {
rabad1 0:34bcb9bae5ea 349 motorDriver.standby();
rabad1 0:34bcb9bae5ea 350 }
rabad1 0:34bcb9bae5ea 351
rabad1 1:e918d1aa9c26 352 /* Name: moveForward()
rabad1 1:e918d1aa9c26 353 * Desc: commands forward motion
rabad1 1:e918d1aa9c26 354 * Inputs: none
rabad1 1:e918d1aa9c26 355 * Outputs: none
rabad1 1:e918d1aa9c26 356 */
rabad1 0:34bcb9bae5ea 357 void moveForward(void)
rabad1 0:34bcb9bae5ea 358 {
rabad1 0:34bcb9bae5ea 359 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 360 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 361 }
rabad1 0:34bcb9bae5ea 362
rabad1 1:e918d1aa9c26 363 /* Name: moveBackward()
rabad1 1:e918d1aa9c26 364 * Desc: commands backward motion
rabad1 1:e918d1aa9c26 365 * Inputs: none
rabad1 1:e918d1aa9c26 366 * Outputs: none
rabad1 1:e918d1aa9c26 367 */
rabad1 0:34bcb9bae5ea 368 void moveBackward(void)
rabad1 0:34bcb9bae5ea 369 {
rabad1 0:34bcb9bae5ea 370 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 371 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 372 }
rabad1 0:34bcb9bae5ea 373
rabad1 1:e918d1aa9c26 374 /* Name: moveLeft()
rabad1 1:e918d1aa9c26 375 * Desc: commands left motion
rabad1 1:e918d1aa9c26 376 * Inputs: none
rabad1 1:e918d1aa9c26 377 * Outputs: none
rabad1 1:e918d1aa9c26 378 */
rabad1 0:34bcb9bae5ea 379 void moveLeft(void)
rabad1 0:34bcb9bae5ea 380 {
rabad1 0:34bcb9bae5ea 381 motorDriver.motorA_ccw();
rabad1 0:34bcb9bae5ea 382 motorDriver.motorB_cw();
rabad1 0:34bcb9bae5ea 383 }
rabad1 0:34bcb9bae5ea 384
rabad1 1:e918d1aa9c26 385 /* Name: moveRight()
rabad1 1:e918d1aa9c26 386 * Desc: commands right motion
rabad1 1:e918d1aa9c26 387 * Inputs: none
rabad1 1:e918d1aa9c26 388 * Outputs: none
rabad1 1:e918d1aa9c26 389 */
rabad1 0:34bcb9bae5ea 390 void moveRight(void)
rabad1 0:34bcb9bae5ea 391 {
rabad1 0:34bcb9bae5ea 392 motorDriver.motorA_cw();
rabad1 0:34bcb9bae5ea 393 motorDriver.motorB_ccw();
rabad1 0:34bcb9bae5ea 394 }