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

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