Name changes

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

@@ -3,24 +3,23 @@
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.team4909.robot.operator.controllers.BionicF310;
 import frc.team4909.robot.sensors.LidarLitePWM;
 import frc.team4909.robot.sensors.Stream;
-import frc.team4909.robot.subsystems.StiltWheel.StiltWheelSubsystem;
-import frc.team4909.robot.subsystems.StiltWheel.commands.MoveStiltWheels;
-import frc.team4909.robot.subsystems.climber.ClimberSubsystem;
-import frc.team4909.robot.subsystems.climber.commands.*;
+import frc.team4909.robot.subsystems.stiltwheel.StiltWheelSubsystem;
+import frc.team4909.robot.subsystems.stiltwheel.commands.MoveStiltWheels;
+import frc.team4909.robot.subsystems.stilts.StiltSubsystem;
+import frc.team4909.robot.subsystems.stilts.commands.*;
 import frc.team4909.robot.subsystems.drivetrain.DriveTrainSubsystem;
 import frc.team4909.robot.subsystems.drivetrain.commands.InvertDriveDirection;
 import frc.team4909.robot.subsystems.drivetrain.commands.TogglePreciseMode;
 import frc.team4909.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.team4909.robot.subsystems.elevator.commands.*;
-import frc.team4909.robot.subsystems.elevatorarm.ElevatorArmSubsystem;
-import frc.team4909.robot.subsystems.elevatorarm.commands.*;
+import frc.team4909.robot.subsystems.wrist.WristSubsystem;
+import frc.team4909.robot.subsystems.wrist.commands.*;
 import frc.team4909.robot.subsystems.intake.IntakeSubsystem;
 import frc.team4909.robot.subsystems.intake.commands.*;
 
