allowing servo to multitask with sensor

Projects Programming
1

Although I have previously gotten a similiar idea to work with a servo and millis the timing all worked out because the servo didn't have to spin unless it got a specific signal from an RC reciever. Now that I just want the servo to be constantly rotating back and forth I am having trouble getting it to move smoothly and not jitter. I'm pretty sure it's because the time is constantly telling the servo to move in both directions but I'm not sure what the best way to fix this problem would be. Any insight would be great.

#include <Servo.h>
#include <NewPing.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *drive = AFMS.getMotor(1);
Adafruit_DCMotor *steer = AFMS.getMotor(2);

#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define MAX_DISTANCE 75
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo ultraSonic;
int servoPin = A2;
int pos = 0;

unsigned long previousMillis = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis3 = 0;

long onTime = 1;
long onTime2 = 40;
long onTime3 = 40;


void setup() 
{
 AFMS.begin();
 Serial.begin(9600);
 drive->setSpeed(255);
 steer->setSpeed(255);
 ultraSonic.attach(servoPin);
}

void loop() 
{
 unsigned long currentMillis = millis();
 int distance = sonar.ping_in();
 Serial.print("Ping: ");
 Serial.print(distance);
 Serial.println("in");
 
 if(currentMillis - previousMillis2 > onTime2)
 { 
 for(pos = 20; pos < 110; pos++)
 {
  ultraSonic.write(pos);
 }
 previousMillis2 = currentMillis;
 }
 if(currentMillis - previousMillis3 < onTime3)
 {
  for(pos = 110; pos > 20; pos--)
  {
    ultraSonic.write(pos);
  }
  previousMillis3 = currentMillis;
 }
 if(distance >= 10) 
 {
  drive->run(FORWARD);
  steer->run(FORWARD);
   if(currentMillis - previousMillis2 > onTime2)
 { 
 for(pos = 20; pos < 110; pos++)
 {
  ultraSonic.write(pos);
 }
 previousMillis2 = currentMillis;
 }
 }
 if((distance < 10) && (distance > 0))
 {
  if(currentMillis - previousMillis >= onTime)
  {
  drive->run(BACKWARD);
  steer->run(BACKWARD);
  }
  previousMillis = currentMillis;
 }
if(distance == 0)
  {
  drive->run(FORWARD);
  steer->run(BACKWARD);
  } 
}
2

I'm pretty sure it's because the time is constantly telling the servo to move in both directions but I'm not sure what the best way to fix this problem would be. Any insight would be great.

Even if it worked then your current code would sweep the servo back and forth and only take a distance reading at one end of the travel

You need something like this

servo position = 20
increment = 1

start of loop()
  if it is time to move the servo
    add increment to the servo position
    move the servo to the new position
    take a sonar reading
    act on the sonar value
    if the servo position is 110 or 20
      increment = -increment
    end if
  end if
end of loop()
3

Have a look at Several Things at a Time which includes code to move a servo

...R