Elevator impl

config_robot/proto/501robot.props

@@ -93,6 +93,14 @@ Elevator.autoCommandName = ElevatorDoNothing
 Elevator.teleCommandName = ElevatorDoNothing
 
 
+#####################
+# Incrementer
+#####################
+Incrementer.className = 
+Incrementer.autoCommandName = IncrementerDoNothing
+Incrementer.teleCommandName = IncrementerDoNothing
+
+
 #####################
 # Shooter
 #####################

config_robot/real/501robot.props

@@ -58,7 +58,15 @@ Vision.className = VisionSensor
 TurretLocation.className = StubTurretLocationSensor
 #TurretLocation.className = TurretLocationSensor
 
+#####################
+# ElevatorSensor
+#####################
+ElevatorSensor.className = StubElevatorSensor
 
+#####################
+# IncrementerSensor
+#####################
+IncrementerSensor.className = IncrementerSensor
 
 #########################################
 #########################################
@@ -96,6 +104,14 @@ Elevator.teleCommandName = ElevatorManualControl
 #Elevator.teleCommandName = ElevatorDoNothing
 
 
+#####################
+# Incrementer
+#####################
+Incrementer.className = IncrementerSubsystem
+Incrementer.autoCommandName = IncrementerDoNothing
+Incrementer.teleCommandName = IncrementerManualControl
+
+
 #####################
 # Shooter
 #####################

config_robot/suitcase/501robot.props

@@ -93,6 +93,13 @@ Elevator.autoCommandName = ElevatorDoNothing
 Elevator.teleCommandName = ElevatorDoNothing
 
 
+#####################
+# Incrementer
+#####################
+Incrementer.className = 
+Incrementer.defaultCommandName = IncrementerDoNothing
+
+
 #####################
 # Shooter
 #####################

config_robot/zester/501robot.props

@@ -95,6 +95,13 @@ Elevator.teleCommandName = ElevatorManualControl
 #Elevator.teleCommandName = ElevatorDoNothing
 
 
+#####################
+# Incrementer
+#####################
+Incrementer.className = 
+Incrementer.defaultCommandName = IncrementerDoNothing
+
+
 #####################
 # Shooter
 #####################

electrical_desc.md

@@ -2,36 +2,35 @@
 
 | Mechanism        | Controller Type | CAN ID | Position    | PDB ID |
 | ---------------- | --------------- | ------ | ----------- | ------ |
-| Elevator         | Talon SRX       | 50     | Bottom      | 5      |
-| Elevator         | Talon SRX       | 51     | Top         | 7      |
+| Elevator         | Talon SRX       | 50     | Lower       | 5      |
+| Incrementer      | Talon SRX       | 51     | Upper       | 7      |
 | Climber          | Spark Max       | 55     | Right       | 12     |
 | Climber          | Spark Max       | 56     | Left        | 13     |
 | Drive            | Spark Max       | 11     | Left Front  | 15     |
 | Drive            | Spark Max       | 12     | Left Rear   | 14     |
 | Drive            | Spark Max       | 13     | Right Front | 0      |
 | Drive            | Spark Max       | 14     | Right Rear  | 1      |
-| Incrementer      | Talon SRX       | 23     | N/A         | TBD    |
 | Intake           | Talon SRX       | 41     | N/A         | 11     |
 | Intake Retractor | Talon SRX       | 42     | N/A         | 9      |
 | Shooter          | Spark Max       | 21     | Left        | 2      |
 | Shooter          | Spark Max       | 22     | Right       | 3      |
 | Turret           | Spark Max       | 20     | N/A         | 6      |
 |                  |                 |        |             |        |
-| Pneumatics       | PCM             | ??     |             | 8      |
+| Pneumatics       | PCM             | 0      |             | 8      |
 |                  |                 |        |             |        | 
 | Vision           | Limelight       | N/A    | N/A         | 10     |
 
 ## Sensors
 
-| Mechanism  | Sensor Type  | Port      | Position |
-| ---------- | ------------ | --------- |----------|
-| Elevator   | Limit Switch | 9         | Lower    |
-| Elevator   | Limit Switch | ???       | Upper    |
-| Climber    | ???          | ???       | N/A      |
-| Climber    | ???          | ???       | N/A      |
-| Drive      | IMU          | MXP       | N/A      |
-| Turret     | Proximity    | Digital 8 | N/A      |
+| Mechanism   | Sensor Type  | Port      | Position |
+| ----------- | ------------ | --------- |----------|
+| Elevator    | Ultrasonic-y | 0         | Lower    |
+| Incrementer | Ultrasonic-y | 1         | Upper    |
+| Climber     | ???          | ???       | N/A      |
+| Climber     | ???          | ???       | N/A      |
+| Drive       | IMU          | MXP       | N/A      |
+| Turret      | Proximity    | Digital 8 | N/A      |
 
-| Drive      | Relative Encoder | Left  | Left Front (Talon SRX)  |
-| Drive      | Relative Encoder | Right | Right Front (Talon SRX) |
+| Drive       | Relative Encoder | Left  | Left Front (Talon SRX)  |
+| Drive       | Relative Encoder | Right | Right Front (Talon SRX) |
 

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

@@ -14,10 +14,10 @@
 import frc.robot.sensors.vision.VisionFactory;
 import frc.robot.subsystems.elevator.ElevatorFactory;
 import frc.robot.subsystems.elevator.IElevatorSubsystem;
+import frc.robot.subsystems.incrementer.IIncrementerSubsystem;
+import frc.robot.subsystems.incrementer.IncrementerFactory;
 import frc.robot.subsystems.shooter.IShooterSubsystem;
 import frc.robot.subsystems.shooter.ShooterFactory;
-import frc.robot.subsystems.intake.IIntakeSubsystem;
-import frc.robot.subsystems.intake.IntakeFactory;
 
 import riolog.RioLogger;
 
@@ -29,19 +29,19 @@ public class FirePoseVision extends PKCommandBase {
     private final IVisionSensor vision;
 
     private final IShooterSubsystem shooter;
+    private final IIncrementerSubsystem incrementer;
     private final IElevatorSubsystem elevator;
-    private final IIntakeSubsystem intake;
 
     public FirePoseVision() {
         logger.info("constructing {}", getName());
 
         vision = VisionFactory.getInstance();
 
         shooter = ShooterFactory.getInstance();
+        incrementer = IncrementerFactory.getInstance();
         elevator = ElevatorFactory.getInstance();
-        intake = IntakeFactory.getInstance();
 
-        addRequirements(shooter, elevator, intake);
+        addRequirements(shooter, incrementer, elevator);
 
         logger.info("constructed {}", getName());
     }
@@ -50,32 +50,43 @@ public FirePoseVision() {
     public void execute() {
         super.execute();
 
-        intake.pullIn();
-        elevator.lift();
-
         shooter.shoot();
 
+        /* Logic to be used once the shooter PID (target velocity) is a thing */
+        // boolean visionGood = (vision.isActive() && vision.isLocked()) ||
+        // !(vision.isActive());
+        // if (visionGood && shooter.atTargetVelocity()) {
+        // incrementer.increment();
+        // if (!incrementer.isFull()) {
+        // elevator.lift();
+        // }
+        // } else {
+        // incrementer.stop();
+        // elevator.stop();
+        // }
         boolean visionGood = (vision.isActive() && vision.isLocked()) || !(vision.isActive());
-        if (visionGood && shooter.atTargetVelocity()) {
-            elevator.lift();
-            elevator.increment();
+        if (visionGood) {
+            incrementer.increment();
+            if (!incrementer.isFull()) {
+                elevator.lift();
+            } else {
+                elevator.stop();
+            }
         } else {
-            // ballevator.liftToLimit();
-            elevator.stopIncrement();
+            incrementer.stop();
+            elevator.stop();
         }
     }
 
     @Override
     public void end(boolean interrupted) {
         super.end(interrupted);
 
+        incrementer.stop();
         elevator.stop();
-        elevator.stopIncrement();
 
         // Don't stop shooter (default is idle command)
         // shooter.stop();
-
-        intake.stop();
     }
 
 }

src/main/java/frc/robot/commands/elevator/ElevatorLift.java

@@ -27,15 +27,13 @@ public void execute() {
         super.execute();
 
         elevator.lift();
-        elevator.increment();
     }
 
     @Override
     public void end(boolean interrupted) {
         super.end(interrupted);
 
         elevator.stop();
-        elevator.stopIncrement();
     }
 
 }

src/main/java/frc/robot/commands/elevator/ElevatorManualControl.java

@@ -34,14 +34,7 @@ public void execute() {
         if (liftSpeed == 0) {
             elevator.stop();
         } else if (liftSpeed > 0) {
-            elevator.lift();
-        }
-
-        double incrementSpeed = oi.getElevatorSpeed();
-        if (incrementSpeed == 0) {
-            elevator.stopIncrement();
-        } else if (incrementSpeed > 0) {
-            elevator.increment();
+            elevator.liftToLimit();
         }
     }
 }

src/main/java/frc/robot/commands/incrementer/IncrementerCommandBase.java

