LIDAR Anywhere

// This application uses an Arduino and CC3000 shield to measure
// distance using the LidarLite scanner. The readings are delivered real-time
// to the mobile prototyping application: Blynk.

// Libraries: Adafruit_CC3000, BlinkSimpleCC3000, IC2, SPI
// ** Requires connection to Blynk service over WiFi **
// Author: Traci Ruthkoski
// Last Modified: 6/6/15

#define BLYNK_PRINT Serial    // Comment this out to disable prints and save space

// These are the interrupt and control pins for СС3000
#define ADAFRUIT_CC3000_IRQ   3
#define ADAFRUIT_CC3000_VBAT  5
#define ADAFRUIT_CC3000_CS    10

#include <SPI.h>
#include <Adafruit_CC3000.h>
#include <BlynkSimpleCC3000.h>
#include <I2C.h>

// Global Variables
char LIDARLite_ADDRESS = 0x62; // LIDAR-Lite I2C Address

// state variables
float get_distance = 0;
int smoothing = 10;

// You should get Auth Token in the Blynk App. 
// Go to the Project Settings (nut icon).
char auth[] = "Your Blynk Auth Token goes here";

void setup()
{
 
  Serial.begin(9600); 
  I2c.begin(); // Opens & joins the irc bus as master
  delay(100); // Waits to make sure everything is powered up before sending or receiving data  
  I2c.timeOut(50); // Sets a timeout to ensure no locking up of sketch if I2C communicat
  Blynk.begin(auth, "Your network SSID goes here", "", WLAN_SEC_UNSEC);
   llWriteAndWait(0x00,0x00);
}

// get slider position to determine smoothing
//BLYNK_WRITE(7)
//{
// smoothing = 2;
//}

// button press in app to get distance measurement
BLYNK_WRITE(5)
{
  get_distance = llGetDistance();
// get_distance = llGetDistanceAverage(5);
  // convert from mm to meters
 get_distance = get_distance;
 
 Serial.println("Distance");
 Serial.println(get_distance); 
 
 // show distance value on Blynk app
 Blynk.virtualWrite(6, get_distance);
 //Blynk.virtualWrite(8, gps);
}
 
void loop()
{
    Blynk.run();
 //   if(cc3000) {
  // turn on the wifi LED in app
 // Blynk.virtualWrite(30, millis()/1000); 
//}

}


/* ==========================================================================================================================================
Basic read and write functions for LIDAR-Lite, waits for success message (0 or ACK) before proceeding
=============================================================================================================================================*/

// Write a register and wait until it responds with success
void llWriteAndWait(char myAddress, char myValue){
  uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses     
  while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
    nackack = I2c.write(LIDARLite_ADDRESS,myAddress, myValue); // Write to LIDAR-Lite Address with Value
    delay(2); // Wait 2 ms to prevent overpolling
  }
}

// Read 1-2 bytes from a register and wait until it responds with sucess
byte llReadAndWait(char myAddress, int numOfBytes, byte arrayToSave[2]){
  uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses     
  while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
    nackack = I2c.read(LIDARLite_ADDRESS,myAddress, numOfBytes, arrayToSave); // Read 1-2 Bytes from LIDAR-Lite Address and store in array
    delay(2); // Wait 2 ms to prevent overpolling
  }
  return arrayToSave[2]; // Return array for use in other functions
}

/* ==========================================================================================================================================
Get 2-byte distance from sensor and combine into single 16-bit int
=============================================================================================================================================*/

int llGetDistance(){
  llWriteAndWait(0x00,0x04); // Write 0x04 to register 0x00 to start getting distance readings
  byte myArray[2]; // array to store bytes from read function
  llReadAndWait(0x8f,2,myArray); // Read 2 bytes from 0x8f
  int distance = (myArray[0] << 8) + myArray[1];  // Shift high byte [0] 8 to the left and add low byte [1] to create 16-bit int
  return(distance);
}

/* ==========================================================================================================================================
Average readings from velocity and distance
int numberOfReadings - the number of readings you want to average (0-9 are possible, 2-9 are reccomended)
=============================================================================================================================================*/

int llGetDistanceAverage(int numberOfReadings){ 
  if(numberOfReadings < 2){
    numberOfReadings = 2; // If the number of readings to be taken is less than 2, default to 2 readings
  }
  int sum = 0; // Variable to store sum
  for(int i = 0; i < numberOfReadings; i++){ 
      sum = sum + llGetDistance(); // Add up all of the readings
  }
  sum = sum/numberOfReadings; // Divide the total by the number of readings to get the average
  return(sum);
}