Diff: robot.cpp

Revision:

29:1132155bc7da

Parent:

27:688409727452

Child:

32:ff71f61bb9f6

--- a/robot.cpp	Fri Mar 21 00:48:29 2014 +0000
+++ b/robot.cpp	Fri Mar 21 03:16:15 2014 +0000
@@ -25,70 +25,7 @@
 
 //driveforward, but set up so that 
 int robot::absDriveForward(double desangle, int distance){
-    return smoothMove(distance, 0, 5);
-    /*Timer tim;
-    int dir=1;
-    // if we are traveling forward a negative distance, go backwards
-    if(distance < 0){
-        dir = -1; //is multiplied by the final output
-        distance = -distance;
-    }
-    int i,move;
-    int maxSpeed=41;
-    int stepsPerInc=2;
-    int stepsToMax=(maxSpeed-1)*stepsPerInc;
-    int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
-    int stopRaise,startFall;
-    if(distance<distToMax*2){
-        // can't get up to full speed before needing to slow down
-        //find new max speed, time to get there, and distance there
-        maxSpeed =  ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
-        stepsToMax=(maxSpeed-1)*stepsPerInc;
-        distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
-        stopRaise = stepsToMax;
-        startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
-        DBGPRINT("Insufficent Ramp-Up\r\n",1);
-    } else {
-        // can get up to full speed, so we will
-        stopRaise=stepsToMax;
-        startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
-        DBGPRINT("Sufficent Ramp-Up\r\n",1);
-    }
-    DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
-    tim.start();
-    move = 0;
-    int totalMove=0;
-    gyro.stop();
-    for(i=0;i<startFall+stepsToMax;i++){
-        //start clock for this frame
-        tim.reset();
-        if(i<=stopRaise && i%stepsPerInc==0){
-            //increase speed every (stepsPerInc) steps up to maxSpeed
-            move++;
-        }
-        if(i==startFall-1){
-            //calibration step before we start falling to get an exact result
-            move = distance-totalMove-distToMax;
-        }
-        if(i>=startFall && (i-startFall)%stepsPerInc==0){
-            if(i==startFall) //reset move after calibration step
-                move=maxSpeed;
-            
-            //decrement every stepsPerInc steps until stopped
-            move--;
-        }
-        totalMove+=move;
-        motors.moveForward(move*dir);
-        //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
-        while(tim.read_ms()<5);
-        gyro.gyroUpkeep();
-        addforward(double(move*dir)*0.0035362);
-        //waits until 10ms have passed for the 100hz timing
-        while(tim.read_ms()<10);
-    }
-    gyro.start();
-    return totalMove*dir;
-    */
+    return smoothMove(distance, 0, 40);
 }
 
 int robot::smoothMove(int distance, int rotate, int maxSpeed){
@@ -129,6 +66,14 @@
     move = 0;
     int totalMove=0;
     gyro.stop();
+    int xStorage[40];
+    int xMaxVal=-3000;
+    int xMinVal=3000;
+    long xSumVal=0;
+    int zStorage[40];
+    int zMaxVal=-3000;
+    int zMinVal=3000;
+    long zSumVal=0;
     for(i=0;i<startFall+stepsToMax;i++){
         //start clock for this frame
         tim.reset();
@@ -153,11 +98,46 @@
         //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
         while(tim.read_ms()<5);
         gyro.gyroUpkeep();
+        if(distance==16000 && maxSpeed == 6){
+            if(gyro.xmag>xMaxVal)
+                xMaxVal=gyro.xmag;
+            if(gyro.xmag<xMinVal)
+                xMinVal=gyro.xmag;
+            if(gyro.zmag>zMaxVal)
+                zMaxVal=gyro.zmag;
+            if(gyro.zmag<zMinVal)
+                zMinVal=gyro.zmag;
+            if(i%75==0){
+                xSumVal+=gyro.xmag;
+                zSumVal+=gyro.zmag;
+                xStorage[i/75]=gyro.xmag;
+                zStorage[i/75]=gyro.zmag;
+                DBGPRINT("Saved Val %d \t%d \t%d \t%f\r\n",i/75,gyro.xmag,gyro.zmag,gyro.getZDegrees());
+            }
+        }
         addforward(double(move*dir)*0.0035362);
         //waits until 10ms have passed for the 100hz timing
         while(tim.read_ms()<10);
     }
     gyro.start();
+    if(distance==16000 && maxSpeed == 6){
+        double xAvg = 75.0*(double)xSumVal/(startFall+stepsToMax);
+        double zAvg = 75.0*(double)zSumVal/(startFall+stepsToMax);
+        double xRMSSum=0;
+        double zRMSSum=0;
+        for(i=0;i<(startFall+stepsToMax)/75;i++){
+            xRMSSum+=(xStorage[i]-xAvg)*(xStorage[i]-xAvg);
+            zRMSSum+=(zStorage[i]-zAvg)*(zStorage[i]-zAvg);
+        }
+        double xRMS = sqrt(10.0*xRMSSum/(startFall+stepsToMax));
+        double zRMS = sqrt(10.0*zRMSSum/(startFall+stepsToMax));
+        for(i=0;i<(startFall+stepsToMax)/75;i++){
+            double compDir = atan2(((double)xStorage[i]-gyro.xoffs)*gyro.xamp,((double)zStorage[i]-gyro.zoffs)*gyro.zamp);
+            double compDir2 = atan2(((double)xStorage[i]-xAvg)/xRMS,((double)zStorage[i]-zAvg)/zRMS);
+            DBGPRINT("Saved Val %d \t%d \t%d \t%f \t%f\r\n",i,xStorage[i],zStorage[i],compDir*180.0/3.14159,compDir2*180.0/3.14159);
+        }
+        DBGPRINT("Calibrated to (%d,%f,%f) and (%d,%f,%f)\r\n",xMaxVal-xMinVal,xAvg,xRMS,zMaxVal-zMinVal,zAvg,zRMS);
+    }
     return totalMove*dir;
     //tim.stop();//may not need
 }