Object Tracking Sensor

In this project I made an object tracker using the dsPIC30F4011 PIC and a SHARP infrared distance sensor mounted on a servo motor.

The idea was to scan left and right with the servo motor until the IR sensor detected an object.
When the edge of an object is detected it will turn away from it until it no longer detects it then turn back towards it. So the servo will be constantly scanning the edge of the object even when the object is moving.

This is a diagram of the system:

This is a flow chart of my system:

The output voltage of the infrared sensor was then measured at different distances, the results were as follows:

Plotted on a graph it looked like this:

Here is my program:

//////////////////////////////////////////////////////////////////////////////
// Robotics 3.1 servo assignment 1.                                         //
// Tracking Sensor using ds pic & IR sensor                                 //
// Aron Horan 10/10/13                                                      //
//////////////////////////////////////////////////////////////////////////////
//
// Servo-mounted rangefinder scans left and right. When an object is detected,
// the rangefinder moves back and forth tracking the left edge of the object.
//
//////////////////////////////////////////////////////////////////////////////

#include <xc.h>
#include <libpic30.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

// Configure max and min OC1RS values
int min = 200;          // OC1RS value for 0 degrees
int max = 1150;         // OC1RS value for 180 degrees

// Function prototypes
void setup();
unsigned int read_analog_channel(int sensor);

int main()
{
    // Configure UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
    
    // Configure digital i/o, analog inputs, Timer 2, Output Compare
    setup();

    // Define some numbered states.
    // These values won't change, so we can
    // declare them as "const" which prevents
    // them from getting changed by accident.
    const int scan_left = 1;
    const int scan_right = 2;
    const int object_detected = 3;

    // Set initial state and then enter state loop
    int distance;
    int angle = 1;
    int state = scan_right;
    int threshold = 350;
    
    while(1)
    {
        // Update distance reading
        distance = read_analog_channel(0);
        
        if (state == scan_right)
        {
            // Turn on YELLOW LED to show its in scan right state
            LATE = 0b0001; // RE0 high, RE1-3 low
            
            // Keep moving right
            angle++;
            
            // If max angle is reached, scan in other direction
            if (angle >= 180) state = scan_left;
            
            // If an object is detected, switch state
            if (distance >= threshold) state = object_detected;
            __delay32(150000); // 5ms delay
        }
        else if (state == scan_left)
        {
            // Turn on GREEN LED to show its in scan left state
            LATE = 0b0010; // RE1 high

            // Keep moving left
            angle--;
            
            // If min angle is reached, scan in other direction
            if (angle <= 0) state = scan_right;
            
            // If an object is detected, switch state
            if (distance >= threshold) state = object_detected;
            __delay32(150000); // 5ms delay
        }
        else if (state == object_detected)
        {
            // Turn on RED LED to show its in object detected state
            LATE = 0b0100; // RE2 high

            // Scan left looking for left edge of detected object
            angle--;
            
            // If the minimum angle is reached, just stay still
            if (angle <= 0) angle = 0;
            if (angle >= 180) angle = 180;
            
            // If the object disappears, begin scanning right again
            if (distance < threshold) state = scan_right;
            __delay32(150000); // 5ms delay
        }

        // Set servo to current angle 
        OC1RS = min + (angle/180.0)*(max-min);      
    }
    
    return 0;
}

void setup()
{
    // Configure RD0, RD1 and as outputs
    TRISD = 0b1100;
    TRISE = 0b111110000; // set RE0-3 as digital outputs

    // Configure Timer 2 (default timer for output compare)
    T2CONbits.TCKPS = 0b10; // Timer 2 prescaler 1:64
    PR2 = 9375;             // Timer 2 period (20ms)
    T2CONbits.TON = 1;      // Enable Timer 2

    // Configure Output Compare channel 1 (OC1)
    OC1CONbits.OCM = 0b101; // continuous pulse mode
    OC1R = 0;               // pulse start time
    OC1RS = min;            // pulse stop time

    // Configure digital I/O
    LATD = 0;
    TRISD = 0b11111110;

    // Configure analog inputs
    TRISB = 0x01FF;      // Port B all inputs
    ADPCFG = 0xFF00;     // Lowest 8 PORTB pins are analog inputs
    ADCON1 = 0;          // Manually clear SAMP to end sampling, start conversion
    ADCON2 = 0;          // Voltage reference from AVDD and AVSS
    ADCON3 = 0x0005;     // Manual Sample, ADCS=5 -> Tad = 3*Tcy = 0.1us
    ADCON1bits.ADON = 1; // Turn ADC ON
}

// This function reads a single sample from the specified
// analog input. It should take less than 2.5us if the chip
// is running at about 30 MIPS.
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 1.2us
    return ADCBUF0;
}

Here it is in action!

The End!