diff --git a/FRC_20230418_172752.wpilog b/FRC_20230418_172752.wpilog new file mode 100644 index 0000000..1cf05b8 Binary files /dev/null and b/FRC_20230418_172752.wpilog differ diff --git a/simgui.json b/simgui.json index fc4c75d..5319b5b 100644 --- a/simgui.json +++ b/simgui.json @@ -39,6 +39,7 @@ "/FMSInfo": "FMSInfo", "/LiveWindow/Tank/All": "Motor Controller", "/LiveWindow/Tank/Drive": "DifferentialDrive", + "/LiveWindow/Tank/Gyro": "Gyro", "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input", "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", "/Shuffleboard/Driver/Gyro": "Gyro", @@ -82,6 +83,9 @@ "/SmartDashboard/Field": { "Robot": { "style": "Track" + }, + "window": { + "visible": true } }, "/SmartDashboard/Scheduler": { diff --git a/src/main/java/frc/robot/Interfaces/CANSparkMax.java b/src/main/java/frc/robot/Interfaces/CANSparkMax.java index 9fc1648..118f08b 100644 --- a/src/main/java/frc/robot/Interfaces/CANSparkMax.java +++ b/src/main/java/frc/robot/Interfaces/CANSparkMax.java @@ -462,11 +462,11 @@ public double getMaxOutputRange() { // absolute encoder stuff public double getAbsoluteEncoderPosition() { throwIfClosed(); - return getAlternateEncoder(4096).getPosition(); + return getAbsoluteEncoder(Type.kDutyCycle).getPosition(); } public double getAbsoluteEncoderVelocity() { throwIfClosed(); - return getAlternateEncoder(4096).getVelocity(); + return getAbsoluteEncoder(Type.kDutyCycle).getVelocity(); } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 72206cf..8473c62 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -16,6 +16,8 @@ public class DriveCommand extends CommandBase { @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_drivebase; + private boolean isFinishedVar = false; + @@ -47,9 +49,11 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - // we need to return the value in Constants.isDriveCommandFinished but in a static way while keeping the value non-static in Constants - private static final boolean x = Constants.isDriveCommandFinished; - return x; + return isFinishedVar; + } + + public void setFinished(boolean x) { + isFinishedVar = x; } @Override diff --git a/src/main/java/frc/robot/commands/PickUpPieceCommand.java b/src/main/java/frc/robot/commands/PickUpPieceCommand.java new file mode 100644 index 0000000..b6f5329 --- /dev/null +++ b/src/main/java/frc/robot/commands/PickUpPieceCommand.java @@ -0,0 +1,38 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Claw; + +public class PickUpPieceCommand extends CommandBase { + /** Creates a new PickUpPieceCommand. */ + private Claw m_Claw; + public PickUpPieceCommand(Claw claw) { + // Use addRequirements() here to declare subsystem dependencies. + m_Claw = claw; + addRequirements(claw); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_Claw.clawMotor.set(0.5); + } + + // 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/frc/robot/commands/Tracking.java b/src/main/java/frc/robot/commands/Tracking.java deleted file mode 100644 index eb7b3b0..0000000 --- a/src/main/java/frc/robot/commands/Tracking.java +++ /dev/null @@ -1,21 +0,0 @@ -// 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 frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class Tracking extends ParallelDeadlineGroup { - /** Creates a new Tracking. */ - public Tracking() { - // Add the deadline command in the super() call. Add other commands using - // addCommands(). - super(new InstantCommand()); - // addCommands(new FooCommand(), new BarCommand()); - } -} diff --git a/src/main/java/frc/robot/commands/XTracking.java b/src/main/java/frc/robot/commands/XTracking.java index 871508e..5a1c475 100644 --- a/src/main/java/frc/robot/commands/XTracking.java +++ b/src/main/java/frc/robot/commands/XTracking.java @@ -4,42 +4,53 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; import frc.robot.subsystems.Tank; +import frc.robot.subsystems.Tracking.Constants; import frc.robot.subsystems.Tracking.Target; -import frc.robot.RobotContainer; -import frc.robot.commands.*; +import frc.robot.subsystems.Tracking.Constants.xConstants; -public class XTracking extends CommandBase { +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class XTracking extends PIDCommand { - private double tx; - /** Creates a new XTracking. */ - public XTracking(Tank tank, Target target) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(tank, target); - } + private double kp; + private double ki; + private double kd; - // Called when the command is initially scheduled. - @Override - public void initialize() { - // stop the DriveCommand + public static double kOutput; - } + private static Tank m_tank; - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - tx = Target.getTX(); + /** Creates a new XTracking. */ + public XTracking(Tank tank) { + + super( + // The controller that the command will use + new PIDController(Constants.xConstants.getkP(), Constants.xConstants.getkI(), Constants.xConstants.getkD()), + // This should return the measurement + Target::getTX, + // This should return the setpoint (can also be a constant) + () -> Constants.xConstants.getkSetpoint(), + // This uses the output + output -> { + // Use the output here + kOutput = output; + + m_tank.arcadeDrive(YTracking.kOutput, output); + }); + addRequirements(tank); + m_tank = tank; + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. } - // 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; + return Constants.xConstants.getLowerBound() < (Target.getTX() - Constants.xConstants.getkSetpoint()) + && (Target.getTX() - Constants.xConstants.getkSetpoint()) < Constants.xConstants.getUpperBound(); } } diff --git a/src/main/java/frc/robot/commands/YTracking.java b/src/main/java/frc/robot/commands/YTracking.java new file mode 100644 index 0000000..42b5b25 --- /dev/null +++ b/src/main/java/frc/robot/commands/YTracking.java @@ -0,0 +1,42 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc.robot.subsystems.Tank; +import frc.robot.subsystems.Tracking.Constants; +import frc.robot.subsystems.Tracking.Target; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class YTracking extends PIDCommand { + public static double kOutput; + /** Creates a new YTracking. */ + public YTracking() { + super( + // The controller that the command will use + new PIDController(Constants.yConstants.getkP(), Constants.yConstants.getkI(), Constants.yConstants.getkD()), + // This should return the measurement + Target::getTY, + // This should return the setpoint (can also be a constant) + () -> Constants.yConstants.getkSetpoint(), + // This uses the output + output -> { + // Use the output here + kOutput = output; + Tank.arcadeDrive(XTracking.kOutput, output); + }); + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index ccef4db..8d351ae 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -6,6 +6,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVPhysicsSim; +import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -41,7 +42,6 @@ public class Arm extends SubsystemBase { public AbsoluteEncoder encoder; - public Arm() { kP = 0.0001; @@ -58,6 +58,7 @@ public Arm() { armMotor.setIdleMode(IdleMode.kBrake); armMotor.setSmartCurrentLimit(30, 40); armMotor.burnFlash(); + // set addChild("Arm Motor", armMotor); armPID = armMotor.getPIDController(); @@ -68,8 +69,12 @@ public Arm() { armPID.setIZone(kIz); armPID.setFF(kFF); armPID.setOutputRange(kMinOutput, kMaxOutput); + + // set the spark max to alternate encoder mode - AbsoluteEncoder encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle); + + // configure the data port on top to be used with the REV Through Bore Encoder using the absolute encoder adapter + encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle); encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry(); @@ -99,8 +104,6 @@ public void moveArm(double value) { armMotor.set(motorDrive); } - - @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation diff --git a/src/main/java/frc/robot/subsystems/Tracking/Constants.java b/src/main/java/frc/robot/subsystems/Tracking/Constants.java new file mode 100644 index 0000000..42e09a2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Tracking/Constants.java @@ -0,0 +1,297 @@ +// 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 frc.robot.subsystems.Tracking; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** + * Class for the tracking constants. + */ +public class Constants extends SubsystemBase { + + public static class xConstants { + public static double kP = 0.0001; + public static double kI = 0; + public static double kD = 0; + public static double kIz = 0; + public static double kFF = 0; + public static double kMaxOutput = 1; + public static double kMinOutput = -1; + public static double kSetpoint = 0; + + public static double lowerBound; + public static double upperBound; + + /** + * @return the lowerBound + */ + public static double getLowerBound() { + return lowerBound; + } + /** + * @param lowerBound the lowerBound to set + */ + public static void setLowerBound(double lowerBound) { + xConstants.lowerBound = lowerBound; + } + /** + * @return the upperBound + */ + public static double getUpperBound() { + return upperBound; + } + /** + * @param upperBound the upperBound to set + */ + public static void setUpperBound(double upperBound) { + xConstants.upperBound = upperBound; + } + /** + * @return the kP + */ + public static double getkP() { + return kP; + } + /** + * @param kP the kP to set + */ + public static void setkP(double kP) { + xConstants.kP = kP; + } + /** + * @return the kI + */ + public static double getkI() { + return kI; + } + /** + * @param kI the kI to set + */ + public void setkI(double kI) { + this.kI = kI; + } + /** + * @return the kD + */ + public static double getkD() { + return kD; + } + /** + * @param kD the kD to set + */ + public void setkD(double kD) { + this.kD = kD; + } + /** + * @return the kIz + */ + public double getkIz() { + return kIz; + } + /** + * @param kIz the kIz to set + */ + public void setkIz(double kIz) { + this.kIz = kIz; + } + /** + * @return the kFF + */ + public double getkFF() { + return kFF; + } + /** + * @param kFF the kFF to set + */ + public void setkFF(double kFF) { + this.kFF = kFF; + } + /** + * @return the kMaxOutput + */ + public double getkMaxOutput() { + return kMaxOutput; + } + /** + * @param kMaxOutput the kMaxOutput to set + */ + public void setkMaxOutput(double kMaxOutput) { + this.kMaxOutput = kMaxOutput; + } + /** + * @return the kMinOutput + */ + public double getkMinOutput() { + return kMinOutput; + } + /** + * @param kMinOutput the kMinOutput to set + */ + public void setkMinOutput(double kMinOutput) { + this.kMinOutput = kMinOutput; + } + /** + * @return the kSetpoint + */ + public static double getkSetpoint() { + return kSetpoint; + } + /** + * @param kSetpoint the kSetpoint to set + */ + public void setkSetpoint(double kSetpoint) { + this.kSetpoint = kSetpoint; + } + } + + public static class yConstants { + public static double kP = 0.0001; + public static double kI = 0; + public static double kD = 0; + public double kIz = 0; + public double kFF = 0; + public double kMaxOutput = 1; + public double kMinOutput = -1; + public static double kSetpoint = 0; + + public static double lowerBound = 0; + public static double upperBound = 0; + /** + * @return the lowerBound + */ + public static double getLowerBound() { + return lowerBound; + } + /** + * @param lowerBound the lowerBound to set + */ + public static void setLowerBound(double lowerBound) { + yConstants.lowerBound = lowerBound; + } + /** + * @return the upperBound + */ + public static double getUpperBound() { + return upperBound; + } + /** + * @param upperBound the upperBound to set + */ + public static void setUpperBound(double upperBound) { + yConstants.upperBound = upperBound; + } + /** + * @return the kP + */ + public static double getkP() { + return kP; + } + /** + * @param kP the kP to set + */ + public void setkP(double kP) { + this.kP = kP; + } + /** + * @return the kI + */ + public static double getkI() { + return kI; + } + /** + * @param kI the kI to set + */ + public void setkI(double kI) { + this.kI = kI; + } + /** + * @return the kD + */ + public static double getkD() { + return kD; + } + /** + * @param kD the kD to set + */ + public void setkD(double kD) { + this.kD = kD; + } + /** + * @return the kIz + */ + public double getkIz() { + return kIz; + } + /** + * @param kIz the kIz to set + */ + public void setkIz(double kIz) { + this.kIz = kIz; + } + /** + * @return the kFF + */ + public double getkFF() { + return kFF; + } + /** + * @param kFF the kFF to set + */ + public void setkFF(double kFF) { + this.kFF = kFF; + } + /** + * @return the kMaxOutput + */ + public double getkMaxOutput() { + return kMaxOutput; + } + /** + * @param kMaxOutput the kMaxOutput to set + */ + public void setkMaxOutput(double kMaxOutput) { + this.kMaxOutput = kMaxOutput; + } + /** + * @return the kMinOutput + */ + public double getkMinOutput() { + return kMinOutput; + } + /** + * @param kMinOutput the kMinOutput to set + */ + public void setkMinOutput(double kMinOutput) { + this.kMinOutput = kMinOutput; + } + /** + * @return the kSetpoint + */ + public static double getkSetpoint() { + return kSetpoint; + } + /** + * @param kSetpoint the kSetpoint to set + */ + public void setkSetpoint(double kSetpoint) { + this.kSetpoint = kSetpoint; + } + } + /** Creates a new X. */ + public Constants() { + + } + + @Override + public void periodic() { + // Use the output here + } + + +} diff --git a/src/main/java/frc/robot/subsystems/Tracking/GoTo.java b/src/main/java/frc/robot/subsystems/Tracking/GoTo.java new file mode 100644 index 0000000..e53278b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Tracking/GoTo.java @@ -0,0 +1,46 @@ +// 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 frc.robot.subsystems.Tracking; + +import java.util.Map; + +import edu.bearbots.BearLib.drivebase.Drivebase; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.Tank; + +public class GoTo extends SubsystemBase { + // Mostly just a class for Shuffleboard, displaying the PID values and where the robot is going/where the target is + + private ShuffleboardTab tab = Shuffleboard.getTab("Auto Pickup"); + private ShuffleboardLayout layout = tab.getLayout("Target", BuiltInLayouts.kGrid).withPosition(0, 0).withSize(2, 2); + + private GenericEntry x = layout.add("X", 0).withWidget(BuiltInWidgets.kNumberBar).withPosition(0, 0).withSize(1, 1).getEntry(); + private GenericEntry y = layout.add("Y", 0).withWidget(BuiltInWidgets.kNumberBar).withPosition(1, 0).withSize(1, 1).getEntry(); + private GenericEntry area = layout.add("Area", 0).withWidget(BuiltInWidgets.kNumberBar).withPosition(0, 1).withSize(1, 1).getEntry(); + private GenericEntry type = layout.add("Type", false).withWidget(BuiltInWidgets.kBooleanBox).withPosition(1, 1).withSize(1, 1).withProperties(Map.of("Color when true", "Blue", "Color when false", "#fff4d", "Title", "Target Class")).getEntry(); + + + + /** Creates a new GoTo. */ + public GoTo() { + addChild("Drivebase", Tank.drive); + tab.add("Drivebase", Tank.drive).withWidget(BuiltInWidgets.kDifferentialDrive).withPosition(2, 0).withSize(2, 2); + + tab.addCamera("Limelight", "limelight", "http://10.69.64.11:5800").withPosition(0, 2).withSize(4, 2); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Tracking/Target.java b/src/main/java/frc/robot/subsystems/Tracking/Target.java index 9f82262..efd5768 100644 --- a/src/main/java/frc/robot/subsystems/Tracking/Target.java +++ b/src/main/java/frc/robot/subsystems/Tracking/Target.java @@ -80,7 +80,7 @@ public void periodic() { public static double getTX() { return NetworkTableInstance.getDefault().getTable(name).getEntry("tx").getDouble(0); } - public double getTY() { + public static double getTY() { return NetworkTableInstance.getDefault().getTable(name).getEntry("ty").getDouble(0); } public double getTA() { diff --git a/src/main/java/frc/robot/subsystems/Tracking/X.java b/src/main/java/frc/robot/subsystems/Tracking/X.java deleted file mode 100644 index 97af335..0000000 --- a/src/main/java/frc/robot/subsystems/Tracking/X.java +++ /dev/null @@ -1,34 +0,0 @@ -// 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 frc.robot.subsystems.Tracking; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; - -public class X extends PIDSubsystem { - - private ShuffleboardTab tab = Shuffleboard.getTab("Auto Pickup"); - /** Creates a new X. */ - public X() { - super( - // The PIDController used by the subsystem - new PIDController(0, 0, 0)); - addChild(getName(), m_controller); - } - - @Override - public void useOutput(double output, double setpoint) { - // Use the output here - } - - @Override - public double getMeasurement() { - // Return the process variable measurement here - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - } -}