Matthew Furseman / Mbed 2 deprecated Robo

A WiFiDipCortex based robot. Control is via sockets over WiFi. See also: https://github.com/mfurseman/robo-android

Dependencies:   Motordriver USBDevice cc3000_hostdriver_mbedsocket_hacked mbed

main.cpp@7:ee8b630b0a33, 2014-11-19 (annotated)

Committer:
mfurseman
Date:
Wed Nov 19 22:08:14 2014 +0000
Revision:
7:ee8b630b0a33
Parent:
5:7f5fcee1737d 
Some tidying:;  * Moved wait time into #define;  * Stop motors on disconnect;  * Moved some redundant debug statments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurseman 4:1b5c2a2cdeb7 1 #include <stdint.h>
mfurseman 4:1b5c2a2cdeb7 2 #include <mbed.h>
mfurseman 4:1b5c2a2cdeb7 3 #include "motordriver.h"
mfurseman 0:993d6b65e255 4 #include "cc3000.h"
mfurseman 2:50c151183047 5 #include "TCPSocketConnection.h"
mfurseman 2:50c151183047 6 #include "TCPSocketServer.h"
mfurseman 1:b66a2d756c8a 7
mfurseman 4:1b5c2a2cdeb7 8
mfurseman 3:ba11f6207550 9 /* MAC 08:00:28:57:43:b8 */
mfurseman 3:ba11f6207550 10
mfurseman 3:ba11f6207550 11 /* Quickly change debug flag to remove USB serial code */
mfurseman 3:ba11f6207550 12 //#define DEBUG
mfurseman 0:993d6b65e255 13 #ifdef DEBUG
mfurseman 2:50c151183047 14 #include "USBSerial.h"
mfurseman 4:1b5c2a2cdeb7 15 USBSerial serial;
mfurseman 0:993d6b65e255 16 #define debug(x, ...) serial.printf(x, ##__VA_ARGS__);
mfurseman 0:993d6b65e255 17 #else
mfurseman 0:993d6b65e255 18 #define debug(x, ...)
mfurseman 0:993d6b65e255 19 #endif
mfurseman 0:993d6b65e255 20
mfurseman 5:7f5fcee1737d 21 /* Network constants */
mfurseman 5:7f5fcee1737d 22 #define SERVER_PORT 5678
mfurseman 7:ee8b630b0a33 23 #define CC3000_WAIT 15
mfurseman 5:7f5fcee1737d 24
mfurseman 3:ba11f6207550 25 /* Client commands */
mfurseman 3:ba11f6207550 26 #define CMD_NULL 0
mfurseman 4:1b5c2a2cdeb7 27 #define CMD_C_ECHO 0x61 // 'a'
mfurseman 5:7f5fcee1737d 28 #define CMD_C_LED_ON 0x62 // 'b'
mfurseman 5:7f5fcee1737d 29 #define CMD_C_LED_OFF 0x63 // 'c'
mfurseman 5:7f5fcee1737d 30 #define CMD_C_PRINT_UINT32 0x64 // 'd'
mfurseman 5:7f5fcee1737d 31 #define CMD_C_MOTOR_LEFT_DUTY 0x65 // 'e'
mfurseman 5:7f5fcee1737d 32 #define CMD_C_MOTOR_RIGHT_DUTY 0x66 // 'f'
mfurseman 5:7f5fcee1737d 33 #define CMD_C_MOTOR_LEFT_COAST 0x67 // 'g'
mfurseman 5:7f5fcee1737d 34 #define CMD_C_MOTOR_RIGHT_COAST 0x68 // 'h'
mfurseman 5:7f5fcee1737d 35 #define CMD_C_MOTOR_LEFT_STOP 0x69 // 'i'
mfurseman 7:ee8b630b0a33 36 #define CMD_C_MOTOR_RIGHT_STOP 0x6a // 'j'
mfurseman 3:ba11f6207550 37
mfurseman 1:b66a2d756c8a 38
mfurseman 0:993d6b65e255 39 using namespace mbed_cc3000;
mfurseman 0:993d6b65e255 40
mfurseman 1:b66a2d756c8a 41
mfurseman 0:993d6b65e255 42 /* On board LED */
mfurseman 0:993d6b65e255 43 DigitalOut led(P0_1);
mfurseman 0:993d6b65e255 44
mfurseman 5:7f5fcee1737d 45 /* Motors: allow breaking */
mfurseman 5:7f5fcee1737d 46 Motor leftMotor(P0_8, P1_24, P0_4, true);
mfurseman 5:7f5fcee1737d 47 Motor rightMotor(P0_9, P1_13, P1_14, true);
mfurseman 4:1b5c2a2cdeb7 48
mfurseman 0:993d6b65e255 49 /* Serial library for WiFi module */
mfurseman 0:993d6b65e255 50 cc3000 wifi(p28, p27, p30, SPI(p21, p14, p37));
mfurseman 0:993d6b65e255 51
mfurseman 0:993d6b65e255 52 /* Struct to hold connection data */
mfurseman 0:993d6b65e255 53 tNetappIpconfigRetArgs ipinfo;
mfurseman 0:993d6b65e255 54
mfurseman 0:993d6b65e255 55
mfurseman 5:7f5fcee1737d 56 /**
mfurseman 5:7f5fcee1737d 57 * Prints CC3000 connection info
mfurseman 5:7f5fcee1737d 58 */
mfurseman 3:ba11f6207550 59 void printConnectionInfo()
mfurseman 3:ba11f6207550 60 {
mfurseman 0:993d6b65e255 61 if (( wifi.is_enabled() ) && ( wifi.is_dhcp_configured() )) {
mfurseman 0:993d6b65e255 62 wifi.get_ip_config(&ipinfo);
mfurseman 3:ba11f6207550 63 }
mfurseman 0:993d6b65e255 64 if (! wifi.is_enabled() ) {
mfurseman 3:ba11f6207550 65 debug("CC3000 Disabled\r\n");
mfurseman 3:ba11f6207550 66 } else if ( wifi.is_dhcp_configured() ) {
mfurseman 0:993d6b65e255 67 debug("SSID : %-33s|\r\n", ipinfo.uaSSID);
mfurseman 3:ba11f6207550 68 debug("IP : %-35s|\r\n", wifi.getIPAddress());
mfurseman 3:ba11f6207550 69 } else if ( wifi.is_connected() ) {
mfurseman 3:ba11f6207550 70 debug("Connecting, waiting for DHCP\r\n");
mfurseman 3:ba11f6207550 71 } else {
mfurseman 3:ba11f6207550 72 debug("Not Connected\r\n");
mfurseman 0:993d6b65e255 73 }
mfurseman 0:993d6b65e255 74 }
mfurseman 0:993d6b65e255 75
mfurseman 1:b66a2d756c8a 76
mfurseman 5:7f5fcee1737d 77 /**
mfurseman 5:7f5fcee1737d 78 * WiFi DipCortex board setup
mfurseman 5:7f5fcee1737d 79 */
mfurseman 3:ba11f6207550 80 void init()
mfurseman 3:ba11f6207550 81 {
mfurseman 3:ba11f6207550 82 NVIC_SetPriority(SSP1_IRQn, 0x0);
mfurseman 0:993d6b65e255 83 NVIC_SetPriority(PIN_INT0_IRQn, 0x1);
mfurseman 3:ba11f6207550 84
mfurseman 0:993d6b65e255 85 // SysTick set to lower priority than Wi-Fi SPI bus interrupt
mfurseman 3:ba11f6207550 86 NVIC_SetPriority(SysTick_IRQn, 0x2);
mfurseman 3:ba11f6207550 87
mfurseman 0:993d6b65e255 88 // Enable RAM1
mfurseman 0:993d6b65e255 89 LPC_SYSCON->SYSAHBCLKCTRL |= (0x1 << 26);
mfurseman 3:ba11f6207550 90
mfurseman 1:b66a2d756c8a 91 // This may be neccassary for CC3000
mfurseman 0:993d6b65e255 92 wait(1);
mfurseman 1:b66a2d756c8a 93 }
mfurseman 1:b66a2d756c8a 94
mfurseman 0:993d6b65e255 95
mfurseman 5:7f5fcee1737d 96 /**
mfurseman 5:7f5fcee1737d 97 * Connects WiFi assuming existing SmartConfig
mfurseman 5:7f5fcee1737d 98 */
mfurseman 3:ba11f6207550 99 void connectWifi()
mfurseman 3:ba11f6207550 100 {
mfurseman 0:993d6b65e255 101 wifi.start(0);
mfurseman 0:993d6b65e255 102 wait_ms(750);
mfurseman 1:b66a2d756c8a 103 wifi._wlan.ioctl_set_connection_policy(0, 0, 1);
mfurseman 1:b66a2d756c8a 104 // TODO: Timeout and switch on smart config here
mfurseman 2:50c151183047 105 // TODO: Use static IP if possible
mfurseman 1:b66a2d756c8a 106 }
mfurseman 0:993d6b65e255 107
mfurseman 2:50c151183047 108
mfurseman 5:7f5fcee1737d 109 /**
mfurseman 5:7f5fcee1737d 110 * Brute force check to see if the connection is connected.
mfurseman 5:7f5fcee1737d 111 * CC3000 doesn't detect dropped connection until send is
mfurseman 5:7f5fcee1737d 112 * called.
mfurseman 5:7f5fcee1737d 113 */
mfurseman 5:7f5fcee1737d 114 bool isConnected(TCPSocketConnection *connection) {
mfurseman 7:ee8b630b0a33 115 wait_ms(CC3000_WAIT);
mfurseman 7:ee8b630b0a33 116 debug("Checking conneciton is connected\r\n");
mfurseman 7:ee8b630b0a33 117 connection->set_blocking(false, 2000);
mfurseman 7:ee8b630b0a33 118 return connection->send("xxx\r\n", 5) > 0;
mfurseman 5:7f5fcee1737d 119 }
mfurseman 5:7f5fcee1737d 120
mfurseman 5:7f5fcee1737d 121
mfurseman 5:7f5fcee1737d 122 /**
mfurseman 5:7f5fcee1737d 123 * Reads a long from the client and returns it as a float
mfurseman 5:7f5fcee1737d 124 * between 0.0 and 1.0, which represents the relative
mfurseman 5:7f5fcee1737d 125 * magnitude of the byte between 0 and UINT32_MAX.
mfurseman 5:7f5fcee1737d 126 */
mfurseman 5:7f5fcee1737d 127 float getUint32AsFloat(TCPSocketConnection* connection) {
mfurseman 5:7f5fcee1737d 128 uint32_t int_buffer;
mfurseman 5:7f5fcee1737d 129 connection->set_blocking(false, 2000);
mfurseman 5:7f5fcee1737d 130 int status = connection->receive_all((char*)&int_buffer, sizeof(int_buffer)); // 4 Bytes
mfurseman 5:7f5fcee1737d 131 debug("Command print int32 recieved: %u with status: %d\r\n", int_buffer, status);
mfurseman 5:7f5fcee1737d 132 int_buffer = ntohl(int_buffer);
mfurseman 5:7f5fcee1737d 133 debug("Converted to host byte order: %u\r\n", int_buffer);
mfurseman 5:7f5fcee1737d 134 debug("Conversion of uint32_t to float: %f\r\n", ((int_buffer*1.0f) / ((uint32_t)-1)));
mfurseman 5:7f5fcee1737d 135 return (int_buffer*1.0f) / ((uint32_t)-1);
mfurseman 5:7f5fcee1737d 136 }
mfurseman 5:7f5fcee1737d 137
mfurseman 5:7f5fcee1737d 138
mfurseman 5:7f5fcee1737d 139 /**
mfurseman 5:7f5fcee1737d 140 * Reads a byte from the client and returns it as a float
mfurseman 5:7f5fcee1737d 141 * between 0.0 and 1.0, which represents the relative
mfurseman 5:7f5fcee1737d 142 * magnitude of the byte between 0 and UINT8_MAX.
mfurseman 5:7f5fcee1737d 143 */
mfurseman 5:7f5fcee1737d 144 float getUint8AsFloat(TCPSocketConnection* connection) {
mfurseman 5:7f5fcee1737d 145 uint8_t char_buffer;
mfurseman 5:7f5fcee1737d 146 connection->set_blocking(false, 2000);
mfurseman 5:7f5fcee1737d 147 int status = connection->receive_all((char*)&char_buffer, sizeof(char_buffer)); // 1 Byte
mfurseman 5:7f5fcee1737d 148 debug("Command left motor duty received: %d with status %d\r\n", char_buffer, status);
mfurseman 5:7f5fcee1737d 149 debug("Converstion from uint8_t to float: %f\r\n", ((char_buffer*1.0f) / ((uint8_t)-1)));
mfurseman 7:ee8b630b0a33 150 return ((char_buffer*1.0f) / ((uint8_t)-1));
mfurseman 5:7f5fcee1737d 151 }
mfurseman 5:7f5fcee1737d 152
mfurseman 5:7f5fcee1737d 153
mfurseman 5:7f5fcee1737d 154 /**
mfurseman 5:7f5fcee1737d 155 * Client connection loop. This monitors the socket for
mfurseman 5:7f5fcee1737d 156 * connections and proccesses them when they are
mfurseman 5:7f5fcee1737d 157 * recieved. It returns when the client disconnects.
mfurseman 5:7f5fcee1737d 158 */
mfurseman 5:7f5fcee1737d 159 void monitorConnection(TCPSocketConnection* connection) {
mfurseman 5:7f5fcee1737d 160 int timeout_counter = 1;
mfurseman 5:7f5fcee1737d 161 while(1) {
mfurseman 7:ee8b630b0a33 162 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 163 char command = 0;
mfurseman 5:7f5fcee1737d 164 connection->set_blocking(false, 5); // 5 ms time out is min for CC3000
mfurseman 5:7f5fcee1737d 165 int status = connection->receive(&command, 1);
mfurseman 5:7f5fcee1737d 166 if(status == 1) {
mfurseman 5:7f5fcee1737d 167 debug("Recieved data from connection: %d with status %d\r\n", command, status);
mfurseman 5:7f5fcee1737d 168 switch(command) {
mfurseman 5:7f5fcee1737d 169 case CMD_C_ECHO:
mfurseman 7:ee8b630b0a33 170 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 171 char buffer[3];
mfurseman 5:7f5fcee1737d 172 connection->set_blocking(false, 2000);
mfurseman 5:7f5fcee1737d 173 status = connection->receive_all(buffer, sizeof(buffer));
mfurseman 5:7f5fcee1737d 174 debug("Echo test recieved: %s Status: %d\r\n", buffer, status);
mfurseman 5:7f5fcee1737d 175
mfurseman 7:ee8b630b0a33 176 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 177 status = connection->send_all(buffer, sizeof(buffer));
mfurseman 5:7f5fcee1737d 178 debug("Echo test send completed with status: %d\r\n");
mfurseman 5:7f5fcee1737d 179 break;
mfurseman 5:7f5fcee1737d 180
mfurseman 5:7f5fcee1737d 181 case CMD_C_LED_ON:
mfurseman 5:7f5fcee1737d 182 led = 1;
mfurseman 5:7f5fcee1737d 183 break;
mfurseman 5:7f5fcee1737d 184
mfurseman 5:7f5fcee1737d 185 case CMD_C_LED_OFF:
mfurseman 5:7f5fcee1737d 186 led = 0;
mfurseman 5:7f5fcee1737d 187 break;
mfurseman 5:7f5fcee1737d 188
mfurseman 5:7f5fcee1737d 189 case CMD_C_PRINT_UINT32:
mfurseman 7:ee8b630b0a33 190 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 191 { // Variable declerations inside switch must be in higher scope.
mfurseman 5:7f5fcee1737d 192 // This is only okay as it's debugging code.
mfurseman 5:7f5fcee1737d 193 float printFloat = getUint32AsFloat(connection);
mfurseman 5:7f5fcee1737d 194 debug("Print uint32 got float with value %f\r\n", printFloat);
mfurseman 5:7f5fcee1737d 195 }
mfurseman 5:7f5fcee1737d 196 break;
mfurseman 5:7f5fcee1737d 197
mfurseman 5:7f5fcee1737d 198 case CMD_C_MOTOR_LEFT_DUTY:
mfurseman 7:ee8b630b0a33 199 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 200 leftMotor.speed(2.0f * getUint8AsFloat(connection) - 1.0f); // Convert to +/- 1
mfurseman 5:7f5fcee1737d 201 break;
mfurseman 5:7f5fcee1737d 202
mfurseman 5:7f5fcee1737d 203 case CMD_C_MOTOR_RIGHT_DUTY:
mfurseman 7:ee8b630b0a33 204 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 205 rightMotor.speed(2.0f * getUint8AsFloat(connection) - 1.0f); // Convert to +/- 1
mfurseman 5:7f5fcee1737d 206 break;
mfurseman 5:7f5fcee1737d 207
mfurseman 5:7f5fcee1737d 208 case CMD_C_MOTOR_LEFT_COAST:
mfurseman 5:7f5fcee1737d 209 leftMotor.coast();
mfurseman 5:7f5fcee1737d 210 break;
mfurseman 5:7f5fcee1737d 211
mfurseman 5:7f5fcee1737d 212 case CMD_C_MOTOR_RIGHT_COAST:
mfurseman 5:7f5fcee1737d 213 rightMotor.coast();
mfurseman 5:7f5fcee1737d 214 break;
mfurseman 5:7f5fcee1737d 215
mfurseman 5:7f5fcee1737d 216 case CMD_C_MOTOR_LEFT_STOP:
mfurseman 7:ee8b630b0a33 217 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 218 leftMotor.stop(getUint8AsFloat(connection));
mfurseman 5:7f5fcee1737d 219 break;
mfurseman 5:7f5fcee1737d 220
mfurseman 5:7f5fcee1737d 221 case CMD_C_MOTOR_RIGHT_STOP:
mfurseman 7:ee8b630b0a33 222 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 223 rightMotor.stop(getUint8AsFloat(connection));
mfurseman 5:7f5fcee1737d 224 break;
mfurseman 5:7f5fcee1737d 225
mfurseman 5:7f5fcee1737d 226 default:
mfurseman 5:7f5fcee1737d 227 debug("Command %d not recognised\r\n", command);
mfurseman 5:7f5fcee1737d 228 break;
mfurseman 5:7f5fcee1737d 229 }
mfurseman 5:7f5fcee1737d 230 }
mfurseman 5:7f5fcee1737d 231
mfurseman 5:7f5fcee1737d 232 /* Check to see if the non-blocking socket is closed */
mfurseman 5:7f5fcee1737d 233 if((timeout_counter++) % 100 == 0) {
mfurseman 5:7f5fcee1737d 234 if(!isConnected(connection)) {
mfurseman 5:7f5fcee1737d 235 debug("Client disconected\r\n");
mfurseman 7:ee8b630b0a33 236 leftMotor.coast();
mfurseman 7:ee8b630b0a33 237 rightMotor.coast();
mfurseman 5:7f5fcee1737d 238 break;
mfurseman 5:7f5fcee1737d 239 }
mfurseman 5:7f5fcee1737d 240 }
mfurseman 5:7f5fcee1737d 241 }
mfurseman 5:7f5fcee1737d 242 }
mfurseman 5:7f5fcee1737d 243
mfurseman 5:7f5fcee1737d 244
mfurseman 5:7f5fcee1737d 245 /**
mfurseman 5:7f5fcee1737d 246 * Where it all begins.
mfurseman 5:7f5fcee1737d 247 */
mfurseman 3:ba11f6207550 248 int main(void)
mfurseman 3:ba11f6207550 249 {
mfurseman 2:50c151183047 250 init();
mfurseman 2:50c151183047 251 debug("Completed init()\r\n");
mfurseman 2:50c151183047 252 printConnectionInfo();
mfurseman 3:ba11f6207550 253
mfurseman 1:b66a2d756c8a 254 connectWifi();
mfurseman 2:50c151183047 255 debug("Completed connectWifi()\r\n");
mfurseman 2:50c151183047 256 printConnectionInfo();
mfurseman 0:993d6b65e255 257
mfurseman 0:993d6b65e255 258 while(1) {
mfurseman 3:ba11f6207550 259 debug("\r\nOne second client attachment loop\r\n");
mfurseman 0:993d6b65e255 260 printConnectionInfo();
mfurseman 3:ba11f6207550 261
mfurseman 4:1b5c2a2cdeb7 262 TCPSocketConnection client;
mfurseman 3:ba11f6207550 263 TCPSocketServer server;
mfurseman 3:ba11f6207550 264
mfurseman 7:ee8b630b0a33 265 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 266 server.bind(SERVER_PORT);
mfurseman 3:ba11f6207550 267 server.listen();
mfurseman 5:7f5fcee1737d 268 int32_t status = server.accept(client);
mfurseman 3:ba11f6207550 269 debug("Accept client returned with status %d\r\n", status);
mfurseman 3:ba11f6207550 270 if(status >= 0) {
mfurseman 7:ee8b630b0a33 271 wait_ms(CC3000_WAIT);
mfurseman 5:7f5fcee1737d 272 client.set_blocking(false, 1000);
mfurseman 3:ba11f6207550 273 debug("Connection from: %s \r\n", client.get_address());
mfurseman 5:7f5fcee1737d 274
mfurseman 5:7f5fcee1737d 275 monitorConnection(&client);
mfurseman 3:ba11f6207550 276
mfurseman 3:ba11f6207550 277 debug("Client connection lost\r\n");
mfurseman 3:ba11f6207550 278 }
mfurseman 0:993d6b65e255 279 led = !led;
mfurseman 5:7f5fcee1737d 280 wait(1);
mfurseman 3:ba11f6207550 281 }
mfurseman 0:993d6b65e255 282 }