@@ -0,0 +1,39 @@
+/*-----------------------------------------------------------------------*/
+/* Copyright (c) Team 501 - The PowerKnights. All Rights Reserved.       */
+/* Open Source Software - may be modified and shared by other FRC teams  */
+/* under the terms of the Team501 license. The code must be accompanied  */
+/* by the Team 501 - The PowerKnights license file in the root directory */
+/* of this project.                                                      */
+/*-----------------------------------------------------------------------*/
+
+package frc.robot.commands.incrementer;
+
+import frc.robot.commands.PKCommandBase;
+import frc.robot.subsystems.incrementer.IIncrementerSubsystem;
+import frc.robot.subsystems.incrementer.IncrementerFactory;
+
+import riolog.PKLogger;
+import riolog.RioLogger;
+
+/**
+ * Add your docs here.
+ */
+abstract class IncrementerCommandBase extends PKCommandBase {
+
+    /** Our classes' logger **/
+    private static final PKLogger logger = RioLogger.getLogger(IncrementerCommandBase.class.getName());
+
+    // Handle to our subsystem
+    protected IIncrementerSubsystem incrementer;
+
+    public IncrementerCommandBase() {
+        logger.info("constructing {}", getName());
+
+        incrementer = IncrementerFactory.getInstance();
+
+        addRequirements(incrementer);
+
+        logger.info("constructed");
+    }
+
+}

src/main/java/frc/robot/commands/incrementer/IncrementerDoNothing.java

@@ -0,0 +1,25 @@
+/*-----------------------------------------------------------------------*/
+/* Copyright (c) Team 501 - The PowerKnights. All Rights Reserved.       */
+/* Open Source Software - may be modified and shared by other FRC teams  */
+/* under the terms of the Team501 license. The code must be accompanied  */
+/* by the Team 501 - The PowerKnights license file in the root directory */
+/* of this project.                                                      */
+/*-----------------------------------------------------------------------*/
+
+package frc.robot.commands.incrementer;
+
+import riolog.PKLogger;
+import riolog.RioLogger;
+
+public class IncrementerDoNothing extends IncrementerCommandBase {
+
+    /** Our classes' logger **/
+    private static final PKLogger logger = RioLogger.getLogger(IncrementerDoNothing.class.getName());
+
+    public IncrementerDoNothing() {
+        logger.info("constructing {}", getName());
+
+        logger.info("constructed");
+    }
+
+}

src/main/java/frc/robot/commands/incrementer/IncrementerIncrement.java

@@ -0,0 +1,43 @@
+/*-----------------------------------------------------------------------*/
+/* Copyright (c) Team 501 - The PowerKnights. All Rights Reserved.       */
+/* Open Source Software - may be modified and shared by other FRC teams  */
+/* under the terms of the Team501 license. The code must be accompanied  */
+/* by the Team 501 - The PowerKnights license file in the root directory */
+/* of this project.                                                      */
+/*-----------------------------------------------------------------------*/
+
+package frc.robot.commands.incrementer;
+
+import frc.robot.subsystems.incrementer.IIncrementerSubsystem;
+
+import riolog.PKLogger;
+import riolog.RioLogger;
+
+/**
+ * Add your docs here.
+ */
+public class IncrementerIncrement extends IncrementerCommandBase {
+
+    /** Our classes' logger **/
+    private static final PKLogger logger = RioLogger.getLogger(IncrementerIncrement.class.getName());
+
+    // Handle to our subsystem
+    protected IIncrementerSubsystem incrementer;
+
+    public IncrementerIncrement() {
+        logger.info("constructing {}", getName());
+
+        logger.info("constructed");
+    }
+
+    @Override
+    public void execute() {
+        incrementer.increment();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        incrementer.stop();
+    }
+
+}

src/main/java/frc/robot/commands/incrementer/IncrementerManualControl.java

@@ -0,0 +1,40 @@
+/*-----------------------------------------------------------------------*/
+/* Copyright (c) Team 501 - The PowerKnights. All Rights Reserved.       */
+/* Open Source Software - may be modified and shared by other FRC teams  */
+/* under the terms of the Team501 license. The code must be accompanied  */
+/* by the Team 501 - The PowerKnights license file in the root directory */
+/* of this project.                                                      */
+/*-----------------------------------------------------------------------*/
+
+package frc.robot.commands.incrementer;
+
+import riolog.PKLogger;
+import riolog.RioLogger;
+
+public class IncrementerManualControl extends IncrementerOICommandBase {
+
+    /** Our classes' logger **/
+    private static final PKLogger logger = RioLogger.getLogger(IncrementerManualControl.class.getName());
+
+    /**
+     * Creates a new DriveJoystickControl.
+     */
+    public IncrementerManualControl() {
+        logger.info("constructing {}", getName());
+
+        logger.info("constructed");
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        super.execute();
+
+        double incrementSpeed = oi.getIntakeSpeed();
+        if (incrementSpeed == 0) {
+            incrementer.stop();
+        } else if (incrementSpeed > 0) {
+            incrementer.incrementToLimit();
+        }
+    }
+}