Elevator code

ChargedUp/.OutlineViewer/outlineviewer.json

@@ -0,0 +1,46 @@
+{
+  "Clients": {
+    "open": true
+  },
+  "Connections": {
+    "open": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "5985"
+  },
+  "Server": {
+    "open": true
+  },
+  "retained": {
+    "LiveWindow": {
+      "open": true
+    }
+  },
+  "shuffleboard": {
+    "Publishers": {
+      "open": true
+    },
+    "Subscribers": {
+      "open": true
+    },
+    "open": true
+  },
+  "transitory": {
+    "FMSInfo": {
+      "open": true
+    },
+    "LiveWindow": {
+      "open": true
+    },
+    "Shuffleboard": {
+      ".metadata": {
+        "open": true
+      },
+      ".recording": {
+        "open": true
+      },
+      "open": true
+    }
+  }
+}

ChargedUp/.SysId/sysid.json

@@ -0,0 +1,10 @@
+{
+  "SysId": {
+    "Analysis Type": "General Mechanism",
+    "NetworkTables Settings": {
+      "serverTeam": "5985"
+    },
+    "mode": "Client (NT4)",
+    "serverTeam": "5985"
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Config;
+import frc.robot.Subsystems.HorizontalExtension;
+import frc.robot.Subsystems.VerticalExtension;
+import frc.robot.Subsystems.Wrist;
+
+public class ArmHighPosCommand extends CommandBase {
+  private final Wrist m_wrist;
+  private final HorizontalExtension m_horizontalExtension;
+  private final VerticalExtension m_verticalExtension;
+  /** Creates a new ArmHighPosCommand. */
+  public ArmHighPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) {
+    m_wrist = wristSubsystem;
+    m_verticalExtension = verticalExtensionSubsystem;
+    m_horizontalExtension = horizontalExtensionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(Config.kArmHighPosWrist);
+    System.out.println("hellow");
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_verticalExtension.setPosition(Config.kArmHighPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY));
+    m_horizontalExtension.setPosition(Config.kArmHighPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY));
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition();
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Config;
+import frc.robot.Subsystems.HorizontalExtension;
+import frc.robot.Subsystems.VerticalExtension;
+import frc.robot.Subsystems.Wrist;
+
+public class ArmHomePosCommand extends CommandBase {
+  private final Wrist m_wrist;
+  private final HorizontalExtension m_horizontalExtension;
+  private final VerticalExtension m_verticalExtension;
+  /** Creates a new ArmHomePosCommand. */
+  public ArmHomePosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) {
+    m_wrist = wristSubsystem;
+    m_verticalExtension = verticalExtensionSubsystem;
+    m_horizontalExtension = horizontalExtensionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(Config.kArmHomePosWrist);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_verticalExtension.setPosition(Config.kArmHomePosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY));
+    m_horizontalExtension.setPosition(Config.kArmHomePosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY));
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition();
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Config;
+import frc.robot.Subsystems.HorizontalExtension;
+import frc.robot.Subsystems.VerticalExtension;
+import frc.robot.Subsystems.Wrist;
+
+public class ArmLowPosCommand extends CommandBase {
+  private final Wrist m_wrist;
+  private final HorizontalExtension m_horizontalExtension;
+  private final VerticalExtension m_verticalExtension;
+  /** Creates a new ArmLowPosCommand. */
+  public ArmLowPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) {
+    m_wrist = wristSubsystem;
+    m_verticalExtension = verticalExtensionSubsystem;
+    m_horizontalExtension = horizontalExtensionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(Config.kArmLowPosWrist);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_verticalExtension.setPosition(Config.kArmLowPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY));
+    m_horizontalExtension.setPosition(Config.kArmHighPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition();
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmMediumPosCommand.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Config;
+import frc.robot.Subsystems.HorizontalExtension;
+import frc.robot.Subsystems.VerticalExtension;
+import frc.robot.Subsystems.Wrist;
+
+public class ArmMediumPosCommand extends CommandBase {
+  private final Wrist m_wrist;
+  private final HorizontalExtension m_horizontalExtension;
+  private final VerticalExtension m_verticalExtension;
+  /** Creates a new ArmsumPosCommand. */
+  public ArmMediumPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) {
+    m_wrist = wristSubsystem;
+    m_verticalExtension = verticalExtensionSubsystem;
+    m_horizontalExtension = horizontalExtensionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(Config.kArmMedPosWrist);
+    System.out.println("hellow");
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_verticalExtension.setPosition(Config.kArmMedPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmMedPosX, Config.kArmMedPosY));
+    m_horizontalExtension.setPosition(Config.kArmMedPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmMedPosX, Config.kArmMedPosY));
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition();
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Config;
+import frc.robot.Subsystems.HorizontalExtension;
+import frc.robot.Subsystems.VerticalExtension;
+import frc.robot.Subsystems.Wrist;
+
+public class ArmShelfPosCommand extends CommandBase {
+  private final Wrist m_wrist;
+  private final HorizontalExtension m_horizontalExtension;
+  private final VerticalExtension m_verticalExtension;
+  /** Creates a new ArmShelfPosCommand. */
+  public ArmShelfPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) {
+    m_wrist = wristSubsystem;
+    m_verticalExtension = verticalExtensionSubsystem;
+    m_horizontalExtension = horizontalExtensionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(Config.kArmShelfPosWrist);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_verticalExtension.setPosition(Config.kArmShelfPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY));
+    m_horizontalExtension.setPosition(Config.kArmShelfPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition();
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/LowerWrist.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Subsystems.Wrist;
+import frc.robot.Subsystems.Wrist.WristPosition;
+
+public class LowerWrist extends CommandBase {
+  private final Wrist m_wrist;
+  /** Creates a new RaiseWrist. */
+  public LowerWrist(Wrist subsystem) {
+    m_wrist = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(WristPosition.LOWERED);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true; //should investigate delay here TODO
+  }
+}

ChargedUp/src/main/java/frc/robot/Commands/Arm/RaiseWrist.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Subsystems.Wrist;
+import frc.robot.Subsystems.Wrist.WristPosition;
+
+public class RaiseWrist extends CommandBase {
+  private final Wrist m_wrist;
+  /** Creates a new RaiseWrist. */
+  public RaiseWrist(Wrist subsystem) {
+    m_wrist = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_wrist.setWristPosition(WristPosition.RAISED);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true; //should investigate delay here TODO
+  }
+}