mart256, thanks!
I tried it, but I get the same result.
Here's my sketch:
// this sketch moves a robot vehicle forward,
// using a 9110 motor driver chip and
// sweeping a servo (connected to pin 9) back and forth
// when the sensor detects an obstacle, the robot turns away and continues to move forward
//========================================
// ----------LIBRARIES--------------
#include <Servo.h>
#include <NewPing.h>
// --------CONSTANTS (won't change)---------------
const int servoPin = 9; // the pin number for the servo signal
const int servoMinDegrees = 0; // the limits to servo movement
const int servoMaxDegrees = 180; //speeder position numbers are on the left end of the sweep
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 11 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
const int AIA = 3; // (pwm) pin 9 connected to pin A-IA (left motor fwrd)
const int AIB = 5; // (pwm) pin 5 connected to pin A-IB (left motor bkwd)
const int BIA = 4; // (pwm) pin 10 connected to pin B-IA (right motor fwrd)
const int BIB = 6; // (pwm) pin 6 connected to pin B-IB (right motor bkwd)
byte speed = 250; // change this (0-255) to control the speed of the motors
//------------ VARIABLES (will change)---------------------
Servo myservo; // create servo object to control a servo
int Distance = 0;
int servoPosition = 90; // the current angle of the servo - starting at 90.
//int servoSlowInterval = 10; // millisecs between servo moves in two-speed case
//int servoFastInterval = 10; // millisecs between servo moves in two-speed cas
int servoInterval = 5; //this sets the constant sweep speed if not using the two-speed feature
int servoDegrees = 1; // amount servo moves at each step
// will be changed to negative value for movement in the other direction
unsigned long currentMillis = 0; // stores the value of millis() in each iteration of loop()
unsigned long previousServoMillis = 0; // the time when the servo was last moved
//========================================
void setup() {
//Serial.begin(115200);
// Serial.println("Starting Servo Sweep Using Millis function"); // so we know what sketch is running
pinMode(TRIGGER_PIN, OUTPUT); // set up sensor pins
pinMode(ECHO_PIN, INPUT);
myservo.write(servoPosition); // sets the initial position
myservo.attach(servoPin);
pinMode(AIA, OUTPUT); // set motor pins to output
pinMode(AIB, OUTPUT);
pinMode(BIA, OUTPUT);
pinMode(BIB, OUTPUT);
digitalWrite(3, speed); //start both motors forward
digitalWrite(4, speed);
}
//========================================
void loop() { //Notice that none of the action happens in loop() apart from reading millis()
// it just calls the functions that have the action code
currentMillis = millis(); // capture the latest value of millis()
servoSweep();
}
//============Subroutines ========================================
void forward()
{
digitalWrite(AIA, speed);
digitalWrite(AIB, 0);
digitalWrite(BIA, speed);
digitalWrite(BIB, 0);
}
void backward()
{
digitalWrite(AIA, 0);
digitalWrite(AIB, speed);
digitalWrite(BIA, 0);
digitalWrite(BIB, speed);
}
void rightSpin()
{
digitalWrite(AIA, speed);
digitalWrite(AIB, 0);
digitalWrite(BIA, 0);
digitalWrite(BIB, speed);
}
void leftSpin()
{
digitalWrite(AIA, 0);
digitalWrite(AIB, speed);
digitalWrite(BIA, speed);
digitalWrite(BIB, 0);
}
void rightSlide()
{
digitalWrite(AIA, speed);
digitalWrite(AIB, 0);
digitalWrite(BIA, 0);
digitalWrite(BIB, 0);
}
void leftSlide()
{
digitalWrite(AIA, 0);
digitalWrite(AIB, 0);
digitalWrite(BIA, speed);
digitalWrite(BIB, 0);
}
void fullStop()
{
digitalWrite(AIA, 0);
digitalWrite(AIB, 0);
digitalWrite(BIA, 0);
digitalWrite(BIB, 0);
}
void servoSweep() {
if (currentMillis - previousServoMillis >= servoInterval) { // its time for another move
previousServoMillis += servoInterval; // update
servoPosition = servoPosition + servoDegrees; // servoDegrees might be negative
if ((servoPosition >= servoMaxDegrees) || (servoPosition <= servoMinDegrees)) { // if the servo is at either extreme change the sign of the degrees to make it move the other way
servoDegrees = - servoDegrees; // reverse direction
servoPosition = servoPosition + servoDegrees; // and update the position to ensure it is within range
}
if (servoPosition == 1) { // if servo facing right side
//int Distance = 199; // Distance must never equal 0
Distance = sonar.ping_cm(); // ping
if (Distance !=0) { // if Non-zero value for Distance
if (Distance < 30) { // If object is close
//Serial.print("object within "); //output to monitor for debugging
//Serial.print (Distance);
//Serial.println (" cm on right");
fullStop();
delay(50);
//Serial.println (" stopped ");
leftSlide();
delay(3000);
//Serial.println (" sliding left ");
forward();
//Serial.println (" moving forward ");
}
}
}
if (servoPosition == 90) { // ping if servo facing front
//int Distance = 199;
Distance = sonar.ping_cm();
if (Distance !=0) {
//If object is within 30 cm, avoid
if (Distance < 30) {
//Serial.print("object within ");
//Serial.print (Distance);
//Serial.println (" cm in front ");
fullStop();
delay(50);
backward();
delay(1000);
leftSpin();
delay(2000);
forward();
}
}
}
if (servoPosition == 179) { // ping if servo facing left side
//int Distance = 199;
Distance = sonar.ping_cm();
if (Distance !=0) {
//If object is within 30 cm, avoid
if (Distance < 30) {
//Serial.print("object within ");
// Serial.print (Distance);
// Serial.println (" cm on left");
fullStop();
delay(50);
rightSlide();
delay(3000);
forward();
}
}
}
myservo.write(servoPosition); // make the servo move to the next position
// and record the time when the move happened (??)
// Serial.println(servoPosition);
}
}
//========================================END
