//
// ServoEaser0ManualEasing.ino - show how to send manual easing commands
//
// 2011, TeamPneumo, Tod E. Kurt, http://todbot.com/blog/
//
//

#include "Servo.h"
#include "ServoEaser.h"

const int ledPin   = 13;
const int servoPin = 7;

int servoFrameMillis = 20;  // minimum time between servo updates

Servo servo1;
ServoEaser servoEaser;

//
void setup()
{
  Serial.begin(19200);
  Serial.println("ServoEaser0ManualEasing");

  // first, get the servo ready
  servo1.attach( servoPin );

  //Serial.println("moving to 90 degrees immediately");
  // begin with a framerate, no starting position, we don't know
  servoEaser.begin( servo1, servoFrameMillis );

  // do manual easing
  Serial.println("moving to 20 degrees over 2 seconds");
  servoEaser.easeTo( 20, 2000);
}

//
void loop()
{
  servoEaser.update();

  printCurrPos();

  if ( servoEaser.hasArrived() ) {
    Serial.println("ServoEaser arrived at 20 degrees,");
    Serial.println("going to 160 in 2 seconds");
    servoEaser.easeTo( 160, 2000 );
  }

}

void printCurrPos()
{
  static long nextPrintTime;

  if ( (long)(millis() - nextPrintTime) >= 0 ) {
    nextPrintTime += 200; // 100 millisecs between print statements
    Serial.print("currPos: ");
    Serial.println( servoEaser.getCurrPos() );
  }
}



Sweep.ino

diagram.json

//
// ServoEaser.h -- Arduino library for Servo easing
//
// ServoEaser is an Arduino library makes doing animation-style movements 
// with servos easier.
//
// Features:
// - Supports directly telling a servo to ease to a position over a duration
// - Supports playing of previously-defined "move lists"
// - User-specified easing functions (defaults to easeInOutCubic),see Easing lib
// - User-specified "servo arrived" function for async callback when move done
// - Supports both integer angle movement and a "microseconds" mode for 
//    fine-grained control with float-ing point angles
// - Works with both new Arduino 1.0 and Arduino 0022.
//
// Uses concepts from:
// -- http://portfolio.tobiastoft.dk/331886/Easing-library-for-Arduino
// -- http://jesusgollonet.com/processing/pennerEasing/
// -- http://robertpenner.com/easing/
// -- http://robertpenner.com/easing/easing_demo.html
//
// 2011, TeamPneumo, Tod E. Kurt, http://todbot.com/blog/
// 
//


#ifndef ServoEaser_h
#define ServoEaser_h

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include <Servo.h>

// simple struct to encapsulate a servo move 
typedef struct _servoMove {
    int pos;   // position of servo in degrees
    int dur;   // duration in milliseconds to get to and stay at that position
} ServoMove;


// define "EasingFunc" callback function
// you can make your own of these to use
// t: current time, b: beginning value, c: change in value, d: duration
// t and d can be in frames or seconds/milliseconds
//
typedef float (*EasingFunc)(float t, float b, float c, float d); 

// define "ArrivedFunc" to be called when servo arrives at position
// arguments provided are: currPos of servo & movesIndex of move list (if appl)
typedef void (*ArrivedFunc)(int currPos, int movesIndex);


class ServoEaser 
{
private:
    Servo *servo;      // what servo we're operating on
    int frameMillis;  // minimum update time between servo moves
    float startPos;   // where servo started its tween
    float currPos;    // current servo position, best of our knowledge

    float changePos;  // from servoMove list

    unsigned long durMillis;    // from servoMove list
    unsigned long startMillis; // time when we started

    unsigned long lastMillis; // time time we did something

    ServoMove* moves; // list of user-supplied servo moves
    int movesIndex;   // where in the moves list we are
    int movesCount;   // number of moves in the moves list
    int movesReps;    // number of repeats, 0==forever

    EasingFunc easingFunc; // func that describes tween motion
    ArrivedFunc arrivedFunc; // func to call when servo arrives at dest
    
    boolean running;  // is servo easer running?
    boolean arrived;  // has servo arrived at its destination
    boolean useMicros;// do math using float angles and microseconds 
    boolean flipped;  // is servo mounted upside down so 0-180 -> 180->0?

    void getNextPos();
    int angleToMicros(float angle);

    // duplicate Servo min/max micros functionality
    int8_t min;   // minimum is this value times 4 added to MIN_PULSE_WIDTH    
    int8_t max;   // maximum is this value times 4 added to MAX_PULSE_WIDTH   

public:
    
    // set up a servoeaser to use a particular servo
    //void begin(Servo s, int frameTime, int startPos);
    void begin(Servo& s, int frameTime); //, int startPos);
    void begin(Servo& s, int frameTime, ServoMove* moves, int movesCount);

    void reset();
    
    // tell a servo to play a list of servo moves
    // movesCount is number of moves in the list
    // movesReps is number of times to repeat
    void play( ServoMove* moves, int movesCount);
    void play( ServoMove* moves, int movesCount, int movesReps);
    void play( ServoMove* moves, int movesCount, int movesReps, int movesIndex);

    // for manual movement of the servo (non moves list)
    void easeTo( int pos, int durMillis );

    // get current position in degrees
    float getCurrPos();
    
    // call this periodically in loop()
    void update();

    // you can set your own easing function
    void setEasingFunc( EasingFunc );
    // can optionally set a function to call when servo arrives at dest
    void setArrivedFunc( ArrivedFunc func );

    // has servo arrived at its commaned move point?
    boolean hasArrived();
    // is the servo running (start/stop flag)
    boolean isRunning();

    void start();
    void stop();

    void setFlipped( boolean t );
    boolean isFlipped();

    // mirrors of Servos min/max values, set these if you need differnt
    // min/max microsecond bounds  (unfortunately, we can't get at 
    // Servo's values for these.  These values are only used if
    // you enable "useMicroseconds()"
    void setMinMaxMicroseconds( int min, int max );

    // enable fine-grained mode by doing servo.writeMicroseconds() 
    void useMicroseconds(boolean t);

    boolean usingMicroseconds();

};



#endif


ServoEaser.h

//
// ServoEaser.cpp -- Arduino library for Servo easing
//
// Uses concepts from:
// -- http://portfolio.tobiastoft.dk/331886/Easing-library-for-Arduino
// -- http://jesusgollonet.com/processing/pennerEasing/
// -- http://robertpenner.com/easing/
// -- http://robertpenner.com/easing/easing_demo.html
//
// 2011, TeamPneumo, Tod E. Kurt, http://todbot.com/blog/
// 
//

#include "ServoEaser.h"
#include <math.h>

#define DEBUG 0

// FIXME: this debug setup sucks
#if (DEBUG > 0)
#define debug_update() { \
        Serial.print("update: currPos:"); Serial.print(currPos);        \
        Serial.print(" startPos:"); Serial.print(startPos);             \
        Serial.print(" changePos:"); Serial.print(changePos);           \
        Serial.print(" startMillis:"); Serial.print(startMillis);                     \
        Serial.print(" arrived:");Serial.println(((arrived)?"true":"false"));}
#define debug_getnextpos() { \
        Serial.print("getNextPos:"); Serial.print(movesIndex); \
        Serial.print(" changePos:"); Serial.print(changePos);     \
        Serial.print(" moves[i].pos:"); Serial.print(moves[movesIndex].pos); \
        Serial.print(" durMillis:"); Serial.print(durMillis); \
        Serial.print(" startMillis:"); Serial.println(startMillis); }
#define debug_reset() { \
        Serial.print("reset: currPos:"); Serial.print(currPos); \
        Serial.print(" changePos:"); Serial.print(changePos); \
        Serial.print(" durMillis:"); Serial.print(durMillis); \
        Serial.print(" movesIndex:"); Serial.print(movesIndex); \
        Serial.print(" startMillis:"); Serial.println(startMillis); }
#else
#define debug_update() {}
#define debug_getnextpos() {}
#define debug_reset() {}
#endif

// default easing function
// this is from Easing::easeInOutCubic()
// t: current time, b: beginning value, c: change in value, d: duration
// t and d can be in frames or seconds/milliseconds
inline float ServoEaser_easeInOutCubic(float t, float b, float c, float d)
{
    if ((t/=d/2) < 1) return c/2*t*t*t + b;
	return c/2*((t-=2)*t*t + 2) + b;
}

// set up an easer with a servo and a moves list
void ServoEaser::begin(Servo& s, int frameTime, 
                       ServoMove* mlist, int mcount)
{
    begin( s, frameTime ); //, servo.read() );
    play( mlist, mcount );
}

// set up an easer with just a servo and a starting position
void ServoEaser::begin(Servo& s, int frameTime)
{
    servo = &s;
    frameMillis = frameTime;

    flipped = false;
    arrived = true;

    easingFunc = ServoEaser_easeInOutCubic;
    arrivedFunc = NULL;

    useMicros = false;

    movesIndex = 0;
    
    reset();

}

