Michael Ernst Peter / PM2_Libary

Libary for PM2.

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

SensorBar.cpp@32:bb074bb17d39, 2022-06-22 (annotated)

Committer:
pmic
Date:
Wed Jun 22 14:05:13 2022 +0000
Revision:
32:bb074bb17d39
Parent:
28:6b30640c9e2e 
Updated SensorBar so it is not blocking the program if it is not connected

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