Continuous Servo Issues - Code Fixes

[1noahb1]

I have been working on my most ambitious arduino project for about a week now. The project is a home radar and defense system, and uses multiple continuous servos to move around and target, along with rotate the radar ultrasonic sensor. I've been testing it with Tinkercad circuits and Wokwi, and it worked perfectly. But when I got out my old giga R1, the right drive servo wouldn't stop spinning. I tried troubleshooting with AI and help from my teacher, but to no avail. Help!

///////////////////////////////////////
/// HOME A.D.S - AIR DEFENSE SYSTEM ///
///////////////////////////////////////

#include <Servo.h>

// Pins
const int JOY_X_PIN = A0;
const int JOY_Y_PIN = A1;
const int JOY_SW_PIN = 26;

const int PIR_PIN  = 22;
const int TRIG_PIN = 24;
const int ECHO_PIN = 25;

const int LEFT_DRIVE_SERVO_PIN  = 6;
const int RIGHT_DRIVE_SERVO_PIN = 7;
const int TOP_SERVO_PIN         = 8;
const int SCAN_SERVO_PIN        = 9;

// RGB LED pins
const int RGB_RED_PIN   = 30;
const int RGB_GREEN_PIN = 31;
const int RGB_BLUE_PIN  = 32;

// Servo objects
Servo leftDriveServo;
Servo rightDriveServo;
Servo topServo;
Servo scanServo;

// Separate stop values
const int LEFT_DRIVE_STOP  = 90;
const int RIGHT_DRIVE_STOP = 94;
const int TOP_SERVO_STOP   = 90;

// Drive ranges
const int DRIVE_OFFSET_MIN = -30;
const int DRIVE_OFFSET_MAX = 30;
const int JOYSTICK_DEADZONE = 3;

// Top servo offsets
const int TOP_CW_OFFSET  = 25;
const int TOP_CCW_OFFSET = -25;

// Scan servo settings
const int SCAN_START_POS = 10;
const int SCAN_MIN = 10;
const int SCAN_MAX = 170;

// Reverse options
const bool REVERSE_LEFT_DRIVE  = false;
const bool REVERSE_RIGHT_DRIVE = true;
const bool REVERSE_TOP_SERVO   = false;

// Current commands
int leftDriveCmd = LEFT_DRIVE_STOP;
int rightDriveCmd = RIGHT_DRIVE_STOP;

// Top state
int topState = 0;   // 0 = front, 180 = back
int topTargetState = 0;
bool topTurning = false;
unsigned long topTurnStartTime = 0;
unsigned long TOP_TURN_TIME_MS = 900;

// Top output tracking
int topTurnOutputCmd = TOP_SERVO_STOP;

// Scan state
int scanPos = SCAN_START_POS;
bool scanForward = true;

// Keyboard override
bool keyboardMode = false;
int kbMove = 0;
int kbTurn = 0;

// Alert settings
const int DETECTION_DISTANCE_CM = 60;
unsigned long ACTION_COOLDOWN_MS = 5000;
unsigned long lastActionTime = 0;
unsigned long ALERT_HOLD_MS = 1200;
unsigned long alertHoldUntil = 0;

// Timers
unsigned long lastScanUpdate = 0;
unsigned long lastRadarSend = 0;
unsigned long lastDistanceRead = 0;
unsigned long lastJoyButtonTime = 0;

// Timing values
const unsigned long SCAN_INTERVAL_MS = 15;
const unsigned long RADAR_SEND_INTERVAL_MS = 35;
const unsigned long DISTANCE_READ_INTERVAL_MS = 40;
const unsigned long BUTTON_DEBOUNCE_MS = 250;

// Distance cache
long lastDistance = -1;

// Alert flash state machine
bool alertActive = false;
bool alertLedOn = false;
int alertToggleCount = 0;
int alertTargetToggles = 0;
unsigned long alertLastChange = 0;
unsigned long alertOnTime = 150;
unsigned long alertOffTime = 150;

// Helper functions
int clampValue(int value, int minVal, int maxVal) {
  if (value < minVal) {
    return minVal;
  } else if (value > maxVal) {
    return maxVal;
  } else {
    return value;
  }
}

int applyContinuousCommand(int stopValue, int offsetValue, bool reversed) {
  int cmd;

  if (reversed) {
    cmd = stopValue - offsetValue;
  } else {
    cmd = stopValue + offsetValue;
  }

  return clampValue(cmd, 0, 180);
}

