Merge pull request #2 from StuyPulse/se/elevator

Se/elevator

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,29 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Visualizers/Elevator": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Elevator": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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

@@ -5,6 +5,8 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.commands.elevator.ElevatorToBottom;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -15,12 +17,15 @@ public class Robot extends TimedRobot {
     private Command auto;
 
     /*************************/
-    /*** ROBOT SCHEDULEING ***/
+    /*** ROBOT SCHEDULING ***/
     /*************************/
 
     @Override
     public void robotInit() {
         robot = new RobotContainer();
+
+        // Check with Philip to automatically reset elevator encoder
+        // new ElevatorToBottom().schedule();
     }
 
     @Override

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

@@ -9,6 +9,7 @@
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
+import com.stuypulse.robot.subsystems.elevator.Elevator;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -21,6 +22,7 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final Elevator elevator = Elevator.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();

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

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.elevator;
+
+import com.stuypulse.robot.constants.Constants.Elevator;
+
+public class ElevatorToBottom extends ElevatorToHeight{
+    public ElevatorToBottom(){
+        super(Elevator.MIN_HEIGHT_METERS);
+    }
+}

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java

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

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

@@ -0,0 +1,29 @@
+package com.stuypulse.robot.commands.elevator;
+
+import com.stuypulse.robot.subsystems.elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand; 
+import edu.wpi.first.wpilibj2.command.Command;  
+
+public class ElevatorToHeight extends InstantCommand {
+
+    public static Command untilDone(double height) {
+        return new ElevatorToHeight(height)
+            .andThen(new WaitUntilCommand(() -> Elevator.getInstance().atTargetHeight()));
+    }
+
+    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/ElevatorToLvl2Alt.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java

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

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.elevator;
+
+import com.stuypulse.robot.constants.Constants;
+
+public class ElevatorToTop extends ElevatorToHeight{
+    public ElevatorToTop() {
+        super(Constants.Elevator.MAX_HEIGHT_METERS);
+    }
+}

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

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.constants;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+    public interface Elevator {
+        double MIN_HEIGHT_METERS = Units.inchesToMeters(9.09375); // FROM THE BOTTOM OF FIXED STAGE TO TOP OF CARRIAGE
+        double MAX_HEIGHT_METERS = Units.inchesToMeters(77); // FROM THE BOTTOM OF FIXED STAGE TO TOP ELEVATOR
+
+        double MASS_KG = 10.0;
+        double DRUM_RADIUS_METERS = (MAX_HEIGHT_METERS / Encoders.NUM_ROTATIONS_TO_REACH_TOP * Encoders.GEARING) / 2 / Math.PI;
+
+        public interface Encoders {
+            double GEARING = 4.0;
+
+            double NUM_ROTATIONS_TO_REACH_TOP = (6 + 9.0 / 24) * GEARING; // Number of rotations that the motor has to spin, NOT the gear
+
+            double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP;
+            double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60;
+
+            double DISTANCE_PER_ROTATION = 0.0;
+        }
+    }
+}

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

@@ -12,4 +12,10 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Elevator {
+        int MOTOR = 0;
+        int BOTTOM_SWITCH = 1;
+        int TOP_SWITCH = 2;
+    }
 }

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

@@ -5,7 +5,6 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
 /*-
@@ -14,4 +13,54 @@
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-public interface Settings {}
+public interface Settings {
+
+    double DT = 0.020; // 20ms Differential Time
+
+    public interface Elevator {
+        SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0);
+        SmartNumber MAX_ACCEL_METERS_PER_SECOND_PER_SECOND = new SmartNumber("Elevator/Max Accel (m per s^2)", 2.0);
+
+        // CHANGE
+        double HANDOFF_HEIGHT_METERS = 0.1;
+        // front and funnel
+        double ALT_L2_HEIGHT_METERS = 0.25;
+        double ALT_L3_HEIGHT_METERS = 0.5;
+        double ALT_L4_HEIGHT_METERS = 0.75;
+        double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2
+        double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3
+        double FUNNEL_L4_HEIGHT_METERS = 0.8;
+
+        double RAMP_RATE_CURRENT = 1.0;
+        double RAMP_RATE_VOLTAGE = 0.1;
+
+        // FIND OUT REAL GEAR RATIO
+        double GEAR_RATIO = 1.0/5.0;
+        double CURRENT_LIMIT = 5.0;
+
+        // Magic motion, change RPS
+        double TARGET_CRUISE_VELOCITY = 0.0; //Rotations Per Second
+        double TARGET_ACCELERATION = 0.0; //Rotations Per Second^2
+        double TARGET_JERK = 0.0; //Rotations Per Second^3
+
+        SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02);
+
+        public interface PID {
+            //tune
+            SmartNumber kP = new SmartNumber("Elevator/Controller/kP",0.0);
+            SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0);
+            SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.0);
+        }
+
+        public interface FF {
+            SmartNumber kS = new SmartNumber("Elevator/Controller/kS",0.20506);
+            SmartNumber kV = new SmartNumber("Elevator/Controller/kV",3.7672);
+            SmartNumber kA = new SmartNumber("Elevator/Controller/kA", 0.27);
+            SmartNumber kG = new SmartNumber("Elevator/Controller/kG", 1.37);
+        }
+
+        public interface Simulation {
+            double SCALE_FACTOR = 0.5 + 2.5/77;
+        }
+    }
+}