Servo & LED Not Working Properly Together

Projects Programming
2

some time ago I wrote a library to assist in this type of problem. It serves to "Sweep" the servo toward its intended destination in a non-blocking fashion (i.e. no use of delay()). It also allow you to set up a function to call when the servo either starts or stops moving. That is also in the example. Just try your servo on each of the 2 Servo instances (separately).

Here is example .ino

#include "Sweep.h"

void startMoving(int value);
void stoppedMoving(int value);

Sweep servo1(startMoving, stoppedMoving);  //callback functions to execute when the servo starts or stops moving.
Sweep servo2;  // callback functions not required

void setup() 
{
  Serial.begin(9600);
  servo1.begin(4);
  servo2.begin(5);
  pinMode(13, OUTPUT);
  Serial.print("Total Attached Servos:\t");
  Serial.println(Sweep::getServoCount());
}

void loop() 
{
  Sweep::update();  // this is necessary
  
  static unsigned long lastMillis = 0;
  static bool toggle = true;
  if(millis() - lastMillis > 5000)
  {
    Serial.print(F("Moving servo1 to:\t"));
    Serial.println(toggle? 180:0);
    servo1.moveTo(toggle? 180:0);
    toggle = !toggle;
    lastMillis = millis();
  }

  if (Serial.available())
  {
    int target = constrain(Serial.parseInt(), 0, 180);
    Serial.print(F("Moving servo2 to:\t"));
    Serial.println(target);
    servo2.moveTo(target);
  }
}

void startMoving(int value)
{
  Serial.print(F("servo1 motion started: "));
  Serial.println(value);
  digitalWrite(13, HIGH);
}

void stoppedMoving(int value)
{
  Serial.print(F("servo1 motion completed: "));
  Serial.println(value);
  digitalWrite(13, LOW);
}

the .h file:

#ifndef SWEEP_H
#define SWEEP_H

#include <Servo.h>
#include "Arduino.h"

#define MOTION_SPEED_INTERVAL 20  // milliseconds
#define MAX_SERVO_INSTANCES 8

class Sweep : public Servo {

  public:
    Sweep(void(*_startCallback)(int), void(*_endCallback)(int));
    Sweep();
    void begin(int servo);
    void moveTo(uint32_t newPosition);
    static void moveAllTo(uint32_t angle);
    static void update();
    static void update(uint32_t now);
    static int getServoCount();
    int getPosition (void) const;
    
  private:
    enum MotionState{
      ARM_STANDBY,
      ARM_MOVING,
    };
    void(*startCallback)(int);
    void(*endCallback)(int);
    static Sweep* instanceAddress;
    static int instanceCount;
    int servoPin;
    MotionState motionState = ARM_MOVING;
    uint32_t lastMotionMillis = 0;
    int currentPosition;
    int targetPosition;
    uint32_t sensedChangedMillis;
    static void setTargetPosition(Sweep* instance, int position);
};

#endif

and the .cpp:

#include "Sweep.h"
#include "Arduino.h"

int Sweep::instanceCount = 0;
Sweep* instances[MAX_SERVO_INSTANCES] = {nullptr};

Sweep::Sweep()
{
  instances[instanceCount++] = this;
}

Sweep::Sweep(void(*_startCallback)(int), void(*_endCallback)(int))
{
  instances[instanceCount++] = this;
  startCallback = _startCallback;
  endCallback = _endCallback;
}

int Sweep::getServoCount()
{
  return instanceCount;
}

void Sweep::begin(int servo)
{
  servoPin = servo;
  pinMode(servoPin, OUTPUT);
  attach(servoPin);
  moveTo(0);
  delay(1000);
  detach();
}

void Sweep::setTargetPosition(Sweep* instance, int position)  //flip-flop for the right vs. levt hands.
{
  instance->targetPosition = position;
}

void Sweep::moveTo(uint32_t newPosition)
{
  setTargetPosition(this, newPosition);
}

void Sweep::moveAllTo(uint32_t angle)
{
  for(auto i : instances)
  {
    setTargetPosition(i, angle);
  }
}

void Sweep::update()
{
  update(millis());
}

void Sweep::update(uint32_t now)
{
  for(auto srv : instances)
  {
    switch (srv->motionState)
    {
      case ARM_MOVING:
        if(now - srv->lastMotionMillis > MOTION_SPEED_INTERVAL)
        {
          if(srv->targetPosition == srv->currentPosition)
          {
            if(srv->attached())
            {
              srv->detach();
              srv->motionState = ARM_STANDBY;
              srv->endCallback(srv->getPosition());
            }
          }
          else
          {
            if(!srv->attached())
            {
              srv->attach(srv->servoPin);
            }
            if(srv->targetPosition > srv->currentPosition)
            {
              srv->currentPosition++;
              srv->write(srv->currentPosition);
            }
            else
            {
              srv->currentPosition--;
              srv->write(srv->currentPosition);
            }
          }
          srv->lastMotionMillis = now;
        }
        break;
      case ARM_STANDBY:
        if(srv->targetPosition != srv->currentPosition)    
        {
          srv->motionState = ARM_MOVING;
          srv->startCallback(srv->getPosition());
        }
      break;
    }
  }
}

int Sweep::getPosition(void) const
{
  return currentPosition;
}