Closing a lid with Servo and For loop

Dan0 · June 6, 2018, 5:38pm

Hello again, I have a nice little project to open and close a waste paper bin lid with a servo and ultrasonic sensor

all works fine and as you can see in the vid I will upload later I have 3D printed the mechanism only of the bin to test it

I wanted the lid to open at full speed but slowly close, in fact get slower as it closes

my code for this however is a bit ugly and closing is a bit jumpy

is there a way to achieve the servo angle and delay both increasing incrementally until at the closed position with a For Loop?

#include <Servo.h>
#define trigpin 5//set trigpin
#define echopin 6//set echopin
Servo myservo;// declare servo name type servo
  int duration, distance;//declare variable for unltrasonic sensor
  
void setup() {
  Serial.begin(9600);
  pinMode(trigpin, OUTPUT);
 
  pinMode(echopin, INPUT);
  myservo.attach(2);// attach your servo
myservo.writeMicroseconds(150);
  // put your setup code here, to run once:
}
void loop() {
 myservo.write(180);//  set servo to starting position
//ultrasonic code 
 digitalWrite(trigpin,HIGH);
  _delay_ms(500);
  digitalWrite(trigpin, LOW);
  
  duration=pulseIn(echopin,HIGH); 
  distance=(duration/2)/29.1; 
 if(distance <=10)// if ultrasonic sensor detects an obstacle less than 10cm in 90 degree angle.
 {
 myservo.write(48); //servo rotates at full speed to open lid
 delay(10000); //waits 10 seconds
 myservo.write(50);//closes a step at a time
 delay(100);//starts to close fast
 myservo.write(55);
 delay(105);
 myservo.write(60);
 delay(110);
 myservo.write(65);
 delay(115);
 myservo.write(70);
 delay(120);
 myservo.write(75);
 delay(125);
 myservo.write(80);
 delay(130);
 myservo.write(85);
delay(135);
 myservo.write(90);
 delay(140);
 myservo.write(95);
 delay(145);
 myservo.write(100);
 delay(150);
 myservo.write(105);
 delay(155);
 myservo.write(110);
 delay(160);
 myservo.write(115);
 delay(165);
 myservo.write(120);
 delay(170);
 myservo.write(125);
 delay(175);
 myservo.write(130);
 delay(180);
 myservo.write(135);
 delay(185);
 myservo.write(140);
 delay(190);
 myservo.write(145);
 delay(195);
 myservo.write(150);
 delay(200);
 myservo.write(155);
 delay(205);
 myservo.write(160);
 delay(210);
 myservo.write(165);
 delay(220);
 myservo.write(170);
 delay(225);
 myservo.write(175);
 delay(230);//slows as its almost closed
 myservo.write(180);//closed
}
else
{

}
  Serial.print("cm"); //print distance unit cm
Serial.println(distance);//distance
 
}

regards

Dan

Danois90 · June 6, 2018, 5:57pm

Your code looks a bit confusing, what you want is probably a for loop with exponentially increasing delay:

#define CLOSED_POSITION 180
#define OPEN_POSITION 48
#define CLOSE_TIMEOUT 10000

bool open = false;
unsigned long openMillis;

void setup()
{
  //Fill me up!
}

bool detectObstacle()
{
  //Return true if obstacle is detected
}

void loop()
{
  if (detectObstacle())
  {
    open = true;
    openMillis = millis();
    myservo.write(OPEN_POSITION);
  }
  else if (open && (millis() - openMillis >= CLOSE_TIMEOUT))
  {
    open = false;
    float dd = 100.0f;
    for (int i = OPEN_POSITION; i <= CLOSED_POSITION; i++)
    {
      myservo.write(i);
      delay(round(dd));
      dd *= 1.05f; //Exponentially increase delay, try different values
    }
  }
}

The bucket stays open while obstacle is detected, when no longer detected it will close after a certain amount of timeout.

Dan0 · June 6, 2018, 6:02pm

unable to upload the vid

the ultrasonic sensor will be on top of the lid so it will not be detecting therefore the 10second delay

the rest of the delays are to close the lid, would be better if I could upload a short vid of it in action

Dan

