/** * Calculate up/down speed. * * @param vu * current up/down speed * @param dt * diff of time * @param time * current time * @return new forward speed */ private double calcUpSpeed(double vu, double dt, double time) { double desiredSpeed = 0; if (speeds.get(Speeds.MOVE_UP).isActive(time)) { desiredSpeed = UP_SPEED; } else if (speeds.get(Speeds.MOVE_DOWN).isActive(time)) { desiredSpeed = -UP_SPEED; } // Acceleration double a = UP_ACCELERATRION; return speedFollower(vu, dt, desiredSpeed, a); }
/** * Calculate side speed. * * @param vs * current side speed speed * @param dt * diff of time * @param time * current time * @return new forward speed */ private double calcSideSpeed(double vs, double dt, double time) { double desiredSpeed = 0; if (speeds.get(Speeds.MOVE_LEFT).isActive(time)) { desiredSpeed = -SIDE_SPEED; } else if (speeds.get(Speeds.MOVE_RIGHT).isActive(time)) { desiredSpeed = SIDE_SPEED; } // Acceleration double a = SIDE_ACCELERATRION; return speedFollower(vs, dt, desiredSpeed, a); }
/** * Calculate horizontal rotation speed. * * @param wh * current horizontal rotation speed * @param dt * diff of time * @param time * current time * @return new horizontal rotation */ private double calcHorizontallySpeed(double wh, double dt, double time) { double desiredSpeed = 0; double a = ROTATE_STOP_ACCELERATRION; if (speeds.get(Speeds.ROTATE_LEFT).isActive(time)) { desiredSpeed = ROTATE_SPEED; a = ROTATE_ACCELERATRION; } else if (speeds.get(Speeds.ROTATE_RIGHT).isActive(time)) { desiredSpeed = -ROTATE_SPEED; a = ROTATE_ACCELERATRION; } // Acceleration return speedFollower(wh, dt, desiredSpeed, a); }
return speedFollower(vf, dt, desiredSpeed, a);