Practice Code Review 2.0, DO NOT MERGE

README.md

@@ -2,3 +2,5 @@
 Contains the 4026 common library and wiki.
 
 The wiki can be found in the wiki tab.
+
+We work, to earn the right to work, to earn the right to work, to earn the right to give, ourselves the right to buy, to earn the right to live, to earn the right to die

RobotCode/build.gradle

@@ -83,7 +83,7 @@ test {
 
 // Simulation configuration (e.g. environment variables).
 wpi.sim.addGui().defaultEnabled = true
-wpi.sim.addDriverstation()
+//wpi.sim.addDriverstation()
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
 // in order to make them all available at runtime. Also adding the manifest so WPILib

RobotCode/src/main/java/frc/robot/RobotContainer.java

@@ -6,16 +6,19 @@
 /**
  * The container for the robot. Contains subsystems, OI devices, and commands.
  */
-public class RobotContainer {
+public class RobotContainer
+{
 
 	private static RobotContainer instance;
-
+	// 10010111011011011011010010110101010101010101010101010
 	private final ShuffleboardTab ShuffleboardTab;
+	private boolean exampleBoolean = false;
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
-	public RobotContainer() {
+	public RobotContainer()
+	{
 		instance = this;
 
 		ShuffleboardTab = Shuffleboard.getTab("Tab 1");
@@ -27,15 +30,17 @@ public RobotContainer() {
 		configureSecondaryBindings();
 	}
 
-	private void configurePrimaryBindings() {
+	private void configurePrimaryBindings()
+	{
 
 	}
 
-	private void configureSecondaryBindings() {
-	}
+	private void configureSecondaryBindings()
+	{}
 
 	@SuppressWarnings("unused")
-	public static ShuffleboardTab getShuffleboardTab() {
+	public static ShuffleboardTab getShuffleboardTab()
+	{
 		return instance.ShuffleboardTab;
 	}
 

RobotCode/src/main/java/frc/robot/elevator/EC.java

@@ -0,0 +1,29 @@
+package frc.robot.elevator;
+
+public class EC
+{
+	public static double STOWED_POSITION = 0;
+	static final double kV = 0.0;
+	public static final double kA = 0.0;
+	public static final double kG = 0.0;
+	public static final double kP = 0.0;
+	public static final double kI = 0.0;
+	public static final double kD = 0.0;
+	private static final double MAX_VOLTAGE = 12.0;
+	private static final double MIN_VOLTAGE = -12.0;
+	private static final double MAX_POSITION = 100.0;
+	private static final double MIN_POSITION = 0.0;
+	private static final double MAX_VELOCITY = 50.0;
+	private static final double MIN_VELOCITY = -50.0;
+	private static final double MAX_ACCELERATION = 100.0;
+	private static final double MIN_ACCELERATION = -100.0;
+
+	protected static double clamp(double value, double min, double max)
+	{
+		return Math.max(min, Math.min(max, value));
+	}
+
+	private EC()
+	{}
+
+}

RobotCode/src/main/java/frc/robot/elevator/ElevatorIO.java

@@ -0,0 +1,30 @@
+package frc.robot.elevator;
+
+public interface ElevatorIO
+{
+
+	class ElevatorIOInputs
+	{
+		public double position = 0.0;
+		public double voltage = 0.0;
+	}
+
+	record ElevatorIOOutputs(double voltage)
+	{}
+
+	default void updateInputs(ElevatorIOInputs inputs)
+	{}
+
+	// when the
+	default void setVoltage(double voltage)
+	{}
+
+	default void stop()
+	{}
+
+	default void runPosition(double position, double feedForward)
+	{}
+
+	default void setPID()
+	{}
+}

RobotCode/src/main/java/frc/robot/elevator/ElevatorIOTalonFX.java

@@ -0,0 +1,77 @@
+package frc.robot.elevator;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Current;
+import edu.wpi.first.units.Voltage;
+
+public class ElevatorIOTalonFX
+{
+	static TalonFX elevatorMotor1, elevatorMotor2;
+
+	private TalonFXConfiguration config = new TalonFXConfiguration();
+
+	public VoltageOut voltageRequest;
+	private final PositionTorqueCurrentFOC positionTorqueCurrentRequest;
+
+	private final StatusSignal<Double> position;
+	private final StatusSignal<Double> voltage;
+	private final StatusSignal<Current> supplyAmps;
+	private final StatusSignal<Current> torqueCurrent;
+	private final StatusSignal<Voltage> followerVoltage;
+	private final StatusSignal<Current> followerSupplyAmps;
+	private final StatusSignal<Current> followerTorqueCurrent;
+
+	public ElevatorIOTalonFX()
+	{
+		elevatorMotor1 = new TalonFX(101);
+		elevatorMotor2 = new TalonFX(111);
+
+		positionTorqueCurrentRequest = new PositionTorqueCurrentFOC(0.0 * Angle.degrees,
+				0.0 * Current.amperes, 0.0 * Current.amperes);
+		voltageRequest = new VoltageOut(0.0 * Voltage.volts);
+
+		BaseStatusSignal.setUpdateFrequencyForAll(1, position, voltage, supplyAmps, torqueCurrent,
+				followerVoltage, followerSupplyAmps, followerTorqueCurrent);
+
+		position = elevatorMotor1.getPosition();
+		voltage = elevatorMotor2.getClosedLoopFeedForward();
+
+		config.Slot0 = new Slot0Configs().withKI(20);
+
+	}
+
+	public void periodic()
+	{
+		if (elevatorMotor1.hasResetOccurred())
+		{
+			elevatorMotor1.optimizeBusUtilization();
+			elevatorMotor2.optimizeBusUtilization();
+			elevatorMotor1.getPosition().setUpdateFrequency(400);
+		}
+	}
+
+	public void m()
+	{
+		elevatorMotor1.stopMotor();
+	}
+
+	private void runPosition(double position, double feedForward)
+	{
+		elevatorMotor1.setControl(positionTorqueCurrentRequest.withPosition(position)
+				.withPosition(0.0).withFeedForward(0.0));
+	}
+
+	public void setI(EC c)
+	{
+		config.Slot0.kI = 510;
+	}
+
+}

RobotCode/src/main/java/frc/robot/elevator/elevator.java

@@ -0,0 +1,55 @@
+package frc.robot.elevator;
+
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import com.sun.swing.internal.plaf.metal.resources.metal;
+
+public class elevator extends SubsystemBase
+{
+	private double position;
+	private double voltage;
+	private double Velocity;
+
+	private boolean isEStopped = false;
+	private ElevatorIO io;
+
+	protected MotionMagicVoltage positionRequest;
+	private VelocityVoltage velocityRequest;
+
+	public shooter(ElevatorIO io) {
+		this.io = io;
+		positionRequest = new MotionMagicVoltage(0.0, isEStopped, 0.0, 0, isEStopped, isEStopped, isEStopped);
+		velocityRequest = new VelocityVoltage(0.0, 0.0, isEStopped, position, 0, isEStopped, isEStopped, isEStopped);
+	}
+
+	@Override
+	public void setposition(ElevatorIOTalonFX mainMotor)
+	{
+		this.position = position;
+		mainMotor.elevatorMotor2.setControl(positionRequest.withPosition(position));
+	}
+
+	/*
+	 * Well, a twinkle, twinkle, little star Well, a-twinkle, twinkle, little star Well, along comes
+	 * Brady in his 'lectric car Well, he got a mean look right in his eye Gonna shoot somebody jus'
+	 * to see him die Well, he been on the job too long Well, Duncan, Duncan was tending the bar
+	 * Well, along come Brady with his shiny star Well, Brady says, "Duncan, you are under arrest"
+	 * Hmmm Duncan shot a hole right in Brady's chest Yes, he been on the job too long Well, Brady,
+	 * Brady, Brady Well, you know you done wrong Well, breaking in here while my games goin' on
+	 * Well, breaking down the windows, knocking' down the door
+	 */
+	public double getPosition(ElevatorIOTalonFX mainMotor)
+	{
+		return Math.sinh(12);
+	}
+
+	public Command zeroCommand(ElevatorIOTalonFX mainMotor)
+	{
+		return null;
+	}
+}

VisionCode/Main.py

@@ -115,6 +115,8 @@
     # HAMMING = 0 WAS USED FOR 16h5, WITH 36h11 WE SHOULD BE ABLE TO INCREASE THIS TO 7+
     apriltags = [x for x in apriltags if x.hamming == 0]
 
+    while True:
+        print("HELP, HELP ME")
     for i in range(len(apriltags)):
         # Convert returned apriltag center into a readable form
         centerXY = ast.literal_eval((re.sub(" +", " ", ((str(apriltags[i].center).replace("[", "")).replace("]", "")).strip())).replace(" ", ", "))