Diff: main.cpp

Revision:

0:5e090fec6099

Child:

1:5402c62ad487

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 01 13:21:32 2017 +0000
@@ -0,0 +1,220 @@
+#include "mbed.h"
+#include "TFC.h"
+#include "MotorControl.h"
+#include "Vision.h"
+#include "MMA8451Q.h"
+
+Serial pc(USBTX,USBRX);
+Serial debug(PTD3,PTD2);
+Ticker convTicker,batLevelTicker;
+Timeout camExpoTimeout;
+Timer t;
+Motors motors;
+Vision vision;
+
+uint32_t convCounter = 0;
+
+float getSteeringValue(float pixelIndex)
+{
+    return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0248)*pixelIndex-0.9129;
+    //return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0224)*pixelIndex-0.9218;
+}
+
+float flt[4]={0,0,0,0};
+float steeringFilter(float x)
+{
+    return 2.0*x;
+    
+/*  uint8_t i;
+    
+    for(i=3;i>0;i--)
+    {
+        flt[i] = flt[i-1];
+    }
+    flt[0] = x;
+    
+    return (flt[0]+flt[1]*0.75+flt[2]*0.5+flt[3]*0.25)/2.5;*/
+}
+
+void applySteeringCommand(float value)
+{
+    float clamp = 0.55;
+    if(value > clamp)
+        value = clamp;
+    if(value < -clamp)
+        value = -clamp;
+    TFC_SetServo(0,value);
+}
+
+void stopCameraExposure()
+{
+}
+
+void dataConversionRoutine()
+{
+    uint8_t mask;
+    
+    // convertion of motor current is done at each tick
+    mask = ADC_MASK_CAPTURE_MOTOR_CURRENT;
+    
+    // convertion of line scan is done each 5 ticks
+    if((convCounter % 5) == 0)
+        mask |= ADC_MASK_CAPTURE_LINE_SCAN;
+    
+    // convertion of pot and battery is done each 5 ticks
+    if((convCounter % 5) == 2)
+        mask |= ADC_MASK_CAPTURE_POT_0 | ADC_MASK_CAPTURE_POT_1 | ADC_MASK_CAPTURE_BATTERY_LEVEL;
+
+    TFC_StartDataAcquisition(mask);
+    
+    convCounter++;
+    if(convCounter == 4000000000)
+        convCounter = 0;
+    
+    //TFC_StartDataAcquisition(ADC_MASK_CAPTURE_ALL);
+    //camExpoTimeout.attach(&stopCameraExposure,camExposure);
+}
+
+void updateBatteryLevel()
+{
+    float batteryVoltage;
+    batteryVoltage = motors.getAverageBatteryVoltage();
+    TFC_SetBatteryLED_Level(batteryVoltage*3.3-18.8);
+}
+
+void saveADCMeas()
+{
+    //TFC_BAT_LED2_TOGGLE;
+    if(TFC_MotorCurrentReady)
+    {
+        TFC_MotorCurrentReady = 0;
+        motors.saveMotorCurrentMeasure(TFC_ReadMotACurrentRaw(),TFC_ReadMotBCurrentRaw()); 
+    }
+    if(TFC_LineScanImageReady)
+    {
+        TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame
+        vision.saveData((uint16_t*)TFC_LineScanImage0);
+    }
+    if(TFC_BatteryVoltageReady)
+    {
+        TFC_BatteryVoltageReady = 0;
+        motors.saveBatteryVoltageMeasure(TFC_ReadBatteryVoltageRaw());
+    }
+}
+
+int main()
+{
+    //uint8_t i;
+    uint8_t r,l;
+    float motorPWM,steering;
+    float pixelTarget;
+    struct lineScanData lineData;
+    uint32_t lastcount = 0;
+    bool debugon = false;
+    
+    //MMA8451Q acc(PTE25, PTE24, (0x1d<<1));
+    
+    TFC_Init();
+    
+    pc.baud(115200);
+    debug.baud(115200);
+    
+    convTicker.attach_us(&dataConversionRoutine, 2000);
+    TFC_ConvertionReadyCallback.attach(&saveADCMeas);
+    
+    batLevelTicker.attach(&updateBatteryLevel,0.2);
+    
+    motors.setSpeedTargetValue(2.0);
+
+    while(1)
+    {
+        if(TFC_ReadPushButton(0))
+        {
+            motors.stop();
+            debugon = false;
+        }
+        
+        if(TFC_ReadPushButton(1))
+        {
+            motors.start();
+            debugon = true;
+        }
+        
+        vision.processTasks();
+        lineData = vision.getRoadData();
+        r = lineData.rightEdgeIndex;
+        //if(r>=115)
+        //    r = 128;
+        l = lineData.leftEdgeIndex;
+        //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);
+        applySteeringCommand(steeringFilter(steering));
+
+        motors.processTasks();
+        //motorPWM = TFC_ReadPot(0);
+        //motorPWM = (TFC_GetDIP_Switch()*0.05+0.25);
+        //motors.setFixedPWMValue(motorPWM);
+        
+        /*if(TFC_LineScanImageReady)
+        {
+            TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame
+
+            // calculation time measure
+            t.reset();
+            t.start();
+            
+            //lineScanLightAdjust((uint16_t*)TFC_LineScanImage0);
+            lineData = lineScanProcess((uint16_t*)TFC_LineScanImage0);
+            pixelTarget = (lineData.leftEdgeIndex+lineData.rightEdgeIndex)/2.0;
+            steering = getSteeringValue(pixelTarget);
+            
+            applySteeringCommand(steeringFilter(steering));
+            
+            // stop calculation time measurement
+            t.stop();
+            
+            //pc.printf("Bat: %fV, PWM:%f, MA:%frps, MB:%frps\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getWheelRPS('A'),motors.getWheelRPS('B'));
+            //pc.printf("Bat: %fV, PWM:%f, MA:%fV, MB:%fV\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
+            //pc.printf("Bat: %fV, PWM:%f, MotA:%f, MotB:%f, speedA:%f, speedB:%f\r",TFC_ReadBatteryVoltage(),motorPWM,TFC_ReadMotACurrent(),TFC_ReadMotBCurrent(),getMotSpeed('A',motorPWM),getMotSpeed('B',motorPWM));
+            //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
+        {
+            //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]);
+            
+            //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");
+            
+            for(i=0;i<128;i++)
+            {
+                pc.printf("%d ",TFC_LineScanImage0[i]);
+            }
+            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.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);
+            //debug.printf("%d %f %f %f\n",lastcount,acc.getAccX(),acc.getAccY(),steering);
+        }
+    }
+}
\ No newline at end of file