NagaokaRoboticsClub_mbedTeam / S-ShapeModelFloat

S-curve acceleration / deceleration model generator (using FPU)

Dependents:   2021NHK_B_main

position_controller.cpp@3:65ae32169b33, 2018-09-15 (annotated)

Committer:
tanabe2000
Date:
Sat Sep 15 05:05:27 2018 +0000
Revision:
3:65ae32169b33
Parent:
2:2c5e4f521390
Child:
4:4cc455e372c2
targetmode????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 1:aed2cca33061 1 #include "position_controller.h"
UCHITAKE 1:aed2cca33061 2
tanabe2000 3:65ae32169b33 3 PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
takeuchi 2:2c5e4f521390 4 accDistance(accDistance_),
takeuchi 2:2c5e4f521390 5 decDistance(decDistance_),
takeuchi 2:2c5e4f521390 6 initialVelocity(initialVelocity_),
takeuchi 2:2c5e4f521390 7 terminalVelocity(terminalVelocity_),
tanabe2000 3:65ae32169b33 8 maxVelocity(maxVelocity_)
UCHITAKE 1:aed2cca33061 9 {
tanabe2000 3:65ae32169b33 10 }
tanabe2000 3:65ae32169b33 11
tanabe2000 3:65ae32169b33 12 void PositionController::targetXY(int targetX_, int targetY_)
tanabe2000 3:65ae32169b33 13 {
tanabe2000 3:65ae32169b33 14 targetX = targetX_;
tanabe2000 3:65ae32169b33 15 targetY = targetY_;
tanabe2000 3:65ae32169b33 16 target = hypot((double)targetX, (double)targetY);
tanabe2000 3:65ae32169b33 17 radians = atan2((double)targetY, (double)targetX);
takeuchi 2:2c5e4f521390 18 if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
takeuchi 2:2c5e4f521390 19 enoughDistance = true;
UCHITAKE 1:aed2cca33061 20 } else {
takeuchi 2:2c5e4f521390 21 enoughDistance = false;
takeuchi 2:2c5e4f521390 22 maxVelocity = initialVelocity + (maxVelocity - initialVelocity) * (target/(accDistance+decDistance));
takeuchi 2:2c5e4f521390 23 accTrue = accDistance * (target/(accDistance+decDistance));
takeuchi 2:2c5e4f521390 24 decTrue = decDistance * (target/(accDistance+decDistance));
UCHITAKE 1:aed2cca33061 25 }
takeuchi 2:2c5e4f521390 26 }
takeuchi 2:2c5e4f521390 27
takeuchi 2:2c5e4f521390 28 double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
takeuchi 2:2c5e4f521390 29 {
takeuchi 2:2c5e4f521390 30 return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
UCHITAKE 1:aed2cca33061 31 }
UCHITAKE 1:aed2cca33061 32
takeuchi 2:2c5e4f521390 33 double PositionController::getVelocity(int position)
tanabe2000 3:65ae32169b33 34 {
takeuchi 2:2c5e4f521390 35 double vel = 0;
takeuchi 2:2c5e4f521390 36 if(enoughDistance) {
takeuchi 2:2c5e4f521390 37 if(position < 0) {
takeuchi 2:2c5e4f521390 38 vel = initialVelocity;
takeuchi 2:2c5e4f521390 39 } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
takeuchi 2:2c5e4f521390 40 vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
takeuchi 2:2c5e4f521390 41 } else if(position < target - (decDistance * fabs(maxVelocity - terminalVelocity))) {
UCHITAKE 1:aed2cca33061 42 vel = maxVelocity;
takeuchi 2:2c5e4f521390 43 } else if(position < target) {
takeuchi 2:2c5e4f521390 44 vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decDistance * fabs(maxVelocity - terminalVelocity), decDistance * fabs(maxVelocity - terminalVelocity));
tanabe2000 3:65ae32169b33 45 } else if(position < 2 * target) {
takeuchi 2:2c5e4f521390 46 vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
takeuchi 2:2c5e4f521390 47 } else {
takeuchi 2:2c5e4f521390 48 vel = -maxVelocity;
UCHITAKE 1:aed2cca33061 49 }
UCHITAKE 1:aed2cca33061 50 } else {
takeuchi 2:2c5e4f521390 51 if(position < 0) {
takeuchi 2:2c5e4f521390 52 vel = initialVelocity;
takeuchi 2:2c5e4f521390 53 } else if(position < accTrue) {
tanabe2000 3:65ae32169b33 54 vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accTrue);
takeuchi 2:2c5e4f521390 55 } else if(position < target) {
takeuchi 2:2c5e4f521390 56 vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decTrue, decTrue);
tanabe2000 3:65ae32169b33 57 } else if(position < 2 * target) {
takeuchi 2:2c5e4f521390 58 vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
takeuchi 2:2c5e4f521390 59 } else {
takeuchi 2:2c5e4f521390 60 vel = -maxVelocity;
UCHITAKE 1:aed2cca33061 61 }
UCHITAKE 1:aed2cca33061 62 }
UCHITAKE 1:aed2cca33061 63 return vel;
UCHITAKE 1:aed2cca33061 64 }
UCHITAKE 1:aed2cca33061 65
UCHITAKE 1:aed2cca33061 66 void PositionController::compute(int positionX, int positionY)
UCHITAKE 1:aed2cca33061 67 {
takeuchi 2:2c5e4f521390 68 velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
takeuchi 2:2c5e4f521390 69 velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
UCHITAKE 1:aed2cca33061 70 }
UCHITAKE 1:aed2cca33061 71
takeuchi 2:2c5e4f521390 72 double PositionController::getVelocityX()
UCHITAKE 1:aed2cca33061 73 {
UCHITAKE 1:aed2cca33061 74 return velocity[0];
UCHITAKE 1:aed2cca33061 75 }
UCHITAKE 1:aed2cca33061 76
takeuchi 2:2c5e4f521390 77 double PositionController::getVelocityY()
UCHITAKE 1:aed2cca33061 78 {
UCHITAKE 1:aed2cca33061 79 return velocity[1];
UCHITAKE 1:aed2cca33061 80 }
takeuchi 2:2c5e4f521390 81