Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libdragonfly / rangefinder.c @ 1462

History | View | Annotate | Download (5.15 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26

    
27
/**
28
 * @file rangefinder.c
29
 * @brief Rangefinders
30
 *
31
 * Implementation of functions for rangefinder use.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

    
36
/*
37
  Authors: James Kong and Greg Tress
38

39
  Last Modified: 4/30/06 by James
40
  -Started log_distance conversion function !!!NOT COMPLETE!!!
41
  -Cleaning up comments
42

43
  -----------------
44
  rangefinder.c
45
  Using Sharp GP2D02 IR Rangefinder
46

47
  Vin is the input to the rangefinder, designated RANGE_CTRL.
48
  Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from
49

50
  Expected Initial Conditions:
51
  Vin is high and Vout should read high.
52

53
  Usage:
54
  1.) Set Vin low. Vout should read low.
55
  2.) Wait for high on Vout.
56
  3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).
57
  4.) Set Vin high for 2ms or more to turn off rangefinder
58

59
*/
60
#include <avr/pgmspace.h>
61

    
62
#include "dragonfly_defs.h"
63
#include "analog.h"
64
#include "dio.h"
65
#include "rangefinder.h"
66

    
67
unsigned char range_initd=0;
68

    
69

    
70
/*
71
  read_distance returns the 8-bit reading from the rangefinder
72
  parameters:
73
  range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
74

75
  NOTE:
76
  The Sharp GD2D02 returns values on a decreasing logrithmic scale.
77
  So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.
78
  Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.
79
  At this point, we are only reading from one rangefinder [RANGE_IN0].
80
*/
81

    
82
// constants
83
/* Nasty IR approximation table
84
   I'm using this for the heck of it.  We can do whatever.
85

86
   Note the minimum value is .4V (20), and the maximum is 2.6V (133).
87
   Gives distance in mm.
88

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

91
   This is only valid for the GP2D12, with objects directly ahead and more than
92
   10cm from the detector.  See the datasheet for more information.
93
*/
94

    
95
static int IR_dist_conversion[114] PROGMEM = {
96
  800,791,751,714,681,651,623,597,574,552,531,512,494,478,462,447
97
  ,434,421,408,397,386,375,365,356,347,338,330,322,315,307,301,294
98
  ,288,282,276,270,265,260,255,250,245,241,237,232,228,224,221,217
99
  ,213,210,207,203,200,197,194,191,189,186,183,181,178,176,173,171
100
  ,169,166,164,162,160,158,156,154,152,151,149,147,145,144,142,140
101
  ,139,137,136,134,133,131,130,129,127,126,125,124,122,121,120,119
102
  ,118,117,115,114,113,112,111,110,109,108,107,106,105,105,104,103
103
  ,102,101
104
};
105

    
106
/**
107
 * @defgroup rangefinder Rangefinder
108
 * @brief Functions for using the IR rangefinders
109
 * 
110
 * Functions for using the IR rangefinders.
111
 *
112
 * @{
113
 **/
114

    
115
/**
116
 * Initializes the rangefinders. This must be called before
117
 * range_read_distance.
118
 *
119
 * @return 0 if init succesfull, an error code otherwise
120
 *
121
 * @see range_read_distance
122
 **/
123
int range_init(void)
124
{
125
  if(range_initd)
126
    return ERROR_INIT_ALREADY_INITD;
127

    
128
  digital_output(_PIN_B4,0);
129

    
130
  range_initd=1;
131
  return 0;
132
}
133

    
134
/**
135
 * Reads the distance measured by one of the rangefinders.
136
 * This distance is in arbitrary units.
137
 *
138
 * @param range_id the rangefinder to use. This should be one
139
 * of the constants IR1 - IR5.
140
 *
141
 * @return the distance measured by the rangefinder, or an error code
142
 *
143
 * @see range_init
144
 **/
145
int range_read_distance(int range_id) {
146
  if(!range_initd)
147
    return -3;
148

    
149
  return linearize_distance(analog8(range_id));
150
}
151

    
152
/**
153
 * Transforms distance readings from logarithmic to linear scale.
154
 * This probably isn't the function you are looking for.
155
 *
156
 * Note: pgm_read_word() needs additional testing
157
 *
158
 * @param value the 8-bit analog value from rangefinder
159
 *
160
 * @return linearized distance reading from rangefinder (integer in [101,800])
161
 **/
162
int linearize_distance(int value) {
163
  if(value < MIN_IR_ADC8) {
164
    return -1;
165
  } else if(value > MAX_IR_ADC8) {
166
    return -1;
167
  } else {
168
    return pgm_read_word(&(IR_dist_conversion[value - MIN_IR_ADC8]));
169
  }
170
}
171

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