Update AlgaeCoralerConstants and adjust Drive subsystem scaling factors

Author: Tortuga-AM

src/main/java/frc/robot/Manager/Manager.java

@@ -35,26 +35,13 @@ public Manager() {
 		// Scoring/intaking Coral
 		addTrigger(IDLE, CORAL_OUT, () -> DRIVER_CONTROLLER.getYButtonPressed());
 
-		// Auto stop scoring corral:
-
-		// addTrigger(CORAL_OUT, CORAL_BLOCK, DRIVER_CONTROLLER::getYButtonPressed);
-		// addTrigger(CORAL_BLOCK, IDLE, DRIVER_CONTROLLER::getYButtonPressed);
-
 		// Scoring/intaking Algae
 		addTrigger(IDLE, ALGAE_IN, DRIVER_CONTROLLER::getBButtonPressed);
 		addTrigger(ALGAE_IN, HOLDING, DRIVER_CONTROLLER::getBButtonPressed);
 		addTrigger(HOLDING, ALGAE_OUT, DRIVER_CONTROLLER::getBButtonPressed);
 		addTrigger(ALGAE_IN, IDLE, DRIVER_CONTROLLER::getXButtonPressed);
 		//Auto hold algae
 
-		//Zero Motors auto and manually
-		// addRunnableTrigger(algaeCoraler::zero, () -> getState() == ManagerStates.IDLE && !algaeCoraler.motorZeroed());
-		// addRunnableTrigger(algaeCoraler::resetMotorsZeroed, DRIVER_CONTROLLER::getAButtonPressed);
-
-		// Back to IDLE button is handled by if statement in run  vstate.
-
-		//Auto Stuff
-
 	}
 
 	@Override

src/main/java/frc/robot/Robot.java

@@ -9,10 +9,8 @@
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.AutoManager.AutoManager;
 import frc.robot.Manager.Manager;
 import frc.robot.Subsystems.Drive.Drive;
