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 "EthernetInterface.h"
00008 Mutex lock;
00009 
00010 EthernetInterface eth;
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 
00021 float left_motor = 0.0;
00022 float right_motor = 0.0;
00023 void ether_f(void const *args);
00024 void motor_f();
00025 void time_disp(void const *args);
00026 int main()
00027 {
00028     Thread t1(time_disp);
00029     Thread t2(ether_f);
00030     while(1) {
00031         motor_f();
00032         Thread::wait(250);
00033     }
00034 }
00035 
00036 void time_disp(void const *args)
00037 {
00038     while(true) {
00039         time_t temp = time(NULL);
00040         lock.lock();
00041         lcd.cls();
00042         lcd.printf("%s\n", ctime(&temp));
00043         lock.unlock();
00044         Thread::wait(4000);
00045 
00046         lock.lock();
00047         lcd.cls();
00048         lcd.printf("%s\n", eth.getIPAddress());
00049         lock.unlock();
00050         Thread::wait(4000);
00051     }
00052 }
00053 
00054 void motor_f()
00055 {
00056     lock.lock();
00057     lcd.cls();
00058     lcd.printf("Testing Motor\n");
00059     lock.unlock();
00060     while (1) {
00061         m2.speed(left_motor);
00062         m1.speed(right_motor);
00063     }
00064 }
00065 
00066 void ether_f(void const *args)
00067 {
00068     pc.printf("Testing Internet Connection!\n");
00069     led1 = 1;
00070     eth.init(); //Use DHCP
00071     Thread::wait(1000);
00072     pc.printf("MAC: %s\n", eth.getMACAddress());
00073     led2 = 1;
00074     eth.connect(30000);
00075     led1 = !led1;
00076     led2 = !led2;
00077     pc.printf("IP: %s\n", eth.getIPAddress());
00078     /* Code to Update RTC on mbed */
00079     /*
00080     UDPSocket sock;
00081     sock.init();
00082 
00083     Endpoint nist;
00084     nist.set_address("time-a.nist.gov", 37);
00085 
00086     char out_buffer[] = "plop"; // Does not matter
00087     sock.sendTo(nist, out_buffer, sizeof(out_buffer));
00088 
00089     char in_buffer[4];
00090     int n = sock.receiveFrom(nist, in_buffer, sizeof(in_buffer));
00091 
00092     unsigned int timeRes = ntohl( *((unsigned int*)in_buffer));
00093     set_time(timeRes - 2208988800U - 4*3600);
00094     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);
00095     time_t temp = time(NULL);
00096     lcd.cls();
00097     lcd.printf("%s\n", ctime(&temp));
00098     sock.close();
00099     */
00100     UDPSocket server;
00101     server.bind(7);
00102 
00103     Endpoint client;
00104     char buffer[256];
00105     while (true) {
00106         int n = server.receiveFrom(client, buffer, sizeof(buffer));
00107         lock.lock();
00108         lcd.cls();
00109         lcd.printf("Received from:\n %s", client.get_address());
00110         lock.unlock();
00111         buffer[5] = NULL;
00112         pc.printf("%s\n", buffer);
00113         if (buffer[0] == '1') led1 = 1;
00114         else led1 = 0;
00115         if (buffer[1] == '1') led2 = 1;
00116         else led2 = 0;
00117         if (buffer[2] == '1') led3 = 1;
00118         else led3 = 0;
00119         if (buffer[3] == '1') led4 = 1;
00120         else led4 = 0;
00121 
00122         if (buffer[4] == '+') {
00123             left_motor += 0.1;
00124             if (left_motor > 1) left_motor = 1;
00125         } else if (buffer[4] == '-') {
00126             left_motor -= 0.1;
00127             if (left_motor < -1) left_motor = -1;
00128         } else if (buffer[4] == 'B') {
00129             left_motor = 0;
00130         } else if (buffer[4] == 'F') {
00131             if (left_motor < 0) left_motor = -1;
00132             else left_motor = 1;
00133         }
00134 
00135 
00136         if (buffer[4] == '+') {
00137             right_motor += 0.1;
00138             if (right_motor > 1) right_motor = 1;
00139         } else if (buffer[4] == '-') {
00140             right_motor -= 0.1;
00141             if (right_motor < -1) right_motor = -1;
00142         } else if (buffer[4] == 'B') {
00143             right_motor = 0;
00144         } else if (buffer[4] == 'F') {
00145             if (right_motor < 0) right_motor = -1;
00146             else right_motor = 1;
00147         }
00148 
00149         if (buffer[4] == 'L') {
00150             right_motor += 0.05;
00151             left_motor -= 0.05;
00152             if (right_motor > 1) right_motor = 1;
00153             if (left_motor < -1) left_motor = -1;
00154         }
00155 
00156         if (buffer[4] == 'R') {
00157             right_motor -= 0.05;
00158             left_motor += 0.05;
00159             if (left_motor > 1) left_motor = 1;
00160             if (right_motor < -1) right_motor = -1;
00161         }
00162         if (buffer[4] == 'S') {
00163             float temp = (right_motor + left_motor)/2;
00164             right_motor = temp;
00165             left_motor = temp;
00166         }
00167     }
00168     eth.disconnect();
00169 
00170 }