// RGB LED helpers
void setRGB(bool redOn, bool greenOn, bool blueOn) {
  digitalWrite(RGB_RED_PIN, redOn ? HIGH : LOW);
  digitalWrite(RGB_GREEN_PIN, greenOn ? HIGH : LOW);
  digitalWrite(RGB_BLUE_PIN, blueOn ? HIGH : LOW);
}

void startRedAlert(int times, unsigned long onTime, unsigned long offTime) {
  alertActive = true;
  alertLedOn = false;
  alertToggleCount = 0;
  alertTargetToggles = times * 2;
  alertLastChange = millis();
  alertOnTime = onTime;
  alertOffTime = offTime;
  setRGB(false, false, false);
}

void updateAlertFlash() {
  if (!alertActive) {
    return;
  }

  unsigned long now = millis();
  unsigned long interval = alertLedOn ? alertOnTime : alertOffTime;

  if (now - alertLastChange >= interval) {
    alertLastChange = now;
    alertLedOn = !alertLedOn;
    alertToggleCount++;

    if (alertLedOn) {
      setRGB(true, false, false);
    } else {
      setRGB(false, false, false);
    }

    if (alertToggleCount >= alertTargetToggles) {
      alertActive = false;
      alertLedOn = false;
      setRGB(false, false, false);
    }
  }
}

// Top control
void stopTopServo() {
  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurning = false;
}

void startTopTurnToState(int newTargetState) {
  if (topTurning) {
    return;
  }

  if (newTargetState == topState) {
    return;
  }

  topTargetState = newTargetState;

  int offset = 0;
  if (newTargetState == 180) {
    offset = TOP_CW_OFFSET;
  } else {
    offset = TOP_CCW_OFFSET;
  }

  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, offset, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurnStartTime = millis();
  topTurning = true;
}

void updateTopTurn() {
  if (!topTurning) {
    return;
  }

  unsigned long now = millis();

  if (now - topTurnStartTime >= TOP_TURN_TIME_MS) {
    stopTopServo();
    topState = topTargetState;
  }
}

void zeroTopState() {
  stopTopServo();
  topState = 0;
  topTargetState = 0;
}

// Serial command handling
// Commands:
// KB,move,turn
// KBMODE,0
// KBMODE,1
// ALERT
// ZERO
void handleSerialLine(String line) {
  line.trim();

  if (line.length() == 0) {
    return;
  }

  if (line.startsWith("KBMODE,")) {
    int mode = line.substring(7).toInt();

    if (mode == 1) {
      keyboardMode = true;
    } else {
      keyboardMode = false;
      kbMove = 0;
      kbTurn = 0;
    }
  } else if (line.startsWith("KB,")) {
    int firstComma = line.indexOf(',');
    int secondComma = line.indexOf(',', firstComma + 1);

    if (firstComma >= 0 && secondComma > firstComma) {
      kbMove = clampValue(line.substring(firstComma + 1, secondComma).toInt(), -30, 30);
      kbTurn = clampValue(line.substring(secondComma + 1).toInt(), -30, 30);
    }
  } else if (line == "ALERT") {
    startRedAlert(4, 150, 150);
  } else if (line == "ZERO") {
    zeroTopState();
  }
}

void readSerialCommands() {
  while (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    handleSerialLine(line);
  }
}

// Drive control
void updateDrive() {
  int moveValue = 0;
  int turnValue = 0;

  if (keyboardMode) {
    moveValue = kbMove;
    turnValue = kbTurn;
  } else {
    int xVal = analogRead(JOY_X_PIN);
    int yVal = analogRead(JOY_Y_PIN);

    moveValue = map(yVal, 0, 1023, -30, 30);
    turnValue = map(xVal, 0, 1023, -30, 30);

    if (abs(moveValue) < JOYSTICK_DEADZONE) {
      moveValue = 0;
    }

    if (abs(turnValue) < JOYSTICK_DEADZONE) {
      turnValue = 0;
    }
  }

  int leftOffset = clampValue(moveValue + turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);
  int rightOffset = clampValue(moveValue - turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);

  leftDriveCmd = applyContinuousCommand(LEFT_DRIVE_STOP, leftOffset, REVERSE_LEFT_DRIVE);
  rightDriveCmd = applyContinuousCommand(RIGHT_DRIVE_STOP, rightOffset, REVERSE_RIGHT_DRIVE);

  leftDriveServo.write(leftDriveCmd);
  rightDriveServo.write(rightDriveCmd);
}

