Examples with State edu.wpi.first.math.trajectory.Trajectory.State used on opensource projects

Example 1 with State

use of edu.wpi.first.math.trajectory.Trajectory.State in project Entropy2022 by Team138Entropy.

the class AutoUtil method getInvertedStates.

/**
 * Returns a cloned, reversed list of the given states.
 * Each state is reconstructed with negative velocity and
 * the correct timepoint for the following backwards
 */
public static List<State> getInvertedStates(List<State> states) {
    List<State> invertedStates = new ArrayList<State>(states);
    Collections.reverse(invertedStates);
    for (int i = 0; i < invertedStates.size(); i++) {
        State currState = invertedStates.get(i);
        State newState = new State(states.get(i).timeSeconds, currState.velocityMetersPerSecond * -1, currState.accelerationMetersPerSecondSq, currState.poseMeters, currState.curvatureRadPerMeter);
        invertedStates.set(i, newState);
    }
    return invertedStates;
}

Example 2 with State

use of edu.wpi.first.math.trajectory.Trajectory.State in project RobotCode2022 by Mechanical-Advantage.

the class MotionProfileCommand method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    State setpoint = trajectory.sample(timer.get());
    Logger.getInstance().recordOutput("Odometry/ProfileSetpoint", new double[] { setpoint.poseMeters.getX(), setpoint.poseMeters.getY(), setpoint.poseMeters.getRotation().getRadians() });
    ChassisSpeeds chassisSpeeds = controller.calculate(drive.getPose(), setpoint);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
    drive.driveVelocity(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
}