File content as of revision 1:a01dc8b52be4:

//Never code working properly for changing addresses of Lidar units 
//Refresh range and velocity functions work well

#include "LidarLite.h"
#include "mbed.h"
#include "PeripheralPins.h"
#include "PeripheralNames.h"

LidarLite::LidarLite(PinName sda, PinName scl):
    i2c_(sda, scl),debug(USBTX,USBRX)
    {
    i2c_.frequency(100000); //I2C @ 100kHz
    wait(0.5);
    //configure(int = 0, char = 0x62);
}

/* =============================================================================
  Configure
  Sets the configuration for the sensor, typically this is done in the begin()
  command, but sometimes (especially for multi-sensor applications) you will
  need to do this separately.
  Parameters
  ------------------------------------------------------------------------------
  - configuration: set the configuration for the sensor
    - default or 0 = equivelent to writing 0x00 to 0x00, i.e. full reset of
      sensor, if you write nothing for configuration or 0, the sensor will init-
      iate normally
    - 1 = high speed setting, set the aquisition count to 1/3 the default (works
      great for stronger singles) can be a little noisier
============================================================================= */
void LidarLite::configure(int configuration, char LidarLiteI2cAddress){
  uint8_t nackack = 1;
  switch (configuration){
    case 0: //  Default configuration
        nackack=i2c_.write(LidarLiteI2cAddress);        //device address with write condition
        if(nackack != 1)break;                       //No Ack, return False
        nackack=i2c_.write(0x00);                       //device ram address where obj value is present
        if(nackack != 1)break;                       //No Ack, return False
        //ack=i2c_.write(LidarLiteI2cAddress|0x00);   //device address with write condition (read is 0x01)
        //if(!ack)return false;                       //No Ack, return False
        i2c_.write(0x00);                              
    break;
    /*
    case 1: //  Set aquisition count to 1/3 default value, faster reads, slightly
            //  noisier values
      write(0x04,0x00,LidarLiteI2cAddress);
    break;
    case 2: //  Low noise, low sensitivity: Pulls decision criteria higher
            //  above the noise, allows fewer false detections, reduces
            //  sensitivity
      write(0x1c,0x20,LidarLiteI2cAddress);
    break;
    case 3: //  High noise, high sensitivity: Pulls decision criteria into the
            //  noise, allows more false detections, increses sensitivity
      write(0x1c,0x60,LidarLiteI2cAddress);
    break;
    */
  }
}



/* =============================================================================
  Change I2C Address for Single Sensor
  LIDAR-Lite now has the ability to change the I2C address of the sensor and
  continue to use the default address or disable it. This function only works
  for single sensors. When the sensor powers off and restarts this value will
  be lost and will need to be configured again.
  There are only certain address that will work with LIDAR-Lite so be sure to
  review the "Notes" section below
  Process
  ------------------------------------------------------------------------------
  1.  Read the two byte serial number from register 0x96
  2.  Write the low byte of the serial number to 0x18
  3.  Write the high byte of the serial number to 0x19
  4.  Write the new address you want to use to 0x1a
  5.  Choose whether to use the default address or not (you must do one of the
      following to commit the new address):
      1.  If you want to keep the default address, write 0x00 to register 0x1e
      2.  If you do not want to keep the default address write 0x08 to 0x1e
  Parameters
  ------------------------------------------------------------------------------
  - newI2cAddress: the hex value of the I2C address you want the sensor to have
  - disablePrimaryAddress (optional): true/false value to disable the primary
    address, default is false (i.e. leave primary active)
  - currentLidarLiteAddress (optional): the default is 0x62, but can also be any
    value you have previously set (ex. if you set the address to 0x66 and dis-
    abled the default address then needed to change it, you would use 0x66 here)
  Example Usage
  ------------------------------------------------------------------------------
  1.  //  Set the value to 0x66 with primary address active and starting with
      //  0x62 as the current address
      myLidarLiteInstance.changeAddress(0x66);
  Notes
  ------------------------------------------------------------------------------
    Possible Address for LIDAR-Lite
    7-bit address in binary form need to end in "0". Example: 0x62 = 01100010 so
    that works well for us. Essentially any even numbered hex value will work
    for 7-bit address.
    8-bit read address in binary form need to end in "00". Example: the default
    8-bit read address for LIDAR-Lite is 0xc4 = 011000100. Essentially any hex
    value evenly divisable by "4" will work.
  =========================================================================== */