// Joystick button reset
void updateJoystickCalibration() {
  unsigned long now = millis();

  if (digitalRead(JOY_SW_PIN) == LOW) {
    if (now - lastJoyButtonTime >= BUTTON_DEBOUNCE_MS) {
      zeroTopState();
      lastJoyButtonTime = now;
    }
  }
}

// Motion response
void updateMotionResponse() {
  if (topTurning) {
    return;
  }

  if (alertActive) {
    return;
  }

  if (millis() < alertHoldUntil) {
    return;
  }

  int motionDetected = digitalRead(PIR_PIN);

  if (motionDetected == HIGH) {
    if (topState != 180) {
      startTopTurnToState(180);
    }
  } else {
    if (topState != 0) {
      startTopTurnToState(0);
    }
  }
}

// Ultrasonic
long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) {
    return -1;
  } else {
    return duration * 0.034 / 2;
  }
}

void updateDistanceReading() {
  unsigned long now = millis();

  if (now - lastDistanceRead >= DISTANCE_READ_INTERVAL_MS) {
    lastDistanceRead = now;
    lastDistance = readDistanceCM();
  }
}

// Scan servo
void updateScanServo() {
  unsigned long now = millis();

  if (now - lastScanUpdate >= SCAN_INTERVAL_MS) {
    lastScanUpdate = now;

    if (scanForward) {
      scanPos++;
      if (scanPos >= SCAN_MAX) {
        scanPos = SCAN_MAX;
        scanForward = false;
      }
    } else {
      scanPos--;
      if (scanPos <= SCAN_MIN) {
        scanPos = SCAN_MIN;
        scanForward = true;
      }
    }

    scanServo.write(scanPos);
  }
}

// Directional alert behavior
void handleUltrasonicAlert(long distance) {
  unsigned long currentTime = millis();

  if (distance > 0 && distance <= DETECTION_DISTANCE_CM) {
    if (currentTime - lastActionTime >= ACTION_COOLDOWN_MS) {
      if (!topTurning) {
        if (scanPos >= 90 && topState != 180) {
          startTopTurnToState(180);
        } else if (scanPos < 90 && topState != 0) {
          startTopTurnToState(0);
        }
      }

      startRedAlert(4, 150, 150);
      lastActionTime = currentTime;
      alertHoldUntil = currentTime + ALERT_HOLD_MS;
    }
  }
}

// Serial output
// SCAN,angle,distance,motion,topState,leftCmd,rightCmd,keyboardMode
void sendRadarData(long distance) {
  int motionDetected = digitalRead(PIR_PIN);

  Serial.print("SCAN,");
  Serial.print(scanPos);
  Serial.print(",");
  Serial.print(distance);
  Serial.print(",");
  Serial.print(motionDetected);
  Serial.print(",");
  Serial.print(topState);
  Serial.print(",");
  Serial.print(leftDriveCmd);
  Serial.print(",");
  Serial.print(rightDriveCmd);
  Serial.print(",");
  Serial.println(keyboardMode ? 1 : 0);
}

void updateRadarSend() {
  unsigned long now = millis();

  if (now - lastRadarSend >= RADAR_SEND_INTERVAL_MS) {
    lastRadarSend = now;
    sendRadarData(lastDistance);
  }
}

// Setup
void setup() {
  Serial.begin(9600);
  Serial.setTimeout(10);

  pinMode(JOY_SW_PIN, INPUT_PULLUP);
  pinMode(PIR_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE_PIN, OUTPUT);

  leftDriveServo.attach(LEFT_DRIVE_SERVO_PIN);
  rightDriveServo.attach(RIGHT_DRIVE_SERVO_PIN);
  topServo.attach(TOP_SERVO_PIN);
  scanServo.attach(SCAN_SERVO_PIN);

  leftDriveServo.write(applyContinuousCommand(LEFT_DRIVE_STOP, 0, REVERSE_LEFT_DRIVE));
  rightDriveServo.write(applyContinuousCommand(RIGHT_DRIVE_STOP, 0, REVERSE_RIGHT_DRIVE));
  topServo.write(applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO));
  scanServo.write(scanPos);

  setRGB(false, false, false);
}

// Main loop
void loop() {
  readSerialCommands();
  updateDrive();
  updateJoystickCalibration();
  updateTopTurn();
  updateScanServo();
  updateDistanceReading();
  handleUltrasonicAlert(lastDistance);
  updateMotionResponse();
  updateAlertFlash();
  updateRadarSend();
}


