Weimen Li / Mbed 2 deprecated MAE433_Library

Drivers for the mini robot designed for Princeton's MAE 433 course.

Dependencies:   mbed-dsp mbed-rtos mbed

Dependents:   MAE433_Library_Tester RobotBalancerv2

FXOS8700CQ.cpp@8:da95e4ccbe4c, 2016-06-30 (annotated)

Committer:
Electrotiger
Date:
Thu Jun 30 23:52:51 2016 +0000
Revision:
8:da95e4ccbe4c
Parent:
3:72db173215c7 
Decreased H-Bridge Period to Provide for Even Finer Control.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Electrotiger 0:9afc272fa65f 1 /**
Electrotiger 0:9afc272fa65f 2 * @file FXOS8700CQ.cpp
Electrotiger 0:9afc272fa65f 3 * @date June 5th, 2016
Electrotiger 0:9afc272fa65f 4 * @author Weimen Li
Electrotiger 0:9afc272fa65f 5 * @brief Source file for FXOS8700CQ driver.
Electrotiger 0:9afc272fa65f 6 */
Electrotiger 0:9afc272fa65f 7
Electrotiger 3:72db173215c7 8 #include <FXOS8700CQ.hpp>
Electrotiger 0:9afc272fa65f 9
Electrotiger 0:9afc272fa65f 10 // These defines are extracted from Page. 25 of the datasheet, Rev. 7.0.
Electrotiger 0:9afc272fa65f 11 // FXOS8700CQ I2C address
Electrotiger 0:9afc272fa65f 12 const uint8_t FXOS8700CQ_SLAVE_ADDR = 0x1C; // with pins SA1=1, SA0=0, preshifted
Electrotiger 0:9afc272fa65f 13
Electrotiger 0:9afc272fa65f 14 // FXOS8700CQ internal register addresses
Electrotiger 0:9afc272fa65f 15 const char FXOS8700CQ_STATUS = 0x00;
Electrotiger 0:9afc272fa65f 16 const char FXOS8700CQ_OUT_X_MSB = 0x01;
Electrotiger 0:9afc272fa65f 17 const uint8_t FXOS8700CQ_WHOAMI = 0x0D;
Electrotiger 0:9afc272fa65f 18 const uint8_t FXOS8700CQ_XYZ_DATA_CFG = 0x0E;
Electrotiger 0:9afc272fa65f 19 const uint8_t FXOS8700CQ_CTRL_REG1 = 0x2A;
Electrotiger 0:9afc272fa65f 20 const uint8_t FXOS8700CQ_CTRL_REG2 = 0x2B;
Electrotiger 0:9afc272fa65f 21 const uint8_t FXOS8700CQ_CTRL_REG4 = 0x2D; /// Interrupt enable register.
Electrotiger 0:9afc272fa65f 22 const uint8_t FXOS8700CQ_CTRL_REG5 = 0x2E; /// Interrupt pin register.
Electrotiger 0:9afc272fa65f 23 const uint8_t FXOS8700CQ_M_CTRL_REG1 = 0x5B;
Electrotiger 0:9afc272fa65f 24 const uint8_t FXOS8700CQ_M_CTRL_REG2 = 0x5C;
Electrotiger 0:9afc272fa65f 25 const uint8_t FXOS8700CQ_WHOAMI_VAL = 0xC7;
Electrotiger 0:9afc272fa65f 26
Electrotiger 0:9afc272fa65f 27 // Other defines:
Electrotiger 0:9afc272fa65f 28 static const uint32_t I2CFastModeHz = 400000;
Electrotiger 0:9afc272fa65f 29 static const uint32_t I2CNormalModeHz = 100000;
Electrotiger 0:9afc272fa65f 30 static const uint32_t I2CSuperSlowModeHz = 25000;
Electrotiger 0:9afc272fa65f 31
Electrotiger 0:9afc272fa65f 32 FXOS8700CQ::FXOS8700CQ(PinName SDA, PinName SCL, PinName INT1, PinName INT2, AccelerometerSensitivity setting)
Electrotiger 0:9afc272fa65f 33 : I2CObj(SDA, SCL), dataReadyInt(INT2), accelSensitivitySetting(setting),
Electrotiger 0:9afc272fa65f 34 accelInt2Float((accelSensitivitySetting == TWO) ? 1. / 4096. :
Electrotiger 0:9afc272fa65f 35 (accelSensitivitySetting == FOUR) ? 1. / 2048. : 1. / 1024.),
Electrotiger 0:9afc272fa65f 36 xOffset(0), yOffset(0), zOffset(0)
Electrotiger 0:9afc272fa65f 37 {
Electrotiger 0:9afc272fa65f 38
Electrotiger 0:9afc272fa65f 39 // Set the bus to operate in fast or normal mode.
Electrotiger 0:9afc272fa65f 40 I2CObj.frequency(I2CFastModeHz);
Electrotiger 0:9afc272fa65f 41
Electrotiger 0:9afc272fa65f 42 // Following routine is based off of manufacturer's datasheet example.
Electrotiger 0:9afc272fa65f 43 // Create a buffer to hold information to be transfered.
Electrotiger 0:9afc272fa65f 44 char dataPacket[64];
Electrotiger 0:9afc272fa65f 45
Electrotiger 0:9afc272fa65f 46 /* Reset the device, placing it into standby mode, to set registers. */
Electrotiger 0:9afc272fa65f 47 // standby
Electrotiger 0:9afc272fa65f 48 // [6] = reset = 1;
Electrotiger 0:9afc272fa65f 49 dataPacket[0] = FXOS8700CQ_CTRL_REG2; // First byte is register address.
Electrotiger 0:9afc272fa65f 50 dataPacket[1] = 0b01000000; // Second byte is data - bit 6 set to 1 means reset.
Electrotiger 0:9afc272fa65f 51 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 52 // Wait 2 ms for the reset to take place.
Electrotiger 0:9afc272fa65f 53 wait_ms(2);
Electrotiger 0:9afc272fa65f 54 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 55
Electrotiger 0:9afc272fa65f 56 /* Configure the device to send an interrupt when data is ready. */
Electrotiger 0:9afc272fa65f 57 //[7-1]: Disable all other interrupt sources.
Electrotiger 0:9afc272fa65f 58 //[0]: Enable data-ready interrupt with 1.
Electrotiger 0:9afc272fa65f 59 dataPacket[0] = FXOS8700CQ_CTRL_REG4; // First byte is register address.
Electrotiger 0:9afc272fa65f 60 dataPacket[1] = 0b00000001;
Electrotiger 0:9afc272fa65f 61 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 62 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 63 /* Configure the data-ready interrupt to be on INT2 Pin. */
Electrotiger 0:9afc272fa65f 64 //[7-1]: Other interrupt source pin assignment - we don't care because we only use the data ready interrupt.
Electrotiger 0:9afc272fa65f 65 //[0]: Set to 0 for data-ready interrupt to be on INT2.
Electrotiger 0:9afc272fa65f 66 dataPacket[0] = FXOS8700CQ_CTRL_REG5; // First byte is register address.
Electrotiger 0:9afc272fa65f 67 dataPacket[1] = 0b00000000;
Electrotiger 0:9afc272fa65f 68 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 69 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 70
Electrotiger 0:9afc272fa65f 71 /* Disable the magnetometer. */
Electrotiger 0:9afc272fa65f 72 // write 0001 1111 = 0x1F to magnetometer control register 1
Electrotiger 0:9afc272fa65f 73 // [7]: m_acal=0: auto calibration disabled
Electrotiger 0:9afc272fa65f 74 // [6]: m_rst=0: no one-shot magnetic reset
Electrotiger 0:9afc272fa65f 75 // [5]: m_ost=0: no one-shot magnetic measurement
Electrotiger 0:9afc272fa65f 76 // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
Electrotiger 0:9afc272fa65f 77 // [1-0]: m_hms=00=0: only set the accelerometer to be active.
Electrotiger 0:9afc272fa65f 78 dataPacket[0] = FXOS8700CQ_M_CTRL_REG1; // First byte is register address.
Electrotiger 0:9afc272fa65f 79 dataPacket[1] = 0b00011100; // Second byte is data.
Electrotiger 0:9afc272fa65f 80 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 81 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 82
Electrotiger 0:9afc272fa65f 83
Electrotiger 0:9afc272fa65f 84 /* Configure the device to disable the high-pass filter and
Electrotiger 0:9afc272fa65f 85 * use the setrange.
Electrotiger 0:9afc272fa65f 86 */
Electrotiger 0:9afc272fa65f 87 // [7]: reserved
Electrotiger 0:9afc272fa65f 88 // [6]: reserved
Electrotiger 0:9afc272fa65f 89 // [5]: reserved
Electrotiger 0:9afc272fa65f 90 // [4]: High pass filter filter disabled with 0.
Electrotiger 0:9afc272fa65f 91 // [3]: reserved
Electrotiger 0:9afc272fa65f 92 // [2]: reserved
Electrotiger 0:9afc272fa65f 93 // [1-0]: fs=10 for accelerometer range of +/-8g range with 0.976mg/LSB
Electrotiger 0:9afc272fa65f 94 uint8_t fs =
Electrotiger 0:9afc272fa65f 95 (accelSensitivitySetting == TWO) ? 0b00 :
Electrotiger 0:9afc272fa65f 96 (accelSensitivitySetting == FOUR) ? 0b01 :
Electrotiger 0:9afc272fa65f 97 0b10;
Electrotiger 0:9afc272fa65f 98 dataPacket[0] = FXOS8700CQ_XYZ_DATA_CFG; // First byte is register address.
Electrotiger 0:9afc272fa65f 99 dataPacket[1] = 0b00000000 | fs; // Second byte is data.
Electrotiger 0:9afc272fa65f 100 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 101 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 102 /*
Electrotiger 0:9afc272fa65f 103 * Set the device out of standby mode.
Electrotiger 0:9afc272fa65f 104 */
Electrotiger 0:9afc272fa65f 105 // [7-6]: aslp_rate=00 // Sleep mode output data rate is 50 Hz.
Electrotiger 0:9afc272fa65f 106 // [5-3]: dr=000 for 800Hz data rate in accelerometer only mode.
Electrotiger 0:9afc272fa65f 107 // [2]: lnoise=... for non-low noise mode (Low noise only possible for 2 g or 4 g accelerometer range.)
Electrotiger 0:9afc272fa65f 108 // [1]: f_read=0 for normal 16 bit reads
Electrotiger 0:9afc272fa65f 109 // [0]: active=1 to take the part out of standby and enable sampling
Electrotiger 0:9afc272fa65f 110 uint8_t lNoise = (accelSensitivitySetting != EIGHT) ? 0b100 : 0b000;
Electrotiger 0:9afc272fa65f 111 dataPacket[0] = FXOS8700CQ_CTRL_REG1; // First byte is register address.
Electrotiger 0:9afc272fa65f 112 dataPacket[1] = 0b00000001 | lNoise; // Second byte is data.
Electrotiger 0:9afc272fa65f 113 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, dataPacket, 2);
Electrotiger 0:9afc272fa65f 114 //I2CObj.stop();
Electrotiger 0:9afc272fa65f 115
Electrotiger 0:9afc272fa65f 116 // Attach the data-ready interrupt to a member interrupt handler.
Electrotiger 0:9afc272fa65f 117 // Interrupts are active-low by default.
Electrotiger 0:9afc272fa65f 118 dataReadyInt.fall(this, &FXOS8700CQ::dataReadyISR);
Electrotiger 0:9afc272fa65f 119
Electrotiger 0:9afc272fa65f 120 }
Electrotiger 0:9afc272fa65f 121
Electrotiger 0:9afc272fa65f 122 FXOS8700CQ::~FXOS8700CQ() {
Electrotiger 0:9afc272fa65f 123 // TODO Auto-generated destructor stub
Electrotiger 0:9afc272fa65f 124 }
Electrotiger 0:9afc272fa65f 125
Electrotiger 0:9afc272fa65f 126 void FXOS8700CQ::readAccelerometer(float *xAccel, float *yAccel, float *zAccel) {
Electrotiger 0:9afc272fa65f 127 // In case an interrupt occurs in the middle of our write, we spin while the data is not consistent.
Electrotiger 0:9afc272fa65f 128 while((*xAccel != lastxAccel) || (*yAccel != lastyAccel) || (*zAccel != lastzAccel)) {
Electrotiger 0:9afc272fa65f 129 *xAccel = lastxAccel;
Electrotiger 0:9afc272fa65f 130 *yAccel = lastyAccel;
Electrotiger 0:9afc272fa65f 131 *zAccel = lastzAccel;
Electrotiger 0:9afc272fa65f 132 }
Electrotiger 0:9afc272fa65f 133 }
Electrotiger 0:9afc272fa65f 134
Electrotiger 0:9afc272fa65f 135 void FXOS8700CQ::dataReadyISR(void) {
Electrotiger 0:9afc272fa65f 136 char inputBuffer[7];
Electrotiger 0:9afc272fa65f 137 I2CObj.write(FXOS8700CQ_SLAVE_ADDR, &FXOS8700CQ_STATUS, 1);
Electrotiger 0:9afc272fa65f 138 // Read the accelerometer values back from the slave.
Electrotiger 0:9afc272fa65f 139 I2CObj.read(FXOS8700CQ_SLAVE_ADDR, inputBuffer,7);
Electrotiger 0:9afc272fa65f 140
Electrotiger 0:9afc272fa65f 141 // copy the 14 bit accelerometer byte data into 16 bit words
Electrotiger 0:9afc272fa65f 142 lastxAccel = accelInt2Float * ((int16_t) ((inputBuffer[1] << 8) | inputBuffer[2]) >> 2) - xOffset;
Electrotiger 0:9afc272fa65f 143 lastyAccel = accelInt2Float * ((int16_t) ((inputBuffer[3] << 8) | inputBuffer[4]) >> 2) - yOffset;
Electrotiger 0:9afc272fa65f 144 lastzAccel = accelInt2Float * ((int16_t) ((inputBuffer[5] << 8) | inputBuffer[6]) >> 2) - zOffset;
Electrotiger 0:9afc272fa65f 145 }
Electrotiger 0:9afc272fa65f 146
Electrotiger 0:9afc272fa65f 147