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@0:34bcb9bae5ea, 2013-11-24 (annotated)

Committer:
rabad1
Date:
Sun Nov 24 22:24:25 2013 +0000
Revision:
0:34bcb9bae5ea
Child:
1:e918d1aa9c26
initial release

Who changed what in which revision?

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