Examples with RamseteController edu.wpi.first.math.controller.RamseteController used on opensource projects

Example 6 with RamseteController

use of edu.wpi.first.math.controller.RamseteController in project RapidReact2022 by team223.

the class AutoDrive1 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;
    SmartDashboard.putNumber("Target X", goal.poseMeters.getX());
    SmartDashboard.putNumber("Target Y", goal.poseMeters.getY());
    SmartDashboard.putNumber("Actual X", RobotContainer.driveSubsystem.getPosition().getX());
    SmartDashboard.putNumber("Actual Y", RobotContainer.driveSubsystem.getPosition().getY());
    double right = wheelSpeeds.rightMetersPerSecond;
    double leftSpeed = RobotContainer.driveSubsystem.getLeftEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    double rightSpeed = RobotContainer.driveSubsystem.getRightEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    System.out.println(leftSpeed + "vs" + left);
    RobotContainer.driveSubsystem.setMotors(drivePid.calculate(leftSpeed, left), drivePid.calculate(rightSpeed, right));
}

Example 7 with RamseteController

use of edu.wpi.first.math.controller.RamseteController in project 2022 by Team5705.

the class RobotContainer method getAutonomousCommand.

// -----------------------------------------------------------------------------------------------------------------------------------------
public Command getAutonomousCommand() {
    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint = new DifferentialDriveVoltageConstraint(new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, 10);
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(pathWeaver.kMaxSpeedMetersPerSecond, pathWeaver.kMaxAccelerationMetersPerSecondSquared).setKinematics(pathWeaver.kDriveKinematics).addConstraint(autoVoltageConstraint);
    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
    new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
    List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
    new Pose2d(3, 0, new Rotation2d(0)), // Pass config
    config);
    RamseteCommand ramseteCommand = new RamseteCommand(// Trayectoria cargada desde pathweaver
    trajectory, powertrain::getPose, new RamseteController(pathWeaver.kRamseteB, pathWeaver.kRamseteZeta), new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, powertrain::getWheelSpeeds, new PIDController(pathWeaver.kPDriveVel, 0, 0), new PIDController(pathWeaver.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    powertrain::setVolts, powertrain);
    // Reset odometry to the starting pose of the trajectory.
    powertrain.resetOdometry(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> powertrain.setVolts(0, 0));
}

Example 8 with RamseteController

use of edu.wpi.first.math.controller.RamseteController in project RapidReactFRC by CyberLions-8521.

the class TrajectoryFollower method getRamseteCommand.

// classes
public static Command getRamseteCommand(Trajectory path, Drivebase m_drive) {
    RamseteCommand auto = new RamseteCommand(path, m_drive::getPose, new RamseteController(TrajectoryConstants.RAMSETE_B, TrajectoryConstants.RAMSETE_ZETA), // AutoConstants.autoMotorFeedforward,
    new SimpleMotorFeedforward(TrajectoryConstants.KS, TrajectoryConstants.KV, TrajectoryConstants.KA), TrajectoryConstants.DRIVE_KINEMATICS, m_drive::getWheelSpeeds, new PIDController(TrajectoryConstants.P_DRIVE, 0, 0), new PIDController(TrajectoryConstants.P_DRIVE, 0, 0), m_drive::tankDriveVolts, m_drive);
    m_drive.resetOdometry(path.getInitialPose());
    return auto.andThen(() -> m_drive.tankDriveVolts(0, 0));
}

Example 9 with RamseteController

use of edu.wpi.first.math.controller.RamseteController in project RobotCore by FRC-568.

the class RobotContainer method getAutonomousCommand.

public Command getAutonomousCommand() {
    final String trajectoryJSON = "PathWeaver/pathweaver.json";
    Trajectory trajectory = null;
    try {
        final Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
        trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (final IOException ex) {
        DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace());
    }
    final RamseteCommand ramseteCommand = new RamseteCommand(trajectory, drive::getPose, new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta), new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter), DriveConstants.kDriveKinematics, drive::getWheelSpeeds, new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(DriveConstants.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    drive::tankDriveVolts, drive);
    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> drive.tankDriveVolts(0, 0));
}

Example 10 with RamseteController

use of edu.wpi.first.math.controller.RamseteController 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));
}