Danois90 · June 6, 2018, 6:05pm

Upload the video to youtube or something. You can disable the constant obstacle detection by altering the line "if (detectObstacle())" to "if (!open && detectObstacle())"

Robin2 · June 6, 2018, 6:08pm

If you use Servo.writeMicroseconds() you can move the servo arm in much smaller steps.

...R

Dan0 · June 6, 2018, 7:07pm

Robin2:
If you use Servo.writeMicroseconds() you can move the servo arm in much smaller steps.

...R

ahha thanks, good idea, that may require many more lines of code with delays though hence my idea of using "For"

UKHeliBob · June 6, 2018, 7:09pm

that may require many more lines of code with delays though

Have you looked at the Servo Sweep example ?

Dan0 · June 6, 2018, 7:13pm

Danois90:
Upload the video to youtube or something. You can disable the constant obstacle detection by altering the line "if (detectObstacle())" to "if (!open && detectObstacle())"

can't be arsed to subscriber to YouTube and upload just for a vid, thats another distraction I don't have time for

the obstacle detection will not be a factor, the ultrasonic sensor will be in the lid of the bin so as it tilts up it will not be detecting anything apart from the ceiling

the body of the bin is split down the middle enabling removal of the "tray" and emptying so the sensor cannot be on the from of the bottom section

Dan

Dan0 · June 6, 2018, 7:19pm

here is the first itteration in CAD with the section I have printed for testing. needs some modification obviously

servo fits exactly at the back and links with some stainless wire

Robin2 · June 6, 2018, 7:25pm

Please display your image(s) in your post so we can see it(them) without downloading it(them). See this Simple Image Guide

...R

PaulMurrayCbr · June 7, 2018, 7:00am

enum LidState {
CLOSED,  OPEN, CLOSING
} lidState = CLOSED;

const int  CLOSED_POSITION = 180;
const int OPEN_POSITION = 48;
const uint32_t CLOSE_TIMEOUT = 10000;
const uint32_t HOLD_OPEN_TIMEOUT = 2000;

uint32_t stateMs; // the millis() at which time we entered this state
uint32_t closingMs; // the millis() when we last updated the lid during closing

loop() {
  check the ultrasonic;

  if(we got incoming) {
    myservo.write(OPEN_POSITION);
    lidState = OPEN;
    stateMs = millis();
  }
  else {
    // we don't got incoming
    switch(lidState) {
    case CLOSED:
      // do nothing;
      break;

    case OPEN:
      if(millis()-stateMs < HOLD_OPEN_TIMEOUT) {
        // leave the lid open for a bit;
        return;
      }
      else {
        // time to start the closing sequence.
        lidState = CLOSING;
        // leave the servo where it is, because it is already where it should be for closing
        stateMs = millis();
        closingMs = stateMs; // treat "now" as the time when we last updated the closing lid
      }
      break;

      case CLOSING:
        if(millis()-stateMs < CLOSE_TIMEOUT) {
          handle_closing_sequence();
        }
        else {
          // make sure the lid is fully closed
          myservo.write(CLOSED_POSITION);
          lidState = CLOSED;
        }
        break;
    }
  }
}

void handle_closing_sequence() {
  // I don't want to spam the servo with updates, so 
  // I'll only update the closing position every 10th of a second.
  // this may need to be shorter, if the movement is jerky

  if(millis() - closingMs < 100) return;

  closingMs = millis();
  double closedAmount = (double)(closingMs - stateMs) / (double)CLOSE_TIMEOUT;
  if(closedAmount >= 1) return; // this can happen in edge cases

  // I want the lid to slowly close towards the end. Since closedAmount varies between 0 and 1,
  // I can do this by squaring the inverse - the open amount. This will give a constant 
  // deceleration as the lid closes. There are other ways.
  // You can use a cubic to get a soft start and end to the movement, or even a quintic 
  // to eliminate 'jerk'

  double openAmount = 1-closedAmount;
  openAmount = openAmount * openAmount;

  myservo.write(
    CLOSED_POSITION + openAmount * (OPEN_POSITION-CLOSED_POSITION)
  );
}

PaulMurrayCbr · June 7, 2018, 7:52am

