Diff: robot.cpp

Revision:

7:3b2cf7efe5d1

Parent:

6:62d498ee97cf

Child:

8:c23cd84befa2

--- a/robot.cpp	Mon Nov 18 23:45:12 2013 +0000
+++ b/robot.cpp	Fri Nov 22 18:00:18 2013 +0000
@@ -3,21 +3,24 @@
 #define MIN(a,b) ((b<a)?(b):(a))
 #define MAX(a,b) ((b>a)?(b):(a))
 
+//this is the main place pinouts are put
+//also sets up the control system constants
 robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), gyro(PTE0, PTE1),
             right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4){
     
     bigenc.setDirections(1,-1,1,1);
     left.setReversed(1);
     x=y=rot=0;
+    
+    //control system constants
     pfac=0.00035;
     ifac=0.00000001;
     dfac=0.000001;
     
     angfac=0.0000016;
-    //blah
 }
 
-
+//this is the main thing that both turns and goes forward
 int robot::driveForward(double desangle, int distance){
     bigenc.resetAll();
     const int* constbuf = bigenc.getVals();
@@ -28,12 +31,15 @@
     double minmain=0.05;
     double minalt=0.05;
     
+    //find a point in front of where we're heading
     int targetang = desangle*4050000.0/360.0;//gyro.getZ();
     int startang = targetang;
     double angle=double(startang)*2*3.14159/4050000.0;
     double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
     double targy = y + double((distance==0)?10000:distance)*0.0035362*sin(angle)*1.5;
     int invfactor = 0;
+    
+    //if going backwards, point away from the point
     if(distance<0) {
         invfactor = 2025000;
     }
@@ -59,6 +65,7 @@
         //wait(0.05);
         constbuf = bigenc.getVals();
         
+        //control system (Proportional, Derivative, Integral)
         pmain = distance - (constbuf[0]+constbuf[1])/2;
         dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
         imain += pmain;
@@ -73,12 +80,17 @@
         
         //DBGPRINT("%f like %f [%f] {%d,%d,%f,%f}\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159))*360.0/4050000.0,angle*180/3.14159,rot,pmain,dmain,fmod(rot*180.0/3.14159-desangle+3600000.0,360.0), realfac);\
         
+        //finds the difference between the angle to the imaginary point we're headed to and the current angle and turns it into a power level
         realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
-        realfac = MAX(MIN(realfac,0.3),-0.3);
+        realfac = MAX(MIN(realfac,0.3),-0.3); // limits how much the motors can turn to fix the angle
         
+        //uses PID control for the forward/back motions and adds in the angular component
+        //the forward/back motions is limited to a certain speed
         double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
         double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
-        if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.05 || realfac >= 0.05) ){
+        
+        //if we haven't settled, but also aren't moving, then speed up until it moves
+        if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.02 || realfac >= 0.02) ){
             if (leftpow>0){
                 if (leftpow<minalt){
                     leftpow=minalt;
@@ -97,11 +109,15 @@
         left.setPower(leftpow);
         right.setPower(rightpow);
         //int deltaTraveled=MAX(MIN(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1]),MIN(constbuf[2]-prev[2],constbuf[3]-prev[3])),0);
+        
+        //how far we've moved in the last timestep
         int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
         //DBGPRINT("\t %d\r\n",deltaTraveled);
-        addforward(double(deltaTraveled)*0.0035362);
+        addforward(double(deltaTraveled)*0.0035362); //update our position
         distTraveled+=deltaTraveled;
         loopcount++;
+        
+        //increase min speed so that it will actually move
         if((dmain<5&&dmain>-5)&&minmain<0.2){
             minmain+=0.003;
         } else if (minmain>=0.05) {
@@ -116,6 +132,8 @@
     left.brake();
     right.brake();
     DBGPRINT("Loops: %d\r\n",loopcount);
+    
+    //catch the slowdown movement
     wait(0.2);
     for(i=0;i<4;i++)
         prev[i]=constbuf[i];
@@ -126,6 +144,7 @@
     return 0;
 }
 
+//add the motion in the direction that the robot is facing
 void robot::addforward(double dist){
     double angle=double(gyro.getZ())*2*3.14159/4050000.0;
     x+=dist*cos(angle);
@@ -133,6 +152,7 @@
     rot=angle;
 }
 
+//doesn't work yet
 int robot::moveTo(double xInches, double yInches){
     double power=.2;
     turntowards(xInches,yInches);
@@ -175,6 +195,7 @@
     
 }
 
+//also doesn't work
 int robot::turntowards(double xInches, double yInches){
     double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
     double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
@@ -193,7 +214,7 @@
     return 1;
 }
     
-
+//still no
 double robot::turn(double power, double degrees){
     bigenc.resetAll();
     const int* constbuf = bigenc.getVals();