NagaokaRoboticsClub_mbedTeam / S-ShapeModelFloat

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

Dependents:   2021NHK_B_main

position_controller.cpp@4:4cc455e372c2, 2019-06-25 (annotated)

Committer:
highfieldsnj
Date:
Tue Jun 25 08:28:21 2019 +0000
Revision:
4:4cc455e372c2
Parent:
3:65ae32169b33 
double -> float

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 1:aed2cca33061 1 #include "position_controller.h"
UCHITAKE 1:aed2cca33061 2
highfieldsnj 4:4cc455e372c2 3 PositionController::PositionController(float accDistance_, float decDistance_, float initialVelocity_, float 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_;
highfieldsnj 4:4cc455e372c2 16 target = hypot((float)targetX, (float)targetY);
highfieldsnj 4:4cc455e372c2 17 radians = atan2((float)targetY, (float)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
highfieldsnj 4:4cc455e372c2 28 float PositionController::generateSineWave(float x, float initV, float termV, float start, float length)
takeuchi 2:2c5e4f521390 29 {
highfieldsnj 4:4cc455e372c2 30 return ((termV - initV) * sin(F_PI * ((2.0f * x - 2.0f * start - length)/(2.0f * length))) + termV + initV)/2.0f;
UCHITAKE 1:aed2cca33061 31 }
UCHITAKE 1:aed2cca33061 32
highfieldsnj 4:4cc455e372c2 33 float PositionController::getVelocity(int position)
tanabe2000 3:65ae32169b33 34 {
highfieldsnj 4:4cc455e372c2 35 float vel = 0;
takeuchi 2:2c5e4f521390 36 if(enoughDistance) {
highfieldsnj 4:4cc455e372c2 37 if(position < 0.0f) {
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));
highfieldsnj 4:4cc455e372c2 45 } else if(position < 2.0f * 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 {
highfieldsnj 4:4cc455e372c2 68 velocity[0] = getVelocity(((float)positionX / (float)targetX)*target) * cos(radians);
highfieldsnj 4:4cc455e372c2 69 velocity[1] = getVelocity(((float)positionY / (float)targetY)*target) * sin(radians);
UCHITAKE 1:aed2cca33061 70 }
UCHITAKE 1:aed2cca33061 71
highfieldsnj 4:4cc455e372c2 72 float PositionController::getVelocityX()
UCHITAKE 1:aed2cca33061 73 {
UCHITAKE 1:aed2cca33061 74 return velocity[0];
UCHITAKE 1:aed2cca33061 75 }
UCHITAKE 1:aed2cca33061 76
highfieldsnj 4:4cc455e372c2 77 float PositionController::getVelocityY()
UCHITAKE 1:aed2cca33061 78 {
UCHITAKE 1:aed2cca33061 79 return velocity[1];
UCHITAKE 1:aed2cca33061 80 }
takeuchi 2:2c5e4f521390 81