intake

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index 60dcbfe6f274ec3266a7a9f26fc5a6f3e425da3c..ff801b941fdaf50ab2269454873502d10b272a91 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -22,12 +22,10 @@ import frc.robot.subsystems.turret.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.util.FieldZone;
 import frc.robot.util.ShootingTarget;
-import frc.robot.util.Vision.TurretVision;
 
 public class SimpleAutoShoot extends Command {
     private Turret turret;
     private Drivetrain drivetrain;
-    private TurretVision turretVision;
     private Shooter shooter;
 
     private double fieldAngleRad;

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e0631aa4444c44b3d0494661a24f6956f155ae81..2f47c82763f835a971fcb30bd710b14520aadfe6 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -18,8 +18,7 @@ import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.Intake.Intake;
 import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
@@ -75,13 +74,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         if(intake != null){
             driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
                 if(intakeBoolean){
-                    intake.setSetpoint(IntakeConstants.INTAKE_ANGLE);
-                    intake.setFlyWheel();
+                    intake.extend();
+                    intake.spinStart();
                     intakeBoolean = false;
                 }
                 else{
-                    intake.setSetpoint(IntakeConstants.STOW_ANGLE);
-                    intake.stopFlyWheel();
+                    intake.retract();
+                    intake.spinStop();
                 }
             }));
         }

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 1db640e6524725e87b81db2f0bfddbd1729efccc..7e374bd6e1c6ddc99cca04fb27349fe461de8f0c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
-import frc.robot.util.ChineseRemainderTheorem;
 
 public class Turret extends SubsystemBase implements TurretIO{
 
@@ -108,24 +107,24 @@ public class Turret extends SubsystemBase implements TurretIO{
 
                //TODO: replace this stuff with Chinese Remainder Theorem calculator -- ignore this for now
 
-               double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
-               double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
+               // double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
+               // double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
 
-               int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)
-                               % TurretConstants.LEFT_ENCODER_TEETH;
+               // int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)
+               //              % TurretConstants.LEFT_ENCODER_TEETH;
 
-               int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
-                               % TurretConstants.RIGHT_ENCODER_TEETH;
+               // int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
+               //              % TurretConstants.RIGHT_ENCODER_TEETH;
 
-               int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
+               // int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
 
-               double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
-        * TurretConstants.RIGHT_ENCODER_TEETH;
+               // double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
+        // * TurretConstants.RIGHT_ENCODER_TEETH;
 
-               double turretRotations = turretIndex / (double) totalTeeth;
+               // double turretRotations = turretIndex / (double) totalTeeth;
 
-               double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
-               motor.setPosition(motorRotations);
+               // double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
+               // motor.setPosition(motorRotations);
 
                motor.setPosition(0.0); //TODO: remove after chinese remainder theorem works