Hatch experimental (#107)

* more competition changes

* hatch move back

* disable characterized drive

Author: SaltyJoke

Robot2019/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.3.2"
+    id "edu.wpi.first.GradleRIO" version "2019.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

Robot2019/src/main/java/frc/robot/OI.java

@@ -11,19 +11,19 @@
 import edu.wpi.cscore.VideoSink;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import frc.robot.commands.ToggleClimberRails;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
 import frc.robot.commands.GradualDrive;
-import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.IntakeCargo;
-import frc.robot.commands.ToggleHatchIntake;
+import frc.robot.commands.IntakeHatch;
 import frc.robot.commands.ManualClimb;
 import frc.robot.commands.NormalDrive;
 import frc.robot.commands.ResetWobble;
 import frc.robot.commands.SetArcadeOrTank;
 import frc.robot.commands.SlowDrive;
 import frc.robot.commands.ToggleCamera;
+import frc.robot.commands.ToggleClimberRails;
+import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.ToggleLight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
@@ -74,7 +74,7 @@ public class OI {
     gradDriveBtn.whenPressed(new GradualDrive());
 
     hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
-    hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp));
+    hatchIntakeBtn.whenPressed(new IntakeHatch(hp, dt));
     hatchEjectBtn = new JoystickButton(manipulator, Manip.Y);
     hatchEjectBtn.whenPressed(new ToggleHatchEject(hp));
 
@@ -83,13 +83,13 @@ public class OI {
     cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values
     cargoEjectBtn.whenPressed(new EjectCargo(cargo));
 
-    climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
+    climberRailBtn = new JoystickButton(manipulator, Manip.RB_rShoulder);
     climberRailBtn.whenPressed(new ToggleClimberRails(climber));
 
-    autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
+    autoClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
     autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy, lights));
 
-    manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
+    manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
     manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights));
 
     toggleCameraBtn = new JoystickButton(leftJoy, 2);

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

@@ -19,6 +19,7 @@
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.HatchPanel;
 import frc.robot.subsystems.Lights;
+import edu.wpi.first.wpilibj.Timer;
 
 public class Robot extends TimedRobot {
   private static Drivetrain dt;
@@ -28,6 +29,7 @@ public class Robot extends TimedRobot {
   private static Climber climber;
   private static Lights lights;
   private static String fname1, fname2, fname3, fname4;
+  private static Timer timey;
 
   @Override
   public void robotInit() {
@@ -56,6 +58,8 @@ public void robotInit() {
 
     dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
     SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
+
+    timey = new Timer();
   }
 
   /**
@@ -83,15 +87,22 @@ public void disabledPeriodic() {
 
   @Override
   public void autonomousInit() {
+    timey.reset();
+    timey.start();
+    hp.grab();
   }
 
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
+
+    SmartDashboard.putNumber("Time left", (int) (15 - timey.get() + 1));
   }
 
   @Override
   public void teleopInit() {
+    timey.reset();
+    timey.start();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -101,6 +112,8 @@ public void teleopInit() {
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+
+    SmartDashboard.putNumber("Time left", (int) (135 - timey.get() + 1));
   }
 
   @Override

Robot2019/src/main/java/frc/robot/RobotMap.java

@@ -121,7 +121,7 @@ private static WPI_TalonSRX createConfiguredTalon(int port) {
     catchError(tsrx.configPeakCurrentLimit(0, 0));
     catchError(tsrx.configPeakCurrentDuration(0, 0));
     // 40 Amps is the amp limit of a CIM. lThe PDP has 40 amp circuit breakers,
-    catchError(tsrx.configContinuousCurrentLimit(40, 0));
+    catchError(tsrx.configContinuousCurrentLimit(30, 0));
     tsrx.enableCurrentLimit(true);
     catchError(tsrx.configNeutralDeadband(0.001, 10));
     tsrx.setNeutralMode(NeutralMode.Brake);

Robot2019/src/main/java/frc/robot/commands/GradualDrive.java

@@ -26,7 +26,7 @@ public GradualDrive() {
   // Called once when the command executes
   @Override
   protected void initialize() {
-    SmartDashboard.putBoolean("Gradual Drive", !SmartDashboard.getBoolean("Gradual Drive", false));
+    SmartDashboard.putBoolean("Gradual Drive", !SmartDashboard.getBoolean("Gradual Drive", true));
   }
 
 }

Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.HatchPanel;
+
+public class IntakeHatch extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public IntakeHatch(HatchPanel hp, Drivetrain dt) {
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+    addSequential(new ToggleHatchIntake(hp));
+    addSequential(new WaitTime(dt, 0.25));
+    addSequential(new MoveBack(dt, 0.25));
+  }
+}

Robot2019/src/main/java/frc/robot/commands/KeepClimber.java

@@ -13,7 +13,7 @@
 public class KeepClimber extends Command {
   private Climber climber;
 
-  private final double retractSpeed = -1;
+  private final double retractSpeed = 0.25;
 
   public KeepClimber(Climber climber) {
     // Use requires() here to declare subsystem dependencies

Robot2019/src/main/java/frc/robot/commands/ManualClimb.java

@@ -21,7 +21,7 @@ public class ManualClimb extends Command {
   final private double retractDist = 0;
   final private double climbDist = 24.46; // In inches
 
-  final private int climbJoyAxis = 1;
+  final private int climbJoyAxis = 3;
 
   public ManualClimb(Climber climber, Joystick manip, Lights lights) {
     // Use requires() here to declare subsystem dependencies
@@ -32,6 +32,13 @@ public ManualClimb(Climber climber, Joystick manip, Lights lights) {
     this.climber = climber;
     this.lights = lights;
     this.manip = manip;
+
+    if (!SmartDashboard.containsKey("Climber Max Height")) {
+      SmartDashboard.putNumber("Climber Max Height", 25);
+    }
+    if (!SmartDashboard.containsKey("Climber Drive Limit")) {
+      SmartDashboard.putNumber("Climber Drive Limit", 0);
+    }
   }
 
   // Called just before this Command runs the first time
@@ -53,6 +60,12 @@ protected void execute() {
     // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) {
     //   climbSpeed = 0;
     // }
+    
+    // climber soft stop to prevent bindinng
+    if (climber.getEncDistance() > SmartDashboard.getNumber("Climber Max Height", 25) && (-climbSpeed) > SmartDashboard.getNumber("Climber Drive Limit", 0)) {
+      climbSpeed = -SmartDashboard.getNumber("Climber Drive Limit", 0);
+    }
+
     climber.runClimber(climbSpeed);
 
     double angle = climber.getAngle();

Robot2019/src/main/java/frc/robot/commands/MoveBack.java

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.subsystems.Drivetrain;
+
+public class MoveBack extends Command {
+  Timer tim;
+  Drivetrain dt;
+  double time;
+  public MoveBack(Drivetrain dt, double time) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(dt);
+    this.dt = dt;
+    this.time = time;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    tim = new Timer();
+    tim.reset();
+    tim.start();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    dt.drive(-1, -1);
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return tim.get() >= time;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

Robot2019/src/main/java/frc/robot/commands/NormalDrive.java

@@ -26,7 +26,7 @@ public NormalDrive() {
   // Called once when the command executes
   @Override
   protected void initialize() {
-    SmartDashboard.putBoolean("Characterized Drive", !SmartDashboard.getBoolean("Characterized Drive", false));
+    SmartDashboard.putBoolean("Characterized Drive", false); // !SmartDashboard.getBoolean("Characterized Drive", false));
   }
 
 }