florida comp

src/main/java/frc4388/robot/subsystems/Intake.java

@@ -145,6 +145,10 @@ public class Intake extends SubsystemBase {
     talonIntake.set(IntakeConstants.INTAKE_SPEED);
   }
 
+  public void talonSpinIntakeMotor(double speed) {
+    talonIntake.set(speed);
+  }
+
   public boolean getTalonIntakeLimitSwitchState() {
     if(r = talonIntake.getForwardLimit().getValue().value == 0) {
       return true;
@@ -174,6 +178,15 @@ public class Intake extends SubsystemBase {
     }
   }
 
+  public void ampPosition() {
+    PositionVoltage request = new PositionVoltage(-0);
+    talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
+  } 
+
+  public void ampShoot(double speed) {
+    talonSpinIntakeMotor(speed);
+  }
+
 
   // ! NEO Methods
   //hanoff

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -33,7 +33,8 @@ public class  SwerveDrive extends SubsystemBase {
 
   private RobotGyro gyro;
 
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
+  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   
   public double rotTarget = 0.0;
   public Rotation2d orientRotTarget = new Rotation2d();
@@ -87,6 +88,41 @@ public class  SwerveDrive extends SubsystemBase {
     setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
+  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+
+      double rot = 0;
+      
+      // ! drift correction
+      if (rightStick.getNorm() > 0.05) {
+        rotTarget = gyro.getAngle();
+        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
+      //  SmartDashboard.putBoolean("drift correction", false);
+        stopped = false;
+      } else if(leftStick.getNorm() > 0.05) {
+        if (!stopped) {
+          stopModules();
+          stopped = true;
+        }
+
+     //   SmartDashboard.putBoolean("drift correction", true);
+        
+        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
+      }
+
+      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
+      Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+
+      // Convert field-relative speeds to robot-relative speeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+    } else {      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
   public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
 
     Translation2d rightStick = new Translation2d(rightX, rightY);
@@ -157,6 +193,11 @@ public class  SwerveDrive extends SubsystemBase {
     return this.kinematics;
   }
 
+  public boolean getSpeedState() {
+    
+    return false;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run\
@@ -170,6 +211,7 @@ public class  SwerveDrive extends SubsystemBase {
       this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
     } else {
       this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+
     }
   }
 
@@ -220,4 +262,6 @@ public class  SwerveDrive extends SubsystemBase {
     }
   }
 
+  
+
 }