diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 97d01af..a40e9c9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -12,7 +12,7 @@ import java.util.function.Consumer; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +// import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -113,7 +113,7 @@ public final class Constants { public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; public static final double TRACK_WIDTH_METERS = 0.648; - public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); + // public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); /* Remote Sensors */ public static final int REMOTE_0 = 0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 11d171a..5e3b0d4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -73,7 +73,7 @@ public class Robot extends TimedRobot { */ @Override public void disabledInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + // m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); /* Builds Autos */ m_robotContainer.buildAutos(); SmartDashboard.putString("Is Auto Start?", "NAH"); @@ -82,7 +82,7 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - m_robotContainer.resetOdometry(new Pose2d()); + // m_robotContainer.resetOdometry(new Pose2d()); // m_robotContainer.idenPath(); if (m_modeChooser.getSelected() != Mode.get()) Mode.set(m_modeChooser.getSelected()); @@ -96,7 +96,7 @@ public class Robot extends TimedRobot { //m_robotContainer.buildAutos(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + // m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); // m_robotContainer.setDriveGearState(true); m_initialTime = System.currentTimeMillis(); @@ -134,7 +134,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); + // m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); // m_robotContainer.setDriveGearState(true); // m_robotContainer.shiftClimberRatchet(false); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2fec160..3a0f5d3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,8 +47,8 @@ import frc4388.robot.Constants.OIConstants; // import frc4388.robot.commands.climber.DisengageRatchet; // import frc4388.robot.commands.climber.RunClimberWithTriggers; // import frc4388.robot.commands.climber.RunLevelerWithJoystick; -import frc4388.robot.commands.drive.DriveWithJoystick; -import frc4388.robot.commands.drive.PlaySongDrive; +// import frc4388.robot.commands.drive.DriveWithJoystick; +// import frc4388.robot.commands.drive.PlaySongDrive; // import frc4388.robot.commands.drive.SetShooterToOdo; import frc4388.robot.commands.drive.VisionUpdateOdometry; // import frc4388.robot.commands.intake.RunIntakeWithTriggers; @@ -62,12 +62,12 @@ import frc4388.robot.commands.drive.VisionUpdateOdometry; // import frc4388.robot.commands.storage.ManageStorage; import frc4388.robot.subsystems.Camera; // import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Drive; +// import frc4388.robot.subsystems.Drive; // import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.Leveler; // import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.Pneumatics; +// import frc4388.robot.subsystems.Pneumatics; // import frc4388.robot.subsystems.Shooter_1; // import frc4388.robot.subsystems.ShooterAim_1; // import frc4388.robot.subsystems.ShooterHood_1; @@ -86,7 +86,7 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* Subsystems */ - private final Drive m_robotDrive = new Drive(); + // private final Drive m_robotDrive = new Drive(); // private final Pneumatics m_robotPneumatics = new Pneumatics(); private final LED m_robotLED = new LED(); // private final Intake m_robotIntake = new Intake(); @@ -194,11 +194,11 @@ public class RobotContainer { // A driver test button // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)); // B driver test button - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); - // Y driver test button - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON).whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive)); - // X driver test button - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand()); + // new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); + // // Y driver test button + // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON).whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive)); + // // X driver test button + // new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand()); /* Driver Buttons */ // sets solenoids into high gear // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); @@ -230,8 +230,8 @@ public class RobotContainer { // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) // .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); - // new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - // .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive)); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(new VisionUpdateOdometry(m_robotVision));//, m_robotShooterAim, m_robotDrive)); // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); @@ -381,15 +381,15 @@ public class RobotContainer { } //TODO - TrajectoryConfig getTrajectoryConfig() { - return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics); - } + // TrajectoryConfig getTrajectoryConfig() { + // return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) + // // Add kinematics to ensure max speed is actually obeyed + // .setKinematics(DriveConstants.kDriveKinematics); + // } - public RamseteCommand getRamseteCommand(Trajectory trajectory) { - return new RamseteCommand(trajectory, m_robotDrive::getPose, new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive); - } + // public RamseteCommand getRamseteCommand(Trajectory trajectory) { + // return new RamseteCommand(trajectory, m_robotDrive::getPose, new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive); + // } public RamseteCommand[] buildPaths(String[] paths) { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; @@ -407,8 +407,8 @@ public class RobotContainer { Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); initialTrajectory = trajectory; - RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); - ramseteCommands[0] = ramseteCommand; + // RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + // ramseteCommands[0] = ramseteCommand; times[0] = initialTrajectory.getTotalTimeSeconds(); } @@ -417,8 +417,8 @@ public class RobotContainer { String trajectoryJSON = "paths/" + path + ".wpilib.json"; Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); - RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); - ramseteCommands[i] = ramseteCommand; + // RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + // ramseteCommands[i] = ramseteCommand; times[i] = trajectory.getTotalTimeSeconds(); } } catch (Exception e) { @@ -435,9 +435,9 @@ public class RobotContainer { * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to */ - public void setDriveNeutralMode(NeutralMode mode) { - m_robotDrive.setDriveTrainNeutralMode(mode); - } + // public void setDriveNeutralMode(NeutralMode mode) { + // m_robotDrive.setDriveTrainNeutralMode(mode); + // } /** * Sets the gear of the drivetrain @@ -451,13 +451,13 @@ public class RobotContainer { // m_robotClimber.shiftServo(state); // } - public void resetOdometry(Pose2d pose) { - m_robotDrive.setOdometry(pose); - } + // public void resetOdometry(Pose2d pose) { + // m_robotDrive.setOdometry(pose); + // } - public void resetGyroYawRobotContainer(double angle) { - m_robotDrive.resetGyroYaw(angle); - } + // public void resetGyroYawRobotContainer(double angle) { + // m_robotDrive.resetGyroYaw(angle); + // } /** * Used for analog inputs like triggers and axises. diff --git a/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java deleted file mode 100644 index 901fdcb..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java +++ /dev/null @@ -1,93 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class DrivePositionMPAux extends CommandBase { - Drive m_drive; - double m_cruiseVel; - double m_rampDist; - double m_targetPos; - double m_currentVel; - double m_currentPos; - double m_targetGyro; - double m_targetVel; - double m_rampAcc; - long m_startTime; - long m_rampRate; - int m_counter; - - /** - * Creates a new DrivePositionMPAux. - * - * @param subsystem The drive subsystem - * @param cruiseVel The target velocity for the motors in in/s - * @param rampDist The distance before cruise velocity is reached in inches - * @param rampRate The time to reach the cruise velocity in seconds - * @param targetPos The target position - */ - public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10; - m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW; - m_rampRate = (long) rampRate * 1000; - m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW; - //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360; - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_currentVel = m_drive.m_rightFrontMotorVel; - m_currentPos = m_drive.m_rightFrontMotorPos; - m_targetPos = m_targetPos + m_currentPos; - m_targetVel = m_currentVel; - m_startTime = System.currentTimeMillis(); - m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate; - m_counter = 0; - } - - // Called every m_isRamptime the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_currentVel = m_drive.m_rightFrontMotorVel; - m_currentPos = m_drive.m_rightFrontMotorPos; - if (System.currentTimeMillis() - m_startTime < m_rampRate) { - // Ramping - m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs; - m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); - } else if (m_targetPos - m_currentPos > m_rampDist) { - // Cruising - m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro); - } else { - // Deramp PID - m_drive.runDrivePositionPID(m_targetPos, m_targetGyro); - } - m_counter ++; - } - - // 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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) { - //return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java deleted file mode 100644 index 3f63d25..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class DriveStraightAtVelocityPID extends CommandBase { - Drive m_drive; - double m_targetVel; - double m_targetGyro; - /** - * Creates a new DriveStraightAtVelocityPID. - * @param subsystem The drive subsystem - * @param targetVel The target velocity for the motors in units - */ - public DriveStraightAtVelocityPID(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_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); - } - - // 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/drive/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java deleted file mode 100644 index 1ec57b0..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java +++ /dev/null @@ -1,86 +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.drive; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -public class DriveStraightToPositionMM extends CommandBase { - Drive m_drive; - Pneumatics m_pneumatics; - double m_targetPosIn; - double m_targetPosOut; - double m_targetGyro; - boolean isGoneFast; - int i; - - /** - * Creates a new DriveToDistancePID. - * @param subsystem drive subsystem - * @param targetPos distance to travel in inches - */ - public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_pneumatics = subsystem2; - try { - if (m_pneumatics.m_isSpeedShiftHigh) { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; - } else { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; - } - } catch (Exception e) { - System.err.println("Error In Motion Magic Switch Gains."); - } - addRequirements(m_drive); - //SmartDashboard.putNumber("Distance Target Inches", targetPos); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - System.err.println("PID START \n | \n |"); - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY); - isGoneFast = false; - i = 0; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); - m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); - //SmartDashboard.putBoolean("MM Run", true); - i++; - } - - // 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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ - //SmartDashboard.putBoolean("MM Run", false); - return true; - } else { - if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) { - isGoneFast = true; - } - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java deleted file mode 100644 index b910380..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java +++ /dev/null @@ -1,79 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -public class DriveStraightToPositionPID extends CommandBase { - Drive m_drive; - Pneumatics m_pneumatics; - double m_targetPosIn; - double m_targetPosOut; - double m_targetGyro; - int i; - - /** - * Creates a new DriveToDistancePID. - * @param subsystem drive subsystem - * @param targetPos distance to travel in inches - */ - public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_pneumatics = subsystem2; - try { - if (m_pneumatics.m_isSpeedShiftHigh) { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; - } else { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; - } - } catch (Exception e) { - System.err.println("Error In Motion Magic Switch Gains."); - } - addRequirements(m_drive); - //SmartDashboard.putNumber("Distance Target Inches", targetPos); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //System.err.println("PID START \n | \n |"); - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY); - i = 0; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro); - } - - // 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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){ - return true; - } else { - i++; - //System.err.println(i); - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java deleted file mode 100644 index 782e59f..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java +++ /dev/null @@ -1,64 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; -import frc4388.utility.controller.IHandController; - -public class DriveWithJoystick extends CommandBase { - private Drive m_drive; - private IHandController m_controller; - private Pneumatics m_pneumatics; - - /** - * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller. - * Applies a curved ramp to the input from the controllers to make the robot less "touchy". - * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_pneumatics = subsystem2; - m_controller = controller; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double moveInput = m_controller.getLeftYAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR; - double steerInput = m_controller.getRightXAxis() * DriveConstants.STEER_WITH_JOYSTICK_FACTOR; - double moveOutput = Math.copySign(1 - Math.cos(Math.PI * moveInput / 2), moveInput); - double cosMultiplier = m_pneumatics.m_isSpeedShiftHigh ? DriveConstants.COS_MULTIPLIER_HIGH : DriveConstants.COS_MULTIPLIER_LOW; - double deadzone = 0.1; - double steerOutput = Math.copySign(Math.cos(Math.PI * steerInput / 2) * (deadzone - cosMultiplier) + cosMultiplier, steerInput); - m_drive.driveWithInput(moveOutput, steerOutput * 0.8); - } - - // 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/drive/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java deleted file mode 100644 index 6444141..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java +++ /dev/null @@ -1,77 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpiutil.math.MathUtil; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.utility.controller.IHandController; - -public class DriveWithJoystickAuxPID extends CommandBase { - Drive m_drive; - double m_targetGyro; - long lastTime; - IHandController m_controller; - - /** - * Creates a new DriveWithJoystickAuxPID. - */ - public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_controller = controller; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - lastTime = System.currentTimeMillis(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - double moveInput = m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); - double moveOutput = 0; - long deltaTime = System.currentTimeMillis() - lastTime; - lastTime = System.currentTimeMillis(); - if (moveInput >= 0){ - moveOutput = -Math.cos(1.571*moveInput)+1; - } else { - moveOutput = Math.cos(1.571*moveInput)-1; - } - - m_targetGyro += 2 * steerInput * deltaTime; - - m_targetGyro = MathUtil.clamp(m_targetGyro, - currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), - currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); - - m_drive.driveWithInputAux(moveOutput, m_targetGyro); - - //System.err.println("Target: " + m_targetGyro); - //System.err.println("Current: " + currentGyro); - //System.err.println(); - } - - // 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/drive/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java deleted file mode 100644 index 56d2f96..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java +++ /dev/null @@ -1,142 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; -import frc4388.utility.controller.IHandController; - -public class DriveWithJoystickDriveStraight extends CommandBase { - Drive m_drive; - Pneumatics m_pneumatics; - double m_targetGyro, m_currentGyro; - double m_stopPos; - long m_currTime, m_deltaTime; - long m_deadTimeSteer, m_deadTimeMove; - long m_deadTimeout = 100; - IHandController m_controller; - boolean m_isInterrupted; - double highGearMultiplier = 1; - double lowGearMultiplier = 1; - - /** - * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller. - * Applies a curved ramp to the input from the controllers to make the robot less "touchy". - * Also uses PIDs to keep the robot on course when given a "dead" or 0 input. - * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_pneumatics = m_drive.m_pneumaticsSubsystem; - m_controller = controller; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_currTime = System.currentTimeMillis(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1); - double moveInput = -m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); - double moveOutput = 0; - m_deltaTime = System.currentTimeMillis() - m_currTime; - m_currTime = System.currentTimeMillis(); - - if (m_isInterrupted) { - resetGyroTarget(); - m_isInterrupted = false; - } - - /* If steer stick is being used */ - if (steerInput != 0) { - m_deadTimeSteer = m_currTime; - } - - /* Curves the moveInput to be slightly more gradual at first */ - if (moveInput >= 0) { - moveOutput = -Math.cos(1.571*moveInput)+1; - } else { - moveOutput = Math.cos(1.571*moveInput)-1; - } - - /* If steer stick has not been used for less than 1 sec */ - if (m_currTime - m_deadTimeSteer < m_deadTimeout) { - runDriveWithInput(moveOutput, steerInput); - resetGyroTarget(); - } - /* If steer stick has not been used for 1 sec */ - else { - runDriveStraight(moveOutput); - } - } - - private void runDriveWithInput(double move, double steer) { - - double cosMultiplier; - double steerOutput = 0; - double deadzone = .1; - - if (m_pneumatics.m_isSpeedShiftHigh) { - cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; - } else { - cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; - } - - /* Curves the steer output to be similarily gradual */ - if (steer > 0) { - steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier; - } else if (steer < 0) { - steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier; - } - m_drive.driveWithInput(move, steerOutput); - //System.out.println("Driving With Input"); - } - - private void runDriveStraight(double move) { - m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - //System.out.println("Driving Straight with Target: " + m_targetGyro); - } - - /** - * set target angle to current angle (prevents buildup of gyro error). - */ - private void resetGyroTarget() { - //m_targetGyro = m_currentGyro; - if (m_pneumatics.m_isSpeedShiftHigh) { - m_targetGyro = m_currentGyro - + highGearMultiplier * m_drive.getTurnRate(); - } else { - m_targetGyro = m_currentGyro - + lowGearMultiplier * m_drive.getTurnRate(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_isInterrupted = interrupted; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java deleted file mode 100644 index 13523af..0000000 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java +++ /dev/null @@ -1,194 +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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpiutil.math.MathUtil; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; -import frc4388.utility.controller.IHandController; - -public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { - Drive m_drive; - Pneumatics m_pneumatics; - double m_targetGyro, m_currentGyro; - double m_stopPos; - long m_currTime, m_deltaTime; - long m_deadTimeSteer, m_deadTimeMove; - long m_deadTimeout = 100; - IHandController m_controller; - boolean m_isInterrupted; - - /* Deadassist Constants */ - final float stopPosVelCoefLow = 1; - final float stopPosVelCoefHigh = 3; - final float cosMultiplierLow = 0.55f; - final float cosMultiplierHigh = 0.35f; - final float targetAngleCoefLow = 5; - final float targetAngleCoefHigh = 5; - final float gyroVelCoefLow = 1; - final float gyroVelCoefHigh = 3; - - /* Deadassist Coeficients */ - final float stopPosVelCoef = 1; - final float cosMultiplier = 0.55f; - final float targetAngleCoef = 5; - final float gyroVelCoef = 1; - - /** - * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. - * Applies a curved ramp to the input from the controllers to make the robot less "touchy". - * Also uses PIDs to keep the robot on course when given a "dead" or 0 input. - * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystemDrive; - m_pneumatics = subsystemPneumatics; - m_controller = controller; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_currTime = System.currentTimeMillis(); - resetGyroTarget(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); - double moveInput = -m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); - double moveOutput = 0; - m_deltaTime = System.currentTimeMillis() - m_currTime; - m_currTime = System.currentTimeMillis(); - - if (m_isInterrupted) { - resetGyroTarget(); - m_isInterrupted = false; - } - - /* If move stick is being used */ - if (moveInput != 0) { - m_deadTimeMove = m_currTime; - m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition() - + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity()); - } - /* If steer stick is being used */ - if (steerInput != 0) { - m_deadTimeSteer = m_currTime; - } - - /* Curves the moveInput to be slightly more gradual at first */ - if (moveInput >= 0) { - moveOutput = -Math.cos(1.571*moveInput)+1; - } else { - moveOutput = Math.cos(1.571*moveInput)-1; - } - - if (m_pneumatics.m_isSpeedShiftHigh) { - runDriveWithInput(moveOutput, steerInput); - resetGyroTarget(); - } - /* If move stick has been pressed within 1 sec */ - else if (m_currTime - m_deadTimeMove < m_deadTimeout) { - /* If steer stick has not been used for less than 1 sec */ - if (m_currTime - m_deadTimeSteer < m_deadTimeout) { - runDriveWithInput(moveOutput, steerInput); - resetGyroTarget(); - } - /* If steer stick has not been used for 1 sec */ - else { - runDriveStraight(moveOutput); - } - } - /* If the move stick has not been used for 1 sec */ - else { - runStoppedTurn(steerInput); - } - } - - private void runDriveWithInput(double move, double steerInput) { - double cosMultiplier = .70; - double steerOutput = 0; - double deadzone = .1; - /* Curves the steer output to be similarily gradual */ - if (steerInput > 0){ - steerOutput = -(cosMultiplier - deadzone)*Math.cos(1.571*steerInput)+(cosMultiplier); - } else if (steerInput < 0) { - steerOutput = (cosMultiplier - deadzone)*Math.cos(1.571*steerInput)-(cosMultiplier); - } - - m_drive.driveWithInput(move, steerOutput); - //System.out.println("Driving With Input"); - } - - private void runDriveStraight(double move) { - m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - //System.out.println("Driving Straight with Target: " + m_targetGyro); - } - - private void runStoppedTurn(double steer) { - double cosMultiplier = 0.55; - double steerOutput = 0; - double deadzone = .2; - /* Curves the steer output to be similarily gradual */ - if (steer > 0) { - steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone); - } else if (steer < 0) { - steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone); - } - - updateGyroTarget(steerOutput); - double currentPos = m_drive.m_rightFrontMotorPos; - if (Math.abs(currentPos - m_stopPos) > 200) { - m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); - } else { - m_drive.driveWithInputAux(0, m_targetGyro); - } - //System.out.println("Turning with Target: " + m_targetGyro); - } - - /** - * If AuxPID is enabled, then update using the steer input - */ - private void updateGyroTarget(double steerInput) { - m_targetGyro -= 5 * steerInput * m_deltaTime; - m_targetGyro = MathUtil.clamp( m_targetGyro, - m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3), - m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3)); - } - - /** - * set target angle to current angle (prevents buildup of gyro error). - */ - private void resetGyroTarget() { - //m_targetGyro = m_currentGyro; - m_targetGyro = m_currentGyro - + m_drive.getTurnRate(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_isInterrupted = interrupted; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java deleted file mode 100644 index 6165ffd..0000000 --- a/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java +++ /dev/null @@ -1,28 +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.drive; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GotoCoordinatesFieldRelative extends SequentialCommandGroup { - /** - * Creates a new GotoCoordinatesFieldRelative. - */ - public GotoCoordinatesFieldRelative(Drive susbsytem, Pneumatics subsystem2, double xTarget, double yTarget) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java deleted file mode 100644 index eaa0f07..0000000 --- a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.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.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class PlaySongDrive extends CommandBase { - private Drive m_drive; - - /** - * Creates a new PlaySongDrive. - */ - public PlaySongDrive(Drive subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_drive.m_rightFrontMotor.set(0); - m_drive.m_leftFrontMotor.set(0); - m_drive.m_rightBackMotor.set(0); - m_drive.m_leftBackMotor.set(0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_drive.playSong(); - //System.err.println("Playing " + m_drive.m_orchestra.isPlaying()); - //m_drive.m_driveTrain.feedWatchdog(); - } - - // 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/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index cee917a..602a56f 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -23,14 +23,14 @@ import edu.wpi.first.wpiutil.math.numbers.N5; import edu.wpi.first.wpiutil.math.numbers.N6; import frc4388.robot.Constants.VOPConstants; import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Drive; +// import frc4388.robot.subsystems.Drive; // import frc4388.robot.subsystems.ShooterAim_1; import frc4388.robot.subsystems.Vision; public class VisionUpdateOdometry extends CommandBase { private Vision m_limeLight; // private ShooterAim_1 m_shooterAim; - private Drive m_driveTrain; + // private Drive m_driveTrain; private double xPos; private double yPos; @@ -44,11 +44,11 @@ public class VisionUpdateOdometry extends CommandBase { * @param shooterAim replace with Turret subsystem for integration with 2022 * @param driveTrain replace with Swerve subsystem for integration with 2022 */ - public VisionUpdateOdometry(Vision limeLight, Drive driveTrain) { + public VisionUpdateOdometry(Vision limeLight) { m_limeLight = limeLight; // m_shooterAim = shooterAim; - m_driveTrain = driveTrain; - addRequirements(m_limeLight, m_driveTrain); + // m_driveTrain = driveTrain; + addRequirements(m_limeLight);//, m_driveTrain); // // Turn camera on but leave LEDs off // NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); @@ -90,16 +90,16 @@ public class VisionUpdateOdometry extends CommandBase { System.out.println("Never gonna give you up: " + distance); double[] ypr = new double[3]; - Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022 - double relativeAngle = Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]); - rotation = new Rotation2d(ypr[0]); + // Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022 + double relativeAngle = 0;//Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]); + rotation = new Rotation2d(0); xPos = Math.cos(relativeAngle) * distance; yPos = Math.sin(relativeAngle) * distance; translate = new Translation2d(xPos, yPos); Pose2d pose = new Pose2d(translate, rotation); - m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter + // m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter SmartDashboard.putNumber("x:", pose.getX()); SmartDashboard.putNumber("y:", pose.getY()); m_limeLight.setLEDs(false); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java deleted file mode 100644 index 945b2b0..0000000 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ /dev/null @@ -1,951 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 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.subsystems; - -import java.io.File; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.FollowerType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.SensorTerm; -import com.ctre.phoenix.motorcontrol.StatusFrame; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.music.Orchestra; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.GyroBase; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.Gains; - -public class Drive extends SubsystemBase { - /* Create Motors, Gyros, etc */ - public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); - public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - public static GyroBase m_pigeonGyro; - public boolean m_isReversed; - - /* Drive objects to manage Drive Train */ - public DifferentialDrive m_driveTrain; - public final DifferentialDriveOdometry m_odometry; - public Orchestra m_orchestra; - - /* Pneumatics Subsystem */ - public Pneumatics m_pneumaticsSubsystem; - // Shooter_1 m_shooter; - - /* Low Gear Gains */ - public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; - public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; - public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW; - public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW; - - /* High Gear Gains */ - public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH; - public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH; - public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH; - public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; - - /* Back Motor Gains */ - public static Gains m_gainsVelocityBack = DriveConstants.DRIVE_VELOCITY_GAINS_BACK; - - /* Timey Whimey */ - public long m_currentTimeMs = System.currentTimeMillis(); - public long m_lastTimeMs = m_currentTimeMs; - public long m_deltaTimeMs = 0; - public long m_currentTimeSec = m_currentTimeMs / 1000; - - /* Position Tracking */ - public double m_rightFrontMotorPos = 0; - public double m_rightFrontMotorVel = 0; - - public double m_totalLeftDistanceInches = 0; - public double m_totalRightDistanceInches = 0; - - public double m_currentLeftPosTicks = 0; - public double m_currentRightPosTicks = 0; - public double m_lastLeftPosTicks = 0; - public double m_lastRightPosTicks = 0; - - public double m_lastAngleYaw = 0; - public double m_currentAngleYaw = 0; - - public double m_lastAngleGotoCoordinates; - /* Smart Dashboard Objects */ - SendableChooser m_songChooser = new SendableChooser(); - - /* Misc */ - public String m_currentSong = ""; - public String[] songsStrings; - - /** - * Add your docs here. - */ - public Drive() { - /* factory default values */ - m_leftFrontMotor.configFactoryDefault(); - m_rightFrontMotor.configFactoryDefault(); - m_leftBackMotor.configFactoryDefault(); - m_rightBackMotor.configFactoryDefault(); - m_pigeon.configFactoryDefault(); - resetGyroYaw(0); - - m_pigeonGyro = getGyroInterface(); - - /* Config Open Loop Ramp so we don't make sudden output changes */ - m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Config Supply Current Limit (Use only for debugging) */ - // m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - // m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - // m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - // m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - - /* Config deadbands so that */ - m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); - - /* PID for Front Motor Control in Teleop */ - try { - if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { - setRightMotorGains(true); - } else { - setRightMotorGains(false); - } - } catch (Exception e) { - System.err.println("Error while trying to switch gains."); - } - - /* PID for Back Motor Control in Tank Drive Vel */ - m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Reset Sensors for WPI_TalonFXs */ - resetEncoders(); - - /* Configure the left Talon's selected sensor as local QuadEncoder */ - m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source - DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - - /* Configure the left back Talon's selected sensor as local QuadEncoder */ - m_leftBackMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source - DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - - /* Configure the right back Talon's selected sensor as local QuadEncoder */ - m_rightBackMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, // Local Feedback Source - DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - - /* - * Configure the Remote Talon's selected sensor as a remote sensor for the right - * Talon - */ - m_rightFrontMotor.configRemoteFeedbackFilter(m_leftFrontMotor.getDeviceID(), // Device ID of Source - RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - - /* Diff Signal signal to be used for Distance */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, - DriveConstants.DRIVE_TIMEOUT_MS); - - /* Configure Diff [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.configSelectedFeedbackCoefficient(0.5, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); - - /* - * Configure the Pigeon IMU to the other Remote Slot available on the right - * Talon - */ - m_rightFrontMotor.configRemoteFeedbackFilter(m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw, - DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Config Remote1 to be used for Aux PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, - DriveConstants.DRIVE_TIMEOUT_MS); - - /** - * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local - * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's - * local output is PID0 - PID1, and other side Talon is PID0 + PID1 - */ - m_rightFrontMotor.configAuxPIDPolarity(DriveConstants.isAuxPIDInverted, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Set status frame periods to ensure we don't have stale data */ - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); - - /** - * Max out the peak output (for all modes). However you can limit the output of - * a given PID object with configClosedLoopPeakOutput(). - */ - m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); - - /** - * 1ms per loop. PID loop can be slowed down if need be. For example, - if - * sensor updates are too slow - sensor deltas are very small per update, so - * derivative error never gets large enough to be useful. - sensor movement is - * very slow causing the derivative error to be near zero. - */ - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, DriveConstants.CLOSED_LOOP_TIME_MS, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_leftBackMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightBackMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, DriveConstants.CLOSED_LOOP_TIME_MS, - DriveConstants.DRIVE_TIMEOUT_MS); - - /* Set up Differential Drive */ - m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - - /* Set up Differential Drive Odometry. */ - m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), - new Pose2d(0, 0, new Rotation2d())); - - /* Set up Orchestra */ - m_orchestra = new Orchestra(); - - /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); - m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); - m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); - - /* Set up music for drive train */ - m_orchestra.addInstrument(m_leftBackMotor); - m_orchestra.addInstrument(m_rightFrontMotor); - m_orchestra.addInstrument(m_rightBackMotor); - m_orchestra.addInstrument(m_leftFrontMotor); - - /* Create chooser to choose song to play */ - File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs"); - System.err.println(songsDir.getPath()); - songsStrings = songsDir.list(); - for (String songString : songsStrings) { - m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString); - } - Shuffleboard.getTab("Songs").add(m_songChooser); - selectSong(songsStrings[0]); - - /* Start counting time */ - m_lastTimeMs = System.currentTimeMillis(); - } - - @Override - public void periodic() { - updateTime(); - updateAngles(); - updatePosition(); - updateSmartDashboard(); - } - - /** - * Passes subsystem needed. - * - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(Pneumatics subsystem) { - m_pneumaticsSubsystem = subsystem; - // m_shooter = shooter; - // m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); - } - - public void updateTime() { - m_lastTimeMs = m_currentTimeMs; - m_currentTimeMs = System.currentTimeMillis(); - m_currentTimeSec = m_currentTimeMs / 1000; - m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; - } - - public void updateAngles() { - m_lastAngleYaw = m_currentAngleYaw; - m_currentAngleYaw = getGyroYaw(); - } - - public void updatePosition() { - m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); - m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); - - m_lastRightPosTicks = m_currentRightPosTicks; - m_lastLeftPosTicks = m_currentLeftPosTicks; - m_currentRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition(); - m_currentLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(); - - m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks); - m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); - - updateOdometry(m_isReversed); - } - - public void updateOdometry(boolean reversed){ - if (reversed){ - - m_odometry.update(Rotation2d.fromDegrees( -getGyroYaw()-180), - -inchesToMeters(getDistanceInches(m_rightFrontMotor)), - inchesToMeters(getDistanceInches(m_leftFrontMotor))); - } - else - { - m_odometry.update(Rotation2d.fromDegrees( getHeading()), - inchesToMeters(getDistanceInches(m_leftFrontMotor)), - -inchesToMeters(getDistanceInches(m_rightFrontMotor))); - } - } - - public void switchReversed(boolean reversed) - { - m_isReversed = reversed; - } - - /** - * Runs percent output control on the moving and steering of the drive train, - * using the Differential Drive class to manage the two inputs - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(-move, steer); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); - } - - /** - * Runs percent output control on the drive train while using an AUX PID for - * rotation - * - * @param targetPos The position to drive to in units - * @param targetGyro The angle to drive at in units - */ - public void driveWithInputAux(double move, double targetGyro) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - - m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro); - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); - - m_driveTrain.feedWatchdog(); - } - - /** - * Runs position PID. Position is absolute and displacement should be handled on - * the command side. - * - * @param targetPos The position to drive to in units - * @param targetGyro The angle to drive at in units - */ - public void runDrivePositionPID(double targetPos, double targetGyro) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - - m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro); - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); - - m_driveTrain.feedWatchdog(); - } - - /** - * Runs velocity PID - * - * @param targetVel The velocity to drive at in units - * @param targetGyro The angle to drive at in units - */ - public void runDriveVelocityPID(double targetVel, double targetGyro) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - - m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); - - m_driveTrain.feedWatchdog(); - } - - /** - * Runs motion magic PID while driving straight - * - * @param targetPos The position to drive to in units - * @param targetGyro The angle to drive at in units - */ - public void runMotionMagicPID(double targetPos, double targetGyro) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - - m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); - m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); - - m_driveTrain.feedWatchdog(); - - } - - /** - * Runs a Turning PID to rotate a to a target angle - * - * @param targetAngle target angle in degrees - */ - public void runTurningPID(double targetAngle) { - // double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - - runDriveVelocityPID(0, targetAngle); - } - - /** - * Controls the left and right sides of the drive with velocity targets. - * - * @param leftSpeed the commanded left speed - * @param rightSpeed the commanded right speed - */ - public void tankDriveVelocity(double leftSpeed, double rightSpeed) { - // DifferentialDriveWheelSpeeds wheelSpeeds = new - // DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); - // ChassisSpeeds chassisSpeeds = - // DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds); - // double moveVelMPS = chassisSpeeds.vxMetersPerSecond; - // double angleVelRad = chassisSpeeds.omegaRadiansPerSecond; - // double angleVelDeg = Math.toDegrees(angleVelRad); - - // m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000); - // m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, - // m_currentAngleYaw-(360), - // m_currentAngleYaw+(360)); - // double targetGyro = (m_kinematicsTargetAngle / 360) * - // DriveConstants.TICKS_PER_GYRO_REV; - double moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME; - double moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME; - - // SmartDashboard.putNumber("Move Vel Left", moveVelLeft); - // SmartDashboard.putNumber("Move Vel Right", moveVelRight); - - // runDriveVelocityPID(moveVel*2, targetGyro); - - m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - - m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight); - m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft); - m_leftFrontMotor.follow(m_leftBackMotor); - m_rightFrontMotor.follow(m_rightBackMotor); - - m_driveTrain.feedWatchdog(); - } - - /** - * Selects a song to play! - * - * @param song The name of the song to be played - */ - public void selectSong(String song) { - SmartDashboard.putString("Selected Song", song); - m_orchestra.loadMusic(song); - } - - /* - * Plays Music! - */ - public void playSong() { - m_orchestra.play(); - } - - /** - * Resets the encoders for both motors. - */ - public void resetEncoders() { - m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - - m_totalLeftDistanceInches = 0; - m_totalRightDistanceInches = 0; - } - - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void setOdometry(Pose2d pose) { - resetEncoders(); - resetGyroYaw(pose.getRotation().getDegrees()); - m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); - } - - - /** - * Resets the yaw of the pigeon - */ - public void resetGyroYaw(double angle) { - m_pigeon.setYaw(angle); - m_pigeon.setAccumZAngle(angle); - resetGyroAngles(angle); - } - - /** - * Add docs here - */ - public void resetGyroAngles(double angle) { - m_lastAngleYaw = angle; - m_currentAngleYaw = angle; - } - - /** - * Returns the current yaw of the pigeon - */ - public double getGyroYaw() { - - double[] ypr = new double[3]; - - m_pigeon.getYawPitchRoll(ypr); - return ypr[0]; - } - - /** - * Returns the current pitch of the pigeon - */ - public double getGyroPitch() { - double[] ypr = new double[3]; - - m_pigeon.getYawPitchRoll(ypr); - return ypr[1]; - } - - /** - * Returns the current roll of the pigeon - */ - public double getGyroRoll() { - double[] ypr = new double[3]; - - m_pigeon.getYawPitchRoll(ypr); - return ypr[2]; - } - - public GyroBase getGyroInterface() { - return new GyroBase(){ - - @Override - public void close() throws Exception { - // TODO Auto-generated method stub - } - - @Override - public void reset() { - // TODO Auto-generated method stub - resetGyroYaw(0); - } - - @Override - public double getRate() { - // TODO Auto-generated method stub - return getTurnRate(); - } - - @Override - public double getAngle() { - // TODO Auto-generated method stub - return getGyroYaw(); - } - - @Override - public void calibrate() { - // TODO Auto-generated method stub - } - }; - } - - // lol - // sko - // ridge - // brayden=bad coder - - /** - * Returns the heading of the robot - * - * @return The robot's heading in degrees, from -180 to 180 - */ - public double getHeading() { - return Math.IEEEremainder(getGyroYaw(), 360); - } - - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - public double getTurnRate() { - double deltaYaw = m_currentAngleYaw - m_lastAngleYaw; - double turnRate = 1000 * deltaYaw / m_deltaTimeMs; - return turnRate; - } - - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return m_odometry.getPoseMeters(); - } - - /** - * Returns current wheel speeds of robot. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds(getVelocityInchesPerSecond(m_leftBackMotor), - -getVelocityInchesPerSecond(m_rightBackMotor)); - } - - /** - * Gets the encoder value (position) of a motor - * - * @param falcon The motor to get the position of - * @return The position of the motor in inches - */ - public double getDistanceInches(WPI_TalonFX falcon) { - return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()); - } - - /** - * Gets the encoder value (velocity) of a motor - * - * @param falcon The motor to get the velocity of - * @return The velocity of the motor in inches per second - */ - public double getVelocityInchesPerSecond(WPI_TalonFX falcon) { - return ticksToInches( - falcon.getSensorCollection().getIntegratedSensorPosition() / DriveConstants.TICK_TIME_TO_SECONDS); - } - - /** - * Converts a value in ticks to inches. - * - * @param ticks The value in ticks to convert - * @return The converted value in inches - */ - public double ticksToInches(double ticks) { - if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { - return ticks * DriveConstants.INCHES_PER_TICK_HIGH; - } else { - return ticks * DriveConstants.INCHES_PER_TICK_LOW; - } - } - - /** - * Converts a value in inches to ticks. - * - * @param inches The value in inches to convert - * @return The converted value in ticks - */ - public double inchesToTicks(double inches) { - if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { - return inches * DriveConstants.TICKS_PER_INCH_HIGH; - } else { - return inches * DriveConstants.TICKS_PER_INCH_LOW; - } - } - - /** - * Converts a value in inches to meters. - * - * @param inches The value in inches to convert - * @return The converted value in meters - */ - public double inchesToMeters(double inches) { - return inches * DriveConstants.METERS_PER_INCH; - } - - /** - * Converts a value in meters to inches. - * - * @param meters The value in meters to convert - * @return The converted value in inches - */ - public double metersToInches(double meters) { - return meters * DriveConstants.INCHES_PER_METER; - } - - /** - * Converts a value in inches per second to miles per hour - * @param ips The value in inches per second to convert - * @return The value in miles per hour - */ - public double inchesPerSecondToMilesPerHour(double ips) { - double mph = ips * (3600) * (1/63360); - return mph; - } - - public void setRightMotorGains(boolean isHighGear) { - if (!isHighGear) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_MOTION_MAGIC, - m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); - } else { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, - DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_MOTION_MAGIC, - m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, - DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); - } - } - - /** - * Sets Motors to a NeutralMode. - * - * @param mode NeutralMode to set motors to - */ - public void setDriveTrainNeutralMode(NeutralMode mode) { - m_leftFrontMotor.setNeutralMode(mode); - m_rightFrontMotor.setNeutralMode(mode); - m_leftBackMotor.setNeutralMode(mode); - m_rightBackMotor.setNeutralMode(mode); - } - - public void updateSmartDashboard() { - try { - - // SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - // SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - // SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - - SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro); - SmartDashboard.putData("Drive Train", m_driveTrain); - - SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition()); - SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition()); - - SmartDashboard.putNumber("Average Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - - double avgSpeedMPH = inchesPerSecondToMilesPerHour(10 * ticksToInches(m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY))); - - SmartDashboard.putNumber("Avg Speed MPH", avgSpeedMPH); - - //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); - //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); - //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); - //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - - double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - - //SmartDashboard.putNumber("Left Motor RPM", leftRPM); - //SmartDashboard.putNumber("Right Motor RPM", rightRPM); - - //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); - - //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); - - SmartDashboard.putNumber("Left Motor Pos Meters", inchesToMeters(getDistanceInches(m_rightFrontMotor))); - SmartDashboard.putNumber("Right Motor Pos Meters", inchesToMeters(getDistanceInches(m_leftFrontMotor))); - - /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); - */ - - //SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); - //SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - - //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); - - //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); - - //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - - SmartDashboard.putString("Odometry Values Meters", getPose().toString()); - //SmartDashboard.putNumber("Odometry Heading", getHeading()); - - //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - //SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); - - if (m_currentSong != m_songChooser.getSelected()){ - m_currentSong = m_songChooser.getSelected(); - selectSong(m_currentSong); - - //System.err.println(m_currentSong); - } - } catch (Exception e) { - System.err.println("Error while using Drive SmartDashboard"); - // e.printStackTrace(System.err); - } - } - - -} diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java deleted file mode 100644 index 1a6010a..0000000 --- a/src/main/java/frc4388/robot/subsystems/Pneumatics.java +++ /dev/null @@ -1,90 +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.subsystems; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.PneumaticsConstants; - -public class Pneumatics extends SubsystemBase { - /* Create Solenoids */ - public DoubleSolenoid m_speedShift = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, - PneumaticsConstants.SPEED_SHIFT_FORWARD_ID, - PneumaticsConstants.SPEED_SHIFT_REVERSE_ID ); - - public DoubleSolenoid m_coolFalcon = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, - PneumaticsConstants.COOL_FALCON_FORWARD_ID, - PneumaticsConstants.COOL_FALCON_REVERSE_ID ); - - /* Get Drive Subsystem */ - Drive m_driveSubsystem; - - public boolean m_isSpeedShiftHigh; - - /** - * Creates a new Pneumatics. - */ - public Pneumatics() { - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - runFalconCooling(); - - SmartDashboard.putBoolean("Gear Shift", m_isSpeedShiftHigh); - } - - /** - * Passes subsystem needed. - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(Drive subsystem) { - m_driveSubsystem = subsystem; - } - - /** - * Set to high or low gear based on boolean state, true = high, false = low - * @param state Chooses between high or low gear - */ - public void setShiftState(boolean state) { - if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kReverse); - } - if (state == false) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - m_driveSubsystem.setRightMotorGains(state); - m_isSpeedShiftHigh = state; - } - - /** - * Set to open or close solenoid that cools the falcon, true = open, false = close - * @param state Chooses between open and close - */ - public void coolFalcon(boolean state) { - if (state == true) { - m_coolFalcon.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { - m_coolFalcon.set(DoubleSolenoid.Value.kReverse); - } - } - - /** - * Runs coolFalcon every 30 seconds for 1 second. - */ - public void runFalconCooling() { - if (m_driveSubsystem.m_currentTimeSec % 30 == 0) { - coolFalcon(true); - } else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - } - } -}