diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4ae4474..0e904eb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,12 +25,7 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; - /* PID Constants Drive*/ - public static final int DRIVE_SLOT_IDX = 0; - public static final int DRIVE_PID_LOOP_IDX = 0; - public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); - + public static final double ENCODER_TICKS_PER_REV = 2048; } @@ -38,18 +33,7 @@ public final class Constants { public static final int INTAKE_SPARK_ID = 1; } - public static final class ElevatorConstants { - public static final int TALON_1 = 7; - public static final int TALON_2 = 8; - /* PID Constants Elevator */ - public static final int ELEVATOR_SLOT_IDX = 0; - public static final int ELEVATOR_PID_LOOP_IDX = 1; - public static final int ELEVATOR_TIMEOUT_MS = 30; - public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0); - - public static final double ENCODER_TICKS_PER_REV = 2048; - } public static final class ShooterConstants { public static final int SHOOTER_FALCON_ID = 8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 153c296..532edaa 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,9 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.DriveAtVelocityPID; -import frc4388.robot.commands.DriveToDistanceMM; -import frc4388.robot.commands.DriveToDistancePID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; @@ -81,15 +78,6 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveToDistancePID(m_robotDrive, 5000)); - - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000)); - - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java deleted file mode 100644 index 3d3e96c..0000000 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class DriveAtVelocityPID extends CommandBase { - Drive m_drive; - double m_targetVel; - double m_leftTarget; - double m_rightTarget; - /** - * Creates a new DriveAtVelocityPID. - */ - public DriveAtVelocityPID(Drive subsystem, double targetVel) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_targetVel = targetVel; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_targetVel; - m_rightTarget = -m_targetVel; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDriveVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java b/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java deleted file mode 100644 index df7ff44..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistanceMM.java +++ /dev/null @@ -1,57 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class DriveToDistanceMM extends CommandBase { - Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; - - /** - * Creates a new DriveToDistancePID. - */ - public DriveToDistanceMM(Drive subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_distance = distance; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java b/src/main/java/frc4388/robot/commands/DriveToDistancePID.java deleted file mode 100644 index 79b27e9..0000000 --- a/src/main/java/frc4388/robot/commands/DriveToDistancePID.java +++ /dev/null @@ -1,60 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class DriveToDistancePID extends CommandBase { - Drive m_drive; - double m_distance; - double m_leftTarget; - double m_rightTarget; - - /** - * Creates a new DriveToDistancePID. - */ - public DriveToDistancePID(Drive subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_distance = distance; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance; - m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance); - SmartDashboard.putNumber("Left Target", m_leftTarget); - SmartDashboard.putNumber("Right Target", m_rightTarget); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget); - m_drive.runDrivePositionPID(m_drive.m_rightFrontMotor, m_rightTarget); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){ - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 9ccb252..ef20818 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -34,8 +34,6 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - public static Gains m_driveGains = DriveConstants.DRIVE_GAINS; - /** * Add your docs here. */ @@ -53,63 +51,17 @@ public class Drive extends SubsystemBase { m_rightBackMotor.follow(m_rightFrontMotor); /* set neutral mode */ - setDriveTrainNeutralMode(NeutralMode.Brake); + setDriveTrainNeutralMode(NeutralMode.Coast); m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(true); + m_driveTrain.setRightSideInverted(true); m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - - /* Motor Encoders */ - //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - - /*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/ - - setDriveTrainGains(); - - m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Smart Dashboard Initial Values */ - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition()); - - SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); - - int closedLoopTimeMs = 1; - m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - - } @Override @@ -118,23 +70,9 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - - SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - - m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP); - m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); - m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); - m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); - - setDriveTrainGains(); - } catch (Exception e) { - - System.err.println("The programming team has failed successfully in the Drive Subsystem."); - + System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic."); + e.printStackTrace(System.err); } } @@ -149,23 +87,6 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setNeutralMode(mode); } - /** - * Add your docs here. - */ - public void setDriveTrainGains(){ - m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX); - m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS); - } - /** * Add your docs here. */ @@ -173,18 +94,6 @@ public class Drive extends SubsystemBase { m_driveTrain.arcadeDrive(move, steer); } - public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) { - talon.set(TalonFXControlMode.Position, targetPos); - } - - public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) { - talon.set(TalonFXControlMode.Velocity, targetVel); - } - - public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){ - talon.set(TalonFXControlMode.MotionMagic, targetPos); - } - public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr);