├── .gitignore ├── README.md ├── SmoothServo.cpp ├── SmoothServo.h ├── SmoothServoProcessing ├── Easing.pde ├── Queue.pde ├── Servo.pde ├── ServoState.pde ├── ServoTask.pde ├── ServoTaskStatus.pde ├── SmoothServo.pde └── SmoothServoProcessing.pde ├── examples └── BasicMotion │ └── BasicMotion.ino └── readme ├── motion-example.gif └── motion-example.mov /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | cmake-build-debug/CMakeFiles 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SmoothServo 2 | The idea was to build an arduino library for smooth servo motion control. 3 | 4 | **IMPORTANT**: Only the *proof of concept* in **processing** has been implemented yet! 5 | 6 | ## Idea 7 | The idea behind SmoothServo is, to create a library which controls the motion of a servo. Every motion should be start and and with an easing to avoid inertial force. 8 | 9 | Every command to the servo is queued as task in a task list and needs a time to run and an easing curve. 10 | 11 | The task then is split up into three different tasks: 12 | 13 | * start (with easing) 14 | * motion (linear) 15 | * end (with easing) 16 | 17 | If now a new task is coming into the queue, but the servo is still moving, the motion task will bit canceled and the servo will run the end task. So there is never a fast stop of the servo. 18 | 19 | ![Example](readme/motion-example.gif) 20 | 21 | *SmoothServo simulation.* 22 | 23 | ## Interface 24 | 25 | There are advanced commands: 26 | 27 | * moveTo(int position, int time) 28 | * stop() 29 | * stop(int time) 30 | 31 | And more specific which are used by the library: 32 | 33 | * addTask(Task task) 34 | * cancelCurrentTask() 35 | * cancelAllTasks() 36 | * update() 37 | 38 | A task object has following attributes: 39 | 40 | * Type (Start, Motion, End) 41 | * Target : Int 42 | * Time : Int 43 | * State : Enum (Waiting, Running, Finished, Canceled) 44 | -------------------------------------------------------------------------------- /SmoothServo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SmoothServo.cpp - Library for smooth motion servo control. 3 | Created by Florian Bruggisser, June 12, 2017. 4 | Released into the public domain. 5 | */ 6 | 7 | #include 8 | #include "SmoothServo.h" 9 | 10 | 11 | SmoothServo::SmoothServo(Servo servo) 12 | { 13 | _servo = servo; 14 | } 15 | 16 | void SmoothServo::test() 17 | { 18 | delay(5000); 19 | } 20 | -------------------------------------------------------------------------------- /SmoothServo.h: -------------------------------------------------------------------------------- 1 | /* 2 | SmoothServo.cpp - Library for smooth motion servo control. 3 | Created by Florian Bruggisser, June 12, 2017. 4 | Released into the public domain. 5 | */ 6 | 7 | #ifndef SmoothServo_h 8 | #define SmoothServo_h 9 | 10 | #include 11 | 12 | class SmoothServo 13 | { 14 | public: 15 | SmoothServo(Servo servo); 16 | void test(); 17 | private: 18 | Servo _servo; 19 | }; 20 | 21 | #endif 22 | 23 | -------------------------------------------------------------------------------- /SmoothServoProcessing/Easing.pde: -------------------------------------------------------------------------------- 1 | // t = current time, b = start value, c = change in value, d = duration 2 | public static float easeInQuad(float t, float b, float c, float d) { 3 | t /= d; 4 | return c*t*t + b; 5 | } 6 | 7 | public static float linearTween (float t, float b, float c, float d) { 8 | return c*t/d + b; 9 | } 10 | 11 | public static float easeOutQuad (float t, float b, float c, float d) { 12 | t /= d; 13 | return -c * t*(t-2) + b; 14 | } 15 | 16 | public static float easeInSine (float t, float b, float c, float d) { 17 | return Math.round(-c * Math.cos(t/d * (PI/2)) + c + b); 18 | } 19 | 20 | public static float easeOutSine (float t, float b, float c, float d) { 21 | return Math.round(c * Math.sin(t/d * (PI/2)) + b); 22 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/Queue.pde: -------------------------------------------------------------------------------- 1 | public class Queue { 2 | 3 | private Object[] queue; // The underlying array 4 | private int size; // The maximal capacity 5 | private int head = 0; // Pointer to head of queue 6 | private int tail = 0; // Pointer to tail of queue 7 | private boolean empty = true; // Whether the queue is empty or not 8 | 9 | /** 10 | * Implements a generic FIFO queue with only the two basic 11 | * operations, enqueue and dequeue that inserts and retrieves 12 | * and element respectively. 13 | * @param size the number of elements the queue can maximally hold 14 | */ 15 | public Queue(int size) { 16 | this.queue = new Object[size]; 17 | this.size = size; 18 | } 19 | 20 | /** 21 | * Inserts an element into the queue. 22 | * @param elem the element to insert into the queue 23 | * @throws Exception when the queue is full 24 | */ 25 | public void enqueue(T elem) { 26 | // Check if the queue is full and throw exception 27 | if (head == tail && !empty) { 28 | println("Cannot enqueue " + elem); 29 | } 30 | 31 | // The queue has space left, enqueue the item 32 | queue[tail] = elem; 33 | tail = (tail + 1) % size; 34 | empty = false; 35 | } 36 | 37 | /** 38 | * Removes an element from the queue and returns it. 39 | * @throws Exception when the queue is empty 40 | */ 41 | public T dequeue() { 42 | // Check if queue is empty and throw exception 43 | if (empty) { 44 | println("The queue is empty"); 45 | } 46 | 47 | // The queue is not empty, return element 48 | T elem = (T) queue[head]; 49 | head = (head + 1) % size; 50 | empty = (head == tail); 51 | return elem; 52 | } 53 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/Servo.pde: -------------------------------------------------------------------------------- 1 | class Servo 2 | { 3 | int position = 90; 4 | 5 | public Servo() 6 | { 7 | } 8 | 9 | public void write(int position) 10 | { 11 | this.position = position; 12 | 13 | OscMessage msg = new OscMessage("/1/servo"); 14 | 15 | msg.add((float)map(position, 0, 180, 0, 1)); 16 | oscP5.send(msg, broadCast); 17 | } 18 | 19 | public void render() 20 | { 21 | fill(89, 171, 227); 22 | noStroke(); 23 | rectMode(CENTER); 24 | rotate(radians(90 + position)); 25 | rect(0, 50, 30, 100, 7); 26 | 27 | fill(89, 171, 227); 28 | noStroke(); 29 | ellipse(0, 0, 30, 30); 30 | 31 | noFill(); 32 | stroke(0); 33 | ellipse(0, 0, 15, 15); 34 | } 35 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/ServoState.pde: -------------------------------------------------------------------------------- 1 | enum ServoState 2 | { 3 | ACCELERATION, 4 | LINEARMOTION, 5 | DECELERATION, 6 | BRAKE 7 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/ServoTask.pde: -------------------------------------------------------------------------------- 1 | class ServoTask 2 | { 3 | ServoState state; 4 | ServoTaskStatus status = ServoTaskStatus.CREATED; 5 | 6 | float velocity; 7 | float acceleration; 8 | 9 | int startPosition; 10 | int targetPosition; 11 | 12 | float accelerationTarget; 13 | float linearMotionTarget; 14 | float decelerationTarget; 15 | 16 | float duration; 17 | int startTime; 18 | 19 | float accelerationTime; 20 | float linearMotionTime; 21 | float decelerationTime; 22 | 23 | float accelerationPath; 24 | float linearMotionPath; 25 | float decelerationPath; 26 | 27 | float brakeTarget; 28 | float brakeTime; 29 | float brakePath; 30 | 31 | int direction; 32 | 33 | boolean shouldBrake = false; 34 | 35 | public ServoTask(int targetPosition, float velocity, float acceleration) 36 | { 37 | this.targetPosition = targetPosition; 38 | this.velocity = velocity; 39 | this.acceleration = acceleration; 40 | 41 | this.state = ServoState.ACCELERATION; 42 | } 43 | 44 | public void start(int startPosition) 45 | { 46 | status = ServoTaskStatus.RUNNING; 47 | 48 | this.startPosition = startPosition; 49 | this.startTime = millis(); 50 | 51 | // caluclate time and path 52 | accelerationTime = calculateMotionTime(0, velocity, acceleration); 53 | decelerationTime = calculateMotionTime(velocity, 0, acceleration); 54 | 55 | accelerationPath = calculateMotionPath(0, accelerationTime, acceleration); 56 | decelerationPath = calculateMotionPath(velocity, decelerationTime, acceleration * -1); 57 | 58 | duration = 0; 59 | 60 | // calculate positions 61 | int pathLength = abs(targetPosition - startPosition); 62 | direction = getSign(targetPosition - startPosition); 63 | 64 | linearMotionPath = max(0, pathLength - (accelerationPath + decelerationPath)); 65 | linearMotionTime = linearMotionPath / velocity; 66 | 67 | // recalculate accelerationTime if no linear motion possible 68 | if (linearMotionPath == 0) 69 | { 70 | accelerationPath = pathLength / 2; 71 | decelerationPath = pathLength / 2; 72 | 73 | accelerationTime = calculateMotionTime(acceleration, accelerationPath); 74 | decelerationTime = calculateMotionTime(acceleration, decelerationPath); 75 | } 76 | 77 | duration = accelerationTime + linearMotionTime + decelerationTime; 78 | 79 | accelerationTarget = startPosition + (direction * accelerationPath); 80 | linearMotionTarget = accelerationTarget + (direction * linearMotionPath); 81 | decelerationTarget = linearMotionTarget + (direction * decelerationPath); 82 | 83 | println("New Task:"); 84 | 85 | println("acceleration: " + acceleration); 86 | println("maxVelocity: " + velocity); 87 | 88 | println("AccP: " + accelerationPath); 89 | println("LinP: " + linearMotionPath); 90 | println("DecP: " + decelerationPath); 91 | 92 | println("----"); 93 | 94 | println("AccT: " + accelerationTime); 95 | println("LinT: " + linearMotionTime); 96 | println("DecT: " + decelerationTime); 97 | 98 | println("----"); 99 | 100 | println("Start: " + startPosition); 101 | println("Target: " + targetPosition); 102 | 103 | println("AccS: " + accelerationTarget); 104 | println("LinS: " + linearMotionTarget); 105 | println("DecS: " + decelerationTarget); 106 | 107 | println("---"); 108 | println("duration: " + duration); 109 | } 110 | 111 | public void stop() 112 | { 113 | // difference to break start 114 | int position = nextPosition(0); 115 | float diffToBreak = linearMotionTarget - position; 116 | 117 | shouldBrake = true; 118 | 119 | if (state == ServoState.LINEARMOTION) 120 | { 121 | startTime = millis(); 122 | startPosition = position; 123 | brakeTime = calculateMotionTime(velocity, 0, acceleration); 124 | brakePath = calculateMotionPath(velocity, brakeTime, acceleration * -1); 125 | brakeTarget = startPosition + (direction * brakePath); 126 | 127 | state = ServoState.BRAKE; 128 | 129 | println("Stopping, current: " + position + " diff: " + diffToBreak); 130 | } 131 | } 132 | 133 | public int nextPosition(int currentPosition) 134 | { 135 | // fix for 0 or 1 motion 136 | if (abs(targetPosition - startPosition) < 2) 137 | { 138 | status = ServoTaskStatus.FINISHED; 139 | return targetPosition; 140 | } 141 | 142 | switch(state) 143 | { 144 | case ACCELERATION: 145 | return acceleration(); 146 | 147 | case LINEARMOTION: 148 | return linearMotion(); 149 | 150 | case DECELERATION: 151 | return deceleration(); 152 | 153 | case BRAKE: 154 | return brake(); 155 | } 156 | 157 | return 0; 158 | } 159 | 160 | // t = current time, b = start value, c = change in value, d = duration 161 | 162 | public int acceleration() 163 | { 164 | float t = millis() - startTime; 165 | float b = startPosition; 166 | float c = accelerationTarget - startPosition; 167 | float d = accelerationTime; 168 | 169 | // check state switch 170 | if (t >= d) 171 | { 172 | state = ServoState.LINEARMOTION; 173 | if (shouldBrake) 174 | { 175 | stop(); 176 | } 177 | } 178 | 179 | return Math.round(easeInQuad(t, b, c, d)); 180 | } 181 | 182 | public int linearMotion() 183 | { 184 | float t = millis() - (startTime + accelerationTime); 185 | float b = accelerationTarget; 186 | float c = linearMotionTarget - accelerationTarget; 187 | float d = linearMotionTime; 188 | 189 | // check state switch 190 | if (t >= d) 191 | { 192 | state = ServoState.DECELERATION; 193 | println("changing to deceleration state"); 194 | } 195 | 196 | if (d <= 0) 197 | { 198 | // skip linear motion 199 | return deceleration(); 200 | } 201 | 202 | return Math.round(linearTween(t, b, c, d)); 203 | } 204 | 205 | public int deceleration() 206 | { 207 | float t = millis() - (startTime + accelerationTime + linearMotionTime); 208 | float b = linearMotionTarget; 209 | float c = decelerationTarget - linearMotionTarget; 210 | float d = decelerationTime; 211 | 212 | // check state switch 213 | if (t >= d) 214 | { 215 | status = ServoTaskStatus.FINISHED; 216 | } 217 | 218 | return Math.round(easeOutQuad(t, b, c, d)); 219 | } 220 | 221 | public int brake() 222 | { 223 | float t = millis() - startTime; 224 | float b = startPosition; 225 | float c = brakeTarget - startPosition; 226 | float d = brakeTime; 227 | 228 | // check state switch 229 | if (t >= d) 230 | { 231 | status = ServoTaskStatus.FINISHED; 232 | } 233 | 234 | return Math.round(easeOutQuad(t, b, c, d)); 235 | } 236 | 237 | private int getSign(int n) 238 | { 239 | return n >= 0 ? 1 : -1; 240 | } 241 | 242 | private float calculateMotionTime(float vI, float vF, float a) 243 | { 244 | return Math.abs((vF - vI) / a); 245 | } 246 | 247 | private float calculateMotionPath(float vI, float t, float a) 248 | { 249 | return Math.round(((vI * t) + (0.5 * a * Math.pow(t, 2)))); 250 | } 251 | 252 | private float calculateMotionTime(float a, float d) 253 | { 254 | return Math.round(Math.sqrt(2 * d / a)); 255 | } 256 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/ServoTaskStatus.pde: -------------------------------------------------------------------------------- 1 | enum ServoTaskStatus 2 | { 3 | CREATED, 4 | RUNNING, 5 | FINISHED, 6 | CANCELED 7 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/SmoothServo.pde: -------------------------------------------------------------------------------- 1 | class SmoothServo 2 | { 3 | Queue tasks = new Queue(30); 4 | ServoTask task = null; 5 | 6 | Servo servo; 7 | 8 | int servoPosition; 9 | 10 | // max speed per seconds 11 | float maxVelocity; 12 | float maxAcceleration; 13 | 14 | public SmoothServo(Servo servo, float maxVelocity, float maxAcceleration) 15 | { 16 | this.servo = servo; 17 | this.maxVelocity = maxVelocity; 18 | this.maxAcceleration = maxAcceleration; 19 | 20 | // init servo to have same position 21 | servoPosition = 90; 22 | servo.write(servoPosition); 23 | } 24 | 25 | public void moveTo(int targetPosition) 26 | { 27 | moveTo(targetPosition, maxVelocity); 28 | } 29 | 30 | public void moveTo(int targetPosition, float velocity) 31 | { 32 | moveTo(targetPosition, maxVelocity, maxAcceleration); 33 | } 34 | 35 | public void moveTo(int targetPosition, float velocity, float acceleration) 36 | { 37 | tasks.enqueue(new ServoTask(targetPosition, velocity * maxVelocity, acceleration * maxAcceleration)); 38 | } 39 | 40 | public void stop() 41 | { 42 | if (task == null) 43 | return; 44 | 45 | task.stop(); 46 | } 47 | 48 | long start = 0; 49 | 50 | public void update() 51 | { 52 | // check if we need a new task from the queue 53 | if (task == null 54 | || task.status == ServoTaskStatus.FINISHED 55 | || task.status == ServoTaskStatus.CANCELED) 56 | { 57 | if (tasks.empty) 58 | return; 59 | 60 | // get new task from queue 61 | task = tasks.dequeue(); 62 | task.start(servoPosition); 63 | 64 | graph = new int[2000]; 65 | gp = 0; 66 | s1 = -1; 67 | s2 = -1; 68 | 69 | start = millis(); 70 | } 71 | 72 | // update task 73 | servoPosition = task.nextPosition(servoPosition); 74 | servo.write(servoPosition); 75 | 76 | if (s1 == -1 && task.state == ServoState.LINEARMOTION) 77 | s1 = gp; 78 | 79 | if (s2 == -1 && task.state == ServoState.DECELERATION) 80 | s2 = gp; 81 | 82 | graph[gp++] = servoPosition; 83 | 84 | // check if task is finished 85 | if (task.status == ServoTaskStatus.FINISHED || task.status == ServoTaskStatus.CANCELED) 86 | { 87 | println("took: " + (millis() - start) + "ms"); 88 | println("task finished!"); 89 | println("====> Difference: S: " + task.targetPosition + " I: " + servoPosition + " (" + (task.targetPosition - servoPosition) + ")"); 90 | task = null; 91 | } 92 | } 93 | } -------------------------------------------------------------------------------- /SmoothServoProcessing/SmoothServoProcessing.pde: -------------------------------------------------------------------------------- 1 | import oscP5.*; 2 | import netP5.*; 3 | 4 | Servo xAxis = new Servo(); 5 | SmoothServo smoothXAxis; 6 | 7 | int[] graph = new int[2000]; 8 | int gp = 0; 9 | 10 | int s1 = -1; 11 | int s2 = -1; 12 | 13 | OscP5 oscP5; 14 | NetAddress broadCast; 15 | 16 | void setup() 17 | { 18 | size(500, 500, FX2D); 19 | 20 | // setup osc 21 | oscP5 = new OscP5(this, 8000); 22 | 23 | broadCast = new NetAddress("172.20.10.2", 8000); 24 | 25 | // max speed = 180 ms per 60° -> 0.3333° per 1ms 26 | smoothXAxis = new SmoothServo(xAxis, 60.0 / 180.0, 0.02); 27 | } 28 | 29 | void draw() 30 | { 31 | background(255); 32 | 33 | // update smooth servo 34 | smoothXAxis.update(); 35 | 36 | // draw line 37 | stroke(0); 38 | strokeWeight(1); 39 | line(0, height / 2, width, height / 2); 40 | 41 | pushMatrix(); 42 | translate(width / 2, height / 2); 43 | xAxis.render(); 44 | popMatrix(); 45 | 46 | drawGraph(); 47 | 48 | // fps 49 | fill(0); 50 | text("Position: " + xAxis.position + "°", 20, 20); 51 | } 52 | 53 | void drawGraph() 54 | { 55 | float zero = height / 7 * 6; 56 | float maxH = zero - 100; 57 | float border = 20; 58 | float maxW = width - 2 * border; 59 | 60 | stroke(0); 61 | line(border, zero, width - border, zero); 62 | 63 | float lastX = border; 64 | float lastY = zero; 65 | 66 | for (int i = 0; i < gp; i++) 67 | { 68 | float x = map(i, 0, gp, border, width - border); 69 | float y = map(graph[i], 0, 180, zero, maxH); 70 | 71 | //draw line 72 | stroke(255, 0, 0); 73 | noFill(); 74 | line(lastX, lastY, x, y); 75 | 76 | stroke(255, 0, 0); 77 | noFill(); 78 | ellipseMode(CENTER); 79 | ellipse(x, y, 5, 5); 80 | 81 | if (i == s1 || i == s2) 82 | { 83 | // vertical line 84 | stroke(0, 255, 0); 85 | noFill(); 86 | line(x, zero - 100, x, zero); 87 | } 88 | 89 | lastX = x; 90 | lastY = y; 91 | } 92 | } 93 | 94 | void keyPressed() 95 | { 96 | if (key == ' ') 97 | { 98 | int target = (int)random(0, 180); 99 | println("Moving to " + target + "°"); 100 | smoothXAxis.moveTo(target, 0.25); 101 | } else 102 | { 103 | // stop 104 | smoothXAxis.stop(); 105 | } 106 | } -------------------------------------------------------------------------------- /examples/BasicMotion/BasicMotion.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | Servo myServo; 5 | SmoothServo servo(myServo); 6 | 7 | void setup() { 8 | servo.test(); 9 | } 10 | 11 | void loop() { 12 | 13 | } 14 | -------------------------------------------------------------------------------- /readme/motion-example.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cansik/smooth-servo/d2ef61d24d68019491f778ede912c0ceec47c66c/readme/motion-example.gif -------------------------------------------------------------------------------- /readme/motion-example.mov: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cansik/smooth-servo/d2ef61d24d68019491f778ede912c0ceec47c66c/readme/motion-example.mov --------------------------------------------------------------------------------