1 | #include <ZumoBuzzer.h>
|
---|
2 | #include <ZumoMotors.h>
|
---|
3 | #include <Pushbutton.h>
|
---|
4 | #include <QTRSensors.h>
|
---|
5 | #include <ZumoReflectanceSensorArray.h>
|
---|
6 | #include <avr/pgmspace.h>
|
---|
7 | #include <LSM303.h>
|
---|
8 | #include <L3G.h>
|
---|
9 | #include <RunningAverage.h>
|
---|
10 | #include <Accelerometer.h>
|
---|
11 |
|
---|
12 | template <typename T> float heading(LSM303::vector<T> v, LSM303 *compass)
|
---|
13 | {
|
---|
14 | float x_scaled = 2.0*(float)(v.x - compass->m_min.x) / (compass->m_max.x - compass->m_min.x) - 1.0;
|
---|
15 | float y_scaled = 2.0*(float)(v.y - compass->m_min.y) / (compass->m_max.y - compass->m_min.y) - 1.0;
|
---|
16 |
|
---|
17 | float angle = atan2(y_scaled, x_scaled)*180 / M_PI;
|
---|
18 | if (angle < 0)
|
---|
19 | angle += 360;
|
---|
20 | return angle;
|
---|
21 | }
|
---|
22 |
|
---|
23 | #define CRB_REG_M_2_5GAUSS 0x60 // CRB_REG_M value for magnetometer +/-2.5 gauss full scale
|
---|
24 | #define CRA_REG_M_220HZ 0x1C // CRA_REG_M value for magnetometer 220 Hz update rate
|
---|
25 |
|
---|
26 | class ZumoCompass : public LSM303
|
---|
27 | {
|
---|
28 | public:
|
---|
29 | ZumoCompass() {};
|
---|
30 | void begin() {
|
---|
31 | Wire.begin();
|
---|
32 | // Initiate LSM303
|
---|
33 | init();
|
---|
34 | // Enables accelerometer and magnetometer
|
---|
35 | enableDefault();
|
---|
36 | writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
|
---|
37 | writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ); // 220 Hz compass update rate
|
---|
38 | // Set calibrated values to compass.m_max and compass.m_min
|
---|
39 | m_max.x = -4418;
|
---|
40 | m_max.y = 1180;
|
---|
41 | m_min.x = -6016;
|
---|
42 | m_min.y = -772;
|
---|
43 | };
|
---|
44 |
|
---|
45 | float averageHeading() {
|
---|
46 | LSM303::vector<int32_t> avg = {0, 0, 0};
|
---|
47 |
|
---|
48 | for(int i = 0; i < 10; i ++)
|
---|
49 | {
|
---|
50 | read();
|
---|
51 | avg.x += m.x;
|
---|
52 | avg.y += m.y;
|
---|
53 | }
|
---|
54 | avg.x /= 10.0;
|
---|
55 | avg.y /= 10.0;
|
---|
56 |
|
---|
57 | // avg is the average measure of the magnetic vector.
|
---|
58 | return ::heading(avg, (LSM303 *)this);
|
---|
59 | }
|
---|
60 | };
|
---|
61 |
|
---|
62 | class ZumoGyro : public L3G
|
---|
63 | {
|
---|
64 | public:
|
---|
65 | ZumoGyro(void){};
|
---|
66 |
|
---|
67 | /* turnAngle is a 32-bit unsigned integer representing the amount
|
---|
68 | the robot has turned since the last time turnSensorReset was
|
---|
69 | called. This is computed solely using the Z axis of the gyro, so
|
---|
70 | it could be inaccurate if the robot is rotated about the X or Y
|
---|
71 | axes.
|
---|
72 |
|
---|
73 | Our convention is that a value of 0x20000000 represents a 45
|
---|
74 | degree counter-clockwise rotation. This means that a uint32_t
|
---|
75 | can represent any angle between 0 degrees and 360 degrees. If
|
---|
76 | you cast it to a signed 32-bit integer by writing
|
---|
77 | (int32_t)turnAngle, that integer can represent any angle between
|
---|
78 | -180 degrees and 180 degrees. */
|
---|
79 | uint32_t turnAngle = 0;
|
---|
80 |
|
---|
81 | int32_t turnAngleDegree = 0;
|
---|
82 |
|
---|
83 | // turnRate is the current angular rate of the gyro, in units of
|
---|
84 | // 0.07 degrees per second.
|
---|
85 | int16_t turnRate;
|
---|
86 |
|
---|
87 | // This is the average reading obtained from the gyro's Z axis
|
---|
88 | // during calibration.
|
---|
89 | int16_t gyroOffset;
|
---|
90 |
|
---|
91 | // This variable helps us keep track of how much time has passed
|
---|
92 | // between readings of the gyro.
|
---|
93 | uint16_t gyroLastUpdate = 0;
|
---|
94 |
|
---|
95 | // This constant represents a turn of 45 degrees.
|
---|
96 | const int32_t turnAngle45 = 0x20000000;
|
---|
97 |
|
---|
98 | // This constant represents a turn of 90 degrees.
|
---|
99 | const int32_t turnAngle90 = turnAngle45 * 2;
|
---|
100 |
|
---|
101 | // This constant represents a turn of approximately 1 degree.
|
---|
102 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45;
|
---|
103 |
|
---|
104 | // This should be called to set the starting point for measuring
|
---|
105 | // a turn. After calling this, turnAngle will be 0.
|
---|
106 | void turnSensorReset() {
|
---|
107 | gyroLastUpdate = micros();
|
---|
108 | turnAngle = 0;
|
---|
109 | turnAngleDegree = 0;
|
---|
110 | }
|
---|
111 |
|
---|
112 | // Read the gyro and update the angle. This should be called as
|
---|
113 | // frequently as possible while using the gyro to do turns.
|
---|
114 | void turnSensorUpdate() {
|
---|
115 | // Read the measurements from the gyro.
|
---|
116 | read();
|
---|
117 | turnRate = g.z - gyroOffset;
|
---|
118 |
|
---|
119 | // Figure out how much time has passed since the last update (dt)
|
---|
120 | uint16_t m = micros();
|
---|
121 | uint16_t dt = m - gyroLastUpdate;
|
---|
122 | gyroLastUpdate = m;
|
---|
123 |
|
---|
124 | // Multiply dt by turnRate in order to get an estimation of how
|
---|
125 | // much the robot has turned since the last update.
|
---|
126 | // (angular change = angular velocity * time)
|
---|
127 | int32_t d = (int32_t)turnRate * dt;
|
---|
128 |
|
---|
129 | // The units of d are gyro digits times microseconds. We need
|
---|
130 | // to convert those to the units of turnAngle, where 2^29 units
|
---|
131 | // represents 45 degrees. The conversion from gyro digits to
|
---|
132 | // degrees per second (dps) is determined by the sensitivity of
|
---|
133 | // the gyro: 0.07 degrees per second per digit.
|
---|
134 | //
|
---|
135 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
|
---|
136 | // = 14680064/17578125 unit/(digit*us)
|
---|
137 | turnAngle += (int64_t)d * 14680064 / 17578125;
|
---|
138 | turnAngleDegree = ((int32_t)turnAngle / turnAngle1);
|
---|
139 | }
|
---|
140 |
|
---|
141 | void turnSensorSetup() {
|
---|
142 | Wire.begin();
|
---|
143 | init();
|
---|
144 |
|
---|
145 | // 800 Hz output data rate,
|
---|
146 | // low-pass filter cutoff 100 Hz
|
---|
147 | writeReg(L3G::CTRL1, 0b11111111);
|
---|
148 |
|
---|
149 | // 2000 dps full scale
|
---|
150 | writeReg(L3G::CTRL4, 0b00100000);
|
---|
151 |
|
---|
152 | // High-pass filter disabled
|
---|
153 | writeReg(L3G::CTRL5, 0b00000000);
|
---|
154 |
|
---|
155 | // Delay to give the user time to remove their finger.
|
---|
156 | delay(500);
|
---|
157 |
|
---|
158 | // Calibrate the gyro.
|
---|
159 | int32_t total = 0;
|
---|
160 | for (uint16_t i = 0; i < 1024; i++)
|
---|
161 | {
|
---|
162 | // Wait for new data to be available, then read it.
|
---|
163 | while(!readReg(L3G::STATUS_REG) & 0x08);
|
---|
164 | read();
|
---|
165 | // Add the Z axis reading to the total.
|
---|
166 | total += g.z;
|
---|
167 | }
|
---|
168 | gyroOffset = total / 1024;
|
---|
169 |
|
---|
170 | // Display the angle (in degrees from -180 to 180) until the
|
---|
171 | // user presses A.
|
---|
172 | turnSensorReset();
|
---|
173 | };
|
---|
174 | };
|
---|
175 |
|
---|
176 | class ZumoBuzzer2 : public ZumoBuzzer
|
---|
177 | {
|
---|
178 | public:
|
---|
179 | ZumoBuzzer2(){};
|
---|
180 | void playOn(void) {play(">g32>>c32");noTone(6);};
|
---|
181 | void playStart(void) {playNote(NOTE_G(4), 500, 15);}
|
---|
182 | void playNum(int cnt) {
|
---|
183 | for (int i = 0; i < cnt; i++){
|
---|
184 | delay(1000);
|
---|
185 | playNote(NOTE_G(3), 50, 12);
|
---|
186 | }
|
---|
187 | };
|
---|
188 | };
|
---|
189 |
|
---|
190 | class ZumoLED
|
---|
191 | {
|
---|
192 | public:
|
---|
193 | ZumoLED(){};
|
---|
194 | void init(){pinMode(13, OUTPUT);digitalWrite(13, LOW);};
|
---|
195 | void on(){digitalWrite(13, HIGH);};
|
---|
196 | void off(){digitalWrite(13, LOW);};
|
---|
197 | void set(int i){
|
---|
198 | digitalWrite(13, (i == 1)? HIGH : LOW);
|
---|
199 | };
|
---|
200 | };
|
---|
201 |
|
---|
202 | class ZumoReflectanceSensorArray2 : public ZumoReflectanceSensorArray {
|
---|
203 | public:
|
---|
204 | unsigned int values[6];
|
---|
205 | #ifndef ARDUINO_ARCH_SAMD
|
---|
206 | ZumoReflectanceSensorArray2() : ZumoReflectanceSensorArray(QTR_NO_EMITTER_PIN){
|
---|
207 |
|
---|
208 | };
|
---|
209 | #else /* ARDUINO_ARCH_SAMD */
|
---|
210 | ZumoReflectanceSensorArray2() : ZumoReflectanceSensorArray(2){
|
---|
211 |
|
---|
212 | };
|
---|
213 | #endif /* !ARDUINO_ARCH_SAMD */
|
---|
214 | void update(void){
|
---|
215 | read(values);
|
---|
216 | }
|
---|
217 | unsigned int value(int i){
|
---|
218 | if((i <= 6) && (i > 0)) {
|
---|
219 | return values[i-1];
|
---|
220 | }
|
---|
221 | return 0;
|
---|
222 | }
|
---|
223 | };
|
---|
224 |
|
---|
225 | ZumoReflectanceSensorArray2 reflectances;
|
---|
226 | Pushbutton button(ZUMO_BUTTON);
|
---|
227 | ZumoLED led;
|
---|
228 | ZumoBuzzer2 buzzer;
|
---|
229 | ZumoMotors motors;
|
---|
230 | ZumoCompass compass;
|
---|
231 | ZumoGyro gyro;
|
---|
232 |
|
---|
233 | void
|
---|
234 | ZumoInit(void) {
|
---|
235 | led.init();
|
---|
236 | button.init();
|
---|
237 | }
|
---|