void LidarLite::changeAddress(char newI2cAddress,  bool disablePrimaryAddress, char currentLidarLiteAddress){
  //  Array to save the serial number
  char serialNumber[2];
  char serialAdr[1] = {0x96};
  //char newI2cAddressArray[1];
  uint8_t nackack = 1;

  //  Read two bytes from 0x96 to get the serial number
  //read(0x96,2,serialNumber,false,currentLidarLiteAddress);
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), serialAdr, 1);
    }
    nackack = 1; 
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.read((currentLidarLiteAddress<<1)|0x01, serialNumber, 2);
    }
    /*               
    nackack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x96);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x01);       //device address with write condition (write is 0x00) 
    //if(!ack)return false;                               //No Ack, return False
    serialNumber[0]=i2c_.read(1);     //Tobj low byte
    serialNumber[1]=i2c_.read(1);     //Tobj high byte
    */
  
  //  Write the low byte of the serial number to 0x18
  //write(0x18,serialNumber[0],currentLidarLiteAddress);
    char lowbyte[2] = {0x18, serialNumber[0]};
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), lowbyte, 2);
    } 
    /*
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write(currentLidarLiteAddress, write, 2);
    }                    
    ack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x18);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x00);       //device address with write condition (read is 0x01) 
    //if(!ack)return false;                               //No Ack, return False
    i2c_.write(serialNumber[0]);     //Tobj low byte
    */
    
  //  Write the high byte of the serial number of 0x19
  //write(0x19,serialNumber[1],currentLidarLiteAddress); 
    char highbyte[2] = {0x19, serialNumber[1]};
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), highbyte, 2);
    }
    /*             
    ack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x19);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x00);       //device address with write condition (read is 0x01) 
    //if(!ack)return false;                               //No Ack, return False
    i2c_.write(serialNumber[1]);     //Tobj high byte
    */
  
  //  Write the new address to 0x1a
  //write(0x1a,newI2cAddress,currentLidarLiteAddress); 
    char newaddress[2] = {0x1a, newI2cAddress};
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), newaddress, 2);
    }
    /*              
    ack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x1a);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x00);       //device address with write condition (read is 0x01) 
    //if(!ack)return false;                               //No Ack, return False
    i2c_.write(newI2cAddress);       //Tobj high byte
    */

  //while(newI2cAddress != newI2cAddressArray[0]){
  //  read(0x1a,1,newI2cAddressArray,false,currentLidarLiteAddress);
  //}
  
  printf("WIN!");
  //  Choose whether or not to use the default address of 0x62
  if(disablePrimaryAddress){
    //write(0x1e,0x08,currentLidarLiteAddress); 
    char disable[2] = {0x1e, 0x08};
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), disable, 2);
    }
    /*                
    ack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x1e);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x00);       //device address with write condition (read is 0x01) 
    //if(!ack)return false;                               //No Ack, return False
    i2c_.write(0x08);       //Tobj high byte
    */
  }else{
    //write(0x1e,0x00,currentLidarLiteAddress);
    char enable[2] = {0x1e, 0x00};
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write((currentLidarLiteAddress<<1), enable, 2);
    } 
    /*                  
    ack=i2c_.write(currentLidarLiteAddress);            //device address with write condition
    if(!ack)return;                               //No Ack, return False
    ack=i2c_.write(0x1e);                               //device ram address where Tobj value is present
    if(!ack)return;                               //No Ack, return False
    //ack=i2c_.write(currentLidarLiteAddress|0x00);       //device address with write condition (read is 0x01) 
    //if(!ack)return false;                               //No Ack, return False
    i2c_.write(0x00);       //Tobj high byte
    */
  }
  return;
}


   
/* =============================================================================
  Change I2C Address for Multiple Sensors
  Using the new I2C address change feature, you can also change the address for
  multiple sensors using the PWR_EN line connected to Arduino's digital pins.
  Address changes will be lost on power off.
  Process
  ------------------------------------------------------------------------------
  1.
  Parameters
  ------------------------------------------------------------------------------
  - numberOfSensors: int representing the number of sensors you have connected
  - pinArray: array of the digital pins your sensors' PWR_EN line is connected
    to
  - i2cAddressArray: array of the I2C address you want to assign to your sen-
    sors, the order should reflect the order of the pinArray (see not for poss-
    ible addresses below)
  - usePartyLine(optional): true/false value of weather or not to leave 0x62
    available to all sensors for write (default is false)
  Example Usage
  ------------------------------------------------------------------------------
  1.  //  Assign new address to the sensors connected to sensorsPins and disable
      //  0x62 as a partyline to talk to all of the sensors
      int sensorPins[] = {2,3,4};
      unsigned char addresses[] = {0x66,0x68,0x64};
      myLidarLiteInstance.changeAddressMultisensor(3,sensorPins,addresses);
  Notes
  ------------------------------------------------------------------------------
    Possible Address for LIDAR-Lite
    7-bit address in binary form need to end in "0". Example: 0x62 = 01100010 so
    that works well for us. Essentially any even numbered hex value will work
    for 7-bit address.
    8-bit read address in binary form need to end in "00". Example: the default
    8-bit read address for LIDAR-Lite is 0xc4 = 011000100. Essentially any hex
    value evenly divisable by "4" will work.
  =========================================================================== */
