Merge pull request #8 from strykeforce/superStructure

Initial Super structure

.OutlineViewer/outlineviewer.json

@@ -0,0 +1,23 @@
+{
+  "Clients": {
+    "open": true
+  },
+  "Connections": {
+    "open": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "10.27.67.2"
+  },
+  "Server": {
+    "open": true
+  },
+  "WallEye_Client@1": {
+    "open": true
+  },
+  "transitory": {
+    "AdvantageKit": {
+      "open": true
+    }
+  }
+}

src/main/java/frc/robot/constants/SuperStructureConstants.java

@@ -0,0 +1,40 @@
+package frc.robot.constants;
+
+public class SuperStructureConstants {
+  // SEQUENCE
+  public static final double kElbowMinToMoveWrist = 0.0;
+  public static final double kWristMinToMoveElbow = 0.0;
+
+  // SHOOT
+  public static final double kWristShootSetPoint = 0.0;
+
+  // AMP
+  public static final double kWristAmpSetPoint = 0.0;
+  public static final double kElbowAmpSetPoint = 0.0;
+  public static final double kShooterAmpSetPoint = 0.0;
+
+  // PRE-CLIMB
+  public static final double kWristPreClimbSetPoint = 0.0;
+  public static final double kElbowPreClimbSetPoint = 0.0;
+  public static final double kShooterPreClimbSetPoint = 0.0;
+
+  // TRAP
+  public static final double kWristTrapSetPoint = 0.0;
+  public static final double kElbowTrapSetPoint = 0.0;
+  public static final double kShooterTrapSetPoint = 0.0;
+
+  // POST-CLIMB
+  public static final double kWristPostClimbSetPoint = 0.0;
+  public static final double kElbowPostClimbSetPoint = 0.0;
+  public static final double kShooterPostClimbSetPoint = 0.0;
+
+  // INTAKE
+  public static final double kWristIntakeSetPoint = 0.0;
+  public static final double kElbowIntakeSetPoint = 0.0;
+  public static final double kShooterIntakeSetPoint = 0.0;
+
+  // DEFENSE
+  public static final double kWristDefenseSetPoint = 0.0;
+  public static final double kElbowDefenseSetPoint = 0.0;
+  public static final double kShooterDefenseSetPoint = 0.0;
+}

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -5,9 +5,13 @@
 import frc.robot.subsystems.elbow.ElbowSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
+import frc.robot.subsystems.superStructure.SuperStructure;
 import frc.robot.subsystems.vision.VisionSubsystem;
+import java.util.Set;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
 