The method outlined above is equivalent to:

  closedAmount = -closedAmount*closedAmount + closedAmount*2;
  myservo.write(
    OPEN_POSITION + closedAmount * (CLOSED_POSITION-OPEN_POSITION)
  );

A logistical curve is infinitely differentiable and so will give an infinitely soft start and end to the motion. But it would take infintely long to fully close the lid.

Giving bigger starting and ending X values in the code below gives less jerk at the start and the end, at the price of the lid having to move faster in the center range of its motion. You'd want to increase them to the point where the lid is moving as fast as the servo can move it in the center of its motion, with the constraint that it all has to happen within CLOSING_TIME millis. In this example, I give a bigger value for the ending X so as to make the end of the motion smoother.

#define LOGISTICAL(x) (exp(x)/(exp(x)+1))

// pecomputed constants
const double STARTING_X =  -3;
const double ENDING_X =  4;
const double STARTING_Y = LOGISTICAL(STARTING_X);
const double ENDING_Y = LOGISTICAL(ENDING_X);

// code in handle_closing_sequence
  double closedAmount = (double)(closingMs - stateMs) / (double)CLOSE_TIMEOUT;
  double x = STARTING_X + closedAmount * (ENDING_X-STARTING_X);
  double y = LOGISTICAL(x);

  // convert to a 0-1 value, 
  y = (y-STARTING_Y)/(ENDING_Y-STARTING_Y);

  // convert to servo position
  int position = OPEN_POSITION + y * (CLOSED_POSITION-OPEN_POSITION);
  myservo.write(position);

Dan0 · June 7, 2018, 7:55am

UKHeliBob:
Have you looked at the Servo Sweep example ?

I did, I think that is where I got the idea for the code I used, its still pretty jumpy though, I will take another look at it

Dan

Dan0 · June 7, 2018, 8:15am

Robin2:
Please display your image(s) in your post so we can see it(them) without downloading it(them). See this Simple Image Guide

...R

I dont have anywhere to host images on a website

Robin2 · June 7, 2018, 8:35am

Dan0:
I dont have anywhere to host images on a website

Your images are already hosted on this website as you have added them to your Reply #8

...R

Dan0 · June 7, 2018, 8:37am

PaulMurrayCbr:
enum LidState {

CLOSED, OPEN, CLOSING
} lidState = CLOSED;

const int CLOSED_POSITION = 180;
const int OPEN_POSITION = 48;
const uint32_t CLOSE_TIMEOUT = 10000;
const uint32_t HOLD_OPEN_TIMEOUT = 2000;

uint32_t stateMs; // the millis() at which time we entered this state
uint32_t closingMs; // the millis() when we last updated the lid during closing

loop() {
check the ultrasonic;

if(we got incoming) {
myservo.write(OPEN_POSITION);
lidState = OPEN;
stateMs = millis();
}
else {
// we don't got incoming
switch(lidState) {
case CLOSED:
// do nothing;
break;

case OPEN:
if(millis()-stateMs < HOLD_OPEN_TIMEOUT) {
// leave the lid open for a bit;
return;
}
else {
// time to start the closing sequence.
lidState = CLOSING;
// leave the servo where it is, because it is already where it should be for closing
stateMs = millis();
closingMs = stateMs; // treat "now" as the time when we last updated the closing lid
}
break;

case CLOSING:
if(millis()-stateMs < CLOSE_TIMEOUT) {
handle_closing_sequence();
}
else {
// make sure the lid is fully closed
myservo.write(CLOSED_POSITION);
lidState = CLOSED;
}
break;
}
}
}

void handle_closing_sequence() {
// I don't want to spam the servo with updates, so
// I'll only update the closing position every 10th of a second.
// this may need to be shorter, if the movement is jerky

if(millis() - closingMs < 100) return;

closingMs = millis();
double closedAmount = (double)(closingMs - stateMs) / (double)CLOSE_TIMEOUT;
if(closedAmount >= 1) return; // this can happen in edge cases

// I want the lid to slowly close towards the end. Since closedAmount varies between 0 and 1,
// I can do this by squaring the inverse - the open amount. This will give a constant
// deceleration as the lid closes. There are other ways.
// You can use a cubic to get a soft start and end to the movement, or even a quintic
// to eliminate 'jerk'

double openAmount = 1-closedAmount;
openAmount = openAmount * openAmount;

myservo.write(
CLOSED_POSITION + openAmount * (OPEN_POSITION-CLOSED_POSITION)
);
}

