diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8daeb93..0cd20ac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,24 +7,13 @@ 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.*; @@ -132,53 +121,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // no auto - 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)); + return new InstantCommand(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3d4cd7b..a583e33 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -508,10 +508,4 @@ 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