Climbing changes for no-legs

robot/src/main/java/frc/robot/Constants.java

@@ -21,9 +21,11 @@ public class Constants {
 
     // --- Drivetrain Constants ---
     public static class Drivetrain {
-        // We are using CAN based controllers. Each controller has an ID associated with it
+        // We are using CAN based controllers. Each controller has an ID associated with
+        // it
         // We use these IDs to directly communicate with a selected device over CAN bus
-        // The ids for these CAN devices were assigned via the SparkMax Utility application
+        // The ids for these CAN devices were assigned via the SparkMax Utility
+        // application
         public static final int kLeftLeaderId = 1;
         public static final int kLeftFollowerId = 2;
         public static final int kRightLeaderId = 3;
@@ -45,6 +47,10 @@ public static class Claw {
         public static final double kHatchQuickHoldSpeed = .4;
     }
 
+    public static class Intake {
+        public static final int kArmWheelId = 2;
+    }
+
     public static class Arm {
         public static final int kMasterId = 0;
         public static final int kSlaveId = 1;
@@ -82,18 +88,13 @@ public static class LogitechController {
     }
 
     public static enum TargetHeight {
-        GROUND,
-        LOW,  // Hatch load, cs hatch, r1 hatch  
-        CARGO_LOAD,
-        CS_CARGO_SCORE,
-        R1_CARGO_SCORE,
-        R2_HATCH,
-        R2_CARGO_SCORE,
+        GROUND, LOW, // Hatch load, cs hatch, r1 hatch
+        CARGO_LOAD, CS_CARGO_SCORE, R1_CARGO_SCORE, R2_HATCH, R2_CARGO_SCORE,
     }
 
-
     private static String[] heightNames;
-    private static EnumMap<TargetHeight, Integer> targetHeightMap = new EnumMap<TargetHeight, Integer>(TargetHeight.class);
+    private static EnumMap<TargetHeight, Integer> targetHeightMap = new EnumMap<TargetHeight, Integer>(
+            TargetHeight.class);
 
     public static void initTargetHeights() {
         targetHeightMap.put(TargetHeight.GROUND, Arm.kMinPosition);

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

@@ -11,6 +11,7 @@
 import frc.robot.subsystems.Claw;
 import frc.robot.util.DriveHelper;
 import frc.robot.commands.SetClawTargetMode;
+import frc.robot.commands.IntakeSpin;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.TargetHeight;
@@ -31,6 +32,10 @@ public class OI {
 
     private SendableChooser<TargetHeight> targetHeightChooser;
 
+    // The auxilery controller is used for non driving / controlling functions.
+    // Things like climing would be handled with the aux controller.
+    private XboxController mAuxController = new XboxController(1);
+
     /**
      * Get requested X-axis movement speed from the controller Based on a Constants
      * value, we may negate this to ensure it gives a positive value when we want to
@@ -40,7 +45,9 @@ public class OI {
      */
     public double getDriveSpeed() {
         // left stick, Y axis
-        return DriveHelper.handleDeadband((Constants.LogitechController.kInvertMoveSpeed ? -1 : 1) * mDriveController.getLeftStickY(), Constants.kDriveControllerDeadband);
+        return DriveHelper.handleDeadband(
+                (Constants.LogitechController.kInvertMoveSpeed ? -1 : 1) * mDriveController.getLeftStickY(),
+                Constants.kDriveControllerDeadband);
     }
 
     /**
@@ -89,14 +96,15 @@ public TargetHeight getSelectedDashboardHeight() {
     }
 
     public void changeSelectedDashboardHeight() {
-        
+
     }
 
     public OI() {
         this.mOperatorControoler.buttonX.whenPressed(new SetClawTargetMode(Claw.TargetMode.CARGO));
         this.mOperatorControoler.buttonB.whenPressed(new SetClawTargetMode(Claw.TargetMode.HATCH));
         this.mOperatorControoler.buttonA.whileHeld(new SetClawSpinMode(Claw.SpinMode.INTAKE));
         this.mOperatorControoler.buttonY.whileHeld(new SetClawSpinMode(Claw.SpinMode.EXHAUST));
+        this.mOperatorControoler.buttonStart.whileHeld(new IntakeSpin(true));
 
         this.mOperatorControoler.leftBumper.whenPressed(new SetArmFromDashboard(false));
         this.mOperatorControoler.rightBumper.whenPressed(new SetArmFromDashboard(false));

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

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Command;
+import frc.robot.subsystems.Intake;
+
+/* 
+ * Command to start or stop spinning the wheels on the end of the intake arms.
+ */
+public class IntakeSpin extends Command {
+    private Intake intake = Intake.getInstance();
+    private boolean spin;
+
+    public IntakeSpin(boolean spin) {
+        requires(intake);
+        this.spin = spin;
+    }
+
+    // Called just before this Command runs the first time
+    @Override
+    protected void initialize() {
+        System.out.println("Starting IntakeSpin");
+
+        if (spin) {
+            intake.spinWheels(true);
+        } else {
+            intake.spinWheels(false);
+        }
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    // This is blank because the comment is called continuously by
+    // the button that is connected to it
+    @Override
+    protected void execute() {
+
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    // This only ends when killed by the button its connected to (??)
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    @Override
+    protected void end() {
+        intake.spinWheels(false);
+        System.out.println("Ending IntakeSpin");
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    @Override
+    protected void interrupted() {
+        System.out.println("IntakeSpin Interrupted");
+        end();
+    }
+}

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

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+import frc.robot.Constants;
+
+/*
+ * Originally there was supposed to be a climbing arm to reach hab2 or hab3. However
+ * since the claw is not able to grab the cargo from the floor we are repurposing that 
+ * climbing arm so that it is now an intake arm. (That's right, the robot will not climb; maybe 
+ * next year).
+ * 
+ * Each intake arm (two of the, one on right side and one on left side) will have wheels
+ * controlled by one motor (VictorSPX controller). So this subsystem simply needs to 
+ * be able to handling proper spinning of those wheels.
+ */
+
+public class Intake extends Subsystem {
+
+    // Generally, return a singleton instance of the subsystem
+    private static Intake sInstance;
+
+    // Always return the same instance
+    public static Intake getInstance() {
+        if (sInstance == null) {
+            sInstance = new Intake();
+        }
+
+        return sInstance;
+    }
+
+    private static final double WHEEL_SPEED = .7f;
+
+    private VictorSPX armWheels; // A Victor on Channel 2
+
+    private Intake() {
+        armWheels = new VictorSPX(Constants.Intake.kArmWheelId);
+    }
+
+    public void resetToDefault() {
+        spinWheels(false);
+    }
+
+    /* Turn on or off the spinning of the arm wheels */
+    public void spinWheels(boolean onOff) {
+        System.out.println("Intake Wheels " + (onOff ? "Spinning" : "Not Spinning"));
+        armWheels.set(ControlMode.PercentOutput, onOff ? WHEEL_SPEED : .0f);
+    }
+
+    /* Return true if the arm wheels are spinning, false otherwise */
+    public boolean areWheelsSpinning() {
+        return armWheels.getMotorOutputPercent() >= .0f;
+    }
+
+    /**
+     * This is currently not used as the mode selections are managed by the default
+     * variables and controllers
+     */
+    @Override
+    public void initDefaultCommand() {
+        // Set the default command for a subsystem here.
+        // No default command for this subsystem
+    }
+
+}