Diff: main.cpp

Revision:

15:b10859606504

Parent:

14:a30aa3b29a2e

Child:

17:e247d58d9f42

diff -r a30aa3b29a2e -r b10859606504 main.cpp
--- a/main.cpp	Fri Mar 07 21:49:22 2014 +0000
+++ b/main.cpp	Tue Mar 11 19:58:31 2014 +0000
@@ -7,6 +7,7 @@
 Serial pc(USBTX, USBRX);
 
 robot bot;
+servo s(PTC9);
 
 /*
 //actual competition code, kinda
@@ -31,8 +32,24 @@
     double targetAngle=0.0;
     DBGPRINT("AA\n\r",1);  // to see if robot powers up at least
     
-    // Initialize sensors
-    //hcsr04(2,1) : trigger_out(trigger);
+    //test servo
+    s.toPosition(45);
+    
+    
+    //timer issue navigation test
+    bot.absDriveForward(0,50);//assuming the bot stops in this function
+    /*
+    DBGPRINT("%f",bot.gyro.getZDegrees());
+    bot.gyro.stop();//stopping the gyro timer
+    //bot.gyro.gyroUpkeepTicker.detach();//stopping the gyro timer
+    //use sensor to find distance
+    bot.pingLeft.trigger();
+    wait(0.1);
+    float pingresult = bot.pingLeft.inches();
+    DBGPRINT("Distance = %f\r\n",pingresult);
+    bot.gyro.start();//stopping the gyro timer
+    DBGPRINT("%f",bot.gyro.getZDegrees());
+    */
     
     // Loop
     while(1) {
@@ -42,7 +59,7 @@
         
         // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
         switch(tmpchar){
-            case 'e': //drive in a smallish square
+            /*case 'e': //drive in a smallish square
                 bot.driveForward(0,3000);
                 bot.driveForward(-90,0);
                 bot.driveForward(-90,3000);
@@ -52,7 +69,7 @@
                 bot.driveForward(-270,3000);
                 bot.driveForward(0,0);
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
-                break;
+                break;*/
             case 'q': //poll the encoders
                 constbuf = bot.bigenc.getVals();
                 DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
@@ -61,19 +78,23 @@
                 targetAngle = bot.gyro.getZDegrees();
                 break;
             case 'w': // turn 90 degrees counter clockwise from the starting rotation
-                bot.driveForward(90,0);
+                
+                s.toPosition(0);
+                //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
-                bot.driveForward(-90,0);
+                s.toPosition(180);
+                //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
-                bot.driveForward(180,0);
+                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;
             case 'd': // turn back to starting rotation
-                bot.driveForward(0,0);
+                //bot.driveForward(0,0);
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'W': // drive forward a small bit
@@ -84,7 +105,8 @@
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'E': //drive forward five times as much
-                bot.driveForward(targetAngle,5000);
+                //bot.driveForward(targetAngle,5000);
+                bot.absDriveForward(0,5000);
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'X': // small reverse
@@ -95,7 +117,8 @@
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'C': // big reverse
-                bot.driveForward(targetAngle,-5000);
+                //bot.driveForward(targetAngle,-5000);
+                bot.absDriveForward(0,-5000);
                 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
                 break;
             case 'p': // poll the gyro
@@ -133,7 +156,7 @@
             case 'i':
                 //put distance ping here
                 bot.pingLeft.trigger();
-                wait(1);
+                wait(0.1);
                 float pingresult = bot.pingLeft.inches();
                 DBGPRINT("Distance = %f\r\n",pingresult);
                 break;
@@ -173,8 +196,8 @@
                 
                // brake if we get an unknown command
             default:
-                bot.left.brake();
-                bot.right.brake();
+                //bot.left.brake();
+                //bot.right.brake();
                 break;
                 
         }