Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/IMU.cpp@136:906ac19fb850, 2017-05-22 (annotated)

Committer:
PESGruppe1
Date:
Mon May 22 13:18:13 2017 +0000
Revision:
136:906ac19fb850
Parent:
30:34fba0036bba 
New finepostitioning mit vor und zur?ckdrehen 1 grad;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 30:34fba0036bba 1 /*
cittecla 30:34fba0036bba 2 * IMU.cpp
cittecla 30:34fba0036bba 3 * Copyright (c) 2017, ZHAW
cittecla 30:34fba0036bba 4 * All rights reserved.
cittecla 30:34fba0036bba 5 */
cittecla 30:34fba0036bba 6
cittecla 30:34fba0036bba 7 #include "IMU.h"
cittecla 30:34fba0036bba 8
cittecla 30:34fba0036bba 9 using namespace std;
cittecla 30:34fba0036bba 10
cittecla 30:34fba0036bba 11 const float IMU::PERIOD = 0.001f; // period of filter task, given in [s]
cittecla 30:34fba0036bba 12 const float IMU::PI = 3.14159265f; // the constant PI
cittecla 30:34fba0036bba 13 const float IMU::LOWPASS_FILTER_FREQUENCY = 6.3f; // frequency of lowpass filter, given in [rad/s]
cittecla 30:34fba0036bba 14
cittecla 30:34fba0036bba 15 /**
cittecla 30:34fba0036bba 16 * Creates an IMU object.
cittecla 30:34fba0036bba 17 * @param spi a reference to an spi controller to use.
cittecla 30:34fba0036bba 18 * @param csG the chip select output for the gyro sensor.
cittecla 30:34fba0036bba 19 * @param csXM the chip select output for the accelerometer and the magnetometer.
cittecla 30:34fba0036bba 20 */
cittecla 30:34fba0036bba 21 IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
cittecla 30:34fba0036bba 22
cittecla 30:34fba0036bba 23 // initialize SPI interface
cittecla 30:34fba0036bba 24
cittecla 30:34fba0036bba 25 spi.format(8, 3);
cittecla 30:34fba0036bba 26 spi.frequency(1000000);
cittecla 30:34fba0036bba 27
cittecla 30:34fba0036bba 28 // reset chip select lines to logical high
cittecla 30:34fba0036bba 29
cittecla 30:34fba0036bba 30 csG = 1;
cittecla 30:34fba0036bba 31 csXM = 1;
cittecla 30:34fba0036bba 32
cittecla 30:34fba0036bba 33 wait_us(1);
cittecla 30:34fba0036bba 34
cittecla 30:34fba0036bba 35 // initialize gyro
cittecla 30:34fba0036bba 36
cittecla 30:34fba0036bba 37 writeRegister(csG, CTRL_REG1_G, 0x0F); // enable gyro in all 3 axis
cittecla 30:34fba0036bba 38
cittecla 30:34fba0036bba 39 // initialize accelerometer
cittecla 30:34fba0036bba 40
cittecla 30:34fba0036bba 41 writeRegister(csXM, CTRL_REG0_XM, 0x00);
cittecla 30:34fba0036bba 42 writeRegister(csXM, CTRL_REG1_XM, 0x5F);
cittecla 30:34fba0036bba 43 writeRegister(csXM, CTRL_REG2_XM, 0x00);
cittecla 30:34fba0036bba 44 writeRegister(csXM, CTRL_REG3_XM, 0x04);
cittecla 30:34fba0036bba 45
cittecla 30:34fba0036bba 46 // initialize magnetometer
cittecla 30:34fba0036bba 47
cittecla 30:34fba0036bba 48 writeRegister(csXM, CTRL_REG5_XM, 0x94);
cittecla 30:34fba0036bba 49 writeRegister(csXM, CTRL_REG6_XM, 0x00);
cittecla 30:34fba0036bba 50 writeRegister(csXM, CTRL_REG7_XM, 0x00);
cittecla 30:34fba0036bba 51 writeRegister(csXM, CTRL_REG4_XM, 0x04);
cittecla 30:34fba0036bba 52 writeRegister(csXM, INT_CTRL_REG_M, 0x09);
cittecla 30:34fba0036bba 53
cittecla 30:34fba0036bba 54 // initialize local variables
cittecla 30:34fba0036bba 55
cittecla 30:34fba0036bba 56 magnetometerXMin = 1000.0f;
cittecla 30:34fba0036bba 57 magnetometerXMax = -1000.0f;
cittecla 30:34fba0036bba 58 magnetometerYMin = 1000.0f;
cittecla 30:34fba0036bba 59 magnetometerYMax = -1000.0f;
cittecla 30:34fba0036bba 60
cittecla 30:34fba0036bba 61 lowpassFilter.setPeriod(PERIOD);
cittecla 30:34fba0036bba 62 lowpassFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
cittecla 30:34fba0036bba 63
cittecla 30:34fba0036bba 64 heading = 0.0f;
cittecla 30:34fba0036bba 65
cittecla 30:34fba0036bba 66 // start periodic task
cittecla 30:34fba0036bba 67
cittecla 30:34fba0036bba 68 ticker.attach(callback(this, &IMU::run), PERIOD);
cittecla 30:34fba0036bba 69 }
cittecla 30:34fba0036bba 70
cittecla 30:34fba0036bba 71 /**
cittecla 30:34fba0036bba 72 * Deletes the IMU object.
cittecla 30:34fba0036bba 73 */
cittecla 30:34fba0036bba 74 IMU::~IMU() {
cittecla 30:34fba0036bba 75
cittecla 30:34fba0036bba 76 ticker.detach();
cittecla 30:34fba0036bba 77 }
cittecla 30:34fba0036bba 78
cittecla 30:34fba0036bba 79 /**
cittecla 30:34fba0036bba 80 * This private method allows to write a register value.
cittecla 30:34fba0036bba 81 * @param cs the chip select output to use, either csG or csXM.
cittecla 30:34fba0036bba 82 * @param address the 7 bit address of the register.
cittecla 30:34fba0036bba 83 * @param value the value to write into the register.
cittecla 30:34fba0036bba 84 */
cittecla 30:34fba0036bba 85 void IMU::writeRegister(DigitalOut& cs, char address, char value) {
cittecla 30:34fba0036bba 86
cittecla 30:34fba0036bba 87 cs = 0;
cittecla 30:34fba0036bba 88 wait_us(1);
cittecla 30:34fba0036bba 89
cittecla 30:34fba0036bba 90 spi.write(0x7F & address);
cittecla 30:34fba0036bba 91 spi.write(value & 0xFF);
cittecla 30:34fba0036bba 92
cittecla 30:34fba0036bba 93 wait_us(1);
cittecla 30:34fba0036bba 94 cs = 1;
cittecla 30:34fba0036bba 95 }
cittecla 30:34fba0036bba 96
cittecla 30:34fba0036bba 97 /**
cittecla 30:34fba0036bba 98 * This private method allows to read a register value.
cittecla 30:34fba0036bba 99 * @param cs the chip select output to use, either csG or csXM.
cittecla 30:34fba0036bba 100 * @param address the 7 bit address of the register.
cittecla 30:34fba0036bba 101 * @return the value read from the register.
cittecla 30:34fba0036bba 102 */
cittecla 30:34fba0036bba 103 char IMU::readRegister(DigitalOut& cs, char address) {
cittecla 30:34fba0036bba 104
cittecla 30:34fba0036bba 105 cs = 0;
cittecla 30:34fba0036bba 106 wait_us(1);
cittecla 30:34fba0036bba 107
cittecla 30:34fba0036bba 108 spi.write(0x80 | address);
cittecla 30:34fba0036bba 109 int value = spi.write(0xFF);
cittecla 30:34fba0036bba 110
cittecla 30:34fba0036bba 111 wait_us(1);
cittecla 30:34fba0036bba 112 cs = 1;
cittecla 30:34fba0036bba 113
cittecla 30:34fba0036bba 114 return (char)(value & 0xFF);
cittecla 30:34fba0036bba 115 }
cittecla 30:34fba0036bba 116
cittecla 30:34fba0036bba 117 /**
cittecla 30:34fba0036bba 118 * Reads the gyroscope about the x-axis.
cittecla 30:34fba0036bba 119 * @return the rotational speed about the x-axis given in [rad/s].
cittecla 30:34fba0036bba 120 */
cittecla 30:34fba0036bba 121 float IMU::readGyroX() {
cittecla 30:34fba0036bba 122
cittecla 30:34fba0036bba 123 char low = readRegister(csG, OUT_X_L_G);
cittecla 30:34fba0036bba 124 char high = readRegister(csG, OUT_X_H_G);
cittecla 30:34fba0036bba 125
cittecla 30:34fba0036bba 126 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 127
cittecla 30:34fba0036bba 128 return (float)value/32768.0f*245.0f*PI/180.0f;
cittecla 30:34fba0036bba 129 }
cittecla 30:34fba0036bba 130
cittecla 30:34fba0036bba 131 /**
cittecla 30:34fba0036bba 132 * Reads the gyroscope about the y-axis.
cittecla 30:34fba0036bba 133 * @return the rotational speed about the y-axis given in [rad/s].
cittecla 30:34fba0036bba 134 */
cittecla 30:34fba0036bba 135 float IMU::readGyroY() {
cittecla 30:34fba0036bba 136
cittecla 30:34fba0036bba 137 char low = readRegister(csG, OUT_Y_L_G);
cittecla 30:34fba0036bba 138 char high = readRegister(csG, OUT_Y_H_G);
cittecla 30:34fba0036bba 139
cittecla 30:34fba0036bba 140 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 141
cittecla 30:34fba0036bba 142 return (float)value/32768.0f*245.0f*PI/180.0f;
cittecla 30:34fba0036bba 143 }
cittecla 30:34fba0036bba 144
cittecla 30:34fba0036bba 145 /**
cittecla 30:34fba0036bba 146 * Reads the gyroscope about the z-axis.
cittecla 30:34fba0036bba 147 * @return the rotational speed about the z-axis given in [rad/s].
cittecla 30:34fba0036bba 148 */
cittecla 30:34fba0036bba 149 float IMU::readGyroZ() {
cittecla 30:34fba0036bba 150
cittecla 30:34fba0036bba 151 char low = readRegister(csG, OUT_Z_L_G);
cittecla 30:34fba0036bba 152 char high = readRegister(csG, OUT_Z_H_G);
cittecla 30:34fba0036bba 153
cittecla 30:34fba0036bba 154 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 155
cittecla 30:34fba0036bba 156 return (float)value/32768.0f*245.0f*PI/180.0f;
cittecla 30:34fba0036bba 157 }
cittecla 30:34fba0036bba 158
cittecla 30:34fba0036bba 159 /**
cittecla 30:34fba0036bba 160 * Reads the acceleration in x-direction.
cittecla 30:34fba0036bba 161 * @return the acceleration in x-direction, given in [m/s2].
cittecla 30:34fba0036bba 162 */
cittecla 30:34fba0036bba 163 float IMU::readAccelerationX() {
cittecla 30:34fba0036bba 164
cittecla 30:34fba0036bba 165 char low = readRegister(csXM, OUT_X_L_A);
cittecla 30:34fba0036bba 166 char high = readRegister(csXM, OUT_X_H_A);
cittecla 30:34fba0036bba 167
cittecla 30:34fba0036bba 168 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 169
cittecla 30:34fba0036bba 170 return (float)value/32768.0f*2.0f*9.81f;
cittecla 30:34fba0036bba 171 }
cittecla 30:34fba0036bba 172
cittecla 30:34fba0036bba 173 /**
cittecla 30:34fba0036bba 174 * Reads the acceleration in y-direction.
cittecla 30:34fba0036bba 175 * @return the acceleration in y-direction, given in [m/s2].
cittecla 30:34fba0036bba 176 */
cittecla 30:34fba0036bba 177 float IMU::readAccelerationY() {
cittecla 30:34fba0036bba 178
cittecla 30:34fba0036bba 179 char low = readRegister(csXM, OUT_Y_L_A);
cittecla 30:34fba0036bba 180 char high = readRegister(csXM, OUT_Y_H_A);
cittecla 30:34fba0036bba 181
cittecla 30:34fba0036bba 182 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 183
cittecla 30:34fba0036bba 184 return (float)value/32768.0f*2.0f*9.81f;
cittecla 30:34fba0036bba 185 }
cittecla 30:34fba0036bba 186
cittecla 30:34fba0036bba 187 /**
cittecla 30:34fba0036bba 188 * Reads the acceleration in z-direction.
cittecla 30:34fba0036bba 189 * @return the acceleration in z-direction, given in [m/s2].
cittecla 30:34fba0036bba 190 */
cittecla 30:34fba0036bba 191 float IMU::readAccelerationZ() {
cittecla 30:34fba0036bba 192
cittecla 30:34fba0036bba 193 char low = readRegister(csXM, OUT_Z_L_A);
cittecla 30:34fba0036bba 194 char high = readRegister(csXM, OUT_Z_H_A);
cittecla 30:34fba0036bba 195
cittecla 30:34fba0036bba 196 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 197
cittecla 30:34fba0036bba 198 return (float)value/32768.0f*2.0f*9.81f;
cittecla 30:34fba0036bba 199 }
cittecla 30:34fba0036bba 200
cittecla 30:34fba0036bba 201 /**
cittecla 30:34fba0036bba 202 * Reads the magnetic field in x-direction.
cittecla 30:34fba0036bba 203 * @return the magnetic field in x-direction, given in [Gauss].
cittecla 30:34fba0036bba 204 */
cittecla 30:34fba0036bba 205 float IMU::readMagnetometerX() {
cittecla 30:34fba0036bba 206
cittecla 30:34fba0036bba 207 char low = readRegister(csXM, OUT_X_L_M);
cittecla 30:34fba0036bba 208 char high = readRegister(csXM, OUT_X_H_M);
cittecla 30:34fba0036bba 209
cittecla 30:34fba0036bba 210 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 211
cittecla 30:34fba0036bba 212 return (float)value/32768.0f*2.0f;
cittecla 30:34fba0036bba 213 }
cittecla 30:34fba0036bba 214
cittecla 30:34fba0036bba 215 /**
cittecla 30:34fba0036bba 216 * Reads the magnetic field in x-direction.
cittecla 30:34fba0036bba 217 * @return the magnetic field in x-direction, given in [Gauss].
cittecla 30:34fba0036bba 218 */
cittecla 30:34fba0036bba 219 float IMU::readMagnetometerY() {
cittecla 30:34fba0036bba 220
cittecla 30:34fba0036bba 221 char low = readRegister(csXM, OUT_Y_L_M);
cittecla 30:34fba0036bba 222 char high = readRegister(csXM, OUT_Y_H_M);
cittecla 30:34fba0036bba 223
cittecla 30:34fba0036bba 224 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 225
cittecla 30:34fba0036bba 226 return (float)value/32768.0f*2.0f;
cittecla 30:34fba0036bba 227 }
cittecla 30:34fba0036bba 228
cittecla 30:34fba0036bba 229 /**
cittecla 30:34fba0036bba 230 * Reads the magnetic field in x-direction.
cittecla 30:34fba0036bba 231 * @return the magnetic field in x-direction, given in [Gauss].
cittecla 30:34fba0036bba 232 */
cittecla 30:34fba0036bba 233 float IMU::readMagnetometerZ() {
cittecla 30:34fba0036bba 234
cittecla 30:34fba0036bba 235 char low = readRegister(csXM, OUT_Z_L_M);
cittecla 30:34fba0036bba 236 char high = readRegister(csXM, OUT_Z_H_M);
cittecla 30:34fba0036bba 237
cittecla 30:34fba0036bba 238 short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
cittecla 30:34fba0036bba 239
cittecla 30:34fba0036bba 240 return (float)value/32768.0f*2.0f;
cittecla 30:34fba0036bba 241 }
cittecla 30:34fba0036bba 242
cittecla 30:34fba0036bba 243 /**
cittecla 30:34fba0036bba 244 * Reads the compass heading about the z-axis.
cittecla 30:34fba0036bba 245 * @return the compass heading in the range -PI to +PI, given in [rad].
cittecla 30:34fba0036bba 246 */
cittecla 30:34fba0036bba 247 float IMU::readHeading() {
cittecla 30:34fba0036bba 248
cittecla 30:34fba0036bba 249 return heading;
cittecla 30:34fba0036bba 250 }
cittecla 30:34fba0036bba 251
cittecla 30:34fba0036bba 252 /**
cittecla 30:34fba0036bba 253 * This method is called periodically by the ticker object and contains the
cittecla 30:34fba0036bba 254 * calculation and filtering of the heading information.
cittecla 30:34fba0036bba 255 */
cittecla 30:34fba0036bba 256 void IMU::run() {
cittecla 30:34fba0036bba 257
cittecla 30:34fba0036bba 258 // read actual measurements from magnetometer registers
cittecla 30:34fba0036bba 259
cittecla 30:34fba0036bba 260 float magnetometerX = readMagnetometerX();
cittecla 30:34fba0036bba 261 float magnetometerY = readMagnetometerY();
cittecla 30:34fba0036bba 262
cittecla 30:34fba0036bba 263 // adjust the minimum and maximum limits, if needed
cittecla 30:34fba0036bba 264
cittecla 30:34fba0036bba 265 if (magnetometerXMin > magnetometerX) magnetometerXMin = magnetometerX;
cittecla 30:34fba0036bba 266 if (magnetometerXMax < magnetometerX) magnetometerXMax = magnetometerX;
cittecla 30:34fba0036bba 267 if (magnetometerYMin > magnetometerY) magnetometerYMin = magnetometerY;
cittecla 30:34fba0036bba 268 if (magnetometerYMax < magnetometerY) magnetometerYMax = magnetometerY;
cittecla 30:34fba0036bba 269
cittecla 30:34fba0036bba 270 // calculate adjusted magnetometer values (gain and offset compensation)
cittecla 30:34fba0036bba 271
cittecla 30:34fba0036bba 272 if (magnetometerXMin < magnetometerXMax) magnetometerX = (magnetometerX-magnetometerXMin)/(magnetometerXMax-magnetometerXMin)-0.5f;
cittecla 30:34fba0036bba 273 if (magnetometerYMin < magnetometerYMax) magnetometerY = (magnetometerY-magnetometerYMin)/(magnetometerYMax-magnetometerYMin)-0.5f;
cittecla 30:34fba0036bba 274
cittecla 30:34fba0036bba 275 // calculate heading with atan2 from x and y magnetometer measurements
cittecla 30:34fba0036bba 276
cittecla 30:34fba0036bba 277 float heading = atan2(-magnetometerX, -magnetometerY);
cittecla 30:34fba0036bba 278
cittecla 30:34fba0036bba 279 // filter heading with discrete first-order lowpass filter
cittecla 30:34fba0036bba 280
cittecla 30:34fba0036bba 281 if ((this->heading > PI/2.0f) && (heading < -PI/2.0f)) heading += 2.0f*PI;
cittecla 30:34fba0036bba 282 else if ((this->heading < -PI/2.0f) && (heading > PI/2.0f)) heading -= 2.0f*PI;
cittecla 30:34fba0036bba 283
cittecla 30:34fba0036bba 284 heading = lowpassFilter.filter(heading);
cittecla 30:34fba0036bba 285
cittecla 30:34fba0036bba 286 while (heading > PI) heading -= 2.0f*PI;
cittecla 30:34fba0036bba 287 while (heading < -PI) heading += 2.0f*PI;
cittecla 30:34fba0036bba 288
cittecla 30:34fba0036bba 289 this->heading = heading;
cittecla 30:34fba0036bba 290 }