[232] | 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 | }
|
---|