diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3678f33..89d2b77 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,23 +30,30 @@ public final class Constants { public static final int PIGEON_ID = 6; /* Drive Inversions */ - public static final boolean isRightMotorInverted = false; + public static final boolean isRightMotorInverted = true; public static final boolean isLeftMotorInverted = false; public static final boolean isRightArcadeInverted = false; public static final boolean isAuxPIDInverted = false; /* Drive Configuration */ - public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config + public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor public static final double NEUTRAL_DEADBAND = 0.04; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); - public static final int CLOSED_LOOP_TIME_MS = 1; + public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + + /* Drive Train Characteristics */ + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; + public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; + public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; + public static final double TICKS_PER_MOTOR_REV = 2048; /* PID Constants Drive*/ - public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 20000; public static final int DRIVE_ACCELERATION = 7000; @@ -78,13 +85,6 @@ public final class Constants { public final static int SLOT_TURNING = 2; public final static int SLOT_MOTION_MAGIC = 3; - /* Drive Train Characteristics */ - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; - public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; - public static final double WHEEL_DIAMETER_INCHES = 6; - public static final double TICKS_PER_GYRO_REV = 8192; - /* Ratio Calculation */ public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double TICK_TIME_TO_SECONDS = 0.1; @@ -156,6 +156,16 @@ public final class Constants { public static final double STORAGE_MIN_OUTPUT = -1.0; public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); } + + public static final class PneumaticsConstants { + public static final int PCM_MODULE_ID = 7; + + public static final int SPEED_SHIFT_FORWARD_ID = 0; + public static final int SPEED_SHIFT_REVERSE_ID = 1; + + public static final int COOL_FALCON_FORWARD_ID = 3; + public static final int COOL_FALCON_REVERSE_ID = 2; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1029754..4f894c9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -105,7 +105,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(true); + //m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2156bd0..18dee5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -21,12 +21,15 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; +import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -39,9 +42,12 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TurnDegrees; +import frc4388.robot.commands.Wait; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -56,6 +62,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* Subsystems */ 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(); private final Shooter m_robotShooter = new Shooter(); @@ -75,6 +82,10 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + /* Passing Drive and Pneumatics Subsystems */ + m_robotPneumatics.passRequiredSubsystem(m_robotDrive); + m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + configureButtonBindings(); /* Default Commands */ @@ -103,11 +114,11 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new TurnDegrees(90, m_robotDrive)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) @@ -120,11 +131,11 @@ public class RobotContainer { /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); /* Operator Buttons */ //TODO: Shooter Buttons @@ -175,11 +186,32 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + + //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those + //This assumes that we are positioned against the right wall with our shooter facing the target. + return new SequentialCommandGroup(new Wait(2, m_robotDrive), + //add aim command + //add shooter command + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0), + new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), + new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + //add aim command + //add shooter command +//Below this would be the picking up additional balls outside of those in the trench directly behind us - // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + new TurnDegrees(-150, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0), + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new TurnDegrees(75, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0), + new TurnDegrees(-45, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0)); } - TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND, @@ -231,7 +263,7 @@ public class RobotContainer { * @param state the gearing of the gearbox (true is high, false is low) */ public void setDriveGearState(boolean state) { - m_robotDrive.setShiftState(state); + m_robotPneumatics.setShiftState(state); } /** diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 68d9538..ea76f96 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase { 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 units + * @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, double targetGyro) { + 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; @@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase { 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. @@ -72,6 +74,7 @@ public class DrivePositionMPAux extends CommandBase { // Deramp PID m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro); } + m_counter ++; } // Called once the command ends or is interrupted. @@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) { - return true; + 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/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index f51621a..01375a4 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; import frc4388.utility.controller.IHandController; @@ -38,26 +39,27 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = -m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); + double moveInput = m_controller.getRightXAxis(); + double steerInput = -m_controller.getLeftYAxis(); double moveOutput = 0; double steerOutput = 0; - if (moveInput >= 0){ - moveOutput = -Math.cos(1.571*moveInput)+1; + if (steerInput >= 0){ + steerOutput = -Math.cos(1.571*steerInput)+1; } else { - moveOutput = Math.cos(1.571*moveInput)-1; + steerOutput = Math.cos(1.571*steerInput)-1; } - double cosMultiplier = .55; + double cosMultiplier = 1.0; double deadzone = .1; - 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; + if (moveInput > 0){ + moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier; + } else if (moveInput < 0) { + moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier; } else { - steerOutput = 0; + moveOutput = 0; } - + + SmartDashboard.putNumber("Steer Output Test", moveOutput); m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 12d605c..5a68a9f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -12,10 +12,12 @@ 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; @@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { * 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 subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @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 subsystem, IHandController controller) { + public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; + m_drive = subsystemDrive; + m_pneumatics = subsystemPneumatics; m_controller = controller; addRequirements(m_drive); } @@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - if (m_drive.m_isSpeedShiftHigh) { + if (m_pneumatics.m_isSpeedShiftHigh) { runDriveWithInput(moveOutput, steerInput); resetGyroTarget(); } diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java index 2b47050..52eb5dc 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) { + if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) { return true; } return false; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fca1e7b..422d2c7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.PneumaticsConstants; import frc4388.robot.Gains; public class Drive extends SubsystemBase { - /* Create Motors, Gyros, Solenoids, etc */ + /* 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 DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1); - public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); /* Drive objects to manage Drive Train */ public DifferentialDrive m_driveTrain; public final DifferentialDriveOdometry m_odometry; public Orchestra m_orchestra; + /* Pneumatics Subsystem */ + Pneumatics m_pneumaticsSubsystem; + /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; @@ -90,7 +92,6 @@ public class Drive extends SubsystemBase { SendableChooser m_songChooser = new SendableChooser(); /* Misc */ - public boolean m_isSpeedShiftHigh; String m_currentSong = ""; /** @@ -105,13 +106,6 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - /* 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); - /* 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); @@ -119,10 +113,10 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configOpenloopRamp(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); + //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); @@ -251,6 +245,13 @@ public class Drive extends SubsystemBase { /* 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); @@ -273,14 +274,33 @@ public class Drive extends SubsystemBase { @Override public void periodic() { + updateTime(); + updateAngles(); + updatePosition(); + updateSmartDashboard(); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Pneumatics subsystem) { + m_pneumaticsSubsystem = subsystem; + } + + 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(); @@ -295,9 +315,6 @@ public class Drive extends SubsystemBase { m_odometry.update(Rotation2d.fromDegrees( getHeading()), getDistanceInches(m_leftFrontMotor), -getDistanceInches(m_rightFrontMotor)); - - runFalconCooling(); - updateSmartDashboard(); } /** @@ -305,7 +322,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(steer, move); + m_driveTrain.arcadeDrive(move, steer); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -431,47 +448,6 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - /** - * 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); - } - 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); - } - } - - /** - * - */ - public void runFalconCooling() { - if (m_currentTimeSec % 30 == 0) { - coolFalcon(true); - SmartDashboard.putBoolean("Solenoid", true); - } else if ((m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - SmartDashboard.putBoolean("Solenoid", false); - } - } - /** * Selects a song to play! * @param song The name of the song to be played @@ -624,7 +600,7 @@ public class Drive extends SubsystemBase { * @return The converted value in inches */ public double ticksToInches(double ticks) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return ticks * DriveConstants.INCHES_PER_TICK_HIGH; } else { return ticks * DriveConstants.INCHES_PER_TICK_LOW; @@ -637,7 +613,7 @@ public class Drive extends SubsystemBase { * @return The converted value in ticks */ public double inchesToTicks(double inches) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return inches * DriveConstants.TICKS_PER_INCH_HIGH; } else { return inches * DriveConstants.TICKS_PER_INCH_LOW; @@ -747,8 +723,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); 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()); + //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); + //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -780,9 +756,9 @@ public class Drive extends SubsystemBase { 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 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 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)); @@ -790,7 +766,7 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Odometry Heading", getHeading()); //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - //SmartDashboard.putNumber("Delta Time", m_deltaTime); + SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected(); diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java new file mode 100644 index 0000000..1c373ed --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* 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.RobotContainer; +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(); + } + + /** + * 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); + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 3d460fe..b6eb85e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants;