Graham Miles / Mbed 2 deprecated dotbot

a robot you can talk to when you're lonely

Dependencies:   HALLFX_ENCODER Motor RPCInterface mbed

main.cpp@2:747b84e54088, 2016-12-13 (annotated)

Committer:
gmiles3
Date:
Tue Dec 13 16:00:55 2016 +0000
Revision:
2:747b84e54088
Parent:
0:1fb00e911fe6 
minor changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gmiles3 0:1fb00e911fe6 1 #include "mbed.h"
gmiles3 0:1fb00e911fe6 2 #include "Car.h"
gmiles3 0:1fb00e911fe6 3 #include "mywifi.h"
gmiles3 0:1fb00e911fe6 4 #include "Motor.h"
gmiles3 0:1fb00e911fe6 5 #include "HALLFX_ENCODER.h"
gmiles3 0:1fb00e911fe6 6 #include "mbed_rpc.h"
gmiles3 0:1fb00e911fe6 7
gmiles3 0:1fb00e911fe6 8 Serial pc(USBTX, USBRX);
gmiles3 0:1fb00e911fe6 9 Serial esp(p28, p27);
gmiles3 2:747b84e54088 10 char ssid[32] = "DelosLobbyWifi";
gmiles3 2:747b84e54088 11 char pwd[32] = "freezeallmotorfunctions";
gmiles3 0:1fb00e911fe6 12 char port[32] = "1035"; // must be port forwarded
gmiles3 2:747b84e54088 13 char timeout[32] = "28800"; // 28800 is max
gmiles3 0:1fb00e911fe6 14 volatile int tx_in=0;
gmiles3 0:1fb00e911fe6 15 volatile int tx_out=0;
gmiles3 0:1fb00e911fe6 16 volatile int rx_in=0;
gmiles3 0:1fb00e911fe6 17 volatile int rx_out=0;
gmiles3 0:1fb00e911fe6 18 const int buffer_size = 4095;
gmiles3 0:1fb00e911fe6 19 char tx_buffer[buffer_size+1];
gmiles3 0:1fb00e911fe6 20 char rx_buffer[buffer_size+1];
gmiles3 0:1fb00e911fe6 21 char cmdbuff[1024];
gmiles3 0:1fb00e911fe6 22 char rx_line[1024];
gmiles3 0:1fb00e911fe6 23
gmiles3 0:1fb00e911fe6 24
gmiles3 0:1fb00e911fe6 25 DigitalOut led1(LED1);
gmiles3 0:1fb00e911fe6 26 DigitalOut led2(LED2);
gmiles3 0:1fb00e911fe6 27 DigitalOut led3(LED3);
gmiles3 0:1fb00e911fe6 28 DigitalOut led4(LED4);
gmiles3 0:1fb00e911fe6 29
gmiles3 0:1fb00e911fe6 30 float left_speed = .5;
gmiles3 0:1fb00e911fe6 31 float right_speed = .5;
gmiles3 0:1fb00e911fe6 32 float clicksTravelled;
gmiles3 0:1fb00e911fe6 33 Motor left_motor(p26,p24,p25); // pwm, fwd, rev
gmiles3 0:1fb00e911fe6 34 Motor right_motor(p21, p23, p22); // pwm, fwd, rev
gmiles3 0:1fb00e911fe6 35 HALLFX_ENCODER right_hall(p13); // left hall effect encoder
gmiles3 0:1fb00e911fe6 36 HALLFX_ENCODER left_hall(p14); // left hall effect encoder
gmiles3 0:1fb00e911fe6 37 float encoderFactor = 40;
gmiles3 0:1fb00e911fe6 38 RPCFunction rpcTurnRight(&carTurnRight, "turnRight");
gmiles3 0:1fb00e911fe6 39 RPCFunction rpcTurnLeft(&carTurnLeft, "turnLeft");
gmiles3 0:1fb00e911fe6 40 RPCFunction rpcMoveForward(&carMoveForwardDistance, "moveForward");
gmiles3 0:1fb00e911fe6 41 RPCFunction rpcMoveBackward(&carMoveBackwardDistance, "moveBackward");
gmiles3 0:1fb00e911fe6 42 RPCFunction rpcTurn(&carTurn, "turn");
gmiles3 0:1fb00e911fe6 43
gmiles3 0:1fb00e911fe6 44 int main() {
gmiles3 0:1fb00e911fe6 45 clicksTravelled = 0.0;
gmiles3 0:1fb00e911fe6 46 pc.baud(9600);
gmiles3 0:1fb00e911fe6 47 esp.baud(9600);
gmiles3 0:1fb00e911fe6 48 led1=0,led2=0,led3=0,led4=0;
gmiles3 0:1fb00e911fe6 49 esp.attach(&Rx_interrupt, Serial::RxIrq);
gmiles3 0:1fb00e911fe6 50 esp.attach(&Tx_interrupt, Serial::TxIrq);
gmiles3 0:1fb00e911fe6 51 wait(5);
gmiles3 0:1fb00e911fe6 52 connectToNetwork();
gmiles3 0:1fb00e911fe6 53 char rpc_in[256];
gmiles3 0:1fb00e911fe6 54 char rpc_out[256];
gmiles3 0:1fb00e911fe6 55 while (1) {
gmiles3 0:1fb00e911fe6 56 getReply();
gmiles3 0:1fb00e911fe6 57 memset(&rpc_in[0], 0, sizeof(rpc_in));
gmiles3 2:747b84e54088 58 memset(&rpc_out[0], 0, sizeof(rpc_out));
gmiles3 2:747b84e54088 59 int length = (int)rx_line[3] - 48; // bytes 0 to 2 are trash; byte 3 is length of message
gmiles3 0:1fb00e911fe6 60 if (length > 0 && length < 256) {
gmiles3 0:1fb00e911fe6 61 for (int i = 0; i < length; i++) {
gmiles3 2:747b84e54088 62 rpc_in[i] = rx_line[i+4]; // bytes 4 to length+3 are the valid data
gmiles3 0:1fb00e911fe6 63 }
gmiles3 0:1fb00e911fe6 64 RPC::call(rpc_in, rpc_out);
gmiles3 0:1fb00e911fe6 65 pc.printf("%s\n", rpc_out);
gmiles3 0:1fb00e911fe6 66 }
gmiles3 2:747b84e54088 67 // lambda function is event-triggered and non-persistent
gmiles3 2:747b84e54088 68 // after it terminates, we need to close the existing connection and start another one
gmiles3 2:747b84e54088 69 strcpy(cmdbuff, "srv:close()\r\n");
gmiles3 0:1fb00e911fe6 70 sendCMD();
gmiles3 0:1fb00e911fe6 71 wait(.5);
gmiles3 0:1fb00e911fe6 72 getReply();
gmiles3 0:1fb00e911fe6 73 strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
gmiles3 0:1fb00e911fe6 74 strcat(cmdbuff, timeout);
gmiles3 0:1fb00e911fe6 75 strcat(cmdbuff, ")\r\n");
gmiles3 0:1fb00e911fe6 76 sendCMD();
gmiles3 0:1fb00e911fe6 77 wait(.5);
gmiles3 0:1fb00e911fe6 78 getReply();
gmiles3 0:1fb00e911fe6 79 strcpy(cmdbuff, "srv:listen(");
gmiles3 0:1fb00e911fe6 80 strcat(cmdbuff, port);
gmiles3 0:1fb00e911fe6 81 strcat(cmdbuff, ",function(conn)\r\n");
gmiles3 0:1fb00e911fe6 82 sendCMD();
gmiles3 0:1fb00e911fe6 83 wait(.5);
gmiles3 0:1fb00e911fe6 84 getReply();
gmiles3 0:1fb00e911fe6 85 strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
gmiles3 0:1fb00e911fe6 86 sendCMD();
gmiles3 0:1fb00e911fe6 87 wait(.5);
gmiles3 0:1fb00e911fe6 88 getReply();
gmiles3 0:1fb00e911fe6 89 strcpy(cmdbuff, "conn:send('");
gmiles3 0:1fb00e911fe6 90 strcat(cmdbuff, reportStatus());
gmiles3 0:1fb00e911fe6 91 strcat(cmdbuff, "')\r\n");
gmiles3 0:1fb00e911fe6 92 sendCMD();
gmiles3 0:1fb00e911fe6 93 wait(.5);
gmiles3 0:1fb00e911fe6 94 getReply();
gmiles3 0:1fb00e911fe6 95 strcpy(cmdbuff, "print(payload)\r\n");
gmiles3 0:1fb00e911fe6 96 sendCMD();
gmiles3 0:1fb00e911fe6 97 wait(.5);
gmiles3 0:1fb00e911fe6 98 getReply();
gmiles3 0:1fb00e911fe6 99 strcpy(cmdbuff, "end)\r\n");
gmiles3 0:1fb00e911fe6 100 sendCMD();
gmiles3 0:1fb00e911fe6 101 wait(.5);
gmiles3 0:1fb00e911fe6 102 getReply();
gmiles3 0:1fb00e911fe6 103 strcpy(cmdbuff, "end)\r\n");
gmiles3 0:1fb00e911fe6 104 sendCMD();
gmiles3 0:1fb00e911fe6 105 wait(.5);
gmiles3 0:1fb00e911fe6 106 getReply();
gmiles3 0:1fb00e911fe6 107 }
gmiles3 0:1fb00e911fe6 108
gmiles3 0:1fb00e911fe6 109 }
gmiles3 0:1fb00e911fe6 110
gmiles3 0:1fb00e911fe6 111
gmiles3 0:1fb00e911fe6 112
gmiles3 0:1fb00e911fe6 113
gmiles3 0:1fb00e911fe6 114
gmiles3 0:1fb00e911fe6 115
gmiles3 0:1fb00e911fe6 116
gmiles3 0:1fb00e911fe6 117 /* CAR FUNCTIONS */
gmiles3 0:1fb00e911fe6 118
gmiles3 0:1fb00e911fe6 119 void carStop() {
gmiles3 0:1fb00e911fe6 120 left_motor.speed(0);
gmiles3 0:1fb00e911fe6 121 right_motor.speed(0);
gmiles3 0:1fb00e911fe6 122 }
gmiles3 0:1fb00e911fe6 123
gmiles3 0:1fb00e911fe6 124 void carResetEncoders() {
gmiles3 0:1fb00e911fe6 125 left_hall.reset();
gmiles3 0:1fb00e911fe6 126 right_hall.reset();
gmiles3 0:1fb00e911fe6 127 wait(0.1);
gmiles3 0:1fb00e911fe6 128 }
gmiles3 0:1fb00e911fe6 129
gmiles3 0:1fb00e911fe6 130 void carMoveForwardDistance(Arguments *in, Reply *out) {
gmiles3 0:1fb00e911fe6 131 carResetEncoders();
gmiles3 0:1fb00e911fe6 132 long diff;
gmiles3 0:1fb00e911fe6 133 int dist = in->getArg<int>();
gmiles3 0:1fb00e911fe6 134 float enc_dist = dist * encoderFactor;
gmiles3 0:1fb00e911fe6 135 volatile float left_val= left_hall.read();
gmiles3 0:1fb00e911fe6 136 volatile float right_val = right_hall.read();
gmiles3 0:1fb00e911fe6 137 while (left_val < enc_dist || left_val < enc_dist) {
gmiles3 0:1fb00e911fe6 138 left_val = left_hall.read();
gmiles3 0:1fb00e911fe6 139 right_val = right_hall.read();
gmiles3 0:1fb00e911fe6 140 diff = left_val - right_val;
gmiles3 0:1fb00e911fe6 141 float k = 0.5;
gmiles3 0:1fb00e911fe6 142 if (diff < 0) { // left has moved less than right
gmiles3 0:1fb00e911fe6 143 left_motor.speed(left_speed);
gmiles3 0:1fb00e911fe6 144 right_motor.speed(k*right_speed);
gmiles3 0:1fb00e911fe6 145 } else if (diff > 0) { // right has moved less than left
gmiles3 0:1fb00e911fe6 146 left_motor.speed(k*left_speed);
gmiles3 0:1fb00e911fe6 147 right_motor.speed(right_speed);
gmiles3 0:1fb00e911fe6 148 } else {
gmiles3 0:1fb00e911fe6 149 left_motor.speed(left_speed);
gmiles3 0:1fb00e911fe6 150 right_motor.speed(right_speed);
gmiles3 0:1fb00e911fe6 151 }
gmiles3 0:1fb00e911fe6 152 }
gmiles3 0:1fb00e911fe6 153 clicksTravelled += left_val;
gmiles3 0:1fb00e911fe6 154 left_motor.speed(0);
gmiles3 0:1fb00e911fe6 155 right_motor.speed(0);
gmiles3 0:1fb00e911fe6 156 }
gmiles3 0:1fb00e911fe6 157
gmiles3 0:1fb00e911fe6 158 void carMoveBackwardDistance(Arguments *in, Reply *out) {
gmiles3 0:1fb00e911fe6 159 int dist = in->getArg<int>();
gmiles3 0:1fb00e911fe6 160 if (dist > 100) {
gmiles3 0:1fb00e911fe6 161 dist = 100;
gmiles3 0:1fb00e911fe6 162 }
gmiles3 0:1fb00e911fe6 163 carResetEncoders();
gmiles3 0:1fb00e911fe6 164 long diff;
gmiles3 0:1fb00e911fe6 165 float enc_dist = dist * encoderFactor;
gmiles3 0:1fb00e911fe6 166 volatile float left_val= left_hall.read();
gmiles3 0:1fb00e911fe6 167 volatile float right_val = right_hall.read();
gmiles3 0:1fb00e911fe6 168 while (left_val < enc_dist || right_val < enc_dist) {
gmiles3 0:1fb00e911fe6 169 left_val = left_hall.read();
gmiles3 0:1fb00e911fe6 170 right_val = right_hall.read();
gmiles3 0:1fb00e911fe6 171 diff = left_val-right_val;
gmiles3 0:1fb00e911fe6 172 float k = 0.8;
gmiles3 0:1fb00e911fe6 173 if (diff < 0) { // left has moved less than right
gmiles3 0:1fb00e911fe6 174 left_motor.speed(-left_speed);
gmiles3 0:1fb00e911fe6 175 right_motor.speed(-k*right_speed);
gmiles3 0:1fb00e911fe6 176 } else if (diff > 0) { // right has moved less than left
gmiles3 0:1fb00e911fe6 177 left_motor.speed(-k*left_speed);
gmiles3 0:1fb00e911fe6 178 right_motor.speed(-right_speed);
gmiles3 0:1fb00e911fe6 179 } else {
gmiles3 0:1fb00e911fe6 180 left_motor.speed(-left_speed);
gmiles3 0:1fb00e911fe6 181 right_motor.speed(-right_speed);
gmiles3 0:1fb00e911fe6 182 }
gmiles3 0:1fb00e911fe6 183 }
gmiles3 0:1fb00e911fe6 184 clicksTravelled += left_val;
gmiles3 0:1fb00e911fe6 185 carStop();
gmiles3 0:1fb00e911fe6 186 }
gmiles3 0:1fb00e911fe6 187
gmiles3 0:1fb00e911fe6 188 void carTurn(Arguments *in, Reply *out) {
gmiles3 0:1fb00e911fe6 189 carResetEncoders();
gmiles3 0:1fb00e911fe6 190 float enc_dist = in->getArg<float>();
gmiles3 0:1fb00e911fe6 191 volatile float left_val= left_hall.read();
gmiles3 0:1fb00e911fe6 192 while (left_val < enc_dist) {
gmiles3 0:1fb00e911fe6 193 left_val = left_hall.read();
gmiles3 0:1fb00e911fe6 194 left_motor.speed(left_speed);
gmiles3 0:1fb00e911fe6 195 right_motor.speed(-right_speed);
gmiles3 0:1fb00e911fe6 196 }
gmiles3 0:1fb00e911fe6 197 carStop();
gmiles3 0:1fb00e911fe6 198 }
gmiles3 0:1fb00e911fe6 199
gmiles3 0:1fb00e911fe6 200 void carTurnRight(Arguments *in, Reply *out) {
gmiles3 0:1fb00e911fe6 201 carResetEncoders();
gmiles3 0:1fb00e911fe6 202 float enc_dist = 170;
gmiles3 0:1fb00e911fe6 203 volatile float left_val= left_hall.read();
gmiles3 0:1fb00e911fe6 204 while (left_val < enc_dist) {
gmiles3 0:1fb00e911fe6 205 left_val = left_hall.read();
gmiles3 0:1fb00e911fe6 206 left_motor.speed(left_speed);
gmiles3 0:1fb00e911fe6 207 right_motor.speed(-right_speed);
gmiles3 0:1fb00e911fe6 208 }
gmiles3 0:1fb00e911fe6 209 carStop();
gmiles3 0:1fb00e911fe6 210 if (out != NULL) out->putData("GOT HERE");
gmiles3 0:1fb00e911fe6 211 }
gmiles3 0:1fb00e911fe6 212
gmiles3 0:1fb00e911fe6 213 void carTurnLeft(Arguments *in, Reply *out) {
gmiles3 0:1fb00e911fe6 214 carResetEncoders();
gmiles3 0:1fb00e911fe6 215 float enc_dist = 170;
gmiles3 0:1fb00e911fe6 216 volatile float left_val= left_hall.read();
gmiles3 0:1fb00e911fe6 217 while (left_val < enc_dist) {
gmiles3 0:1fb00e911fe6 218 left_val = left_hall.read();
gmiles3 0:1fb00e911fe6 219 left_motor.speed(-left_speed);
gmiles3 0:1fb00e911fe6 220 right_motor.speed(right_speed);
gmiles3 0:1fb00e911fe6 221 }
gmiles3 0:1fb00e911fe6 222 carStop();
gmiles3 0:1fb00e911fe6 223 }
gmiles3 0:1fb00e911fe6 224
gmiles3 0:1fb00e911fe6 225
gmiles3 0:1fb00e911fe6 226
gmiles3 0:1fb00e911fe6 227
gmiles3 0:1fb00e911fe6 228
gmiles3 0:1fb00e911fe6 229
gmiles3 0:1fb00e911fe6 230
gmiles3 0:1fb00e911fe6 231
gmiles3 0:1fb00e911fe6 232
gmiles3 0:1fb00e911fe6 233
gmiles3 0:1fb00e911fe6 234 /* WIFI FUNCTIONS */
gmiles3 0:1fb00e911fe6 235
gmiles3 0:1fb00e911fe6 236 void connectToNetwork() {
gmiles3 0:1fb00e911fe6 237 pc.printf("# Resetting ESP\r\n");
gmiles3 0:1fb00e911fe6 238 strcpy(cmdbuff,"node.restart()\r\n");
gmiles3 0:1fb00e911fe6 239 sendCMD();
gmiles3 0:1fb00e911fe6 240 wait(5);
gmiles3 0:1fb00e911fe6 241 getReply();
gmiles3 0:1fb00e911fe6 242
gmiles3 0:1fb00e911fe6 243 led1=1,led2=0,led3=0;
gmiles3 0:1fb00e911fe6 244 pc.printf("# Setting Mode\r\n");
gmiles3 0:1fb00e911fe6 245 strcpy(cmdbuff, "wifi.setmode(wifi.STATION)\r\n");
gmiles3 0:1fb00e911fe6 246 sendCMD();
gmiles3 0:1fb00e911fe6 247 getReply();
gmiles3 0:1fb00e911fe6 248
gmiles3 0:1fb00e911fe6 249 wait(2);
gmiles3 0:1fb00e911fe6 250 led1=0,led2=1,led3=0;
gmiles3 0:1fb00e911fe6 251 pc.printf("# Connecting to AP\r\n");
gmiles3 0:1fb00e911fe6 252 pc.printf("# ssid = %s\t\tpwd = %s\r\n", ssid, pwd);
gmiles3 0:1fb00e911fe6 253 strcpy(cmdbuff, "wifi.sta.config(\"");
gmiles3 0:1fb00e911fe6 254 strcat(cmdbuff, ssid);
gmiles3 0:1fb00e911fe6 255 strcat(cmdbuff, "\",\"");
gmiles3 0:1fb00e911fe6 256 strcat(cmdbuff, pwd);
gmiles3 0:1fb00e911fe6 257 strcat(cmdbuff, "\")\r\n");
gmiles3 0:1fb00e911fe6 258 sendCMD();
gmiles3 0:1fb00e911fe6 259 getReply();
gmiles3 0:1fb00e911fe6 260
gmiles3 0:1fb00e911fe6 261 wait(2);
gmiles3 0:1fb00e911fe6 262 led1=0,led2=0,led3=1;
gmiles3 0:1fb00e911fe6 263 pc.printf("# Get IP Address\r\n");
gmiles3 0:1fb00e911fe6 264 strcpy(cmdbuff, "print(wifi.sta.getip())\r\n");
gmiles3 0:1fb00e911fe6 265 sendCMD();
gmiles3 0:1fb00e911fe6 266 getReply();
gmiles3 0:1fb00e911fe6 267
gmiles3 0:1fb00e911fe6 268 wait(2);
gmiles3 0:1fb00e911fe6 269 led1=1,led2=0,led3=0;
gmiles3 0:1fb00e911fe6 270 pc.printf("# Get Connection Status\r\n");
gmiles3 0:1fb00e911fe6 271 strcpy(cmdbuff, "print(wifi.sta.status())\r\n");
gmiles3 0:1fb00e911fe6 272 sendCMD();
gmiles3 0:1fb00e911fe6 273 getReply();
gmiles3 0:1fb00e911fe6 274
gmiles3 0:1fb00e911fe6 275 wait(2);
gmiles3 0:1fb00e911fe6 276 led1=0,led2=1,led3=0;
gmiles3 0:1fb00e911fe6 277 pc.printf("# Listen on Port\r\n");
gmiles3 0:1fb00e911fe6 278 strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
gmiles3 0:1fb00e911fe6 279 strcat(cmdbuff, timeout);
gmiles3 0:1fb00e911fe6 280 strcat(cmdbuff, ")\r\n");
gmiles3 0:1fb00e911fe6 281 sendCMD();
gmiles3 0:1fb00e911fe6 282 getReply();
gmiles3 0:1fb00e911fe6 283
gmiles3 0:1fb00e911fe6 284 wait(2);
gmiles3 0:1fb00e911fe6 285 led1=0,led2=0,led3=1;
gmiles3 0:1fb00e911fe6 286 strcpy(cmdbuff, "srv:listen(");
gmiles3 0:1fb00e911fe6 287 strcat(cmdbuff, port);
gmiles3 0:1fb00e911fe6 288 strcat(cmdbuff, ",function(conn)\r\n");
gmiles3 0:1fb00e911fe6 289 sendCMD();
gmiles3 0:1fb00e911fe6 290 getReply();
gmiles3 0:1fb00e911fe6 291
gmiles3 0:1fb00e911fe6 292 wait(2);
gmiles3 0:1fb00e911fe6 293 led1=1,led2=0,led3=0;
gmiles3 0:1fb00e911fe6 294 strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
gmiles3 0:1fb00e911fe6 295 sendCMD();
gmiles3 0:1fb00e911fe6 296 getReply();
gmiles3 0:1fb00e911fe6 297
gmiles3 0:1fb00e911fe6 298 wait(2);
gmiles3 0:1fb00e911fe6 299 led1=0,led2=1,led3=0;
gmiles3 0:1fb00e911fe6 300 strcpy(cmdbuff, "conn:send('");
gmiles3 0:1fb00e911fe6 301 strcat(cmdbuff, reportStatus());
gmiles3 0:1fb00e911fe6 302 strcat(cmdbuff, "')\r\n");
gmiles3 0:1fb00e911fe6 303 sendCMD();
gmiles3 0:1fb00e911fe6 304 getReply();
gmiles3 0:1fb00e911fe6 305
gmiles3 0:1fb00e911fe6 306 wait(2);
gmiles3 0:1fb00e911fe6 307 led1=0,led2=0,led3=1;
gmiles3 0:1fb00e911fe6 308 strcpy(cmdbuff, "print(payload)\r\n");
gmiles3 0:1fb00e911fe6 309 sendCMD();
gmiles3 0:1fb00e911fe6 310 getReply();
gmiles3 0:1fb00e911fe6 311
gmiles3 0:1fb00e911fe6 312 wait(2);
gmiles3 0:1fb00e911fe6 313 led1=1,led2=0,led3=0;
gmiles3 0:1fb00e911fe6 314 strcpy(cmdbuff, "end)\r\n");
gmiles3 0:1fb00e911fe6 315 sendCMD();
gmiles3 0:1fb00e911fe6 316 getReply();
gmiles3 0:1fb00e911fe6 317
gmiles3 0:1fb00e911fe6 318 wait(2);
gmiles3 0:1fb00e911fe6 319 led1=0,led2=1,led3=0;
gmiles3 0:1fb00e911fe6 320 strcpy(cmdbuff, "end)\r\n");
gmiles3 0:1fb00e911fe6 321 sendCMD();
gmiles3 0:1fb00e911fe6 322 getReply();
gmiles3 0:1fb00e911fe6 323
gmiles3 0:1fb00e911fe6 324 wait(2);
gmiles3 0:1fb00e911fe6 325 //led1=1,led2=1,led3=1;
gmiles3 0:1fb00e911fe6 326 led1=0,led2=0,led3=0;
gmiles3 0:1fb00e911fe6 327 pc.printf("# Ready\r\n");
gmiles3 0:1fb00e911fe6 328 }
gmiles3 0:1fb00e911fe6 329
gmiles3 0:1fb00e911fe6 330 void sendCMD()
gmiles3 0:1fb00e911fe6 331 {
gmiles3 0:1fb00e911fe6 332 int i;
gmiles3 0:1fb00e911fe6 333 char temp_char;
gmiles3 0:1fb00e911fe6 334 bool empty;
gmiles3 0:1fb00e911fe6 335 i = 0;
gmiles3 0:1fb00e911fe6 336 // Start Critical Section - don't interrupt while changing global buffer variables
gmiles3 0:1fb00e911fe6 337 NVIC_DisableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 338 empty = (tx_in == tx_out);
gmiles3 0:1fb00e911fe6 339 while ((i==0) || (cmdbuff[i-1] != '\n')) {
gmiles3 0:1fb00e911fe6 340 // Wait if buffer full
gmiles3 0:1fb00e911fe6 341 if (((tx_in + 1) % buffer_size) == tx_out) {
gmiles3 0:1fb00e911fe6 342 // End Critical Section - need to let interrupt routine empty buffer by sending
gmiles3 0:1fb00e911fe6 343 NVIC_EnableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 344 while (((tx_in + 1) % buffer_size) == tx_out) {
gmiles3 0:1fb00e911fe6 345 }
gmiles3 0:1fb00e911fe6 346 // Start Critical Section - don't interrupt while changing global buffer variables
gmiles3 0:1fb00e911fe6 347 NVIC_DisableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 348 }
gmiles3 0:1fb00e911fe6 349 tx_buffer[tx_in] = cmdbuff[i];
gmiles3 0:1fb00e911fe6 350 i++;
gmiles3 0:1fb00e911fe6 351 tx_in = (tx_in + 1) % buffer_size;
gmiles3 0:1fb00e911fe6 352 }
gmiles3 0:1fb00e911fe6 353 if (esp.writeable() && (empty)) {
gmiles3 0:1fb00e911fe6 354 temp_char = tx_buffer[tx_out];
gmiles3 0:1fb00e911fe6 355 tx_out = (tx_out + 1) % buffer_size;
gmiles3 0:1fb00e911fe6 356 // Send first character to start tx interrupts, if stopped
gmiles3 0:1fb00e911fe6 357 esp.putc(temp_char);
gmiles3 0:1fb00e911fe6 358 }
gmiles3 0:1fb00e911fe6 359 // End Critical Section
gmiles3 0:1fb00e911fe6 360 NVIC_EnableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 361 return;
gmiles3 0:1fb00e911fe6 362 }
gmiles3 0:1fb00e911fe6 363
gmiles3 0:1fb00e911fe6 364 // Read a line from the large rx buffer from rx interrupt routine
gmiles3 0:1fb00e911fe6 365 void getReply() {
gmiles3 0:1fb00e911fe6 366 int i;
gmiles3 0:1fb00e911fe6 367 i = 0;
gmiles3 0:1fb00e911fe6 368 // Start Critical Section - don't interrupt while changing global buffer variables
gmiles3 0:1fb00e911fe6 369 NVIC_DisableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 370 // Loop reading rx buffer characters until end of line character
gmiles3 0:1fb00e911fe6 371 while ((i==0) || (rx_line[i-1] != '\r')) {
gmiles3 0:1fb00e911fe6 372 // Wait if buffer empty
gmiles3 0:1fb00e911fe6 373 if (rx_in == rx_out) {
gmiles3 0:1fb00e911fe6 374 // End Critical Section - need to allow rx interrupt to get new characters for buffer
gmiles3 0:1fb00e911fe6 375 NVIC_EnableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 376 while (rx_in == rx_out) {
gmiles3 0:1fb00e911fe6 377 }
gmiles3 0:1fb00e911fe6 378 // Start Critical Section - don't interrupt while changing global buffer variables
gmiles3 0:1fb00e911fe6 379 NVIC_DisableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 380 }
gmiles3 0:1fb00e911fe6 381 rx_line[i] = rx_buffer[rx_out];
gmiles3 0:1fb00e911fe6 382 i++;
gmiles3 0:1fb00e911fe6 383 rx_out = (rx_out + 1) % buffer_size;
gmiles3 0:1fb00e911fe6 384 }
gmiles3 0:1fb00e911fe6 385 // End Critical Section
gmiles3 0:1fb00e911fe6 386 NVIC_EnableIRQ(UART1_IRQn);
gmiles3 0:1fb00e911fe6 387 rx_line[i-1] = 0;
gmiles3 0:1fb00e911fe6 388 return;
gmiles3 0:1fb00e911fe6 389 }
gmiles3 0:1fb00e911fe6 390
gmiles3 0:1fb00e911fe6 391 char* reportStatus() {
gmiles3 0:1fb00e911fe6 392 char str[30];
gmiles3 0:1fb00e911fe6 393 float inchesTravelled = clicksTravelled / encoderFactor ;
gmiles3 0:1fb00e911fe6 394 sprintf(str, "%.1f", inchesTravelled);
gmiles3 0:1fb00e911fe6 395 return str;
gmiles3 0:1fb00e911fe6 396 }
gmiles3 0:1fb00e911fe6 397
gmiles3 0:1fb00e911fe6 398 // Interupt Routine to read in data from serial port
gmiles3 0:1fb00e911fe6 399 void Rx_interrupt() {
gmiles3 0:1fb00e911fe6 400 //led3=1;
gmiles3 0:1fb00e911fe6 401 // Loop just in case more than one character is in UART's receive FIFO buffer
gmiles3 0:1fb00e911fe6 402 // Stop if buffer full
gmiles3 0:1fb00e911fe6 403 while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
gmiles3 0:1fb00e911fe6 404 rx_buffer[rx_in] = esp.getc();
gmiles3 0:1fb00e911fe6 405 // Uncomment to Echo to USB serial to watch data flow
gmiles3 0:1fb00e911fe6 406 pc.putc(rx_buffer[rx_in]);
gmiles3 0:1fb00e911fe6 407 rx_in = (rx_in + 1) % buffer_size;
gmiles3 0:1fb00e911fe6 408 }
gmiles3 0:1fb00e911fe6 409 return;
gmiles3 0:1fb00e911fe6 410 }
gmiles3 0:1fb00e911fe6 411
gmiles3 0:1fb00e911fe6 412
gmiles3 0:1fb00e911fe6 413 // Interupt Routine to write out data to serial port
gmiles3 0:1fb00e911fe6 414 void Tx_interrupt() {
gmiles3 0:1fb00e911fe6 415 //led2=1;
gmiles3 0:1fb00e911fe6 416 // Loop to fill more than one character in UART's transmit FIFO buffer
gmiles3 0:1fb00e911fe6 417 // Stop if buffer empty
gmiles3 0:1fb00e911fe6 418 while ((esp.writeable()) && (tx_in != tx_out)) {
gmiles3 0:1fb00e911fe6 419 esp.putc(tx_buffer[tx_out]);
gmiles3 0:1fb00e911fe6 420 tx_out = (tx_out + 1) % buffer_size;
gmiles3 0:1fb00e911fe6 421 }
gmiles3 0:1fb00e911fe6 422 //led2=0;
gmiles3 0:1fb00e911fe6 423 return;
gmiles3 0:1fb00e911fe6 424 }