Part of the OU_Davis_Old_Robot Library

QTR_8A.cpp

Committer:
DrewSchaef
Date:
2017-11-01
Revision:
0:c4f495ae2ec6

File content as of revision 0:c4f495ae2ec6:

//ported from arduino with some changes 
//by Austin Saunders

#include "mbed.h"
#include "QTR_8A.h"
#include "MCP3008.h"

//6 sensors

/*
QTR_8A::QTR_8A(PinName L3, PinName L2, PinName L1, PinName R1, PinName R2, PinName R3, PinName E) : _L3(L3), _L2(L2), _L1(L1), _R1(R1), _R2(R2), _R3(R3), _E(E)
{
    calibratedMinimum=0;
    calibratedMaximum=0;
}
*/
MCP3008 Ain(p11,p12,p13,p14);

QTR_8A::QTR_8A(PinName E) :  _E(E)
{
    calibratedMinimum=0;
    calibratedMaximum=0;
    emittersOff();
}



QTR_8A::~QTR_8A()
{

}


void QTR_8A::emittersOn()
{
    _E = 1;
    wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!!
}

void QTR_8A::emittersOff()
{
    _E = 0;
    wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!!
}

void QTR_8A::read(unsigned int *sensor_values)
{
    int numSamplesPerSensor = 4;
    //reset the values
    for(int i=0; i<8; i++) {
        sensor_values[i] = 0;
    }
    emittersOn();
    for (int j = 0; j < numSamplesPerSensor; j++) {
        sensor_values[0]+=Ain.read(0);
        sensor_values[1]+=Ain.read(1);
        sensor_values[2]+=Ain.read(2);
        sensor_values[3]+=Ain.read(3);
        sensor_values[4]+=Ain.read(4);
        sensor_values[5]+=Ain.read(5);
        sensor_values[6]+=Ain.read(6);
        sensor_values[7]+=Ain.read(7);
    }
    emittersOff();
    // get the rounded average of the readings for each sensor
    for (int i = 0; i < 8; i++) {
        sensor_values[i] = (sensor_values[i]+(numSamplesPerSensor>>1))/numSamplesPerSensor;
    }

}

void QTR_8A::calibrate()
{
    calibrateon(&calibratedMinimum,&calibratedMaximum);
}

void QTR_8A::calibrateon(unsigned int**calibratedMinimum,unsigned int**calibratedMaximum){

    unsigned int sensor_values[8];
    unsigned int max_sensor_values[8];
    unsigned int min_sensor_values[8];

    // Allocate the arrays if necessary.
    if(*calibratedMaximum == 0) {
        *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*8);

        // If the malloc failed, don't continue.
        if(*calibratedMaximum == 0) {
            return;
        }
        // Initialize the max and min calibrated values to values that
        // will cause the first reading to update them.

        for(int i=0; i<8; i++) {
            (*calibratedMaximum)[i] = 0;
        }
    }
    if(*calibratedMinimum == 0) {
        *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*8);

        // If the malloc failed, don't continue.
        if(*calibratedMinimum == 0) {
            return;
        }

        for(int i=0; i<8; i++) {
            (*calibratedMinimum)[i] = 1023;
        }
    }

    for(int j=0; j<10; j++) {
        read(sensor_values);
        for(int i=0; i<8; 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];
        }
    }

    // record the min and max calibration values
    for(int i=0; i<8; i++) {
        if(min_sensor_values[i] > (*calibratedMaximum)[i])
            (*calibratedMaximum)[i] = min_sensor_values[i];
        if(max_sensor_values[i] < (*calibratedMinimum)[i])
            (*calibratedMinimum)[i] = max_sensor_values[i];
    }
}



void QTR_8A::resetCalibration()
{
    for(int i=0; i<8; i++) {
        calibratedMinimum[i] = 1023;
        calibratedMaximum[i] = 0;
    }
}

void QTR_8A::readCalibrated(unsigned int *sensor_values)
{
    if(!calibratedMinimum || !calibratedMaximum) {
        return;
    }
    read(sensor_values);

    for(int i=0; i<8; i++) {
        unsigned int calmin,calmax;
        unsigned int denominator;
        calmax = calibratedMaximum[i];
        calmin = calibratedMinimum[i];
        denominator = calmax - calmin;

        signed int x = 0;
        if(denominator != 0) {
            x = (((signed long)sensor_values[i]) - calmin)* 1000 / denominator;
        }
        if(x < 0) {
            x = 0;
        } else if(x > 1000) {
            x = 1000;
        }
        sensor_values[i] = x;

    }
}

int QTR_8A::readLine(unsigned int *sensor_values)
{
    unsigned int on_line = 0;
    unsigned long avg; // this is for the weighted total, which is long
    // before division
    unsigned 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(int i=0; i<8; i++) {
        int value = sensor_values[i];


        // keep track of whether we see the line at all
        if(value > 350) {
            on_line = 1;
        }

        // only average in values that are above a noise threshold
        if(value > 20) {
            avg += (long)(value) * (i * 1000);
            sum += value;
        }
    }

    if(!on_line) {
        // If it last read to the left of center, return 0.
        if(last_value < 3500) {
            return 0;
        }

        // If it last read to the right of center, return the max.
        else {
            return 7000;
        }

    }

    last_value = avg/sum;

    return last_value;
}