File content as of revision 96:7465ab270e7a:

#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;
}