Examples with PathPlannerState com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState used on opensource projects

Example 1 with PathPlannerState

use of com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState in project FRC_Rapid_React_2022 by CommandoRobotics.

the class FollowTrajectoryCommand method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    double curTime = m_timer.get();
    double dt = curTime - m_prevTime;
    // Get the desired state of the robot (cast to a PathPlannerState to get rotation)
    var desiredState = (PathPlannerState) m_trajectory.sample(curTime);
    ChassisSpeeds targetChassisSpeeds;
    // Use the Holonomic Controller to get the ChassisSpeeds required to get to the next state based on the previous state
    if (usingCustomdriveRotationInput) {
        SmartDashboard.putBoolean("Using PathPlanner Rotation", false);
        targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
    } else {
        SmartDashboard.putBoolean("Using PathPlanner Rotation", true);
        targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, desiredState.holonomicRotation);
    }
    // Change ChassisSpeeds to WheelSpeeds (m/s) using mecanum kinematics
    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
    // Set the PID setpoints for each wheel
    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
    if (outputWheelSpeeds) {
        m_outputWheelSpeeds.accept(targetWheelSpeeds);
    } else {
        double frontLeftOutput;
        double rearLeftOutput;
        double frontRightOutput;
        double rearRightOutput;
        // Calculate the feedforward for each wheel based on the given feedforward for the chassis
        final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint, (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
        final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint, (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
        final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint, (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
        final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint, (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
        // Get the outputs of each PID controllers for each wheel based on the setpoint and feedforward
        frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
        rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
        frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
        rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
        // Feed the output voltages of the PID loops to the drive train as a MecanumDriveMotorVoltages
        m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
    }
    m_prevTime = curTime;
    m_prevSpeeds = targetWheelSpeeds;
}

Example 2 with PathPlannerState

use of com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState in project RapidReact2022-340 by Greater-Rochester-Robotics.

the class DriveFollowTrajectory method initialize.

// Called when the command is initially scheduled.
@Override
public void initialize() {
    RobotContainer.swerveDrive.setIsOdometry(true);
    timer.reset();
    timer.start();
    PathPlannerState initialState = (PathPlannerState) trajectory.sample(0.0);
    xController.reset();
    yController.reset();
    rotationController.reset(initialState.holonomicRotation.getDegrees());
    if (resetOdometry) {
        RobotContainer.swerveDrive.setCurPose2d(new Pose2d(initialState.poseMeters.getTranslation(), RobotContainer.swerveDrive.getGyroRotation2d()));
    // RobotContainer.swerveDrive.setGyro(initialState.holonomicRotation.getDegrees());
    }
}

Example 3 with PathPlannerState

use of com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState in project RapidReact by team3042.

the class PPMecanumControllerCommand method initialize.

@Override
public void initialize() {
    // Define the initial state of the trajectory
    PathPlannerState initialState = (PathPlannerState) m_trajectory.sample(0);
    Robot.drivetrain.resetOdometry(new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation));
    m_timer.reset();
    m_timer.start();
}

Example 4 with PathPlannerState

use of com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState in project RapidReact2022-340 by Greater-Rochester-Robotics.

the class DriveFollowTrajectory method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    double time = timer.get();
    PathPlannerState desiredState = (PathPlannerState) trajectory.sample(time);
    // System.out.println("HR:"+ desiredState.holonomicRotation.getDegrees());
    ChassisSpeeds robotSpeed = pathController.calculate(RobotContainer.swerveDrive.getCurPose2d(), desiredState, desiredState.holonomicRotation);
    RobotContainer.swerveDrive.driveRobotCentric(robotSpeed, true, false);
// Position Graph
// SmartDashboard.putNumber("PIDTarget", desiredState.getPos());
// SmartDashboard.putNumber("PIDActual", pathController.getTotalDistance());
// // Heading Graph
// SmartDashboard.putNumber("PIDTarget", desiredState.getHeading().getDegrees());
// SmartDashboard.putNumber("PIDActual", pathController.getCurrentHeading().getDegrees());
// Rotation Graph
// SmartDashboard.putNumber("PIDTarget", desiredState.getRotation().getDegrees());
// SmartDashboard.putNumber("PIDActual", RobotContainer.swerveDrive.getCurPose2d().getRotation().getDegrees());
}