Diff: TRSensors.cpp

Revision:

95:250afd53b710

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TRSensors.cpp	Sat Jun 08 12:00:38 2019 +0000
@@ -0,0 +1,141 @@
+#include "TRSensors.h"
+
+#define NUMSENSORS 5
+
+TRSensors::TRSensors(PinName p1, PinName p2, PinName p3, PinName p4): spi(p2, p3, p4), cs(p1, 1){
+    _numSensors = NUMSENSORS;
+    
+    calibratedMin = (uint16_t*)malloc(sizeof(uint16_t) * _numSensors);
+    calibratedMax = (uint16_t*)malloc(sizeof(uint16_t) * _numSensors);
+    
+    for(int i=0;i<_numSensors;i++)
+    {
+        calibratedMin[i] = 1023;
+        calibratedMax[i] = 0;
+    }
+    
+    spi.format(16,0);
+    spi.frequency(2000000);
+}
+
+TRSensors::~TRSensors(){
+    free(calibratedMin);
+    free(calibratedMax);
+}
+
+void TRSensors::AnalogRead(uint16_t *sensor_value){
+    unsigned int channel;
+    uint16_t tmp[6];
+    
+    for(channel = 0; channel < 6; channel++)
+    {
+        cs=0;
+        wait_us(2);
+        tmp[channel] = spi.write(channel<<12);
+        cs=1;
+        wait_us(21);
+    }
+    
+    for(int i=1; i<=5; i++)
+        sensor_value[i-1] = (tmp[i]>>6);
+}
+
+void TRSensors::calibrate(){
+    int i, j;
+    uint16_t sensor_values[_numSensors];
+    uint16_t max_sensor_values[_numSensors];
+    uint16_t min_sensor_values[_numSensors];
+    
+    for(j=0;j<10;j++)
+    {
+        AnalogRead(sensor_values);
+        for(i=0;i<_numSensors;i++)
+        {
+            // set the max we found THIS time
+            if(j == 0 || max_sensor_values[i] < sensor_values[i])
+                max_sensor_values[i] = sensor_values[i];
+
+            // set the min we found THIS time
+            if(j == 0 || min_sensor_values[i] > sensor_values[i])
+                min_sensor_values[i] = sensor_values[i];
+        }
+    }
+    
+    for(i=0;i<_numSensors;i++)
+    {
+        if(min_sensor_values[i] > calibratedMax[i])
+            calibratedMax[i] = min_sensor_values[i];
+        if(max_sensor_values[i] < calibratedMin[i])
+            calibratedMin[i] = max_sensor_values[i];
+    }
+}
+
+void TRSensors::readCalibrated(uint16_t *sensor_values){
+    int i;
+
+    // read the needed values
+    AnalogRead(sensor_values);
+
+    for(i=0;i<_numSensors;i++)
+    {
+        uint16_t denominator;
+
+        denominator = calibratedMax[i] - calibratedMin[i];
+
+        uint16_t x = 0;
+        if(denominator != 0)
+            x = (sensor_values[i] - calibratedMin[i])
+                * 1000 / denominator;
+        if(x < 0)
+            x = 0;
+        else if(x > 1000)
+            x = 1000;
+        sensor_values[i] = x;
+    }
+}
+
+uint16_t TRSensors::readLine(uint16_t *sensor_values, unsigned char white_line){
+    unsigned char i, on_line = 0;
+    unsigned long avg; // this is for the weighted total, which is long
+                       // before division
+    int sum; // this is for the denominator which is <= 64000
+    static int last_value=0; // assume initially that the line is left.
+
+    readCalibrated(sensor_values);
+
+    avg = 0;
+    sum = 0;
+  
+    for(i=0;i<_numSensors;i++) {
+        int value = (int)sensor_values[i];
+
+        if(!white_line)
+            value = 1000-value;
+        sensor_values[i] = value;
+        // keep track of whether we see the line at all
+        if(value > 300) {
+            on_line = 1;
+        }
+        
+        // only average in values that are above a noise threshold
+        if(value > 50) {
+            avg += (long)(value) * (i * 1000);
+            sum += value;
+        }
+    }
+
+    if(!on_line)
+    {
+        // If it last read to the left of center, return 0.
+         if(last_value < (_numSensors-1)*1000/2)
+             return 0;
+        
+        // If it last read to the right of center, return the max.
+         else
+             return (_numSensors-1)*1000;
+    }
+
+    last_value = avg/sum;
+
+    return last_value;
+}