Revision 3:3e3d1df8aa3a, committed 2015-07-03

Comitter:

LucasUIUC

Date:

Fri Jul 03 00:14:36 2015 +0000

Parent:

2:8e4c6f97b740

Commit message:

Test1

Changed in this revision

diff -r 8e4c6f97b740 -r 3e3d1df8aa3a main.cpp
--- a/main.cpp	Thu Jul 02 00:13:03 2015 +0000
+++ b/main.cpp	Fri Jul 03 00:14:36 2015 +0000
@@ -17,16 +17,19 @@
         blocks = pixy.getBlocks();
         float p;   
         float q;
-        float ServoP1;
-        float ServoP2;
+        //float ServoP1;
+        //float ServoP2;
         
         if (blocks) {
             i++;
-                        
-                    p = pixy.blocks[i].x;
-                    ServoP1 = ((((160 - p)*9/32) + 45)*1/90);
-                    myservo = ServoP1;
+                   // if ((i % 10) == 0)  {
+                    //p = pixy.blocks[i].x;// x position from pixy
+                    //ServoP1 = ((((160 - p)*9/32) + 45)*1/90);
+                    //pc.printf("Motor position %d\n is %.2f\n\r",i, ServoP1);
+                    //myservo = ServoP1;
+         //}
          
+                  
             if (i % 50 == 0) {
                 pc.printf("Detected %d:\n\r", blocks);
                 for (j = 0; j < blocks; j++) {
@@ -34,13 +37,34 @@
                                         
                     pixy.blocks[j].print(pc);
                     
-                    q = pixy.blocks[j].x;
-                    ServoP2 = ((((160 - q)*9/32) + 45)*1/90);
-                    pc.printf("Motor position is %.2f\n\r", ServoP2);
+                    q = pixy.blocks[j].x;// This line gives the x position from pixy
+                    
+                    //ServoP2 = ((((160 - q)*9/32) + 45)*1/90);
+                    //pc.printf("Motor position is %.2f\n\r", ServoP2);
+                    float ang = ((160 - q)*9/32);
+                    pc.printf("angle position is %.3f\n\r", ang);
+                    
+                    if ((ang >= 0.25)||(ang <= 45.0)){
+                        p -=0.05;
+                        myservo = p;  
+                        }                                               
+                    if ((ang <= -0.25)||(ang <= -45.0)){
+                        p +=0.05;
+                        myservo = p;
+                        }                            
+                    if ((ang <= 0.25)||(ang >= 0.25)){
+                        p += 0.0;
+                        myservo = p;
+                        } 
+                       /*if ((ang >= -45)||(ang <= 45.0)){
+                        p +=0.05;
+                        myservo = p; 
+                        }*/
+                        pc.printf("Motor position is %.3f\n\r", p);
                     //myservo = ServoP;
-   // wait(0.05);
+                    // wait(0.05);
                 }
             }
         }
-    }  
-}
\ No newline at end of file
+    }
+}