Diff: main.cpp

Revision:

48:519deb1d4dff

Parent:

45:9c520ebc38e0

Child:

49:7d172c133dbf

diff -r 9c520ebc38e0 -r 519deb1d4dff main.cpp
--- a/main.cpp	Thu Apr 03 02:44:36 2014 +0000
+++ b/main.cpp	Thu Apr 03 06:17:32 2014 +0000
@@ -27,10 +27,10 @@
     wait(0.1);
     robot bot;
     gamePlayer game(bot);
-    servo s(PTC9);
+    /*servo s(PTC9);
     servo s2(PTC8);
     servo s3(PTA5);
-    servo s4(PTA4);
+    servo s4(PTA4);*/
     // Initialize variables
     leds = 0x2;
     char tmpchar = 0;
@@ -40,10 +40,10 @@
     DBGPRINT("AA\n\r",1);  // to see if robot powers up at least
     
     //test servo
-    s.toPosition(-45);
+    /*s.toPosition(-45);
     s2.toPosition(60);
-    s3.toPosition(45);
-    s4.toPosition(45);
+    s3.toPosition(250);
+    s4.toPosition(90);*/
     
     int s1pos = 45;
     int s2pos = 45;
@@ -97,17 +97,17 @@
                 break;
             case 'w': // turn 90 degrees counter clockwise from the starting rotation
                 
-                s.toPosition(-40);
+                //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);
                 break;
             case 'x': // turn 90 degrees clockwise from the starting rotation
-                s.toPosition(220);
+                //s.toPosition(220);
                 //bot.driveForward(-90,0);
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'a': // turn the opposite direction from the starting one
-                s.toPosition(90);
+                //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);
                 break;
@@ -207,11 +207,13 @@
                 //DBGPRINT("angfac = %f\r\n",bot.angfac);
                 break;
             case 'T':
-                DBGPRINT("Finding Gap\r\n",1);
-                s.toPosition(-45);
-                s2.toPosition(60);
+                DBGPRINT("Grabbing Tool\r\n",1);
+                //s.toPosition(-45);
+                //s2.toPosition(60);
+                bot.grabPosition();
                 wait(0.5);
-                s2.toPosition(-10);
+                bot.grab();
+                /*s2.toPosition(-10);
                 wait(0.5);
                 //bot.cont.start();
                 bot.cont.resetTicks();
@@ -224,7 +226,7 @@
                 }
                 bot.cont.rampSpeed(0,0);
                 s.toPosition(130);
-                while(!bot.cont.stopped());
+                while(!bot.cont.stopped());*/
                 //bot.cont.stop();
                 //game.checkWaves();
                 break;
@@ -248,20 +250,20 @@
                 bot.cont.rampSpeed(0,0);
                 while(!bot.cont.stopped());
                 bot.cont.stop();*/
-                //s.toPosition(s1pos+=5);
-                bot.alignWithRig();
+                //s3.toPosition(s1pos+=5);
+                //bot.alignWithRig();
                 DBGPRINT("1 pos = %d\r\n",s1pos);
                 break;
             case 'H':
-                s.toPosition(s1pos-=5);
+                //s3.toPosition(s1pos-=5);
                 DBGPRINT("1 pos = %d\r\n",s1pos);
                 break;
             case 'U':
-                s2.toPosition(s2pos+=5);
+                //s4.toPosition(s2pos+=5);
                 DBGPRINT("2 pos = %d\r\n",s2pos);
                 break;
             case 'J':
-                s2.toPosition(s2pos-=5);
+                //s4.toPosition(s2pos-=5);
                 DBGPRINT("2 pos = %d\r\n",s2pos);
                 break;