├── .gitignore
├── images
├── motionprofile_const.png
├── motionprofile_trapezoidal.png
└── motionprofile_in_experiment.png
├── LICENSE
├── README.md
├── cpp
├── MotionProfile.h
└── MotionProfile.cpp
└── Matlab
└── MotionProfileClass.m
/.gitignore:
--------------------------------------------------------------------------------
1 | .idea
--------------------------------------------------------------------------------
/images/motionprofile_const.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WRidder/MotionProfileGenerator/HEAD/images/motionprofile_const.png
--------------------------------------------------------------------------------
/images/motionprofile_trapezoidal.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WRidder/MotionProfileGenerator/HEAD/images/motionprofile_trapezoidal.png
--------------------------------------------------------------------------------
/images/motionprofile_in_experiment.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WRidder/MotionProfileGenerator/HEAD/images/motionprofile_in_experiment.png
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | The MIT License (MIT)
2 |
3 | Copyright (c) 2015 Wilbert van de Ridder
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
23 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Motion profile generator
2 | A library which generates a motion profile (trapezoidal or constant) to reach a given setpoint while adhering to maximum velocity and acceleration settings. The generator is able to both calculate a complete path beforehand and generating it on the fly based on the current position and velocity.
3 |
4 | Matlab and cpp (Arduino specific) libraries are available. It should be relatively easy to port the cpp version to other platforms.
5 |
6 | ## Features
7 | * On the fly profile generation
8 | * Supports Trapezoidal and Constant motion profiles
9 |
10 | ## Usage
11 | ```cpp
12 | #include "MotionProfile.h"
13 |
14 | /**
15 | * Initialization
16 | *
17 | * @param int aVelocityMax maximum velocity (units/s)
18 | * @param int aAccelerationMax maximum acceleration (units/s^2)
19 | * @param short aMethod method of profile generation (1 = trapezoidal)
20 | * @param int aSampleTime sample time (ms)
21 | */
22 | MotionProfile trapezoidalProfile = new MotionProfile(200, 100, 1, 10);
23 |
24 | /**
25 | * Usage
26 | */
27 | // Update setpoint for profile calculation and retrieve calculated setpoint
28 | float finalSetpoint = 1000;
29 | float setpoint = trapezoidalProfile->update(finalSetpoint)
30 |
31 | // Check if profile is finished
32 | if (trapezoidalProfile->getFinished()) {};
33 |
34 | // Reset internal state
35 | trapezoidalProfile->reset();
36 | ```
37 |
38 | ## Example graphs
39 | ### Trapezoidal motion profile
40 | 
41 | Limit velocity at 0.15 units/s and acceleration at 0.1 units/s^2.
42 |
43 | ### Constant motion profile
44 | 
45 | Limit velocity at 0.15 units/s.
46 |
47 | ### Usage of generator in an experiment
48 | 
49 |
50 | ## Author
51 | This library has been developed by [Wilbert van de Ridder](http://www.github.com/WRidder) as part of a BSc assignment at the [University of Twente](http://www.utwente.nl).
52 |
--------------------------------------------------------------------------------
/cpp/MotionProfile.h:
--------------------------------------------------------------------------------
1 | #ifndef __MOTIONPROFILE_H__
2 | #define __MOTIONPROFILE_H__
3 |
4 | #if defined(ARDUINO) && ARDUINO >= 100
5 | #include "Arduino.h"
6 | #else
7 | #include "WProgram.h"
8 | #endif
9 |
10 | /**
11 | * Provides a simple trapezoidal motion profile generator.
12 | *
13 | *
14 | * Usage:
15 | * // Includes
16 | * #include "MotionProfile.h"
17 | *
18 | * // Define state functions
19 | * static const State stateArr[] = {
20 | * State(STATE_NAME_ENTER, STATE_NAME_UPDATE, STATE_NAME_EXIT)
21 | * };
22 | *
23 | *
24 | *
25 | * @author Wilbert van de Ridder
26 | * @version 1.0
27 | * @since 2014-05-12
28 | */
29 | class MotionProfile {
30 | public:
31 | /**
32 | * Constructor
33 | *
34 | * @param int aVelocityMax maximum velocity
35 | * @param int aAccelerationMax maximum acceleration
36 | * @param short aMethod method of profile generation
37 | */
38 | MotionProfile(float aVelocityMax, float aAccelerationMax, short aMethod);
39 |
40 | /**
41 | * Constructor
42 | *
43 | * @param int aVelocityMax maximum velocity
44 | * @param int aAccelerationMax maximum acceleration
45 | * @param short aMethod method of profile generation
46 | */
47 | MotionProfile(float aVelocityMax, float aAccelerationMax, short aMethod, int aSampleTime);
48 |
49 | void init();
50 |
51 | /**
52 | * Updates the state, generating new setpoints
53 | *
54 | * @param aSetpoint The current setpoint.
55 | */
56 | float update(float aSetpoint);
57 |
58 | bool getFinished();
59 | void setCompFactor(int aFactor);
60 | void setMaxVelocity(float aMaxVelocity);
61 | void setMaxAcceleration(float aMaxVelocity);
62 | void pause();
63 | void reset();
64 | private:
65 | /**
66 | * Increments the state number.
67 | *
68 | * @see
69 | currentState
70 | */
71 | bool timeCalculation();
72 | void stateCalculation();
73 | void calculateConstantVelocityProfile(float);
74 | void calculateTrapezoidalProfile(float);
75 |
76 | static const short nrMethods = 2;
77 |
78 | float maxVelocity;
79 | float maxAcceleration;
80 | short method;
81 |
82 | float position;
83 | float oldPosition;
84 | float velocity;
85 | float oldVelocity;
86 | float acceleration;
87 |
88 | unsigned long lastTime;
89 | float delta;
90 | int sampleTime;
91 |
92 | int compFactor;
93 | bool isFinished;
94 | };
95 | #endif
96 |
--------------------------------------------------------------------------------
/Matlab/MotionProfileClass.m:
--------------------------------------------------------------------------------
1 | classdef MotionProfileClass < handle
2 | % The following properties can be set only by class methods
3 | properties (SetAccess = private)
4 | maxVelocity = 0;
5 | maxAcceleration = 0;
6 | method = 1;
7 | position = 0;
8 | oldPosition = 0;
9 | velocity = 0;
10 | oldVelocity = 0;
11 | acceleration = 0;
12 | lastTime = 0;
13 | delta = 0;
14 | nrMethods = 2;
15 | end
16 | methods
17 | % Constructor
18 | function MP = MotionProfileClass(aVelocityMax,aAccelerationMax, aMethod)
19 | MP.maxVelocity = aVelocityMax;
20 | MP.maxAcceleration = aAccelerationMax;
21 | MP.method = aMethod;
22 |
23 | % Check method
24 | if MP.method < 0 || MP.method > MP.nrMethods - 1
25 | MP.method = 1;
26 | end
27 | end
28 |
29 | % Public methods
30 | function valid = timeCalculation(MP, time)
31 | if (MP.lastTime == 0)
32 | MP.lastTime = time;
33 | valid = false;
34 | else
35 | MP.delta = (time - MP.lastTime);
36 | MP.lastTime = time;
37 | valid = true;
38 | end
39 | end
40 |
41 | function update(MP, aSetpoint, time)
42 | timeValid = MP.timeCalculation(time);
43 |
44 | % Shift state variables
45 | MP.oldPosition = MP.position;
46 | MP.oldVelocity = MP.velocity;
47 |
48 | % Calculate new setpoint if time is valid
49 | if (timeValid)
50 | MP.calculateTrapezoidalProfile(aSetpoint);
51 | end
52 |
53 | % Calculate state
54 | MP.stateCalculation();
55 | end
56 |
57 | function stateCalculation(MP)
58 | MP.velocity = (MP.position - MP.oldPosition) / MP.delta;
59 | if isnan(MP.velocity)
60 | MP.velocity = 0;
61 | end
62 |
63 | MP.acceleration = (MP.velocity - MP.oldVelocity) / MP.delta;
64 | if isnan(MP.acceleration)
65 | MP.acceleration = 0;
66 | end
67 | end
68 |
69 | function calculateTrapezoidalProfile(MP, setpoint)
70 | % Checks for: max velocity, max acceleration
71 | % Check if we should be de-accelerating
72 | if (MP.velocity^2 / MP.maxAcceleration) / 2 >= abs(setpoint - MP.position)
73 | % We need to de-accelerate
74 | if MP.velocity < 0
75 | MP.position = (MP.velocity + MP.maxAcceleration * MP.delta) * MP.delta + MP.position;
76 | elseif MP.velocity > 0
77 | MP.position = (MP.velocity - MP.maxAcceleration * MP.delta) * MP.delta + MP.position;
78 | end
79 | else
80 | % We're not too close yet, either accelerate or maintain velocity
81 | if (abs(MP.velocity) < MP.maxVelocity || (setpoint < MP.position && MP.velocity > 0) || (setpoint > MP.position && MP.velocity < 0))
82 | % Accelerate, but keep in mind the maximum acceleration
83 | if setpoint > MP.position
84 | newSpeed = MP.velocity + MP.maxAcceleration * MP.delta;
85 | if (newSpeed > MP.maxVelocity)
86 | newSpeed = MP.maxVelocity;
87 | end
88 | MP.position = newSpeed * MP.delta + MP.position;
89 | else
90 | newSpeed = MP.velocity - MP.maxAcceleration * MP.delta;
91 | if (abs(newSpeed) > MP.maxVelocity)
92 | newSpeed = -MP.maxVelocity;
93 | end
94 | MP.position = newSpeed * MP.delta + MP.position;
95 | end
96 | else
97 | % Keep velocity constant at this maximum
98 | if setpoint > MP.position
99 | MP.position = MP.maxVelocity * MP.delta + MP.position;
100 | else
101 | MP.position = -MP.maxVelocity * MP.delta + MP.position;
102 | end
103 | end
104 | end
105 | end
106 | % Private methods
107 | end % methods
108 | end % classdef
--------------------------------------------------------------------------------
/cpp/MotionProfile.cpp:
--------------------------------------------------------------------------------
1 | #include "MotionProfile.h"
2 |
3 | MotionProfile::MotionProfile(float aVelocityMax, float aAccelerationMax, short aMethod)
4 | : maxVelocity(aVelocityMax), maxAcceleration(aAccelerationMax), method(aMethod)
5 | {
6 | sampleTime = 0;
7 | init();
8 | }
9 |
10 | MotionProfile::MotionProfile(float aVelocityMax, float aAccelerationMax, short aMethod, int aSampleTime)
11 | : maxVelocity(aVelocityMax), maxAcceleration(aAccelerationMax), method(aMethod), sampleTime(aSampleTime)
12 | {
13 | init();
14 | }
15 |
16 | void MotionProfile::init() {
17 | // Check the method
18 | if (0 > method || method > nrMethods - 1) {
19 | // Set trapezoidal as default
20 | method = 1;
21 | }
22 |
23 | // Time variables
24 | lastTime = 0;
25 | delta = 0;
26 |
27 | // State variables
28 | reset();
29 |
30 | // Comparison precision
31 | compFactor = 6;
32 |
33 | // Misc
34 | isFinished = false;
35 | }
36 |
37 | float MotionProfile::update(float setpoint) {
38 | unsigned long now = millis();
39 | unsigned long timeChange = (now - lastTime);
40 |
41 | if(timeChange >= sampleTime) {
42 | // Calculate elapsed time
43 | bool valid = timeCalculation();
44 |
45 | // Check if finished
46 | isFinished = static_cast(position * (10^compFactor)) == static_cast(setpoint * (10^compFactor));
47 | //Serial.println(isFinished);
48 |
49 | if (!isFinished) {
50 | // Shift state variables
51 | oldPosition = position;
52 | oldVelocity = velocity;
53 |
54 | if (valid) {
55 | // Check what type of motion profile to use
56 | switch(method) {
57 | case 0:
58 | // Constant velocity profile
59 | calculateConstantVelocityProfile(setpoint);
60 | break;
61 | case 1:
62 | // Trapezoidal velocity profile
63 | calculateTrapezoidalProfile(setpoint);
64 | break;
65 | }
66 | }
67 |
68 | // Update state
69 | stateCalculation();
70 | }
71 | }
72 | return position;
73 | }
74 |
75 | bool MotionProfile::timeCalculation() {
76 | // Get current time in microseconds
77 | long currentTime = micros();
78 | if (lastTime == 0) {
79 | lastTime = currentTime;
80 | return false;
81 | }
82 |
83 | // Delta is measured in seconds, thus divide by 1e6 because we're using microseconds.
84 | delta = static_cast(currentTime - lastTime) / 1000000;
85 | lastTime = currentTime;
86 | return true;
87 | }
88 |
89 | void MotionProfile::stateCalculation() {
90 | // Contains NaN checks, see: https://stackoverflow.com/questions/570669/checking-if-a-double-or-float-is-nan-in-c
91 | // Calculate velocity
92 | velocity = (position - oldPosition) / delta;
93 | if (velocity != velocity) {
94 | velocity = 0;
95 | }
96 |
97 | // Calculate acceleration
98 | acceleration = (velocity - oldVelocity) / delta;
99 | if (acceleration != acceleration) {
100 | acceleration = 0;
101 | }
102 | }
103 |
104 | void MotionProfile::calculateConstantVelocityProfile(float setpoint) {
105 | float suggestedVelocity = (setpoint - position) / delta;
106 |
107 | if (suggestedVelocity > maxVelocity) {
108 | position += maxVelocity * delta;
109 | }
110 | else if (suggestedVelocity < -maxVelocity) {
111 | position += -maxVelocity * delta;
112 | }
113 | else {
114 | position += suggestedVelocity * delta;
115 | }
116 | }
117 |
118 | void MotionProfile::calculateTrapezoidalProfile(float setpoint) {
119 | // Check if we need to de-accelerate
120 | if (((velocity * velocity) / maxAcceleration) / 2 >= abs(setpoint - position)) {
121 | if (velocity < 0) {
122 | position += (velocity + maxAcceleration * delta) * delta;
123 | }
124 | else if (velocity > 0) {
125 | position += (velocity - maxAcceleration * delta) * delta;
126 | }
127 | }
128 | else {
129 | // We're not too close yet, so no need to de-accelerate. Check if we need to accelerate or maintain velocity.
130 | if (abs(velocity) < maxVelocity || (setpoint < position && velocity > 0) || (setpoint > position && velocity < 0)) {
131 | // We need to accelerate, do so but check the maximum acceleration.
132 | // Keep velocity constant at the maximum
133 | float suggestedVelocity = 0.0;
134 | if (setpoint > position) {
135 | suggestedVelocity = velocity + maxAcceleration * delta;
136 | if (suggestedVelocity > maxVelocity) {
137 | suggestedVelocity = maxVelocity;
138 | }
139 | }
140 | else {
141 | suggestedVelocity = velocity - maxAcceleration * delta;
142 | if (abs(suggestedVelocity) > maxVelocity) {
143 | suggestedVelocity = -maxVelocity;
144 | }
145 | }
146 | position += suggestedVelocity * delta;
147 | }
148 | else {
149 | // Keep velocity constant at the maximum
150 | if (setpoint > position) {
151 | position += maxVelocity * delta;
152 | }
153 | else {
154 | position += -maxVelocity * delta;
155 | }
156 | }
157 | }
158 | }
159 |
160 | void MotionProfile::pause() {
161 | lastTime = 0;
162 | }
163 |
164 | // Getters and setters
165 | bool MotionProfile::getFinished() {
166 | return isFinished;
167 | }
168 |
169 | void MotionProfile::setMaxVelocity(float aMaxVelocity) {
170 | maxVelocity = aMaxVelocity;
171 | }
172 |
173 | void MotionProfile::setMaxAcceleration(float aMaxAcceleration) {
174 | maxAcceleration = aMaxAcceleration;
175 | }
176 |
177 | void MotionProfile::setCompFactor(int aFactor) {
178 | compFactor = aFactor;
179 | }
180 |
181 | void MotionProfile::reset() {
182 | // Reset all state variables
183 | position = 0;
184 | oldPosition = 0;
185 | velocity = 0;
186 | oldVelocity = 0;
187 | acceleration = 0;
188 | }
--------------------------------------------------------------------------------