Se/elevator

simgui.json

@@ -1,8 +1,24 @@
 {
+  "HALProvider": {
+    "Encoders": {
+      "window": {
+        "visible": true
+      }
+    },
+    "PWM Outputs": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Visualizers/Elevator": {
       "/SmartDashboard/algae": "Mechanism2d"
     },
     "windows": {
@@ -25,7 +41,10 @@
         "open": true
       },
       "SmartDashboard": {
+        "Elevator": {
+
         "algae": {
+
           "open": true
         },
         "open": true
@@ -37,8 +56,22 @@
       "open": true
     },
     "Server": {
+
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      }
+    },
+    "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
+
       "open": true
     },
     "visible": true
+
   }
 }

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,12 +5,23 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.constants.Settings.RobotType;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
 
+    public static final RobotType ROBOT;
+
+    static {
+        if (Robot.isSimulation())
+            ROBOT = RobotType.SIM;
+        else
+            ROBOT = RobotType.fromString(System.getenv("serialnum"));
+    }
+
     private RobotContainer robot;
     private Command auto;
 

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,6 +5,15 @@
 
 package com.stuypulse.robot;
 
+import java.lang.annotation.ElementType;
+import com.stuypulse.robot.commands.Elevator.ElevatorToBottom;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl1;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl2;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl3;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl4;
+import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
 import javax.management.openmbean.OpenType;
 
 // Algae Commands
@@ -34,13 +43,14 @@ public class RobotContainer {
     public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER);
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
-    // Subsystem
+    // Subsystems
+    public final Elevator elevator = Elevator.getInstance();
     public final Algae algae = Algae.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
-    // Robot container
+    // Robot Container
 
     public RobotContainer() {
         configureDefaultCommands();
@@ -58,16 +68,32 @@ private void configureDefaultCommands() {}
     /*** BUTTONS ***/
     /***************/
 
-
     private void configureButtonBindings() {
         configureDriverBindings();
         configureOperatorBindings();
     }
 
-    private void configureDriverBindings() {}
+    private void configureDriverBindings() {
+
+    }
 
     private void configureOperatorBindings() {
 
+        operator.getDPadDown()
+            .onTrue(new ElevatorToLvl1());
+
+        operator.getDPadLeft()
+            .onTrue(new ElevatorToLvl2());
+
+        operator.getDPadRight()
+            .onTrue(new ElevatorToLvl3());    
+
+        operator.getDPadUp()
+            .onTrue(new ElevatorToLvl4());
+
+        operator.getLeftButton()
+            .onTrue(new ElevatorToBottom());
+
         operator.getBottomButton()
             .onTrue(new AlgaeStow());
 

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToBottom extends ElevatorToHeight {
+
+    public ElevatorToBottom() {
+        super(Elevator.MIN_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java

@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;   
+
+public class ElevatorToHeight extends InstantCommand {
+    private final Elevator elevator;
+    private final double targetHeight;
+
+    public ElevatorToHeight(double targetHeight){
+        elevator = Elevator.getInstance();
+        this.targetHeight = targetHeight;
+
+        addRequirements(elevator);
+    }
+
+    public void initialize(){
+        elevator.setTargetHeight(targetHeight);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl1 extends ElevatorToHeight{
+    public ElevatorToLvl1(){
+        super(Elevator.L1);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl2 extends ElevatorToHeight {
+    public ElevatorToLvl2(){
+        super(Elevator.L2);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl3 extends ElevatorToHeight {
+    public ElevatorToLvl3(){
+        super(Elevator.L3);
+    }
+}

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl4 extends ElevatorToHeight{
+    public ElevatorToLvl4(){
+        super(Elevator.L4);
+    }
+}

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -13,6 +13,12 @@ public interface Gamepad {
         int DEBUGGER = 2;
     }
 
+    public interface Elevator {
+        int LEFT = 0;
+        int RIGHT = 1;
+        int SWITCH = 2;
+    }
+
     public interface Algae {
         int ROLLER_ID = 0; // update ports later -- def won't be 0
         int PIVOT_ID = 1; // update ports later

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -5,9 +5,10 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.util.Units;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
@@ -16,6 +17,71 @@
  */
 public interface Settings {
 
+    // checks the current RIO's serial number to determine which robot is running
+    public enum RobotType {
+
+        AUNT_MARY("0000000"),
+        SIM("");
+
+        public final String serialNum;
+
+        RobotType(String serialNum) {
+            this.serialNum = serialNum;
+        }
+
+        public static RobotType fromString(String serialNum) {
+            for (RobotType robot : RobotType.values()) {
+                if (robot.serialNum.equals(serialNum.toUpperCase())) {
+                    return robot;
+                }
+            }
+
+            return RobotType.SIM;
+        }
+    }
+
+    double DT = 0.020;
+
+    public interface Robot {
+        double kG = 100.0;
+    }
+
+    public interface Elevator {
+        double MIN_HEIGHT = 0.0;
+        double MAX_HEIGHT = 12.0;
+        double MAX_ACCELERATION = 2.0;
+        double MAX_VELOCITY = 3.0;
+        double ENCODER_CONVERSION_FACTOR = 0;
+
+        double MASS = 25.0;
+        double GEARING = 1.0/9.0;
+        double DRUM_RADIUS = Units.inchesToMeters(1.0);
+
+        double L1 = 1;
+        double L2 = 2;
+        double L3 = 3;
+        double L4 = 4;
+
+        double POSITION_CONVERSION_FACTOR = 1.0;
+        double VELOCITY_CONVERSION_FACTOR = 1.0;
+
+        double SCALE_FACTOR = 0.5;
+
+        public interface PID {
+            SmartNumber kP = new SmartNumber("kP",1.5);
+            SmartNumber kI = new SmartNumber("kI",0.0);
+            SmartNumber kD = new SmartNumber("kD",0.2);
+        }
+
+        public interface FF {
+            SmartNumber kS = new SmartNumber("kS",0.20506);
+            SmartNumber kV = new SmartNumber("kV",3.7672);
+            SmartNumber kA = new SmartNumber("kA", 0.27);
+            SmartNumber kG = new SmartNumber("kG", 0.37);
+        }
+    }
+}
+
     public interface Algae {
         double RAISED_ANGLE = 0.0;          // CHANGE
         double PROCESSOR_ANGLE = 0.0;       // CHANGE 
@@ -44,4 +110,3 @@ public interface FF{
         }
     }
 }
-

src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java

@@ -0,0 +1,45 @@
+package com.stuypulse.robot.subsystems.Elevator;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Settings.RobotType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Elevator extends SubsystemBase {
+
+    private static final Elevator instance;
+
+    static {
+        if (Robot.ROBOT == RobotType.AUNT_MARY) {
+            instance = new ElevatorImpl();
+        } else {
+            instance = new ElevatorSimu();
+        }
+    }
+
+    private final ElevatorVisualizer visualizer;
+
+    public static Elevator getInstance() {
+        return instance;
+    }    
+
+    public Elevator() {
+        visualizer = new ElevatorVisualizer();
+    }
+
+    public abstract void setTargetHeight(double height);
+
+    public abstract double getTargetHeight();
+
+    public abstract double getHeight();
+
+    public abstract void stopElevator();
+
+    public abstract boolean atBottom();
+
+    public void periodic() {
+        visualizer.update();
+    }
+
+}
+