void LidarLite::changeAddressMultiPwrEn(int numOfSensors, DigitalOut *pinArray, char *i2cAddressArray, char currentAddress){
    for (int i = 0; i < numOfSensors; i++){
      //pinMode(pinArray[i], OUTPUT); // Pin to first LIDAR-Lite Power Enable line
      wait(3);
      //pinArray[i] = 0;
      //wait(5);
      pinArray[i] = 1; //digitalWrite(pinArray[i], HIGH);
      wait(3);
      //configure(int = 0, char = 0x62);
      changeAddress(i2cAddressArray[i],true,currentAddress); // We have to turn off the party line to actually get these to load
      pinArray[i] = 0;
      wait(3);
    }
    //return newI2cAddress;
}



//get functions can be used for returning values while refresh functions can be used for updating without return,
//this may come in handy for threading, just remove the return statements in the refresh functions and make them void
int16_t LidarLite::getRange_cm()
{
   return(distance_LL);
}

int16_t LidarLite::getVelocity_cms()
{
    if(velocity_LL < 127)
        return(velocity_LL*10);
    else
        return((velocity_LL-256)*10);
}



//Refresh Range for returning distance
int16_t LidarLite::refreshRange(char LIDARLite_WriteAdr, char LIDARLite_ReadAdr)
{
    uint8_t nackack;
    char write[2]={SET_CommandReg, AcqMode};
    char read_dist[1]={GET_Distance2BReg};
    char dist[2];
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write(LIDARLite_WriteAdr, write, 2);
    }
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write(LIDARLite_WriteAdr, read_dist, 1);
    }    
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.read(LIDARLite_ReadAdr, dist, 2);
    }
    distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
    
    return(distance_LL);
}



//Refreshes velocity for return velocity
int16_t LidarLite::refreshVelocity(char LIDARLite_WriteAdr, char LIDARLite_ReadAdr)
{
    uint8_t nackack;
    char write[2]={SET_CommandReg, AcqMode};
    char read_vel[1]={GET_VelocityReg};
    char vel[1];
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write(LIDARLite_WriteAdr, write, 2);
    }
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.write(LIDARLite_WriteAdr, read_vel, 1);
    }    
    
    nackack=1;
    while(nackack !=0)
    {
        wait_ms(1);
        nackack = i2c_.read(LIDARLite_ReadAdr, vel, 1);
    }
    velocity_LL = (uint16_t)vel[0];
    
    if(velocity_LL < 127)
        return(velocity_LL*10);
    else
        return((velocity_LL-256)*10);
}