// My main sources are listed below:
// Joystick Code based on “arduinogetstarted.com/tutorials/arduino-joystick”
// Motion Sensor Code based on “arduinogetstarted.com/tutorials/arduino-motion-sensor”
// PC-to-Arduino Serial Code based on “arduinogetstarted.com/tutorials/arduino-serial-monitor”
// Millis() Function Syntax based on “docs.arduino.cc/language-reference/en/functions/time/millis/”

[van_der_decken]

If you write a minimal sketch and write the various stop values to your servos, do they stop?

[LarryD]

FYI

• To stop a continuous servo, detach the servo.

[UKHeliBob]

@1noahb1

the right drive servo wouldn't stop spinning

Do you mean that it spins slowly in one direction or the other whatever value of RIGHT_DRIVE_STOP you use ?

[xfpd]

@1noahb1

right drive servo wouldn't stop

Show the values being sent to the right (and left) during "stop?" Should be R94, L90.

[alto777]

@1noahb1

and it worked perfectly.

Post a link to the perfectly working wokwi project.

a7

[xfpd]

What is the format for entering keyboard commands?

My guess why "right always moves" is because something to do with "Serial" / "String" has cleared its buffer, leaving "0" which is interpreted as "skid-turn right" (left stops, right moves).

Click for code (slightly modified for Serial Terminal presentation)

// https://forum.arduino.cc/t/continuous-servo-issues-code-fixes/1440288

///////////////////////////////////////
/// HOME A.D.S - AIR DEFENSE SYSTEM ///
///////////////////////////////////////

#include <Servo.h>

// Pins
const int JOY_X_PIN = A0;
const int JOY_Y_PIN = A1;
const int JOY_SW_PIN = 26;

const int PIR_PIN  = 22;

const int TRIG_PIN = 24;
const int ECHO_PIN = 25;

const int LEFT_DRIVE_SERVO_PIN  = 6;
const int RIGHT_DRIVE_SERVO_PIN = 7;
const int TOP_SERVO_PIN         = 8;
const int SCAN_SERVO_PIN        = 9;

// RGB LED pins
const int RGB_RED_PIN   = 30;
const int RGB_GREEN_PIN = 31;
const int RGB_BLUE_PIN  = 32;

// Servo objects
Servo leftDriveServo;
Servo rightDriveServo;
Servo topServo;
Servo scanServo;

// Separate stop values
const int LEFT_DRIVE_STOP  = 90;
const int RIGHT_DRIVE_STOP = 94;
const int TOP_SERVO_STOP   = 90;

// Drive ranges
const int DRIVE_OFFSET_MIN = -30;
const int DRIVE_OFFSET_MAX = 30;
const int JOYSTICK_DEADZONE = 3;

// Top servo offsets
const int TOP_CW_OFFSET  = 25;
const int TOP_CCW_OFFSET = -25;

// Scan servo settings
const int SCAN_START_POS = 10;
const int SCAN_MIN = 10;
const int SCAN_MAX = 170;

// Reverse options
const bool REVERSE_LEFT_DRIVE  = false;
const bool REVERSE_RIGHT_DRIVE = true;
const bool REVERSE_TOP_SERVO   = false;

// Current commands
int leftDriveCmd = LEFT_DRIVE_STOP;
int rightDriveCmd = RIGHT_DRIVE_STOP;

// Top state
int topState = 0;   // 0 = front, 180 = back
int topTargetState = 0;
bool topTurning = false;
unsigned long topTurnStartTime = 0;
unsigned long TOP_TURN_TIME_MS = 900;

// Top output tracking
int topTurnOutputCmd = TOP_SERVO_STOP;

// Scan state
int scanPos = SCAN_START_POS;
bool scanForward = true;

// Keyboard override
bool keyboardMode = false;
int kbMove = 0;
int kbTurn = 0;

// Alert settings
const int DETECTION_DISTANCE_CM = 60;
unsigned long ACTION_COOLDOWN_MS = 5000;
unsigned long lastActionTime = 0;
unsigned long ALERT_HOLD_MS = 1200;
unsigned long alertHoldUntil = 0;

// Timers
unsigned long lastScanUpdate = 0;
unsigned long lastRadarSend = 0;
unsigned long lastDistanceRead = 0;
unsigned long lastJoyButtonTime = 0;

