Merge pull request #7 from FRC6334/release/deeprun

Release/deeprun

build.gradle

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

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

@@ -27,10 +27,14 @@ public static void init() {
     auxStick = new Joystick(RobotMap.auxillaryStick);
 
     Button moveArmToPos = new JoystickButton(leftDriveStick, 9);
-    Button movePivotDown = new JoystickButton(auxStick, 4);
-    Button movePivotUp = new JoystickButton(auxStick, 5);
+    Button moveArmToHighestRocket = new JoystickButton(leftDriveStick, 8);
+    Button driveClimberForward = new JoystickButton(rightDriveStick, 4);
+    Button driveClimberBackwards = new JoystickButton(rightDriveStick, 5);
 
+    moveArmToHighestRocket.whileHeld(new MoveToHighRocket());
     moveArmToPos.whileHeld(new MoveTo47());
+    driveClimberForward.whileHeld(new DriveClimberForward());
+    driveClimberBackwards.whileHeld(new DriveClimberBackwards());
   }
 
   public static Joystick getRightDriveStick() {

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

@@ -42,7 +42,7 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture();
   }
 
-  /**
+  /**h
    * This function is called every robot packet, no matter the mode. Use
    * this for items like diagnostics that you want ran during disabled,
    * autonomous, teleoperated and test.
@@ -89,6 +89,7 @@ public void autonomousInit() {
      * autonomousCommand = new ExampleCommand(); break; }
      */
     System.out.println("auto init");
