Arduino sample Radar with VL53L0X Time of Flight Distance Sensor





Arduino Sketch


#include "U8glib.h"
#include <Servo.h>
#include <Wire.h>
#include <VL53L0X.h>


VL53L0X sensor;

#define start_degree 15
#define limit_degree 165

U8GLIB_ST7920_128X64_1X u8g(13,11,10, U8G_PIN_NONE);

int arr[152]={0};

Servo myServo;

int degree = start_degree;
int incDegree = 1;

void draw(void) {
 
 float angle = float(degree +90) / 57.29577951  ; // radians
int length = 63 ;
int x0 = 64;
int y0 = 64;
int x1 = x0 + sin(angle) * length;
int y1 = y0 + cos(angle) * length;

 
  // graphic commands to redraw the complete screen should be placed here 
  u8g.setFont(u8g_font_unifont);
  //u8g.setFont(u8g_font_osb21);
 // u8g.drawStr( 0, 22, degree);
  u8g.drawLine(x0, y0, x1, y1);


  drawLineAngle(15);
  drawLineAngle(165);
 
  u8g.drawCircle(64,64,63,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

   u8g.drawCircle(64,64,50,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

   u8g.drawCircle(64,64,40,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

  u8g.drawCircle(64,64,30,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

  u8g.drawCircle(64,64,20,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

  u8g.drawCircle(64,64,10,U8G_DRAW_UPPER_LEFT|U8G_DRAW_UPPER_RIGHT);

  fillTarget();

}

void setup(void) {

 

   Serial.begin(9600);

   Wire.begin();

  sensor.init();
  sensor.setTimeout(500);
 
  // flip screen, if required
  // u8g.setRot180();
 
  // set SPI backup if required
  //u8g.setHardwareBackup(u8g_backup_avr_spi);

  // assign default color value
  if ( u8g.getMode() == U8G_MODE_R3G3B2 ) {
    u8g.setColorIndex(255);     // white
  }
  else if ( u8g.getMode() == U8G_MODE_GRAY2BIT ) {
    u8g.setColorIndex(3);         // max intensity
  }
  else if ( u8g.getMode() == U8G_MODE_BW ) {
    u8g.setColorIndex(1);         // pixel on
  }
  else if ( u8g.getMode() == U8G_MODE_HICOLOR ) {
    u8g.setHiColorByRGB(255,255,255);
  }
 
  pinMode(8, OUTPUT);

   myServo.attach(7);
 
  myServo.write(degree);
  delay(5000);

  sensor.startContinuous();
}

void loop(void) {

  long distance;
  int  n = degree - (start_degree -1);
 
  turnServo();

   distance = sensor.readRangeContinuousMillimeters();


   arr[n] = distance;
   
  if (sensor.timeoutOccurred()) {Serial.print(" TIMEOUT");
      delay(5000);
  }

 // Serial.println(distance);
 
  // picture loop
  u8g.firstPage(); 
  do {
    draw();
  } while( u8g.nextPage() );
 
  // rebuild the picture after some delay
  //delay(50);
}
void turnServo(){

  // Serial.println( degree - (start_degree -1) );
   myServo.write( degree );
      delay(50);
      degree+= incDegree;
      if( degree > limit_degree ){
        incDegree = -1;
      }
      else if( degree < start_degree ){
        incDegree = 1;
      }
}
void drawLineAngle( float  angle ){
   angle = (angle +90) / 57.29577951  ; // radians
int length = 63 ;
int x0 = 64;
int y0 = 64;
int x1 = x0 + sin(angle) * length;
int y1 = y0 + cos(angle) * length;

 
  // graphic commands to redraw the complete screen should be placed here 


  u8g.drawLine(x0, y0, x1, y1);
}
void fillTarget(){


   for( int i =0 ; i<151;i++){
        if(  (arr[i] != 0) && (arr[i] < 400) ){
           float angle = ((i+15) +90) / 57.29577951  ; // radians
int length = arr[i]/10 ;
int x0 = 64;
int y0 = 64;
int x1 = x0 + sin(angle) * length;
int y1 = y0 + cos(angle) * length;

  u8g.drawDisc(x1,y1,1);
        }
   }
}


ความคิดเห็น