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

Dependents:   2021NHK_B_main

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers position_controller.cpp Source File

position_controller.cpp

00001 #include "position_controller.h"
00002 
00003 PositionController::PositionController(float accDistance_, float decDistance_, float initialVelocity_, float 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((float)targetX, (float)targetY);
00017     radians = atan2((float)targetY, (float)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 float PositionController::generateSineWave(float x, float initV, float termV, float start, float length)
00029 {
00030     return ((termV - initV) * sin(F_PI * ((2.0f * x - 2.0f * start - length)/(2.0f * length))) + termV + initV)/2.0f;
00031 }
00032 
00033 float PositionController::getVelocity(int position)
00034 {
00035     float vel = 0;
00036     if(enoughDistance) {
00037         if(position < 0.0f) {
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.0f * 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(((float)positionX / (float)targetX)*target) * cos(radians);
00069     velocity[1] = getVelocity(((float)positionY / (float)targetY)*target) * sin(radians);
00070 }
00071 
00072 float PositionController::getVelocityX()
00073 {
00074     return velocity[0];
00075 }
00076 
00077 float PositionController::getVelocityY()
00078 {
00079     return velocity[1];
00080 }
00081