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, Greg Tress, Emily Hart

  Last Modified: 5/12/10 by Emily

  - Added Butterworth filtering functions. As of right now, the filter should

    only be used if you really know what you are doing. These functions were

    designed to be used with a task scheduler, which is not currently complete.

    This implementation uses the RTC and runs massive computations in the RTC

    interrupt.

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

#include "rangefinder.h"

#include "analog.h"

#include "dio.h"

#include "time.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].

*/

// constants

/* Nasty IR approximation table

   I'm using this for the heck of it.  We can do whatever.

   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 GP2D12, 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

};

/* 1 if the filter is enabled, else 0 */

static int use_filter;

// VALUE - MIN_IR_LINEAR is stored so only 8 bits are needed

/* X values for the Butterworth filter, for each rangefinder */

static uint8_t butter_x[5][4];

/* Y values for the Butterworth filter, for each rangefinder */

static uint8_t butter_y[5][3];

/* How many consecutive -1s have been seen by each rangefinder */

static uint8_t neg_one_count[5];

/**

 * @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. This function does not initialize the filter.

 *

 * @see range_read_distance

 **/

void range_init(void)

{

  digital_output(_PIN_B4,0);

  use_filter = 0;

}

/**

 * Initializes the rangefinders with an option to enable the Butterworth

 * filtering.

 *

 * As of 5/12/2010, the filter should only be used if you really know what you

 * are doing. It was designed to be used with a task scheduler, which is not

 * currently complete. This implementation uses the RTC and runs massive

 * computations in the RTC interrupt.

 *

 * @param filter 1 to enable the filter, 0 to leave it turned off

 **/

void range_init_filter(int filter)

{

    range_init();

    if(filter){

        use_filter = 1;

        butter_init();

    }

}

/**

 * 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

 *

 * @see range_init

 **/

int range_read_distance(int range_id) {

  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 [101,800])

 **/

int linearize_distance(int value) {

  if(value < MIN_IR_ADC8) {

    return -1;

  } else if(value > MAX_IR_ADC8) {

    return -1;

  } else {

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

  }

}

/**

 * Initializes the butterworth filter.

 **/

void butter_init(void)

{

    int i;

    // init -1 count to 3 for each rangefinder

    // this will cause them to restart the filter

    for(i=0; i<5; i++)

    {

        neg_one_count[i] = 3;

    }

    // set up the rtc to run butter_task periodically

    rtc_init(SIXTEENTH_SECOND, butter_task);

}

/*

 * Reads each rangefinder and sends its value through the Butterworth filter.

 * This task should be run frequently so that range_read_filtered_distance

 * returns fresh values

 */

void butter_task(void)

{

    butter_filter(IR1, range_read_distance(IR1));

    butter_filter(IR2, range_read_distance(IR2));

    butter_filter(IR3, range_read_distance(IR3));

    butter_filter(IR4, range_read_distance(IR4));

    butter_filter(IR5, range_read_distance(IR5));

}

/*

 * Butterworth helper function that takes a rangefinder ID and turns it into

 * an array index between 0 and 4.

 */

int get_range_index(int range_id){

    switch(range_id){

    case IR1:

        return 0;

    case IR2:

        return 1;

    case IR3:

        return 2;

    case IR4:

        return 3;

    case IR5:

        return 4;

    default: // should never happen

        return 0xff;

    }

}

/**

 * Puts the given value from the given rangefinder through the Butterworth

 * filter. This function should be called every time a new value is read from a

 * rangefinder.

 *

 * The Butterworth filter has a cutoff frequency of 5Hz and an order of 3

 *

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

 * IR1 - IR5.

 * @param the value read from that rangefinder

 **/

void butter_filter(int range_id, int val)

{

    int range_index = get_range_index(range_id);

    // we have a non-error value

    if(val > -1 && val <= MAX_IR_LINEAR)

    {

      // we just passed two or fewer -1's: act as though none seen

      if(neg_one_count[range_index] < 3)

      {

        // shift the values of the arrays to the left

        int i;

        for(i=0; i<2; i++)

        {

          butter_x[range_index][i] = butter_x[range_index][i+1];

          butter_y[range_index][i] = butter_y[range_index][i+1];

        }

        butter_x[range_index][2] = butter_x[range_index][3];

        // add the new value to the X array

        butter_x[range_index][3] = val - MIN_IR_LINEAR;

      }

      // we just passed three or more -1 values: reset filter with new value

      else

      {

        int i;

        // fill x and y values with the new value

        for(i=0; i<3; i++)

        {

          butter_x[range_index][i] = val - MIN_IR_LINEAR;

          butter_y[range_index][i] = val - MIN_IR_LINEAR;

        }

        butter_x[range_index][3] = val - MIN_IR_LINEAR;

      }

      // reset the -1 count value

      neg_one_count[range_index] = 0;

      /*

       * butterworth filter the last values

       *

       * butterworth filter equation is

       * y(t) = x(t-3)/6 + x(t-2)/2 + x(t-1)/2 + x(t)/6 - y(t-2)/3

       *

       * values are multiplied by 16 before divisions, and divided by 16 at the

       * very end to mitigate rounding errors

       *

       * 8 is added before dividing by 16 so that numbers are rounded instead of

       * truncated

       */

      int16_t temp1 = (int16_t)butter_x[range_index][3] +

          (int16_t)butter_x[range_index][0] + (2*MIN_IR_LINEAR);

      int16_t temp2 = (int16_t)butter_x[range_index][2] +

          (int16_t)butter_x[range_index][1] + (2*MIN_IR_LINEAR);

      int16_t temp3 = (int16_t)butter_y[range_index][0] + MIN_IR_LINEAR;

      int16_t filtered_big = ((((temp1*8)-(temp3*16))/3)+(temp2*8)+8)/16;

      filtered_big -= MIN_IR_LINEAR;

      uint8_t filtered = filtered_big > 0xff ? 0xff : (uint8_t)filtered_big;

      butter_y[range_index][2] = filtered;

    }

    // -1 seen - don't want to store it

    else

    {

        // increment -1 count, preventing overflow

        neg_one_count[range_index] = neg_one_count[range_index] == 0xff ? 0xff :

            neg_one_count[range_index]+1;

    }

}

/**

 * Returns the most recent filtered reading of the rangefinder. The raw

 * rangefinder values have been run through a Butterworth filter.

 *

 * If the filter was not initialized in rangefinder_init, will return the

 * unfiltered value from the rangefinder.

 *

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

 * IR1 - IR5.

 **/

int range_read_filtered_distance(int range_id){

    if(!use_filter){

        return range_read_distance(range_id);

    }

    int range_index = get_range_index(range_id);

    // haven't seen too many -1s recently - return filtered value

    if(neg_one_count[range_index] < 3)

    {

        return butter_y[range_index][2] + MIN_IR_LINEAR;

    }

    // have seen several -1s - return -1

    return -1;

}

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