Ankit Patel / Mbed 2 deprecated Robot_Control_UDP_Wifi

Control a robot over the internet using UDP and a Wifly module (WiFi).

Dependencies:   Motor TextLCD WiflyInterface mbed-rtos mbed

main.cpp@0:c0dc3a76f3d4, 2013-10-17 (annotated)

Committer:
apatel336
Date:
Thu Oct 17 13:27:56 2013 +0000
Revision:
0:c0dc3a76f3d4
Initial Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apatel336 0:c0dc3a76f3d4 1 #include "mbed.h"
apatel336 0:c0dc3a76f3d4 2 #include "math.h"
apatel336 0:c0dc3a76f3d4 3 #include "TextLCD.h"
apatel336 0:c0dc3a76f3d4 4 #include "rtos.h"
apatel336 0:c0dc3a76f3d4 5 #include "time.h"
apatel336 0:c0dc3a76f3d4 6 #include "Motor.h"
apatel336 0:c0dc3a76f3d4 7 #include "WiflyInterface.h"
apatel336 0:c0dc3a76f3d4 8
apatel336 0:c0dc3a76f3d4 9 Mutex lock;
apatel336 0:c0dc3a76f3d4 10 WiflyInterface wireless(p9, p10, p19, p20, "mbed", "123456", WPA);
apatel336 0:c0dc3a76f3d4 11 Serial pc(USBTX, USBRX);
apatel336 0:c0dc3a76f3d4 12 Motor m1(p21, p7, p8);
apatel336 0:c0dc3a76f3d4 13 Motor m2(p22, p5, p6); // pwm, fwd, rev
apatel336 0:c0dc3a76f3d4 14 DigitalOut led1(LED1);
apatel336 0:c0dc3a76f3d4 15 DigitalOut led2(LED2);
apatel336 0:c0dc3a76f3d4 16 DigitalOut led4(LED4);
apatel336 0:c0dc3a76f3d4 17 DigitalOut led3(LED3);
apatel336 0:c0dc3a76f3d4 18 TextLCD lcd(p30, p29, p28, p27, p26, p25); // rs, e, d4-d7
apatel336 0:c0dc3a76f3d4 19
apatel336 0:c0dc3a76f3d4 20 float left_motor = 0.0;
apatel336 0:c0dc3a76f3d4 21 float right_motor = 0.0;
apatel336 0:c0dc3a76f3d4 22 void ether_f(void const *args);
apatel336 0:c0dc3a76f3d4 23 void motor_f();
apatel336 0:c0dc3a76f3d4 24 void time_disp(void const *args);
apatel336 0:c0dc3a76f3d4 25 int main()
apatel336 0:c0dc3a76f3d4 26 {
apatel336 0:c0dc3a76f3d4 27 Thread t1(time_disp);
apatel336 0:c0dc3a76f3d4 28 Thread t2(ether_f);
apatel336 0:c0dc3a76f3d4 29 while(1) {
apatel336 0:c0dc3a76f3d4 30 motor_f();
apatel336 0:c0dc3a76f3d4 31 Thread::wait(250);
apatel336 0:c0dc3a76f3d4 32 }
apatel336 0:c0dc3a76f3d4 33 }
apatel336 0:c0dc3a76f3d4 34 void time_disp(void const *args)
apatel336 0:c0dc3a76f3d4 35 {
apatel336 0:c0dc3a76f3d4 36 while(true) {
apatel336 0:c0dc3a76f3d4 37 time_t temp = time(NULL);
apatel336 0:c0dc3a76f3d4 38 lock.lock();
apatel336 0:c0dc3a76f3d4 39 lcd.cls();
apatel336 0:c0dc3a76f3d4 40 lcd.printf("%s\n", ctime(&temp));
apatel336 0:c0dc3a76f3d4 41 lock.unlock();
apatel336 0:c0dc3a76f3d4 42 Thread::wait(4000);
apatel336 0:c0dc3a76f3d4 43
apatel336 0:c0dc3a76f3d4 44 lock.lock();
apatel336 0:c0dc3a76f3d4 45 lcd.cls();
apatel336 0:c0dc3a76f3d4 46 lcd.printf("%s\n", wireless.getIPAddress());
apatel336 0:c0dc3a76f3d4 47 lock.unlock();
apatel336 0:c0dc3a76f3d4 48 Thread::wait(4000);
apatel336 0:c0dc3a76f3d4 49 }
apatel336 0:c0dc3a76f3d4 50 }
apatel336 0:c0dc3a76f3d4 51
apatel336 0:c0dc3a76f3d4 52 void motor_f()
apatel336 0:c0dc3a76f3d4 53 {
apatel336 0:c0dc3a76f3d4 54 lock.lock();
apatel336 0:c0dc3a76f3d4 55 lcd.cls();
apatel336 0:c0dc3a76f3d4 56 lcd.printf("Testing Motor\n");
apatel336 0:c0dc3a76f3d4 57 lock.unlock();
apatel336 0:c0dc3a76f3d4 58 while (1) {
apatel336 0:c0dc3a76f3d4 59 m2.speed(left_motor);
apatel336 0:c0dc3a76f3d4 60 m1.speed(right_motor);
apatel336 0:c0dc3a76f3d4 61 }
apatel336 0:c0dc3a76f3d4 62 }
apatel336 0:c0dc3a76f3d4 63
apatel336 0:c0dc3a76f3d4 64 void ether_f(void const *args)
apatel336 0:c0dc3a76f3d4 65 {
apatel336 0:c0dc3a76f3d4 66 pc.printf("Testing Internet Connection!\n");
apatel336 0:c0dc3a76f3d4 67 led1 = 1;
apatel336 0:c0dc3a76f3d4 68 wireless.init();
apatel336 0:c0dc3a76f3d4 69 while (!wireless.connect()); // join the network
apatel336 0:c0dc3a76f3d4 70 led2 = 1;
apatel336 0:c0dc3a76f3d4 71 pc.printf("IP Address is %s\n\r", wireless.getIPAddress());
apatel336 0:c0dc3a76f3d4 72 /* Code to Update RTC on mbed */
apatel336 0:c0dc3a76f3d4 73 /*
apatel336 0:c0dc3a76f3d4 74 UDPSocket sock;
apatel336 0:c0dc3a76f3d4 75 sock.init();
apatel336 0:c0dc3a76f3d4 76 Endpoint nist;
apatel336 0:c0dc3a76f3d4 77 nist.set_address("time-a.nist.gov", 37);
apatel336 0:c0dc3a76f3d4 78 char out_buffer[] = "plop"; // Does not matter
apatel336 0:c0dc3a76f3d4 79 sock.sendTo(nist, out_buffer, sizeof(out_buffer));
apatel336 0:c0dc3a76f3d4 80
apatel336 0:c0dc3a76f3d4 81 char in_buffer[4];
apatel336 0:c0dc3a76f3d4 82 int n = sock.receiveFrom(nist, in_buffer, sizeof(in_buffer));
apatel336 0:c0dc3a76f3d4 83
apatel336 0:c0dc3a76f3d4 84 unsigned int timeRes = ntohl( *((unsigned int*)in_buffer));
apatel336 0:c0dc3a76f3d4 85 set_time(timeRes - 2208988800U - 4*3600);
apatel336 0:c0dc3a76f3d4 86 pc.printf("Received %d bytes from server %s on port %d: %u seconds since 1/01/1900 00:00 GMT\n", n, nist.get_address(), nist.get_port(), timeRes);
apatel336 0:c0dc3a76f3d4 87 time_t temp = time(NULL);
apatel336 0:c0dc3a76f3d4 88 lcd.cls();
apatel336 0:c0dc3a76f3d4 89 lcd.printf("%s\n", ctime(&temp));
apatel336 0:c0dc3a76f3d4 90 sock.close();
apatel336 0:c0dc3a76f3d4 91 */
apatel336 0:c0dc3a76f3d4 92 UDPSocket server;
apatel336 0:c0dc3a76f3d4 93 server.bind(7);
apatel336 0:c0dc3a76f3d4 94
apatel336 0:c0dc3a76f3d4 95 Endpoint client;
apatel336 0:c0dc3a76f3d4 96 char buffer[256];
apatel336 0:c0dc3a76f3d4 97 while (true) {
apatel336 0:c0dc3a76f3d4 98 int n = server.receiveFrom(client, buffer, sizeof(buffer));
apatel336 0:c0dc3a76f3d4 99 lock.lock();
apatel336 0:c0dc3a76f3d4 100 lcd.cls();
apatel336 0:c0dc3a76f3d4 101 lcd.printf("Received from:\n %s", client.get_address());
apatel336 0:c0dc3a76f3d4 102 lock.unlock();
apatel336 0:c0dc3a76f3d4 103 buffer[5] = NULL;
apatel336 0:c0dc3a76f3d4 104 pc.printf("%s\n", buffer);
apatel336 0:c0dc3a76f3d4 105 if (buffer[0] == '1') led1 = 1;
apatel336 0:c0dc3a76f3d4 106 else led1 = 0;
apatel336 0:c0dc3a76f3d4 107 if (buffer[1] == '1') led2 = 1;
apatel336 0:c0dc3a76f3d4 108 else led2 = 0;
apatel336 0:c0dc3a76f3d4 109 if (buffer[2] == '1') led3 = 1;
apatel336 0:c0dc3a76f3d4 110 else led3 = 0;
apatel336 0:c0dc3a76f3d4 111 if (buffer[3] == '1') led4 = 1;
apatel336 0:c0dc3a76f3d4 112 else led4 = 0;
apatel336 0:c0dc3a76f3d4 113
apatel336 0:c0dc3a76f3d4 114 if (buffer[4] == '+') {
apatel336 0:c0dc3a76f3d4 115 left_motor += 0.1;
apatel336 0:c0dc3a76f3d4 116 if (left_motor > 1) left_motor = 1;
apatel336 0:c0dc3a76f3d4 117 } else if (buffer[4] == '-') {
apatel336 0:c0dc3a76f3d4 118 left_motor -= 0.1;
apatel336 0:c0dc3a76f3d4 119 if (left_motor < -1) left_motor = -1;
apatel336 0:c0dc3a76f3d4 120 } else if (buffer[4] == 'B') {
apatel336 0:c0dc3a76f3d4 121 left_motor = 0;
apatel336 0:c0dc3a76f3d4 122 } else if (buffer[4] == 'F') {
apatel336 0:c0dc3a76f3d4 123 if (left_motor < 0) left_motor = -1;
apatel336 0:c0dc3a76f3d4 124 else left_motor = 1;
apatel336 0:c0dc3a76f3d4 125 }
apatel336 0:c0dc3a76f3d4 126
apatel336 0:c0dc3a76f3d4 127
apatel336 0:c0dc3a76f3d4 128 if (buffer[4] == '+') {
apatel336 0:c0dc3a76f3d4 129 right_motor += 0.1;
apatel336 0:c0dc3a76f3d4 130 if (right_motor > 1) right_motor = 1;
apatel336 0:c0dc3a76f3d4 131 } else if (buffer[4] == '-') {
apatel336 0:c0dc3a76f3d4 132 right_motor -= 0.1;
apatel336 0:c0dc3a76f3d4 133 if (right_motor < -1) right_motor = -1;
apatel336 0:c0dc3a76f3d4 134 } else if (buffer[4] == 'B') {
apatel336 0:c0dc3a76f3d4 135 right_motor = 0;
apatel336 0:c0dc3a76f3d4 136 } else if (buffer[4] == 'F') {
apatel336 0:c0dc3a76f3d4 137 if (right_motor < 0) right_motor = -1;
apatel336 0:c0dc3a76f3d4 138 else right_motor = 1;
apatel336 0:c0dc3a76f3d4 139 }
apatel336 0:c0dc3a76f3d4 140
apatel336 0:c0dc3a76f3d4 141 if (buffer[4] == 'L') {
apatel336 0:c0dc3a76f3d4 142 right_motor += 0.05;
apatel336 0:c0dc3a76f3d4 143 left_motor -= 0.05;
apatel336 0:c0dc3a76f3d4 144 if (right_motor > 1) right_motor = 1;
apatel336 0:c0dc3a76f3d4 145 if (left_motor < -1) left_motor = -1;
apatel336 0:c0dc3a76f3d4 146 }
apatel336 0:c0dc3a76f3d4 147
apatel336 0:c0dc3a76f3d4 148 if (buffer[4] == 'R') {
apatel336 0:c0dc3a76f3d4 149 right_motor -= 0.05;
apatel336 0:c0dc3a76f3d4 150 left_motor += 0.05;
apatel336 0:c0dc3a76f3d4 151 if (left_motor > 1) left_motor = 1;
apatel336 0:c0dc3a76f3d4 152 if (right_motor < -1) right_motor = -1;
apatel336 0:c0dc3a76f3d4 153 }
apatel336 0:c0dc3a76f3d4 154 if (buffer[4] == 'S') {
apatel336 0:c0dc3a76f3d4 155 float temp = (right_motor + left_motor)/2;
apatel336 0:c0dc3a76f3d4 156 right_motor = temp;
apatel336 0:c0dc3a76f3d4 157 left_motor = temp;
apatel336 0:c0dc3a76f3d4 158 }
apatel336 0:c0dc3a76f3d4 159 }
apatel336 0:c0dc3a76f3d4 160 wireless.disconnect();
apatel336 0:c0dc3a76f3d4 161 }