#include <ESP32Servo.h>

 

static const int servoPinRW = 5; // on Pin5, Right Wheel Servo

static const int servoPinLW = 6; // on Pin6, Left Wheel Servo

static const int servoPinLidar = 7; // on Pin7, Lidar Angle Servo

 

Servo servoRW;

Servo servoLW;

Servo servoLidar;

 

void setup() {

  Serial.begin(115200);

 

  servoRW.attach(servoPinRW);

  servoLW.attach(servoPinLW);

  servoLidar.attach(servoPinLidar);

}

 

void loop() {

  // Test Right Wheel Servo for continuous rotation (forward)

  Serial.println("Testing Right Wheel Servo: Forward");

  servoRW.write(180); // Corresponds to full forward on most continuous servos

  delay(2000); // Run for 2 seconds

 

  // Stop the Right Wheel Servo

  Serial.println("Stopping Right Wheel Servo");

  servoRW.write(90); // Corresponds to stop on most continuous servos

  delay(1000);

 

  // Test Left Wheel Servo for continuous rotation (forward)

  Serial.println("Testing Left Wheel Servo: Forward");

  servoLW.write(0); // Corresponds to full forward on most continuous servos

  delay(2000); // Run for 2 seconds

 

  // Stop the Left Wheel Servo

  Serial.println("Stopping Left Wheel Servo");

  servoLW.write(90); // Corresponds to stop on most continuous servos

  delay(1000);

 

  // Servo for Lidar Tilt Angle for Pseudo 3D Scan

  // This servo should only move a total of 45 degrees, centered on 90.

  // The range will be from (90 - 22.5) to (90 + 22.5), which is 67.5 to 112.5.

  // We'll use integer values, so from 68 to 112.

  Serial.println("Testing Lidar Servo");

  for(int posDegrees = 68; posDegrees <= 112; posDegrees++) {

    servoLidar.write(posDegrees);

    delay(30);

  }

  delay(1000);

 

  // Move back to the starting point

  for(int posDegrees = 112; posDegrees >= 68; posDegrees--) {

    servoLidar.write(posDegrees);

    delay(30);

  }

  

  // Return to the center position (90 degrees) and hold

  servoLidar.write(90);

  delay(5000);

}