TeamSurface / Libs

Nim leo niiiim

Motion.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kiwicjam 0:da791f233257 1 /*
Kiwicjam 0:da791f233257 2 * Motion.cpp
Kiwicjam 0:da791f233257 3 * Copyright (c) 2018, ZHAW
Kiwicjam 0:da791f233257 4 * All rights reserved.
Kiwicjam 0:da791f233257 5 */
Kiwicjam 0:da791f233257 6
Kiwicjam 0:da791f233257 7 #include <cmath>
Kiwicjam 0:da791f233257 8 #include <algorithm>
Kiwicjam 0:da791f233257 9 #include "Motion.h"
Kiwicjam 0:da791f233257 10
Kiwicjam 0:da791f233257 11 using namespace std;
Kiwicjam 0:da791f233257 12
Kiwicjam 0:da791f233257 13 const float Motion::DEFAULT_LIMIT = 1.0f; // default value for limits
Kiwicjam 0:da791f233257 14 const float Motion::MINIMUM_LIMIT = 1.0e-9f; // smallest value allowed for limits
Kiwicjam 0:da791f233257 15
Kiwicjam 0:da791f233257 16 /**
Kiwicjam 0:da791f233257 17 * Creates a <code>Motion</code> object.
Kiwicjam 0:da791f233257 18 * The values for position, velocity and acceleration are set to 0.
Kiwicjam 0:da791f233257 19 */
Kiwicjam 0:da791f233257 20 Motion::Motion() {
Kiwicjam 0:da791f233257 21
Kiwicjam 0:da791f233257 22 position = 0.0;
Kiwicjam 0:da791f233257 23 velocity = 0.0f;
Kiwicjam 0:da791f233257 24
Kiwicjam 0:da791f233257 25 profileVelocity = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 26 profileAcceleration = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 27 profileDeceleration = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 28 }
Kiwicjam 0:da791f233257 29
Kiwicjam 0:da791f233257 30 /**
Kiwicjam 0:da791f233257 31 * Creates a <code>Motion</code> object with given values for position and velocity.
Kiwicjam 0:da791f233257 32 * @param position the initial position value of this motion, given in [m] or [rad].
Kiwicjam 0:da791f233257 33 * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
Kiwicjam 0:da791f233257 34 */
Kiwicjam 0:da791f233257 35 Motion::Motion(double position, float velocity) {
Kiwicjam 0:da791f233257 36
Kiwicjam 0:da791f233257 37 this->position = position;
Kiwicjam 0:da791f233257 38 this->velocity = velocity;
Kiwicjam 0:da791f233257 39
Kiwicjam 0:da791f233257 40 profileVelocity = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 41 profileAcceleration = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 42 profileDeceleration = DEFAULT_LIMIT;
Kiwicjam 0:da791f233257 43 }
Kiwicjam 0:da791f233257 44
Kiwicjam 0:da791f233257 45 /**
Kiwicjam 0:da791f233257 46 * Creates a <code>Motion</code> object with given values for position and velocity.
Kiwicjam 0:da791f233257 47 * @param motion another <code>Motion</code> object to copy the values from.
Kiwicjam 0:da791f233257 48 */
Kiwicjam 0:da791f233257 49 Motion::Motion(const Motion& motion) {
Kiwicjam 0:da791f233257 50
Kiwicjam 0:da791f233257 51 position = motion.position;
Kiwicjam 0:da791f233257 52 velocity = motion.velocity;
Kiwicjam 0:da791f233257 53
Kiwicjam 0:da791f233257 54 profileVelocity = motion.profileVelocity;
Kiwicjam 0:da791f233257 55 profileAcceleration = motion.profileAcceleration;
Kiwicjam 0:da791f233257 56 profileDeceleration = motion.profileDeceleration;
Kiwicjam 0:da791f233257 57 }
Kiwicjam 0:da791f233257 58
Kiwicjam 0:da791f233257 59 /**
Kiwicjam 0:da791f233257 60 * Deletes the Motion object.
Kiwicjam 0:da791f233257 61 */
Kiwicjam 0:da791f233257 62 Motion::~Motion() {}
Kiwicjam 0:da791f233257 63
Kiwicjam 0:da791f233257 64 /**
Kiwicjam 0:da791f233257 65 * Sets the values for position and velocity.
Kiwicjam 0:da791f233257 66 * @param position the desired position value of this motion, given in [m] or [rad].
Kiwicjam 0:da791f233257 67 * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
Kiwicjam 0:da791f233257 68 */
Kiwicjam 0:da791f233257 69 void Motion::set(double position, float velocity) {
Kiwicjam 0:da791f233257 70
Kiwicjam 0:da791f233257 71 this->position = position;
Kiwicjam 0:da791f233257 72 this->velocity = velocity;
Kiwicjam 0:da791f233257 73 }
Kiwicjam 0:da791f233257 74
Kiwicjam 0:da791f233257 75 /**
Kiwicjam 0:da791f233257 76 * Sets the values for position and velocity.
Kiwicjam 0:da791f233257 77 * @param motion another <code>Motion</code> object to copy the values from.
Kiwicjam 0:da791f233257 78 */
Kiwicjam 0:da791f233257 79 void Motion::set(const Motion& motion) {
Kiwicjam 0:da791f233257 80
Kiwicjam 0:da791f233257 81 position = motion.position;
Kiwicjam 0:da791f233257 82 velocity = motion.velocity;
Kiwicjam 0:da791f233257 83 }
Kiwicjam 0:da791f233257 84
Kiwicjam 0:da791f233257 85 /**
Kiwicjam 0:da791f233257 86 * Sets the position value.
Kiwicjam 0:da791f233257 87 * @param position the desired position value of this motion, given in [m] or [rad].
Kiwicjam 0:da791f233257 88 */
Kiwicjam 0:da791f233257 89 void Motion::setPosition(double position) {
Kiwicjam 0:da791f233257 90
Kiwicjam 0:da791f233257 91 this->position = position;
Kiwicjam 0:da791f233257 92 }
Kiwicjam 0:da791f233257 93
Kiwicjam 0:da791f233257 94 /**
Kiwicjam 0:da791f233257 95 * Gets the position value.
Kiwicjam 0:da791f233257 96 * @return the position value of this motion, given in [m] or [rad].
Kiwicjam 0:da791f233257 97 */
Kiwicjam 0:da791f233257 98 double Motion::getPosition() {
Kiwicjam 0:da791f233257 99
Kiwicjam 0:da791f233257 100 return position;
Kiwicjam 0:da791f233257 101 }
Kiwicjam 0:da791f233257 102
Kiwicjam 0:da791f233257 103 /**
Kiwicjam 0:da791f233257 104 * Sets the velocity value.
Kiwicjam 0:da791f233257 105 * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
Kiwicjam 0:da791f233257 106 */
Kiwicjam 0:da791f233257 107 void Motion::setVelocity(float velocity) {
Kiwicjam 0:da791f233257 108
Kiwicjam 0:da791f233257 109 this->velocity = velocity;
Kiwicjam 0:da791f233257 110 }
Kiwicjam 0:da791f233257 111
Kiwicjam 0:da791f233257 112 /**
Kiwicjam 0:da791f233257 113 * Gets the velocity value.
Kiwicjam 0:da791f233257 114 * @return the velocity value of this motion, given in [m/s] or [rad/s].
Kiwicjam 0:da791f233257 115 */
Kiwicjam 0:da791f233257 116 float Motion::getVelocity() {
Kiwicjam 0:da791f233257 117
Kiwicjam 0:da791f233257 118 return velocity;
Kiwicjam 0:da791f233257 119 }
Kiwicjam 0:da791f233257 120
Kiwicjam 0:da791f233257 121 /**
Kiwicjam 0:da791f233257 122 * Sets the limit for the velocity value.
Kiwicjam 0:da791f233257 123 * @param profileVelocity the limit of the velocity.
Kiwicjam 0:da791f233257 124 */
Kiwicjam 0:da791f233257 125 void Motion::setProfileVelocity(float profileVelocity) {
Kiwicjam 0:da791f233257 126
Kiwicjam 0:da791f233257 127 if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 128 }
Kiwicjam 0:da791f233257 129
Kiwicjam 0:da791f233257 130 /**
Kiwicjam 0:da791f233257 131 * Sets the limit for the acceleration value.
Kiwicjam 0:da791f233257 132 * @param profileAcceleration the limit of the acceleration.
Kiwicjam 0:da791f233257 133 */
Kiwicjam 0:da791f233257 134 void Motion::setProfileAcceleration(float profileAcceleration) {
Kiwicjam 0:da791f233257 135
Kiwicjam 0:da791f233257 136 if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 137 }
Kiwicjam 0:da791f233257 138
Kiwicjam 0:da791f233257 139 /**
Kiwicjam 0:da791f233257 140 * Sets the limit for the deceleration value.
Kiwicjam 0:da791f233257 141 * @param profileDeceleration the limit of the deceleration.
Kiwicjam 0:da791f233257 142 */
Kiwicjam 0:da791f233257 143 void Motion::setProfileDeceleration(float profileDeceleration) {
Kiwicjam 0:da791f233257 144
Kiwicjam 0:da791f233257 145 if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 146 }
Kiwicjam 0:da791f233257 147
Kiwicjam 0:da791f233257 148 /**
Kiwicjam 0:da791f233257 149 * Sets the limits for velocity, acceleration and deceleration values.
Kiwicjam 0:da791f233257 150 * @param profileVelocity the limit of the velocity.
Kiwicjam 0:da791f233257 151 * @param profileAcceleration the limit of the acceleration.
Kiwicjam 0:da791f233257 152 * @param profileDeceleration the limit of the deceleration.
Kiwicjam 0:da791f233257 153 */
Kiwicjam 0:da791f233257 154 void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
Kiwicjam 0:da791f233257 155
Kiwicjam 0:da791f233257 156 if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 157 if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 158 if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
Kiwicjam 0:da791f233257 159 }
Kiwicjam 0:da791f233257 160
Kiwicjam 0:da791f233257 161 /**
Kiwicjam 0:da791f233257 162 * Gets the time needed to move to a given target position.
Kiwicjam 0:da791f233257 163 * @param targetPosition the desired target position given in [m] or [rad].
Kiwicjam 0:da791f233257 164 * @return the time to move to the target position, given in [s].
Kiwicjam 0:da791f233257 165 */
Kiwicjam 0:da791f233257 166 float Motion::getTimeToPosition(double targetPosition) {
Kiwicjam 0:da791f233257 167
Kiwicjam 0:da791f233257 168 // calculate position, when velocity is reduced to zero
Kiwicjam 0:da791f233257 169
Kiwicjam 0:da791f233257 170 double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
Kiwicjam 0:da791f233257 171
Kiwicjam 0:da791f233257 172 if (targetPosition > stopPosition) { // positive velocity required
Kiwicjam 0:da791f233257 173
Kiwicjam 0:da791f233257 174 if (velocity > profileVelocity) { // slow down to profile velocity first
Kiwicjam 0:da791f233257 175
Kiwicjam 0:da791f233257 176 float t1 = (velocity-profileVelocity)/profileDeceleration;
Kiwicjam 0:da791f233257 177 float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
Kiwicjam 0:da791f233257 178 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 179
Kiwicjam 0:da791f233257 180 return t1+t2+t3;
Kiwicjam 0:da791f233257 181
Kiwicjam 0:da791f233257 182 } else if (velocity > 0.0f) { // speed up to profile velocity
Kiwicjam 0:da791f233257 183
Kiwicjam 0:da791f233257 184 float t1 = (profileVelocity-velocity)/profileAcceleration;
Kiwicjam 0:da791f233257 185 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 186 float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
Kiwicjam 0:da791f233257 187
Kiwicjam 0:da791f233257 188 if (t2 < 0.0f) {
Kiwicjam 0:da791f233257 189 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 190 t1 = (maxVelocity-velocity)/profileAcceleration;
Kiwicjam 0:da791f233257 191 t2 = 0.0f;
Kiwicjam 0:da791f233257 192 t3 = maxVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 193 }
Kiwicjam 0:da791f233257 194
Kiwicjam 0:da791f233257 195 return t1+t2+t3;
Kiwicjam 0:da791f233257 196
Kiwicjam 0:da791f233257 197 } else { // slow down to zero first, and then speed up to profile velocity
Kiwicjam 0:da791f233257 198
Kiwicjam 0:da791f233257 199 float t1 = -velocity/profileDeceleration;
Kiwicjam 0:da791f233257 200 float t2 = profileVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 201 float t4 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 202 float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
Kiwicjam 0:da791f233257 203
Kiwicjam 0:da791f233257 204 if (t3 < 0.0f) {
Kiwicjam 0:da791f233257 205 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 206 t2 = maxVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 207 t3 = 0.0f;
Kiwicjam 0:da791f233257 208 t4 = maxVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 209 }
Kiwicjam 0:da791f233257 210
Kiwicjam 0:da791f233257 211 return t1+t2+t3+t4;
Kiwicjam 0:da791f233257 212 }
Kiwicjam 0:da791f233257 213
Kiwicjam 0:da791f233257 214 } else { // negative velocity required
Kiwicjam 0:da791f233257 215
Kiwicjam 0:da791f233257 216 if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
Kiwicjam 0:da791f233257 217
Kiwicjam 0:da791f233257 218 float t1 = (-profileVelocity-velocity)/profileDeceleration;
Kiwicjam 0:da791f233257 219 float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
Kiwicjam 0:da791f233257 220 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 221
Kiwicjam 0:da791f233257 222 return t1+t2+t3;
Kiwicjam 0:da791f233257 223
Kiwicjam 0:da791f233257 224 } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
Kiwicjam 0:da791f233257 225
Kiwicjam 0:da791f233257 226 float t1 = (velocity+profileVelocity)/profileAcceleration;
Kiwicjam 0:da791f233257 227 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 228 float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
Kiwicjam 0:da791f233257 229
Kiwicjam 0:da791f233257 230 if (t2 < 0.0f) {
Kiwicjam 0:da791f233257 231 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 232 t1 = (velocity-minVelocity)/profileAcceleration;
Kiwicjam 0:da791f233257 233 t2 = 0.0f;
Kiwicjam 0:da791f233257 234 t3 = -minVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 235 }
Kiwicjam 0:da791f233257 236
Kiwicjam 0:da791f233257 237 return t1+t2+t3;
Kiwicjam 0:da791f233257 238
Kiwicjam 0:da791f233257 239 } else { // slow down to zero first, and then speed up to (negative) profile velocity
Kiwicjam 0:da791f233257 240
Kiwicjam 0:da791f233257 241 float t1 = velocity/profileDeceleration;
Kiwicjam 0:da791f233257 242 float t2 = profileVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 243 float t4 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 244 float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
Kiwicjam 0:da791f233257 245
Kiwicjam 0:da791f233257 246 if (t3 < 0.0f) {
Kiwicjam 0:da791f233257 247 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 248 t2 = -minVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 249 t3 = 0.0f;
Kiwicjam 0:da791f233257 250 t4 = -minVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 251 }
Kiwicjam 0:da791f233257 252
Kiwicjam 0:da791f233257 253 return t1+t2+t3+t4;
Kiwicjam 0:da791f233257 254 }
Kiwicjam 0:da791f233257 255 }
Kiwicjam 0:da791f233257 256 }
Kiwicjam 0:da791f233257 257
Kiwicjam 0:da791f233257 258 /**
Kiwicjam 0:da791f233257 259 * Increments the current motion towards a given target velocity.
Kiwicjam 0:da791f233257 260 * @param targetVelocity the desired target velocity given in [m/s] or [rad/s].
Kiwicjam 0:da791f233257 261 * @param period the time period to increment the motion values for, given in [s].
Kiwicjam 0:da791f233257 262 */
Kiwicjam 0:da791f233257 263 void Motion::incrementToVelocity(float targetVelocity, float period) {
Kiwicjam 0:da791f233257 264
Kiwicjam 0:da791f233257 265 if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
Kiwicjam 0:da791f233257 266 else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
Kiwicjam 0:da791f233257 267
Kiwicjam 0:da791f233257 268 if (targetVelocity > 0.0f) {
Kiwicjam 0:da791f233257 269
Kiwicjam 0:da791f233257 270 if (velocity > targetVelocity) { // slow down to target velocity
Kiwicjam 0:da791f233257 271
Kiwicjam 0:da791f233257 272 float t1 = (velocity-targetVelocity)/profileDeceleration;
Kiwicjam 0:da791f233257 273
Kiwicjam 0:da791f233257 274 if (t1 > period) {
Kiwicjam 0:da791f233257 275 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 276 velocity += -profileDeceleration*period;
Kiwicjam 0:da791f233257 277 } else {
Kiwicjam 0:da791f233257 278 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 279 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 280 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 281 }
Kiwicjam 0:da791f233257 282
Kiwicjam 0:da791f233257 283 } else if (velocity > 0.0f) { // speed up to target velocity
Kiwicjam 0:da791f233257 284
Kiwicjam 0:da791f233257 285 float t1 = (targetVelocity-velocity)/profileAcceleration;
Kiwicjam 0:da791f233257 286
Kiwicjam 0:da791f233257 287 if (t1 > period) {
Kiwicjam 0:da791f233257 288 position += (double)((velocity+profileAcceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 289 velocity += profileAcceleration*period;
Kiwicjam 0:da791f233257 290 } else {
Kiwicjam 0:da791f233257 291 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 292 velocity += profileAcceleration*t1;
Kiwicjam 0:da791f233257 293 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 294 }
Kiwicjam 0:da791f233257 295
Kiwicjam 0:da791f233257 296 } else { // slow down to zero first, and then speed up to target velocity
Kiwicjam 0:da791f233257 297
Kiwicjam 0:da791f233257 298 float t1 = -velocity/profileDeceleration;
Kiwicjam 0:da791f233257 299 float t2 = targetVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 300
Kiwicjam 0:da791f233257 301 if (t1 > period) {
Kiwicjam 0:da791f233257 302 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 303 velocity += profileDeceleration*period;
Kiwicjam 0:da791f233257 304 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 305 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 306 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 307 position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
Kiwicjam 0:da791f233257 308 velocity += profileAcceleration*(period-t1);
Kiwicjam 0:da791f233257 309 } else {
Kiwicjam 0:da791f233257 310 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 311 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 312 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 313 velocity += profileAcceleration*t2;
Kiwicjam 0:da791f233257 314 position += (double)(velocity*(period-t1-t2));
Kiwicjam 0:da791f233257 315 }
Kiwicjam 0:da791f233257 316 }
Kiwicjam 0:da791f233257 317
Kiwicjam 0:da791f233257 318 } else {
Kiwicjam 0:da791f233257 319
Kiwicjam 0:da791f233257 320 if (velocity < targetVelocity) { // slow down to (negative) target velocity
Kiwicjam 0:da791f233257 321
Kiwicjam 0:da791f233257 322 float t1 = (targetVelocity-velocity)/profileDeceleration;
Kiwicjam 0:da791f233257 323
Kiwicjam 0:da791f233257 324 if (t1 > period) {
Kiwicjam 0:da791f233257 325 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 326 velocity += profileDeceleration*period;
Kiwicjam 0:da791f233257 327 } else {
Kiwicjam 0:da791f233257 328 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 329 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 330 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 331 }
Kiwicjam 0:da791f233257 332
Kiwicjam 0:da791f233257 333 } else if (velocity < 0.0f) { // speed up to (negative) target velocity
Kiwicjam 0:da791f233257 334
Kiwicjam 0:da791f233257 335 float t1 = (velocity-targetVelocity)/profileAcceleration;
Kiwicjam 0:da791f233257 336
Kiwicjam 0:da791f233257 337 if (t1 > period) {
Kiwicjam 0:da791f233257 338 position += (double)((velocity-profileAcceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 339 velocity += -profileAcceleration*period;
Kiwicjam 0:da791f233257 340 } else {
Kiwicjam 0:da791f233257 341 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 342 velocity += -profileAcceleration*t1;
Kiwicjam 0:da791f233257 343 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 344 }
Kiwicjam 0:da791f233257 345
Kiwicjam 0:da791f233257 346 } else { // slow down to zero first, and then speed up to (negative) target velocity
Kiwicjam 0:da791f233257 347
Kiwicjam 0:da791f233257 348 float t1 = velocity/profileDeceleration;
Kiwicjam 0:da791f233257 349 float t2 = -targetVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 350
Kiwicjam 0:da791f233257 351 if (t1 > period) {
Kiwicjam 0:da791f233257 352 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 353 velocity += -profileDeceleration*period;
Kiwicjam 0:da791f233257 354 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 355 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 356 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 357 position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
Kiwicjam 0:da791f233257 358 velocity += -profileAcceleration*(period-t1);
Kiwicjam 0:da791f233257 359 } else {
Kiwicjam 0:da791f233257 360 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 361 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 362 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 363 velocity += -profileAcceleration*t2;
Kiwicjam 0:da791f233257 364 position += (double)(velocity*(period-t1-t2));
Kiwicjam 0:da791f233257 365 }
Kiwicjam 0:da791f233257 366 }
Kiwicjam 0:da791f233257 367 }
Kiwicjam 0:da791f233257 368 }
Kiwicjam 0:da791f233257 369
Kiwicjam 0:da791f233257 370 /**
Kiwicjam 0:da791f233257 371 * Increments the current motion towards a given target position.
Kiwicjam 0:da791f233257 372 * @param targetPosition the desired target position given in [m] or [rad].
Kiwicjam 0:da791f233257 373 * @param period the time period to increment the motion values for, given in [s].
Kiwicjam 0:da791f233257 374 */
Kiwicjam 0:da791f233257 375 void Motion::incrementToPosition(double targetPosition, float period) {
Kiwicjam 0:da791f233257 376
Kiwicjam 0:da791f233257 377 // calculate position, when velocity is reduced to zero
Kiwicjam 0:da791f233257 378
Kiwicjam 0:da791f233257 379 double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
Kiwicjam 0:da791f233257 380
Kiwicjam 0:da791f233257 381 if (targetPosition > stopPosition) { // positive velocity required
Kiwicjam 0:da791f233257 382
Kiwicjam 0:da791f233257 383 if (velocity > profileVelocity) { // slow down to profile velocity first
Kiwicjam 0:da791f233257 384
Kiwicjam 0:da791f233257 385 float t1 = (velocity-profileVelocity)/profileDeceleration;
Kiwicjam 0:da791f233257 386 float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
Kiwicjam 0:da791f233257 387 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 388
Kiwicjam 0:da791f233257 389 if (t1 > period) {
Kiwicjam 0:da791f233257 390 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 391 velocity += -profileDeceleration*period;
Kiwicjam 0:da791f233257 392 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 393 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 394 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 395 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 396 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 397 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 398 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 399 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 400 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
Kiwicjam 0:da791f233257 401 velocity += -profileDeceleration*(period-t1-t2);
Kiwicjam 0:da791f233257 402 } else {
Kiwicjam 0:da791f233257 403 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 404 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 405 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 406 position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
Kiwicjam 0:da791f233257 407 velocity += -profileDeceleration*t3;
Kiwicjam 0:da791f233257 408 }
Kiwicjam 0:da791f233257 409
Kiwicjam 0:da791f233257 410 } else if (velocity > 0.0f) { // speed up to profile velocity
Kiwicjam 0:da791f233257 411
Kiwicjam 0:da791f233257 412 float t1 = (profileVelocity-velocity)/profileAcceleration;
Kiwicjam 0:da791f233257 413 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 414 float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
Kiwicjam 0:da791f233257 415
Kiwicjam 0:da791f233257 416 if (t2 < 0.0f) {
Kiwicjam 0:da791f233257 417 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 418 t1 = (maxVelocity-velocity)/profileAcceleration;
Kiwicjam 0:da791f233257 419 t2 = 0.0f;
Kiwicjam 0:da791f233257 420 t3 = maxVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 421 }
Kiwicjam 0:da791f233257 422
Kiwicjam 0:da791f233257 423 if (t1 > period) {
Kiwicjam 0:da791f233257 424 position += (double)((velocity+profileAcceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 425 velocity += profileAcceleration*period;
Kiwicjam 0:da791f233257 426 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 427 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 428 velocity += profileAcceleration*t1;
Kiwicjam 0:da791f233257 429 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 430 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 431 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 432 velocity += profileAcceleration*t1;
Kiwicjam 0:da791f233257 433 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 434 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
Kiwicjam 0:da791f233257 435 velocity += -profileDeceleration*(period-t1-t2);
Kiwicjam 0:da791f233257 436 } else {
Kiwicjam 0:da791f233257 437 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 438 velocity += profileAcceleration*t1;
Kiwicjam 0:da791f233257 439 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 440 position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
Kiwicjam 0:da791f233257 441 velocity += -profileDeceleration*t3;
Kiwicjam 0:da791f233257 442 }
Kiwicjam 0:da791f233257 443
Kiwicjam 0:da791f233257 444 } else { // slow down to zero first, and then speed up to profile velocity
Kiwicjam 0:da791f233257 445
Kiwicjam 0:da791f233257 446 float t1 = -velocity/profileDeceleration;
Kiwicjam 0:da791f233257 447 float t2 = profileVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 448 float t4 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 449 float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
Kiwicjam 0:da791f233257 450
Kiwicjam 0:da791f233257 451 if (t3 < 0.0f) {
Kiwicjam 0:da791f233257 452 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 453 t2 = maxVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 454 t3 = 0.0f;
Kiwicjam 0:da791f233257 455 t4 = maxVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 456 }
Kiwicjam 0:da791f233257 457
Kiwicjam 0:da791f233257 458 if (t1 > period) {
Kiwicjam 0:da791f233257 459 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 460 velocity += profileDeceleration*period;
Kiwicjam 0:da791f233257 461 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 462 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 463 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 464 position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
Kiwicjam 0:da791f233257 465 velocity += profileAcceleration*(period-t1);
Kiwicjam 0:da791f233257 466 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 467 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 468 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 469 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 470 velocity += profileAcceleration*t2;
Kiwicjam 0:da791f233257 471 position += (double)(velocity*(period-t1-t2));
Kiwicjam 0:da791f233257 472 } else if (t1+t2+t3+t4 > period) {
Kiwicjam 0:da791f233257 473 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 474 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 475 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 476 velocity += profileAcceleration*t2;
Kiwicjam 0:da791f233257 477 position += (double)(velocity*t3);
Kiwicjam 0:da791f233257 478 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
Kiwicjam 0:da791f233257 479 velocity += -profileDeceleration*(period-t1-t2-t3);
Kiwicjam 0:da791f233257 480 } else {
Kiwicjam 0:da791f233257 481 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 482 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 483 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 484 velocity += profileAcceleration*t2;
Kiwicjam 0:da791f233257 485 position += (double)(velocity*t3);
Kiwicjam 0:da791f233257 486 position += (double)((velocity-profileDeceleration*0.5f*t4)*t4);
Kiwicjam 0:da791f233257 487 velocity += -profileDeceleration*t4;
Kiwicjam 0:da791f233257 488 }
Kiwicjam 0:da791f233257 489 }
Kiwicjam 0:da791f233257 490
Kiwicjam 0:da791f233257 491 } else { // negative velocity required
Kiwicjam 0:da791f233257 492
Kiwicjam 0:da791f233257 493 if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
Kiwicjam 0:da791f233257 494
Kiwicjam 0:da791f233257 495 float t1 = (-profileVelocity-velocity)/profileDeceleration;
Kiwicjam 0:da791f233257 496 float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
Kiwicjam 0:da791f233257 497 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 498
Kiwicjam 0:da791f233257 499 if (t1 > period) {
Kiwicjam 0:da791f233257 500 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 501 velocity += profileDeceleration*period;
Kiwicjam 0:da791f233257 502 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 503 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 504 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 505 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 506 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 507 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 508 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 509 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 510 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
Kiwicjam 0:da791f233257 511 velocity += profileDeceleration*(period-t1-t2);
Kiwicjam 0:da791f233257 512 } else {
Kiwicjam 0:da791f233257 513 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 514 velocity += profileDeceleration*t1;
Kiwicjam 0:da791f233257 515 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 516 position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
Kiwicjam 0:da791f233257 517 velocity += profileDeceleration*t3;
Kiwicjam 0:da791f233257 518 }
Kiwicjam 0:da791f233257 519
Kiwicjam 0:da791f233257 520 } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
Kiwicjam 0:da791f233257 521
Kiwicjam 0:da791f233257 522 float t1 = (velocity+profileVelocity)/profileAcceleration;
Kiwicjam 0:da791f233257 523 float t3 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 524 float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
Kiwicjam 0:da791f233257 525
Kiwicjam 0:da791f233257 526 if (t2 < 0.0f) {
Kiwicjam 0:da791f233257 527 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 528 t1 = (velocity-minVelocity)/profileAcceleration;
Kiwicjam 0:da791f233257 529 t2 = 0.0f;
Kiwicjam 0:da791f233257 530 t3 = -minVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 531 }
Kiwicjam 0:da791f233257 532
Kiwicjam 0:da791f233257 533 if (t1 > period) {
Kiwicjam 0:da791f233257 534 position += (double)((velocity-profileAcceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 535 velocity += -profileAcceleration*period;
Kiwicjam 0:da791f233257 536 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 537 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 538 velocity += -profileAcceleration*t1;
Kiwicjam 0:da791f233257 539 position += (double)(velocity*(period-t1));
Kiwicjam 0:da791f233257 540 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 541 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 542 velocity += -profileAcceleration*t1;
Kiwicjam 0:da791f233257 543 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 544 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
Kiwicjam 0:da791f233257 545 velocity += profileDeceleration*(period-t1-t2);
Kiwicjam 0:da791f233257 546 } else {
Kiwicjam 0:da791f233257 547 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 548 velocity += -profileAcceleration*t1;
Kiwicjam 0:da791f233257 549 position += (double)(velocity*t2);
Kiwicjam 0:da791f233257 550 position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
Kiwicjam 0:da791f233257 551 velocity += profileDeceleration*t3;
Kiwicjam 0:da791f233257 552 }
Kiwicjam 0:da791f233257 553
Kiwicjam 0:da791f233257 554 } else { // slow down to zero first, and then speed up to (negative) profile velocity
Kiwicjam 0:da791f233257 555
Kiwicjam 0:da791f233257 556 float t1 = velocity/profileDeceleration;
Kiwicjam 0:da791f233257 557 float t2 = profileVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 558 float t4 = profileVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 559 float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
Kiwicjam 0:da791f233257 560
Kiwicjam 0:da791f233257 561 if (t3 < 0.0f) {
Kiwicjam 0:da791f233257 562 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
Kiwicjam 0:da791f233257 563 t2 = -minVelocity/profileAcceleration;
Kiwicjam 0:da791f233257 564 t3 = 0.0f;
Kiwicjam 0:da791f233257 565 t4 = -minVelocity/profileDeceleration;
Kiwicjam 0:da791f233257 566 }
Kiwicjam 0:da791f233257 567
Kiwicjam 0:da791f233257 568 if (t1 > period) {
Kiwicjam 0:da791f233257 569 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
Kiwicjam 0:da791f233257 570 velocity += -profileDeceleration*period;
Kiwicjam 0:da791f233257 571 } else if (t1+t2 > period) {
Kiwicjam 0:da791f233257 572 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 573 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 574 position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
Kiwicjam 0:da791f233257 575 velocity += -profileAcceleration*(period-t1);
Kiwicjam 0:da791f233257 576 } else if (t1+t2+t3 > period) {
Kiwicjam 0:da791f233257 577 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 578 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 579 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 580 velocity += -profileAcceleration*t2;
Kiwicjam 0:da791f233257 581 position += (double)(velocity*(period-t1-t2));
Kiwicjam 0:da791f233257 582 } else if (t1+t2+t3+t4 > period) {
Kiwicjam 0:da791f233257 583 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 584 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 585 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 586 velocity += -profileAcceleration*t2;
Kiwicjam 0:da791f233257 587 position += (double)(velocity*t3);
Kiwicjam 0:da791f233257 588 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
Kiwicjam 0:da791f233257 589 velocity += profileDeceleration*(period-t1-t2-t3);
Kiwicjam 0:da791f233257 590 } else {
Kiwicjam 0:da791f233257 591 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
Kiwicjam 0:da791f233257 592 velocity += -profileDeceleration*t1;
Kiwicjam 0:da791f233257 593 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
Kiwicjam 0:da791f233257 594 velocity += -profileAcceleration*t2;
Kiwicjam 0:da791f233257 595 position += (double)(velocity*t3);
Kiwicjam 0:da791f233257 596 position += (double)((velocity+profileDeceleration*0.5f*t4)*t4);
Kiwicjam 0:da791f233257 597 velocity += profileDeceleration*t4;
Kiwicjam 0:da791f233257 598 }
Kiwicjam 0:da791f233257 599 }
Kiwicjam 0:da791f233257 600 }
Kiwicjam 0:da791f233257 601 }
Kiwicjam 0:da791f233257 602