dis 4

Author: Spikes-2212

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

@@ -75,9 +75,9 @@ public void autonomousInit() {
         drivetrain.resetGyro();
         drivetrain.resetRelativeEncoders();
 //        JustDrive auto = new JustDrive(drivetrain);
-//        DriveAndPlaceL1 auto = new DriveAndPlaceL1(drivetrain, coralJoint, storage);
+        DriveAndPlaceL1 auto = new DriveAndPlaceL1(drivetrain, coralJoint, storage);
 //        IBelieve auto = new IBelieve(drivetrain, coralJoint, storage);
-        DriveWithAngle auto = new DriveWithAngle(drivetrain, coralJoint, storage);
+//        DriveWithAngle auto = new DriveWithAngle(drivetrain, coralJoint, storage);
         auto.schedule();
     }
 

src/main/java/frc/robot/commands/RotateStorage.java

@@ -62,7 +62,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        coralJoint.setDefaultCommand(new RotateStorage(coralJoint, coralJoint::getPosition));
+        coralJoint.setDefaultCommand(new RotateStorage(coralJoint, setpoint));
     }
 }
 

src/main/java/frc/robot/commands/autonomous/DriveAndPlaceL1.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.autonomous;
 
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.Drive;
@@ -19,9 +20,10 @@ public class DriveAndPlaceL1 extends SequentialCommandGroup {
 
     public DriveAndPlaceL1(Drivetrain drivetrain, CoralJoint coralJoint, Storage storage) {
         addCommands(
+                new MoveGenericSubsystem(coralJoint, -0.15).withTimeout(1),
                 new DriveAtAngle(drivetrain, DRIVE_SPEED, () -> 0.0, () -> 0.0,
                         false, false).withTimeout(DRIVE_TIMEOUT),
-                new DriveAtAngle(drivetrain, () -> -0.2, () -> 0.0, () -> 0.0, false, false).withTimeout(0.5),
+                new DriveAtAngle(drivetrain, () -> -0.2, () -> 0.0, () -> 0.0, false, false).withTimeout(0.75),
                 new RotateStorage(coralJoint, CoralJoint.StoragePose.L1),
                 new ReleaseCoral(storage),
                 new InstantCommand(() -> {

src/main/java/frc/robot/commands/autonomous/DriveWithAngle.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.autonomous;
 
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
 import com.spikes2212.dashboard.SpikesLogger;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -20,13 +21,14 @@ public class DriveWithAngle extends SequentialCommandGroup {
     private static final Supplier<Double> DRIVE_SPEED_WITH_ANGLE_BACKWARD = () -> -0.2;
     private static final Supplier<Double> DRIVE_SPEED_FORWARD = () -> 0.7;
     private static final double DRIVE_TIMEOUT_WITH_ANGLE_FORWARD = 3;
-    private static final double DRIVE_TIMEOUT_WITH_ANGLE_BACKWARD = 0.5;
+    private static final double DRIVE_TIMEOUT_WITH_ANGLE_BACKWARD = 0.75;
     private static final double DRIVE_TIMEOUT_FORWARD = 2;
 
     private final SpikesLogger logger = new SpikesLogger();
 
     public DriveWithAngle(Drivetrain drivetrain, CoralJoint coralJoint, Storage storage) {
         addCommands(
+                new MoveGenericSubsystem(coralJoint, -0.15).withTimeout(1),
                 new DriveAtAngle(drivetrain, DRIVE_SPEED_WITH_ANGLE_FORWARD, () -> 0.0, () -> 0.0, false,
                         false).withTimeout(DRIVE_TIMEOUT_WITH_ANGLE_FORWARD),
                 new Drive(drivetrain, DRIVE_SPEED_FORWARD, () -> 0.0, () -> 0.0, false,

src/main/java/frc/robot/subsystems/CoralJoint.java

@@ -18,7 +18,7 @@ public class CoralJoint extends SmartMotorControllerGenericSubsystem {
 
     public enum StoragePose {
 
-        INTAKE(12), L1(-10), L2(-30),
+        INTAKE(12), L1(-11.5), L2(-30),
         L3(-15),
         RESTING(-30);
 //        RESTING(0.0);

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -31,7 +31,7 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double DEGREES_TO_FLIP = 180;
     private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
-    private static final double DRIVE_CURRENT_LIMIT = 40;
+    private static final double DRIVE_CURRENT_LIMIT = 70;
     private static final double TURN_CURRENT_LIMIT = 40;
 
     private final TalonFXWrapper driveMotor;
@@ -80,7 +80,7 @@ public void configureDriveController() {
         driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
                 * INCHES_TO_METERS * Math.PI);
         driveMotor.setInverted(driveInverted);
-//                driveMotor.getConfigurator().apply(new CurrentLimitsConfigs().withSupplyCurrentLimit(DRIVE_CURRENT_LIMIT));
+                driveMotor.getConfigurator().apply(new CurrentLimitsConfigs().withSupplyCurrentLimit(DRIVE_CURRENT_LIMIT));
     }
 
     public void configureTurnController() {
@@ -188,5 +188,7 @@ public void end(boolean fuckYou) {
                 driveMotor.stopMotor();
             }
         });
+        namespace.putNumber("drive current", driveMotor::getCurrent);
+        namespace.putNumber("turn current", turnMotor::getCurrent);
     }
 }