Revision 2:6383a282c70c, committed 2013-05-13

Comitter:

johnson6987

Date:

Mon May 13 13:30:26 2013 +0000

Parent:

1:b8ce18c28de9

Child:

4:bae1c47dc112

Commit message:

For Adam's Work

Changed in this revision

--- a/Queue.lib	Thu Feb 21 22:51:08 2013 +0000
+++ b/Queue.lib	Mon May 13 13:30:26 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/johnson6987/code/Queue/#9877a4bf36db
+http://mbed.org/users/johnson6987/code/Queue/#e65bf125f094

--- a/Servo.lib	Thu Feb 21 22:51:08 2013 +0000
+++ b/Servo.lib	Mon May 13 13:30:26 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/johnson6987/code/Servo/#d43f850019b6
+http://mbed.org/users/johnson6987/code/Servo/#cb12510fd64e

--- a/main.cpp	Thu Feb 21 22:51:08 2013 +0000
+++ b/main.cpp	Mon May 13 13:30:26 2013 +0000
@@ -11,8 +11,12 @@
 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)};
 
@@ -23,6 +27,30 @@
     float button[12];
 };
 
+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)
@@ -36,7 +64,7 @@
            &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]);
+   // 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,30 +86,46 @@
     
     */
     
-   /*
-    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;
+   
+    
+    if (X<.1 && Y<.1 && X>-.1 && Y>-.1) {
+        Ml=.5;
+        Mr=.5;
     } else {
-
-        if (A>.1 || A<-.1) {
-            ML=1-(A+1)/2;
-            MR=(A+1)/2;
+    
+        if (Y>0 && X>0) {
+            ML=sqrt((X*X)+(Y*Y));
+            MR=Y-X;
+        }
+        if (Y>0 && X<0) {
+            ML=Y+X;
+            MR=sqrt((X*X)+(Y*Y));
         }
-        if (B>.25 || B<-.25) {
-            ML=1-(B+1)/2;
-            MR=1-(B+1)/2;
+        if (Y<0 && X<0) {
+            ML=Y-X;
+            MR=-sqrt((X*X)+(Y*Y));
+        }
+        if (Y<0 && X>0) {
+            ML=-sqrt((X*X)+(Y*Y));
+            MR=Y+X;
         }
+    Ml=(ML+1)/2.0;
+    Mr=(1-MR)/2.0;
     }
+    pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read());
+    /*
     
-    */
-    ML=1;
+    /*ML=1;
     MR=0;
     for (int x=0; x<8; x++) {
         leds[x]=(bool)Joy.button[x];
-    }
+    }*/
 }
 
 void onUDPSocketEvent(UDPSocketEvent e)
@@ -119,11 +163,23 @@
 
     ErrorTime.start();
 
+right();
+forward(1);
+right();
+forward(1);
+right();
+forward(1);
+right();
+forward(1);
+left();
+left();
+backwards(.2);
+
     while (1) {
         if ( UDP_queue.Get(messageBufferIncoming))messageProcess();
         if (ErrorTime.read()>.2) {
-            MR=.50;
-            ML=0.5;
+            Mr=.50;
+            Ml=0.5;
             for (int x=0; x<8; x++)leds[x]=0;
             ErrorTime.reset();
         }