Using ping1d and servo with arduino nano every

Hi there,

I am trying to control a servo and the ping1d at the same time using an arduino nano every board. At the moment, I have code which sweeps the motor between a minimum and maximum angle in 1 degree implements with a delay at each increment, which works well and the motor moves smoothly. But, when I add code to take distance measurements at each 1 degree increment the motor becomes very jerky (moving backwards and forwards) and often moves out of sync with what is being displayed in the serial monitor. I would like it so the motor moves smoothly while taking distance measurements. I’m new to arduino so I really have no idea what could be causing this issue.

I just wondered if anybody here has had a similar problem and knew how to fix it or if anyone can spot something obviously wrong with my code? Thank you!

#include <Servo.h>
#include "ping1d.h"
#include "SoftwareSerial.h"
static const uint8_t arduinoRxPin = 9;
static const uint8_t arduinoTxPin = 10;
SoftwareSerial pingSerial = SoftwareSerial(arduinoRxPin, arduinoTxPin);
static Ping1D ping { pingSerial };
Servo myservo;
int pos;
int timed = 1000/2;
int min_angle = 0;
String max_angle;
int increment = 5;

void setup() {
  // put your setup code here, to run once:
myservo.attach(3);
pingSerial.begin(9600);
Serial.begin(115200);
while (!ping.initialize()){
  Serial.println("/nPing device failed to initialize!");
  Serial.print("green wire connects to: ");
  Serial.println(arduinoTxPin);
  Serial.print("white wire connects to: ");
  Serial.println(arduinoRxPin);
  delay(1000);
}
}

void loop() {
  // put your main code here, to run repeatedly:
int pos=0;
myservo.write(pos);
Serial.println("Enter maximum angle value");

while (Serial.available()==0){}
max_angle=Serial.readStringUntil('\n');
Serial.print("Rotating ");
Serial.print(max_angle);
Serial.println(" degrees");

for (pos = min_angle; pos <= max_angle.toInt(); pos+=increment){
  while (ping.update()==0){}
  myservo.write(pos);
  Serial.print("Angle: ");
  Serial.println(pos);
  Serial.print("Distance: ");
  Serial.println(ping.distance());
  delay(timed);
}

for (pos = max_angle.toInt(); pos >= min_angle; pos-=increment){
  while (ping.update()==0){}
  myservo.write(pos);
  Serial.print("Angle: ");
  Serial.println(pos);
  Serial.print("Distance: ");
  Serial.println(ping.distance());
  delay(timed);
}
}

Hi @ben2, welcome to the forum :slight_smile:

The problem here is basically that the Servo and SoftwareSerial libraries both use the same timer, so when software serial is communicating with the Ping the servo timing gets funky, which causes the fluctuations.

To avoid that, there are a few different possible approaches:

  1. Use a different servo library, which doesn’t use the same timer
  2. Use an Arduino with additional timers, and support for HardwareSerial
  3. Stop commanding the servo while communicating with the Ping

The code below takes the last approach, which is potentially the easiest way of avoiding the issue, but it comes with the caveat that if the servo is under load it may have undesired rotations while communicating with the Ping, because it’s not being commanded to stay in place at that time.

#include <Servo.h>
#include "ping1d.h"
#include "SoftwareSerial.h"

/* NOTE: Servo and SoftwareSerial clash!
 *   Both libraries use the same timer.
 *   This code avoids clashes by only keeping the servo attached while moving.
 *   If the servo is under load, undesired movement may occur while using Ping.
 */

/* Ping Sonar */
void readSonar(); // declare readSonar function
static const uint8_t arduinoRxPin = 9;
static const uint8_t arduinoTxPin = 10;
SoftwareSerial pingSerial = SoftwareSerial(arduinoRxPin, arduinoTxPin);
static Ping1D ping { pingSerial };
// speed of sound in operating medium (~343 m/s for air, ~1500 m/s for seawater)
static const uint32_t v_sound = 343000; // mm/s

/* Servo */
void moveServo(int angle, bool display=true); // declare moveServo function
Servo myservo;
// angles, in degrees
int pos = 0;
int max_angle;
int min_angle = 0;
int increment = 5;
static const uint8_t servoPin = 3;
// ms to allow for each rotation
//  can reduce multiplier to speed up, until not enough time to turn
uint32_t moveTime = 50 * abs(increment);


void setup() {
  // put your setup code here, to run once:
  // Initialise serial connections
  pingSerial.begin(9600);
  Serial.begin(115200);
  // Initialise Ping device (ensure correctly connected)
  while (!ping.initialize()){
    Serial.println("/nPing device failed to initialize!");
    Serial.print("green wire connects to: ");
    Serial.println(arduinoTxPin);
    Serial.print("white wire connects to: ");
    Serial.println(arduinoRxPin);
    delay(1000);
  }
  ping.set_speed_of_sound(v_sound);
  // Initialise servo position
  moveServo(min_angle, false);
}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.println("Enter maximum angle value");
  
  while (Serial.available()==0){}
  max_angle = Serial.readStringUntil('\n').toInt();
  Serial.print("Rotating ");
  Serial.print(max_angle);
  Serial.println(" degrees");

  for (pos = min_angle; pos <= max_angle; pos+=increment){
    moveServo(pos);
    readSonar();
  }

  for (pos = max_angle; pos >= min_angle; pos-=increment){
    moveServo(pos);
    readSonar();
  }
}

/* Gets a sonar measurement, and displays the results. */
void readSonar() {
  // request measurements until one arrives
  while (!ping.update()){}
  // display the results
  Serial.print("Distance: ");
  Serial.print(ping.distance());
  Serial.print(" mm\tConfidence: ");
  Serial.print(ping.confidence());
  Serial.println("%");
}

/* Engages the servo while moving to the requested 'angle'.
 *  If 'display' is true, prints the current angle request.
 */
void moveServo(int angle, bool display) {
  myservo.attach(servoPin);
  myservo.write(angle);
  delay(moveTime);
  if (display) {
    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print("\t");
  }
  myservo.detach();
}

By the way, I’ve edited your post to have your code in a code block, so it’s easier for others to read, and to copy for testing. You can read about how to do that (and more) in the How to Use the Blue Robotics Forums post :slight_smile:

1 Like

Hi @EliotBR,

Thank you so much for making that clear and offering those solutions!
I have given the code ago and it has solved my issue and so far the servo hasn’t made any undesired rotations!

Again Thank you! :smiley:

1 Like