hidaka sato / SensorBar

SparkFun Line Follower Array Library. based on https://github.com/sparkfun/SparkFun_Line_Follower_Array_Arduino_Library

Dependents:   WRS_mechanamu_test

SensorBar.cpp@0:c1d649c7b904, 2018-08-31 (annotated)

Committer:
sgrsn
Date:
Fri Aug 31 02:52:24 2018 +0000
Revision:
0:c1d649c7b904
First commit

Who changed what in which revision?

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