Diff: main.cpp

Revision:

50:ec6cc79132e8

Parent:

49:7d172c133dbf

Child:

51:4892c9238c33

--- a/main.cpp	Thu Apr 03 13:17:04 2014 +0000
+++ b/main.cpp	Thu Apr 03 19:42:44 2014 +0000
@@ -181,6 +181,7 @@
                 break;
             case 'y':
                 bot.pollForShapes();
+                DBGPRINT("dist %f\r\n",(480.0-bot.triY)/75.0 + 1.65);
                 break;
             case 'h':
                 DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
@@ -284,6 +285,47 @@
                 
                 // poll the compass ( currently gives bad values )
             case 'Q':
+                bot.nearCamera();
+                wait(1.5);
+                bot.extendStick();
+                wait(0.75);
+                bot.downCamera();
+                wait(1.5);
+                bot.pollForShapes();
+                if(bot.triX != 0){
+                    //found triangle
+                    //float dist = (9.75*(480.0-bot.triY))/480.0 + 0.5;
+                    float dist = (480.0-bot.triY)/75.0 + 1.65;
+                    bot.smoothMove(290*(dist-6.0), 0, 5);
+                    bot.smoothMove(-600, 1, 5); // turn left
+                    bot.smoothMove(150, 1, 5); // turn right
+                    bot.smoothMove(290*(-3.5), 0, 5);
+                    bot.smoothMove(-350, 1, 5); // turn left
+                    bot.smoothMove(350, 1, 5); // turn back right
+                    bot.nearCamera();
+                    wait(1.0);
+                    bot.retractStick();
+                    wait(1.0);
+                    bot.downCamera();
+                    wait(1.5);
+                    bot.smoothMove(290*(4.0), 0, 5);
+                    bot.pollForShapes();
+                    if(bot.triX != 0 && bot.triRot > 52 && bot.triRot < 67){
+                        if(bot.triX > 362 || bot.triX < 337){
+                            int correction = (100*(bot.triX - 349))/60;
+                            bot.smoothMove(correction, 1, 5); // turn rightish to correct
+                            wait(0.5);
+                            bot.pollForShapes();
+                            
+                            //right is positive and decreases
+                        }
+                        //dist = (9.75*(480.0-bot.triY))/480.0 + 0.5;
+                        dist = (480.0-bot.triY)/75.0 + 1.65;
+                        bot.smoothMove(290*(dist), 0, 5);
+                        bot.grab();
+                    }
+                }
+            /*
                 bot.downCamera();
                 wait(3);
                 bot.pollForShapes();
@@ -316,6 +358,7 @@
                     }
                     
                 }
+                */
                 //bot.gyro.checkCompass();
                 //DBGPRINT("Compass vector = %d\t%d\t%d\t%f\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag,bot.gyro.compDir*180.0/3.14159);
                 break;