Diff: main.cpp

Revision:

0:8101522bb770

Child:

2:6383a282c70c

Child:

3:9844ec849c12

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jan 29 18:24:44 2013 +0000
@@ -0,0 +1,133 @@
+#include "mbed.h"
+#include "EthernetNetIf.h"
+#include "UDPSocket.h"
+#include "queue.h"
+#include "Servo.h"
+
+Ticker NetPoll;
+Timer ErrorTime;
+Queue UDP_queue(256,20);
+Serial pc(USBTX, USBRX);
+EthernetNetIf * eth;
+UDPSocket UDP;
+Host Robot;
+Servo ML(p21);
+Servo MR(p22);
+
+DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)};
+
+char messageBufferIncoming[256];
+
+struct Joystick {
+    float axis[4];
+    float button[12];
+};
+
+
+Joystick Joy;
+void messageProcess(void)
+{
+
+    ErrorTime.reset();
+    // pc.printf("%s\r\n",messageBufferIncoming);
+    sscanf(messageBufferIncoming,"%f%f%f%f",\
+           &Joy.axis[0],\
+           &Joy.axis[1],\
+           &Joy.axis[2],\
+           &Joy.axis[3]);
+
+    pc.printf("%f %f %f %f\r\n", Joy.axis[0],Joy.axis[1],Joy.axis[2],Joy.axis[3]);
+    /*
+    sscanf(messageBufferIncoming,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f",\
+    &Joy.axis[0],\
+    &Joy.axis[1],\
+    &Joy.axis[2],\
+    &Joy.axis[3],\
+    &Joy.button[0],\
+    &Joy.button[1],\
+    &Joy.button[2],\
+    &Joy.button[3],\
+    &Joy.button[4],\
+    &Joy.button[5],\
+    &Joy.button[6],\
+    &Joy.button[7],\
+    &Joy.button[8],\
+    &Joy.button[9],\
+    &Joy.button[10],\
+    &Joy.button[11]);
+    
+    */
+    
+   /*
+    float A=Joy.axis[1];
+    float B=Joy.axis[0];
+    if (A<.1 && B<.1 && B>-.1 && A>-.1) {
+        MR=.5;
+        ML=.5;
+    } else {
+
+        if (A>.1 || A<-.1) {
+            ML=1-(A+1)/2;
+            MR=(A+1)/2;
+        }
+        if (B>.25 || B<-.25) {
+            ML=1-(B+1)/2;
+            MR=1-(B+1)/2;
+        }
+    }
+    
+    */
+    ML=1;
+    MR=0;
+    for (int x=0; x<8; x++) {
+        leds[x]=(bool)Joy.button[x];
+    }
+}
+
+void onUDPSocketEvent(UDPSocketEvent e)
+{
+    switch (e) {
+        case UDPSOCKET_READABLE: //The only event for now
+            char buf[256] = {0};
+            Host host;
+            while ( int len = UDP.recvfrom( buf, 255, &host ) ) {
+                if ( len <= 0 )
+                    break;
+                UDP_queue.Put(buf);
+            }
+            break;
+    }
+}
+
+
+int main()
+{
+
+    eth = new EthernetNetIf(
+        IpAddr(IpAddr(192,168,1,102)),  // My IP Address
+        IpAddr(IpAddr(255,255,255,0)), //Network Mask
+        IpAddr(IpAddr(192,168,1,2)), //Gateway
+        IpAddr(IpAddr())  //DNS
+    );
+    Robot.setIp(IpAddr(192,168,1,102));
+    Robot.setPort(12345);
+
+    eth->setup();
+    UDP.setOnEvent(&onUDPSocketEvent);
+    UDP.bind(Robot);
+    NetPoll.attach_us(&Net::poll,500);
+
+    ErrorTime.start();
+
+    while (1) {
+        if ( UDP_queue.Get(messageBufferIncoming))messageProcess();
+        if (ErrorTime.read()>.2) {
+            MR=.50;
+            ML=0.5;
+            for (int x=0; x<8; x++)leds[x]=0;
+            ErrorTime.reset();
+        }
+
+    }
+}
+