diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0cd20ac..8daeb93 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,13 +7,24 @@ package frc4388.robot; +import java.util.List; + import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; @@ -121,7 +132,53 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // no auto - return new InstantCommand(); + var autoVoltageConstraint = + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10); + + TrajectoryConfig config = + new TrajectoryConfig( DriveConstants.kMaxSpeedMetersPerSecond, + DriveConstants.kMaxAccelerationMetersPerSecondSquared) + + .setKinematics(DriveConstants.kDriveKinematics) + + .addConstraint(autoVoltageConstraint); + + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(0)), + + List.of( + new Translation2d(1, 1), + new Translation2d(2, -1) + ), + + new Pose2d(3, 0, new Rotation2d(0)), + + config + ); + + RamseteCommand ramseteCommand = new RamseteCommand( + exampleTrajectory, + m_robotDrive::getPose, + new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta), + new SimpleMotorFeedforward( DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + m_robotDrive::getWheelSpeeds, + new PIDController(DriveConstants.kPDriveVel, 0, 0), + new PIDController(DriveConstants.kPDriveVel, 0, 0), + + m_robotDrive::tankDriveVolts, + m_robotDrive + ); + + return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a583e33..3d4cd7b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -508,4 +508,10 @@ public class Drive extends SubsystemBase { public double ticksToInches(double ticks) { return ticks * DriveConstants.INCHES_PER_TICK; } + + public void tankDriveVolts(double leftVolts, double rightVolts) { + m_leftFrontMotor.setVoltage(leftVolts); + m_rightFrontMotor.setVoltage(-rightVolts); + m_driveTrain.feedWatchdog(); + } } \ No newline at end of file