bool trapezoid1(float t, float &posOut, float &speedOut) {
return TrapezoidalMotion(10, // pathLength
2, // maxSpeed
1, // maxAcc
t, // timeIntoLap
0, // startSpeed
0, // finalSpeed
posOut, // &posOut
speedOut); // &speedOut
}
boost::optional<RobotInstant> TrapezoidalPath::evaluate(float time) const {
float distance;
float speedOut;
bool valid = TrapezoidalMotion(_pathLength, // PathLength
_maxSpeed, // maxSpeed
_maxAcc, // maxAcc
time, // time
_startSpeed, // startSpeed
_endSpeed, // endSpeed
distance, // posOut
speedOut); // speedOut
if (!valid) return boost::none;
return RobotInstant(MotionInstant(_pathDirection * distance + _startPos,
_pathDirection * speedOut));
}
