position_controller.cpp

00001 #include "position_controller.h"
00002 
00003 PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
00004     accDistance(accDistance_),
00005     decDistance(decDistance_),
00006     initialVelocity(initialVelocity_),
00007     terminalVelocity(terminalVelocity_),
00008     maxVelocity(maxVelocity_)
00009 {
00010 }
00011 
00012 void PositionController::targetXY(int targetX_, int targetY_)
00013 {
00014     targetX = targetX_;
00015     targetY = targetY_;
00016     target = hypot((double)targetX, (double)targetY);
00017     radians = atan2((double)targetY, (double)targetX);
00018     if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
00019         enoughDistance = true;
00020     } else {
00021         enoughDistance = false;
00022         maxVelocity = initialVelocity + (maxVelocity - initialVelocity) * (target/(accDistance+decDistance));
00023         accTrue = accDistance * (target/(accDistance+decDistance));
00024         decTrue = decDistance * (target/(accDistance+decDistance));
00025     }
00026 }
00027 
00028 double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
00029 {
00030     return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
00031 }
00032 
00033 double PositionController::getVelocity(int position)
00034 {
00035     double vel = 0;
00036     if(enoughDistance) {
00037         if(position < 0) {
00038             vel = initialVelocity;
00039         } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
00040             vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
00041         } else if(position < target - (decDistance * fabs(maxVelocity - terminalVelocity))) {
00042             vel = maxVelocity;
00043         } else if(position < target) {
00044             vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decDistance * fabs(maxVelocity - terminalVelocity), decDistance * fabs(maxVelocity - terminalVelocity));
00045         } else if(position < 2 * target) {
00046             vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
00047         } else {
00048             vel = -maxVelocity;
00049         }
00050     } else {
00051         if(position < 0) {
00052             vel = initialVelocity;
00053         } else if(position < accTrue) {
00054             vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accTrue);
00055         } else if(position < target) {
00056             vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decTrue, decTrue);
00057         } else if(position < 2 * target) {
00058             vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
00059         } else {
00060             vel = -maxVelocity;
00061         }
00062     }
00063     return vel;
00064 }
00065 
00066 void PositionController::compute(int positionX, int positionY)
00067 {
00068     velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
00069     velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
00070 }
00071 
00072 double PositionController::getVelocityX()
00073 {
00074     return velocity[0];
00075 }
00076 
00077 double PositionController::getVelocityY()
00078 {
00079     return velocity[1];
00080 }
00081