Hi,
I'm developing a low cost 3D LiDAR sensor using the Garmin LiDAR lite v3 with two SG 5010 servo motors on a pan and tilt gimble.
Heavily inspired by: Schematics and Arduino Code – Lidar Scanner
I got the scanner working perfecty, the servo motors were controlling the pitch and yaw as per the code and the LiDAR was outputting x,y,z coords.
However, I went to use the scanner today to try and produce a 3D scan that I could process and the servo motors have stopped working.
The y-axis (pitch) servo has stopped working completely and the x-axis servo works sporadically.
Using the same setup, I ran the example sweep sketch for both servos with no prevail.
I then took apart the gimble and used the servo motors individually using the Uno board and it began working again.
Setup: (4 x AA batteries used in lieu of 5v power)
Code:
#include <Servo.h>
#include <I2C.h>
#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.
Servo serNo01;
Servo serNo02;
int pos01 = 0;
int pos02 = 0;
int start = 1;
void setup()
{
Serial.begin(115200);
serNo01.attach(10); //sweep
serNo02.attach(9); //pitch
I2c.begin(); // Opens & joins the irc bus as master
delay(100); // Waits to make sure everything is powered up before sending or receiving data
I2c.timeOut(50); // Sets a timeout to ensure no locking up of sketch if I2C communication fails
}
void loop() {
if( start == 1){
for(pos02 = 0; pos02 < 45; pos02 += 1) // 0 to 45 degrees with 1 degree step
{
serNo02.write(pos02);
delay(200); // time set 200 ms to reach slowly
GetDist();
// 0 to 145
for(pos01 = 0; pos01 < 145; pos01 += 1) // 0 to 145 degrees with 1 degree step
{
serNo01.write(pos01);
delay(10); // time set 100 ms to reach
GetDist();
}
// 145 to 0
for(pos01 = 145; pos01 >= 0; pos01 -= 1) // 0 to 145 degrees with 1 degree step
{
serNo01.write(pos01);
delay(10); // time set 100 ms to reach
// Take a measurement with receiver bias correction and print to serial terminal
GetDist();
}
}
}
}
void GetDist(){
// Write 0x04 to register 0x00
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.write(LIDARLite_ADDRESS,RegisterMeasure, MeasureValue); // Write 0x04 to 0x00
delay(1); // Wait 1 ms to prevent overpolling
}
byte distanceArray[2]; // array to store distance bytes from read function
// Read 2byte distance from register 0x8f
nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.read(LIDARLite_ADDRESS,RegisterHighLowB, 2, distanceArray); // Read 2 Bytes from LIDAR-Lite Address and store in array
delay(1); // Wait 1 ms to prevent overpolling
}
int distance = (distanceArray[0] << 8) + distanceArray[1]; // Shift high byte [0] 8 to the left and add low byte [1] to create 16-bit int
// Print Distance and axis pos
Serial.print(pos01);
Serial.print(",");
Serial.print(pos02);
Serial.print(",");
Serial.println(distance);
}
Also, if I literally push the x-axis servo motor it begins working with the above code - could this be a power issue?
Thanks in advance for the help!