-
Notifications
You must be signed in to change notification settings - Fork 0
/
2_Compass_and_IR_sensor.ino
160 lines (146 loc) · 4.35 KB
/
2_Compass_and_IR_sensor.ino
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
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
void i2c_initiation() {
if (!i2c_init()) // Initialize everything and check for bus lockup
{
Serial.println("I2C init failed");
blink_LED(3, 500);
}
return;
}
void i2c_set_mode() {
if (!i2c_start((compass_i2c_addr << 1) | I2C_WRITE)) { // start transfer
Serial.println("I2C device busy");
return;
}
i2c_write(OPR_register_addr);
i2c_write(compass_operation_mode);
i2c_stop();
return;
}
double i2c_read_compass() {
static bool firstread = true;
static double head_ref = 0.0;
static double heading_rawdata = 0.0;
static double N = 0.0;
static double heading = 0.0;
if (!i2c_start((compass_i2c_addr << 1) | I2C_WRITE)) { // start transfer
Serial.println("I2C device busy");
return;
}
i2c_write(compass_dataout_register);
i2c_rep_start((compass_i2c_addr << 1) | I2C_READ);
heading_rawdata = i2c_read(false);
N = i2c_read(true);
i2c_stop();
if (firstread) {
head_ref = heading_rawdata + N * 255.0;
firstread = false;
}
heading = (heading_rawdata + N * 255.0 - head_ref) * 0.0625;
if (heading >= 360.0) {
int n = heading / 360.001;
heading -= (double)n * 360.0;
}
else if (heading < 0.0) {
int n = -heading / 360.001;
heading += 360.0 * (double)(n + 1);
}
// Serial.print(heading);
// Serial.print(" ");
return heading;
}
void IR_init() {
Serial.println("Starting TSL2591 Navigation module!");
if (tsl.begin()) {
Serial.println("Found TSL2591 sensor");
}
else {
Serial.println("Not enough sensor found ... check your wiring?");
while (1);
}
configureSensor();
return;
}
void configureSensor(void)
{
// You can change the gain on the fly, to adapt to brighter/dimmer light situations
tsl.setGain(TSL2591_GAIN_LOW); // 1x gain (bright light)
// Changing the integration time gives you a longer time over which to sense light
// longer timelines are slower, but are good in very low light situtations!
tsl.setTiming(TSL2591_INTEGRATIONTIME_100MS); // shortest integration time (bright light)
/* Display the gain and integration time for reference sake */
Serial.println("------------------------------------");
Serial.print ("Gain: ");
tsl2591Gain_t gain = tsl.getGain();
switch (gain)
{
case TSL2591_GAIN_LOW:
Serial.println("One Tsl 1x (Low)");
break;
case TSL2591_GAIN_MED:
Serial.println("One Tsl 25x (Medium)");
break;
case TSL2591_GAIN_HIGH:
Serial.println("One tsl 428x (High)");
break;
case TSL2591_GAIN_MAX:
Serial.println("One tsl 9876x (Max)");
break;
}
Serial.print ("Timing: ");
Serial.print((tsl.getTiming() + 1) * 100, DEC);
Serial.println(" ms");
Serial.println("------------------------------------");
Serial.println("");
return;
}
struct IRPACKAGE IR_read(void)
{
// More advanced data read example. Read 32 bits with top 16 bits IR, bottom 16 bits full spectrum
// That way you can do whatever math and comparisons you want!
IRPACKAGE irpackage;
uint32_t lum = tsl.getFullLuminosity();
uint16_t full, visible;
irpackage.ir = lum >> 16;
full = lum & 0xFFFF;
visible = full - irpackage.ir;
irpackage.ratio = (double)irpackage.ir / visible;
// Serial.print("[ "); Serial.print(millis()); Serial.print(" ms ] ");
// Serial.print("IR: "); Serial.print(irpackage.ir); Serial.print(" ");
// Serial.print("Full: "); Serial.print(full); Serial.print(" ");
// Serial.print("Visible: "); Serial.print(full - irpackage.ir); Serial.print(" ");
// Serial.print("Lux: "); Serial.print(tsl.calculateLux(full, irpackage.ir)); Serial.print(" ");
// Serial.print("IRFULLratio: "); Serial.print(irpackage.ratio); Serial.println(" ");
return irpackage;
}
double get_direction(){
return i2c_read_compass();
}
float get_direction_filter3(){
float total = 0;
total += get_direction();
total += get_direction();
total += get_direction();
return total/3;
}
float get_direction_filter10(){
return get_direction();
// static float temp[9];
// float sum = temp[0];
// for (int i = 1; i < 9; i++){
// sum += temp[i];
// temp[i-1] = temp[i];
// }
// float current = get_direction();
//
// if (abs(current)-abs(sum)/9 > 60) current = get_direction();
//
// sum += current;
// temp[8] = current;
// return sum/10;
}
void clear_compass_buffer(){
float useless;
for (int i = 0; i<10; i++){
useless = get_direction_filter10();
}
}