NagaokaRoboticsClub_mbedTeam / S-ShapeModelFloat

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

Dependents:   2021NHK_B_main

position_controller.cpp@2:2c5e4f521390, 2018-08-20 (annotated)

Committer:
takeuchi
Date:
Mon Aug 20 13:54:33 2018 +0900
Revision:
2:2c5e4f521390
Parent:
1:aed2cca33061
Child:
3:65ae32169b33
??????

Who changed what in which revision?

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