-public class RobotStateSubsystem {
+public class RobotStateSubsystem extends MeasurableSubsystem {
 
   public Alliance getAllianceColor() {
     return Alliance.Red;
@@ -20,19 +24,22 @@ public Alliance getAllianceColor() {
   ShooterSubsystem shooterSubsystem;
   IntakeSubsystem intakeSubsystem;
   ElbowSubsystem elbowSubsystem;
+  SuperStructure superStructure;
 
   // Constructor
   public RobotStateSubsystem(
       VisionSubsystem visionSubsystem,
       DriveSubsystem driveSubsystem,
       ShooterSubsystem shooterSubsystem,
       IntakeSubsystem intakeSubsystem,
-      ElbowSubsystem elbowSubsystem) {
+      ElbowSubsystem elbowSubsystem,
+      SuperStructure superStructure) {
     this.visionSubsystem = visionSubsystem;
     this.driveSubsystem = driveSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.intakeSubsystem = intakeSubsystem;
     this.elbowSubsystem = elbowSubsystem;
+    this.superStructure = superStructure;
   }
 
   // Getter/Setter Methods
@@ -42,6 +49,10 @@ public RobotStateSubsystem(
   // Periodic
 
   // Grapher
+  @Override
+  public Set<Measure> getMeasures() {
+    return null;
+  }
 
   // State
   public enum RobotStates {}

src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java

@@ -0,0 +1,218 @@
+package frc.robot.subsystems.superStructure;
+
+import frc.robot.constants.SuperStructureConstants;
+import frc.robot.subsystems.elbow.ElbowSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
+import frc.robot.subsystems.wrist.WristSubsystem;
+import java.util.Set;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class SuperStructure extends MeasurableSubsystem {
+  // Private Variables
+  WristSubsystem wristSubsystem;
+  ElbowSubsystem elbowSubsystem;
+  ShooterSubsystem shooterSubsystem;
+  Logger logger;
+  SuperStructureStates curState = SuperStructureStates.IDLE;
+  SuperStructureStates nextState = SuperStructureStates.IDLE;
+  private double shooterSpeed = 0.0;
+  private double elbowSetpoint = 0.0;
+  private double wristSetpoint = 0.0;
+  private boolean flipMagazineOut = false;
+
+  // Constructor
+  public SuperStructure(
+      WristSubsystem wristSubsystem,
+      ElbowSubsystem elbowSubsystem,
+      ShooterSubsystem shooterSubsystem) {
+    this.elbowSubsystem = elbowSubsystem;
+    this.wristSubsystem = wristSubsystem;
+    this.shooterSubsystem = shooterSubsystem;
+    logger = LoggerFactory.getLogger(this.getClass());
+  }
+
+  // Getter/Setter Methods
+  public boolean isFinished() {
+    return elbowSubsystem.isFinished() && wristSubsystem.isFinished() && shooterSubsystem.atSpeed();
+  }
+
+  public SuperStructureStates getState() {
+    return curState;
+  }
+
+  // public void setState(SuperStructureStates state) {
+  //   if (state != SuperStructureStates.IDLE) {
+  //     curState = SuperStructureStates.TRANSFER;
+  //     nextState = state;
+  //   } else {
+  //     curState = state;
+  //   }
+  // }
+
+  // Helper Methods
+
+  // Basic methods to go to each position
+  //    Works by setting each axes' setpoint and starting intial movement
+  //      Then determines if elbow or wrist go first by flipMagazineOut boolean
+  public void shoot(double shooterSpeed, double elbowSetpoint) {
+    this.elbowSetpoint = elbowSetpoint;
+    this.shooterSpeed = shooterSpeed;
+    wristSetpoint = SuperStructureConstants.kWristShootSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    wristSubsystem.setPosition(wristSetpoint);
+
+    logger.info("{} -> TRANSFER(SHOOTING)");
+    flipMagazineOut = false;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.SHOOTING;
+  }
+
+  public void preClimb() {
+    elbowSetpoint = SuperStructureConstants.kElbowPreClimbSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristPreClimbSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    wristSubsystem.setPosition(wristSetpoint);
+
+    logger.info("{} -> TRANSFER(PRE_CLIMB)");
+    flipMagazineOut = false;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.PRE_CLIMB;
+  }
+
+  public void postClimb() {
+    elbowSetpoint = SuperStructureConstants.kElbowPostClimbSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristPostClimbSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    wristSubsystem.setPosition(wristSetpoint);
+
+    logger.info("{} -> TRANSFER(POST_CLIMB)");
+    flipMagazineOut = false;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.POST_CLIMB;
+  }
+
+  public void amp() {
+    elbowSetpoint = SuperStructureConstants.kElbowAmpSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristAmpSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterAmpSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    elbowSubsystem.setPosition(elbowSetpoint);
+
+    logger.info("{} -> TRANSFER(AMP)");
+    flipMagazineOut = true;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.AMP;
+  }
+
+  public void trap() {
+    elbowSetpoint = SuperStructureConstants.kElbowTrapSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristTrapSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterTrapSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    elbowSubsystem.setPosition(elbowSetpoint);
+
+    logger.info("{} -> TRANSFER(TRAP)");
+    flipMagazineOut = true;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.TRAP;
+  }
+
+  public void intake() {
+    elbowSetpoint = SuperStructureConstants.kElbowIntakeSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristIntakeSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterIntakeSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    wristSubsystem.setPosition(wristSetpoint);
+
+    logger.info("{} -> TRANSFER(INTAKE)");
+    flipMagazineOut = false;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.INTAKE;
+  }
+
+  public void defense() {
+    elbowSetpoint = SuperStructureConstants.kElbowDefenseSetPoint;
+    wristSetpoint = SuperStructureConstants.kWristDefenseSetPoint;
+    shooterSpeed = SuperStructureConstants.kShooterDefenseSetPoint;
+
+    shooterSubsystem.setSpeed(shooterSpeed);
+    elbowSubsystem.setPosition(elbowSetpoint);
+
+    logger.info("{} -> TRANSFER(DEFENSE)");
+    flipMagazineOut = true;
+    curState = SuperStructureStates.TRANSFER;
+    nextState = SuperStructureStates.DEFENSE;
+  }
+
+  // Periodic
+  @Override
+  public void periodic() {
+    switch (curState) {
+      case IDLE:
+        break;
+      case TRANSFER:
+
+        // Logic to determine how to move axis based on what the final position is
+        if (flipMagazineOut) {
+          if (elbowSubsystem.getPosition() > SuperStructureConstants.kElbowMinToMoveWrist) {
+            wristSubsystem.setPosition(wristSetpoint);
+          }
+        } else {
+          if (wristSubsystem.getPosition() > SuperStructureConstants.kWristMinToMoveElbow) {
+            elbowSubsystem.setPosition(elbowSetpoint);
+          }
+        }
+
+        // Once all subsystems are at position go into the desired state
+        if (isFinished()) {
+          logger.info("TRANSFER -> {}", nextState);
+          curState = nextState;
+        }
+        break;
+
+      case SHOOTING:
+        break;
+      case AMP:
+        break;
+      case PRE_CLIMB:
+        break;
+      case TRAP:
+        break;
+      case POST_CLIMB:
+        break;
+      case INTAKE:
+        break;
+      case DEFENSE:
+        break;
+    }
+  }
+  // Grapher
+  @Override
+  public Set<Measure> getMeasures() {
+    return null;
+  }
+
+  // State Enum
+  public enum SuperStructureStates {
+    IDLE,
+    SHOOTING,
+    AMP,
+    PRE_CLIMB,
+    TRAP,
+    POST_CLIMB,
+    TRANSFER,
+    INTAKE,
+    DEFENSE
+  }
+}