Merge pull request #91 from iamawesomecat/main

PIDFF improvements for pivot (pre 3/19)

src/main/java/frc/team2412/robot/Controls.java

@@ -21,10 +21,12 @@
 import frc.team2412.robot.commands.intake.FeederInCommand;
 import frc.team2412.robot.commands.intake.IntakeRejectCommand;
 import frc.team2412.robot.commands.launcher.FullTargetCommand;
+import frc.team2412.robot.commands.launcher.ManualAngleCommand;
 import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand;
-import frc.team2412.robot.commands.launcher.SetAngleCommand;
 import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
+import frc.team2412.robot.commands.launcher.SetPivotCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
+import frc.team2412.robot.util.TrapAlign;
 
 public class Controls {
 	public static class ControlConstants {
@@ -47,8 +49,9 @@ public static class ControlConstants {
 	// Launcher
 	private final Trigger launcherAmpPresetButton;
 	private final Trigger launcherSubwooferPresetButton;
+	private final Trigger launcherLowerPresetButton;
 	// private final Trigger launcherPodiumPresetButton;
-	// private final Trigger launcherTrapPresetButton;
+	private final Trigger launcherTrapPresetButton;
 	private final Trigger launcherLaunchButton;
 
 	private final Subsystems s;
@@ -60,9 +63,10 @@ public Controls(Subsystems s) {
 
 		launcherAmpPresetButton = codriveController.x();
 		launcherSubwooferPresetButton = codriveController.a();
+		launcherLowerPresetButton = codriveController.y();
 		// launcherPodiumPresetButton = codriveController.povLeft();
-		// launcherTrapPresetButton = codriveController.y();
-		launcherLaunchButton = codriveController.leftBumper();
+		launcherTrapPresetButton = codriveController.rightTrigger();
+		launcherLaunchButton = codriveController.rightBumper();
 		// intake controls (confirmed with driveteam)
 		driveIntakeInButton = driveController.a();
 		driveIntakeStopButton = driveController.b();
@@ -114,7 +118,7 @@ private void bindDrivebaseControls() {
 								() -> Rotation2d.fromRotations(driveController.getRightX())));
 		driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
 		driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels));
-		// driveController
+		// driveController		x
 		// 		.back()
 		// 		.onTrue(
 		// 				new InstantCommand(
@@ -145,12 +149,14 @@ private void bindLauncherControls() {
 		CommandScheduler.getInstance()
 				.setDefaultCommand(
 						s.launcherSubsystem,
-						new SetAngleCommand(
+						new ManualAngleCommand(
 								s.launcherSubsystem,
 								() ->
 										MathUtil.applyDeadband(codriveController.getLeftY(), 0.1)
 												* LauncherSubsystem.ANGLE_MAX_SPEED));
 
+		launcherLowerPresetButton.onTrue(
+				new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE));
 		launcherSubwooferPresetButton.onTrue(
 				new SetAngleLaunchCommand(
 						s.launcherSubsystem,
@@ -166,8 +172,8 @@ private void bindLauncherControls() {
 						s.launcherSubsystem,
 						LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
 						LauncherSubsystem.AMP_AIM_ANGLE));
-		// launcherTrapPresetButton.onTrue(
-		//		TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
+		launcherTrapPresetButton.onTrue(
+				TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
 
 		codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher));
 

src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java

@@ -2,8 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.team2412.robot.commands.launcher.SetAngleCommand;
-import frc.team2412.robot.commands.launcher.SetLaunchSpeedCommand;
+import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
 import frc.team2412.robot.commands.launcher.StopLauncherCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
 
@@ -15,12 +14,11 @@ public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) {
 		this.launcherSubsystem = launcherSubsystem;
 		this.Angle = launcherSubsystem.getAngle();
 		addCommands(
-				new SetAngleCommand(launcherSubsystem, () -> 45),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10),
 				new WaitCommand(2),
-				new SetAngleCommand(launcherSubsystem, () -> 90),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20),
 				new WaitCommand(1),
-				new SetAngleCommand(launcherSubsystem, () -> Angle),
-				new SetLaunchSpeedCommand(launcherSubsystem, 100),
+				new SetAngleLaunchCommand(launcherSubsystem, 500, Angle),
 				new WaitCommand(5),
 				new StopLauncherCommand(launcherSubsystem));
 	}

src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java → src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java

@@ -5,21 +5,19 @@
 import java.util.function.DoubleSupplier;
 // this command adjusts the angle using a speed that is input by the user
 
-public class SetAngleCommand extends Command {
+public class ManualAngleCommand extends Command {
 	private final LauncherSubsystem launcherSubsystem;
-	private final DoubleSupplier launcherAngleSpeed;
+	private final DoubleSupplier launcherAngle;
 
-	public SetAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) {
+	public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) {
 		this.launcherSubsystem = launcherSubsystem;
-		this.launcherAngleSpeed = angleSpeed;
+		this.launcherAngle = angleSpeed;
 		addRequirements(launcherSubsystem);
 	}
 
 	@Override
 	public void execute() {
-		if (launcherAngleSpeed.getAsDouble() != 0.0) {
-			launcherSubsystem.setAngleSpeed(launcherAngleSpeed.getAsDouble());
-		}
+		launcherSubsystem.setAngleManual(launcherAngle.getAsDouble());
 	}
 
 	@Override

src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java

@@ -22,6 +22,7 @@ public SetAngleAmpLaunchCommand(LauncherSubsystem launcherSubsystem, double spee
 	public void initialize() {
 		launcherSubsystem.setAngle(launcherAngle);
 		launcherSubsystem.ampLaunch(launcherSpeed);
+		launcherSubsystem.manualSetpoint(launcherAngle);
 	}
 
 	@Override

src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java

@@ -0,0 +1,26 @@
+package frc.team2412.robot.commands.launcher;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+
+public class SetPivotCommand extends Command {
+	private final LauncherSubsystem launcherSubsystem;
+	private final double angle;
+
+	public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) {
+		this.launcherSubsystem = launcherSubsystem;
+		this.angle = angle;
+		addRequirements(launcherSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		launcherSubsystem.setAngle(angle);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return MathUtil.isNear(angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE);
+	}
+}

src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java

@@ -166,9 +166,13 @@ private void resetMotors() {
 		configureMotor(intakeMotorRight, true);
 
 		configureMotor(ingestMotor, false);
-		configureMotor(indexMotorUpper, 40, true);
+		configureMotor(indexMotorUpper, 40, false);
 
-		configureMotor(feederMotor, 40, true);
+		configureMotor(feederMotor, 40, false);
+		indexMotorUpper
+				.getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen)
+				.enableLimitSwitch(false);
+		indexMotorUpper.burnFlash();
 	}
 
 	public void intakeSet(double speed) {

src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java

@@ -10,6 +10,7 @@
 import com.revrobotics.SparkAbsoluteEncoder.Type;
 import com.revrobotics.SparkPIDController;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
@@ -36,7 +37,12 @@ public class LauncherSubsystem extends SubsystemBase {
 	public static final int AMP_AIM_ANGLE = 335;
 	public static final int SUBWOOFER_AIM_ANGLE = 298;
 	public static final int PODIUM_AIM_ANGLE = 39;
-	public static final int TRAP_AIM_ANGLE = 290;
+	public static final int TRAP_AIM_ANGLE = 317;
+	public static final double MANUAL_MODIFIER = 0.02;
+	public static final double RETRACTED_ANGLE = 255;
+	// offset for FF so parallel to floor is 0
+	public static final double FF_PIVOT_OFFSET = 250;
+
 	// MOTOR VALUES
 	// max Free Speed: 6784 RPM
 	private static final int MAX_FREE_SPEED_RPM = 6784;
@@ -59,12 +65,12 @@ public class LauncherSubsystem extends SubsystemBase {
 	private final RelativeEncoder launcherBottomEncoder;
 	private final SparkAbsoluteEncoder launcherAngleEncoder;
 	private final SparkPIDController launcherAngleOnePIDController;
-
+	// private final SparkPIDController launcherAngleTwoPIDController;
+	private final ArmFeedforward launcherPivotFF = new ArmFeedforward(0.40434, 0.096771, 0.0056403);
 	// arm FF values:
 	// Ks: 0.40434
 	// Kv: 0.096771
 	// Ka: 0.0056403
-
 	private final SparkPIDController launcherTopPIDController;
 	private final SparkPIDController launcherBottomPIDController;
 	private final SimpleMotorFeedforward launcherTopFeedforward =
@@ -74,6 +80,7 @@ public class LauncherSubsystem extends SubsystemBase {
 
 	private double rpmSetpoint;
 	private double angleSetpoint;
+	private double manualAngleSetpoint;
 
 	private GenericEntry setLauncherSpeedEntry;
 
@@ -89,6 +96,8 @@ public class LauncherSubsystem extends SubsystemBase {
 
 	private GenericEntry launcherIsAtSpeed;
 
+	private GenericEntry launcherAngleManual;
+
 	// Constructors
 	public LauncherSubsystem() {
 
@@ -104,6 +113,7 @@ public LauncherSubsystem() {
 		launcherTopEncoder = launcherTopMotor.getEncoder();
 		launcherBottomEncoder = launcherBottomMotor.getEncoder();
 		launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle);
+		manualAngleSetpoint = launcherAngleEncoder.getPosition();
 
 		// PID controllers
 		// Create launcherTopPIDController and launcherTopMotor]
@@ -140,8 +150,8 @@ public void configMotors() {
 		launcherAngleOneMotor.setSmartCurrentLimit(60);
 		launcherAngleTwoMotor.setSmartCurrentLimit(60);
 
-		launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.95f);
-		launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.71f);
+		launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.96f);
+		launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.80f);
 		launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true);
 		launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true);
 
@@ -152,7 +162,6 @@ public void configMotors() {
 		launcherAngleOnePIDController.setI(0);
 		launcherAngleOnePIDController.setD(0.066248);
 		launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED);
-
 		launcherTopPIDController.setP(7.7633E-05);
 		launcherTopPIDController.setI(0);
 		launcherTopPIDController.setD(0);
@@ -209,7 +218,11 @@ public double getAngle() {
 	public void setAngle(double launcherAngle) {
 		angleSetpoint = launcherAngle;
 		launcherAngleOnePIDController.setReference(
-				Units.degreesToRotations(angleSetpoint), ControlType.kPosition);
+				Units.degreesToRotations(angleSetpoint),
+				ControlType.kPosition,
+				0,
+				launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0));
+		manualAngleSetpoint = launcherAngle;
 		// launcherAngleTwoPIDController.setReference(
 		//		Units.degreesToRotations(angleSetpoint), ControlType.kPosition);
 	}
@@ -234,10 +247,25 @@ public double getAngleSpeed() {
 		return launcherAngleEncoder.getVelocity();
 	}
 
-	public void setAngleSpeed(double Speed) {
-		// launcherAngleOnePIDController.setReference(Speed, ControlType.kVelocity);
-		// launcherAngleTwoPIDController.setReference(Speed, ControlType.kVelocity);
-		launcherAngleOneMotor.set(Speed);
+	public void setAngleManual(double joystickInput) {
+		manualAngleSetpoint =
+				MathUtil.clamp(manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, 0.80f, 0.96f);
+
+		if (Units.degreesToRotations(getAngle()) > 0.80
+				&& Units.degreesToRotations(getAngle()) < 0.96) {
+			launcherAngleOnePIDController.setReference(
+					manualAngleSetpoint,
+					ControlType.kPosition,
+					0,
+					launcherPivotFF.calculate(
+							Units.degreesToRadians(
+									Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET),
+							0));
+		}
+	}
+
+	public void manualSetpoint(double setpoint) {
+		manualAngleSetpoint = setpoint;
 	}
 
 	private void initShuffleboard() {
@@ -292,6 +320,12 @@ private void initShuffleboard() {
 						.withProperties(Map.of("Min", -MAX_FREE_SPEED_RPM, "Max", MAX_FREE_SPEED_RPM))
 						.withPosition(5, 0)
 						.getEntry();
+		launcherAngleManual =
+				Shuffleboard.getTab("Launcher")
+						.add("Launcher manual increase", 0)
+						.withSize(1, 1)
+						.withWidget(BuiltInWidgets.kTextView)
+						.getEntry();
 		Shuffleboard.getTab("Launcher")
 				.add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID"))
 				.withPosition(2, 0);
@@ -313,6 +347,7 @@ public void periodic() {
 		launcherTopFlywheelTemp.setDouble(launcherTopMotor.getMotorTemperature());
 		launcherBottomFlyWheelTemp.setDouble(launcherTopMotor.getMotorTemperature());
 		launcherIsAtSpeed.setBoolean(isAtSpeed());
+		launcherAngleManual.setDouble(manualAngleSetpoint);
 	}
 
 	private SysIdRoutine getArmSysIdRoutine() {

src/main/java/frc/team2412/robot/util/TrapAlign.java

@@ -21,29 +21,35 @@ public class TrapAlign {
 		// trap that faces source
 		new Pose2d(new Translation2d(4.3, 3.09), Rotation2d.fromDegrees(-300)),
 		// trap that faces mid
-		new Pose2d(new Translation2d(6, 4), Rotation2d.fromDegrees(180))
+		// DO THIS ONE FIRST
+		// brute force the X lol
+		new Pose2d(new Translation2d(5.8, 4.10), Rotation2d.fromDegrees(180))
 	};
 
 	private static final Pose2d[] RED_TRAP_POSES = {
 		// trap that faces amp
-		new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-60)),
+		new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-120)),
 		// trap that faces source
-		new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(120)),
+		new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(300)),
 		// trap that faces mid
-		new Pose2d(new Translation2d(10, 4), Rotation2d.fromDegrees(0))
+		// DO THIS ONE FIRST
+		// brute force the X lol
+		new Pose2d(new Translation2d(10.8, 4.10), Rotation2d.fromDegrees(0))
 	};
 
 	private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) {
 		Pose2d robotPose = drivebaseSubsystem.getPose();
-		Pose2d trapPose =
-				robotPose.nearest(
-						List.of(
-								(DriverStation.getAlliance().get().equals(Alliance.Blue))
-										? BLUE_TRAP_POSES
-										: RED_TRAP_POSES));
-
+		boolean isBlue;
+		if (!DriverStation.getAlliance().isEmpty()) {
+			isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue);
+		} else {
+			isBlue = false;
+		}
+		// figures out which trap to go to
+		Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES));
+		// sets the point for the path to go to
 		List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, trapPose);
-
+		// this is flipped
 		PathPlannerPath path =
 				new PathPlannerPath(
 						bezierPoints,
@@ -53,6 +59,11 @@ private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) {
 								DrivebaseSubsystem.MAX_ANGULAR_VELOCITY,
 								DrivebaseSubsystem.MAX_ANGULAR_ACCELERAITON),
 						new GoalEndState(0.0, trapPose.getRotation()));
+		// path.flipPath(); Returns path except it's flipped
+		// this unflips it
+		if (!isBlue) {
+			path = path.flipPath();
+		}
 		return AutoBuilder.followPath(path);
 	}
 
@@ -79,6 +90,7 @@ public void initialize() {
 			trapCommand.initialize();
 			launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE);
 			launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM);
+			launcherSubsystem.manualSetpoint(LauncherSubsystem.TRAP_AIM_ANGLE);
 		}
 
 		@Override