@@ -68,28 +67,14 @@ public class Robot extends TimedRobot {
   public static DriveTrainSubsystem drivetrainSubsystem;
   public static IntakeSubsystem intakeSubsystem;
   public static ElevatorSubsystem elevatorSubsystem;
-  public static ElevatorArmSubsystem elevatorArmSubsystem;
-  public static ClimberSubsystem climberSubsystem;
+  public static WristSubsystem wristSubsystem;
+  public static StiltSubsystem stiltSubsystem;
   public static StiltWheelSubsystem stiltWheelSubsystem;
   public static Compressor c;
 
   // Sensors
   public static LidarLitePWM lidar;
 
-  /**
-   * map a number from one range to another
-   * 
-   * @param {num} value the value to be mapped
-   * @param {num} old_min the minimum of value
-   * @param {num} old_max the maximum of value
-   * @param {num} new_min the new minimum value
-   * @param {num} new_max the new maximum value
-   * @return {num} the value remaped on the range [new_min new_max]
-   */
-  public static double map(double value, double old_min, double old_max, double new_min, double new_max) {
-    return (value - old_min) / (old_max - old_min) * (new_max - new_min) + new_min;
-  }
-
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -113,8 +98,8 @@ public void robotInit() {
     drivetrainSubsystem = new DriveTrainSubsystem();
     intakeSubsystem = new IntakeSubsystem();
     elevatorSubsystem = new ElevatorSubsystem();
-    elevatorArmSubsystem = new ElevatorArmSubsystem();
-    climberSubsystem = new ClimberSubsystem();
+    wristSubsystem = new WristSubsystem();
+    stiltSubsystem = new StiltSubsystem();
     stiltWheelSubsystem = new StiltWheelSubsystem();
 
     // Sensors
@@ -216,6 +201,7 @@ public void robotPeriodic() {
   }
 
   public void autonomousInit() {
+    new MoveUpToLimit().start();
     // elevatorSubsystem.setSensorZero();
     // elevatorArmSubsystem.setSensorZero();
     // climberSubsystem.setSensorZero();
@@ -240,8 +226,8 @@ public void disabledPeriodic() {
     Scheduler.getInstance().run(); 
 
     Robot.elevatorSubsystem.updateHoldingPos();
-    Robot.elevatorArmSubsystem.holdingPosition = Robot.elevatorArmSubsystem.getPosition();
-    Robot.climberSubsystem.updateHoldingPos();
+    Robot.wristSubsystem.updateHoldingPos();
+    Robot.stiltSubsystem.updateHoldingPos();
   }
 
   public void teleopPeriodic() {

src/main/java/frc/team4909/robot/subsystems/StiltWheel/StiltWheelSubsystem.java

@@ -1,12 +1,9 @@
-package frc.team4909.robot.subsystems.StiltWheel;
+package frc.team4909.robot.subsystems.stiltwheel;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
-
-import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotMap;
-import frc.team4909.robot.subsystems.StiltWheel.commands.Default_StiltWheelStop;
+import frc.team4909.robot.subsystems.stiltwheel.commands.Default_StiltWheelStop;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 

src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/Default_StiltWheelStop.java

@@ -1,4 +1,4 @@
-package frc.team4909.robot.subsystems.StiltWheel.commands;
+package frc.team4909.robot.subsystems.stiltwheel.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team4909.robot.Robot;

src/main/java/frc/team4909/robot/subsystems/StiltWheel/commands/MoveStiltWheels.java

@@ -1,4 +1,4 @@
-package frc.team4909.robot.subsystems.StiltWheel.commands;
+package frc.team4909.robot.subsystems.stiltwheel.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team4909.robot.Robot;

src/main/java/frc/team4909/robot/subsystems/drivetrain/DriveTrainSubsystem.java

@@ -9,10 +9,9 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team4909.robot.RobotMap;
-import frc.team4909.robot.operator.controllers.BionicF310;
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
-import frc.team4909.robot.subsystems.drivetrain.commands.Drive;
+import frc.team4909.robot.subsystems.drivetrain.commands.Default_Drive;
 
 public class DriveTrainSubsystem extends Subsystem {
     CANSparkMax frontLeftSparkMax, rearLeftSparkMax, frontRightSparkMax, rearRightSparkMax;
@@ -88,7 +87,7 @@ public void togglePreciseMode() {
     }
 
     protected void initDefaultCommand() {
-        setDefaultCommand(new Drive());
+        setDefaultCommand(new Default_Drive());
     }
 
     @Override

src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Drive.java → src/main/java/frc/team4909/robot/subsystems/drivetrain/commands/Default_Drive.java

@@ -4,8 +4,8 @@
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.operator.controllers.BionicF310;
 
-public class Drive extends Command {
-    public Drive() {
+public class Default_Drive extends Command {
+    public Default_Drive() {
         requires(Robot.drivetrainSubsystem);
     }
 

src/main/java/frc/team4909/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -6,13 +6,9 @@
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-
-import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import com.ctre.phoenix.motorcontrol.ControlMode;
-
-import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
 import frc.team4909.robot.RobotMap;
 import frc.team4909.robot.subsystems.elevator.commands.Default_ElevHoldPos;

src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java

@@ -10,8 +10,8 @@
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
 import frc.team4909.robot.RobotMap;
-import frc.team4909.robot.subsystems.intake.commands.CargoIntakeHold;
-import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose;
+import frc.team4909.robot.subsystems.intake.commands.Default_CargoIntakeHold;
+import frc.team4909.robot.subsystems.intake.commands.Default_HatchPanelIntakeClose;
 
 public class IntakeSubsystem extends Subsystem {
     DoubleSolenoid hatchPanelSolenoid;
@@ -37,13 +37,7 @@ public void hatchPanelIntakeClose() {
     }
 
     public void holdCargoIntake(){
-        // if(hasCargo()){
-        //     setCargoIntakeSpeed(RobotConstants.cargoIntakeHoldSpeed);
-        // } else {
-        //     setCargoIntakeSpeed(0.0625);
-
         setCargoIntakeSpeed(RobotConstants.cargoIntakeHoldSpeed);
-        // }
     }
 
     public void setCargoIntakeSpeed(double speed) {
@@ -86,8 +80,8 @@ protected void initDefaultCommand() {
 
                 // Revert to Closed by Default, Will Simplify While
                 // Held/Toggle Open Commands in Future
-                addParallel(new HatchPanelIntakeClose());
-                addParallel(new CargoIntakeHold());
+                addParallel(new Default_HatchPanelIntakeClose());
+                addParallel(new Default_CargoIntakeHold());
             }
         });
     }

src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeHold.java → src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_CargoIntakeHold.java

@@ -4,8 +4,8 @@
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
 
-public class CargoIntakeHold extends Command{
-    public CargoIntakeHold(){
+public class Default_CargoIntakeHold extends Command{
+    public Default_CargoIntakeHold(){
         requires(Robot.intakeSubsystem);
     }
 

src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeClose.java → src/main/java/frc/team4909/robot/subsystems/intake/commands/Default_HatchPanelIntakeClose.java

@@ -3,9 +3,9 @@
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import frc.team4909.robot.Robot;
 
-public class HatchPanelIntakeClose extends InstantCommand {
+public class Default_HatchPanelIntakeClose extends InstantCommand {
 
-    public HatchPanelIntakeClose() {
+    public Default_HatchPanelIntakeClose() {
         requires(Robot.intakeSubsystem);
     }
 

src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java

@@ -1,7 +1,6 @@
 package frc.team4909.robot.subsystems.intake.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
-import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose;
 import frc.team4909.robot.Robot;
 
 public class HatchPanelIntakeOpen extends Command {

src/main/java/frc/team4909/robot/subsystems/climber/ClimberSubsystem.java → src/main/java/frc/team4909/robot/subsystems/stilts/StiltSubsystem.java

@@ -1,28 +1,25 @@
-package frc.team4909.robot.subsystems.climber;
+package frc.team4909.robot.subsystems.stilts;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
-
-import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team4909.robot.RobotMap;
-import frc.team4909.robot.subsystems.climber.commands.Default_StiltsHoldPos;
-import frc.team4909.robot.Robot;
+import frc.team4909.robot.subsystems.stilts.commands.Default_StiltsHoldPos;
 import frc.team4909.robot.RobotConstants;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
-public class ClimberSubsystem extends Subsystem {
+public class StiltSubsystem extends Subsystem {
 
     // All motor controllers should be private.
     // Methods that allow safe motion should be provided by the subsystem
     private WPI_TalonSRX climberLiftMaster;
     private WPI_VictorSPX climberLiftSlave;
     private int holdingStiltsPosition = 0;
 
-    public ClimberSubsystem() {
+    public StiltSubsystem() {
         //super should always be called to ensure proper subystem initialization
         super();
         climberLiftMaster = new WPI_TalonSRX(RobotMap.climberMasterSRXID);
@@ -128,6 +125,21 @@ public void holdPosition() {
         setPosition(holdingStiltsPosition);
     }
 
+    public boolean isAtTop() {
+        //@todo not sure if fwd or reverse
+        return climberLiftMaster.getSensorCollection().isFwdLimitSwitchClosed();
+    }
+
+    public void setCurrentLimit(int maxAmps) {
+        if (maxAmps == 0) {
+            climberLiftMaster.enableCurrentLimit(false);
+        } else {
+            climberLiftMaster.enableCurrentLimit(true);
+            climberLiftMaster.configPeakCurrentLimit(0, RobotConstants.timeoutMs);
+            climberLiftMaster.configContinuousCurrentLimit(maxAmps, RobotConstants.timeoutMs);
+        }
+    }
+