NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2019_usiro_v6

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen

lib/S-ShapeModel/position_controller.cpp@0:3ad208cbea5f, 2019-09-13 (annotated)

Committer:
skouki
Date:
Fri Sep 13 02:19:03 2019 +0000
Revision:
0:3ad208cbea5f
2019/09/13ver

Who changed what in which revision?

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