Colony

/**

 * Copyright (c) 2007 Colony Project

 *

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 *

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 *

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file rangefinder.c

 * @brief Rangefinders

 *

 * Implementation of functions for rangefinder use.

 *

 * @author Colony Project, CMU Robotics Club

 **/

/*

  Authors: James Kong and Greg Tress

  Last Modified: 10/30/09 by Dan Shope

  -Revised LUT to match GP2D120X Sensors (4-30cm)

  -range_read_distance() now returns -1 for below lower bound, -2 for beyond upper bound

  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 <avr/pgmspace.h>

#include "dragonfly_defs.h"

#include "analog.h"

#include "dio.h"

#include "rangefinder.h"

unsigned char range_initd=0;

/*

  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].

*/

// constants

/* Averaged over 5 robots, should give real (mm) values to +/-10 millimeters

   Note the minimum value is .4V (20), and the maximum is 2.6V (133).

   Gives distance in mm.

   excel formula(valid for inputs 20-133):  ROUND(2353.6*(E2^(-1.1146))*10,0)

   This is only valid for the GP2D120X, with objects directly ahead and more than

   10cm from the detector.  See the datasheet for more information.

*/

static int IR_dist_conversion[72] PROGMEM = {

        327,315,303,291,281,271,262,253,245,238,231,224,218,212,206,200,

        195,190,185,181,177,173,168,165,161,158,155,151,148,145,143,140,

        137,134,132,130,127,125,123,121,119,117,115,114,111,110,108,106,

        105,104,102,100,99,98,97,95,94,93,91,90,89,88,87,86,84,83,83,82,

        81,80,79,78

};

/**

 * @defgroup rangefinder Rangefinder

 * @brief Functions for using the IR rangefinders

 *

 * Functions for using the IR rangefinders.

 *

 * @{

 **/

/**

 * Initializes the rangefinders. This must be called before

 * range_read_distance.

 *

 * @see range_read_distance

 **/

int range_init(void)

{

  if(range_initd)

    return ERROR_INIT_ALREADY_INITD;

  digital_output(_PIN_B4,0);

  range_initd=1;

  return 0;

}

/**

 * Reads the distance measured by one of the rangefinders.

 * This distance is in arbitrary units.

 *

 * @param range_id the rangefinder to use. This should be one

 * of the constants IR1 - IR5.

 *

 * @return the distance measured by the rangefinder, or an error code

 *

 * @see range_init

 **/

int range_read_distance(int range_id) {

  if(!range_initd)

    return -3;

  return linearize_distance(analog8(range_id));

}

/**

 * Transforms distance readings from logarithmic to linear scale.

 * This probably isn't the function you are looking for.

 *

 * Note: pgm_read_word() needs additional testing

 *

 * @param value the 8-bit analog value from rangefinder

 *

 * @return linearized distance reading from rangefinder (integer in [78,327])

 * @return (-2) if below MIN_IR_ADC8

 * @return (-1) if beyond MAX_IR_ADC8

 **/

int linearize_distance(int value) {

  if(value < MIN_IR_ADC8) {

    return -2;

  } else if(value > MAX_IR_ADC8) {

    return -1;

  } else {

    return pgm_read_word(&(IR_dist_conversion[value - MIN_IR_ADC8]));

  }

}

/** @} **/ //end defgroup