// Timing values
const unsigned long SCAN_INTERVAL_MS = 15;
const unsigned long RADAR_SEND_INTERVAL_MS = 35;
const unsigned long DISTANCE_READ_INTERVAL_MS = 40;
const unsigned long BUTTON_DEBOUNCE_MS = 250;

// Distance cache
long lastDistance = -1;

// Alert flash state machine
bool alertActive = false;
bool alertLedOn = false;
int alertToggleCount = 0;
int alertTargetToggles = 0;
unsigned long alertLastChange = 0;
unsigned long alertOnTime = 150;
unsigned long alertOffTime = 150;

// Helper functions
int clampValue(int value, int minVal, int maxVal) {
  if (value < minVal) {
    return minVal;
  } else if (value > maxVal) {
    return maxVal;
  } else {
    return value;
  }
}

int applyContinuousCommand(int stopValue, int offsetValue, bool reversed) {
  int cmd;

  if (reversed) {
    cmd = stopValue - offsetValue;
  } else {
    cmd = stopValue + offsetValue;
  }

  return clampValue(cmd, 0, 180);
}

// RGB LED helpers
void setRGB(bool redOn, bool greenOn, bool blueOn) {
  digitalWrite(RGB_RED_PIN, redOn ? HIGH : LOW);
  digitalWrite(RGB_GREEN_PIN, greenOn ? HIGH : LOW);
  digitalWrite(RGB_BLUE_PIN, blueOn ? HIGH : LOW);
}

void startRedAlert(int times, unsigned long onTime, unsigned long offTime) {
  alertActive = true;
  alertLedOn = false;
  alertToggleCount = 0;
  alertTargetToggles = times * 2;
  alertLastChange = millis();
  alertOnTime = onTime;
  alertOffTime = offTime;
  setRGB(false, false, false);
}

void updateAlertFlash() {
  if (!alertActive) {
    return;
  }

  unsigned long now = millis();
  unsigned long interval = alertLedOn ? alertOnTime : alertOffTime;

  if (now - alertLastChange >= interval) {
    alertLastChange = now;
    alertLedOn = !alertLedOn;
    alertToggleCount++;

    if (alertLedOn) {
      setRGB(true, false, false);
    } else {
      setRGB(false, false, false);
    }

    if (alertToggleCount >= alertTargetToggles) {
      alertActive = false;
      alertLedOn = false;
      setRGB(false, false, false);
    }
  }
}

// Top control
void stopTopServo() {
  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurning = false;
}

void startTopTurnToState(int newTargetState) {
  if (topTurning) {
    return;
  }

  if (newTargetState == topState) {
    return;
  }

  topTargetState = newTargetState;

  int offset = 0;
  if (newTargetState == 180) {
    offset = TOP_CW_OFFSET;
  } else {
    offset = TOP_CCW_OFFSET;
  }

  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, offset, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurnStartTime = millis();
  topTurning = true;
}

void updateTopTurn() {
  if (!topTurning) {
    return;
  }

  unsigned long now = millis();

  if (now - topTurnStartTime >= TOP_TURN_TIME_MS) {
    stopTopServo();
    topState = topTargetState;
  }
}

void zeroTopState() {
  stopTopServo();
  topState = 0;
  topTargetState = 0;
}

// Serial command handling
// Commands:
// KB,move,turn
// KBMODE,0
// KBMODE,1
// ALERT
// ZERO
void handleSerialLine(String line) {
  line.trim();

  if (line.length() == 0) {
    return;
  }

  if (line.startsWith("KBMODE,")) {
    int mode = line.substring(7).toInt();

    if (mode == 1) {
      keyboardMode = true;
    } else {
      keyboardMode = false;
      kbMove = 0;
      kbTurn = 0;
    }
  } else if (line.startsWith("KB,")) {
    int firstComma = line.indexOf(',');
    int secondComma = line.indexOf(',', firstComma + 1);

    if (firstComma >= 0 && secondComma > firstComma) {
      kbMove = clampValue(line.substring(firstComma + 1, secondComma).toInt(), -30, 30);
      kbTurn = clampValue(line.substring(secondComma + 1).toInt(), -30, 30);
    }
  } else if (line == "ALERT") {
    startRedAlert(4, 150, 150);
  } else if (line == "ZERO") {
    zeroTopState();
  }
}

void readSerialCommands() {
  while (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    handleSerialLine(line);
  }
}

