S-curve acceleration / deceleration model generator (using FPU)
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Sat Aug 27 2022 07:00:16 by
1.7.2