Diff: main.cpp

Revision:

49:7d172c133dbf

Parent:

48:519deb1d4dff

Child:

50:ec6cc79132e8

--- a/main.cpp	Thu Apr 03 06:17:32 2014 +0000
+++ b/main.cpp	Thu Apr 03 13:17:04 2014 +0000
@@ -95,11 +95,14 @@
             case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
                 targetAngle = bot.gyro.getZDegrees();
                 break;
-            case 'w': // turn 90 degrees counter clockwise from the starting rotation
+            case 'w':
+                DBGPRINT("100 Forward\n\r",1);
+                bot.absDriveForward(0,100);
+                // run first row
                 
-                //s.toPosition(-40);
-                //bot.driveForward(90,0);
-                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
+                /*DBGPRINT("Running First Row\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
+                game.resetFirst();
+                game.runFirstRow();*/
                 break;
             case 'x': // turn 90 degrees clockwise from the starting rotation
                 //s.toPosition(220);
@@ -109,12 +112,17 @@
             case 'a': // turn the opposite direction from the starting one
                 //s.toPosition(90);
                 //bot.driveForward(180,0);
-                DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
+                //DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
+                
+                DBGPRINT("100 Left\n\r",1);
+                bot.smoothMove(-100, 1, 40);
                 break;
             case 'd': // turn back to starting rotation
                 //bot.driveForward(0,0);
-                response=bot.BTLink.sendCmd(0x02,testdata,5);
-                DBGPRINT("=%d\n\r",response);
+                //response=bot.BTLink.sendCmd(0x02,testdata,5);
+                //DBGPRINT("=%d\n\r",response);
+                DBGPRINT("100 Right\n\r",1);
+                bot.smoothMove(100, 1, 40);
                 break;
             case 'W': // drive forward a small bit
                 //bot.driveForward(targetAngle,1000);
@@ -240,8 +248,10 @@
                 //DBGPRINT("pfac = %f\r\n",bot.pfac);
                 break;
             case 's':
-                game.runSecondRow();    //testing second and third row
-                game.runSecondRow();
+                DBGPRINT("100 Back\n\r",1);
+                bot.smoothMove(-100, 0, 40);
+                //game.runSecondRow();    //testing second and third row
+                //game.runSecondRow();
                 break;
             case 'Y':
                 /*bot.cont.start();
@@ -252,25 +262,62 @@
                 bot.cont.stop();*/
                 //s3.toPosition(s1pos+=5);
                 //bot.alignWithRig();
+                
+                bot.grabPosition();
                 DBGPRINT("1 pos = %d\r\n",s1pos);
                 break;
             case 'H':
+                DBGPRINT("2 pos = %d\r\n",bot.incServo());
                 //s3.toPosition(s1pos-=5);
-                DBGPRINT("1 pos = %d\r\n",s1pos);
+                //DBGPRINT("1 pos = %d\r\n",s1pos);
                 break;
             case 'U':
                 //s4.toPosition(s2pos+=5);
+                bot.extendStick();
                 DBGPRINT("2 pos = %d\r\n",s2pos);
                 break;
             case 'J':
                 //s4.toPosition(s2pos-=5);
-                DBGPRINT("2 pos = %d\r\n",s2pos);
+                DBGPRINT("2 pos = %d\r\n",bot.decServo());
+                //DBGPRINT("2 pos = %d\r\n",s2pos);
                 break;
                 
                 // poll the compass ( currently gives bad values )
             case 'Q':
+                bot.downCamera();
+                wait(3);
+                bot.pollForShapes();
+                if(bot.triX != 0){
+                    //found triangle
+                    float dist = (8.25*(480.0-bot.triY))/480.0 + 1.0;
+                    if(!(bot.triRot > 52 && bot.triRot < 67)){
+                        bot.extendStick();
+                        bot.smoothMove(-1000, 1, 5); // turn left
+                        bot.retractStick();
+                        bot.smoothMove(50, 1, 5); // turn right
+                        if(dist>4.0)
+                            bot.smoothMove(290*(dist-5), 0, 5);
+                        bot.smoothMove(425, 1, 5); // turn left
+                        wait(0.5);
+                        bot.pollForShapes();
+                    }
+                    if(bot.triX != 0 && bot.triRot > 52 && bot.triRot < 67){
+                        if(bot.triX > 375 || bot.triX < 360){
+                            int correction = (100*(bot.triX - 367))/65;
+                            bot.smoothMove(correction, 1, 5); // turn rightish to correct
+                            wait(0.5);
+                            bot.pollForShapes();
+                            
+                            //right is positive and decreases
+                        }
+                        dist = (9.6*(480.0-bot.triY))/480.0 + 0.9;
+                        bot.smoothMove(290*(dist-0.2), 0, 5);
+                        bot.grab();
+                    }
+                    
+                }
                 //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);
+                //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;
                 
                // brake if we get an unknown command