Making Servo Run for A Fixed Period of Time after If Statement

[sam_97w]

Hi there, I'm looking for some help with a program I'm trying to create.

I'm trying to set up a program to have my servo motor run for 5 seconds once my proximity sensor senses something at or closer than 100cm. Once the proximity sensor condition has been met, I want my MovePlayServo() function to activate for 5 seconds and stay active for the whole 5 seconds regardless of the sensor condition during the 5 second interval.

So the object can move out of the way after it triggers the sensor and the motor will still run for the full 5 seconds. After the 5 seconds, if the proximity sensor no longer senses the object the servo will not run otherwise it will run for another 5 second interval. Clearly I am going to need an if statement and some way to track time using millis(), but not really sure how to implement this cohesively.

Below is the program I have so far, I'm stuck at the loop section. Any help is much appreciated, thank you!

#include<Servo.h>
Servo PlayServo;
Servo TreatServo;
int PlayServoPosition;
int TreatServoPosition; 
const int Trig = 13; // Proximity sensor trigger pin mapped to input 13 
const int Echo = 12; // Proximity sensor echno pin mapped to input 12 
int Duration;
int Distance;
unsigned long CurrentTime = millis();
bool PlayServoRunning;  
  
void setup()
{
Serial.begin(9600); // Begin serial monitor 
pinMode(Trig,OUTPUT); // Proximity sensor trigger pin mapped as output
pinMode(Echo, INPUT); // Proximity sensor echno pin mapped as input

}

void loop()
{
  if (Distance <= 100) 
  
}

int MovePlayServo(){
 	PlayServoRunning = true; 
  	PlayServo.attach(5); // Attach PlayServo to input 5
 	PlayServoPosition=random(0,180); 
 	PlayServo.write(PlayServoPosition);
 	Serial.print(PlayServoPosition); 
 	Serial.println(" degrees");  
  	delay(1000);
}
  

int MoveTreatServo(){
	TreatServo.attach(6); // Attach PlayServo to input 5
  	TreatServo.write(60);
  	delay(1000);
    TreatServo.write(0);
 	Serial.print(TreatServoPosition); 
 	Serial.println(" degrees");  
}
  
int ReadDistance(){
  digitalWrite(Trig, HIGH);
  delay(1);
  digitalWrite(Trig, LOW);
  Duration = pulseIn(Echo, HIGH);
  Distance = Duration / 58.2;
  Serial.print(Distance);
  Serial.println(" cm"); 
  delay(1000);
}

[anon57585045]

Rephrasing it should help. The motor should run anytime the sensor condition was met, less than 5 seconds ago.

So, anytime the condition is not met, record the time.
Anytime the diff between the current time and that time exceeds 5 seconds, stop the motor. Else run the motor.

[StefanL38]

measured distance is bigger than 100 cm: do nothing

measured distance is less than 100 cm switch motor on

if motor is switched on and measured distance is more than 100 cm: start measuring time

if motor is switched on and measuring time has started: check if 5 seconds have passed by
if motor is switched on and measured distance is less than 100 cm keep motor switched on

those two conditions can be merged into updating a timestamp

if motor is on and measured distance is less than 100 cm: update timestamp
continiously updateing the timestamp keeps the condition false:

if motor is switched on and more than 5 seconds have passed by since timestamp was updated