// Drive control
void updateDrive() {
  int moveValue = 0;
  int turnValue = 0;

  if (keyboardMode) {
    moveValue = kbMove;
    turnValue = kbTurn;
  } else {
    int xVal = analogRead(JOY_X_PIN);
    int yVal = analogRead(JOY_Y_PIN);

    moveValue = map(yVal, 0, 1023, -30, 30);
    turnValue = map(xVal, 0, 1023, -30, 30);

    if (abs(moveValue) < JOYSTICK_DEADZONE) {
      moveValue = 0;
    }

    if (abs(turnValue) < JOYSTICK_DEADZONE) {
      turnValue = 0;
    }
  }

  int leftOffset = clampValue(moveValue + turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);
  int rightOffset = clampValue(moveValue - turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);

  leftDriveCmd = applyContinuousCommand(LEFT_DRIVE_STOP, leftOffset, REVERSE_LEFT_DRIVE);
  rightDriveCmd = applyContinuousCommand(RIGHT_DRIVE_STOP, rightOffset, REVERSE_RIGHT_DRIVE);

  leftDriveServo.write(leftDriveCmd);
  rightDriveServo.write(rightDriveCmd);
}

// Joystick button reset
void updateJoystickCalibration() {
  unsigned long now = millis();

  if (digitalRead(JOY_SW_PIN) == LOW) {
    if (now - lastJoyButtonTime >= BUTTON_DEBOUNCE_MS) {
      zeroTopState();
      lastJoyButtonTime = now;
    }
  }
}

// Motion response
void updateMotionResponse() {
  if (topTurning) {
    return;
  }

  if (alertActive) {
    return;
  }

  if (millis() < alertHoldUntil) {
    return;
  }

  int motionDetected = digitalRead(PIR_PIN);

  if (motionDetected == HIGH) {
    if (topState != 180) {
      startTopTurnToState(180);
    }
  } else {
    if (topState != 0) {
      startTopTurnToState(0);
    }
  }
}

// Ultrasonic
long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) {
    return -1;
  } else {
    return duration * 0.034 / 2;
  }
}

void updateDistanceReading() {
  unsigned long now = millis();

  if (now - lastDistanceRead >= DISTANCE_READ_INTERVAL_MS) {
    lastDistanceRead = now;
    lastDistance = readDistanceCM();
  }
}

// Scan servo
void updateScanServo() {
  unsigned long now = millis();

  if (now - lastScanUpdate >= SCAN_INTERVAL_MS) {
    lastScanUpdate = now;

    if (scanForward) {
      scanPos++;
      if (scanPos >= SCAN_MAX) {
        scanPos = SCAN_MAX;
        scanForward = false;
      }
    } else {
      scanPos--;
      if (scanPos <= SCAN_MIN) {
        scanPos = SCAN_MIN;
        scanForward = true;
      }
    }

    scanServo.write(scanPos);
  }
}

// Directional alert behavior
void handleUltrasonicAlert(long distance) {
  unsigned long currentTime = millis();

  if (distance > 0 && distance <= DETECTION_DISTANCE_CM) {
    if (currentTime - lastActionTime >= ACTION_COOLDOWN_MS) {
      if (!topTurning) {
        if (scanPos >= 90 && topState != 180) {
          startTopTurnToState(180);
        } else if (scanPos < 90 && topState != 0) {
          startTopTurnToState(0);
        }
      }

      startRedAlert(4, 150, 150);
      lastActionTime = currentTime;
      alertHoldUntil = currentTime + ALERT_HOLD_MS;
    }
  }
}

// Serial output
// SCAN,angle,distance,motion,topState,leftCmd,rightCmd,keyboardMode
void sendRadarData(long distance) {
  int motionDetected = digitalRead(PIR_PIN);

  Serial.print("AZ:");
  pad(scanPos);
  Serial.print(" DIST:");
  pad(distance);
  Serial.print(" PIR:");
  Serial.print(motionDetected);
  Serial.print(" TOP:");
  pad(topState);
  Serial.print(" L:");
  pad(leftDriveCmd);
  Serial.print(" R:");
  pad(rightDriveCmd);
  Serial.print(" KYBD:");
  Serial.println(keyboardMode ? 1 : 0);
}

void pad(int val) {
  if (val < 100) Serial.print("0");
  if (val < 10) Serial.print("0");
  Serial.print(val);
}

