Servo Error with Timer1 on LIDAR project

Hi Everyone!

I’ve been working on recreating the rotating Lidar system from this youtube video. Whenever I run the program, the motor spins around twice as its supposed to, but rather than the servo ticking down once, it keeps ticking with each tick about 0.4 seconds apart. I have an if then function controlling the servo write as well as the written height value. The odd part is that the serial monitor displays the correct servo angle, but the servo itself is spazzing out. I’ve attached it to the pin 13 led to confirm that it is in fact not a servo malfunction. Any suggestions are highly appreciated. Here’s the code

#include <SoftwareServo.h>
#include <TimerOne.h>
#include <Wire.h>
#include <LIDARLite.h>

#define DIR_PIN 2
#define STEP_PIN 3

LIDARLite myLidarLite;
SoftwareServo servo;

volatile int stepCount = 0;
volatile int roundCount = 0;

int height = 18;

void setup() {
Serial.begin(115200);

myLidarLite.begin(0, true);
myLidarLite.configure(0);

servo.attach(13);
servo.write(height);

pinMode(DIR_PIN, OUTPUT);
pinMode(STEP_PIN, OUTPUT);

digitalWrite(DIR_PIN, HIGH);

Timer1.initialize();
Timer1.attachInterrupt(tick, 1000000.0f / (3200 * 1));
}

void loop() {
receive(true);

for(int i = 0; i < 99; i++)
receive(false);
}

void receive(bool bias)
{
float angle = ((float)stepCount / 3200.0f) * 360.0f;
Serial.print(108 - height);
Serial.print(",");
Serial.print(angle);
Serial.print(",");
Serial.println(myLidarLite.distance(bias));

if(roundCount >= 2)
{
height++;

if(height >= 140)
height = 18;

servo.write(height);
roundCount = 0;
}

servo.refresh();
}

void tick()
{
digitalWrite(STEP_PIN, HIGH);
delay(1);
digitalWrite(STEP_PIN, LOW);

stepCount++;
if(stepCount >= 3200)
{
stepCount = 0;
roundCount++;
}
}

Your tick() function gets called from the Interrupt Service Routine which means interrupts are disabled which means delay() will not work (interrupts are used to advance elapsed time). I'm surprised your code is working at all. I would expect the delay() function to block forever.

Also, why are you pulsing your servo in the tick() function. That is what servo.refresh() is doing...

Hi,
Welcome to the forum.

Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html . Then look down to item #7 about how to post your code.
It will be formatted in a scrolling window that makes it easier to read.

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png?

Thanks.. Tom.. :slight_smile: