Merge pull request #65 from robototes/oldIntakeChanges

Various QOL and functional changes to match new old intake
robototes · Mar 24, 2023 · ba92c94 · ba92c94
2 parents be9283b + ea9a3af
commit ba92c94
Show file tree

Hide file tree

Showing 6 changed files with 85 additions and 161 deletions.
diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java
@@ -68,8 +68,6 @@ public Controls(Subsystems s) {
 		codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT);
 		this.s = s;
 
-		// TODO: reimpliment arm + wrist presets after comp
-
 		armManualControlOn = codriveController.start();
 		armManualControlOff = codriveController.back();
 

diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java
@@ -194,6 +194,9 @@ public void simulationInit() {
 		if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) {
 			subsystems.drivebaseSubsystem.simInit(sim);
 		}
+		if (Subsystems.SubsystemConstants.ARM_ENABLED) {
+			subsystems.armSubsystem.simInit(sim);
+		}
 	}
 
 	@Override

diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java
@@ -1,7 +1,5 @@
 package frc.team2412.robot.commands.arm;
 
-import static frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType.*;
-
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.team2412.robot.subsystems.ArmSubsystem;
 import frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType;
@@ -10,10 +8,12 @@ public class SetArmCommand extends CommandBase {
 
 	private ArmSubsystem armSubsystem;
 	private PositionType positionType;
+	private double tolerance;
 
-	public SetArmCommand(ArmSubsystem armSubsystem, PositionType positionType) {
+	public SetArmCommand(ArmSubsystem armSubsystem, PositionType positionType, double tolerance) {
 		this.armSubsystem = armSubsystem;
 		this.positionType = positionType;
+		this.tolerance = tolerance;
 	}
 
 	@Override
@@ -31,6 +31,6 @@ public void end(boolean interrupted) {
 
 	@Override
 	public boolean isFinished() {
-		return armSubsystem.isArmNearGoal();
+		return armSubsystem.isArmNearGoal(tolerance);
 	}
 }
diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java
@@ -8,8 +8,16 @@
 public class SetFullArmCommand extends SequentialCommandGroup {
 	public SetFullArmCommand(
 			ArmSubsystem armSubsystem, PositionType positionType, WristPosition wristPosition) {
+		this(armSubsystem, positionType, wristPosition, 0.1);
+	}
+
+	public SetFullArmCommand(
+			ArmSubsystem armSubsystem,
+			PositionType positionType,
+			WristPosition wristPosition,
+			double armTolerance) {
 		addCommands(
-				new SetArmCommand(armSubsystem, positionType),
+				new SetArmCommand(armSubsystem, positionType, armTolerance),
 				new SetWristCommand(armSubsystem, wristPosition));
 	}
 }