// reset easer to initial conditions, does not nuke easingFunc or arrivedFunc
void ServoEaser::reset()
{
    if (servo)
    {
        currPos = servo->read();
    }
    startPos = currPos;  // get everyone in sync
    changePos = 0;       // might be overwritten below

    if( movesCount > 0 ) {
        changePos = moves[ movesIndex ].pos - startPos ;
        durMillis = moves[ movesIndex ].dur;
    }

    startMillis = millis();
    
    debug_reset();
}

//
void ServoEaser::play( ServoMove* mlist, int mcount)
{
    play( mlist, mcount, 0, 0);
}
//
void ServoEaser::play( ServoMove* mlist, int mcount, int mreps )
{
    play( mlist, mcount, mreps, 0 );
}

void ServoEaser::play( ServoMove* mlist, int mcount, int mreps, int mindex)
{
    moves = mlist;
    movesCount = (mcount>0) ? mcount : 0;
    movesReps  = (mreps>0)  ? mreps  : 0;
    movesIndex = (mindex>0) ? mindex : 0;
    //movesIndex = (mindex>0) ? ((mindex<mcount) ? mindex : mcount) : 0; 

    running = true;
    arrived = false;

    reset();
}

// manual, non-moves list, control of easer position
void ServoEaser::easeTo( int pos, int dur )
{
    movesCount = 0;  // no longer doing moves list
    startPos = currPos;
    changePos = pos - startPos;
    durMillis = dur;
    startMillis = millis();
    arrived = false;
    running = true;
}

// used internally to select next servo position
void ServoEaser::getNextPos()
{
    unsigned long oldEndMillis = startMillis + durMillis;

    arrived = true;
    if( arrivedFunc != NULL ) { arrivedFunc( currPos, movesIndex ); }

    if( movesCount==0 ) { // not in a moves list
        return;
    }

    movesIndex++;
    if( movesIndex == movesCount ) {
        movesIndex = 0;  // loop
        movesReps--;
        if( movesReps == 0 ) { // we are done
            stop();
        } else if( movesReps == -1 ) { // we are infinite
            movesReps = 0;
        }
    }
    startPos  = currPos; // current position becomes new start position

    changePos = moves[ movesIndex ].pos - startPos ;
    durMillis = moves[ movesIndex ].dur;

    startMillis = oldEndMillis;
    arrived = false;

    debug_getnextpos();
}

// call this regularly in loop()
void ServoEaser::update()
{
    unsigned long currentMillis = millis();

    if( ((currentMillis - lastMillis) < frameMillis) || !running ) {  // time yet?
        return;
    }
    lastMillis = currentMillis;

    if (currentMillis > (startMillis + durMillis)) {  
        currPos = startPos + changePos; 
        getNextPos(); 
    }
    else {
        currPos = easingFunc( currentMillis - startMillis, startPos, changePos, durMillis );
    }
	
    debug_update();

    float p = (flipped) ? 180.0 - currPos : currPos;
    if ( servo )  {
        if( useMicros ) {
            servo->writeMicroseconds( angleToMicros(p) );
        } else {
            servo->write( p );
        }
    }
}

//
void ServoEaser::setMinMaxMicroseconds(int mi, int ma)
{
    min = mi;
    max = ma;
    useMicros = true;
}

// from Servo.cpp
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // min value in uS 
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // max value in uS

#define in_min 0
#define in_max 180
#define out_min SERVO_MIN()
#define out_max SERVO_MAX()
int ServoEaser::angleToMicros(float angle)
{
    //                 x   in_min,in_max, out_min, out_max
    //int value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
    return (angle-in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//
void ServoEaser::useMicroseconds(boolean t)
{
    useMicros = t;
}
//
boolean ServoEaser::usingMicroseconds()
{
    return useMicros;
}
void ServoEaser::setFlipped(boolean t)
{
    flipped = t;
}
boolean ServoEaser::isFlipped()
{
    return flipped;
}
//
void ServoEaser::start()
{
    if (!running)
    {
        startMillis += millis()-lastMillis;        
        running = true;
    }
}
//
void ServoEaser::stop()
{
    running = false;
}
//
boolean ServoEaser::isRunning()
{ 
    return running;
}
//
boolean ServoEaser::hasArrived()
{ 
    return arrived;
}

float ServoEaser::getCurrPos()
{
    return currPos;
}

// 
void ServoEaser::setEasingFunc( EasingFunc func )
{
    easingFunc = func;
}
//
void ServoEaser::setArrivedFunc( ArrivedFunc func ) 
{
    arrivedFunc = func;
}

//
/*
void ServoEaser::setMovesList( ServoMove* mlist, int mcount )
{
    moves = mlist;
    movesCount = mcount;

    reset();
}
*/

 



ServoEaser.cpp

# Wokwi Library List
# See https://docs.wokwi.com/guides/libraries

# Automatically added based on includes:
Servo
# ---
