Diff: robot.cpp

Revision:

14:a30aa3b29a2e

Parent:

13:c2d14bf733a5

Child:

15:b10859606504

--- a/robot.cpp	Thu Feb 27 00:38:13 2014 +0000
+++ b/robot.cpp	Fri Mar 07 21:49:22 2014 +0000
@@ -6,7 +6,7 @@
 //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), motors(spi,PTD0) {
+            right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4), motors(spi,PTD0), pingLeft(PTA13,PTD5) {
     
     //bigenc.setDirections(1,-1,1,1);
     left.setReversed(1);
@@ -204,10 +204,10 @@
         //wait(0.05);
         constbuf = bigenc.getVals();
         
-        //control system (Proportional, Derivative, Integral)
+        //control system (Proportional, Integral, Derivative) = PID controller
         pmain = distance - (constbuf[0]+constbuf[1])/2;
+        imain += pmain;
         dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
-        imain += pmain;
         
         //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);\
         
@@ -307,7 +307,9 @@
         if(currangle<angle-2){
             right.brake();
         } else {
-            right.setPower(MIN(power,maxPow));
+            right.setPo
+            
+            wer(MIN(power,maxPow));
         }
         DBGPRINT("=%d of %d [%f] (%d, %d, %d, %d) \t{%f,\t%f,\t%f}\r\n",distTraveled,distance, maxPow, constbuf[0], constbuf[1], constbuf[2], constbuf[3],x,y,rot*180.0/3.14159);
         for(i=0;i<4;i++)