diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4f33c0ca..4b2ef2ce 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,8 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.input.gamepads.Xbox; +import com.stuypulse.robot.subsystems.climber.*; + import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -31,10 +33,11 @@ public class RobotContainer { // Gamepads - public final Gamepad driver = new Xbox(Ports.Gamepad.DRIVER); + public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Climber climber = Climber.getInstance(); public final Amper amper = Amper.getInstance(); public final SwerveDrive swerve = SwerveDrive.getInstance(); public final Odometry odometry = Odometry.getInstance(); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 0055daab..2aefe8eb 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -5,9 +5,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; -import com.stuypulse.robot.constants.Settings.Driver.Turn.GyroFeedback; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.SLMath; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index ee9aa26c..8e71aaf1 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -1,42 +1,24 @@ package com.stuypulse.robot.commands.swerve; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Alignment.Rotation; import com.stuypulse.robot.constants.Settings.Alignment.Translation; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.Fiducial; import com.stuypulse.robot.util.HolonomicController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveToShoot extends Command { - /* - * swerve - * holonomi controller - * targetpose pose2d - * targetPose2d as a field object 2d - * - * contrusctor (init them, add requirements) - * initialize (do the math to get the target pose) - * execute (set the target pose to the controller and then setChassisSpeeds to it) - * isFinished (check if the controller is done within the tolerances) - * end (stop the swerve) - * - */ + private final SwerveDrive swerve; private Pose2d targetPose; private final HolonomicController controller; @@ -62,13 +44,12 @@ private Pose2d getSpeakerTargetPose() { Vector2D speakerCenterVec = new Vector2D(Field.getAllianceSpeakerPose().getTranslation()); Vector2D robotVec = new Vector2D(Odometry.getInstance().getPose().getTranslation()); - double speakerOpeningX = Units.inchesToMeters(13.6); // the distances between the robot and the target double Dx = speakerCenterVec.x - robotVec.x; double Dy = speakerCenterVec.y - robotVec.y; // the offset of the speakers opening width from center using similar triangles - double dy = (Dy / Dx) * speakerOpeningX; + double dy = (Dy / Dx) * Field.SPEAKER_OPENING_X; // gets the new speaker target vector to aim at Vector2D speakerTargetVec= new Vector2D(Field.getAllianceSpeakerPose().getX(), Field.getAllianceSpeakerPose().getY() + dy); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java index 9341d1ac..09150a3f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java @@ -1,11 +1,6 @@ package com.stuypulse.robot.commands.swerve; -import com.stuypulse.robot.constants.Settings.Swerve.BackLeft; -import com.stuypulse.robot.constants.Settings.Swerve.BackRight; -import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; -import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.subsystems.swerve.modules.SimModule; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -22,6 +17,7 @@ public SwerveDriveXMode() { @Override public void execute() { SwerveModuleState[] states = new SwerveModuleState[] { + //{front right, front left, back right, back left} new SwerveModuleState(0,Rotation2d.fromDegrees(225)), new SwerveModuleState(0,Rotation2d.fromDegrees(315)), new SwerveModuleState(0,Rotation2d.fromDegrees(45)), diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 701eada3..add35c22 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -96,4 +96,6 @@ public static Pose2d getAllianceSpeakerPose() { boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; return SPEAKER_POSES[isBlue ? 0 : 1]; } + + double SPEAKER_OPENING_X = Units.inchesToMeters(13.6); } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 62982d2b..d6a79bef 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -48,6 +48,11 @@ public interface Conveyor { CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast); } + public interface Climber { + CANSparkMaxConfig LEFT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80); + CANSparkMaxConfig RIGHT_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake, 80); + } + public static class TalonSRXConfig { public final boolean INVERTED; public final NeutralMode NEUTRAL_MODE; diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 46fdb439..a0a1a801 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -13,6 +13,16 @@ public interface Gamepad { int DEBUGGER = 2; } + public interface Climber { + int LEFT_MOTOR = 60; + int RIGHT_MOTOR = 61; + + int TOP_RIGHT_LIMIT = 8; + int TOP_LEFT_LIMIT = 7; + int BOTTOM_RIGHT_LIMIT = 6; + int BOTTOM_LEFT_LIMIT = 5; + } + public interface Amper { int SCORE = 31; int LIFT = 30; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0048f093..6e91f665 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -23,7 +23,23 @@ public interface Settings { double DT = 1.0 / 50.0; - + + public interface Climber { + double MIN_HEIGHT = 0.0; + double MAX_HEIGHT = 0.0; + + double AT_HEIGHT_THRESHOLD = Units.inchesToMeters(1); + + double BANGBANG_VOLTAGE = 8; + + public interface Encoder { + double GEAR_RATIO = 0.0; + + double POSITION_CONVERSION = 0.0; + double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; + } + } + public interface Amper { public interface Score { SmartNumber ROLLER_SPEED = new SmartNumber("Amper/Score/Roller Speed", 1.0); // change later diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java new file mode 100644 index 00000000..116d014b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java @@ -0,0 +1,39 @@ +package com.stuypulse.robot.subsystems.climber; + +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Climber extends SubsystemBase { + private static final Climber instance; + + private final SmartNumber targetHeight; + + static { + instance = new ClimberImpl(); + } + + public static Climber getInstance() { + return instance; + } + + public Climber() { + targetHeight = new SmartNumber("Climber/Target Height", 0.0); + } + + public void setTargetHeight(double height) { + targetHeight.set(height); + } + + public double getTargetHeight() { + return targetHeight.get(); + } + + public abstract double getHeight(); + public abstract double getVelocity(); + + public abstract void setVoltage(double voltage); + + public abstract boolean atTop(); + public abstract boolean atBottom(); +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java new file mode 100644 index 00000000..1b1fa556 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java @@ -0,0 +1,105 @@ +package com.stuypulse.robot.subsystems.climber; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ClimberImpl extends Climber { + private final CANSparkMax rightMotor; + private final CANSparkMax leftMotor; + + private final RelativeEncoder rightEncoder; + private final RelativeEncoder leftEncoder; + + private final DigitalInput topRightLimit; + private final DigitalInput topLeftLimit; + private final DigitalInput bottomRightLimit; + private final DigitalInput bottomLeftLimit; + + protected ClimberImpl() { + rightMotor = new CANSparkMax(Ports.Climber.RIGHT_MOTOR, MotorType.kBrushless); + leftMotor = new CANSparkMax(Ports.Climber.LEFT_MOTOR, MotorType.kBrushless); + + rightEncoder = rightMotor.getEncoder(); + leftEncoder = leftMotor.getEncoder(); + + rightEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); + leftEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); + + rightEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); + leftEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); + + topRightLimit = new DigitalInput(Ports.Climber.TOP_RIGHT_LIMIT); + topLeftLimit = new DigitalInput(Ports.Climber.TOP_LEFT_LIMIT); + bottomRightLimit = new DigitalInput(Ports.Climber.BOTTOM_RIGHT_LIMIT); + bottomLeftLimit = new DigitalInput(Ports.Climber.BOTTOM_LEFT_LIMIT); + + Motors.Climber.LEFT_MOTOR.configure(leftMotor); + Motors.Climber.RIGHT_MOTOR.configure(rightMotor); + } + + @Override + public double getHeight() { + return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; + } + + @Override + public double getVelocity() { + return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; + } + + @Override + public boolean atTop() { + return !topRightLimit.get() || !topLeftLimit.get(); + } + + @Override + public boolean atBottom() { + return !bottomRightLimit.get() || !bottomLeftLimit.get(); + } + + @Override + public void setVoltage(double voltage) { + if (atTop() && voltage > 0) { + DriverStation.reportWarning("Climber Top Limit Reached", false); + voltage = 0.0; + } else if (atBottom() && voltage < 0) { + DriverStation.reportWarning("Climber Bottom Limit Reached", false); + voltage = 0.0; + } + + rightMotor.setVoltage(voltage); + leftMotor.setVoltage(voltage); + } + + @Override + public void periodic() { + super.periodic(); + + if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.AT_HEIGHT_THRESHOLD) { + setVoltage(0.0); + } else if (getHeight() > getTargetHeight()) { + setVoltage(-Settings.Climber.BANGBANG_VOLTAGE); + } else { + setVoltage(+Settings.Climber.BANGBANG_VOLTAGE); + } + + SmartDashboard.putNumber("Climber/Target Height", getTargetHeight()); + SmartDashboard.putNumber("Climber/Height", getHeight()); + + SmartDashboard.putNumber("Climber/Velocity", getVelocity()); + + if (atBottom()) { + leftEncoder.setPosition(Settings.Climber.MIN_HEIGHT); + rightEncoder.setPosition(Settings.Climber.MIN_HEIGHT); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 41e83aaf..2dfa25ae 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -178,7 +178,7 @@ public Translation2d[] getModuleOffsets() { } public ChassisSpeeds getChassisSpeeds() { - return getKinematics().toChassisSpeeds(getModuleStates()); + return kinematics.toChassisSpeeds(getModuleStates()); } /** Setters **/ @@ -266,7 +266,6 @@ public void periodic() { public void simulationPeriodic() { //show gyro angle in simulation - ChassisSpeeds speeds = getChassisSpeeds(); - gyro.setAngleAdjustment(gyro.getAngleAdjustment()- Math.toDegrees(speeds.omegaRadiansPerSecond * Settings.DT)); + gyro.setAngleAdjustment(gyro.getAngle() - Math.toDegrees(getChassisSpeeds().omegaRadiansPerSecond * Settings.DT)); } } \ No newline at end of file