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@5:7f5fcee1737d, 2014-11-17 (annotated)

Committer:
mfurseman
Date:
Mon Nov 17 19:26:30 2014 +0000
Revision:
5:7f5fcee1737d
Parent:
4:1b5c2a2cdeb7
Child:
7:ee8b630b0a33
Removed PWM for motor control, and refactored into separate functions.

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