Colony

/*

Authors: James Kong and Greg Tress

Last Modified: 4/30/06 by James

-Started log_distance conversion function !!!NOT COMPLETE!!!

-Cleaning up comments

-----------------

rangefinder.c

Using Sharp GP2D02 IR Rangefinder

Vin is the input to the rangefinder, designated RANGE_CTRL.

Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from

Expected Initial Conditions:

Vin is high and Vout should read high.

Usage:

1.) Set Vin low. Vout should read low.

2.) Wait for high on Vout.

3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).

4.) Set Vin high for 2ms or more to turn off rangefinder

*/

#include "firefly+_lib.h"

#include "rangefinder.h"

/*

read_distance returns the 8-bit reading from the rangefinder

parameters:

range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]

NOTE:

The Sharp GD2D02 returns values on a decreasing logrithmic scale.

So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.

Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.

At this point, we are only reading from one rangefinder [RANGE_IN0].

*/

int read_distance (int range_id) {  

#ifndef FFPP

        int bitcount;

        int current = 0;

        digital_output(RANGE_CTRL, 0);                                                //set Vin Low

        while(digital_input(range_id) == 0);                                //waits until Vout reads high

        digital_output(RANGE_CTRL, 1);                                                //first Vin clock high transition

        for(bitcount = 8; bitcount != 0; bitcount--){

                digital_output(RANGE_CTRL, 0);                                        //Vin clock low transition

                //clock delay (unnecessary)

                current = current << 1;                                                        //shifts current 8-bit value left

                current |= digital_input(range_id);                                //LSB set to Vout value

                digital_output(RANGE_CTRL, 1);                                        //Vin clock high transition

                //clock delay (unnecessary)

        }                                                                                                        //repeats until all 8 bits are stored

        return current;

#else

        enable_IR();

        _delay_ms(5);                                                                                ///////Figure out what this should be!!!!!!  Maybe if not enabled already?

        return read_IR_val(range_id);

#endif

}

/*

linearize_distance converts an 8-bit rangefinder reading to a centimeter measurement (truncated to an integer)

parameters:

reading - 8-bit rangefinder reading

OFFSET, GAIN, and THRESHOLD are experimentally determined constants

The conversion is a piecewise defined function with a continuity fix at reading = THRESHOLD3

NOTE:

This function is ugly, but it gets the job done without spending a huge amount processing.

Ideally, you want to compare with the read_distance value directly.

*/

int linearize_distance (int reading) {

#ifndef FFPP

        int temp = reading;

        if(reading >  THRESHOLD1){

                temp = temp - OFFSET1;

                return GAIN1 / temp;

        }else if(reading > THRESHOLD2){

                temp = temp - OFFSET2;

                return GAIN2 / temp + 1;

        }else if(reading > THRESHOLD3){

                temp = temp - OFFSET2;

                return GAIN2 / temp;

        }else if(reading == THRESHOLD3){

                return CONTINUITY_FIX;

        }else if(reading > THRESHOLD4){

                temp = temp - OFFSET3;

                return GAIN3 / temp;

        }else{

                return MAX_DIST;

        }

#else

        return convert_IR_distance(reading);

#endif

}

/*

log_distance converts a centimeter value to a rangefinder comparable value

parameters:

distance - centimeter measurement

NOTE:

This is still incomplete, do not use it.

*/

int log_distance(int distance) {

#ifndef FFPP

        if(distance >= MAX_DIST){

                return THRESHOLD4;

        }

        if(distance <= MIN_DIST){

                return 255;

        }        

        switch(distance){                        //Incomplete need to fill out table

        case 60:                                

                return 56;

        case 59:

                return 89;

        }

#endif

        return 0;

}

#ifdef FFPP

static int IR_dist_conversion[114] = {

800,791,751,714,681,651,623,597,574,552,531,512,494,478,462,447

,434,421,408,397,386,375,365,356,347,338,330,322,315,307,301,294

,288,282,276,270,265,260,255,250,245,241,237,232,228,224,221,217

,213,210,207,203,200,197,194,191,189,186,183,181,178,176,173,171

,169,166,164,162,160,158,156,154,152,151,149,147,145,144,142,140

,139,137,136,134,133,131,130,129,127,126,125,124,122,121,120,119

,118,117,115,114,113,112,111,110,109,108,107,106,105,105,104,103

,102,101

};

void enable_IR()

{

  // active low, so set enable to low to turn on IR

  PORTC &= ~(_BV(IR_ENABLE));

  DDRC |= _BV(IR_ENABLE);

}

void disable_IR()

{

  // active low, so set line high and turn to input for good measure (line has pull-up)

  PORTC |= _BV(IR_ENABLE);

  DDRC &= ~(_BV(IR_ENABLE));

}

int read_IR_val(int which)

{

  return analog8(IR_READ + which);

}

int convert_IR_distance(int value)

{

  if(value < MIN_IR_ADC8)

  {

    return -1;

  }

  else if(value > MAX_IR_ADC8)

  {

    return -1;

  }

  else

  {

    return IR_dist_conversion[value - MIN_IR_ADC8];

  }

}

int get_IR_distance(int which)

{

  return convert_IR_distance(read_IR_val(which));

}

#endif