Pic is LIDAR into corner betwen 5-7m away.
Code: Select all
#include <TFT.h>
#include <SPI.h>
#include <TFMini.h>
#include <Servo.h>
//Teensy 3.2 pin definitions:
#define DC 9
#define CS 10
#define MOSI 11
#define RST 12
#define SCK 13
#define SERVO_PIN 23 // servo control signal
#define PINAUDIO1 14 // scanner start sound
#define PINAUDIO2 15 // scanner cycle default sound
#define PINAUDIO3 16 // scanner object >0m & <1m
#define PINAUDIO4 17 // scanner object >1m & <2m
#define PINAUDIO5 18 // scanner object >2m & <3m
#define PINAUDIO6 19 // scanner object >3m & <4m
#define PINAUDIO7 20 // scanner object >4m & <5m
#define PINAUDIO8 21 // scanner object >5m & <6m
#define BLACK 0x0000 // 16 bit color choices
#define RED 0x001F
#define CYAN 0x07FF
#define GREEN 0x0400
#define MAGENTA 0xF81F
#define BLUE 0xF800
#define WHITE 0xFFFF
#define YELLOW 0xFFE0
const int r_beam = 100; // pixel length of green scan radius
const int r2 = (r_beam * .33);
const int r3 = (r_beam * .66);
const int rMin = 0;
const int rMax = 600;
TFT tft = TFT( CS, DC, RST );
Servo servo;
TFMini tfmini;
// distance char array to print to the screen
char rc_Printout[ 5 ];
int interval = 0;
double distance = 0;
int r_beam1 = 10;
void setup( )
{
servo.attach( SERVO_PIN );
Serial.begin(115200);
// set up audio
pinMode(PINAUDIO1, OUTPUT);
pinMode(PINAUDIO2, OUTPUT);
pinMode(PINAUDIO3, OUTPUT);
pinMode(PINAUDIO4, OUTPUT);
pinMode(PINAUDIO5, OUTPUT);
pinMode(PINAUDIO6, OUTPUT);
pinMode(PINAUDIO7, OUTPUT);
pinMode(PINAUDIO8, OUTPUT);
//start the TFMini
Serial1.begin(TFMINI_BAUDRATE);
tfmini.begin(&Serial1);
delay(100);
tfmini.setSingleScanMode();
//clear the TFT
tft.begin( );
tft.background( BLACK );
delay(200);
digitalWrite(PINAUDIO1, HIGH);
delay(100);
digitalWrite(PINAUDIO1, LOW);
//cool start up
for ( int i = 0; i < 3; i ++ )
{
tft.setTextSize( 2 );
tft.stroke( RED );
tft.text( "WARNING", 40, 20 );
tft.text( "INITIALIZING", 10, 40 );
tft.text( "LIDAR", 50, 60 );
tft.stroke( WHITE );
tft.text( "MAX RANGE 10m", 4, 90 );
tft.stroke( GREEN );
tft.rect( 2, 2, 158, 124 ) ;
delay(500);
tft.background( BLACK );
}
tft.setTextSize( 2 );
tft.stroke( RED );
tft.text( "RANGE", 10, 1 );
for ( int i = 0; i < 3; i ++)
{
for ( int i = 0 ; i < 10 ; i++) {
tft.stroke (BLUE);
tft.fill (BLACK);
tft.circle(80, 117,(r_beam1 * i));
delay(10);
}
tft.stroke( RED );
tft.setTextSize( 1 );
tft.text( "10m", 3, 38 );
}
// here comes my cool pulse LIDAR ring effect
for ( int i = 0; i < 3; i ++) {
for ( int i = 0 ; i < 10 ; i++) {
tft.stroke (BLUE);
tft.fill (BLACK);
tft.circle(80, 117, ((r_beam1 * .66) * i));
delay(15);
}
tft.stroke( RED );
tft.setTextSize( 1 );
tft.text( "6.5m", 15, 78 );
}
for ( int i = 0; i < 3; i ++) {
for ( int i = 0 ; i < 10 ; i++) {
tft.stroke (BLUE);
tft.fill (BLACK);
tft.circle(80, 117, ((r_beam1 * .33) * i));
delay(30);
}
tft.stroke( RED );
tft.setTextSize( 1 );
tft.text( "3.3m", 30, 120 );
}
tft.background( BLACK );
}
void loop( )
{
tft.stroke( BLUE );
tft.circle( 80, 128, r_beam );
tft.circle ( 80, 128, r_beam - r2 );
tft.circle ( 80, 128, r_beam - r3);
tft.setTextSize( 2 );
tft.stroke( RED );
tft.text( "RANGE", 10, 1 );
tft.setTextSize( 1 );
tft.stroke( RED );
tft.text( "10m", 3, 38 );
tft.setTextSize( 1 );
tft.text( "6.5m", 15, 78 );
tft.setTextSize( 1 );
tft.text( "3.3m", 30, 120 );
//Left display/servo rotation
for ( int i = 60; i < 120; i = i + 1 )
{ tfmini.externalTrigger();
uint16_t distance = tfmini.getDistance();
servo.write( i );
delay( 30 ); //any longer and scans a bit slow, any less and too fast for clean display/sound activation
//This is for 10m display range
int r = distance * 0.1;
String r_Printout = String( r * 0.1 );
if ( r <= 100 ) {
tft.stroke( 0, 0, 0 );
tft.setTextSize( 2 );
tft.text( rc_Printout, 100, 0 );
r_Printout.toCharArray( rc_Printout, 5 );
tft.stroke( WHITE );
tft.setTextSize( 2 );
tft.text( rc_Printout, 100, 0 );
}
//Sound control of FXMini
//I am sure this can be done in an array, but this is very fast
int range = map(distance, rMin, rMax, 0, 5);
switch (range) {
case 0:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO3, HIGH);
Serial.println(PINAUDIO3);
break;
case 1:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO4, HIGH);
Serial.println(PINAUDIO4);
break;
case 2:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO5, HIGH);
Serial.println(PINAUDIO5);
break;
case 3:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO6, HIGH);
Serial.println(PINAUDIO6);
break;
case 4:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO7, HIGH);
Serial.println(PINAUDIO7);
break;
case 5:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, HIGH);
Serial.println(PINAUDIO8);
break;
default:
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO2, HIGH);
Serial.println(PINAUDIO2);
}
tft.stroke( GREEN );
tft.line( 80, 128, 80 + r_beam * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r_beam * sin( ( 360 - i ) * 3.14 / 180 ) );
if ( r > 0 && r <= r_beam + 2 )
{
tft.circle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 );
tft.fillCircle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 , RED );
if (r < r2) {
tft.fillCircle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 , WHITE );
}
}
}
tft.background( BLACK );
tft.stroke( BLUE );
tft.circle( 80, 128, r_beam );
tft.circle ( 80, 128, r_beam - r2 );
tft.circle ( 80, 128, r_beam - r3);
tft.setTextSize( 2 );
tft.stroke( RED );
tft.text( "RANGE", 10, 1 );
tft.setTextSize( 1 );
tft.stroke( RED );
tft.text( "10m", 3, 38 );
tft.setTextSize( 1 );
tft.text( "6.5m", 15, 78 );
tft.setTextSize( 1 );
tft.text( "3.3m", 30, 120 );
//Right display/servo rotation
for ( int i = 120; i > 60; i = i - 1 )
{
tfmini.externalTrigger();
uint16_t distance = tfmini.getDistance();
servo.write( i );
delay( 30 );
int r = distance * 0.1;
String r_Printout = String( r * 0.1 );
if ( r <= 100 ) {
tft.stroke( 0, 0, 0 );
tft.setTextSize( 2 );
tft.text( rc_Printout, 100, 0 );
r_Printout.toCharArray( rc_Printout, 5 );
tft.stroke( WHITE );
tft.setTextSize( 2 );
tft.text( rc_Printout, 100, 0 );
}
int range = map(distance, rMin, rMax, 0, 5);
switch (range) {
case 0:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO3, HIGH);
Serial.println(PINAUDIO3);
break;
case 1:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO4, HIGH);
Serial.println(PINAUDIO4);
break;
case 2:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO5, HIGH);
Serial.println(PINAUDIO5);
break;
case 3:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO6, HIGH);
Serial.println(PINAUDIO6);
break;
case 4:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO7, HIGH);
Serial.println(PINAUDIO7);
break;
case 5:
digitalWrite(PINAUDIO2, LOW);
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, HIGH);
Serial.println(PINAUDIO8);
break;
default:
digitalWrite(PINAUDIO3, LOW);
digitalWrite(PINAUDIO4, LOW);
digitalWrite(PINAUDIO5, LOW);
digitalWrite(PINAUDIO6, LOW);
digitalWrite(PINAUDIO7, LOW);
digitalWrite(PINAUDIO8, LOW);
digitalWrite(PINAUDIO2, HIGH);
Serial.println(PINAUDIO2);
}
tft.stroke( GREEN );
tft.line( 80, 128, 80 + r_beam * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r_beam * sin( ( 360 - i ) * 3.14 / 180 ) );
if ( r > 0 && r <= r_beam + 2 )
{
tft.circle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 );
tft.fillCircle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 , RED );
if (r < r2) {
tft.fillCircle( 80 + r * cos( ( 360 - i ) * 3.14 / 180 ), 128 + r * sin( ( 360 - i ) * 3.14 / 180 ), 2 , WHITE );
}
}
}
tft.background( BLACK );
}
uint16_t color565( uint8_t r, uint8_t g, uint8_t b )
{
return ( ( r & 0xF8 ) << 8 ) | ( ( g & 0xFC ) << 3 ) | ( b >> 3 );
}