void updateRadarSend() {
  unsigned long now = millis();

  if (now - lastRadarSend >= RADAR_SEND_INTERVAL_MS) {
    lastRadarSend = now;
    sendRadarData(lastDistance);
  }
}

// Setup
void setup() {
  Serial.begin(9600);
  Serial.setTimeout(10);

  pinMode(JOY_SW_PIN, INPUT_PULLUP);
  pinMode(PIR_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE_PIN, OUTPUT);

  leftDriveServo.attach(LEFT_DRIVE_SERVO_PIN);
  rightDriveServo.attach(RIGHT_DRIVE_SERVO_PIN);
  topServo.attach(TOP_SERVO_PIN);
  scanServo.attach(SCAN_SERVO_PIN);

  leftDriveServo.write(applyContinuousCommand(LEFT_DRIVE_STOP, 0, REVERSE_LEFT_DRIVE));
  rightDriveServo.write(applyContinuousCommand(RIGHT_DRIVE_STOP, 0, REVERSE_RIGHT_DRIVE));
  topServo.write(applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO));
  scanServo.write(scanPos);

  setRGB(false, false, false);
}

// Main loop
void loop() {
  readSerialCommands();
  updateDrive();
  updateJoystickCalibration();
  updateTopTurn();
  updateScanServo();
  updateDistanceReading();
  handleUltrasonicAlert(lastDistance);
  updateMotionResponse();
  updateAlertFlash();
  updateRadarSend();
}


// My main sources are listed below:
// Joystick Code based on “arduinogetstarted.com/tutorials/arduino-joystick”
// Motion Sensor Code based on “arduinogetstarted.com/tutorials/arduino-motion-sensor”
// PC-to-Arduino Serial Code based on “arduinogetstarted.com/tutorials/arduino-serial-monitor”
// Millis() Function Syntax based on “docs.arduino.cc/language-reference/en/functions/time/millis/”

Click for Wokwi diagram.json

{
  "version": 1,
  "author": "foreignpigdog x",
  "editor": "wokwi",
  "parts": [
    { "type": "wokwi-arduino-mega", "id": "mega", "top": 0.6, "left": -3.6, "attrs": {} },
    {
      "type": "wokwi-servo",
      "id": "servo1",
      "top": -89.2,
      "left": -74,
      "rotate": 180,
      "attrs": {}
    },
    {
      "type": "wokwi-analog-joystick",
      "id": "joystick1",
      "top": -125.4,
      "left": 351,
      "attrs": {}
    },
    {
      "type": "wokwi-hc-sr04",
      "id": "ultrasonic1",
      "top": 81.9,
      "left": 423.3,
      "rotate": 90,
      "attrs": { "distance": "2" }
    },
    {
      "type": "wokwi-pir-motion-sensor",
      "id": "pir1",
      "top": -101.6,
      "left": 463.02,
      "attrs": {}
    },
    {
      "type": "wokwi-servo",
      "id": "servo2",
      "top": -137.2,
      "left": -74,
      "rotate": 180,
      "attrs": {}
    },
    {
      "type": "wokwi-servo",
      "id": "servo3",
      "top": -185.2,
      "left": -74,
      "rotate": 180,
      "attrs": {}
    },
    {
      "type": "wokwi-servo",
      "id": "servo4",
      "top": -233.2,
      "left": -74,
      "rotate": 180,
      "attrs": {}
    },
    {
      "type": "wokwi-rgb-led",
      "id": "rgb1",
      "top": -168.8,
      "left": 250.7,
      "attrs": { "common": "anode" }
    },
    { "type": "wokwi-text", "id": "text1", "top": -38.4, "left": -96, "attrs": { "text": "L" } },
    { "type": "wokwi-text", "id": "text2", "top": -86.4, "left": -96, "attrs": { "text": "R" } },
    {
      "type": "wokwi-text",
      "id": "text3",
      "top": -134.4,
      "left": -105.6,
      "attrs": { "text": "TOP" }
    },
    {
      "type": "wokwi-text",
      "id": "text4",
      "top": -182.4,
      "left": -105.6,
      "attrs": { "text": "SW" }
    }
  ],
  "connections": [
    [ "joystick1:VCC", "mega:5V.2", "red", [ "v0" ] ],
    [ "joystick1:VERT", "mega:A1", "green", [ "v211.2", "h-115.2" ] ],
    [ "joystick1:HORZ", "mega:A0", "green", [ "v220.8", "h-220.8" ] ],
    [ "joystick1:SEL", "mega:26", "green", [ "v0" ] ],
    [ "joystick1:GND", "mega:GND.5", "black", [ "v0" ] ],
    [ "ultrasonic1:TRIG", "mega:24", "green", [ "h-19.2", "v-77.2", "h-84.2" ] ],
    [ "ultrasonic1:ECHO", "mega:25", "green", [ "h-28.8", "v-88.15" ] ],
    [ "ultrasonic1:GND", "mega:GND.5", "black", [ "h-38.4", "v45.85" ] ],
    [ "ultrasonic1:VCC", "mega:5V.2", "red", [ "h-9.6", "v-87.4" ] ],
    [ "pir1:OUT", "mega:22", "green", [ "v0" ] ],
    [ "pir1:VCC", "mega:5V.2", "red", [ "v0" ] ],
    [ "pir1:GND", "mega:GND.5", "black", [ "v38.4", "h-96.26", "v144" ] ],
    [ "mega:6", "servo1:PWM", "green", [ "v0" ] ],
    [ "mega:7", "servo2:PWM", "green", [ "v0" ] ],
    [ "mega:8", "servo3:PWM", "green", [ "v0" ] ],
    [ "mega:9", "servo4:PWM", "green", [ "v0" ] ],
    [ "mega:GND.1", "servo1:GND", "black", [ "v0" ] ],
    [ "mega:GND.1", "servo2:GND", "black", [ "v0" ] ],
    [ "mega:GND.1", "servo3:GND", "black", [ "v0" ] ],
    [ "mega:GND.1", "servo4:GND", "black", [ "v0" ] ],
    [ "mega:5V", "servo4:V+", "red", [ "v0" ] ],
    [ "mega:5V", "servo3:V+", "red", [ "v0" ] ],
    [ "mega:5V", "servo2:V+", "red", [ "v0" ] ],
    [ "mega:5V", "servo1:V+", "red", [ "v0" ] ],
    [ "rgb1:R", "mega:30", "green", [ "v0" ] ],
    [ "rgb1:G", "mega:31", "green", [ "v0" ] ],
    [ "rgb1:B", "mega:32", "green", [ "v0" ] ],
    [ "mega:GND.1", "rgb1:COM", "black", [ "v-28.8", "h124.6" ] ]
  ],
  "dependencies": {}
}

