final commit from sfr

Team100 · Mar 20, 2022 · f3989ed · f3989ed
1 parent 029eac2
commit f3989ed
Show file tree

Hide file tree

Showing 4 changed files with 86 additions and 16 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -128,7 +128,7 @@ public static final class IntakeMotionParameters {
 
             public static final int ACCEPTABLE_ERROR_TICKS = 10;
 
-            public static final double KP = 2;//1.08
+            public static final double KP = 2.5;//1.08
             public static final double KI = 0.250;
             public static final double KD = 0.032;
             public static final double KF = 0;
@@ -160,7 +160,7 @@ public static final class IntakeSpin {
             public static final class IntakePivot {
                 public static final int CAN_ID = 2; //2
 
-                public static final InvertType INVERT = InvertType.InvertMotorOutput;
+                public static final InvertType INVERT = InvertType.None;
                 public static final int FEEDBACK_PORT = 0;
                 public static final boolean SENSOR_PHASE = false;
 
@@ -221,7 +221,7 @@ public static final class Shooter {
                 public static final IdleMode NEUTRAL_MODE = IdleMode.kCoast;
             }
             public static final class ShooterFollower {
-                public static final int CAN_ID = 6;
+                public static final int CAN_ID = 7;
 
                 public static final boolean INVERT = true;
                 public static final int FEEDBACK_PORT = 0;
@@ -246,7 +246,7 @@ public static final class FrontSensor {
             }
 
             public static final class RearSensor {
-                public static final int ID = 9;
+                public static final int ID = 8;
             }
         }
 
@@ -299,12 +299,13 @@ public static final class IndexerStageTwo {
 
     public static final class ClimberConstants {
         public static final class ClimberMotionParameters {
-            public static final double CLIMBER_PERCENT_OUTPUT = 0.5;
+            public static final double CLIMBER_PERCENT_OUTPUT = 0.6;
             public static final double CLIMBER_ZERO = 0;
 
             public static final double TILT_PERCENT_OUTPUT = 0.1;
 
             public static final int CLIMBER_TOP = -220000;
+            public static final int CLIMBER_LOW_BAR_TOP = -110000;
             public static final int CLIMBER_BOTTOM = -10000;
             public static final double TILT_START = 7000;
             public static final double STATIONARY_LOCK_ANGLE = 10;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,7 +8,6 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -68,8 +67,10 @@ public class RobotContainer {
 
     private final JoystickButton mediaControlButton = new JoystickButton(buttonBoard, 12);
 
-    private final JoystickButton HookDownButton = new JoystickButton(buttonBoard, 1);
-    private final JoystickButton HookUpButton = new JoystickButton(buttonBoard, 16);
+    private final JoystickButton hookDownButton = new JoystickButton(buttonBoard, 1);
+    private final JoystickButton hookUpButton = new JoystickButton(buttonBoard, 16);
+    private final JoystickButton hookUpLowButton = new JoystickButton(rightJoystick, 11);
+    private final JoystickButton hookDownLowButton = new JoystickButton(rightJoystick, 10);
 
     private final JoystickButton stopAll = new JoystickButton(buttonBoard, 4);
 
@@ -97,9 +98,9 @@ public class RobotContainer {
     private final IndexerFeedHigh feedHighCommand = new IndexerFeedHigh(indexer, shooter);
     private final IndexerFeedLow feedLowCommand = new IndexerFeedLow(indexer, shooter);
     private final ClimberStop climberStopCommand = new ClimberStop(climber);
-    private final HookUp HookUpCommand = new HookUp(climber);
-    private final HookDown HookDownCommand = new HookDown(climber, symphony);    
-    private final HookZero HookZeroCommand = new HookZero(climber);
+    private final HookUp hookUpCommand = new HookUp(climber);
+    private final HookDown hookDownCommand = new HookDown(climber, symphony);    
+    private final HookZero hookZeroCommand = new HookZero(climber);
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
@@ -126,8 +127,10 @@ public RobotContainer() {
     private void configureButtonBindings() {
         turboButton.whileHeld(driveFuriousCommand);
         alignButton.whileHeld(alignCommand);
-        HookDownButton.whenPressed(HookDownCommand);
-        HookUpButton.whenPressed(HookUpCommand);
+        hookDownButton.whenPressed(hookDownCommand);
+        hookUpButton.whenPressed(hookUpCommand);
+        hookUpLowButton.whenPressed(new HookUpLow(climber));
+        hookDownLowButton.whenPressed(hookDownCommand);
         slowButton.whileHeld(driveSlowCommand);
         stopAll.whenPressed(new ParallelCommandGroup(new ClimberStop(climber), new IndexerStop(indexer), new BetterIntakeStop(intake), new ShootStop(shooter)));
         intakeButton.whenPressed(new SequentialCommandGroup(new ParallelDeadlineGroup(intakeCommand, intakeIntakeCommand), new WaitCommand(0.2)));
@@ -138,7 +141,7 @@ private void configureButtonBindings() {
 
         mediaControlButton.whenPressed(new InstantCommand(() -> { symphony.play();}, symphony));
 
-        fixClimberButton.whileHeld(HookZeroCommand);
+        fixClimberButton.whileHeld(hookZeroCommand);
         // runPivot.whileHeld(new InstantCommand(() -> { intake.runPivot(0.1); System.out.println("running the thing");}, intake));
     }
 

diff --git a/src/main/java/frc/robot/commands/climber/HookUpLow.java b/src/main/java/frc/robot/commands/climber/HookUpLow.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Climber;
+import frc.robot.Constants.ClimberConstants;
+
+public class HookUpLow extends CommandBase {
+  private final Climber climber;
+  boolean done, first;
+  /** Creates a new HooksUp. */
+  public HookUpLow(Climber climber) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.climber = climber;
+    addRequirements(this.climber);
+
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() { 
+    done=false;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(climber.mainPosition()<=ClimberConstants.ClimberMotionParameters.CLIMBER_LOW_BAR_TOP){
+      // SmartDashboard.putBoolean("we done boys?", true);
+      climber.setWinch(0);
+      done = true;
+    }else{
+      climber.setWinch(-ClimberConstants.ClimberMotionParameters.CLIMBER_PERCENT_OUTPUT);
+      // SmartDashboard.putBoolean("we done boys?", false);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    climber.setWinch(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return done;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/drivetrain/DriveSlow.java b/src/main/java/frc/robot/commands/drivetrain/DriveSlow.java
@@ -7,15 +7,19 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Intake;
 import edu.wpi.first.wpilibj.Joystick;
 
 public class DriveSlow extends CommandBase {
     private final Drivetrain drivetrain;
+    // private final Intake intake;
+    private boolean enabled;
     private final Joystick leftJoystick;
     private final Joystick rightJoystick;
 
     public DriveSlow(Drivetrain dt, Joystick l, Joystick r) {
         drivetrain = dt;
+        // this.intake = intake;
         leftJoystick = l;
         rightJoystick = r;
 
@@ -25,7 +29,11 @@ public DriveSlow(Drivetrain dt, Joystick l, Joystick r) {
 
     // Called when the command is initially scheduled.
     @Override
-    public void initialize() {}
+    public void initialize() {
+        // enabled = intake.isEnabled();
+        // intake.disable();
+        // intake.runPivot(-0.2);
+    }
 
     // Called every time the scheduler runs while the command is scheduled.
     @Override
@@ -34,11 +42,16 @@ public void execute() {
         double right = -leftJoystick.getY() + rightJoystick.getX();
         double limiter = Constants.DrivetrainConstants.DrivetrainMotion.SLOW_SPEED;
         drivetrain.driveWithoutRamp((left*limiter), (right*limiter));
+
+
     }
 
     // Called once the command ends or is interrupted.
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        // intake.runPivot(0);
+        // if (enabled) intake.enable();
+    }
 
     // Returns true when the command should end.
     @Override