-import frc.robot.Subsystems.Vision.Vision;
 import frc.robot.Utilitys.Utilitys;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -23,8 +21,8 @@
 public class Robot extends LoggedRobot {
 
 	private Manager manager;
-	private AutoManager autoManager;
-	private Vision vision;
+	//private AutoManager autoManager;
+	//private Vision vision;
 
 	public Robot() {}
 
@@ -46,8 +44,8 @@ public void robotInit() {
 		}
 
 		manager = Manager.getInstance();
-		autoManager = new AutoManager();
-		vision = Vision.getInstance();
+		//autoManager = new AutoManager();
+		//vision = Vision.getInstance();
 
 		Logger.start();
 		PathfindingCommand.warmupCommand().schedule();
@@ -60,15 +58,15 @@ public void robotInit() {
 	@Override
 	public void robotPeriodic() {
 		manager.periodic();
-		vision.periodic();
+		//vision.periodic();
 		CommandScheduler.getInstance().run();
 		Utilitys.controllers.clearCache();
 	}
 
 	@Override
 	public void autonomousInit() {
 		Drive.getInstance().zeroGyro();
-		CommandScheduler.getInstance().schedule(autoManager.getSelectedCommand());
+		//CommandScheduler.getInstance().schedule(autoManager.getSelectedCommand());
 	}
 
 	@Override
@@ -90,7 +88,7 @@ public void disabledInit() {}
 
 	@Override
 	public void disabledPeriodic() {
-		vision.periodic();
+		//vision.periodic();
 	}
 
 	@Override

src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java

@@ -31,7 +31,7 @@ public final class AlgaeCoralerConstants {
 	//Speeds
 	public static final double ALGAE_IN_SPEED = -1;
 	public static final double ALGAE_OUT_SPEED = 1;
-	public static final double CORAL_OUT_SPEED = -0.25;
+	public static final double CORAL_OUT_SPEED = -0.4;
 	public static final double HOLDING_SPEED = 0;
 	public static final double AUTO_SPEED = -0.43;
 

src/main/java/frc/robot/Subsystems/Drive/Drive.java

@@ -2,25 +2,17 @@
 
 import static frc.robot.GlobalConstants.Controllers.DRIVER_CONTROLLER;
 import static frc.robot.Subsystems.Drive.DriveConstants.*;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import com.pathplanner.lib.pathfinding.Pathfinding;
-import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.GlobalConstants;
 import frc.robot.GlobalConstants.Controllers;
 import frc.robot.GlobalConstants.RobotMode;
 import java.io.File;
-import org.littletonrobotics.junction.Logger;
 import org.team7525.subsystem.Subsystem;
 import swervelib.SwerveDrive;
 import swervelib.SwerveInputStream;
@@ -34,9 +26,6 @@ public class Drive extends Subsystem<DriveStates> {
 	private SwerveInputStream swerveInputs;
 	private SwerveDrive swerveDrive;
 	private boolean fieldRelative;
-
-	// Field/Auto Variables:
-	private Field2d field;
 	private boolean slow;
 	// INSTANCE:
 	private static Drive instance;
@@ -50,26 +39,8 @@ public static Drive getInstance() {
 
 	public Drive() {
 		super("Drive", DriveStates.MANUAL);
-		// Create field:
-		field = new Field2d();
-
-		//Logging for pathplanner
-		PathPlannerLogging.setLogActivePathCallback(poses -> {
-			field.getObject("PATH").setPoses(poses);
-			Logger.recordOutput("Auto/poses", poses.toArray(new Pose2d[poses.size()]));
-		});
-		PathPlannerLogging.setLogActivePathCallback(activePath -> {
-			field.getObject("TRAJECTORY").setPoses(activePath);
-			Logger.recordOutput(
-				"Auto/Trajectory",
-				activePath.toArray(new Pose2d[activePath.size()])
-			);
-		});
-		Pathfinding.ensureInitialized();
-
 		slow = false;
 		fieldRelative = true;
-
 		// Setup SwerveDrive:
 		SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
 		try {
@@ -90,30 +61,7 @@ public Drive() {
 			() -> -1 * (Controllers.DRIVER_CONTROLLER.getLeftX())
 		)
 			.withControllerRotationAxis(() -> -1 * Controllers.DRIVER_CONTROLLER.getRightX())
-			.allianceRelativeControl(true)
-			.driveToPoseEnabled(false);
-
-		AutoBuilder.configure(
-			swerveDrive::getPose, // Robot pose supplier
-			swerveDrive::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
-			swerveDrive::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-			swerveDrive::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
-			new PPHolonomicDriveController(
-				// PPHolonomicController is the built in path following controller for holonomic drive trains
-				PPH_TRANSLATION_PID, // Translation PID constants
-				PPH_ROTATION_PID // Rotation PID constants
-			),
-			DriveConstants.getRobotConfig(), // The robot configuration
-			() -> {
-				var alliance = DriverStation.getAlliance();
-				if (alliance.isPresent()) {
-					return alliance.get() == DriverStation.Alliance.Red;
-				}
-				return false;
-			},
-			this // Reference to this subsystem to set requirements
-		);
-
+			.allianceRelativeControl(true);
 		establishTriggers();
 	}
 
@@ -140,14 +88,14 @@ public void runState() {
 			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
 				slow = false;
 			}
-			swerveInputs.scaleTranslation(0.33);
-			swerveInputs.scaleRotation(0.33);
+			swerveInputs.scaleTranslation(0.1);
+			swerveInputs.scaleRotation(0.1);
 		} else {
 			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
 				slow = true;
 			}
-			swerveInputs.scaleTranslation(1);
-			swerveInputs.scaleRotation(1);
+			swerveInputs.scaleTranslation(0.33);
+			swerveInputs.scaleRotation(0.33);
 		}
 
 		if (fieldRelative) {
@@ -164,7 +112,6 @@ public void runState() {
 
 		SmartDashboard.putBoolean("SLOW MODE", slow);
 		SmartDashboard.putBoolean("FIELD RELATIVE", fieldRelative);
-		SmartDashboard.putData(field);
 	}
 
 	// Vision Code Getters:
@@ -194,12 +141,4 @@ public void addVisionMeasurement(
 		}
 		swerveDrive.updateOdometry();
 	}
-	// // Bum AUTO Stuff:
-	// public void driveForward() {
-	// 	swerveDrive.drive(DRIVE_FORWARD_CHASSIS_SPEED);
-	// }
-
-	// public void sidewaysToRightFace() {
-	// 	swerveDrive.drive(SIDEWAYS_TO_RIGHT_CHASSIS_SPEED);
-	// }
 }