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.
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
Standard Operation
A wireless remote connection from a PC sends commands 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
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?
User | Revision | Line number | New 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 | } |