Diff: main.cpp

Revision:

1:5402c62ad487

Parent:

0:5e090fec6099

Child:

2:eb79d6e3f701

--- a/main.cpp	Thu Jun 01 13:21:32 2017 +0000
+++ b/main.cpp	Tue Jul 04 19:24:29 2017 +0000
@@ -3,6 +3,7 @@
 #include "MotorControl.h"
 #include "Vision.h"
 #include "MMA8451Q.h"
+#include "PIDcontroller.h"
 
 Serial pc(USBTX,USBRX);
 Serial debug(PTD3,PTD2);
@@ -11,11 +12,13 @@
 Timer t;
 Motors motors;
 Vision vision;
+PID steeringPID;
 
 uint32_t convCounter = 0;
 
 float getSteeringValue(float pixelIndex)
 {
+    // return steering angle to send to servo function of pixel target
     return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0248)*pixelIndex-0.9129;
     //return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0224)*pixelIndex-0.9218;
 }
@@ -125,6 +128,10 @@
     batLevelTicker.attach(&updateBatteryLevel,0.2);
     
     motors.setSpeedTargetValue(2.0);
+    
+    steeringPID.setReference(0);
+    steeringPID.setCoefficients(2,0,0);
+    steeringPID.setOutputLimits(-0.6,0.6);
 
     while(1)
     {
@@ -153,8 +160,15 @@
             pixelTarget -= (63.5-l)*0.1;
         else
             pixelTarget += (r-63.5)*0.1;*/
+
         steering = getSteeringValue(pixelTarget);
-        applySteeringCommand(steeringFilter(steering));
+        
+        // previous
+        //applySteeringCommand(steeringFilter(steering));
+        
+        
+        // new
+        applySteeringCommand(steeringPID.processNewValue(steering));        
 
         motors.processTasks();
         //motorPWM = TFC_ReadPot(0);
@@ -185,7 +199,7 @@
             //pc.printf("Bat: %fV, d=%f, time:%dus max light:%d @ %d    \r",TFC_ReadBatteryVoltage(),steering,t.read_us(),lineData.maxLightValue,lineData.mostWhiteIndex);
         }*/
         
-        /*if((convCounter % 100) == 0) // each 0.2 second
+        if((convCounter % 100) == 0) // each 0.2 second
         {
             //pc.printf("%d %d %d %d %d %d %d %d %d\r\n",TFC_LineScanImage0[0],TFC_LineScanImage0[15],TFC_LineScanImage0[31],TFC_LineScanImage0[47],TFC_LineScanImage0[63],TFC_LineScanImage0[79],TFC_LineScanImage0[95],TFC_LineScanImage0[111],TFC_LineScanImage0[127]);
             
@@ -199,18 +213,20 @@
             //for(i=rightEdgeIndex;i<128;i++) pc.printf(" ");
             //pc.printf("]\r\n");
             
-            for(i=0;i<128;i++)
+            pc.printf("%d %d => %f\r\n",l,r,steering);
+            
+            /*for(i=0;i<128;i++)
             {
                 pc.printf("%d ",TFC_LineScanImage0[i]);
             }
-            pc.printf("\r\n");
-        }*/
+            pc.printf("\r\n");*/
+        }
         
         //pc.printf("%d %f %f %f    \r",lastcount,acc.getAccX(),acc.getAccY(),acc.getAccZ());
         if(debugon && (convCounter != lastcount))
         {
             lastcount = convCounter;
-            debug.printf("%d %f %f %f\n",lastcount,motors.getCarSpeed(),motors.getSpeedError(),steering);
+            //debug.printf("%d %f %f %f\n",lastcount,motors.getCarSpeed(),motors.getSpeedError(),steering);
             //debug.printf("%d %f %f %f\n",lastcount,motors.getWheelSpeed('A'),motors.getWheelSpeed('B'),steering);
             //debug.printf("%d %f %f %f\n",lastcount,motors.getMotCurrent('A'),motors.getMotCurrent('B'),steering);
             //debug.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotPWM('B'),steering);