forked from srnilssen/Arduino-Robot-2018
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdistSens_g2d12.c
136 lines (116 loc) · 6.2 KB
/
distSens_g2d12.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
/************************************************************************/
// File: distSens_g2d12.c
// Contributors: Johannes Schrimpf, NTNU Fall 2007
// Jannicke Selnes Tusvik, NTNU Fall 2009
// Erlend Ese, NTNU Spring 2016
//
// Purpose: Driver for infrared distance sensors: Sharp G2D12
//
//
// Port and pins defined in defines.h file
/************************************************************************/
/* AVR includes */
#include <avr/io.h>
/* Custom includes */
#include "distSens_g2d12.h"
#include "defines.h"
/************************************************************************/
// Array to map analog value to distance. Recalibration may be needed.
// Sensors may be due for replacement by a LIDAR
/************************************************************************/
uint8_t ui8_analogToCM[4][256]={
{ // Left sensor connected to PINF4 = 0
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,80,78,77,75,
73,72,70,68,66,65,63,61,60,59,57,56,55,54,53,52,52,51,50,49,48,
47,47,46,45,44,43,43,42,41,41,40,40,39,39,38,38,37,37,36,36,35,
35,35,34,34,34,33,33,33,32,32,32,32,31,31,31,31,30,30,30,29,29,
29,29,28,28,28,28,28,27,27,27,27,26,26,26,26,25,25,25,25,25,24,
24,24,24,24,23,23,23,23,23,23,22,22,22,22,22,22,21,21,21,21,21,
21,21,20,20,20,20,20,20,20,19,19,19,19,19,19,19,19,19,18,18,18,
18,18,18,18,18,18,18,17,17,17,17,17,17,17,17,17,17,17,16,16,16,
16,16,16,16,16,16,16,16,15,15,15,15,15,15,15,15,15,15,15,14,14,
14,14,14,14,14,14,14,14,14,14,13,13,13,13,13,13,13,13,13,13,13,
13,13,13,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
12,12,12,12,12,12,11,11,11,11,11,11,11,10,10,1},
{ // Forward sensor connected to PINF1 = 1
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,80,78,77,75,
73,72,70,68,66,65,63,61,60,59,57,56,55,54,53,52,52,51,50,49,48,
47,47,46,45,44,43,43,42,41,41,40,40,39,39,38,38,37,37,36,36,35,
35,35,34,34,34,33,33,33,32,32,32,32,31,31,31,31,30,30,30,29,29,
29,29,28,28,28,28,28,27,27,27,27,26,26,26,26,25,25,25,25,25,24,
24,24,24,24,23,23,23,23,23,23,22,22,22,22,22,22,21,21,21,21,21,
21,21,20,20,20,20,20,20,20,19,19,19,19,19,19,19,19,19,18,18,18,
18,18,18,18,18,18,18,17,17,17,17,17,17,17,17,17,17,17,16,16,16,
16,16,16,16,16,16,16,16,15,15,15,15,15,15,15,15,15,15,15,14,14,
14,14,14,14,14,14,14,14,14,14,13,13,13,13,13,13,13,13,13,13,13,
13,13,13,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
12,12,12,12,12,12,11,11,11,11,11,11,11,10,10,1},
{ // Right sensor connected to PINF2 = 2
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,80,78,77,75,
73,72,70,68,66,65,63,61,60,59,57,56,55,54,53,52,52,51,50,49,48,
47,47,46,45,44,43,43,42,41,41,40,40,39,39,38,38,37,37,36,36,35,
35,35,34,34,34,33,33,33,32,32,32,32,31,31,31,31,30,30,30,29,29,
29,29,28,28,28,28,28,27,27,27,27,26,26,26,26,25,25,25,25,25,24,
24,24,24,24,23,23,23,23,23,23,22,22,22,22,22,22,21,21,21,21,21,
21,21,20,20,20,20,20,20,20,19,19,19,19,19,19,19,19,19,18,18,18,
18,18,18,18,18,18,18,17,17,17,17,17,17,17,17,17,17,17,16,16,16,
16,16,16,16,16,16,16,16,15,15,15,15,15,15,15,15,15,15,15,14,14,
14,14,14,14,14,14,14,14,14,14,13,13,13,13,13,13,13,13,13,13,13,
13,13,13,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
12,12,12,12,12,12,11,11,11,11,11,11,11,10,10,1},
{// Rear sensor connected to PINF3 = 3
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,80,78,77,75,
73,72,70,68,66,65,63,61,60,59,57,56,55,54,53,52,52,51,50,49,48,
47,47,46,45,44,43,43,42,41,41,40,40,39,39,38,38,37,37,36,36,35,
35,35,34,34,34,33,33,33,32,32,32,32,31,31,31,31,30,30,30,29,29,
29,29,28,28,28,28,28,27,27,27,27,26,26,26,26,25,25,25,25,25,24,
24,24,24,24,23,23,23,23,23,23,22,22,22,22,22,22,21,21,21,21,21,
21,21,20,20,20,20,20,20,20,19,19,19,19,19,19,19,19,19,18,18,18,
18,18,18,18,18,18,18,17,17,17,17,17,17,17,17,17,17,17,16,16,16,
16,16,16,16,16,16,16,16,15,15,15,15,15,15,15,15,15,15,15,14,14,
14,14,14,14,14,14,14,14,14,14,13,13,13,13,13,13,13,13,13,13,13,
13,13,13,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
12,12,12,12,12,12,11,11,11,11,11,11,11,10,10,1},
};
/* Initialize distance sensors and ADC */
void vDistSens_init(){
/* Initialize sensor pins as input */
distSensReg &= ~((1<<distSensFwd) & (1<<distSensLeft) & (1<<distSensRear) & (1<<distSensRight));
/* Internal 2.56V VREG with external capacitor at AREF pin */
/* Datasheet p281 table 26-3 */
ADMUX |= (1<<REFS1) | (1<<REFS0);
/* ADC enable */
ADCSRA |= (1<<ADEN);
/* ADC prescaler setting (div. factor = 16) */
/* Datasheet p257 table 21-5 */
ADCSRA |= (1<<ADPS2) | (0<<ADPS1) | (0<<ADPS0);
}
/* Reads a value from the IR sensor ui8_num and returns a value in cm */
uint8_t ui8DistSens_readCM(uint8_t sensorDirection){
uint8_t ui8_analogValue;
/* Choose channel */
ADMUX = sensorDirection;
/* Enable internal 2,54V AREF */
ADMUX |= (1<<REFS1) | (1<<REFS0);
/* Start conversion */
ADCSRA |= (1<<ADSC);
loop_until_bit_is_clear(ADCSRA, ADSC); // Macro from <avr/io.h>, wait until bit bit in IO register is set.
/* Return the 8 most significant bits from the 10 bit register */
ui8_analogValue = (ADCL >> 2) | (ADCH << 6);
// Returns corresponding distance in CM
return ui8_analogToCM[sensorDirection-1][ui8_analogValue];
}
/* Reads a value from the IR sensor ui8_num and returns analog value */
uint8_t ui8DistSens_readAnalog(uint8_t distSensDir){
uint16_t ui16_analogValue=0;
/* Choose channel */
ADMUX = distSensDir;
/* Enable internal 2,54V AREF */
ADMUX |= (1<<REFS1) | (1<<REFS0);
/* Start conversion */
ADCSRA |= (1<<ADSC);
loop_until_bit_is_clear(ADCSRA, ADSC); // Macro from avr/io.h: Wait until bit bit in IO register sfr is clear.
/* Return the 8 most significant bits from the 10 bit register */
ui16_analogValue += (ADCL >> 2) | (ADCH << 6);
return ((uint8_t)(ui16_analogValue));
}