testing changes

grt192 · Feb 1, 2024 · 228908c · 228908c
1 parent c65fa48
commit 228908c
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 30 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -22,7 +22,7 @@ public static class ElevatorConstants {
 
     public static final float EXTENSION_LIMIT_METERS = 0;
 
-    public static final int ZERO_LIMIT_ID = 0;
+    public static final int ZERO_LIMIT_ID = 2;
 
     public static final int GROUND_POSITION = 0;
     public static final int SPEAKER_POSITION = 0;
@@ -78,8 +78,8 @@ public static class IntakeConstants {
     public static final int PIVOT_MOTOR_ID = 16;
 
     public static final int sensorID = 0;
-    public static final int extendedlimitswitchID = 1;
-    public static final int retractedlimitswitchID = 2;
+    public static final int extendedlimitswitchID = 5;
+    public static final int retractedlimitswitchID = 6;
     //public static final int intakeencoderID = 3;
 
     public static final double rollersclockwise = 1;
@@ -94,11 +94,11 @@ public static class IntakeConstants {
 
 
   public static class ClimbConstants {
-    public static final int LEFT_WINCH_MOTOR_ID = 21;
-    public static final int LEFT_ZERO_LIMIT_ID = 11;
+    public static final int LEFT_WINCH_MOTOR_ID = 10;
+    public static final int LEFT_ZERO_LIMIT_ID = 0;
 
-    public static final int RIGHT_WINCH_MOTOR_ID = 22;
-    public static final int RIGHT_ZERO_LIMIT_ID = 12;
+    public static final int RIGHT_WINCH_MOTOR_ID = 11;
+    public static final int RIGHT_ZERO_LIMIT_ID = 1;
 
     public static final double WINCH_REDUCTION = 9.49;
     public static final double AXLE_PERIMETER_METERS = 6 * Units.inchesToMeters(.289) ;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -70,7 +70,7 @@ public RobotContainer() {
         //construct Test
         // module = new SwerveModule(6, 7, 0);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
-      baseSwerveSubsystem = new SwerveSubsystem();
+      baseSwerveSubsystem = null;// new SwerveSubsystem();
       intakePivotSubsystem = new IntakePivotSubsystem();
       shooterFeederSubsystem = new ShooterFeederSubsystem();
 
@@ -88,21 +88,11 @@ public RobotContainer() {
   }
 
 
-    private void configureBindings() {
+  private void configureBindings() {
 
 
-      elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> {
-        if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){
-          elevatorSubsystem.setManual();
-          elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis());
-        }
-        else{
-          elevatorSubsystem.setAuto();
-        }
-      }));
-
       bButton.onTrue(new InstantCommand(() -> {
-        shooterFeederSubsystem.setFeederMotorSpeed(.7);
+        shooterFeederSubsystem.setFeederMotorSpeed(.4);
       }));
 
       bButton.onFalse(new InstantCommand(() -> {
@@ -132,13 +122,15 @@ private void configureBindings() {
 
       intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {
         if(mechController.getPOV() == 90){
-          intakeRollerSubsystem.setAllRollSpeed(.3, .3);
+          intakeRollerSubsystem.setAllRollSpeed(1, .8);
         } else if (mechController.getPOV() == 270){
-          intakeRollerSubsystem.setAllRollSpeed(-.3, -.3);
+          intakeRollerSubsystem.setAllRollSpeed(-.8, -.5);
         } else {
           intakeRollerSubsystem.setAllRollSpeed(0, 0);
         }
-      }));
+      }, intakeRollerSubsystem));
+
+
 
 
 
@@ -189,7 +181,6 @@ private void configureBindings() {
         }));
 
       }
-
 
     }
 

diff --git a/src/main/java/frc/robot/commands/ShootModeCommand.java b/src/main/java/frc/robot/commands/ShootModeCommand.java
@@ -0,0 +1,20 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.intake.pivot.IntakePivotVerticalCommand;
+import frc.robot.commands.shooter.feed.ShooterFeedShootCommand;
+import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand;
+import frc.robot.subsystems.intake.IntakePivotSubsystem;
+import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
+import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
+
+public class ShootModeCommand extends SequentialCommandGroup {
+    IntakePivotSubsystem intakePivotSubsystem;
+    ShooterFlywheelSubsystem shooterFlywheelSubsystem;
+    ShooterFeederSubsystem shooterFeederSubsystem;
+
+    public ShootModeCommand(){
+        addCommands(new IntakePivotVerticalCommand(intakePivotSubsystem).alongWith(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem)), 
+                    new ShooterFeedShootCommand(shooterFeederSubsystem));
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java
@@ -34,7 +34,7 @@ public ClimbArm(int WINCH_MOTOR_ID, int ZERO_LIMIT_ID) {
             sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true);
         });
 
-        zeroLimitSwitch = new DigitalInput(LEFT_ZERO_LIMIT_ID);
+        zeroLimitSwitch = new DigitalInput(ZERO_LIMIT_ID);
     }
 
     public void update() {

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java
@@ -24,6 +24,7 @@ public class IntakeRollersSubsystem extends SubsystemBase {
   public IntakeRollersSubsystem() {
     integrationMotor = new TalonSRX(INTEGRATION_MOTOR_ID);
     frontMotor = new TalonSRX(FRONT_MOTOR_ID);
+    frontMotor.setInverted(true);
     backMotor = new TalonSRX(BACK_MOTOR_ID);
     sensor = new AnalogPotentiometer(sensorID);
   }
@@ -44,8 +45,8 @@ public void setRollSpeed(double top, double bottom){
 
   public void setAllRollSpeed(double topone, double bottomone){
     frontMotor.set(TalonSRXControlMode.PercentOutput, topone);
-    integrationMotor.set(TalonSRXControlMode.PercentOutput, topone);
-    backMotor.set(TalonSRXControlMode.PercentOutput, bottomone);
+    integrationMotor.set(TalonSRXControlMode.PercentOutput, bottomone);
+    backMotor.set(TalonSRXControlMode.PercentOutput, topone);
   }
 
   public void setRollersOutwards(Boolean pressedA){

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java
@@ -22,7 +22,9 @@ public class ShooterFeederSubsystem extends SubsystemBase{
 
     public ShooterFeederSubsystem(){
         //motors
-        feederMotor = new TalonSRX(10);
+        feederMotor = new TalonSRX(15);
+
+        feederMotor.setInverted(true);
 
         //sensors
         shooterSensor = new ColorSensorV3(I2C.Port.kMXP);

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -16,7 +16,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     public final double PIVOT_SPEED = 0.1;
     final double GEARBOX_RATIO = 18.16; //ask cadders
     public final int ERRORTOLERANCE = 5; //error tolerance for pid
-    final int LIMIT_SWITCH_ID = 1; //placeholder
+    final int LIMIT_SWITCH_ID = 3; //placeholder
     final double CONVERSION_FACTOR = Math.PI/(2*4.57);
 
     //motors
@@ -142,7 +142,7 @@ public void periodic(){
             setAngle(getAutoAimAngle(getDistance()));
         }
 
-        printCurrentAngle();
+        // printCurrentAngle();
 
         // System.out.println("current pos" + rotationEncoder.getPosition());