[gcjr]

@1noahb1

// Separate stop values
const int LEFT_DRIVE_STOP  = 90;
const int RIGHT_DRIVE_STOP = 94;
const int TOP_SERVO_STOP   = 90;

why is RIGHT_DRIVE_STOP 94 and not 90?

[xfpd]

@gcjr

RIGHT_DRIVE_STOP 94 and not 90?

Slight, physical, variation in the servo.

[edit]... but... maybe 94 is too high, and should be verified.

[UKHeliBob]

I await with interest the answer to the question in post #4

[1noahb1]

It seems to spin quickly, as if it was set to 180 and not 94

[1noahb1]

Here is the link: https://wokwi.com/projects/460781156040932353

Wokwi does not support continuous servos, so all the servos in the Wokwi build are standard servos. However, nothing seems to be going awry.

[xfpd]

@1noahb1

Wokwi does not support continuous servos

Examples on how to "show" 360 on a 180:

servo360buttons - Wokwi ESP32, STM32, Arduino Simulator

servo360speed - Wokwi ESP32, STM32, Arduino Simulator

[xfpd]

I've never been able to get the RGBLED to light (programmed for red only, or off). My wokwi (in post #7, which also guesses on the cause of the right 360 issue) or your wokwi in post #12.

And... how is KBMODE activated, and what are the commands?

[xfpd]

@1noahb1

spin quickly, as if it was set to 180

Verify it is in the 180 direction or the 0 direction... see post #7

[1noahb1]

KBMODE is a part of a separate Python script for this project that allows me to use WASD to control its movement. I have tested the Arduino code both with and without the Python UI enabled, and the servo issue is present no matter what.

[1noahb1]

yes.

[van_der_decken]

Then you aren't writing those values to the servos in the sketch you've shown, are you?

[1noahb1]

I am. I actively change the servo stop values depending on which servos I use, and whether they stop at 90 or another value. No matter what value is being pushed to the right drive servo, it always spins to the right constantly.

[xfpd]

@1noahb1

the servo issue is present no matter what

What what? Which direction is it spinning? 0 or 180?