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);
}
}
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:
Use a different servo library, which doesn’t use the same timer
Use an Arduino with additional timers, and support for HardwareSerial
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
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!
Hi all
The servo rocks back and force. Have tried different Nano boards and servos, nothing makes any difference. The Arduino serial monitor is stable and the measurements changes with depth of water as it should. If I use a delay 1000 in the code the rocking smooths a bit, but its still there. What could make these erratic behavior from the servo?
And how to solve it?
Code:
#include "ping1d.h"
#include "SoftwareSerial.h"
#include <Servo.h>
Servo myservo;
// Pin D10 as arduino rx (rx cable from Ping = White).
// Pin D11 as arduino tx (tx cable from Ping = Green).
static const uint8_t arduinoRxPin = 10;
static const uint8_t arduinoTxPin = 11;
SoftwareSerial pingSerial = SoftwareSerial(arduinoRxPin, arduinoTxPin);
static Ping1D ping { pingSerial };
static const uint8_t ledPin = 13;
void setup() {
pingSerial.begin(9600);
Serial.begin(115200);
myservo.attach(9); // (9)=D9
pinMode(ledPin, OUTPUT);
}
void loop() {
if (ping.update()) {
delay(1000);
Serial.println(ping.distance());
if(ping.distance()<=800)
{ myservo.write(55) ; } // 55 är det minsta som går. Mekanisk begränsning där. = Max vinkel för uppgång.
if(ping.distance()>=850)
{ myservo.write(63); }
if(ping.distance()>=900)
{ myservo.write(71); }
if(ping.distance()>=950)
{ myservo.write(74); }
if(ping.distance()>=1000)
{ myservo.write(77); }
if(ping.distance()>=1050)
{ myservo.write(80); }
if(ping.distance()>=1100)
{ myservo.write(90); }
if(ping.distance()>=1150)
{ myservo.write(100); }
if(ping.distance()>=1200)
{ myservo.write(110); }
if(ping.distance()>=2400)
{ myservo.write(120); }
if(ping.distance()>=2500)
{ myservo.write(130); }
if(ping.distance()>=2600)
{ myservo.write(140); }
if(ping.distance()>=2700)
{ myservo.write(150); }
if(ping.distance()>=2800)
{ myservo.write(165); } //165 är det högst som går. Mekanisk begränsning där. = Max vinkel för nedgång.
}
// Toggle the LED to show that the program is running
digitalWrite(ledPin, !digitalRead(ledPin));
}
I’ve moved your post here because it’s on the same topic - I’d recommend you look at the discussion above
By the way, at the moment your code writes to the servo for every checked value less than the measured distance, which could have the effect of somewhat jerkily moving the servo to several positions on the way rather than a single smooth motion based on only the highest threshold region the distance measurement is in. Your code also currently does nothing when the measured distance is between 800 and 850.
You may wish to change your code to do something like
... // includes and initialisation
uint32_t latest_distance;
... // setup
void loop() {
if (ping.update()) {
latest_distance = ping.distance();
Serial.println(latest_distance);
if (latest_distance <= 800) {
myservo.write(55); // 55 är det minsta som går. Mekanisk begränsning där. = Max vinkel för uppgång.
} else if (latest_distance <= 850) { // 800 < latest_distance <= 850
myservo.write(63);
} else if (...) {
...
} else if (latest_distance <= 2800) {
myservo.write(160);
} else { // latest_distance > 2800
myservo.write(165); // 165 är det högst som går. Mekanisk begränsning där. = Max vinkel för nedgång.
}
delay(1000); // Delay after updating servo position
}
... // toggle LED
}
It may also be worth considering using a mathematical function to calculate the servo position from the measured distance, if you don’t want a large if-else block of individual conditions.
As an example, if you wanted the servo angle evenly (linearly) spaced between physical limits, you could do something like:
... // includes and initialisation
// Mekanisk begränsningar
#define SERVO_MIN 55 // det minsta som går. = Max vinkel för uppgång.
#define SERVO_MAX 165 // det högst som går. = Max vinkel för nedgång.
// Distance thresholds
#define DISTANCE_MIN 800
#define DISTANCE_MAX 2800
uint32_t latest_distance;
uint16_t servo_angle;
... // setup
void loop() {
if (ping.update()) {
latest_distance = ping.distance();
Serial.println(latest_distance);
// servo angle determined as linear scale from distance thresholds
servo_angle = map(latest_distance, DISTANCE_MIN, DISTANCE_MAX, SERVO_MIN, SERVO_MAX);
// constrain servo angle to specified limits
servo_angle = constrain(servo_angle, SERVO_MIN, SERVO_MAX);
myservo.write(servo_angle);
delay(1000);
}
... // toggle LED
}
I finaly manage to solv that erratic servo. I tryed alot, changed coding, hardware, servo until
i found a solution. Insted of servo.h I used PWMServo.h and no erratic moves what so ever
(apparently some kind of conflict there).
And one question:
I am about to test one of your suggestions but I think I am doing something wrong here.
Does the “} else { …” lines look right? -It compiles okay, but I am not sure.
} else if (latest_distance <= 4254) {
myservo.write(162);
} else { latest_distance > 4290;
myservo.write(165); // Because of mechanical obstruction the highest number manageable.
}
delay(1000); // Delay after updating servo position
}
}
This expression gets evaluated without doing anything. else is a “catch-all” that corresponds to “everything that didn’t get caught by an earlier explicit condition in the if statement”. It would be equivalent to put something in a comment explaining what you expect that case to cover, e.g.
