Add chute representation

team20 · Feb 8, 2025 · f0f8260 · f0f8260
1 parent f89cffd
commit f0f8260
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -14,12 +14,16 @@
 
 import org.littletonrobotics.urcl.URCL;
 
+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;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 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.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
@@ -33,19 +37,23 @@
 
 public class Robot extends TimedRobot {
 	private Command m_autonomousCommand;
-	private final Mechanism2d m_mechanism = new Mechanism2d(1.5, 2.5);
+	private final Mechanism2d m_mechanism = new Mechanism2d(Units.inchesToMeters(35), Units.inchesToMeters(100));
 	private final AlgaeGrabberSubsystem m_algaeGrabberSubsystem = new AlgaeGrabberSubsystem();
 	private final CheeseStickSubsystem m_cheeseStickSubsystem = new CheeseStickSubsystem();
 	private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 	private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
-	private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem(m_mechanism.getRoot("anchor", 1, 0));
+	private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem(
+			m_mechanism.getRoot("anchor", Units.inchesToMeters(23), 0));
 	private final WristSubsystem m_wristSubsystem = new WristSubsystem(m_elevatorSubsystem.getWristMount());
 	// Changed to PS5
 	private final CommandPS5Controller m_driverController = new CommandPS5Controller(kDriverControllerPort);
 	private final CommandPS5Controller m_operatorController = new CommandPS5Controller(kOperatorControllerPort);
 	private final PowerDistribution m_pdh = new PowerDistribution();
 
 	public Robot() {
+		var dropChute = new MechanismLigament2d("bottom", Units.inchesToMeters(5), 0, 5, new Color8Bit(Color.kBeige));
+		dropChute.append(new MechanismLigament2d("side", Units.inchesToMeters(12), 90, 5, new Color8Bit(Color.kWhite)));
+		m_mechanism.getRoot("dropChute", Units.inchesToMeters(28), Units.inchesToMeters(9)).append(dropChute);
 		SmartDashboard.putData("Superstructure", m_mechanism);
 		SmartDashboard.putData(m_pdh);
 		SmartDashboard.putData(CommandScheduler.getInstance());

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -47,7 +47,7 @@ public class ElevatorSubsystem extends SubsystemBase {
 					new MechanismLigament2d("wristMountPoint", Units.inchesToMeters(5), 180, 10,
 							new Color8Bit(Color.kYellow)))
 			.append(
-					new MechanismLigament2d("wristMount", Units.inchesToMeters(8), -90, 10,
+					new MechanismLigament2d("wristMount", Units.inchesToMeters(8), 90, 10,
 							new Color8Bit(Color.kBlack)));
 
 	/** Creates a new ElevatorSubsystem. */