example numbers
1:42 dist 99 cm timestamp 1:42
1:43 dist 98 cm timestamp 1:43
1:44 dist 95 cm timestamp 1:44
1:45 dist 97 cm timestamp 1:45
1:46 dist 101 cm timestamp 1:45 (dist > 100 ==> no updating of the timestamp
1:47 dist 105 cm timestamp 1:45 (dist > 100 ==> no updating of the timestamp
1:48 dist 103 cm timestamp 1:45 (dist > 100 ==> no updating of the timestamp
1:49 dist 108 cm timestamp 1:45 (dist > 100 ==> no updating of the timestamp
1:50 dist 102 cm timestamp 1:45 (dist > 100 ==> no updating of the timestamp

actualtime - timestamp >= 5
if motor is switched on and more than 5 seconds have passed by: switch motor off

1:50 - 1:45 = 6 ==> stop motor

so everything boils down to
measured distance is less than 100 cm:
switch motor on
update timestamp

if (motor switched on and actual time - timestamp >= 5:
switch motor off

switch servo-motor off is an inadequate description of what the code does
anyway

#include<Servo.h>
Servo PlayServo;
Servo TreatServo;
int PlayServoPosition;
int TreatServoPosition;
const int Trig = 13; // Proximity sensor trigger pin mapped to input 13
const int Echo = 12; // Proximity sensor echno pin mapped to input 12
int Duration;
int Distance;
unsigned long CurrentTime = millis();
unsigned long StartTime = 0;
unsigned long PlayServoTimer = 0;
unsigned long TreatServoTimer = 0;
unsigned long PrinDistanceTimer = 0;

int TreatServoPos = 60;
bool PlayServoRunning;


boolean TimePeriodIsOver (unsigned long &expireTime, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - expireTime >= TimePeriod ) {
    expireTime = currentMillis; // set new expireTime
    return true;                // more time than TimePeriod) has elapsed since last time if-condition was true
  }
  else return false;            // not expired
}



void setup() {
  Serial.begin(9600); // Begin serial monitor
  Serial.println("Setup-Start");
  pinMode(Trig, OUTPUT); // Proximity sensor trigger pin mapped as output
  pinMode(Echo, INPUT); // Proximity sensor echno pin mapped as input
  PlayServo.attach(5); // Attach PlayServo to input 5
  TreatServo.attach(6); // Attach PlayServo to input 5
}

void loop() {
  ReadDistance();
  if (Distance <= 100) {  // whenever distance is smaller than 100
    StartTime = millis(); // update TiemStamp
  }

  if ( TimePeriodIsOver(StartTime, 5000) == false ) { // as long as not 5 seconds since last update of TimeStamp have passed by
    MovePlayServo();  // keep servos moving
    MoveTreatServo();
  }
}


int MovePlayServo() {
  PlayServoRunning = true;
  // only once every 1000 milliseconds
  if ( TimePeriodIsOver(PlayServoTimer, 1000) ) {
    PlayServoPosition = random(0, 180); // move to a new servo position
    PlayServo.write(PlayServoPosition);
    Serial.print(PlayServoPosition);
    Serial.println(" degrees");
  }
}


int MoveTreatServo() {
  // only once every 1000 milliseconds
  if (TimePeriodIsOver(TreatServoTimer, 1000) ) {
    if (TreatServoPos == 0) { // toggle between
      TreatServoPos = 60;     // 60 degress
    }
    else {
      TreatServoPos = 0;
    }
    TreatServo.write(TreatServoPos);    // zero degrees
    Serial.print(TreatServoPosition);
    Serial.println(" degrees");
  }
}

int ReadDistance() {
  digitalWrite(Trig, HIGH);
  delay(1);
  digitalWrite(Trig, LOW);
  Duration = pulseIn(Echo, HIGH);
  Distance = Duration / 58.2;
  // only once every 1000 milliseconds
  if ( TimePeriodIsOver(PrinDistanceTimer, 1000) ) {
    Serial.print(Distance);
    Serial.println(" cm");
  }
}

best regards Stefan

[johnwasser]

int MoveTreatServoForFiveSeconds()
{
  TreatServo.attach(6); // Attach TreatServo to pin 6
  for (int i=0; i < 5; i++)
  {
     TreatServo.write(60);
     delay(1000);
     TreatServo.write(0);
      // Without a delay here, the servo will probably
      // never reach position 0 before it's told to go
      // back to position 60, until the 5 cycles are over.
  }
  // These lines are useless since 'TreatServoPosition' is never set.
  // Serial.print(TreatServoPosition); 
  // Serial.println(" degrees");  
}

[/quote]