Revision 3:9844ec849c12, committed 2013-07-29

Comitter:

johnson6987

Date:

Mon Jul 29 20:06:57 2013 +0000

Parent:

1:b8ce18c28de9

Commit message:

Final Code after Adam H

Changed in this revision

--- a/main.cpp	Thu Feb 21 22:51:08 2013 +0000
+++ b/main.cpp	Mon Jul 29 20:06:57 2013 +0000
@@ -4,17 +4,23 @@
 #include "queue.h"
 #include "Servo.h"
 
+BusOut myleds(LED1, LED2, LED3, LED4);
 Ticker NetPoll;
 Timer ErrorTime;
-Queue UDP_queue(256,20);
+Timer systemtime;
+Queue UDP_queue(256,1);
 Serial pc(USBTX, USBRX);
 EthernetNetIf * eth;
 UDPSocket UDP;
 Host Robot;
-Servo ML(p21);
-Servo MR(p22);
+Servo Ml(p21);
+Servo Mr(p22);
+Servo Arm(p23);
+Servo Elbow(p24);
+float MR=.5;
+float ML=.5;
 
-DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)};
+//DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)};
 
 char messageBufferIncoming[256];
 
@@ -23,11 +29,40 @@
     float button[12];
 };
 
+void dtime(int x){
+myleds=x;
+//pc.printf("%d - time=%f\n",x,systemtime.read());
+}
+
+void right(){
+Ml=1;
+Mr=1;
+wait(.25);
+}
+
+void left(){
+Ml=0;
+Mr=0;
+wait(.25);
+}
+
+void forward(float time){
+Ml=1;
+Mr=0;
+wait(time);
+}
+
+void backwards(float time){
+Ml=0;
+Mr=1;
+wait(time);
+}
+
 
 Joystick Joy;
 void messageProcess(void)
 {
-
+dtime(1);
     ErrorTime.reset();
     // pc.printf("%s\r\n",messageBufferIncoming);
     sscanf(messageBufferIncoming,"%f%f%f%f",\
@@ -35,8 +70,8 @@
            &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]);
+dtime(2);
+   // 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],\
@@ -58,43 +93,65 @@
     
     */
     
-   /*
-    float A=Joy.axis[1];
-    float B=Joy.axis[0];
-    if (A<.1 && B<.1 && B>-.1 && A>-.1) {
-        MR=.5;
-        ML=.5;
+   
+    float Y=Joy.axis[1];
+    float X=Joy.axis[0];
+    Arm=Joy.axis[2];
+    Elbow=(Joy.axis[3]+1)/2.0;
+    Y=-Y;
+    dtime(3);
+    
+    if (X<0.1 && Y<0.1 && X>-0.1 && Y>-0.1) {
+        Ml=0.5;
+        Mr=0.5;
+        dtime(4);
     } else {
-
-        if (A>.1 || A<-.1) {
-            ML=1-(A+1)/2;
-            MR=(A+1)/2;
+    dtime(5);
+        if (Y>0 && X>0) { //quadrant 1
+            ML=sqrt((X*X)+(Y*Y));
+            MR=X-Y;
+        }
+        if (Y>0 && X<0) { //quadrant 2
+            ML=Y+X;
+            MR=-sqrt((X*X)+(Y*Y));
+        }
+        if (Y<0 && X<0) { //quadrant 3
+            ML=-sqrt((X*X)+(Y*Y));
+            MR=X-Y;
         }
-        if (B>.25 || B<-.25) {
-            ML=1-(B+1)/2;
-            MR=1-(B+1)/2;
+        if (Y<0 && X>0) { //quadrant 4
+            ML=X+Y;
+            MR=sqrt((X*X)+(Y*Y));
         }
+    Ml=((ML*ML*ML)+1)/2.0;
+    Mr=((MR*MR*MR)+1)/2.0;
     }
+    dtime(6);
+    //pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read());
+    /*
     
-    */
-    ML=1;
-    MR=0;
+    /*ML=1;
+    //MR=0;
     for (int x=0; x<8; x++) {
         leds[x]=(bool)Joy.button[x];
-    }
+        dtime(7);
+    }*/
 }
 
 void onUDPSocketEvent(UDPSocketEvent e)
 {
+dtime(14);
     switch (e) {
         case UDPSOCKET_READABLE: //The only event for now
             char buf[256] = {0};
             Host host;
-            while ( int len = UDP.recvfrom( buf, 255, &host ) ) {
+           while ( int len = UDP.recvfrom( buf, 255, &host ) ) {
                 if ( len <= 0 )
                     break;
                 UDP_queue.Put(buf);
             }
+               // UDP.recvfrom( messageBufferIncoming, 255, &host );
+                //messageProcess();
             break;
     }
 }
@@ -102,7 +159,7 @@
 
 int main()
 {
-
+    systemtime.start();
     eth = new EthernetNetIf(
         IpAddr(IpAddr(192,168,1,102)),  // My IP Address
         IpAddr(IpAddr(255,255,255,0)), //Network Mask
@@ -122,12 +179,12 @@
     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;
+        dtime(15);
+            Mr=.50;
+            Ml=0.5;
+           
             ErrorTime.reset();
         }
 
     }
 }
-