Examples with ChassisSpeeds edu.wpi.first.math.kinematics.ChassisSpeeds used on opensource projects

Example 21 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project RapidReact2022 by team223.

the class DrivePath method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    RamseteController controller = new RamseteController();
    double currentTime = (double) (System.currentTimeMillis() - startTimeMillis) / 1000;
    Trajectory.State goal = trajectory.sample(currentTime);
    ChassisSpeeds updatedSpeeds = controller.calculate(RobotContainer.driveSubsystem.getPosition(), goal);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(updatedSpeeds);
    double left = wheelSpeeds.leftMetersPerSecond;
    double right = wheelSpeeds.rightMetersPerSecond;
    double leftSpeed = RobotContainer.driveSubsystem.getLeftEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    double rightSpeed = RobotContainer.driveSubsystem.getRightEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    System.out.println(RobotContainer.driveSubsystem.getPosition().getRotation().getDegrees() + "vs " + goal.poseMeters.getRotation().getDegrees());
    System.out.println("target: " + goal.poseMeters.getX() + ", " + goal.poseMeters.getY());
    System.out.println("actual: " + RobotContainer.driveSubsystem.getPosition().getX() + ", " + RobotContainer.driveSubsystem.getPosition().getY());
    RobotContainer.driveSubsystem.setMotors(drivePid.calculate(leftSpeed, left), drivePid.calculate(rightSpeed, -right));
}

Example 22 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project command by MilwaukeeCyberCheese.

the class AutoCommand method end.

/**
 * @param interrupted
 */
@Override
public void end(boolean interrupted) {
    m_autoSubsystem.stopStopwatch();
    m_autoSubsystem.drive(new ChassisSpeeds(0.0, 0.0, 0.0));
}

Example 23 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project command by MilwaukeeCyberCheese.

the class AutoSubsystem method printSpeeds.

public void printSpeeds() {
    String toPrint = "";
    toPrint += "{";
    for (int i = 0; i < speeds.size(); i++) {
        ChassisSpeeds speed = speeds.get(i);
        toPrint += "new ChassisSpeeds(";
        toPrint += speed.vxMetersPerSecond + ",";
        toPrint += speed.vyMetersPerSecond + ",";
        ;
        toPrint += speed.omegaRadiansPerSecond + ")";
        if (i != speeds.size() - 1) {
            toPrint += ",";
        }
    }
    toPrint += "}";
    System.out.println(toPrint);
}

Example 24 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds 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());
}

Example 25 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project FRC_Rapid_React_2022 by CommandoRobotics.

the class FollowTrajectoryCommand method initialize.

// Called when the command is initially scheduled.
@Override
public void initialize() {
    var initialState = m_trajectory.sample(0);
    var initialXVelocity = initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
    var initialYVelocity = initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
    m_prevSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
    // Post the current trajectory to the Field2d object
    Field2d field = (Field2d) SmartDashboard.getData("Field");
    field.getObject("Current Robot Trajectory").setTrajectory(m_trajectory);
    m_timer.reset();
    m_timer.start();
}