Michael Ernst Peter / PM2_Libary

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

SensorBar.cpp@24:f2614d8577a1, 2022-05-05 (annotated)

Committer:
pmic
Date:
Thu May 05 07:40:34 2022 +0000
Revision:
24:f2614d8577a1
Child:
25:51e92f2c89e2
Included SensorBar and AvgFilter (not finished yet)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 24:f2614d8577a1 1 #include "SensorBar.h"
pmic 24:f2614d8577a1 2
pmic 24:f2614d8577a1 3 const float SensorBar::TS = 0.004f; // period of 1 ms
pmic 24:f2614d8577a1 4
pmic 24:f2614d8577a1 5 const char REG_I_ON[16] = {REG_I_ON_0, REG_I_ON_1, REG_I_ON_2, REG_I_ON_3,
pmic 24:f2614d8577a1 6 REG_I_ON_4, REG_I_ON_5, REG_I_ON_6, REG_I_ON_7,
pmic 24:f2614d8577a1 7 REG_I_ON_8, REG_I_ON_9, REG_I_ON_10, REG_I_ON_11,
pmic 24:f2614d8577a1 8 REG_I_ON_12, REG_I_ON_13, REG_I_ON_14, REG_I_ON_15};
pmic 24:f2614d8577a1 9
pmic 24:f2614d8577a1 10 const char REG_T_ON[16] = {REG_T_ON_0, REG_T_ON_1, REG_T_ON_2, REG_T_ON_3,
pmic 24:f2614d8577a1 11 REG_T_ON_4, REG_T_ON_5, REG_T_ON_6, REG_T_ON_7,
pmic 24:f2614d8577a1 12 REG_T_ON_8, REG_T_ON_9, REG_T_ON_10, REG_T_ON_11,
pmic 24:f2614d8577a1 13 REG_T_ON_12, REG_T_ON_13, REG_T_ON_14, REG_T_ON_15};
pmic 24:f2614d8577a1 14
pmic 24:f2614d8577a1 15 const char REG_OFF[16] = {REG_OFF_0, REG_OFF_1, REG_OFF_2, REG_OFF_3,
pmic 24:f2614d8577a1 16 REG_OFF_4, REG_OFF_5, REG_OFF_6, REG_OFF_7,
pmic 24:f2614d8577a1 17 REG_OFF_8, REG_OFF_9, REG_OFF_10, REG_OFF_11,
pmic 24:f2614d8577a1 18 REG_OFF_12, REG_OFF_13, REG_OFF_14, REG_OFF_15};
pmic 24:f2614d8577a1 19
pmic 24:f2614d8577a1 20 const char REG_T_RISE[16] = {0xFF, 0xFF, 0xFF, 0xFF,
pmic 24:f2614d8577a1 21 REG_T_RISE_4, REG_T_RISE_5, REG_T_RISE_6, REG_T_RISE_7,
pmic 24:f2614d8577a1 22 0xFF, 0xFF, 0xFF, 0xFF,
pmic 24:f2614d8577a1 23 REG_T_RISE_12, REG_T_RISE_13, REG_T_RISE_14, REG_T_RISE_15};
pmic 24:f2614d8577a1 24
pmic 24:f2614d8577a1 25 const char REG_T_FALL[16] = {0xFF, 0xFF, 0xFF, 0xFF,
pmic 24:f2614d8577a1 26 REG_T_FALL_4, REG_T_FALL_5, REG_T_FALL_6, REG_T_FALL_7,
pmic 24:f2614d8577a1 27 0xFF, 0xFF, 0xFF, 0xFF,
pmic 24:f2614d8577a1 28 REG_T_FALL_12, REG_T_FALL_13, REG_T_FALL_14, REG_T_FALL_15};
pmic 24:f2614d8577a1 29
pmic 24:f2614d8577a1 30 SensorBar::SensorBar(I2C& i2c, float distAxisToSensor) : i2c(i2c), thread(osPriorityAboveNormal, 4096)
pmic 24:f2614d8577a1 31 {
pmic 24:f2614d8577a1 32 // Store the received parameters into member variables
pmic 24:f2614d8577a1 33 deviceAddress = 0x3E<<1;
pmic 24:f2614d8577a1 34 pinInterrupt = 255;
pmic 24:f2614d8577a1 35 pinOscillator = 255;
pmic 24:f2614d8577a1 36 pinReset = 255;
pmic 24:f2614d8577a1 37 invertBits = 0;
pmic 24:f2614d8577a1 38 barStrobe = 0; //Default always on
pmic 24:f2614d8577a1 39
pmic 24:f2614d8577a1 40 this->distAxisToSensor = distAxisToSensor;
pmic 24:f2614d8577a1 41 lastBarRawValue = lastBarPositionValue = 0;
pmic 24:f2614d8577a1 42
pmic 24:f2614d8577a1 43 clearBarStrobe(); // to illuminate all the time
pmic 24:f2614d8577a1 44 clearInvertBits(); // to make the bar look for a dark line on a reflective surface
pmic 24:f2614d8577a1 45 begin();
pmic 24:f2614d8577a1 46
pmic 24:f2614d8577a1 47 // set up thread
pmic 24:f2614d8577a1 48 thread.start(callback(this, &SensorBar::update));
pmic 24:f2614d8577a1 49 ticker.attach(callback(this, &SensorBar::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 24:f2614d8577a1 50 }
pmic 24:f2614d8577a1 51
pmic 24:f2614d8577a1 52 SensorBar::~SensorBar()
pmic 24:f2614d8577a1 53 {
pmic 24:f2614d8577a1 54 ticker.detach();
pmic 24:f2614d8577a1 55 }
pmic 24:f2614d8577a1 56
pmic 24:f2614d8577a1 57 // --- Functions pulled from the SX1509 driver
pmic 24:f2614d8577a1 58
pmic 24:f2614d8577a1 59 /*
pmic 24:f2614d8577a1 60 void SensorBar::debounceConfig(uint8_t configValue)
pmic 24:f2614d8577a1 61 {
pmic 24:f2614d8577a1 62 // First make sure clock is configured
pmic 24:f2614d8577a1 63 uint8_t tempuint8_t = readByte(REG_MISC);
pmic 24:f2614d8577a1 64 if ((tempuint8_t & 0x70) == 0) {
pmic 24:f2614d8577a1 65 tempuint8_t |= (1 << 4); // Just default to no divider if not set
pmic 24:f2614d8577a1 66 writeByte(REG_MISC, tempuint8_t);
pmic 24:f2614d8577a1 67 }
pmic 24:f2614d8577a1 68 tempuint8_t = readByte(REG_CLOCK);
pmic 24:f2614d8577a1 69 if ((tempuint8_t & 0x60) == 0) {
pmic 24:f2614d8577a1 70 tempuint8_t |= (1 << 6); // default to internal osc.
pmic 24:f2614d8577a1 71 writeByte(REG_CLOCK, tempuint8_t);
pmic 24:f2614d8577a1 72 }
pmic 24:f2614d8577a1 73
pmic 24:f2614d8577a1 74 configValue &= 0b111; // 3-bit value
pmic 24:f2614d8577a1 75 writeByte(REG_DEBOUNCE_CONFIG, configValue);
pmic 24:f2614d8577a1 76 }
pmic 24:f2614d8577a1 77
pmic 24:f2614d8577a1 78 void SensorBar::debounceEnable(uint8_t pin)
pmic 24:f2614d8577a1 79 {
pmic 24:f2614d8577a1 80 unsigned int debounceEnable = readWord(REG_DEBOUNCE_ENABLE_B);
pmic 24:f2614d8577a1 81 debounceEnable |= (1 << pin);
pmic 24:f2614d8577a1 82 writeWord(REG_DEBOUNCE_ENABLE_B, debounceEnable);
pmic 24:f2614d8577a1 83 }
pmic 24:f2614d8577a1 84
pmic 24:f2614d8577a1 85 unsigned int SensorBar::interruptSource(void)
pmic 24:f2614d8577a1 86 {
pmic 24:f2614d8577a1 87 unsigned int intSource = readWord(REG_INTERRUPT_SOURCE_B);
pmic 24:f2614d8577a1 88 writeWord(REG_INTERRUPT_SOURCE_B, 0xFFFF); // Clear interrupts
pmic 24:f2614d8577a1 89 return intSource;
pmic 24:f2614d8577a1 90 }
pmic 24:f2614d8577a1 91
pmic 24:f2614d8577a1 92 void SensorBar::configClock(uint8_t oscSource, uint8_t oscPinFunction, uint8_t oscFreqOut, uint8_t oscDivider)
pmic 24:f2614d8577a1 93 {
pmic 24:f2614d8577a1 94 // RegClock constructed as follows:
pmic 24:f2614d8577a1 95 // 6:5 - Oscillator frequency souce
pmic 24:f2614d8577a1 96 // 00: off, 01: external input, 10: internal 2MHz, 1: reserved
pmic 24:f2614d8577a1 97 // 4 - OSCIO pin function
pmic 24:f2614d8577a1 98 // 0: input, 1 ouptut
pmic 24:f2614d8577a1 99 // 3:0 - Frequency of oscout pin
pmic 24:f2614d8577a1 100 // 0: LOW, 0xF: high, else fOSCOUT = FoSC/(2^(RegClock[3:0]-1))
pmic 24:f2614d8577a1 101 oscSource = (oscSource & 0b11) << 5; // 2-bit value, bits 6:5
pmic 24:f2614d8577a1 102 oscPinFunction = (oscPinFunction & 1) << 4; // 1-bit value bit 4
pmic 24:f2614d8577a1 103 oscFreqOut = (oscFreqOut & 0b1111); // 4-bit value, bits 3:0
pmic 24:f2614d8577a1 104 uint8_t regClock = oscSource | oscPinFunction | oscFreqOut;
pmic 24:f2614d8577a1 105 writeByte(REG_CLOCK, regClock);
pmic 24:f2614d8577a1 106
pmic 24:f2614d8577a1 107 // Config RegMisc[6:4] with oscDivider
pmic 24:f2614d8577a1 108 // 0: off, else ClkX = fOSC / (2^(RegMisc[6:4] -1))
pmic 24:f2614d8577a1 109 oscDivider = (oscDivider & 0b111) << 4; // 3-bit value, bits 6:4
pmic 24:f2614d8577a1 110 uint8_t regMisc = readByte(REG_MISC);
pmic 24:f2614d8577a1 111 regMisc &= ~(0b111 << 4);
pmic 24:f2614d8577a1 112 regMisc |= oscDivider;
pmic 24:f2614d8577a1 113 writeByte(REG_MISC, regMisc);
pmic 24:f2614d8577a1 114 }
pmic 24:f2614d8577a1 115 */
pmic 24:f2614d8577a1 116
pmic 24:f2614d8577a1 117 //Call .setBarStrobing(); to only illuminate while reading line
pmic 24:f2614d8577a1 118 void SensorBar::setBarStrobe()
pmic 24:f2614d8577a1 119 {
pmic 24:f2614d8577a1 120 barStrobe = 1; //Do strobe
pmic 24:f2614d8577a1 121 }
pmic 24:f2614d8577a1 122
pmic 24:f2614d8577a1 123 //Call .clearBarStrobing(); to illuminate all the time
pmic 24:f2614d8577a1 124 void SensorBar::clearBarStrobe()
pmic 24:f2614d8577a1 125 {
pmic 24:f2614d8577a1 126 barStrobe = 0; //Always on
pmic 24:f2614d8577a1 127 }
pmic 24:f2614d8577a1 128
pmic 24:f2614d8577a1 129 // .setInvertBits(); to make the bar functions look for a white line on dark surface
pmic 24:f2614d8577a1 130 void SensorBar::setInvertBits()
pmic 24:f2614d8577a1 131 {
pmic 24:f2614d8577a1 132 invertBits = 1; //Do strobe
pmic 24:f2614d8577a1 133 }
pmic 24:f2614d8577a1 134
pmic 24:f2614d8577a1 135 // .clearInvertBits(); to make the bar look for a dark line on a reflective surface
pmic 24:f2614d8577a1 136 void SensorBar::clearInvertBits()
pmic 24:f2614d8577a1 137 {
pmic 24:f2614d8577a1 138 invertBits = 0; //Always on
pmic 24:f2614d8577a1 139 }
pmic 24:f2614d8577a1 140
pmic 24:f2614d8577a1 141 //****************************************************************************//
pmic 24:f2614d8577a1 142 //
pmic 24:f2614d8577a1 143 // Bar functions
pmic 24:f2614d8577a1 144 //
pmic 24:f2614d8577a1 145 //****************************************************************************//
pmic 24:f2614d8577a1 146
pmic 24:f2614d8577a1 147 uint8_t SensorBar::getRaw()
pmic 24:f2614d8577a1 148 {
pmic 24:f2614d8577a1 149 return lastBarRawValue;
pmic 24:f2614d8577a1 150 }
pmic 24:f2614d8577a1 151
pmic 24:f2614d8577a1 152 int8_t SensorBar::getBinaryPosition()
pmic 24:f2614d8577a1 153 {
pmic 24:f2614d8577a1 154 return lastBarPositionValue;
pmic 24:f2614d8577a1 155 }
pmic 24:f2614d8577a1 156
pmic 24:f2614d8577a1 157 float SensorBar::getAngleRad()
pmic 24:f2614d8577a1 158 {
pmic 24:f2614d8577a1 159 int8_t binaryPosition = getBinaryPosition();
pmic 24:f2614d8577a1 160 float position = static_cast<float>(binaryPosition) / 127.0f * 0.0445f; // 0.0445 m is half of sensor length
pmic 24:f2614d8577a1 161 return atan2f(position, distAxisToSensor);
pmic 24:f2614d8577a1 162 }
pmic 24:f2614d8577a1 163
pmic 24:f2614d8577a1 164 uint8_t SensorBar::getNrofLedsActive()
pmic 24:f2614d8577a1 165 {
pmic 24:f2614d8577a1 166 uint8_t bitsCounted = 0;
pmic 24:f2614d8577a1 167 uint8_t i;
pmic 24:f2614d8577a1 168
pmic 24:f2614d8577a1 169 //count bits
pmic 24:f2614d8577a1 170 for ( i = 0; i < 8; i++ ) {
pmic 24:f2614d8577a1 171 if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
pmic 24:f2614d8577a1 172 bitsCounted++;
pmic 24:f2614d8577a1 173 }
pmic 24:f2614d8577a1 174 }
pmic 24:f2614d8577a1 175 return bitsCounted;
pmic 24:f2614d8577a1 176 }
pmic 24:f2614d8577a1 177
pmic 24:f2614d8577a1 178
pmic 24:f2614d8577a1 179 //****************************************************************************//
pmic 24:f2614d8577a1 180 //
pmic 24:f2614d8577a1 181 // Utilities
pmic 24:f2614d8577a1 182 //
pmic 24:f2614d8577a1 183 //****************************************************************************//
pmic 24:f2614d8577a1 184
pmic 24:f2614d8577a1 185 //Run this once during initialization to configure the SX1509 as a sensor bar
pmic 24:f2614d8577a1 186 //Returns 1 for success
pmic 24:f2614d8577a1 187 uint8_t SensorBar::begin(void)
pmic 24:f2614d8577a1 188 {
pmic 24:f2614d8577a1 189 uint8_t returnVar = 0;
pmic 24:f2614d8577a1 190
pmic 24:f2614d8577a1 191 // Reset the SX1509
pmic 24:f2614d8577a1 192 reset();
pmic 24:f2614d8577a1 193
pmic 24:f2614d8577a1 194 // Communication test. We'll read from two registers with different
pmic 24:f2614d8577a1 195 // default values to verify communication.
pmic 24:f2614d8577a1 196 unsigned int testRegisters = 0;
pmic 24:f2614d8577a1 197 testRegisters = readWord(REG_INTERRUPT_MASK_A); // This should return 0xFF00
pmic 24:f2614d8577a1 198 // Then read a uint8_t that should be 0x00
pmic 24:f2614d8577a1 199 if (testRegisters == 0xFF00) {
pmic 24:f2614d8577a1 200 //Success! Configure the device.
pmic 24:f2614d8577a1 201 writeByte(REG_DIR_A, 0xFF);
pmic 24:f2614d8577a1 202 writeByte(REG_DIR_B, 0xFC);
pmic 24:f2614d8577a1 203 writeByte(REG_DATA_B, 0x01);
pmic 24:f2614d8577a1 204
pmic 24:f2614d8577a1 205 returnVar = 1;
pmic 24:f2614d8577a1 206 } else {
pmic 24:f2614d8577a1 207 returnVar = 0;
pmic 24:f2614d8577a1 208 }
pmic 24:f2614d8577a1 209
pmic 24:f2614d8577a1 210 return returnVar;
pmic 24:f2614d8577a1 211 }
pmic 24:f2614d8577a1 212
pmic 24:f2614d8577a1 213 // Do a software reset
pmic 24:f2614d8577a1 214 void SensorBar::reset()
pmic 24:f2614d8577a1 215 {
pmic 24:f2614d8577a1 216 // No hardware option, try software reset
pmic 24:f2614d8577a1 217 writeByte(REG_RESET, 0x12);
pmic 24:f2614d8577a1 218 writeByte(REG_RESET, 0x34);
pmic 24:f2614d8577a1 219 }
pmic 24:f2614d8577a1 220
pmic 24:f2614d8577a1 221 // readByte(uint8_t registerAddress)
pmic 24:f2614d8577a1 222 // This function reads a single uint8_t located at the registerAddress register.
pmic 24:f2614d8577a1 223 // - deviceAddress should already be set by the constructor.
pmic 24:f2614d8577a1 224 // - Return value is the uint8_t read from registerAddress
pmic 24:f2614d8577a1 225 //
pmic 24:f2614d8577a1 226 // Currently returns 0 if communication has timed out
pmic 24:f2614d8577a1 227 //
pmic 24:f2614d8577a1 228 uint8_t SensorBar::readByte(uint8_t registerAddress)
pmic 24:f2614d8577a1 229 {
pmic 24:f2614d8577a1 230 char readValue;
pmic 24:f2614d8577a1 231 char data[2] = {registerAddress, 0};
pmic 24:f2614d8577a1 232 i2c.write(deviceAddress, data, 1);
pmic 24:f2614d8577a1 233 uint8_t val = i2c.read(deviceAddress, &readValue, 1);
pmic 24:f2614d8577a1 234
pmic 24:f2614d8577a1 235 return readValue;
pmic 24:f2614d8577a1 236 }
pmic 24:f2614d8577a1 237
pmic 24:f2614d8577a1 238 // readWord(uint8_t registerAddress)
pmic 24:f2614d8577a1 239 // This function will read a two-uint8_t word beginning at registerAddress
pmic 24:f2614d8577a1 240 // - A 16-bit unsigned int will be returned.
pmic 24:f2614d8577a1 241 // - The msb of the return value will contain the value read from registerAddress
pmic 24:f2614d8577a1 242 // - The lsb of the return value will contain the value read from registerAddress + 1
pmic 24:f2614d8577a1 243 unsigned int SensorBar::readWord(uint8_t registerAddress)
pmic 24:f2614d8577a1 244 {
pmic 24:f2614d8577a1 245 unsigned int readValue;
pmic 24:f2614d8577a1 246 unsigned int msb, lsb;
pmic 24:f2614d8577a1 247 //unsigned int timeout = RECEIVE_TIMEOUT_VALUE * 2;
pmic 24:f2614d8577a1 248 char data[2] = {registerAddress, 0};
pmic 24:f2614d8577a1 249 char r_data[2];
pmic 24:f2614d8577a1 250 uint8_t val = i2c.write(deviceAddress, data, 1);
pmic 24:f2614d8577a1 251 val = i2c.read(deviceAddress, r_data, 2);
pmic 24:f2614d8577a1 252 msb = ((unsigned int)r_data[0] & 0x00FF) << 8;
pmic 24:f2614d8577a1 253 lsb = ((unsigned int)r_data[1] & 0x00FF);
pmic 24:f2614d8577a1 254 readValue = msb | lsb;
pmic 24:f2614d8577a1 255
pmic 24:f2614d8577a1 256 return readValue;
pmic 24:f2614d8577a1 257 }
pmic 24:f2614d8577a1 258
pmic 24:f2614d8577a1 259 // readBytes(uint8_t firstRegisterAddress, uint8_t * destination, uint8_t length)
pmic 24:f2614d8577a1 260 // This function reads a series of uint8_ts incrementing from a given address
pmic 24:f2614d8577a1 261 // - firstRegsiterAddress is the first address to be read
pmic 24:f2614d8577a1 262 // - destination is an array of uint8_ts where the read values will be stored into
pmic 24:f2614d8577a1 263 // - length is the number of uint8_ts to be read
pmic 24:f2614d8577a1 264 // - No return value.
pmic 24:f2614d8577a1 265 void SensorBar::readBytes(uint8_t firstRegisterAddress, char * destination, uint8_t length)
pmic 24:f2614d8577a1 266 {
pmic 24:f2614d8577a1 267 char data[2] = {firstRegisterAddress, 0};
pmic 24:f2614d8577a1 268 i2c.write(deviceAddress, data, 1);
pmic 24:f2614d8577a1 269 uint8_t val = i2c.read(deviceAddress, destination, length);
pmic 24:f2614d8577a1 270 }
pmic 24:f2614d8577a1 271
pmic 24:f2614d8577a1 272 // writeByte(uint8_t registerAddress, uint8_t writeValue)
pmic 24:f2614d8577a1 273 // This function writes a single uint8_t to a single register on the SX509.
pmic 24:f2614d8577a1 274 // - writeValue is written to registerAddress
pmic 24:f2614d8577a1 275 // - deviceAddres should already be set from the constructor
pmic 24:f2614d8577a1 276 // - No return value.
pmic 24:f2614d8577a1 277 void SensorBar::writeByte(uint8_t registerAddress, uint8_t writeValue)
pmic 24:f2614d8577a1 278 {
pmic 24:f2614d8577a1 279 char data[2] = {registerAddress, writeValue};
pmic 24:f2614d8577a1 280 i2c.write(deviceAddress, data, 2);
pmic 24:f2614d8577a1 281 }
pmic 24:f2614d8577a1 282
pmic 24:f2614d8577a1 283 // writeWord(uint8_t registerAddress, ungisnged int writeValue)
pmic 24:f2614d8577a1 284 // This function writes a two-uint8_t word to registerAddress and registerAddress + 1
pmic 24:f2614d8577a1 285 // - the upper uint8_t of writeValue is written to registerAddress
pmic 24:f2614d8577a1 286 // - the lower uint8_t of writeValue is written to registerAddress + 1
pmic 24:f2614d8577a1 287 // - No return value.
pmic 24:f2614d8577a1 288 void SensorBar::writeWord(uint8_t registerAddress, unsigned int writeValue)
pmic 24:f2614d8577a1 289 {
pmic 24:f2614d8577a1 290 uint8_t msb, lsb;
pmic 24:f2614d8577a1 291 msb = ((writeValue & 0xFF00) >> 8);
pmic 24:f2614d8577a1 292 lsb = (writeValue & 0x00FF);
pmic 24:f2614d8577a1 293 char data[3] = {registerAddress, msb, lsb};
pmic 24:f2614d8577a1 294 i2c.write(deviceAddress, data, 3);
pmic 24:f2614d8577a1 295 }
pmic 24:f2614d8577a1 296
pmic 24:f2614d8577a1 297 // writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
pmic 24:f2614d8577a1 298 // This function writes an array of uint8_ts, beggining at a specific adddress
pmic 24:f2614d8577a1 299 // - firstRegisterAddress is the initial register to be written.
pmic 24:f2614d8577a1 300 // - All writes following will be at incremental register addresses.
pmic 24:f2614d8577a1 301 // - writeArray should be an array of uint8_t values to be written.
pmic 24:f2614d8577a1 302 // - length should be the number of uint8_ts to be written.
pmic 24:f2614d8577a1 303 // - no return value.
pmic 24:f2614d8577a1 304 void SensorBar::writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
pmic 24:f2614d8577a1 305 {
pmic 24:f2614d8577a1 306 char data[10] = {};
pmic 24:f2614d8577a1 307 data[0] = firstRegisterAddress;
pmic 24:f2614d8577a1 308 for(int i = 0; i < length; i++) {
pmic 24:f2614d8577a1 309 data[1+i] = writeArray[i];
pmic 24:f2614d8577a1 310 }
pmic 24:f2614d8577a1 311 i2c.write(deviceAddress, data, length+1);
pmic 24:f2614d8577a1 312 }
pmic 24:f2614d8577a1 313
pmic 24:f2614d8577a1 314 void SensorBar::update()
pmic 24:f2614d8577a1 315 {
pmic 24:f2614d8577a1 316 while(true) {
pmic 24:f2614d8577a1 317 ThisThread::flags_wait_any(threadFlag);
pmic 24:f2614d8577a1 318
pmic 24:f2614d8577a1 319 //Assign values to each bit, -127 to 127, sum, and divide
pmic 24:f2614d8577a1 320 int16_t accumulator = 0;
pmic 24:f2614d8577a1 321 uint8_t bitsCounted = 0;
pmic 24:f2614d8577a1 322 int16_t i;
pmic 24:f2614d8577a1 323
pmic 24:f2614d8577a1 324 //Get the information from the wire, stores in lastBarRawValue
pmic 24:f2614d8577a1 325 if( barStrobe == 1 ) {
pmic 24:f2614d8577a1 326 writeByte(REG_DATA_B, 0x02); //Turn on IR
pmic 24:f2614d8577a1 327 thread_sleep_for(2); // wait_us(2000);
pmic 24:f2614d8577a1 328 writeByte(REG_DATA_B, 0x00); //Turn on feedback
pmic 24:f2614d8577a1 329 } else {
pmic 24:f2614d8577a1 330 writeByte(REG_DATA_B, 0x00); //make sure both IR and indicators are on
pmic 24:f2614d8577a1 331 }
pmic 24:f2614d8577a1 332 //Operate the I2C machine
pmic 24:f2614d8577a1 333 lastBarRawValue = readByte( REG_DATA_A ); //Peel the data off port A
pmic 24:f2614d8577a1 334
pmic 24:f2614d8577a1 335 if( invertBits == 1 ) { //Invert the bits if needed
pmic 24:f2614d8577a1 336 lastBarRawValue ^= 0xFF;
pmic 24:f2614d8577a1 337 }
pmic 24:f2614d8577a1 338
pmic 24:f2614d8577a1 339 if( barStrobe == 1 ) {
pmic 24:f2614d8577a1 340 writeByte(REG_DATA_B, 0x03); //Turn off IR and feedback when done
pmic 24:f2614d8577a1 341 }
pmic 24:f2614d8577a1 342
pmic 24:f2614d8577a1 343 //count bits
pmic 24:f2614d8577a1 344 for ( i = 0; i < 8; i++ ) {
pmic 24:f2614d8577a1 345 if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
pmic 24:f2614d8577a1 346 bitsCounted++;
pmic 24:f2614d8577a1 347 }
pmic 24:f2614d8577a1 348 }
pmic 24:f2614d8577a1 349
pmic 24:f2614d8577a1 350 //Find the vector value of each positive bit and sum
pmic 24:f2614d8577a1 351 for ( i = 7; i > 3; i-- ) { //iterate negative side bits
pmic 24:f2614d8577a1 352 if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
pmic 24:f2614d8577a1 353 accumulator += ((-32 * (i - 3)) + 1);
pmic 24:f2614d8577a1 354 }
pmic 24:f2614d8577a1 355 }
pmic 24:f2614d8577a1 356 for ( i = 0; i < 4; i++ ) { //iterate positive side bits
pmic 24:f2614d8577a1 357 if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
pmic 24:f2614d8577a1 358 accumulator += ((32 * (4 - i)) - 1);
pmic 24:f2614d8577a1 359 }
pmic 24:f2614d8577a1 360 }
pmic 24:f2614d8577a1 361
pmic 24:f2614d8577a1 362 if ( bitsCounted > 0 ) {
pmic 24:f2614d8577a1 363 lastBarPositionValue = accumulator / bitsCounted;
pmic 24:f2614d8577a1 364 } else {
pmic 24:f2614d8577a1 365 lastBarPositionValue = 0;
pmic 24:f2614d8577a1 366 }
pmic 24:f2614d8577a1 367 }
pmic 24:f2614d8577a1 368 }
pmic 24:f2614d8577a1 369
pmic 24:f2614d8577a1 370 void SensorBar::sendThreadFlag()
pmic 24:f2614d8577a1 371 {
pmic 24:f2614d8577a1 372 thread.flags_set(threadFlag);
pmic 24:f2614d8577a1 373 }
pmic 24:f2614d8577a1 374
pmic 24:f2614d8577a1 375 //****************************************************************************//
pmic 24:f2614d8577a1 376 //
pmic 24:f2614d8577a1 377 // Circular buffer
pmic 24:f2614d8577a1 378 //
pmic 24:f2614d8577a1 379 //****************************************************************************//
pmic 24:f2614d8577a1 380 /*
pmic 24:f2614d8577a1 381 //Construct a CircularBuffer type with arguments
pmic 24:f2614d8577a1 382 // uint16_t inputSize: number of elements
pmic 24:f2614d8577a1 383
pmic 24:f2614d8577a1 384 //mbed has CircularBuffer
pmic 24:f2614d8577a1 385 namespace name
pmic 24:f2614d8577a1 386 {
pmic 24:f2614d8577a1 387
pmic 24:f2614d8577a1 388 CircularBuffer::CircularBuffer(uint16_t inputSize)
pmic 24:f2614d8577a1 389 {
pmic 24:f2614d8577a1 390 cBufferData = new int16_t[inputSize];
pmic 24:f2614d8577a1 391 cBufferLastPtr = 0;
pmic 24:f2614d8577a1 392 cBufferElementsUsed = 0;
pmic 24:f2614d8577a1 393 cBufferSize = inputSize;
pmic 24:f2614d8577a1 394 }
pmic 24:f2614d8577a1 395
pmic 24:f2614d8577a1 396 CircularBuffer::~CircularBuffer()
pmic 24:f2614d8577a1 397 {
pmic 24:f2614d8577a1 398 delete[] cBufferData;
pmic 24:f2614d8577a1 399 }
pmic 24:f2614d8577a1 400
pmic 24:f2614d8577a1 401 //Get an element at some depth into the circular buffer
pmic 24:f2614d8577a1 402 //zero is the push location. Max is cBufferSize - 1
pmic 24:f2614d8577a1 403 //
pmic 24:f2614d8577a1 404 //Arguments:
pmic 24:f2614d8577a1 405 // uint16_t elementNum: number of element in
pmic 24:f2614d8577a1 406 //
pmic 24:f2614d8577a1 407 int16_t CircularBuffer::getElement( uint16_t elementNum )
pmic 24:f2614d8577a1 408 {
pmic 24:f2614d8577a1 409 //Translate elementNum into terms of cBufferLastPtr.
pmic 24:f2614d8577a1 410 int16_t virtualElementNum;
pmic 24:f2614d8577a1 411 virtualElementNum = cBufferLastPtr - elementNum;
pmic 24:f2614d8577a1 412 if( virtualElementNum < 0 ) {
pmic 24:f2614d8577a1 413 virtualElementNum += cBufferSize;
pmic 24:f2614d8577a1 414 }
pmic 24:f2614d8577a1 415
pmic 24:f2614d8577a1 416 //Output the value
pmic 24:f2614d8577a1 417 return cBufferData[virtualElementNum];
pmic 24:f2614d8577a1 418 }
pmic 24:f2614d8577a1 419
pmic 24:f2614d8577a1 420 //Put a new element into the buffer.
pmic 24:f2614d8577a1 421 //This also expands the size up to the max size
pmic 24:f2614d8577a1 422 //Arguments:
pmic 24:f2614d8577a1 423 //
pmic 24:f2614d8577a1 424 // int16_t elementVal: value of new element
pmic 24:f2614d8577a1 425 //
pmic 24:f2614d8577a1 426 void CircularBuffer::pushElement( int16_t elementVal )
pmic 24:f2614d8577a1 427 {
pmic 24:f2614d8577a1 428 //inc. the pointer
pmic 24:f2614d8577a1 429 cBufferLastPtr++;
pmic 24:f2614d8577a1 430
pmic 24:f2614d8577a1 431 //deal with roll
pmic 24:f2614d8577a1 432 if( cBufferLastPtr >= cBufferSize ) {
pmic 24:f2614d8577a1 433 cBufferLastPtr = 0;
pmic 24:f2614d8577a1 434 }
pmic 24:f2614d8577a1 435
pmic 24:f2614d8577a1 436 //write data
pmic 24:f2614d8577a1 437 cBufferData[cBufferLastPtr] = elementVal;
pmic 24:f2614d8577a1 438
pmic 24:f2614d8577a1 439 //increase length up to cBufferSize
pmic 24:f2614d8577a1 440 if( cBufferElementsUsed < cBufferSize ) {
pmic 24:f2614d8577a1 441 cBufferElementsUsed++;
pmic 24:f2614d8577a1 442 }
pmic 24:f2614d8577a1 443 }
pmic 24:f2614d8577a1 444
pmic 24:f2614d8577a1 445 //Averages the last n numbers and provides that. Discards fractions
pmic 24:f2614d8577a1 446 int16_t CircularBuffer::averageLast( uint16_t numElements )
pmic 24:f2614d8577a1 447 {
pmic 24:f2614d8577a1 448 //Add up all the elements
pmic 24:f2614d8577a1 449 int32_t accumulator = 0;
pmic 24:f2614d8577a1 450 int8_t i;
pmic 24:f2614d8577a1 451 for( i = 0; i < numElements; i++ ) {
pmic 24:f2614d8577a1 452 accumulator += getElement( i );
pmic 24:f2614d8577a1 453 }
pmic 24:f2614d8577a1 454 //Divide by number of elements
pmic 24:f2614d8577a1 455 accumulator /= numElements;
pmic 24:f2614d8577a1 456 return accumulator;
pmic 24:f2614d8577a1 457 }
pmic 24:f2614d8577a1 458
pmic 24:f2614d8577a1 459 //Returns the current size of the buffer
pmic 24:f2614d8577a1 460 uint16_t CircularBuffer::recordLength()
pmic 24:f2614d8577a1 461 {
pmic 24:f2614d8577a1 462 return cBufferElementsUsed;
pmic 24:f2614d8577a1 463 }
pmic 24:f2614d8577a1 464
pmic 24:f2614d8577a1 465 }
pmic 24:f2614d8577a1 466 */