Elevator start

Co-authored-by: AwesomeDustin4 <[email protected]> Co-authored-by: djjamin <[email protected]> Co-authored-by: Lia M <[email protected]> Co-authored-by: Sophie B <[email protected]>
WindingMotor · Jan 25, 2025 · 6f7f2f3 · 6f7f2f3
1 parent 167d038
commit 6f7f2f3
Show file tree

Hide file tree

Showing 6 changed files with 236 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,7 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.CMD_DriveAlign;
@@ -84,8 +85,18 @@ private void configureButtonBindings() {
 		// Aim at tag 16 for testing
 		// driverController.b().onTrue(new CMD_AimAtAprilTag(swerve, 16, 0.1));
 
-		driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND));
-		driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED));
+		/*SequentialCommandGroup
+			driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND));
+			driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED));
+		}
+			*/
+
+		driverController
+				.a()
+				.onTrue(
+						new ParallelCommandGroup(
+								intake.setState(SUB_Intake.State.ALGAE_GROUND),
+								swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_LEFT)));
 	}
 
 	public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java
@@ -12,12 +12,12 @@ public final class BuildConstants {
 	public static final String MAVEN_GROUP = "";
 	public static final String MAVEN_NAME = "SwerveDrive2025";
 	public static final String VERSION = "unspecified";
-	public static final int GIT_REVISION = 32;
-	public static final String GIT_SHA = "d45110397bccd5d27febd5a779bcede324db9cfa";
-	public static final String GIT_DATE = "2025-01-10 17:46:32 EST";
-	public static final String GIT_BRANCH = "Pesky-Branch";
-	public static final String BUILD_DATE = "2025-01-10 19:40:19 EST";
-	public static final long BUILD_UNIX_TIME = 1736556019556L;
+	public static final int GIT_REVISION = 38;
+	public static final String GIT_SHA = "167d038b6228fb29148746fa97f7517ad5143d57";
+	public static final String GIT_DATE = "2025-01-10 19:51:22 EST";
+	public static final String GIT_BRANCH = "main";
+	public static final String BUILD_DATE = "2025-01-25 11:08:25 EST";
+	public static final long BUILD_UNIX_TIME = 1737821305492L;
 	public static final int DIRTY = 1;
 
 	private BuildConstants() {}

diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorBase.java b/src/main/java/frc/robot/elevator/IO_ElevatorBase.java
@@ -0,0 +1,48 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+///
+///
+/// //
+public interface IO_ElevatorBase {
+
+	@AutoLog
+	public static class ElevatorInputs implements LoggableInputs {
+
+		public double heightM = 0.0;
+		public double velocityMPS = 0.0;
+		public double motorOneCurrent = 0.0;
+		public double motorTwoCurrent = 0.0;
+
+		@Override
+		public void toLog(LogTable table) {
+			table.put("heightM", heightM);
+			table.put("velocityMPS", velocityMPS);
+			table.put("motorOneCurrent", motorOneCurrent);
+			table.put("motorTwoCurrent", motorTwoCurrent);
+		}
+
+		@Override
+		public void fromLog(LogTable table) {
+			heightM = table.get("heightM", heightM);
+			velocityMPS = table.get("velocityMPS", velocityMPS);
+			motorOneCurrent = table.get("motorOneCurrent", motorOneCurrent);
+			motorTwoCurrent = table.get("motorTwoCurrent", motorTwoCurrent);
+		}
+	}
+
+	/** Updates the set of loggable inputs. */
+	public void updateInputs(ElevatorInputs inputs);
+
+	public void setPositionM(double positionM);
+}
diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorRealKracken.java b/src/main/java/frc/robot/elevator/IO_ElevatorRealKracken.java
@@ -0,0 +1,68 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.elevator;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public class IO_ElevatorRealKracken implements IO_ElevatorBase {
+
+	private final TalonFX motorOne;
+	private final TalonFX motorTwo;
+	private final MotionMagicVoltage motorRequest;
+
+	// private SparkMax motorOne;
+	// private SparkMax motorTwo;
+
+	public IO_ElevatorRealKracken() {
+		motorOne = new TalonFX(0);
+		motorTwo = new TalonFX(0);
+		motorRequest = new MotionMagicVoltage(0);
+
+		var motorConfigs = new TalonFXConfiguration();
+
+		// One rotation = X meters of linear travel
+		// Calculate: (Sprocket Circumference in meters) / (Total Gear Reduction)
+		// Example: (0.1524 meters per rotation) / (5:1 gear ratio) = 0.03048 meters per motor rotation
+		double METERS_PER_MOTOR_ROTATION = 0.03048;
+
+		var slot0Configs = motorConfigs.Slot0;
+		slot0Configs.kS = 0.25; // Add 0.25 V output to overcome static friction
+		slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output
+		slot0Configs.kA = 0.01; // An acceleration of 1 rps/s requires 0.01 V output
+		slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output
+		slot0Configs.kI = 0; // no output for integrated error
+		slot0Configs.kD = 0; // no output for error derivative
+
+		// Convert to meters
+		motorConfigs.Feedback.SensorToMechanismRatio = METERS_PER_MOTOR_ROTATION;
+
+		var motionMagicConfigs = motorConfigs.MotionMagic;
+		motionMagicConfigs.MotionMagicCruiseVelocity = 2.0; // meters per second
+		motionMagicConfigs.MotionMagicAcceleration = 4.0; // meters per second squared
+		motionMagicConfigs.MotionMagicJerk = 40.0; // meters per second cubed
+
+		motorOne.getConfigurator().apply(motorConfigs);
+		motorTwo.getConfigurator().apply(motorConfigs);
+	}
+
+	@Override
+	public void updateInputs(ElevatorInputs inputs) {
+		inputs.heightM = motorOne.getPosition().getValueAsDouble();
+		inputs.velocityMPS = motorOne.getVelocity().getValueAsDouble();
+		inputs.motorOneCurrent = motorOne.getSupplyCurrent().getValueAsDouble();
+		inputs.motorTwoCurrent = motorTwo.getSupplyCurrent().getValueAsDouble();
+	}
+
+	@Override
+	public void setPositionM(double positionM) {
+		motorOne.setControl(motorRequest.withPosition(positionM));
+		motorTwo.setControl(motorRequest.withPosition(positionM));
+	}
+}
diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorRealNeo.java b/src/main/java/frc/robot/elevator/IO_ElevatorRealNeo.java
@@ -0,0 +1,38 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.elevator;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+public class IO_ElevatorRealNeo implements IO_ElevatorBase {
+
+	private SparkMax motorOne;
+	private SparkMax motorTwo;
+
+	public IO_ElevatorRealNeo() {
+
+		motorOne = new SparkMax(0, MotorType.kBrushless);
+		motorTwo = new SparkMax(0, MotorType.kBrushless);
+	}
+
+	@Override
+	public void updateInputs(ElevatorInputs inputs) {
+		inputs.heightM = motorOne.getEncoder().getPosition();
+		inputs.velocityMPS = motorOne.getEncoder().getVelocity();
+		inputs.motorOneCurrent = motorOne.getOutputCurrent();
+		inputs.motorTwoCurrent = motorTwo.getOutputCurrent();
+	}
+
+	@Override
+	public void setPositionM(double speed) {
+		// motorOne.set(speed);
+		// motorTwo.set(speed);
+		// Fix this later
+	}
+}
diff --git a/src/main/java/frc/robot/elevator/SUB_Elevator.java b/src/main/java/frc/robot/elevator/SUB_Elevator.java
@@ -0,0 +1,63 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class SUB_Elevator extends SubsystemBase {
+
+	public enum State {
+		STOWED(0.0),
+		CORAL_STATION(1.0),
+		ALGAE_GROUND(0.1),
+		ALGAE_PROCESSOR(0.1),
+		ALGAE_BARGE(2.9),
+		L1_SCORING(0.5),
+		L2_SCORING(0.8),
+		L3_SCORING(1.2),
+		L4_SCORING(1.85);
+
+		private final double heightM;
+
+		State(double heightM) {
+			this.heightM = heightM;
+		}
+
+		public double getHeightM() {
+			return heightM;
+		}
+	}
+
+	private final IO_ElevatorBase io;
+
+	private final IO_ElevatorBase.ElevatorInputs inputs = new IO_ElevatorBase.ElevatorInputs();
+
+	public SUB_Elevator(IO_ElevatorBase io) {
+		this.io = io;
+	}
+
+	@Override
+	public void periodic() {
+
+		// Update inputs
+		io.updateInputs(inputs);
+
+		// Process inputs
+		Logger.processInputs("Elevator", inputs);
+	}
+
+	public Command setState(State state) {
+		return new InstantCommand(
+				() -> {
+					io.setPositionM(state.getHeightM());
+				});
+	}
+}