I'm working on a project where we use 2 mg996 servo and tfmini plus lidar to map a room. But when we wrote a code the vertical servo is shaking. When lidar is not connected there is no shaking. I heard that the shaking is because the vertical servo is idle for a long time while collecting the lidar value and can be avoided by providing the angle value as a 20ms period pwm to the vertical servo(Not sure if that's what they meant).But i dont understand how to create a pwm such that there is no stopping in the code execution in collecting value from the lidar. Here's the code snippet of both servo writing and lidar reading. Can someone tell me what to do.
Thanks in advance.
void loop() {
for(theta=0 ; theta<=180; theta++){
analogWrite(SERVO_theta,
delay(100);
if(theta%2==0){
for(phi=1;phi<=180;phi++){
servoX.write(phi);
delay(12);//delay for each servo angle
reading();//function to obtain the lidar value
}
delay(100);
}
else{
for(phi=180; phi>0;phi--){
servoX.write(phi);
delay(12);
reading();
}
delay(100);
}
}
}
void reading()
{
if (Serial1.available()) { //check if serial port has data input
if(Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[0]=HEADER;
if (Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[1] = HEADER;
for (i = 2; i < 9; i++) { //save data in array
uart[i] = Serial1.read();
}
check = uart[0] + uart[1] + uart[2] + uart[3] + uart[4] + uart[5] + uart[6] + uart[7];
if (uart[8] == (check & 0xff)){ //verify the received data as per protocol
dist = uart[2] + uart[3] * 256; //calculate distance value
//output measure distance value of LiDAR
x=dist*cos(theta* PI / 180.0)*cos(phi* PI / 180.0);//converting spherical to cartesian
y=dist*cos(theta* PI / 180.0)*sin(phi* PI / 180.0);
z=dist*sin(theta* PI / 180.0);
printOutput(x, y, z);
}
}
}
}
}
10 posts - 4 participants