wow that looks interesting, I will give that a go and see what the result is like, I need to read it a few more times to try to follow what happening

Dan

Dan0 · June 7, 2018, 8:47am

PaulMurrayCbr:
The method outlined above is equivalent to:
  closedAmount = -closedAmount*closedAmount + closedAmount*2;

myservo.write(
OPEN_POSITION + closedAmount * (CLOSED_POSITION-OPEN_POSITION)
);




A logistical curve is infinitely differentiable and so will give an infinitely soft start and end to the motion. But it would take infintely long to fully close the lid.

Giving bigger starting and ending X values in the code below gives less jerk at the start and the end, at the price of the lid having to move faster in the center range of its motion. You'd want to increase them to the point where the lid is moving as fast as the servo can move it in the center of its motion, with the constraint that it all has to happen within CLOSING_TIME millis. In this example, I give a bigger value for the ending X so as to make the end of the motion smoother.

#define LOGISTICAL(x) (exp(x)/(exp(x)+1))

// pecomputed constants
const double STARTING_X = -3;
const double ENDING_X = 4;
const double STARTING_Y = LOGISTICAL(STARTING_X);
const double ENDING_Y = LOGISTICAL(ENDING_X);

// code in handle_closing_sequence
double closedAmount = (double)(closingMs - stateMs) / (double)CLOSE_TIMEOUT;
double x = STARTING_X + closedAmount * (ENDING_X-STARTING_X);
double y = LOGISTICAL(x);

// convert to a 0-1 value,
y = (y-STARTING_Y)/(ENDING_Y-STARTING_Y);

// convert to servo position
int position = OPEN_POSITION + y * (CLOSED_POSITION-OPEN_POSITION);
myservo.write(position);

OK will try them both and report back

Dan0 · June 7, 2018, 9:24am

Robin2:
Your images are already hosted on this website as you have added them to your Reply #8

...R

Ahha, cool. thanks for that

UKHeliBob · June 7, 2018, 9:25am

Dan0:
I did, I think that is where I got the idea for the code I used, its still pretty jumpy though, I will take another look at it

Dan

As suggested earlier in the thread, try using writeMicroseconds() in the for loop instead of write(). It gives you better granularity of movement.

Danois90 · June 7, 2018, 12:33pm

Did you try my previous sollution? Another sollution (only tested in a Linux terminal) could be:

#define OPENED_ANGLE 48
#define CLOSED_ANGLE 180

#define SERVO_MIN 1000
#define SERVO_MAX 2000

const int CLOSED_POS = SERVO_MIN + round((float)(SERVO_MAX - SERVO_MIN) / 180.0f * CLOSED_ANGLE);
const int OPENED_POS = SERVO_MIN + round((float)(SERVO_MAX - SERVO_MIN) / 180.0f * OPENED_ANGLE);

void delayMicros(unsigned long value)
{
   delayMicroseconds(value % 1000);
   delay(value / 1000);
}

void closeBucket()
{
   float dd = 10.0f; //Starting delay, this could be a different value
   for (int i = OPENED_POS; i <= CLOSED_POS; i++)
   {
     Servo.writeMicroseconds(i);
     delayMicros(round(dd));
     dd *= 1.012f; //Exponential growth, fiddle with this until you get the right result
   } 
}

Try it and see how it works

Topic		Replies	Views
Can someone explain to me why my code isnt working? Project Guidance	16	479	July 27, 2023
Self Opening Box using Servo Motor and Ultrasonic Sensor Programming Questions	10	2360	May 5, 2021
Cooldown on servo motor Project Guidance	11	81	May 5, 2024
Problems with programming ultra sensor to servo. Programming Questions	8	1005	May 5, 2021
Loop upon powering. Sensors	4	463	May 6, 2021

Closing a lid with Servo and For loop

Related Topics