SensorBar.cpp

00001 #include "SensorBar.h"
00002 
00003 const float SensorBar::TS = 0.004f;                       // period of 1 ms
00004 
00005 const char REG_I_ON[16] = {REG_I_ON_0, REG_I_ON_1, REG_I_ON_2, REG_I_ON_3,
00006                            REG_I_ON_4, REG_I_ON_5, REG_I_ON_6, REG_I_ON_7,
00007                            REG_I_ON_8, REG_I_ON_9, REG_I_ON_10, REG_I_ON_11,
00008                            REG_I_ON_12, REG_I_ON_13, REG_I_ON_14, REG_I_ON_15
00009                           };
00010 
00011 const char REG_T_ON[16] = {REG_T_ON_0, REG_T_ON_1, REG_T_ON_2, REG_T_ON_3,
00012                            REG_T_ON_4, REG_T_ON_5, REG_T_ON_6, REG_T_ON_7,
00013                            REG_T_ON_8, REG_T_ON_9, REG_T_ON_10, REG_T_ON_11,
00014                            REG_T_ON_12, REG_T_ON_13, REG_T_ON_14, REG_T_ON_15
00015                           };
00016 
00017 const char REG_OFF[16] = {REG_OFF_0, REG_OFF_1, REG_OFF_2, REG_OFF_3,
00018                           REG_OFF_4, REG_OFF_5, REG_OFF_6, REG_OFF_7,
00019                           REG_OFF_8, REG_OFF_9, REG_OFF_10, REG_OFF_11,
00020                           REG_OFF_12, REG_OFF_13, REG_OFF_14, REG_OFF_15
00021                          };
00022 
00023 const char REG_T_RISE[16] = {0xFF, 0xFF, 0xFF, 0xFF,
00024                              REG_T_RISE_4, REG_T_RISE_5, REG_T_RISE_6, REG_T_RISE_7,
00025                              0xFF, 0xFF, 0xFF, 0xFF,
00026                              REG_T_RISE_12, REG_T_RISE_13, REG_T_RISE_14, REG_T_RISE_15
00027                             };
00028 
00029 const char REG_T_FALL[16] = {0xFF, 0xFF, 0xFF, 0xFF,
00030                              REG_T_FALL_4, REG_T_FALL_5, REG_T_FALL_6, REG_T_FALL_7,
00031                              0xFF, 0xFF, 0xFF, 0xFF,
00032                              REG_T_FALL_12, REG_T_FALL_13, REG_T_FALL_14, REG_T_FALL_15
00033                             };
00034 
00035 SensorBar::SensorBar(I2C& i2c, float distAxisToSensor) : i2c(i2c), thread(osPriorityAboveNormal, 4096)
00036 {
00037     // Store the received parameters into member variables
00038     deviceAddress = 0x3E<<1;
00039     pinInterrupt = 255;
00040     pinOscillator = 255;
00041     pinReset = 255;
00042     invertBits = 0;
00043     barStrobe = 0; //Default always on
00044 
00045     this->distAxisToSensor = distAxisToSensor;
00046     lastBarRawValue = lastBarPositionValue = 0;
00047 
00048     angle = avg_angle = 0;
00049     nrOfLedsActive = 0;
00050     avg_filter.setup(10);
00051     is_first_avg = true;
00052 
00053     clearBarStrobe();  // to illuminate all the time
00054     clearInvertBits(); // to make the bar look for a dark line on a reflective surface
00055 
00056     // set up thread
00057     if (begin()) {
00058         thread.start(callback(this, &SensorBar::update));
00059         ticker.attach(callback(this, &SensorBar::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
00060     }
00061 }
00062 
00063 SensorBar::~SensorBar()
00064 {
00065     ticker.detach();
00066 }
00067 
00068 // --- Functions pulled from the SX1509 driver
00069 
00070 /*
00071 void SensorBar::debounceConfig(uint8_t configValue)
00072 {
00073     // First make sure clock is configured
00074     uint8_t tempuint8_t = readByte(REG_MISC);
00075     if ((tempuint8_t & 0x70) == 0) {
00076         tempuint8_t |= (1 << 4);    // Just default to no divider if not set
00077         writeByte(REG_MISC, tempuint8_t);
00078     }
00079     tempuint8_t = readByte(REG_CLOCK);
00080     if ((tempuint8_t & 0x60) == 0) {
00081         tempuint8_t |= (1 << 6);    // default to internal osc.
00082         writeByte(REG_CLOCK, tempuint8_t);
00083     }
00084 
00085     configValue &= 0b111; // 3-bit value
00086     writeByte(REG_DEBOUNCE_CONFIG, configValue);
00087 }
00088 
00089 void SensorBar::debounceEnable(uint8_t pin)
00090 {
00091     unsigned int debounceEnable = readWord(REG_DEBOUNCE_ENABLE_B);
00092     debounceEnable |= (1 << pin);
00093     writeWord(REG_DEBOUNCE_ENABLE_B, debounceEnable);
00094 }
00095 
00096 unsigned int SensorBar::interruptSource(void)
00097 {
00098     unsigned int intSource = readWord(REG_INTERRUPT_SOURCE_B);
00099     writeWord(REG_INTERRUPT_SOURCE_B, 0xFFFF);    // Clear interrupts
00100     return intSource;
00101 }
00102 
00103 void SensorBar::configClock(uint8_t oscSource, uint8_t oscPinFunction, uint8_t oscFreqOut, uint8_t oscDivider)
00104 {
00105     // RegClock constructed as follows:
00106     //    6:5 - Oscillator frequency souce
00107     //        00: off, 01: external input, 10: internal 2MHz, 1: reserved
00108     //    4 - OSCIO pin function
00109     //        0: input, 1 ouptut
00110     //    3:0 - Frequency of oscout pin
00111     //        0: LOW, 0xF: high, else fOSCOUT = FoSC/(2^(RegClock[3:0]-1))
00112     oscSource = (oscSource & 0b11) << 5;      // 2-bit value, bits 6:5
00113     oscPinFunction = (oscPinFunction & 1) << 4;   // 1-bit value bit 4
00114     oscFreqOut = (oscFreqOut & 0b1111);   // 4-bit value, bits 3:0
00115     uint8_t regClock = oscSource | oscPinFunction | oscFreqOut;
00116     writeByte(REG_CLOCK, regClock);
00117 
00118     // Config RegMisc[6:4] with oscDivider
00119     // 0: off, else ClkX = fOSC / (2^(RegMisc[6:4] -1))
00120     oscDivider = (oscDivider & 0b111) << 4;   // 3-bit value, bits 6:4
00121     uint8_t regMisc = readByte(REG_MISC);
00122     regMisc &= ~(0b111 << 4);
00123     regMisc |= oscDivider;
00124     writeByte(REG_MISC, regMisc);
00125 }
00126 */
00127 
00128 //Call .setBarStrobing(); to only illuminate while reading line
00129 void SensorBar::setBarStrobe()
00130 {
00131     barStrobe = 1; //Do strobe
00132 }
00133 
00134 //Call .clearBarStrobing(); to illuminate all the time
00135 void SensorBar::clearBarStrobe()
00136 {
00137     barStrobe = 0; //Always on
00138 }
00139 
00140 // .setInvertBits(); to make the bar functions look for a white line on dark surface
00141 void SensorBar::setInvertBits()
00142 {
00143     invertBits = 1; //Do strobe
00144 }
00145 
00146 // .clearInvertBits(); to make the bar look for a dark line on a reflective surface
00147 void SensorBar::clearInvertBits()
00148 {
00149     invertBits = 0; //Always on
00150 }
00151 
00152 //****************************************************************************//
00153 //
00154 //  Bar functions
00155 //
00156 //****************************************************************************//
00157 
00158 uint8_t SensorBar::getRaw()
00159 {
00160     return lastBarRawValue;
00161 }
00162 
00163 int8_t SensorBar::getBinaryPosition()
00164 {
00165     return -lastBarPositionValue;
00166 }
00167 
00168 float SensorBar::getAngleRad()
00169 {
00170     return angle;
00171 }
00172 
00173 float SensorBar::getAvgAngleRad()
00174 {
00175     return avg_angle;
00176 }
00177 
00178 uint8_t SensorBar::getNrOfLedsActive()
00179 {
00180     return nrOfLedsActive;
00181 }
00182 
00183 bool SensorBar::isAnyLedActive()
00184 {
00185     bool retval = false;
00186     if(nrOfLedsActive != 0) {
00187         retval =  true;
00188     }
00189     return retval;
00190 }
00191 
00192 //****************************************************************************//
00193 //
00194 //  Utilities
00195 //
00196 //****************************************************************************//
00197 
00198 //Run this once during initialization to configure the SX1509 as a sensor bar
00199 //Returns 1 for success
00200 bool SensorBar::begin(void)
00201 {
00202     bool returnVar = false;
00203 
00204     // Reset the SX1509
00205     reset();
00206 
00207     // Communication test. We'll read from two registers with different
00208     // default values to verify communication.
00209     unsigned int testRegisters = 0;
00210     testRegisters = readWord(REG_INTERRUPT_MASK_A);   // This should return 0xFF00
00211     // Then read a uint8_t that should be 0x00
00212     if (testRegisters == 0xFF00) {
00213         //Success!  Configure the device.
00214         writeByte(REG_DIR_A, 0xFF);
00215         writeByte(REG_DIR_B, 0xFC);
00216         writeByte(REG_DATA_B, 0x01);
00217 
00218         returnVar = true;
00219     }
00220 
00221     return returnVar;
00222 }
00223 
00224 // Do a software reset
00225 void SensorBar::reset()
00226 {
00227     // No hardware option, try software reset
00228     writeByte(REG_RESET, 0x12);
00229     writeByte(REG_RESET, 0x34);
00230 }
00231 
00232 // readByte(uint8_t registerAddress)
00233 //  This function reads a single uint8_t located at the registerAddress register.
00234 //  - deviceAddress should already be set by the constructor.
00235 //  - Return value is the uint8_t read from registerAddress
00236 //
00237 //  Currently returns 0 if communication has timed out
00238 //
00239 uint8_t SensorBar::readByte(uint8_t registerAddress)
00240 {
00241     char readValue;
00242     char data[2] = {registerAddress, 0};
00243     i2c.write(deviceAddress, data, 1);
00244     uint8_t val = i2c.read(deviceAddress, &readValue, 1);
00245 
00246     return readValue;
00247 }
00248 
00249 // readWord(uint8_t registerAddress)
00250 //  This function will read a two-uint8_t word beginning at registerAddress
00251 //  - A 16-bit unsigned int will be returned.
00252 //      - The msb of the return value will contain the value read from registerAddress
00253 //      - The lsb of the return value will contain the value read from registerAddress + 1
00254 unsigned int SensorBar::readWord(uint8_t registerAddress)
00255 {
00256     unsigned int readValue;
00257     unsigned int msb, lsb;
00258     //unsigned int timeout = RECEIVE_TIMEOUT_VALUE * 2;
00259     char data[2] = {registerAddress, 0};
00260     char r_data[2];
00261     uint8_t val = i2c.write(deviceAddress, data, 1);
00262     val = i2c.read(deviceAddress, r_data, 2);
00263     msb = ((unsigned int)r_data[0] & 0x00FF) << 8;
00264     lsb = ((unsigned int)r_data[1] & 0x00FF);
00265     readValue = msb | lsb;
00266 
00267     return readValue;
00268 }
00269 
00270 // readBytes(uint8_t firstRegisterAddress, uint8_t * destination, uint8_t length)
00271 //  This function reads a series of uint8_ts incrementing from a given address
00272 //  - firstRegsiterAddress is the first address to be read
00273 //  - destination is an array of uint8_ts where the read values will be stored into
00274 //  - length is the number of uint8_ts to be read
00275 //  - No return value.
00276 void SensorBar::readBytes(uint8_t firstRegisterAddress, char * destination, uint8_t length)
00277 {
00278     char data[2] = {firstRegisterAddress, 0};
00279     i2c.write(deviceAddress, data, 1);
00280     uint8_t val = i2c.read(deviceAddress, destination, length);
00281 }
00282 
00283 // writeByte(uint8_t registerAddress, uint8_t writeValue)
00284 //  This function writes a single uint8_t to a single register on the SX509.
00285 //  - writeValue is written to registerAddress
00286 //  - deviceAddres should already be set from the constructor
00287 //  - No return value.
00288 void SensorBar::writeByte(uint8_t registerAddress, uint8_t writeValue)
00289 {
00290     char data[2] = {registerAddress, writeValue};
00291     i2c.write(deviceAddress, data, 2);
00292 }
00293 
00294 // writeWord(uint8_t registerAddress, ungisnged int writeValue)
00295 //  This function writes a two-uint8_t word to registerAddress and registerAddress + 1
00296 //  - the upper uint8_t of writeValue is written to registerAddress
00297 //      - the lower uint8_t of writeValue is written to registerAddress + 1
00298 //  - No return value.
00299 void SensorBar::writeWord(uint8_t registerAddress, unsigned int writeValue)
00300 {
00301     uint8_t msb, lsb;
00302     msb = ((writeValue & 0xFF00) >> 8);
00303     lsb = (writeValue & 0x00FF);
00304     char data[3] = {registerAddress, msb, lsb};
00305     i2c.write(deviceAddress, data, 3);
00306 }
00307 
00308 // writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
00309 //  This function writes an array of uint8_ts, beggining at a specific adddress
00310 //  - firstRegisterAddress is the initial register to be written.
00311 //      - All writes following will be at incremental register addresses.
00312 //  - writeArray should be an array of uint8_t values to be written.
00313 //  - length should be the number of uint8_ts to be written.
00314 //  - no return value.
00315 void SensorBar::writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
00316 {
00317     char data[10] = {};
00318     data[0] = firstRegisterAddress;
00319     for(int i = 0; i < length; i++) {
00320         data[1+i] = writeArray[i];
00321     }
00322     i2c.write(deviceAddress, data, length+1);
00323 }
00324 
00325 void SensorBar::update()
00326 {
00327     while(true) {
00328         ThisThread::flags_wait_any(threadFlag);
00329 
00330         //Assign values to each bit, -127 to 127, sum, and divide
00331         int16_t accumulator = 0;
00332         uint8_t bitsCounted = 0;
00333         int16_t i;
00334 
00335         //Get the information from the wire, stores in lastBarRawValue
00336         if( barStrobe == 1 ) {
00337             writeByte(REG_DATA_B, 0x02); //Turn on IR
00338             thread_sleep_for(2); // wait_us(2000);
00339             writeByte(REG_DATA_B, 0x00); //Turn on feedback
00340         } else {
00341             writeByte(REG_DATA_B, 0x00); //make sure both IR and indicators are on
00342         }
00343         //Operate the I2C machine
00344         lastBarRawValue = readByte( REG_DATA_A );  //Peel the data off port A
00345 
00346         if( invertBits == 1 ) { //Invert the bits if needed
00347             lastBarRawValue ^= 0xFF;
00348         }
00349 
00350         if( barStrobe == 1 ) {
00351             writeByte(REG_DATA_B, 0x03); //Turn off IR and feedback when done
00352         }
00353 
00354         //count bits
00355         for ( i = 0; i < 8; i++ ) {
00356             if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
00357                 bitsCounted++;
00358             }
00359         }
00360 
00361         //Find the vector value of each positive bit and sum
00362         for ( i = 7; i > 3; i-- ) { //iterate negative side bits
00363             if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
00364                 accumulator += ((-32 * (i - 3)) + 1);
00365             }
00366         }
00367         for ( i = 0; i < 4; i++ ) { //iterate positive side bits
00368             if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
00369                 accumulator += ((32 * (4 - i)) - 1);
00370             }
00371         }
00372 
00373         if ( bitsCounted > 0 ) {
00374             lastBarPositionValue = accumulator / bitsCounted;
00375         } else {
00376             lastBarPositionValue = 0;
00377         }
00378 
00379         //Update member variables
00380         angle = updateAngleRad();
00381         nrOfLedsActive = updateNrOfLedsActive();
00382 
00383         if(nrOfLedsActive == 0) {
00384             if(!is_first_avg) {
00385                 avg_filter.reset();
00386                 is_first_avg = true;
00387             }
00388         } else {
00389             if(is_first_avg) {
00390                 is_first_avg = false;
00391                 avg_filter.reset(angle);
00392             }
00393             avg_angle = avg_filter.update(angle);
00394         }
00395     }
00396 }
00397 
00398 float SensorBar::updateAngleRad()
00399 {
00400     int8_t binaryPosition  = getBinaryPosition();
00401     float position = static_cast<float>(binaryPosition) / 127.0f * 0.0445f; // 0.0445 m is half of sensor length
00402     return atan2f(position, distAxisToSensor);
00403 }
00404 
00405 uint8_t SensorBar::updateNrOfLedsActive()
00406 {
00407     uint8_t bitsCounted = 0;
00408     uint8_t i;
00409 
00410     //count bits
00411     for ( i = 0; i < 8; i++ ) {
00412         if ( ((lastBarRawValue >> i) & 0x01) == 1 ) {
00413             bitsCounted++;
00414         }
00415     }
00416     return bitsCounted;
00417 }
00418 
00419 void SensorBar::sendThreadFlag()
00420 {
00421     thread.flags_set(threadFlag);
00422 }
00423 
00424 //****************************************************************************//
00425 //
00426 //  Circular buffer
00427 //
00428 //****************************************************************************//
00429 /*
00430 //Construct a CircularBuffer type with arguments
00431 //  uint16_t inputSize: number of elements
00432 
00433 //mbed has CircularBuffer
00434 namespace name
00435 {
00436 
00437 CircularBuffer::CircularBuffer(uint16_t inputSize)
00438 {
00439     cBufferData = new int16_t[inputSize];
00440     cBufferLastPtr = 0;
00441     cBufferElementsUsed = 0;
00442     cBufferSize = inputSize;
00443 }
00444 
00445 CircularBuffer::~CircularBuffer()
00446 {
00447     delete[] cBufferData;
00448 }
00449 
00450 //Get an element at some depth into the circular buffer
00451 //zero is the push location.  Max is cBufferSize - 1
00452 //
00453 //Arguments:
00454 //  uint16_t elementNum: number of element in
00455 //
00456 int16_t CircularBuffer::getElement( uint16_t elementNum )
00457 {
00458     //Translate elementNum into terms of cBufferLastPtr.
00459     int16_t virtualElementNum;
00460     virtualElementNum = cBufferLastPtr - elementNum;
00461     if( virtualElementNum < 0 ) {
00462         virtualElementNum += cBufferSize;
00463     }
00464 
00465     //Output the value
00466     return cBufferData[virtualElementNum];
00467 }
00468 
00469 //Put a new element into the buffer.
00470 //This also expands the size up to the max size
00471 //Arguments:
00472 //
00473 //  int16_t elementVal: value of new element
00474 //
00475 void CircularBuffer::pushElement( int16_t elementVal )
00476 {
00477     //inc. the pointer
00478     cBufferLastPtr++;
00479 
00480     //deal with roll
00481     if( cBufferLastPtr >= cBufferSize ) {
00482         cBufferLastPtr = 0;
00483     }
00484 
00485     //write data
00486     cBufferData[cBufferLastPtr] = elementVal;
00487 
00488     //increase length up to cBufferSize
00489     if( cBufferElementsUsed < cBufferSize ) {
00490         cBufferElementsUsed++;
00491     }
00492 }
00493 
00494 //Averages the last n numbers and provides that.  Discards fractions
00495 int16_t CircularBuffer::averageLast( uint16_t numElements )
00496 {
00497     //Add up all the elements
00498     int32_t accumulator = 0;
00499     int8_t i;
00500     for( i = 0; i < numElements; i++ ) {
00501         accumulator += getElement( i );
00502     }
00503     //Divide by number of elements
00504     accumulator /= numElements;
00505     return accumulator;
00506 }
00507 
00508 //Returns the current size of the buffer
00509 uint16_t CircularBuffer::recordLength()
00510 {
00511     return cBufferElementsUsed;
00512 }
00513 
00514 }
00515 */