diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f592b4fc..d387b674 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; @@ -14,7 +15,12 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; @@ -37,11 +43,15 @@ public static Robot getInstance() { private final RobotType robotType; public final Controls controls; public final Subsystems subsystems; - public final Sensors sensors; public final SuperStructure superStructure; private final PowerDistribution PDH; + Mechanism2d mech; + MechanismRoot2d root; + MechanismLigament2d m_elevator; + MechanismLigament2d m_wrist; + protected Robot() { // non public for singleton. Protected so test class can subclass @@ -51,6 +61,19 @@ protected Robot() { PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); + mech = new Mechanism2d(1, 2); + root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5)); + SmartDashboard.putData("Mechanism", mech); + m_elevator = + root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed))); + var pivot = + m_elevator.append( + new MechanismLigament2d( + "pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed))); + m_wrist = + pivot.append( + new MechanismLigament2d( + "wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed))); sensors = new Sensors(); subsystems = new Subsystems(sensors); @@ -115,6 +138,8 @@ public void robotPeriodic() { if (subsystems.visionSubsystem != null) { subsystems.visionSubsystem.update(); } + m_elevator.setLength(subsystems.elevatorSubsystem.getHeightMeters()); + m_wrist.setAngle(subsystems.armPivotSubsystem.getAngle()); } @Override diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 8df0d745..98d18a6e 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; @@ -146,6 +147,10 @@ private double getCurrentPosition() { return curPos.getValueAsDouble(); } + public Rotation2d getAngle() { // 0 is forward, + is up + return Rotation2d.fromRotations(getCurrentPosition()); + } + // preset command placeholder /* moves arm to the input position - sets the target position to the inputted position - shrinks the difference between the current position and the target position until they are close enough to work diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 371953a9..dc5e81ef 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -9,6 +9,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Voltage; @@ -52,6 +57,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; + private static final double MOTOR_ROTATIONS_PER_METER = 40; // Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; @@ -90,7 +96,8 @@ public class ElevatorSubsystem extends SubsystemBase { new Alert("Elevator", "Motor 2 not connected", AlertType.kError); private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); - + private final StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); + // Creates a SysIdRoutine SysIdRoutine routine = new SysIdRoutine( @@ -107,6 +114,8 @@ public ElevatorSubsystem() { motorConfigs(); Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition()); + //Elevator pose test + Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition()); Shuffleboard.getTab("Elevator") .addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble()); @@ -133,6 +142,7 @@ public ElevatorSubsystem() { "M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue()); Shuffleboard.getTab("Elevator") .addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble()); + } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -249,6 +259,10 @@ private double getCurrentPosition() { return curPos; } + public double getHeightMeters() { // Elevator height converted to Meters + return getCurrentPosition() / MOTOR_ROTATIONS_PER_METER; + } + private void setCurrentPosition(double pos) { m_motor.setPosition(pos); } @@ -370,6 +384,8 @@ public void periodic() { if (RobotBase.isSimulation()) { m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); + + elevatorPose3d.set(new Pose3d(new Pose2d(0.0, getHeightMeters(), new Rotation2d(0.0,0.0)))); } } }