main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "TextLCD.h"
00004 #include "rtos.h"
00005 #include "time.h"
00006 #include "Motor.h"
00007 #include "WiflyInterface.h"
00008 
00009 Mutex lock;
00010 WiflyInterface wireless(p9, p10, p19, p20, "mbed", "123456", WPA);
00011 Serial pc(USBTX, USBRX);
00012 Motor m1(p21, p7, p8);
00013 Motor m2(p22, p5, p6); // pwm, fwd, rev
00014 DigitalOut led1(LED1);
00015 DigitalOut led2(LED2);
00016 DigitalOut led4(LED4);
00017 DigitalOut led3(LED3);
00018 TextLCD lcd(p30, p29, p28, p27, p26, p25); // rs, e, d4-d7
00019 
00020 float left_motor = 0.0;
00021 float right_motor = 0.0;
00022 void ether_f(void const *args);
00023 void motor_f();
00024 void time_disp(void const *args);
00025 int main()
00026 {
00027     Thread t1(time_disp);
00028     Thread t2(ether_f);
00029     while(1) {
00030         motor_f();
00031         Thread::wait(250);
00032     }
00033 }
00034 void time_disp(void const *args)
00035 {
00036     while(true) {
00037         time_t temp = time(NULL);
00038         lock.lock();
00039         lcd.cls();
00040         lcd.printf("%s\n", ctime(&temp));
00041         lock.unlock();
00042         Thread::wait(4000);
00043 
00044         lock.lock();
00045         lcd.cls();
00046         lcd.printf("%s\n", wireless.getIPAddress());
00047         lock.unlock();
00048         Thread::wait(4000);
00049     }
00050 }
00051 
00052 void motor_f()
00053 {
00054     lock.lock();
00055     lcd.cls();
00056     lcd.printf("Testing Motor\n");
00057     lock.unlock();
00058     while (1) {
00059         m2.speed(left_motor);
00060         m1.speed(right_motor);
00061     }
00062 }
00063 
00064 void ether_f(void const *args)
00065 {
00066     pc.printf("Testing Internet Connection!\n");
00067     led1 = 1;
00068     wireless.init();
00069     while (!wireless.connect()); // join the network
00070     led2 = 1;
00071     pc.printf("IP Address is %s\n\r", wireless.getIPAddress());
00072     /* Code to Update RTC on mbed */
00073     /*
00074     UDPSocket sock;
00075     sock.init();
00076     Endpoint nist;
00077     nist.set_address("time-a.nist.gov", 37);
00078     char out_buffer[] = "plop"; // Does not matter
00079     sock.sendTo(nist, out_buffer, sizeof(out_buffer));
00080 
00081     char in_buffer[4];
00082     int n = sock.receiveFrom(nist, in_buffer, sizeof(in_buffer));
00083 
00084     unsigned int timeRes = ntohl( *((unsigned int*)in_buffer));
00085     set_time(timeRes - 2208988800U - 4*3600);
00086     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);
00087     time_t temp = time(NULL);
00088     lcd.cls();
00089     lcd.printf("%s\n", ctime(&temp));
00090     sock.close();
00091     */
00092     UDPSocket server;
00093     server.bind(7);
00094 
00095     Endpoint client;
00096     char buffer[256];
00097     while (true) {
00098         int n = server.receiveFrom(client, buffer, sizeof(buffer));
00099         lock.lock();
00100         lcd.cls();
00101         lcd.printf("Received from:\n %s", client.get_address());
00102         lock.unlock();
00103         buffer[5] = NULL;
00104         pc.printf("%s\n", buffer);
00105         if (buffer[0] == '1') led1 = 1;
00106         else led1 = 0;
00107         if (buffer[1] == '1') led2 = 1;
00108         else led2 = 0;
00109         if (buffer[2] == '1') led3 = 1;
00110         else led3 = 0;
00111         if (buffer[3] == '1') led4 = 1;
00112         else led4 = 0;
00113 
00114         if (buffer[4] == '+') {
00115             left_motor += 0.1;
00116             if (left_motor > 1) left_motor = 1;
00117         } else if (buffer[4] == '-') {
00118             left_motor -= 0.1;
00119             if (left_motor < -1) left_motor = -1;
00120         } else if (buffer[4] == 'B') {
00121             left_motor = 0;
00122         } else if (buffer[4] == 'F') {
00123             if (left_motor < 0) left_motor = -1;
00124             else left_motor = 1;
00125         }
00126 
00127 
00128         if (buffer[4] == '+') {
00129             right_motor += 0.1;
00130             if (right_motor > 1) right_motor = 1;
00131         } else if (buffer[4] == '-') {
00132             right_motor -= 0.1;
00133             if (right_motor < -1) right_motor = -1;
00134         } else if (buffer[4] == 'B') {
00135             right_motor = 0;
00136         } else if (buffer[4] == 'F') {
00137             if (right_motor < 0) right_motor = -1;
00138             else right_motor = 1;
00139         }
00140 
00141         if (buffer[4] == 'L') {
00142             right_motor += 0.05;
00143             left_motor -= 0.05;
00144             if (right_motor > 1) right_motor = 1;
00145             if (left_motor < -1) left_motor = -1;
00146         }
00147 
00148         if (buffer[4] == 'R') {
00149             right_motor -= 0.05;
00150             left_motor += 0.05;
00151             if (left_motor > 1) left_motor = 1;
00152             if (right_motor < -1) right_motor = -1;
00153         }
00154         if (buffer[4] == 'S') {
00155             float temp = (right_motor + left_motor)/2;
00156             right_motor = temp;
00157             left_motor = temp;
00158         }
00159     }
00160     wireless.disconnect();
00161 }