Project

General

Profile

Revision 1345

Added by Rich Hong over 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

rangefinder.c
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

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

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

  
8
-----------------
9
rangefinder.c
10
Using Sharp GP2D02 IR Rangefinder
43
  -----------------
44
  rangefinder.c
45
  Using Sharp GP2D02 IR Rangefinder
11 46

  
12
Vin is the input to the rangefinder, designated RANGE_CTRL.
13
Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from
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
14 49

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

  
18
Usage:
19
1.) Set Vin low. Vout should read low.
20
2.) Wait for high on Vout.
21
3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).
22
4.) Set Vin high for 2ms or more to turn off rangefinder
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
23 58

  
24 59
*/
25 60

  
......
27 62
#include "rangefinder.h"
28 63

  
29 64
/*
30
read_distance returns the 8-bit reading from the rangefinder
31
parameters:
32
range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
65
  read_distance returns the 8-bit reading from the rangefinder
66
  parameters:
67
  range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]
33 68

  
34
NOTE:
35
The Sharp GD2D02 returns values on a decreasing logrithmic scale.
36
So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.
37
Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.
38
At this point, we are only reading from one rangefinder [RANGE_IN0].
69
  NOTE:
70
  The Sharp GD2D02 returns values on a decreasing logrithmic scale.
71
  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.
72
  At this point, we are only reading from one rangefinder [RANGE_IN0].
39 73
*/
40 74

  
41 75
// constants
42 76
/* Nasty IR approximation table
43
  I'm using this for the heck of it.  We can do whatever.
77
   I'm using this for the heck of it.  We can do whatever.
44 78

  
45
  Note the minimum value is .4V (20), and the maximum is 2.6V (133).
46
  Gives distance in mm.
79
   Note the minimum value is .4V (20), and the maximum is 2.6V (133).
80
   Gives distance in mm.
47 81

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

  
50
  This is only valid for the GP2D12, with objects directly ahead and more than
51
  10cm from the detector.  See the datasheet for more information.
84
   This is only valid for the GP2D12, with objects directly ahead and more than
85
   10cm from the detector.  See the datasheet for more information.
52 86
*/
53 87

  
54 88
#define MIN_IR_ADC8 20
55 89
#define MAX_IR_ADC8 133
56 90

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

  
68 102
int linearize_distance(int value);
......
82 116
 *
83 117
 * @see range_read_distance
84 118
 **/
85
void range_init(void){
119
void range_init(void)
120
{
86 121
  digital_output(_PIN_B4,0);
87 122
}
88 123

  
......
97 132
 *
98 133
 * @see range_init
99 134
 **/
100
int range_read_distance (int range_id) {  
135
int range_read_distance (int range_id) {
101 136
  return linearize_distance(analog8(range_id));
102 137
}
103 138

  
104 139
/** @} **/ //end defgroup
105 140

  
106
int linearize_distance(int value)
107
{
108
  if(value < MIN_IR_ADC8)
109
  {
141
int linearize_distance(int value) {
142
  if(value < MIN_IR_ADC8) {
110 143
    return -1;
111
  }
112
  else if(value > MAX_IR_ADC8)
113
  {
144
  } else if(value > MAX_IR_ADC8) {
114 145
    return -1;
115
  }
116
  else
117
  {
146
  } else {
118 147
    return IR_dist_conversion[value - MIN_IR_ADC8];
119 148
  }
120 149
}
121

  
122

  
123

  

Also available in: Unified diff