+    CommandBase.grabber.setForward();
   }
 
   /**
@@ -105,9 +106,6 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
     //CommandBase.grabber.toggle();
   }
 

src/main/java/frc/robot/commands/AutoDriveToTarget.java

@@ -18,12 +18,9 @@ public class AutoDriveToTarget extends CommandBase {
 
   public AutoDriveToTarget() {
     super("AutoDrive");
-    System.out.println("wew");
-    // Use requires() here to declare subsystem dependencies
     requires(driveTrain);
     requires(vision);
     requires(climber);
-    System.out.println("lad");
 
     limelight = NetworkTableInstance.getDefault().getTable("limelight");
   }

src/main/java/frc/robot/commands/ClimberDrive.java

@@ -45,14 +45,6 @@ protected void execute() {
       System.out.println("Toggle grabber");
     }
 
-    if (rightStick.getRawButtonPressed(9)) {
-      climber.driveBack(0.5);
-    }
-
-    if (auxJoystick.getRawButtonPressed(2)) {
-      climber.driveBack(1);
-    }
-
     if (Math.abs(auxJoystick.getY()) <= 0.05) // Arm is disabled, just drives the climb motors here
       climber.driveArm(0);
     else

src/main/java/frc/robot/commands/CommandBase.java

@@ -11,7 +11,6 @@
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Grabber;
-import frc.robot.subsystems.Vacuum;
 import frc.robot.subsystems.Vision;
 import frc.robot.OI;
 
@@ -23,7 +22,6 @@ public class CommandBase extends Command {
   public static Vision vision;
   public static Climber climber;
   public static Grabber grabber;
-  public static Vacuum vacuum;
 
   // Init all subsystems
   public static void init() {
@@ -33,7 +31,6 @@ public static void init() {
     driveTrain = new DriveTrain();
     vision = new Vision();
     grabber = new Grabber();
-    vacuum = new Vacuum();
 
     OI.init();
   }

src/main/java/frc/robot/commands/VacuumDrive.java → src/main/java/frc/robot/commands/DriveClimberBackwards.java

@@ -7,37 +7,43 @@
 
 package frc.robot.commands;
 
-public class VacuumDrive extends CommandBase { // This is a dummy file
-  public VacuumDrive() {
-    super("dummyvacuumdrive");
+public class DriveClimberBackwards extends CommandBase {
+  public DriveClimberBackwards() {
+    super("driveclimberforward");
     // Use requires() here to declare subsystem dependencies
-    requires(vacuum);
+    requires(climber);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    System.out.println("Driving climber backwards");
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    climber.driveBack(-0.5);
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   public boolean isFinished() {
-    return true;
+    return false;
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    System.out.println("done driving the climber backwards");
+    climber.driveBack(0);
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    System.out.println("climber backwards drive interrupt");
+    climber.driveBack(0);
   }
 }

src/main/java/frc/robot/commands/RunVacuumForward.java → src/main/java/frc/robot/commands/DriveClimberForward.java

@@ -7,24 +7,23 @@
 
 package frc.robot.commands;
 
-public class RunVacuumForward extends CommandBase {
-  public RunVacuumForward() {
-    super("drivevacuumcommand");
+public class DriveClimberForward extends CommandBase {
+  public DriveClimberForward() {
+    super("driveclimberforward");
     // Use requires() here to declare subsystem dependencies
-    requires(vacuum);
+    requires(climber);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    System.out.println("Driving the vacuum motor to create suction...");
+    System.out.println("Driving climber forward");
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    System.out.println("Suction in progress");
-    vacuum.setForward();
+    climber.driveBack(0.5);
   }
 
   // Make this return true when this Command no longer needs to run execute()
@@ -36,13 +35,15 @@ public boolean isFinished() {
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    System.out.println("done driving the climber forwards");
+    climber.driveBack(0);
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
-    System.out.println("Done driving the vacuum motor forwards...");
-    vacuum.setValue(0);
+    System.out.println("climber forward drive interrupted");
+    climber.driveBack(0);
   }
 }

src/main/java/frc/robot/commands/MoveTo47.java

@@ -16,8 +16,6 @@ public class MoveTo47 extends CommandBase {
   public MoveTo47() {
     super("MoveTo47Inches");
     requires(climber);
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
   }
 
   // Called just before this Command runs the first time
@@ -40,6 +38,7 @@ public boolean isFinished() { // Do not worry about this method. It will be inte
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    climber.driveArm(0);
   }
 
   // Called when another command which requires one or more of the same

src/main/java/frc/robot/commands/LiftToHeight.java → src/main/java/frc/robot/commands/MoveToHighRocket.java

@@ -7,38 +7,44 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.RobotMap;
 
-public class LiftToHeight extends Command {
-  public LiftToHeight() {
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
+public class MoveToHighRocket extends CommandBase {
+
+  boolean upThere = false;
+
+  public MoveToHighRocket() {
+    super("MoveToHighRocket");
+    requires(climber);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    System.out.println("Move to high rocket started");
+    climber.setArmPos(-15);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    System.out.println("high rocket pid exec");
   }
 
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
+  public boolean isFinished() { // Do not worry about this method. It will be interrupted when the button is released (2)
     return false;
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
+      climber.driveArm(0);
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    System.out.println("Move to high rocket interrupted");
   }
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PWMTalonSRX;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.RobotMap;
 import frc.robot.commands.ClimberDrive;
@@ -45,6 +46,8 @@ public Climber() {
         arm = new CANSparkMax(5, MotorType.kBrushless);
         armEncoder = new CANEncoder(arm);
         armPID = new CANPIDController(arm);
+        armEncoder.getVelocity();
+        armEncoder.getPosition();
 
         backDriveRight.setInverted(true);
 
@@ -84,9 +87,11 @@ public void driveBack(double amnt) {
   public void toggleAll() {
     if (gg) {
       front.set(REVERSE);
+      Timer.delay(0.195);
       back.set(REVERSE);
     } else {
       front.set(FORWARD);
+      Timer.delay(0.195);
       back.set(FORWARD);
     }
     gg = !gg;

src/main/java/frc/robot/subsystems/Grabber.java

@@ -35,14 +35,17 @@ public Grabber() {
 
   public void toggle() {
     if (diskExtended) {
+      System.out.println("Grabber is closing.");
       disk.set(REVERSE);
     } else {
+      System.out.println("Grabber is opening.");
       disk.set(FORWARD);
     }
     diskExtended = !diskExtended;
   }
 
   public void setForward() {
+    System.out.println("Grabber has been set forward");
     disk.set(FORWARD);
     diskExtended = true;
   }