Hey, so im trying to connect an rplidar which would directly control a servo. Whereever the rplidar senses the greatest distance the servo will direct its wheels towards. Im using the code attached below but I've run into the issue of the servo being inconsistent. Sometimes it'll work and do the desired task, other times is will spin without any seeming control, and others it will not turn at all though power is clearly being provided. Any thoughts?
#include <RPLidar.h>
RPLidar lidar;
#define RPLIDAR_MOTOR 3
const int READINGS = 180;
int i;
int maxval = 0;
int wantangle;
int degreeArray[180];
int distanceArray[180];
const int servopin = 7;
void setup() {
lidar.begin(Serial1);
pinMode(RPLIDAR_MOTOR, OUTPUT);
Serial.begin(115200);
pinMode(servopin, OUTPUT);
}
int getreadings() {
if (IS_OK(lidar.waitPoint())){
for (i = 0; i < READINGS; i++) {
if (IS_OK(lidar.waitPoint())) {
int distance = lidar.getCurrentPoint().distance;
int angle = lidar.getCurrentPoint().angle;
bool startBit = lidar.getCurrentPoint().startBit;
byte quality = lidar.getCurrentPoint().quality;
if (angle <= 180 && angle >= 0 && distance > maxval) {
maxval = distance;
wantangle = maxval;
}
else if (angle > 180 && angle != 0) {
i = i -1;
}
}
}
Serial.print(wantangle);
Serial.print(" degrees");
return wantangle;
} else {
analogWrite(RPLIDAR_MOTOR, 0); // stop the rplidar motor
// try to detect RPLIDAR...
rplidar_response_device_info_t info;
if (IS_OK(lidar.getDeviceInfo(info, 100))) {
// detected...
lidar.startScan();
// start motor rotating at max allowed speed
analogWrite(RPLIDAR_MOTOR, 255);
delay(500);
}
}
}
void sendPWM(int wantangle) {
float pwmValue = map(wantangle, 180, 0, 1172, 1900);
digitalWrite(servopin, HIGH);
delayMicroseconds(pwmValue);
digitalWrite(servopin, LOW);
}
void loop() {
getreadings();
sendPWM(wantangle);
maxval = 0;
delay(200);
}