[WIP] Modeling

.travis.yml

@@ -1,7 +1,11 @@
 language: java
+rust: stable
 
-script:
-  - ./gradlew test
+jobs:
+  include:
+    - stage: "Tests"
+      name: "Unit Tests"
+      script: ./gradlew test
 
 os:
   - osx

build.gradle

@@ -40,9 +40,15 @@ deploy {
 // Set this to true to enable desktop support.
 def includeDesktopSupport = false
 
+repositories {
+    maven { url "https://jitpack.io" }
+}
+
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
 dependencies {
+    compile "com.github.dominikWin:badlog:v0.1.1"
+
     implementation wpi.deps.wpilib()
     nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)

src/main/java/org/team4159/frc/robot/AutoSelector.java

@@ -3,13 +3,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import org.team4159.frc.robot.subsystems.Drivetrain;
-
 public class AutoSelector extends SendableChooser<Command> {
-  public AutoSelector(Drivetrain drivetrain) {
-    Command one = new InstantCommand();
 
-    setDefaultOption("1", one);
-  }
 }

src/main/java/org/team4159/frc/robot/Constants.java

@@ -8,9 +8,10 @@
 
 import org.team4159.lib.hardware.Gearing;
 import org.team4159.lib.hardware.joystick.T16000M;
-import org.team4159.lib.math.Baba;
+import org.team4159.lib.math.MathUtil;
 
 public final class Constants {
+  // not sure where to put these
   public static final class ENCODERS {
     public static final int FALCON_CPR = 2048;
     public static final int MAG_ENCODER_CPR = 4096;
@@ -37,14 +38,14 @@ public static final class BUTTON_IDS {
         public static final int ENABLE_SHOOTER = T16000M.TRIGGER_ID;
         public static final int FLIP_ROBOT_ORIENTATION = T16000M.TOP_MIDDLE_BTN_ID;
         public static final int TOGGLE_ARM = T16000M.TOP_RIGHT_BTN_ID;
-        public static final int RUN_ALL_INTAKE_SUBSYSTEMS = T16000M.PRIMARY_BOTTOM_MIDDLE_BTN_ID;
         public static final int LIMELIGHT_SEEK = T16000M.PRIMARY_TOP_INNER_BTN_ID;
 
         // Debug buttons
         public static final int
           RUN_INTAKE = T16000M.TOP_LEFT_BTN_ID,
           RUN_FEEDER = T16000M.PRIMARY_TOP_OUTER_BTN_ID,
-          RUN_NECK = T16000M.PRIMARY_TOP_MIDDLE_BTN_ID;
+          RUN_NECK = T16000M.PRIMARY_TOP_MIDDLE_BTN_ID,
+          RUN_SHOOTER = T16000M.PRIMARY_BOTTOM_MIDDLE_BTN_ID;
       }
     }
   }
@@ -65,16 +66,16 @@ public static final class CAN_IDS {
     public static final int INTAKE_SPARK = 2; // unknown
     // TODO: Change!
     public static final int LOWER_FEEDER_TALON = 5;
-    public static final int UPPER_FEEDER_SPARK = 6;
-    public static final int NECK_SPARK = 4;
+    public static final int UPPER_FEEDER_SPARK = 4;
+    public static final int NECK_SPARK = 3;
 
     public static final int PIGEON = 0;
   }
 
   public static final class DRIVE_CONSTANTS {
     public static final Gearing GEARING = new Gearing(8.48, ENCODERS.FALCON_CPR);
     public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0);
-    public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * Baba.kTau;
+    public static final double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * MathUtil.kTau;
 
     public static final double METERS_PER_COUNT = WHEEL_CIRCUMFERENCE / GEARING.COUNTS_PER_REV;
 
@@ -100,14 +101,14 @@ public static final class DRIVE_CONSTANTS {
   }
 
   public static final class SHOOTER_CONSTANTS {
-    public static final int COUNTS_PER_SECOND_TO_RPM = ENCODERS.THROUGH_BORE_ENCODER_CPR * 60;
+    public static final double  COUNTS_PER_SECOND_TO_RPM = (1.0 / ENCODERS.THROUGH_BORE_ENCODER_CPR) * 60;
 
     @SuppressWarnings("PointlessArithmeticExpression")
     // 1 RPM i think
     public static final int ACCEPTABLE_SPEED_ERROR = 1 * ENCODERS.THROUGH_BORE_ENCODER_CPR / 60; // counts per second
 
-    public static final int ENCODER_CHANNEL_A_PORT = 2;
-    public static final int ENCODER_CHANNEL_B_PORT = 3;
+    public static final int ENCODER_CHANNEL_A_PORT = 4;
+    public static final int ENCODER_CHANNEL_B_PORT = 5;
     public static final boolean IS_ENCODER_REVERSED = true;
     public static final EncodingType ENCODER_ENCODING_TYPE = EncodingType.k4X;
 
@@ -185,7 +186,8 @@ public static final class INTAKE_CONSTANTS {
   }
 
   public static final class FEEDER_CONSTANTS {
-    public static final double FEEDING_SPEED = 1;
+    public static final double FLOOR_FEEDING_SPEED = 1;
+    public static final double TOWER_FEEDING_SPEED = 0.3;
   }
 
   public static final class FIELD_CONSTANTS {

src/main/java/org/team4159/frc/robot/Robot.java

@@ -16,6 +16,16 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
   }
 
+  @Override
+  public void autonomousInit() {
+    robot_container.zeroSubsystems();
+  }
+
+  @Override
+  public void teleopInit() {
+    robot_container.zeroSubsystems();
+  }
+
   @Override
   public void teleopPeriodic() {
     robot_container.updateSubsystemInputs();

src/main/java/org/team4159/frc/robot/RobotContainer.java

@@ -2,14 +2,11 @@
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj2.command.*;
 
-import org.team4159.frc.robot.controllers.IntakeManager;
 import org.team4159.lib.control.signal.DriveSignal;
-import org.team4159.lib.hardware.Limelight;
+import org.team4159.frc.robot.controllers.complex.IntakeController;
 import org.team4159.frc.robot.subsystems.*;
+import org.team4159.lib.hardware.Limelight;
 
 import static org.team4159.frc.robot.Constants.*;
 
@@ -22,47 +19,40 @@ public class RobotContainer {
   private final Arm arm = new Arm();
   private final Turret turret = new Turret();
 
-  private final IntakeManager intake_controller = new IntakeManager(arm, intake, feeder);
+  private final Limelight limelight = Limelight.getDefault();
+
+  private final IntakeController intake_controller = new IntakeController(arm.getController(), intake, feeder);
 
   private final Joystick left_joy = new Joystick(CONTROLS.LEFT_JOY.USB_PORT);
   private final Joystick right_joy = new Joystick(CONTROLS.RIGHT_JOY.USB_PORT);
   private final Joystick secondary_joy = new Joystick(CONTROLS.SECONDARY_JOY.USB_PORT);
 
- private final AutoSelector auto_selector = new AutoSelector(drivetrain);
+  private final AutoSelector auto_selector = new AutoSelector();
 
   public RobotContainer() {
-    configureCamera();
+    configureCameras();
   }
 
-  private void configureCamera() {
-    CameraServer.getInstance().startAutomaticCapture();
+  private void configureCameras() {
+    //CameraServer.getInstance().startAutomaticCapture();
+    limelight.setLEDMode(Limelight.LEDMode.ForceOff);
   }
 
-  private void configureButtonBindings() {
-//    new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.BUTTON_IDS.ENABLE_SHOOTER)
-//      .whenPressed(() -> shooter.setTargetSpeed(SmartDashboard.getNumber("target_shooter_speed", 0)))
-//      .whenReleased(new InstantCommand(shooter::stop, shooter));
-
-    new JoystickButton(secondary_joy, CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_ALL_INTAKE_SUBSYSTEMS)
-      .whenPressed(new ParallelCommandGroup(
-        new InstantCommand(intake::intake, intake),
-        new InstantCommand(feeder::feed, feeder),
-        new InstantCommand(() -> shooter.setRawSpeed(1), shooter)))
-      .whenReleased(new ParallelCommandGroup(
-        new InstantCommand(intake::stop, intake),
-        new InstantCommand(feeder::stop, feeder),
-        new InstantCommand(() -> shooter.setRawSpeed(0), shooter)));
+  public void zeroSubsystems() {
+    //arm.getController().startZeroing();
+    //turret.getController().startZeroing();
   }
 
   public void updateSubsystemInputs() {
     updateDrivetrainInputs();
-    updateFeederInputs();
 
     // TODO: switch to IntakeController when done testing in isolation
-    updateArmInputs();
+    // updateArmInputs();
     updateIntakeInputs();
+    updateFeederInputs();
 
     updateNeckInputs();
+    updateShooterInputs();
   }
 
   public void updateControllerInputs() {
@@ -71,16 +61,16 @@ public void updateControllerInputs() {
 
   public void updateArmInputs() {
     if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.TOGGLE_ARM)) {
-      arm.setSetpoint(Math.abs(arm.getSetpoint() - ARM_CONSTANTS.RANGE_IN_COUNTS));
+      arm.getController().setSetpoint(Math.abs(arm.getController().getSetpoint() - ARM_CONSTANTS.RANGE_IN_COUNTS));
     }
   }
 
   public void updateDrivetrainInputs() {
     if (secondary_joy.getRawButtonPressed(CONTROLS.SECONDARY_JOY.BUTTON_IDS.FLIP_ROBOT_ORIENTATION)) {
-      drivetrain.flipOrientation();
+      drivetrain.getController().flipDriveOrientation();
     }
 
-    drivetrain.drive(new DriveSignal(left_joy.getY(), right_joy.getY()));
+    drivetrain.getController().demandSignal(new DriveSignal(left_joy.getY(), right_joy.getY()));
   }
 
   public void updateFeederInputs() {
@@ -107,6 +97,15 @@ public void updateNeckInputs() {
     }
   }
 
+  public void updateShooterInputs() {
+    if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_SHOOTER)) {
+      shooter.setRawSpeed(1);
+      shooter.getController().writeToSmartDashboard();
+    } else {
+      shooter.stop();
+    }
+  }
+
   public void updateIntakeControllerInputs() {
     if (secondary_joy.getRawButton(CONTROLS.SECONDARY_JOY.BUTTON_IDS.RUN_INTAKE)) {
       intake_controller.intake();

src/main/java/org/team4159/frc/robot/controllers/ArmController.java

@@ -0,0 +1,68 @@
+package org.team4159.frc.robot.controllers;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import org.team4159.frc.robot.subsystems.Arm;
+import org.team4159.lib.control.ControlLoop;
+
+import static org.team4159.frc.robot.Constants.*;
+
+public class ArmController implements ControlLoop {
+  private enum State {
+    IDLE,
+    ZEROING,
+    CLOSED_LOOP,
+    ESTOP
+  }
+  private State state = State.IDLE;
+
+  private Arm arm;
+
+  private PIDController pid_controller;
+
+  public ArmController(Arm arm) {
+    this.arm = arm;
+
+    pid_controller = new PIDController(
+      ARM_CONSTANTS.kP,
+      ARM_CONSTANTS.kI,
+      ARM_CONSTANTS.kD
+    );
+    pid_controller.setTolerance(ARM_CONSTANTS.ACCEPTABLE_ERROR_IN_COUNTS);
+  }
+
+  @Override
+  public void update() {
+    switch (state) {
+      case IDLE:
+        break;
+      case ZEROING:
+        arm.setRawSpeed(ARM_CONSTANTS.ZEROING_SPEED);
+        if (arm.isLimitSwitchClosed()) {
+          arm.zeroEncoder();
+          state = State.CLOSED_LOOP;
+        }
+        break;
+      case CLOSED_LOOP:
+        double output = pid_controller.calculate(arm.getPosition());
+        arm.setRawVoltage(output);
+        break;
+    }
+  }
+
+  public boolean isAtSetpoint() {
+    return pid_controller.atSetpoint();
+  }
+
+  public void startZeroing() {
+    state = State.ZEROING;
+  }
+
+  public void setSetpoint(int setpoint) {
+    pid_controller.setSetpoint(setpoint);
+  }
+
+  public int getSetpoint() {
+    return (int) pid_controller.getSetpoint();
+  }
+}