diff --git a/build.gradle b/build.gradle index 19b8bce..d1dcd1d 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.3.1" + id "edu.wpi.first.GradleRIO" version "2022.4.1" id "com.diffplug.spotless" version "6.2.0" id "io.freefair.lombok" version "6.3.0" } diff --git a/src/main/java/frc/robot/AutonomousRoute.java b/src/main/java/frc/robot/AutonomousRoute.java index 2a0c454..1d58980 100644 --- a/src/main/java/frc/robot/AutonomousRoute.java +++ b/src/main/java/frc/robot/AutonomousRoute.java @@ -1,6 +1,7 @@ package frc.robot; public enum AutonomousRoute { + DriveOut, Blue1 // Blue_2 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f3ed9ef..3ca30a2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,11 +5,15 @@ package frc.robot; import com.revrobotics.CANSparkMax.IdleMode; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.robot.MotorConfig.PIDConfig; +import frc.robot.MotorConfig.SoftLimit; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -24,8 +28,16 @@ public static final class OIConstants { public static final boolean USE_XBOX = true; public static final int DRIVER_1 = 0; public static final int DRIVER_2 = 1; + public static final int DRIVER_3 = 2; public static final int FRONT_CAMERA = 0; + public static final String FRONT_CAMERA_NAME = "front-camera"; + + public static final double NOTIFY_RATE = 2; // rumble drive feedback once every seconds; + public static final double RUMBLE_LEFT_LOCKED = .5; + public static final double RUMBLE_RIGHT_LOCKED = 0d; + public static final double RUMBLE_LEFT_NO_TARGET = .9; + public static final double RUMBLE_RIGHT_NO_TARGET = .9; } /** Constants related to the Autonomous mode. */ @@ -46,21 +58,23 @@ public static final class AutoConstants { } public static final class DriveConstants { + private static final double OPEN_RAMP_RATE = 0.75; private static final double WHEEL_DIAMETER_M = Units.inchesToMeters(8); - private static final double DRIVE_GEAR_RATIO = (14d / 70d) * (66d / 30d); + private static final double DRIVE_GEAR_RATIO = (70d / 14d) * (66d / 30d); private static final double ENCODER_POSITION_CONVERSION = Math.PI * WHEEL_DIAMETER_M / DRIVE_GEAR_RATIO; // TODO: Find actual values for these! private static final PIDConfig PID_DEFAULTS = PIDConfig.builder() - .kP(0.8) - .kI(0) + .kP(5e-5) + .kI(1e-6) .kD(0) - .kFF(5) - // .maxAcceleration(maxAcceleration) - // .maxVelocity(maxVelocity) - // .outputRangeHigh(outputRangeHigh) - // .outputRangeLow(outputRangeLow) + .kFF(0.000156) + .maxAcceleration(1500) + .maxVelocity(2000) + .outputRangeHigh(1) + .outputRangeLow(-1) + .allowedClosedLoopError(1) // .minOutputVelocity(minOutputVelocity) .build(); public static final MotorConfig FRONT_LEFT = @@ -69,21 +83,24 @@ public static final class DriveConstants { .idleMode(IdleMode.kBrake) .pidConfig(PID_DEFAULTS) .positionConversionFactor(ENCODER_POSITION_CONVERSION) + .openLoopRampRate(OPEN_RAMP_RATE) .build(); public static final MotorConfig FRONT_RIGHT = MotorConfig.builder() .canId(5) + .inverted(true) .idleMode(IdleMode.kBrake) .pidConfig(PID_DEFAULTS) .positionConversionFactor(ENCODER_POSITION_CONVERSION) + .openLoopRampRate(OPEN_RAMP_RATE) .build(); public static final MotorConfig REAR_LEFT = MotorConfig.builder() .canId(8) - .inverted(true) .idleMode(IdleMode.kBrake) .pidConfig(PID_DEFAULTS) .positionConversionFactor(ENCODER_POSITION_CONVERSION) + .openLoopRampRate(OPEN_RAMP_RATE) .build(); public static final MotorConfig REAR_RIGHT = MotorConfig.builder() @@ -92,10 +109,11 @@ public static final class DriveConstants { .inverted(true) .pidConfig(PID_DEFAULTS) .positionConversionFactor(ENCODER_POSITION_CONVERSION) + .openLoopRampRate(OPEN_RAMP_RATE) .build(); + } - public static final int PIGEON_MOTOR_PORT = 1; - + public static final class PoseConstants { // Distance between centers of right and left wheels on robot public static final double kTrackWidth = Units.inchesToMeters(22.09); // Distance between centers of front and back wheels on robot @@ -109,41 +127,149 @@ public static final class DriveConstants { new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); public static final int kEncoderCPR = 4096; - public static final double kWheelDiameterMeters = Units.inchesToMeters(8); + public static final double kWheelDiameterMeters = Units.inchesToMeters(6); public static final double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; - // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! - // These characterization values MUST be determined either experimentally or theoretically - // for *your* robot's drive. - // The SysId tool provides a convenient method for obtaining these values for your robot. - // public static final SimpleMotorFeedforward kFeedforward = - // new SimpleMotorFeedforward(1, 0.8, 0.15); - - // // Example value only - as above, this must be tuned for your drive! - // public static final double kPFrontLeftVel = 0.5; - // public static final double kPRearLeftVel = 0.5; - // public static final double kPFrontRightVel = 0.5; - // public static final double kPRearRightVel = 0.5; + // Physical location of the camera on the robot, relative to the center of the + // robot. + public static final Transform2d CAMERA_TO_ROBOT = + new Transform2d( + new Translation2d(-0.25, 0), // in meters (x,y) + new Rotation2d()); + + // 4 ft. 5⅜ in diameter + private static final double targetWidth = Units.inchesToMeters(4 * 12 + 5 + 3 / 8); // meters + + // See + // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf + // page 197 + private static final double targetHeight = + Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters + public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters + + // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf + // pages 4 and 5 + public static final double kFarTgtXPos = Units.feetToMeters(54); + public static final double kFarTgtYPos = + Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0); + public static final Pose2d kFarTargetPose = + new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0)); } public static final class IntakeConstants { + public static final MotorConfig INTAKE_MOTOR = + MotorConfig.builder() + .canId(6) + .idleMode(IdleMode.kCoast) + .pidConfig( + PIDConfig.builder() + .kP(0.74646) + .kI(0) + .kD(0) + .kFF(0.000156) + // .maxAcceleration(maxAcceleration) + // .maxVelocity(maxVelocity) + // .outputRangeHigh(1) + // .outputRangeLow(-1) + // .minOutputVelocity(minOutputVelocity) + .build()) + .build(); + + public static final MotorConfig INTAKE_LIFT_MOTOR = + MotorConfig.builder() + .canId(12) + .idleMode(IdleMode.kBrake) + // .inverted(true) + .pidConfig( + PIDConfig.builder() + .kP(0.00005) + .kI(0.000001) + .kD(0) + .kFF(0.000156) + .maxVelocity(4700) + .maxAcceleration(8000) + .allowedClosedLoopError(0) + .build()) + .pidConfig( + PIDConfig.builder() + .kP(0.00005) + .kI(0.000001) + .kD(0) + .kFF(0.000156) + .maxVelocity(5700) + .maxAcceleration(2500) + .allowedClosedLoopError(0) + .build()) + .softLimitForward(SoftLimit.builder().limit(29).build()) + .softLimitReverse(SoftLimit.builder().limit(-.5f).build()) + .openLoopRampRate(5) + // .m + .build(); + public static final double UP_POSITION = 0; + public static final double DOWN_POSITION = 29; + public static final double LIFT_THRESHOLD = .5 * DOWN_POSITION; + public static final double WAIT_BEFORE_STARTIING = 1; + public static final double SPEED = 5700 * .90; + } + + public static final class ElevatorConstants { // TODO: find these values from SimId private static final PIDConfig PID_DEFAULTS = PIDConfig.builder() - .kP(0.8) + .kP(0.67755) .kI(0) .kD(0) - .kFF(5) + .kFF(0.000156) // .maxAcceleration(maxAcceleration) // .maxVelocity(maxVelocity) // .outputRangeHigh(outputRangeHigh) // .outputRangeLow(outputRangeLow) // .minOutputVelocity(minOutputVelocity) .build(); - public static final MotorConfig INTAKE_MOTOR = - MotorConfig.builder().canId(4).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).build(); - public static final int SPEED = 5700; + public static final MotorConfig ELEVATOR_MOTOR = + MotorConfig.builder().canId(7).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).build(); + + public static final double SPEED = 5700 * .35; + } + + public static final class ShooterConstants { + private static final PIDConfig PID_DEFAULTS = + PIDConfig.builder() + .kP(0.098557) + .kI(0) + .kD(0) + .kFF(0.000156) + .maxAcceleration(14000) + .maxVelocity(5700) + .outputRangeHigh(1) + .outputRangeLow(-1) + .allowedClosedLoopError(.8) + // .minOutputVelocity(minOutputVelocity) + .build(); + public static final MotorConfig SHOOTER_MOTOR = + MotorConfig.builder() + .canId(13) + .idleMode(IdleMode.kCoast) + .pidConfig(PID_DEFAULTS) + // TODO: if we add a second motor + // .follower( + // + // MotorConfig.builder().canId(15).idleMode(IdleMode.kCoast).pidConfig(PID_DEFAULTS).inverted(true).build() + // ) + .build(); + } + + public static final class SensorConstants { + public static final String CAMERA = "photoncamera"; // TODO: + public static final int PIGEON_MOTOR_PORT = 2; + } + + public static final class VisionConstants { + public static final double HEIGHT_TO_GOAL = Units.inchesToMeters(8 * 12 + 5 + 3 / 8); + public static final double HEIGHT_TO_CAMERA = Units.inchesToMeters(45); // FIXME: + public static final double HEIGHT_FROM_CAMERA_TO_GOAL = HEIGHT_TO_GOAL - HEIGHT_TO_CAMERA; + public static final double CAMERA_TILT = 20; // degrees } } diff --git a/src/main/java/frc/robot/MotorConfig.java b/src/main/java/frc/robot/MotorConfig.java index b4932f6..d331766 100644 --- a/src/main/java/frc/robot/MotorConfig.java +++ b/src/main/java/frc/robot/MotorConfig.java @@ -2,6 +2,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.SparkMaxPIDController; import java.util.List; @@ -15,20 +16,40 @@ public class MotorConfig { private int canId; @Builder.Default private double closedLoopRampRate = 0; @Builder.Default private double openLoopRampRate = 0; + @Builder.Default private MotorType type = MotorType.kBrushless; + private SoftLimit softLimitForward; + private SoftLimit softLimitReverse; @Builder.Default private IdleMode idleMode = IdleMode.kBrake; @Builder.Default private boolean inverted = false; @Builder.Default private double positionConversionFactor = 1; @Singular private List pidConfigs; + private MotorConfig follower; public CANSparkMax createMotor() { + return createMotor(true); + } + + private CANSparkMax createMotor(boolean burnFlash) { var motor = new CANSparkMax(getCanId(), MotorType.kBrushless); motor.restoreFactoryDefaults(); motor.setClosedLoopRampRate(getClosedLoopRampRate()); motor.setIdleMode(getIdleMode()); motor.setInverted(isInverted()); + // motor.enableVoltageCompensation(nominalVoltage); + // motor.setSmartCurrentLimit(stallLimit, freeLimit) motor.setOpenLoopRampRate(getOpenLoopRampRate()); + if (getSoftLimitForward() != null) { + motor.enableSoftLimit(SoftLimitDirection.kForward, true); + motor.setSoftLimit(SoftLimitDirection.kForward, getSoftLimitForward().getLimit()); + } + if (getSoftLimitReverse() != null) { + motor.enableSoftLimit(SoftLimitDirection.kReverse, true); + motor.setSoftLimit(SoftLimitDirection.kReverse, getSoftLimitReverse().getLimit()); + } + motor.getEncoder().setPositionConversionFactor(getPositionConversionFactor()); motor.getEncoder().setVelocityConversionFactor(getPositionConversionFactor() / 60); + motor.getEncoder().setPosition(0); SparkMaxPIDController pidController = motor.getPIDController(); for (int i = 0; i < getPidConfigs().size(); i++) { @@ -45,7 +66,18 @@ public CANSparkMax createMotor() { pidController.setSmartMotionMinOutputVelocity(pidConfig.getMinOutputVelocity(), i); } - motor.burnFlash(); + if (follower != null) { + boolean followerInverted = follower.inverted; + follower.inverted = this.inverted; + + var followerMotor = follower.createMotor(false); + followerMotor.follow(motor, followerInverted); + followerMotor.burnFlash(); + } + + if (burnFlash) { + motor.burnFlash(); + } return motor; } @@ -56,11 +88,17 @@ public static final class PIDConfig { private double kI; private double kD; private double kFF; - private double outputRangeLow; - private double outputRangeHigh; - private double allowedClosedLoopError; + @Builder.Default private double outputRangeLow = -1d; + @Builder.Default private double outputRangeHigh = 1d; + @Builder.Default private double allowedClosedLoopError = 0; private double maxAcceleration; private double maxVelocity; private double minOutputVelocity; } + + @Builder + @Getter + public static final class SoftLimit { + private float limit; + } } diff --git a/src/main/java/frc/robot/Pose2dHandler.java b/src/main/java/frc/robot/Pose2dHandler.java new file mode 100644 index 0000000..662c633 --- /dev/null +++ b/src/main/java/frc/robot/Pose2dHandler.java @@ -0,0 +1,62 @@ +package frc.robot; + +import static frc.robot.Constants.PoseConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.IMU; +import frc.robot.subsystems.Vision; + +public class Pose2dHandler { + private Matrix stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)); + private Matrix localMeasurementStdDevs = VecBuilder.fill(0.01); + private Matrix visionMeasurementStdDevs = + VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); + private MecanumDrivePoseEstimator poseEstimator; + private IMU imu; + private Vision vision; + + public Pose2dHandler(IMU imu, Vision vision) { + // set to [0,0], 0deg and rely on the real initial pose to be set from the autonomous + var initialPose = new Pose2d(); + this.imu = imu; + this.vision = vision; + poseEstimator = + new MecanumDrivePoseEstimator( + imu.getRotation2d(), + initialPose, + DRIVE_KINEMATICS, + stateStdDevs, + localMeasurementStdDevs, + visionMeasurementStdDevs); + } + + public void update(MecanumDriveWheelSpeeds wheelSpeeds) { + poseEstimator.update(imu.getRotation2d(), wheelSpeeds); + + var result = vision.getLatestResult(); + // result.ifPresent(vr -> { + // double imageCaptureTime = Timer.getFPGATimestamp() - vr.getLatencyMillis(); + // Transform2d camToTargetTrans = vr.getCameraToTarget(); + // Pose2d camPose = Constants.kTargetPose.transformBy(camToTargetTrans.inverse()); + // poseEstimator.addVisionMeasurement( + // camPose.transformBy(CAMERA_TO_ROBOT), imageCaptureTime); + // }); + + } + + public void resetToPose(Pose2d pose) { + imu.resetAngle(pose.getRotation().getDegrees()); + poseEstimator.resetPosition(pose, imu.getRotation2d()); + } + + public Pose2d getPoseEst() { + return poseEstimator.getEstimatedPosition(); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b7cf933..7bdd484 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,8 @@ package frc.robot; import com.revrobotics.REVPhysicsSim; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -29,6 +31,13 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + DataLogManager.start(); + // LiveWindow.disableAllTelemetry(); + addPeriodic(() -> NetworkTableInstance.getDefault().flush(), 0.02, 0.01); + addPeriodic(m_robotContainer.getVisionPeriodic(), 0.02, 0.01); + addPeriodic(m_robotContainer.getDrivetrainPeriodic(), 0.02, 0.01); + // addPeriodic(() -> m_robotContainer.updateSmartDashboard(), 0.02, 0.01); + // addPeriodic(() -> m_robotContainer.updateDriveTrainPeriodic(), 0.02, 0.01); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d2c83a..ea6d83f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,30 +6,34 @@ import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; -import frc.robot.commands.ETMecanumControllerCommand; +import frc.robot.commands.DriveCommand; +import frc.robot.commands.RunIntakeElevatorCommand; +import frc.robot.commands.Shoot; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.IMU; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Vision; +import frc.robot.util.RumbleHelper; import java.util.Arrays; import java.util.EnumMap; import java.util.Map; import java.util.function.Function; -import java.util.function.Supplier; import java.util.stream.Collectors; /** @@ -42,11 +46,15 @@ public class RobotContainer { private static final String AUTONOMOUS_WAIT = "AutonomousWait"; private final Field2d field2d; private final Drivetrain drivetrain; - private XboxController driver1XBox; - private Joystick driver1Joystick; - private Supplier yAxisSupplier; - private Supplier xAxisSupplier; - private Supplier zAxisSupplier; + private final Elevator elevator; + private final Intake intake; + private final Shooter shooter; + // private XboxController driver1XBox; + // private Joystick driver1Joystick; + private IMU imu; + private Vision vision; + private Pose2dHandler pose2dHandler; + private DriveCommand driveCommand; private SendableChooser routeChooser = new SendableChooser<>(); private Map trajectories; @@ -55,54 +63,166 @@ public class RobotContainer { public RobotContainer() { field2d = new Field2d(); SmartDashboard.putData("Field", field2d); - drivetrain = new Drivetrain(field2d); + imu = new IMU(); + vision = new Vision(); + vision.setPipeline(1); + pose2dHandler = new Pose2dHandler(imu, vision); + drivetrain = new Drivetrain(pose2dHandler); + elevator = new Elevator(); + intake = new Intake(); + shooter = new Shooter(); + // new PIDTesting(); // Configure the button bindings - configureButtonBindings(); - configureDriver1Controls(); - - configureDrivetrainDefault(); + configureDriver2Controls(); + configureDriver3Controls(); configureAutoChoices(); loadPaths(); - CameraServer.startAutomaticCapture("FrontCamera", OIConstants.FRONT_CAMERA); + // CameraServer.startAutomaticCapture("FrontCamera", OIConstants.FRONT_CAMERA); + // PhotonCamera camera = new PhotonCamera(OIConstants.FRONT_CAMERA_NAME); + // camera.getLatestResult().getBestTarget(). + } + + Runnable getVisionPeriodic() { + return vision::periodic; + } + + Runnable getDrivetrainPeriodic() { + return drivetrain::periodicDrivetrain; } private void configureDriver1Controls() { - if (OIConstants.USE_XBOX) { - driver1XBox = new XboxController(OIConstants.DRIVER_1); - yAxisSupplier = () -> -driver1XBox.getLeftY(); - xAxisSupplier = () -> driver1XBox.getLeftX(); - zAxisSupplier = () -> driver1XBox.getRightX(); - } else { - driver1Joystick = new Joystick(OIConstants.DRIVER_1); - yAxisSupplier = () -> -driver1Joystick.getY(); - xAxisSupplier = () -> driver1Joystick.getX(); - zAxisSupplier = () -> driver1Joystick.getZ(); - } + // if (OIConstants.USE_XBOX) { + XboxController driver1XBox = new XboxController(OIConstants.DRIVER_1); + RumbleHelper rumbler = + new RumbleHelper( + (left, right) -> { + driver1XBox.setRumble(RumbleType.kLeftRumble, left); + driver1XBox.setRumble(RumbleType.kRightRumble, right); + }); + driveCommand = + new DriveCommand( + drivetrain, + () -> -driver1XBox.getLeftY(), + driver1XBox::getLeftX, + driver1XBox::getRightX, + driver1XBox::getLeftTriggerAxis, + vision, + imu, + rumbler); + setDrivetrainDefault(driveCommand); + // createButton(driver1XBox, XboxController.Button.kRightBumper) + // .whenPressed(driveCommand::toggleFieldOriented); + // createButton(driver1XBox, XboxController.Button.kLeftBumper) + // .whenPressed(driveCommand::toggleDriveDirection); + createButton(driver1XBox, XboxController.Button.kStart).whenPressed(imu::reset); + createButton(driver1XBox, XboxController.Button.kRightBumper) + .toggleWhenPressed(new RunIntakeElevatorCommand(intake, elevator)); + + var runShooter = // new Shoot(shooter, elevator); + new RunCommand(shooter::on, shooter) + .alongWith(new WaitCommand(.3).andThen(elevator::runUp, elevator)); + createButton(driver1XBox, XboxController.Button.kA).whenPressed(runShooter); + createButton(driver1XBox, XboxController.Button.kB) + .whenPressed( + new RunCommand(shooter::off, shooter) + .alongWith(new RunCommand(elevator::stop, elevator))); + // createButton(driver1XBox, XboxController.Button.kX).whenPressed(intake::liftOff); + // createButton(driver1XBox, XboxController.Button.kY).whenPressed(intake::slowLower); + + new Button( + () -> { + var rTrigger = driver1XBox.getRightTriggerAxis(); + return rTrigger > .3; + }) + .whenPressed(driveCommand::toggleDriveDirection); + + // new Trigger(() -> (driver1XBox.getRightTriggerAxis() >= 0.7)) + // .debounce(.5) + // .whileActiveOnce( + // new StartEndCommand( + // driveCommand::engageTargetLock, driveCommand::disengageTargetLock)); + // new Button(() -> (driver1XBox.getLeftTriggerAxis() > .5) + // } else { + // Joystick driver1Joystick = new Joystick(OIConstants.DRIVER_1); + // DriveCommand driveCommand = + // new DriveCommand( + // drivetrain, + // () -> -driver1Joystick.getY(), + // () -> driver1Joystick.getX(), + // () -> driver1Joystick.getTwist(), + // () -> 1.0, + // vision, + // imu); + // setDrivetrainDefault(driveCommand); + // } + } + + private void configureDriver2Controls() { + Joystick joystick = new Joystick(OIConstants.DRIVER_2); + createButton(joystick, 6).whenPressed(vision::toggleLED); + createButton(joystick, 5).whenPressed(vision::takePicture); + createButton(joystick, 3).whenPressed(() -> vision.setPipeline(0)); + createButton(joystick, 4).whenPressed(() -> vision.setPipeline(1)); + + createButton(joystick, 11).whenPressed(shooter::on, shooter); + createButton(joystick, 12).whenPressed(shooter::off, shooter); + createButton(joystick, 10).whenPressed(shooter::high, shooter); + createButton(joystick, 8).whenPressed(shooter::low, shooter); + // createButton(joystick, 8).whenPressed(elevator::runDown); } - private void configureDrivetrainDefault() { - Command fieldOriented = + private void configureDriver3Controls() { + Joystick joystick = new Joystick(OIConstants.DRIVER_3); + createButton(joystick, 7).whenPressed(intake::lift); + createButton(joystick, 9).whenPressed(intake::lower); + createButton(joystick, 8).whenPressed(intake::liftOff); + + // new Button(() -> joystick.getPOV() == 0).whenPressed(intake::upABit, intake); + intake.setDefaultCommand( new RunCommand( - () -> - drivetrain.driveCartesian( - xAxisSupplier.get(), yAxisSupplier.get(), zAxisSupplier.get()), - drivetrain); + () -> { + intake.move(joystick.getX()); + }, + intake)); + + createButton(joystick, 11).whenPressed(intake::on); + createButton(joystick, 12).whenPressed(intake::off); - drivetrain.setDefaultCommand(fieldOriented); + createButton(joystick, 5).whenPressed(elevator::runUp); + createButton(joystick, 3).whenPressed(elevator::runDown); + createButton(joystick, 4).whenPressed(elevator::stop); + + createButton(joystick, 10).whenPressed(intake::reset); + } + + private JoystickButton createButton(GenericHID joystick, int buttonNumber) { + return new JoystickButton(joystick, buttonNumber); + } + + private JoystickButton createButton(XboxController controller, XboxController.Button button) { + return createButton(controller, button.value); + } + + private void setDrivetrainDefault(Command command) { + drivetrain.setDefaultCommand(command); } private void configureAutoChoices() { for (AutonomousRoute route : AutonomousRoute.values()) { routeChooser.addOption(route.name(), route); } + SmartDashboard.putData("AutonomousChoice", routeChooser); + + SmartDashboard.putNumber(AUTONOMOUS_WAIT, 0); } private void loadPaths() { trajectories = Arrays.stream(AutonomousRoute.values()) + .filter(r -> r != AutonomousRoute.DriveOut) .collect( Collectors.toMap( Function.identity(), @@ -129,27 +249,46 @@ private void configureButtonBindings() {} * @return the command to run in autonomous */ public Command getAutonomousCommand() { + // var choice = routeChooser.getSelected(); + // if (choice == AutonomousRoute.DriveOut) { + double autoWait = SmartDashboard.getNumber(AUTONOMOUS_WAIT, 0.1); + var driveOut = + new RunCommand( + () -> { + drivetrain.driveCartesian(.8, 0, 0, 0); + }, + drivetrain) + .withTimeout(.5) + .andThen(drivetrain::stop, drivetrain) + .andThen(new Shoot(shooter, elevator)) + .andThen( + new RunCommand( + () -> { + drivetrain.driveCartesian(.8, 0, 0, 0); + }, + drivetrain) + .withTimeout(1)); + // new Shoot(shooter, elevator).alongWith(new DriveOut(drivetrain)); + return new WaitCommand(autoWait) + .andThen(shooter::high, shooter) + .andThen(driveOut) + .andThen(() -> vision.setPipeline(1)) + .andThen(driveCommand::setupDriving); + /* + } // Create config for trajectory - // TrajectoryConfig config = new TrajectoryConfig( - // AutoConstants.MAX_SPEED_METERS_PER_SECOND, - // AutoConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) - // // Add kinematics to ensure max speed is actually obeyed - // .setKinematics(DriveConstants.DRIVE_KINEMATICS); + var trajectory = trajectories.get(choice); + trajectories = null; + field2d.getObject("trajectory").setTrajectory(trajectory); + // Reset odometry to the starting pose of the trajectory. + pose2dHandler.resetToPose(trajectory.getInitialPose()); // use the selected trajectory - Supplier trajectory = - () -> { - var traj = trajectories.get(routeChooser.getSelected()); - field2d.getObject("traj").setTrajectory(traj); - // Reset odometry to the starting pose of the trajectory. - drivetrain.setInitialPose(traj.getInitialPose()); - return traj; - }; - ETMecanumControllerCommand mecanumControllerCommand = - new ETMecanumControllerCommand( + PPMecanumControllerCommand mecanumControllerCommand = + new PPMecanumControllerCommand( trajectory, - drivetrain::getPose, - DriveConstants.DRIVE_KINEMATICS, + pose2dHandler::getPoseEst, + PoseConstants.DRIVE_KINEMATICS, // Position contollers new PIDController(AutoConstants.kPXController, 0, 0), @@ -160,54 +299,20 @@ public Command getAutonomousCommand() { drivetrain::setDriveSpeeds, drivetrain); - /* - * use mine since it controls rotation as well. - * MecanumControllerCommand mecanumControllerCommand = - * new MecanumControllerCommand( - * trajectory, - * drivetrain::getPose, - * // DriveConstants.kFeedforward, - * DriveConstants.DRIVE_KINEMATICS, - * - * // Position contollers - * new PIDController(AutoConstants.kPXController, 0, 0), - * new PIDController(AutoConstants.kPYController, 0, 0), - * new ProfiledPIDController( - * AutoConstants.kPThetaController, 0, 0, - * AutoConstants.kThetaControllerConstraints), - * - * // TODO: desired rotation??? - should come from trajectory - * - * // Needed for normalizing wheel speeds - * AutoConstants.MAX_SPEED_METERS_PER_SECOND, - * - * // Velocity PID's - * // new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), - * // new PIDController(DriveConstants.kPRearLeftVel, 0, 0), - * // new PIDController(DriveConstants.kPFrontRightVel, 0, 0), - * // new PIDController(DriveConstants.kPRearRightVel, 0, 0), - * // drivetrain::getCurrentWheelSpeeds, - * - * drivetrain::setDriveSpeeds, // Consumer for the output speeds - * drivetrain); - */ - - // get the initial pause - // Run path following command, then stop at the end. - Command drive = mecanumControllerCommand.andThen(() -> drivetrain.drivePolar(0, 0, 0)); + // Command drive = mecanumControllerCommand.andThen(() -> drivetrain.drivePolar(0, 0, 0)); - Command runIntake = new InstantCommand(() -> {}); + Command prepIntake = new InstantCommand(() -> {}); + Command prepShooter = new InstantCommand(() -> {}); - Command autonomousJobs = drive.alongWith(runIntake); + Command autonomousJobs = mecanumControllerCommand.alongWith(prepIntake, prepShooter); + // get the initial pause + double autoWait = SmartDashboard.getNumber(AUTONOMOUS_WAIT, 0.0); Command autonomousCommand = - new InstantCommand( - () -> { - double autoWait = SmartDashboard.getNumber(AUTONOMOUS_WAIT, 0.0); - new ScheduleCommand(new WaitCommand(autoWait).andThen(autonomousJobs)); - }); + autoWait > 0 ? new WaitCommand(autoWait).andThen(autonomousJobs) : autonomousJobs; return autonomousCommand; + */ } } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java new file mode 100644 index 0000000..0044d63 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -0,0 +1,194 @@ +package frc.robot.commands; + +import static frc.robot.Constants.OIConstants.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Vision; +import frc.robot.util.RumbleHelper; +import java.util.Optional; +import java.util.function.Supplier; + +public class DriveCommand extends CommandBase { + public enum ForwardDirection { + Intake, + Shooter + } + + private Drivetrain drivetrain; + private Vision vision; + private Gyro gyro; + private Optional rumbler; + private Supplier yAxisSupplier; + private Supplier xAxisSupplier; + private Supplier zAxisSupplier; + private Supplier multiplierSupplier; + private boolean lockAngle = false; + private boolean fieldOriented = true; + private boolean intakeIsFront = true; + // private double driveMultiplier = 1; + private PIDController turnController; + + public DriveCommand( + Drivetrain drivetrain, + Supplier yAxisSupplier, + Supplier xAxisSupplier, + Supplier zAxisSupplier, + Supplier multiplierSupplier, + Vision vision, + Gyro gyro) { + this( + drivetrain, + yAxisSupplier, + xAxisSupplier, + zAxisSupplier, + multiplierSupplier, + vision, + gyro, + Optional.empty()); + } + + public DriveCommand( + Drivetrain drivetrain, + Supplier yAxisSupplier, + Supplier xAxisSupplier, + Supplier zAxisSupplier, + Supplier multiplierSupplier, + Vision vision, + Gyro gyro, + RumbleHelper rumble) { + this( + drivetrain, + yAxisSupplier, + xAxisSupplier, + zAxisSupplier, + multiplierSupplier, + vision, + gyro, + Optional.of(rumble)); + } + + private DriveCommand( + Drivetrain drivetrain, + Supplier yAxisSupplier, + Supplier xAxisSupplier, + Supplier zAxisSupplier, + Supplier multiplierSupplier, + Vision vision, + Gyro gyro, + Optional rumble) { + this.vision = vision; + this.drivetrain = drivetrain; + this.yAxisSupplier = yAxisSupplier; + this.xAxisSupplier = xAxisSupplier; + this.zAxisSupplier = zAxisSupplier; + this.multiplierSupplier = multiplierSupplier; + this.gyro = gyro; + this.rumbler = rumble; + turnController = new PIDController(0.5, 0, 0); + setTargetLock(false); + setForward(ForwardDirection.Intake); + setFieldOriented(true); + addRequirements(drivetrain); + } + + @Override + public void execute() { + + SmartDashboard.putBoolean("FrontIsIntake", intakeIsFront); + var multiplier = Math.max(1 - multiplierSupplier.get(), .5); + SmartDashboard.putNumber("DriveMultiplier", multiplier); + + // if running locked, get theta from vision + if (lockAngle) { + var target = vision.getLatestResult(); + if (target.isPresent()) { + var setpoint = target.get().getYaw(); + var angle = gyro.getAngle(); + drivetrain.driveCartesian( + multiplier * yAxisSupplier.get(), + multiplier * xAxisSupplier.get(), + turnController.calculate(angle, setpoint), + angle); + rumbler.ifPresent(r -> r.rumble(RUMBLE_LEFT_LOCKED, RUMBLE_RIGHT_LOCKED, NOTIFY_RATE)); + + } else { + rumbler.ifPresent( + r -> r.rumble(RUMBLE_LEFT_NO_TARGET, RUMBLE_RIGHT_NO_TARGET, NOTIFY_RATE)); + } + } + // else get theta from joystick + else { + if (fieldOriented) { + drivetrain.driveCartesian( + multiplier * yAxisSupplier.get(), + multiplier * xAxisSupplier.get(), + multiplier * zAxisSupplier.get(), + gyro.getAngle()); + } else { + var driveMultiplier = multiplier * (intakeIsFront ? 1 : -1); + drivetrain.driveCartesian( + driveMultiplier * yAxisSupplier.get(), + driveMultiplier * xAxisSupplier.get(), + multiplier * zAxisSupplier.get()); + } + } + } + + public void setupDriving() { + setFieldOriented(false); + setForward(ForwardDirection.Intake); + } + + public void toggleFieldOriented() { + setFieldOriented(!fieldOriented); + } + + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + SmartDashboard.putBoolean("FieldOriented", fieldOriented); + } + + public void toggleDriveDirection() { + setForward(intakeIsFront ? ForwardDirection.Shooter : ForwardDirection.Intake); + } + + public void setForward(ForwardDirection forwardDirection) { + intakeIsFront = forwardDirection == ForwardDirection.Intake; + } + + // public void setDriveMultiplier(double multiplier) { + // setDriveMultiplier(MathUtil.clamp(multiplier, 0, 1), driveMultiplier); + // } + + // private void setDriveMultiplier(double multiplier, double sign) { + // driveMultiplier = Math.copySign(Math.abs(multiplier), sign); + + // SmartDashboard.putBoolean("FrontIsIntake", driveMultiplier > 0); + // SmartDashboard.putNumber("DriveMultiplier", Math.abs(driveMultiplier)); + // } + + public void toggleTargetLock() { + setTargetLock(!lockAngle); + } + + public void setTargetLock(boolean targetLock) { + lockAngle = targetLock; + if (targetLock) { + setForward(ForwardDirection.Intake); + } + SmartDashboard.putBoolean("TargetLocked", lockAngle); + } + + public void engageTargetLock() {} + + public void disengageTargetLock() {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveOut.java b/src/main/java/frc/robot/commands/DriveOut.java new file mode 100644 index 0000000..8db6cbe --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveOut.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + +public class DriveOut extends CommandBase { + private Drivetrain drivetrain; + private Timer timer = new Timer(); + + public DriveOut(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + if (timer.hasElapsed(1)) { + drivetrain.drivePolar(.6, 0d, 0d); + timer.reset(); + } + } + + @Override + public boolean isFinished() { + return timer.advanceIfElapsed(1.5); + } +} diff --git a/src/main/java/frc/robot/commands/ETMecanumControllerCommand.java b/src/main/java/frc/robot/commands/ETMecanumControllerCommand.java index d9c56ad..96fbc77 100644 --- a/src/main/java/frc/robot/commands/ETMecanumControllerCommand.java +++ b/src/main/java/frc/robot/commands/ETMecanumControllerCommand.java @@ -92,19 +92,14 @@ public void execute() { targetWheelSpeeds.desaturate(maxWheelVelocityMetersPerSecond); - var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; - var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; - var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; - var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; + // var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; + // var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; + // var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; + // var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; // TODO: maybe log entries? - outputWheelSpeeds.accept( - new MecanumDriveWheelSpeeds( - frontLeftSpeedSetpoint, - frontRightSpeedSetpoint, - rearLeftSpeedSetpoint, - rearRightSpeedSetpoint)); + outputWheelSpeeds.accept(targetWheelSpeeds); } @Override diff --git a/src/main/java/frc/robot/commands/RunIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/RunIntakeElevatorCommand.java new file mode 100644 index 0000000..5db9d2c --- /dev/null +++ b/src/main/java/frc/robot/commands/RunIntakeElevatorCommand.java @@ -0,0 +1,68 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; + +public class RunIntakeElevatorCommand extends CommandBase { + private Intake intake; + private Elevator elevator; + private Timer timer = new Timer(); + // private State state = State.GET_BALL0; + private boolean collecting = true; + + public RunIntakeElevatorCommand(Intake intake, Elevator elevator) { + this.intake = intake; + this.elevator = elevator; + addRequirements(intake, elevator); + } + + @Override + public void initialize() { + // lower the intake & delayed start the roller + intake.lower(); + + // state = getState(); + collecting = false; + // System.out.println("Elevator: " + state); + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + if (timer.hasElapsed(.5)) { + intake.on(); + } + + if (elevator.isBallAtIntake()) { + timer.reset(); + collecting = true; + elevator.runUp(); + } + } + + // private State getState() { + // return elevator.isBallAtBottom() ? State.GET_BALL1 : State.GET_BALL0; + // } + + @Override + public boolean isFinished() { + // elevator.isBallAtTop() || + return collecting && elevator.isBallAtBottom() || timer.hasElapsed(10); + } + + @Override + public void end(boolean interrupted) { + elevator.stop(); + intake.off(); + intake.lift(); + } + + // private enum State { + + // GET_BALL0, + // GET_BALL1 + // } +} diff --git a/src/main/java/frc/robot/commands/Shoot.java b/src/main/java/frc/robot/commands/Shoot.java new file mode 100644 index 0000000..eb8e967 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -0,0 +1,44 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Shooter; + +public class Shoot extends CommandBase { + private Shooter shooter; + private Elevator elevator; + private Timer timer = new Timer(); + + public Shoot(Shooter shooter, Elevator elevator) { + this.shooter = shooter; + this.elevator = elevator; + addRequirements(shooter, elevator); + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + shooter.on(); + // elevator.runDown(); + } + + @Override + public void execute() { + if (timer.hasElapsed(1)) { + elevator.runUp(); + } + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(5); + } + + @Override + public void end(boolean interrupted) { + elevator.stop(); + shooter.off(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5fe1b39..d3e1021 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -2,74 +2,63 @@ import static frc.robot.Constants.DriveConstants.*; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; -import com.ctre.phoenix.sensors.PigeonIMU.GeneralStatus; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.MecanumDriveOdometry; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.wpilibj.drive.MecanumDrive; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Pose2dHandler; public class Drivetrain extends SubsystemBase { - CANSparkMax motorFrontLeft; - CANSparkMax motorFrontRight; - CANSparkMax motorBackLeft; - CANSparkMax motorBackRight; - // WPI_TalonSRX pigeonMotorController; + private CANSparkMax motorFrontLeft; + private CANSparkMax motorFrontRight; + private CANSparkMax motorBackLeft; + private CANSparkMax motorBackRight; - private WPI_PigeonIMU pigeon; - private Pose2d pose; + // private Pose2d pose; private MecanumDrive mecanumDrive; - private MecanumDriveOdometry mecanumDriveOdometry; - private Field2d field2d; - - public Drivetrain(Field2d field2d) { - this.field2d = field2d; + private Pose2dHandler pose2dHandler; + // private MecanumDriveOdometry mecanumDriveOdometry; + // private Field2d field2d; + public Drivetrain(Pose2dHandler pose2dHandler) { motorFrontLeft = FRONT_LEFT.createMotor(); motorFrontRight = FRONT_RIGHT.createMotor(); motorBackLeft = REAR_LEFT.createMotor(); motorBackRight = REAR_RIGHT.createMotor(); - mecanumDrive = new MecanumDrive(motorFrontLeft, motorFrontRight, motorBackLeft, motorBackRight); - // mecanumDrive.setDeadband(0.05); - - pigeon = new WPI_PigeonIMU(PIGEON_MOTOR_PORT); - // pigeon.configTemperatureDompensationEnable(true, 0); + mecanumDrive = new MecanumDrive(motorFrontLeft, motorBackLeft, motorFrontRight, motorBackRight); + mecanumDrive.setDeadband(0.06); + this.pose2dHandler = pose2dHandler; + // mecanumDrive.setMaxOutput(0.4); - mecanumDriveOdometry = new MecanumDriveOdometry(DRIVE_KINEMATICS, pigeon.getRotation2d()); + // mecanumDriveOdometry = new MecanumDriveOdometry(DRIVE_KINEMATICS, pigeon.getRotation2d()); } - public void setInitialPose(Pose2d initialPose) { - mecanumDriveOdometry.resetPosition(pose, pigeon.getRotation2d()); - pose = mecanumDriveOdometry.getPoseMeters(); - field2d.setRobotPose(mecanumDriveOdometry.getPoseMeters()); - } + // public void setInitialPose(Pose2d initialPose) { + // // mecanumDriveOdometry.resetPosition(pose, pigeon.getRotation2d()); + // // pose = mecanumDriveOdometry.getPoseMeters(); + // field2d.setRobotPose(mecanumDriveOdometry.getPoseMeters()); + // } - @Override - public void periodic() { + public void periodicDrivetrain() { // Get my wheel speeds var wheelSpeeds = new MecanumDriveWheelSpeeds( motorFrontLeft.getEncoder().getVelocity(), motorFrontRight.getEncoder().getVelocity(), motorBackLeft.getEncoder().getVelocity(), motorBackRight.getEncoder().getVelocity()); - var gyroAngle = pigeon.getRotation2d(); - - // Update the pose - pose = mecanumDriveOdometry.update(gyroAngle, wheelSpeeds); - field2d.setRobotPose(mecanumDriveOdometry.getPoseMeters()); + pose2dHandler.update(wheelSpeeds); SmartDashboard.putNumber("LeftFrontVelocity", wheelSpeeds.frontLeftMetersPerSecond); SmartDashboard.putNumber("RightFrontVelocity", wheelSpeeds.frontRightMetersPerSecond); SmartDashboard.putNumber("LeftRearVelocity", wheelSpeeds.rearLeftMetersPerSecond); SmartDashboard.putNumber("RightRearVelocity", wheelSpeeds.rearRightMetersPerSecond); + // SmartDashboard.putData("pig", pigeon); + + log(); } @Override @@ -78,18 +67,32 @@ public void simulationPeriodic() { super.simulationPeriodic(); } - public Pose2d getPose() { - return pose; - } + // public Pose2d getPose() { + // return pose; + // } + + // public void toggleRobotOriented() { + // robotOriented = !robotOriented; + // } + + // public void toggleDrivingDirection() { + // driveMultiplier *= -1; + // } + + // public void setDriveMultiplier(double multipler) { + // driveMultiplier = Math.copySign(multipler, driveMultiplier); + // } public void driveCartesian(double ySpeed, double xSpeed, double zRotation) { - mecanumDrive.driveCartesian(ySpeed, xSpeed, zRotation); + driveCartesian(ySpeed, xSpeed, zRotation, 0.0); } - public void driveCartesianFieldOriented(double ySpeed, double xSpeed, double zRotation) { - System.out.printf( - "drive y: %d x: %d z: %d heading: %d", ySpeed, xSpeed, zRotation, getCompassHeading()); - mecanumDrive.driveCartesian(ySpeed, xSpeed, zRotation, getCompassHeading()); + public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) { + SmartDashboard.putNumber("ySpeed", ySpeed); + SmartDashboard.putNumber("xSpeed", xSpeed); + SmartDashboard.putNumber("zRotation", zRotation); + SmartDashboard.putNumber("compassHeading", gyroAngle); + mecanumDrive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle); } public void drivePolar(double magnitude, double angle, double zRotation) { @@ -99,16 +102,16 @@ public void drivePolar(double magnitude, double angle, double zRotation) { public void setDriveSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { motorFrontLeft .getPIDController() - .setReference(wheelSpeeds.frontLeftMetersPerSecond, ControlType.kSmartVelocity, 0); + .setReference(wheelSpeeds.frontLeftMetersPerSecond, ControlType.kSmartMotion, 0); motorFrontRight .getPIDController() - .setReference(wheelSpeeds.frontRightMetersPerSecond, ControlType.kSmartVelocity, 0); + .setReference(wheelSpeeds.frontRightMetersPerSecond, ControlType.kSmartMotion, 0); motorBackLeft .getPIDController() - .setReference(wheelSpeeds.rearLeftMetersPerSecond, ControlType.kSmartVelocity, 0); + .setReference(wheelSpeeds.rearLeftMetersPerSecond, ControlType.kSmartMotion, 0); motorBackRight .getPIDController() - .setReference(wheelSpeeds.rearRightMetersPerSecond, ControlType.kSmartVelocity, 0); + .setReference(wheelSpeeds.rearRightMetersPerSecond, ControlType.kSmartMotion, 0); } public void stop() { @@ -120,61 +123,5 @@ public void log() { SmartDashboard.putNumber("Back Left Motor Speed", motorBackLeft.get()); SmartDashboard.putNumber("Front Right Motor Speed", motorFrontRight.get()); SmartDashboard.putNumber("Back Right Motor Speed", motorBackRight.get()); - SmartDashboard.putNumber("Angle", getAngle()); - SmartDashboard.putNumber("Angle Continuous", getAngleContinuous()); - SmartDashboard.putNumber("Compass Angle Absolute", getAbsoluteCompassHeading()); - SmartDashboard.putNumber("Compass Angle", getCompassHeading()); - SmartDashboard.putNumber("Pigeon Temp", pigeon.getTemp()); - var stat = getGeneralStatus(); - SmartDashboard.putNumber("Compass Cal Error", stat.calibrationError); - SmartDashboard.putBoolean("Compass Calibrating", stat.bCalIsBooting); - - if (SmartDashboard.getBoolean("Reset Gyro", false)) { - zero(); - SmartDashboard.putBoolean("Reset Gyro", false); - } - - if (SmartDashboard.getBoolean("Reset Comp", false)) { - resetCompass(); - SmartDashboard.putBoolean("Reset Comp", false); - } - } - - public double getAngle() { - double angle = -pigeon.getFusedHeading() % 360; - double out = (angle < -180) ? angle + 360 : angle; - return (out > 180) ? out - 360 : out; - } - - public double getAngleContinuous() { - return -pigeon.getFusedHeading(); - } - - public double getAbsoluteCompassHeading() { - return pigeon.getAbsoluteCompassHeading(); - } - - public double getCompassHeading() { - return pigeon.getCompassHeading(); - } - - public void resetCompass() { - pigeon.enterCalibrationMode(CalibrationMode.Accelerometer); - } - - public GeneralStatus getGeneralStatus() { - GeneralStatus status = new GeneralStatus(); - pigeon.getGeneralStatus(status); - return status; - } - - public double getTiltAcceleration() { - double[] rawGyroData = new double[3]; - pigeon.getRawGyro(rawGyroData); - return rawGyroData[0]; - } - - public void zero() { - pigeon.setFusedHeading(0); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..42b5029 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import static frc.robot.Constants.ElevatorConstants.*; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Elevator extends SubsystemBase { + private CANSparkMax motor; + private DigitalInput[] throughBeams; + + public Elevator() { + motor = ELEVATOR_MOTOR.createMotor(); + throughBeams = + new DigitalInput[] {new DigitalInput(0), new DigitalInput(1), new DigitalInput(2)}; + SmartDashboard.putNumber("ElevatorSpeed", 0); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("BallInIntake", isBallAtIntake()); + SmartDashboard.putBoolean("BallInBottomElevator", isBallAtBottom()); + SmartDashboard.putBoolean("BallInTopElevator", isBallAtTop()); + } + + public void runUp() { + motor.getPIDController().setReference(-SPEED, CANSparkMax.ControlType.kVelocity); + } + + public void runDown() { + motor.getPIDController().setReference(SPEED * 0.25, CANSparkMax.ControlType.kVelocity); + } + + public void stop() { + motor.set(0); + } + + public void toggle() { + SmartDashboard.putNumber("ElevatorSpeed", motor.get()); + if (Math.abs(motor.get()) > 0) { + stop(); + } else { + runUp(); + } + } + + public boolean isBallAtIntake() { + return throughBeams[0].get(); + } + + public boolean isBallAtBottom() { + return throughBeams[1].get(); + } + + public boolean isBallAtTop() { + return throughBeams[2].get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/IMU.java b/src/main/java/frc/robot/subsystems/IMU.java new file mode 100644 index 0000000..ac69e91 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IMU.java @@ -0,0 +1,119 @@ +package frc.robot.subsystems; + +import static frc.robot.Constants.SensorConstants.PIGEON_MOTOR_PORT; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; +import com.ctre.phoenix.sensors.PigeonIMU.GeneralStatus; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class IMU implements Gyro { + public enum AccelerationAngles { + X, + Y, + Z + } + + private WPI_PigeonIMU pigeon; + + public IMU() { + pigeon = new WPI_PigeonIMU(new WPI_TalonSRX(PIGEON_MOTOR_PORT)); + pigeon.configFactoryDefault(); + pigeon.setYaw(0); + pigeon.setAccumZAngle(0); + reset(); + // pigeon.configTemperatureDompensationEnable(true, 0); + } + + @Override + public double getAngle() { + return pigeon.getAngle(); + } + + public double getModedAngle() { + double angle = getAngle() % 360; + double out = (angle < -180) ? angle + 360 : angle; + return (out > 180) ? out - 360 : out; + } + + public double[] getAccelerometerAngles() { + double[] tiltAngles = new double[3]; + pigeon.getAccelerometerAngles(tiltAngles); + return tiltAngles; + } + + // public double getAngleContinuous() { + // return -pigeon.getFusedHeading(); + // } + + public double getAbsoluteCompassHeading() { + return pigeon.getAbsoluteCompassHeading(); + } + + public double getCompassHeading() { + return pigeon.getCompassHeading(); + } + + public void resetCompass() { + pigeon.enterCalibrationMode(CalibrationMode.Accelerometer); + } + + public GeneralStatus getGeneralStatus() { + GeneralStatus status = new GeneralStatus(); + pigeon.getGeneralStatus(status); + return status; + } + + public double getTiltSpeed() { + double[] rawGyroData = new double[3]; + pigeon.getRawGyro(rawGyroData); + return rawGyroData[0]; + } + + public void log() { + SmartDashboard.putNumber("Angle", getModedAngle()); + SmartDashboard.putNumber("Angle Continuous", getAngle()); + SmartDashboard.putNumber("Compass Angle Absolute", getAbsoluteCompassHeading()); + SmartDashboard.putNumber("Compass Angle", getCompassHeading()); + SmartDashboard.putNumber("Pigeon Temp", pigeon.getTemp()); + var stat = getGeneralStatus(); + SmartDashboard.putNumber("Compass Cal Error", stat.calibrationError); + SmartDashboard.putBoolean("Compass Calibrating", stat.bCalIsBooting); + + if (SmartDashboard.getBoolean("Reset Gyro", false)) { + reset(); + SmartDashboard.putBoolean("Reset Gyro", false); + } + + if (SmartDashboard.getBoolean("Reset Comp", false)) { + resetCompass(); + SmartDashboard.putBoolean("Reset Comp", false); + } + } + + @Override + public void close() throws Exception { + pigeon.close(); + } + + @Override + public void calibrate() { + pigeon.calibrate(); + } + + @Override + public void reset() { + pigeon.reset(); + } + + public void resetAngle(double angle) { + pigeon.setFusedHeading(-angle); + } + + @Override + public double getRate() { + return pigeon.getRate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 7b4aed2..3a2143e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -15,37 +15,76 @@ public class Intake extends SubsystemBase { private final CANSparkMax intakeMotor; + private final CANSparkMax liftMotor; private boolean spinning = false; public Intake() { intakeMotor = INTAKE_MOTOR.createMotor(); + liftMotor = INTAKE_LIFT_MOTOR.createMotor(); + SmartDashboard.putBoolean("Intake", spinning); + SmartDashboard.putNumber("Intake.UpPos", UP_POSITION); + SmartDashboard.putNumber("Intake.DownPos", DOWN_POSITION); + } + + public void reset() { + // something is causing the lift to get stuck in the down position. + liftMotor.getEncoder().setPosition(DOWN_POSITION + 2); + } + + public void move(double amt) { + liftMotor.set(amt); + } + + public void lift() { + var pos = SmartDashboard.getNumber("Intake.UpPos", UP_POSITION); + liftMotor.getPIDController().setReference(pos, ControlType.kSmartMotion, 0); + } + + public void slowLower() { + liftMotor.getPIDController().setReference(15, ControlType.kSmartMotion, 1); + } + + public void lower() { + var pos = SmartDashboard.getNumber("Intake.DownPos", DOWN_POSITION); + liftMotor.getPIDController().setReference(pos, ControlType.kSmartMotion, 0); + } + + public void liftOff() { + liftMotor.set(0); + } + + public boolean isLiftUp() { + return false; + // return liftMotor.getEncoder().getPosition() < LIFT_THRESHOLD; } @Override public void periodic() { - SmartDashboard.putBoolean("Intake", spinning); + SmartDashboard.putNumber("LiftPosition", liftMotor.getEncoder().getPosition()); } - private void setSpeed(int rpm) { - intakeMotor.getPIDController().setReference(rpm, ControlType.kSmartVelocity); + private void setSpeed() { + if (spinning) { + intakeMotor.getPIDController().setReference(SPEED, ControlType.kVelocity); + } else { + intakeMotor.set(0); + } + + SmartDashboard.putBoolean("Intake", spinning); } public void on() { spinning = true; - setSpeed(SPEED); + setSpeed(); } public void off() { spinning = false; - setSpeed(0); + setSpeed(); } public void toggle() { spinning = !spinning; - if (spinning) { - on(); - } else { - off(); - } + setSpeed(); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..cca64af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems; + +import static frc.robot.Constants.ShooterConstants.*; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + private CANSparkMax motor; + private double desiredSpeedHi = .95; + private double desiredSpeedLow = .4; + private double desiredRPM = 5500; + private double rpm = 0; + + public Shooter() { + motor = SHOOTER_MOTOR.createMotor(); + SmartDashboard.putNumber("ShooterDesiredRPM", desiredSpeedHi); // desiredSpeed + SmartDashboard.putNumber("ShooterRPM", 0); + SmartDashboard.putBoolean("ShooterOn", false); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("ShooterRPM", motor.getEncoder().getVelocity()); + SmartDashboard.putBoolean("ShooterOn", motor.getEncoder().getVelocity() > 0); + // motor.getPIDController().setReference(rpm, ControlType.kVelocity); + motor.set(rpm); + } + + // private void setSpeed(double rpm) { + // // + // motor.set(rpm); + // SmartDashboard.putNumber("ShooterDesiredRPM", rpm); + // } + + public void high() { + System.out.println(desiredSpeedHi); + SmartDashboard.putNumber("ShooterDesiredRPM", desiredSpeedHi); + } + + public void low() { + System.out.println(desiredSpeedLow); + SmartDashboard.putNumber("ShooterDesiredRPM", desiredSpeedLow); + } + + public void off() { + rpm = 0; + motor.set(0); + // setSpeed(0); + } + + public void on() { + rpm = SmartDashboard.getNumber("ShooterDesiredRPM", desiredSpeedHi); + // motor.getPIDController().setReference(10, ControlType.kSmartMotion); + // setSpeed(); + } + + public void onLow() { + rpm = 3000; + } + + // public void on(double distance) { + // //TODO: use distance to extrapulate speed + + // } + + // public boolean atSpeed() { + // return Math.abs(desiredSpeed - motor.getEncoder().getVelocity()) < 100d; + // } +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..354dc92 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,101 @@ +package frc.robot.subsystems; + +import static frc.robot.Constants.VisionConstants.*; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.cscore.HttpCamera.HttpCameraKind; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.net.PortForwarder; +import java.util.Optional; + +public class Vision { + private NetworkTable networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + private Optional latestResult = Optional.empty(); + + public Vision() { + PortForwarder.add(5800, "limelight.local", 5800); + PortForwarder.add(5801, "limelight.local", 5801); + PortForwarder.add(5802, "limelight.local", 5802); + PortForwarder.add(5803, "limelight.local", 5803); + PortForwarder.add(5804, "limelight.local", 5804); + PortForwarder.add(5805, "limelight.local", 5805); + HttpCamera limelighCamera = + new HttpCamera("Limelight", "http://limelight.local:5800", HttpCameraKind.kMJPGStreamer); + CameraServer.startAutomaticCapture(limelighCamera); + } + + public Optional getLatestResult() { + return latestResult; + } + + // @Override + public void periodic() { + this.latestResult = Optional.ofNullable(getLatest()); + } + + protected VisionResult getLatest() { + if (networkTable.getEntry("tv").getDouble(0) > 0) { + // have a target! + double tx = networkTable.getEntry("tx").getDouble(0); + double ty = networkTable.getEntry("ty").getDouble(0); + double tl = networkTable.getEntry("tl").getDouble(0); + + return new VisionResult() { + @Override + public double getLatencyMillis() { + return tl; + } + + @Override + public double getDistance() { + double distance = HEIGHT_FROM_CAMERA_TO_GOAL / Math.tan(Math.toRadians(CAMERA_TILT + ty)); + return distance; + } + + @Override + public double getYaw() { + return tx; + } + }; + } + return null; + } + + public void setMode(Mode mode) {} + + public void setPipeline(int pipeline) { + networkTable.getEntry("pipeline").setNumber(pipeline); + } + + public void takePicture() { + networkTable.getEntry("snapshot").setNumber(0); + networkTable.getEntry("snapshot").setNumber(1); + } + + public void setStream(int stream) { + int s = MathUtil.clamp(stream, 0, 3); + networkTable.getEntry("stream").setNumber(s); + } + + public void toggleLED() { + var lMode = networkTable.getEntry("ledMode"); + lMode.setNumber((lMode.getNumber(0).intValue() > 0) ? 1 : 0); + } + + public enum Mode { + Target, + Driver + } + + public interface VisionResult { + // default Pose2d getCameraPose() { return null; } + double getLatencyMillis(); + + double getDistance(); + // Transform2d getCameraToTarget(); + double getYaw(); + } +} diff --git a/src/main/java/frc/robot/util/RumbleHelper.java b/src/main/java/frc/robot/util/RumbleHelper.java new file mode 100644 index 0000000..0259fed --- /dev/null +++ b/src/main/java/frc/robot/util/RumbleHelper.java @@ -0,0 +1,45 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import java.util.concurrent.ConcurrentLinkedQueue; +import java.util.function.BiConsumer; + +public class RumbleHelper { + private Command rumbleCommand = new RumbleCommand(); + private ConcurrentLinkedQueue rumbleData = new ConcurrentLinkedQueue<>(); + private BiConsumer rumbler; + + public RumbleHelper(BiConsumer rumbler) { + this.rumbler = rumbler; + } + + public void rumble(double left, double right, double time) { + rumbleData.add(new Double[] {left, right, time}); + rumbleCommand.schedule(); + } + + private class RumbleCommand extends CommandBase { + private Timer timer = new Timer(); + private Double[] data; + + @Override + public void initialize() { + timer.reset(); + timer.start(); + data = rumbleData.poll(); + rumbler.accept(data[0], data[1]); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(data[2]); + } + + @Override + public void end(boolean interrupted) { + rumbler.accept(0d, 0d); + } + } +}