// *** Lidar + Servo
#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X LidarVL53 = Adafruit_VL53L0X();
int distant = 0;  // *** Dis. Lidar MM.

#include <Servo.h>
Servo myservo;          // *** control servo
#define servoPin 9      // *** pin 9 MCU.
int servoSet_L = 800;   // *** test set servo position 0    
int servoSet_H = 1200;  // *** test set servo position 180  
int pos        = 0;     // *** set servo position
int posStep    = 3;     // *** test 1-10 (2)

void setup() {
  Serial.begin(115200);
   //while (!Serial) { ; } // *** wait for serial port to connect. 
  Serial.println("");
  Serial.println("Serial port to connect"); 
  
   //while (Serial.available() <= 0) { // *** >=0
     //Serial.println("Waiting for input...");
     //delay(500);
     //}
     
  LidarVL53.begin();
  LidarVL53.startRangeContinuous();
  if (LidarVL53.isRangeComplete()); { 
    Serial.println("VL53LOX Range Complete"); 
    }  
  
  pinMode(servoPin, OUTPUT); // pin 9 
  myservo.attach(servoPin, servoSet_L, servoSet_H); // *** set servo
  myservo.attach(servoPin); // *** set servo
  delay(1000);
}

void loop() {
  
  // *** CW.
  for (pos = 0; pos <= 180; pos += posStep) {   
    myservo.write(pos);              
    distant = LidarVL53.readRange(); delay(1);
    distant = distant/10; 
      if (distant > 80) { distant = 80; }                                   
    Serial.print(pos);
    Serial.print(",");
    Serial.print(distant);
    Serial.print("."); 
       //Serial.println("."); 
    delay(1);
  }

  // *** CCW.
  for (pos = 180; pos >= 0; pos -= posStep) {  
    myservo.write(pos);              
    distant = LidarVL53.readRange(); delay(1);
    distant = distant/10; 
      if (distant > 80) { distant = 80; }                                   
    Serial.print(pos);
    Serial.print(",");
    Serial.print(distant);
    Serial.print("."); 
       //Serial.println("."); 
    delay(1);                     
  }

} // *** END loop Lidar + Servo ***