Diff: main.cpp

Revision:

3:c21068661635

Parent:

2:eb79d6e3f701

--- a/main.cpp	Wed Jul 05 20:31:26 2017 +0000
+++ b/main.cpp	Wed Feb 20 22:22:48 2019 +0000
@@ -10,9 +10,11 @@
 Ticker convTicker,batLevelTicker;
 Timeout camExpoTimeout;
 Timer t;
+Ticker test;
 Motors motors;
 Vision vision;
 PID steeringPID;
+PID speedPID;
 
 uint32_t convCounter = 0;
 
@@ -71,7 +73,7 @@
     TFC_StartDataAcquisition(mask);
     
     convCounter++;
-    if(convCounter == 4000000000)
+    if(convCounter >= 4000000000)
         convCounter = 0;
     
     //TFC_StartDataAcquisition(ADC_MASK_CAPTURE_ALL);
@@ -105,9 +107,54 @@
     }
 }
 
+void printCarSpeed(Serial& output, Motors motors, uint32_t index)
+{
+    output.printf("%d %f %f\r\n",index,motors.getCarSpeed(),motors.getSpeedError());
+}
+
+void printAll(Serial& output, Motors motors, float PWM, float steering, uint32_t index)
+{
+    output.printf("%d %f %f %f\n",index,motors.getCarSpeed(), PWM ,steering);
+}
+
+void printEdges(Serial& output, struct lineScanData lineData)
+{
+    uint8_t i;
+    
+    output.printf("[");
+    for(i=0;i<lineData.leftEdgeIndex;i++) output.printf(" ");
+    output.printf("^");
+    for(i=lineData.leftEdgeIndex;i<lineData.mostWhiteIndex;i++) output.printf(" ");
+    output.printf("|");
+    for(i=lineData.mostWhiteIndex;i<lineData.rightEdgeIndex;i++) output.printf(" ");
+    output.printf("^");
+    for(i=lineData.rightEdgeIndex;i<128;i++) output.printf(" ");
+    output.printf("]\r\n");
+}
+
+void printLine(Serial& output)
+{
+    uint8_t i;
+    
+    for(i=0;i<128;i++)
+    {
+        output.printf("%d ",TFC_LineScanImage0[i]);
+    }
+    output.printf("\r\n");
+}
+
+bool dir = false;
+void invertMot()
+{
+    if(dir)
+        motors.setFixedPWMValue(1.0);
+    else
+        motors.setFixedPWMValue(-1.0);
+    dir = !dir;
+}
+
 int main()
 {
-    uint8_t i;
     uint8_t r,l;
     float motorPWM,steering,steeringFiltered;
     float pixelTarget;
@@ -122,6 +169,8 @@
     pc.baud(115200);
     debug.baud(115200);
     
+//    test.attach(&invertMot,2);
+    
     // run convertion routine each 2ms
     convTicker.attach_us(&dataConvertionRoutine, 2000);
     // record data when convertion done
@@ -131,11 +180,15 @@
     
     motors.setFixedPWMMode();
     motors.setSpeedTargetValue(2.0);
-    motors.setPWM(0.45);
+    motors.setFixedPWMValue(0.45);
     
     steeringPID.setReference(0);
-    steeringPID.setCoefficients(1.5,0,0);
+    steeringPID.setCoefficients(1.2,0,0);
     steeringPID.setOutputLimits(-0.6,0.6);
+    
+    speedPID.setReference(1.0);
+    speedPID.setCoefficients(0.6,3,0);
+    speedPID.setOutputLimits(-1.0,1.0);
 
     while(1)
     {
@@ -160,10 +213,6 @@
         //if(l<=13)
         //    l = 0;
         pixelTarget = (r+l)/2.0;
-        /*if(pixelTarget < 63.5)
-            pixelTarget -= (63.5-l)*0.1;
-        else
-            pixelTarget += (r-63.5)*0.1;*/
 
         steering = getSteeringValue(pixelTarget);
         
@@ -172,10 +221,17 @@
         
         
         // new
-        steeringFiltered = steeringPID.processNewValue(steering);
+        steeringFiltered = steeringPID.processNewValue(-steering);
         applySteeringCommand(steeringFiltered);
-        motorPWM = 0.6-abs(steeringFiltered)/2.0;
-        motors.setPWM(motorPWM);
+        
+        // drive proportional to steering
+        motorPWM = 0.5-abs(steeringFiltered)/2.0;
+        motors.setFixedPWMValue(motorPWM);
+        
+        // speed is regulated
+        //speedPID.setReference(1.8-abs(steeringFiltered)*2.5); // set speed to 2m/s max and decrease to 0.5m/s if steering is at max
+        //motorPWM = speedPID.processNewValue(motors.getCarSpeed());
+        //motors.setFixedPWMValue(motorPWM);
 
         motors.processTasks();
         //motorPWM = TFC_ReadPot(0);
@@ -201,34 +257,22 @@
         
         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]);
+            //printEdges(pc);
+            //printLine(pc);
             
-            //pc.printf("[");
-            //for(i=0;i<leftEdgeIndex;i++) pc.printf(" ");
-            //pc.printf("^");
-            //for(i=leftEdgeIndex;i<mostWhiteIndex;i++) pc.printf(" ");
-            //pc.printf("|");
-            //for(i=mostWhiteIndex;i<rightEdgeIndex;i++) pc.printf(" ");
-            //pc.printf("^");
-            //for(i=rightEdgeIndex;i<128;i++) pc.printf(" ");
-            //pc.printf("]\r\n");
-            
-            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("%d %d => %f\r\n",l,r,steering);
+            //pc.printf("%f %f\r",motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
         }
         
         //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.getWheelSpeed('A'),motors.getWheelSpeed('B'),steering);
-            //debug.printf("%d %f %f %f\n",lastcount,motors.getMotCurrent('A'),motors.getMotCurrent('B'),steering);
+            //printAll(debug, motors, motorPWM, steeringFiltered, lastcount);
+            //printCarSpeed(debug, motors, lastcount);
+            //pc.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotCurrent('A'),motors.getWheelSpeed('A'));
+            //pc.printf("%f\r",speedPID.getIntegralTerm());
+            //debug.printf("%d %f %f %f\n",lastcount,motors.getMotCurrent('A'),motors.getMotCurrent('B'),motors.getCarSpeed(),);
             //debug.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotPWM('B'),steering);
             //debug.printf("%d %f %f %f\n",lastcount,acc.getAccX(),acc.getAccY(),steering);
         }