#include <i2c_t3.h>
#include <LIDARLite.h>
//#include <wire.h>
#include <LiquidCrystal.h> //C:\Users\John\Downloads\arduino-1.8.3-windows\arduino-1.8.3\libraries\LiquidCrystal\src\LiquidCrystal.h
#include <Adafruit_MCP23017.h> //For I2C to register specific for MCP23017
#include "sprites_jwb.h"
#include "pitches.h"
// Garmin LIDAR constants (register addresses)
#define PASS (1)
#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define LIDAR_SN_HighByte 0x16 // Fetch this for unlocking
#define LIDAR_SN_LowByte 0x16 // Fetch this for unlocking
#define LIDAR_Unlock_I2C_Addr_High 0x18 // Write SN high byte to unlock.
#define LIDAR_Unlock_I2C_Addr_Low 0x19 // Write SN low byte to unlock.
#define LIDAR_New_I2C_Addr 0x1a // Where to place new I2C addr after 'unlock'.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.
#define PIN_Speaker_Out (2)
#define Pin_Lidar1_Enab (3) // low disables Lidar
#define trigPin (5) // For uSound
#define echoPin (6) // For uSound
#define PIN_Teensy_Led (13) // on-board LED
#define analogInPin (15) // A-D converter for potentiometer
#define analog_2_InPin (23) // A-D converter for thermistor
#define LIDAR_DELAY (50)
#define LIDAR_SHORT_DELAY (10)
long duration;
float uS_distance, Laser_distance;
Adafruit_MCP23017 mcp;
LiquidCrystal lcd(14,16,17,12,11,20);
elapsedMillis sincePrint;
static int bDistanceMode = 0, SpeakerON_OFF = 0;
static unsigned long T1, T2;
void setup()
{
// Set up on-baord LED.....
pinMode(PIN_Teensy_Led,OUTPUT); digitalWrite(PIN_Teensy_Led, HIGH);
//Wire.begin(); // join i2c bus
Wire.begin(I2C_MASTER, 0, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_100, I2C_OP_MODE_DMA );
pinMode(Pin_Lidar1_Enab, OUTPUT);
digitalWrite(Pin_Lidar1_Enab, LOW); // LOW Disables Lidar1 for now
mcp.begin(); // use default address 0
// init switch and LED on port A side of MCP
mcp.pinMode(7, OUTPUT); // An LED at MCP23017 pin 28, Port A, bit 7
mcp.pinMode(6, INPUT); mcp.pullUp(6, HIGH); // A switch at MCP23017 pin 3, Port B, bit 2
// init switch and LED on port B side of MCP
mcp.pinMode(8, OUTPUT); // An LED at MCP23017 pin 1, Port B, bit 0
mcp.pinMode(9, INPUT); mcp.pullUp(9, HIGH); // A switch at MCP23017 pin 2, Port B, bit 1
//Start with LED's off......
mcp.digitalWrite(8, LOW); mcp.digitalWrite(7, LOW);
Serial.begin(115200); //while(!Serial){ Serial.print("Serial good"); }
// Get LCD going......
lcd.begin(20, 4); lcd.clear(); lcd.print("Push buttons");
for ( int n=0; n<=7; n++){ // make the 8 sprites supported by the LCD for bar graph.
lcd.createChar(n, lcd_sprite[n]);
}
//uSound pins;
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
digitalWrite(Pin_Lidar1_Enab, HIGH);
delay(100); // give the lidar 100mS to startup
// while(PASS != Move_Lidar_1_Address()){ // PASS/FAIL
// lcd.printf("I2C Addr FAILED");
// }
digitalWrite(PIN_Teensy_Led, LOW);
}
void loop(){
int out ;
float voltage, V_Thermistor;
int FrequencyOffset = 5000, n;
static int AvgADreading, VoltReadings[25], AccumVoltReads;
ServiceUserSwitches();
/* Now that we have read the user switches, do what the program can do.
*
*/
if (bDistanceMode != 0){ // Pulse and read the ultrasound xducer/
// First, get all the data......will print later.
// Get the ultrasound distance.......
digitalWrite(trigPin, LOW); delayMicroseconds(2); //Make sure Xducer trigger is low long enough to recognize the next pulse.
digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); // Sets the trigPin on HIGH state for 10 micro seconds
duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
uS_distance= (float)(duration*0.034/2)/100; // Convert to meters from uS.
// Then get the laser distance.....
Laser_distance = (float)readDistance()/100; //convert cm to meters.
// lcd.setCursor(0,0); lcd.printf("T-return:%6d", duration); lcd.setCursor(18,0); lcd.printf("uS");
//lcd.setCursor(0,1); lcd.printf("%5.3f", uS_distance);
//lcd.setCursor(9,1); lcd.printf("%5.3f", Laser_distance);
//lcd.setCursor(19,1); lcd.printf("m");
// (get rid of this crap.....)
// 'smooth out that noisy pot/A-D by taking a running average of 25 readings.
AccumVoltReads = 0;
for (n=0; n<=23; n++){
AccumVoltReads += VoltReadings[n]; // might as well accumulate the voltage for an average while we're stepping through.
VoltReadings[n] = VoltReadings[n+1]; // shift the past 23 readings down by one.
}
VoltReadings[24] = analogRead(analog_2_InPin);
AccumVoltReads += VoltReadings[24]; // add on that last new reading.
AvgADreading = AccumVoltReads / 25; // Get the average over the last 25.
FrequencyOffset = AvgADreading*20; // 0-20460 Hz range
lcd.setCursor(0,0);
switch (SpeakerON_OFF){
case 0: lcd.printf("Speaker OFF"); noTone(PIN_Speaker_Out); break;
case 1: lcd.printf("Speaker uSound Mode");
tone(PIN_Speaker_Out, FrequencyOffset - (float)uS_distance * 1000 ); break;
case 2: lcd.printf("Speaker LIDAR Mode");
tone(PIN_Speaker_Out, FrequencyOffset - (float)Laser_distance * 1000); break;
default: SpeakerON_OFF = 0;
lcd.printf("Speaker OFF"); noTone(PIN_Speaker_Out); break;
}
PrintBar(1, 'm', 5, uS_distance);
PrintBar(2, 'm', 5, Laser_distance);
if(SpeakerON_OFF == 2){ tone(PIN_Speaker_Out, FrequencyOffset - (float)Laser_distance * 1000);}
lcd.setCursor(0,3); lcd.printf("dT= uS");
lcd.setCursor(3,3); lcd.printf("%4u", T2-T1);
lcd.setCursor(10,3); lcd.printf(" Fo= Hz");
lcd.setCursor(14,3); lcd.printf("%5u", FrequencyOffset);
//lcd.setCursor(0,3); lcd.printf("A2D:%4d Foff:%5d", VoltReadings[24], AvgADreading*10); // write to the LCD
//lcd.setCursor(16,3); lcd.printf(" m");
}else{ //This is the 'regular' mode which just displays the thermistor output and the voltage of the potentiometer
noTone(PIN_Speaker_Out);
V_Thermistor = (float)(analogRead(analogInPin) * 5.0 / 1023.0); //Read Thermistor
voltage = (float)(analogRead(analog_2_InPin) * 5.0 / 1023.0); //Read Potentiometer
PrintBar(2, 'V', 5.0, voltage);
PrintBar(3, 'T', 1.5, V_Thermistor);
}
/* Laser data to serial monitor (keep for debugging?)
Serial.print(sincePrint);
sincePrint = 0;
Serial.print(">");
Serial.println(out = readDistance());
if (out == 0){
Serial.println("Getting sick");
} */
}
/*
int Move_Lidar_1_Address(){
LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
RegisterMeasure 0x00 // Register to write to initiate ranging.
LIDAR_SN_HighByte 0x16 // Fetch this for unlocking
LIDAR_SN_LowByte 0x16 // Fetch this for unlocking
LIDAR_Unlock_I2C_Addr_High 0x18 // Write SN high byte to unlock.
LIDAR_Unlock_I2C_Addr_Low 0x19 // Write SN low byte to unlock.
LIDAR_New_I2C_Addr 0x1a // Where to place new I2C addr after 'unlock'.
*/
/**********************************
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
T1 = micros();
lcd.println("Reassigning.");
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
// Wait 1 ms to prevent overpolling
Wire.beginTransmission(LIDARLite_ADDRESS);
Wire.read(RegisterMeasure);
Wire.write(MeasureValue);
nackack = Wire.endTransmission(I2C_STOP,1000);
if (nackack!=0){
delayMicroseconds(LIDAR_SHORT_DELAY);
}
}
nackack = 100;
while (nackack != 0){
Wire.beginTransmission(LIDARLite_ADDRESS);
Wire.write(RegisterHighLowB); // sets register pointer to (0x8f)
nackack = Wire.endTransmission(I2C_STOP,1000);
if (nackack!=0){
delayMicroseconds(LIDAR_SHORT_DELAY);
}
Wire.requestFrom(LIDARLite_ADDRESS,2,I2C_STOP,1000 );
//delayMicroseconds(LIDAR_DELAY);
}
int reading = Wire.readByte() ;
reading = reading << 8;
reading |= Wire.readByte();
T2 = micros();
delayMicroseconds(100);
return reading;
*************************************/
//}
int readDistance(){
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
T1 = micros();
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
// Wait 1 ms to prevent overpolling
Wire.beginTransmission(LIDARLite_ADDRESS);
Wire.write(RegisterMeasure);
Wire.write(MeasureValue);
nackack = Wire.endTransmission(I2C_STOP,1000);
if (nackack!=0){
delayMicroseconds(LIDAR_SHORT_DELAY);
}
}
nackack = 100;
while (nackack != 0){
Wire.beginTransmission(LIDARLite_ADDRESS);
Wire.write(RegisterHighLowB); // sets register pointer to (0x8f)
nackack = Wire.endTransmission(I2C_STOP,1000);
if (nackack!=0){
delayMicroseconds(LIDAR_SHORT_DELAY);
}
Wire.requestFrom(LIDARLite_ADDRESS,2,I2C_STOP,1000 );
//delayMicroseconds(LIDAR_DELAY);
}
int reading = Wire.readByte() ;
reading = reading << 8;
reading |= Wire.readByte();
T2 = micros();
delayMicroseconds(100);
return reading;
}
/* Routing to print a bar graph representation of a floating point value.
* arguments are:
* int Bar_LCD_Row -- which area of the LCD for the bar graph. 0-3.
* char Unit -- a sincle charachter which is a unit of measure. S = seconds, V = volts, etc.
* float FullscaleValue -- The maximum expected range.
* float ValueToPlot
*/
void PrintBar(int Bar_LCD_Row, char Unit, float FullscaleValue, float ValueToPlot){
int BarsNeeded,CellsNeeded, n, remainder;
lcd.setCursor(0,Bar_LCD_Row); lcd.print(" "); //Clear plot area
lcd.setCursor(0,Bar_LCD_Row); lcd.printf("%3.2f%c",ValueToPlot,Unit); //Value and unit charachter
BarsNeeded = (ValueToPlot*15*5/FullscaleValue); //Plot over 16 cells on LCD. 5 bars per cell.
CellsNeeded = (BarsNeeded/5);
remainder = BarsNeeded - (CellsNeeded * 5);
if (BarsNeeded <= 75){
for (n=0; n<CellsNeeded; n++){ //if(CellsNeeded > 0)
lcd.write(byte(4)); //sprite for a cell with all 5 bars on
}
if(remainder>0 && remainder<=5)lcd.write(byte(remainder-1)); // plot leftover bars
}
}
/* This silly routine just plays 'shave and a haircut'.
* But......right now it also toggles the programs two main modes!
*/
void PlayTheTune(int scale){
if (scale >= 2) scale = 2;
for (int thisNote = 0; thisNote < 8; thisNote++) { // to calculate the note duration, take one second divided by the note type. e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
int noteDuration = 750 / noteDurations[thisNote];
if(melody[scale*8+thisNote] > 1){ // a 0 in the array of frequencies (tones, or notes) means no note to be played, a 'rest' in music.
tone(PIN_Speaker_Out, melody[scale*8+thisNote], noteDuration); // to distinguish the notes, set a minimum time between them. // the note's duration + 30% seems to work well:
}
int pauseBetweenNotes = 1.5* noteDuration;
if(melody[scale*8+thisNote] > 1){digitalWrite(PIN_Teensy_Led, HIGH);
} delay(pauseBetweenNotes/2);
digitalWrite(PIN_Teensy_Led, LOW); delay(pauseBetweenNotes/2);
noTone(PIN_Speaker_Out); // stop the tone playing:
} //...... END of 'Shave and a Haircut'
}
void ServiceUserSwitches(){ // Check MCP for user input
if(mcp.digitalRead(9) == 0){ //This is the 'mode' switch
mcp.digitalWrite(8, HIGH); // Turn on the LED by the switch
lcd.clear(); //lcd.setCursor(0,0);
if (bDistanceMode != 1){ bDistanceMode = 1;
lcd.printf("Mode: Distance"); // write to the LCD
PlayTheTune(2);
}else{ bDistanceMode = 0;
lcd.printf("Mode: Volt-Temp"); // write to the LCD
PlayTheTune(1);
}
//PlayTheTune(2);
while(mcp.digitalRead(9) == 0){ // First, check port 9, which is on pin 2. // The switch is low active, it is logically 1 when closed (low)
lcd.setCursor(19,3); lcd.write(byte(2)); // Write the confused face sprite.....
}
lcd.clear(); mcp.digitalWrite(8, LOW);
}
// Check the other switch which turns the sound on and off
if(mcp.digitalRead(6) == 0){
lcd.clear(); mcp.digitalWrite(7, HIGH);
SpeakerON_OFF += 1;
if ( SpeakerON_OFF > 2) SpeakerON_OFF = 0;
while(mcp.digitalRead(6) == 0){
lcd.setCursor(19,3); lcd.write(byte(2)); // Write the confused face sprite.....
}
lcd.clear(); mcp.digitalWrite(7, LOW);
}
}
// * [URL]https://os.mbed.com/teams/Impact-Echo/code/LidarLite_Multi-Unit/file/a01dc8b52be4/LidarLite.cpp/[/URL]
/* =============================================================================
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 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);
}
// 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);
}
// 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);
}
char newaddress[2] = {0x1a, newI2cAddress};
nackack=1;
while(nackack !=0)
{
wait_ms(1);
nackack = i2c_.write((currentLidarLiteAddress<<1), newaddress, 2);
}
printf("WIN!");
// Choose whether or not to use the default address of 0x62
if(disablePrimaryAddress){
char disable[2] = {0x1e, 0x08};
nackack=1;
while(nackack !=0)
{
wait_ms(1);
nackack = i2c_.write((currentLidarLiteAddress<<1), disable, 2);
}
}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);
}
}
return;
}
void changeAddressMultiPwrEn(int numOfSensors, char *i2cAddressArray, char currentAddress){
for (int i = 0; i < numOfSensors; i++){
changeAddress(i2cAddressArray[i],true,currentAddress); // We have to turn off the party line to actually get these to load
}
}