diff --git a/simgui.json b/simgui.json index a04b8ead..99dcaf69 100644 --- a/simgui.json +++ b/simgui.json @@ -87,7 +87,7 @@ 0.0, 0.8500000238418579 ], - "height": 338 + "height": 177 } ] } diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 66c874c7..253a9746 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj.util.Color; +import org.carlmontrobotics.subsystems.Led; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -46,30 +47,17 @@ */ public final class Constants { public static final double g = 9.81; // meters per second squared - public static final class Led { - public static final int ledLength = 1000;//lower doesn't work but higher does - public static final int ledPort = 5; - - //public static final Color8Bit defaultColor = new Color8Bit(0, 0, 200); - - - - public static final Color8Bit detectNote = new Color8Bit(250,140,3); - public static final Color8Bit holding = new Color8Bit(0,250,0); - public static final Color8Bit ejectColor = new Color8Bit(250,250,0); - - public static final Color8Bit intakeColor = new Color8Bit(154,0,255); //intake detection = purple - public static final Color8Bit outtakeColor = new Color8Bit(0,9,255); //outtake detection = blue - public static final Color8Bit intakeouttakeColor = new Color8Bit(0,255,9); //intake and outtake (both) = green - //Red when nothing, purple/blue when intake/outtake detect only, green when both - - - //for weird alex stuf - public static Color8Bit startingColor = new Color8Bit(255,0,0); - + + + public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200); + public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255,255,0); + public static final Color8Bit HOLDING_GREEN = new Color8Bit(0,250,0); + public static final int ledPort = 0; + //TODO: figure out how to get port of LED, it could be 0 or } + public static final class Effectorc { // PID values @@ -222,11 +210,11 @@ public static final class Drivetrainc { // seconds it takes to go from 0 to 12 volts(aka MAX) public static final double secsPer12Volts = 0.1; - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) - // Angular velocity = Tangential speed / radius - public static final double maxRCW = maxSpeed / swerveRadius; + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; public static final boolean[] reversed = {false, false, false, false}; // public static final boolean[] reversed = {true, true, true, true}; @@ -267,27 +255,27 @@ public static final class Drivetrainc { public static final double[] kForwardAccels = {1.1047/2, 0.79422/2, 0.77114/2, 1.1003/2}; public static final double[] kBackwardAccels = kForwardAccels; - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of static friction times the gravitational acceleration - // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * g * 2; + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * g * 2; - public static final boolean isGyroReversed = true; + public static final boolean isGyroReversed = true; - // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = {4, 0.0, 0.0}; - public static final double[] yPIDController = {4, 0.0, 0.0}; - public static final double[] thetaPIDController = {0.05, 0.0, 0.001}; + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = {4, 0.0, 0.0}; + public static final double[] yPIDController = {4, 0.0, 0.0}; + public static final double[] thetaPIDController = {0.1, 0.0, 0.00}; public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, driveInversion, reversed, driveModifier, turnInversion); - public static final Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE; + //public static final Limelight.Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE; - //#endregion + //#endregion - //#region Ports + //#region Ports public static final int driveFrontLeftPort = 11; // public static final int driveFrontRightPort = 19; // @@ -304,21 +292,21 @@ public static final class Drivetrainc { public static final int canCoderPortBL = 2; public static final int canCoderPortBR = 1; - //#endregion + //#endregion - //#region Command Constants + //#region Command Constants - public static double kNormalDriveSpeed = 1; // Percent Multiplier - public static double kNormalDriveRotation = 0.5; // Percent Multiplier - public static double kSlowDriveSpeed = 0.4; // Percent Multiplier - public static double kSlowDriveRotation = 0.250; // Percent Multiplier - public static double kAlignMultiplier = 1D/3D; - public static final double kAlignForward = 0.6; + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + public static double kAlignMultiplier = 1D/3D; + public static final double kAlignForward = 0.6; - public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot. + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot. - public static final double[] positionTolerance = {Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5}; // Meters, Meters, Degrees - public static final double[] velocityTolerance = {Units.inchesToMeters(1), Units.inchesToMeters(1), 5}; // Meters, Meters, Degrees/Second + public static final double[] positionTolerance = {Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5}; // Meters, Meters, Degrees + public static final double[] velocityTolerance = {Units.inchesToMeters(1), Units.inchesToMeters(1), 5}; // Meters, Meters, Degrees/Second //#endregion //#region Subsystem Constants @@ -341,22 +329,27 @@ public static final class Autoc { //#endregion } - public static final class Limelight { + public static final class Limelightc { public static final String INTAKE_LL_NAME = "intake-limelight"; public static final String SHOOTER_LL_NAME = "shooter-limelight"; - public static final double ERROR_TOLERANCE = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; - public static final double RESOLUTION_WIDTH = 640; - public static final double MOUNT_ANGLE_DEG = 46.2; //23.228 - public static final double HEIGHT_FROM_GROUND_METERS = Units.inchesToMeters(9); //16.6 - public static final double ARM_TO_OUTTAKE_OFFSET_DEG= 115; - public static final class Apriltag { - public static final int RED_SPEAKER_CENTER_TAG_ID = 4; - public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125 - } + public static final double ERROR_TOLERANCE_RAD = 0.1; + public static final double HORIZONTAL_FOV_DEG = 0; + public static final double RESOLUTION_WIDTH_PIX = 640; + public static final double MOUNT_ANGLE_DEG_SHOOTER = 25; //23.228 + public static final double MOUNT_ANGLE_DEG_INTAKE = -22; //23.228 + public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(56); //16.6 + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); //16.6 + public static final double ARM_TO_OUTTAKE_OFFSET_DEG= 115; + public static final double NOTE_HEIGHT = Units.inchesToMeters(0); + public static final double MIN_MOVEMENT_METERSPSEC = 0.5; + public static final double MIN_MOVEMENT_RADSPSEC = 0.5; + public static final class Apriltag { + public static final int RED_SPEAKER_CENTER_TAG_ID = 4; + public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125 } + } public static final class OI { public static final class Driver { @@ -397,6 +390,7 @@ public static final class Manipulator { public static final int RAISE_CLIMBER = Button.kA.value; public static final int LOWER_CLIMBER = Button.kY.value; } + public static final double JOY_THRESH = 0.01; public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;//woah, this is high. } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 8a95f5d5..f48eaaf7 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -21,12 +21,11 @@ import org.json.simple.parser.JSONParser; //199 files import org.carlmontrobotics.subsystems.*; +import org.carlmontrobotics.subsystems.Led; import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; -import static org.carlmontrobotics.Constants.Armc.AMP_ANGLE_RAD; -import static org.carlmontrobotics.Constants.Armc.GROUND_INTAKE_POS; -import static org.carlmontrobotics.Constants.Armc.HANG_ANGLE_RAD; +import static org.carlmontrobotics.Constants.Armc.*; import static org.carlmontrobotics.Constants.OI.Manipulator.*; import edu.wpi.first.math.geometry.Pose2d; @@ -79,11 +78,11 @@ public class RobotContainer { // 2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); - private final IntakeShooter intakeShooter = new IntakeShooter(); + private final Led led = new Led(intakeShooter); private final Arm arm = new Arm(); private final Drivetrain drivetrain = new Drivetrain(); - private final AuxSystems auxSystems = new AuxSystems(arm, intakeShooter); + private final Limelight limelight = new Limelight(drivetrain); /* These must be equal to the pathPlanner path names from the GUI! */ // Order matters - but the first one is index 1 on the physical selector - index 0 is reserved for null command. @@ -145,7 +144,9 @@ private void setDefaultCommands() { } private void setBindingsDriver() { new JoystickButton(driverController, Driver.resetFieldOrientationButton).onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); - + new JoystickButton(driverController, 1).whileTrue(new AlignToApriltag(drivetrain)); //button A + new JoystickButton(driverController, 2).whileTrue(new AlignToNote(drivetrain)); //button b? + new JoystickButton(driverController, 3).whileTrue(new AutoMATICALLYGetNote(drivetrain, limelight)); //button x? // new JoystickButton(driverController, OI.Driver.slowDriveButton).onTrue(new ParallelCommandGroup( // new InstantCommand(()->drivetrain.setFieldOriented(false)), // new PrintCommand("Setting to ROBOT ORIENTED!!\nRO\nRO\nRO\n")) diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 5ebb4b28..71abb992 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -4,7 +4,7 @@ package org.carlmontrobotics.commands; -import static org.carlmontrobotics.Constants.Limelight.*; +import static org.carlmontrobotics.Constants.Limelightc.*; import org.carlmontrobotics.subsystems.Drivetrain; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.ProxyCommand; diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToNote.java b/src/main/java/org/carlmontrobotics/commands/AlignToNote.java index c39863ef..c8296df7 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToNote.java @@ -4,20 +4,55 @@ package org.carlmontrobotics.commands; -import static org.carlmontrobotics.Constants.Limelight.*; import org.carlmontrobotics.subsystems.Drivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.ProxyCommand; import org.carlmontrobotics.subsystems.LimelightHelpers; -public class AlignToNote extends ProxyCommand { +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Limelightc.INTAKE_LL_NAME; + +public class AlignToNote extends Command { + + public final TeleopDrive teleopDrive; + public final Drivetrain drivetrain; + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2]); + + public AlignToNote(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).minus(Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + //SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).minus(Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], rotationPID.calculate(drivetrain.getHeading())); + } + } - public AlignToNote(Drivetrain dt) { - super(() -> { - Rotation2d fieldOrientedTargetAngle = Rotation2d.fromDegrees(LimelightHelpers.getTX(INTAKE_LL_NAME)).plus(Rotation2d.fromDegrees(dt.getHeading())); - return new RotateToFieldRelativeAngle(fieldOrientedTargetAngle, dt); - }); - super.addRequirements(dt); - } - //REMINDER TO UPLOAD SHOOTER LIMELIGHT WITH THE PIPELINE MODEL :D + @Override + public boolean isFinished() { + return false; + // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + // return rotationPID.atSetpoint(); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java new file mode 100644 index 00000000..d3361099 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Limelightc.*; + +import org.carlmontrobotics.Constants.*; +import org.carlmontrobotics.subsystems.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +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.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.ProxyCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class AutoMATICALLYGetNote extends Command { + /** Creates a new AutoMATICALLYGetNote. */ + private Drivetrain dt; + //private IntakeShooter effector; + private Limelight ll; + private Timer timer = new Timer(); + + public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll /*IntakeShooter effector*/) { + addRequirements(this.dt = dt); + addRequirements(this.ll = ll); + //addRequirements(this.effector = effector); + // Use addRequirements() here to declare subsystem dependencies. + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + //new Intake().finallyDo(()->{this.end(false);}); + SmartDashboard.putBoolean("end", false); + dt.setFieldOriented(false); + } + + @Override + public void execute() { + double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); + double forwardDistErrMeters = ll.getDistanceToNote(); + double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad); + // dt.drive(0,0,0); + dt.drive(Math.max(forwardDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(strafeDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(angleErrRad*2,MIN_MOVEMENT_RADSPSEC)); + //180deg is about 6.2 rad/sec, min is .5rad/sec + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + dt.setFieldOriented(true); + SmartDashboard.putBoolean("end", true); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + //return timer.get() >= 0.5; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/Intake.java b/src/main/java/org/carlmontrobotics/commands/Intake.java index 58b9634f..6ec4ab85 100644 --- a/src/main/java/org/carlmontrobotics/commands/Intake.java +++ b/src/main/java/org/carlmontrobotics/commands/Intake.java @@ -4,22 +4,27 @@ import static org.carlmontrobotics.Constants.Led.*; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AuxSystems; + import org.carlmontrobotics.subsystems.IntakeShooter; +import org.carlmontrobotics.subsystems.Led; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; public class Intake extends Command { //intake until sees game peice or 4sec has passed - private final Timer timer = new Timer(); + private Timer timer; private final IntakeShooter intake; + private double endAt = 0; private final double keepIntakingFor = 0.2; int increaseAmount = 750; int index = 0; public Intake(IntakeShooter intake) { addRequirements(this.intake = intake); + + } @Override @@ -37,12 +42,14 @@ public void initialize() { public void execute() { //Intake Led if (intake.intakeDetectsNote() && !intake.outakeDetectsNote()) { - index++; + index ++; + intake.setRPMIntake(0); intake.setRPMIntake(INTAKE_SLOWDOWN_RPM + index*increaseAmount); } if (intake.outakeDetectsNote() ) { // Timer.delay(keepIntakingFor); + intake.setRPMIntake(0.0); } } @@ -60,5 +67,6 @@ public void end(boolean interrupted) { public boolean isFinished() { return (intake.intakeDetectsNote() && intake.outakeDetectsNote()); // || //timer.hasElapsed(MAX_SECONDS_OVERLOAD); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java new file mode 100644 index 00000000..67a6abb3 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.Constants.Limelightc; +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; +import org.carlmontrobotics.subsystems.LimelightHelpers; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class MoveToNote extends Command { + private final Drivetrain dt; + private final Limelight ll; + private Timer timer = new Timer(); + /** Creates a new MoveToNote. */ + public MoveToNote(Drivetrain dt, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.dt=dt); + addRequirements(this.ll=ll); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + new AlignToNote(dt); + timer.reset(); + timer.start(); + //new Intake().finallyDo(()->{this.end(false);}); + SmartDashboard.putBoolean("end", false); + dt.setFieldOriented(false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double radErr = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); + double distErr = ll.getDistanceToNote(); //meters + double forwardErr = distErr * Math.cos(radErr); + // dt.drive(0,0,0); + dt.drive(Math.max(forwardErr*2, .5), 0, 0); + //180deg is about 6.2 rad/sec, min is .5rad/sec + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + dt.setFieldOriented(true); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() >= 0.5; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToPos.java b/src/main/java/org/carlmontrobotics/commands/MoveToPos.java index ab026e6e..7ff11e09 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToPos.java +++ b/src/main/java/org/carlmontrobotics/commands/MoveToPos.java @@ -5,7 +5,6 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.Arm; -import org.carlmontrobotics.subsystems.AuxSystems; import org.carlmontrobotics.subsystems.IntakeShooter; import edu.wpi.first.wpilibj.Timer; diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToSpeaker.java b/src/main/java/org/carlmontrobotics/commands/MoveToSpeaker.java new file mode 100644 index 00000000..744de104 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/MoveToSpeaker.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.List; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class MoveToSpeaker extends Command { + private final Drivetrain dt; + /** Creates a new MoveToSpeaker. */ + public MoveToSpeaker(Drivetrain dt) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.dt = dt); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + List bezierPoints = PathPlannerPath.bezierFromPoses(dt.getPose(), new Pose2d(1, 1, Rotation2d.fromDegrees(1))); + PathPlannerPath path = new PathPlannerPath(bezierPoints, new PathConstraints(autoMaxSpeedMps, autoMaxAccelMps2, maxRCW, 1), new GoalEndState(1, Rotation2d.fromDegrees(1))); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/PassToIntake.java b/src/main/java/org/carlmontrobotics/commands/PassToIntake.java index b7fc27f9..e04090a7 100644 --- a/src/main/java/org/carlmontrobotics/commands/PassToIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/PassToIntake.java @@ -9,7 +9,6 @@ import static org.carlmontrobotics.Constants.Led.*; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AuxSystems; import org.carlmontrobotics.subsystems.IntakeShooter; import static org.carlmontrobotics.Constants.Effectorc.*; diff --git a/src/main/java/org/carlmontrobotics/commands/PassToOutake.java b/src/main/java/org/carlmontrobotics/commands/PassToOutake.java index d543de82..4cda5c9f 100644 --- a/src/main/java/org/carlmontrobotics/commands/PassToOutake.java +++ b/src/main/java/org/carlmontrobotics/commands/PassToOutake.java @@ -9,7 +9,7 @@ import static org.carlmontrobotics.Constants.Led.*; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AuxSystems; + import org.carlmontrobotics.subsystems.IntakeShooter; import static org.carlmontrobotics.Constants.Effectorc.*; diff --git a/src/main/java/org/carlmontrobotics/commands/RampToRPM.java b/src/main/java/org/carlmontrobotics/commands/RampToRPM.java index de5ca48b..6d5fb261 100644 --- a/src/main/java/org/carlmontrobotics/commands/RampToRPM.java +++ b/src/main/java/org/carlmontrobotics/commands/RampToRPM.java @@ -4,7 +4,7 @@ import static org.carlmontrobotics.Constants.Led.*; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AuxSystems; + import org.carlmontrobotics.subsystems.IntakeShooter; import edu.wpi.first.wpilibj.Timer; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 5f0be14f..4a36cab9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -189,7 +189,7 @@ public void periodic() { // SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent()); // SmartDashboard.putNumber("ArmPos", getArmPos()); - double currTime = Timer.getFPGATimestamp(); + // double currTime = Timer.getFPGATimestamp(); // SmartDashboard.putNumber("Current Time", currTime); //SmartDashboard.putNumber("Last Update (s)", lastMeasuredTime); //setArmTarget(SmartDashboard.getNumber("set arm angle (rad)", 0)); @@ -208,7 +208,7 @@ public void periodic() { // when the value is different - double currentArmPos = getArmPos(); + /*double currentArmPos = getArmPos(); if (currentArmPos != lastArmPos) { lastMeasuredTime = currTime; lastArmPos = currentArmPos; @@ -226,6 +226,7 @@ public void periodic() { autoCancelArmCommand(); + */ } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AuxSystems.java b/src/main/java/org/carlmontrobotics/subsystems/AuxSystems.java deleted file mode 100644 index 686517e8..00000000 --- a/src/main/java/org/carlmontrobotics/subsystems/AuxSystems.java +++ /dev/null @@ -1,222 +0,0 @@ -package org.carlmontrobotics.subsystems; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.carlmontrobotics.subsystems.IntakeShooter; - -import com.ctre.phoenix.led.ColorFlowAnimation; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -import static org.carlmontrobotics.Constants.Effectorc.*; -import static org.carlmontrobotics.Constants.Led.ledLength; -import static org.carlmontrobotics.Constants.Led.startingColor; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.commands.Intake; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -/** - * AuxSystems contains both LED and Rumble functionality, - * and is separate because of it's special needs to take in other subsystems. -*/ -public class AuxSystems extends SubsystemBase { - private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.Led.ledLength); - private final AddressableLED led = new AddressableLED(Constants.Led.ledPort); - private int midpoint = (int) Math.floor(ledBuffer.getLength()/2);//rounds down - //AuxSubsystems watches the two other subsystems in order to rumble/led in peace - //public final Color8Bit defaultColor = new Color8Bit(255,0,0); - private final Arm arm; - private final IntakeShooter effector; - //for led color ramping - private Color8Bit currentColor = new Color8Bit(0,0,0); - - - private LedPoint currentPoint = new LedPoint(); - public boolean matchMode = true; - //^ whether we use normal color checking or flowing color mode - - private double rumble; - //stuf - private Color8Bit[] colorCycle = { - new Color8Bit(255,0,0),//0 red - new Color8Bit(255,127,0),//1 orange - new Color8Bit(255,255,0),//2 yellow - new Color8Bit(127,255,0), - new Color8Bit(0,255,0),//4 blue - new Color8Bit(0,255,127), - new Color8Bit(0,255,255), - new Color8Bit(0,127,255), - new Color8Bit(0,127,255), - new Color8Bit(0,0,255),//9 green - new Color8Bit(127,0,255), - new Color8Bit(255,0,255),//11 purple - new Color8Bit(255,0,127), - new Color8Bit(0,0,0)//13 black - }; - - public AuxSystems(Arm a, IntakeShooter is){ - this.arm = a; - this.effector = is; - //leds - led.setLength(ledBuffer.getLength()); - setLedColor(startingColor, 0 , ledBuffer.getLength()); - led.start(); - - - SmartDashboard.putBoolean("LED Match Mode:", matchMode); - - // int r = 0; - // int g = 0; - // int b = 0; - // //weird alex stuff - // for (int red = 0; red < 255/15; red ++){ - // for (int blue = 0; blue < 255/15; blue ++){ - // for (int green = 0; green < 255/15; green ++){ - // g +=15; - - // currentPoint = currentPoint.addnext(0.1,new Color8Bit(r,g,b)); - - // } - // g = 0; - // b +=15; - - - // } - // b = 0; - // r +=15; - - - //} - for (Color8Bit color : colorCycle){ - currentPoint = currentPoint.addnext(2,color); - - } - - - - currentPoint.start(); - } - - public void setLedColor(Color8Bit color, int start, int end) { - for (int i = start; i < end ; i++) - ledBuffer.setRGB(i, color.red, color.green, color.blue); - led.setData(ledBuffer); - } - @Override - public void periodic(){ - matchMode = SmartDashboard.getBoolean("LED Match Mode:", matchMode); - if (matchMode){ - /** LED RULES - * only intake TOF: lower yellow - * only outake TOF: upper purple - * both TOF: all green - * none: all red - */ - /** RUMBLE RULES - * at setpoint + intake TOF: rumble - * only outake TOF: rumble - * else: no rumble - * none: all red - */ - if (effector.intakeDetectsNote() && effector.outakeDetectsNote()){ - setLedColor(colorCycle[0], 0, ledLength); - }else if (effector.intakeDetectsNote()){ - setLedColor(colorCycle[13], 0, ledLength);//all black - setLedColor(colorCycle[2], 0, midpoint); - } else if (effector.outakeDetectsNote()) { - setLedColor(colorCycle[13], 0, ledLength);//all black - setLedColor(colorCycle[11], midpoint, ledLength); - } else { - setLedColor(colorCycle[0], 0, ledLength); - } - - - }else { - currentColor = currentPoint.currentDesiredColor(); - setLedColor(currentColor,0,Constants.Led.ledLength); - - - } - - - - } - - /** - * LedPoint - * - * Used to represent points of a graph of color, so that you can smoothly transition between them. - * Create an initial point by calling - * LedPoint ledpoint = new LedPoint(); - * or so, and then add points to the graph with - * ledpoint = ledpoint.addnext(2, new Color8Bit(255,0,0)); - * which would smoothly transition from black (000000) to full red (ff0000) in two seconds - * as long as you called - * setLedColor(ledpoint.currentDesiredColor() ,0,Constants.Led.ledLength); - * every periodic - */ - public class LedPoint { - public double startTime,duration; - public int r,g,b; - public LedPoint parent,child; - - //initial black null constructor - public LedPoint() { - this.parent=null; - this.startTime = Timer.getFPGATimestamp(); - this.duration = 0; - this.setColor(new Color8Bit(0,0,0)); - } - //use parent to chain points together - public LedPoint(LedPoint parent, double timeToTarget, Color8Bit color) { - this.parent=parent; - this.duration = timeToTarget; - this.setColor(color); - } - public double start(){ - double now = Timer.getFPGATimestamp(); - - if (parent!=null){ - startTime = parent.start() + parent.duration; - } else { - startTime = now; - } - return startTime; - } - //default assumes startTime is NOW - public LedPoint addnext(double timeToTarget, Color8Bit color) { - this.child = new LedPoint(this, timeToTarget, color); - return this.child; - } - - private void setColor(Color8Bit color){ - this.r=color.red; - this.g=color.green; - this.b=color.blue; - } - - public Color8Bit currentDesiredColor(){//return the color it should be now - double currTime = Timer.getFPGATimestamp(); - if (currTime