Merge pull request #28 from blair-robot-project/mvp-testing

Update after DC Week 2 Event (3/13)

Driver.vpr

@@ -0,0 +1,68 @@
+area_max:100
+area_min:0.0017850625000000004
+area_similarity:0
+aspect_max:20.000000
+aspect_min:0.000000
+black_level:0
+blue_balance:1975
+calibration_type:0
+contour_grouping:0
+contour_sort_final:0
+convexity_max:100
+convexity_min:10
+corner_approx:5.000000
+crop_x_max:1.000000
+crop_x_min:-1.000000
+crop_y_max:1.000000
+crop_y_min:-1.000000
+cross_a_a:1
+cross_a_x:0
+cross_a_y:0
+cross_b_a:1
+cross_b_x:0
+cross_b_y:0
+desc:Driver
+desired_contour_region:0
+dilation_steps:0
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:0
+exposure:177
+force_convex:1
+hue_max:86
+hue_min:42
+image_flip:1
+image_source:0
+img_to_show:0
+intersection_filter:0
+invert_hue:0
+multigroup_max:7
+multigroup_min:1
+multigroup_rejector:0
+pipeline_led_enabled:0
+pipeline_led_power:60
+pipeline_res:0
+pipeline_type:0
+red_balance:1200
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:70
+send_corners:0
+send_raw_contours:0
+solve3d:0
+solve3d_algo:0
+solve3d_bindtarget:1
+solve3d_conf:0.990000
+solve3d_error:8
+solve3d_guess:0
+solve3d_iterations:50
+solve3d_precies:1
+solve3d_precise:0
+solve3d_zoffset:0.000000
+val_max:255
+val_min:70
+x_outlier_miqr:1.5
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:1.5

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id 'java'
     id 'idea'
-    id "edu.wpi.first.GradleRIO" version "2022.3.1"
+    id "edu.wpi.first.GradleRIO" version "2022.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.team449.Main"
@@ -38,8 +38,6 @@ deploy {
 def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 
 dependencies {
-    implementation group: 'com.google.guava', name: 'guava', version: '31.0-jre'
-
     def jacksonVersion = '2.12.5'
     implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: jacksonVersion
     implementation group: 'com.fasterxml.jackson.core', name: 'jackson-core', version: jacksonVersion

src/main/deploy/pathplanner/1ball blue 2.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":7.614591741639,"y":2.957976022713611},"prevControl":null,"nextControl":{"x":7.311960531394373,"y":2.1867545514450453},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.584033943868601,"y":1.962221718037742},"prevControl":{"x":6.335730820927835,"y":1.962221718037742},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":2.0,"maxAcceleration":1.0,"isReversed":false}
+{"waypoints":[{"anchorPoint":{"x":6.130722581729864,"y":5.24235354520531},"prevControl":null,"nextControl":{"x":5.857378262799233,"y":5.496173269926609},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.359501110461299,"y":5.935476639636551},"prevControl":{"x":5.683246458675133,"y":5.604532312332696},"nextControl":{"x":5.683246458675133,"y":5.604532312332696},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.038616212463745,"y":4.510181262355406},"prevControl":{"x":7.584761353049779,"y":3.990094273727063},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.2,"isReversed":false}

src/main/java/frc/team449/Robot.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.team449.javaMaps.FullMap;
-import frc.team449.javaMaps.IntakeTestMap;
 import frc.team449.other.Clock;
 import frc.team449.other.Updater;
 import io.github.oblarg.oblog.Logger;
@@ -22,7 +21,7 @@ public class Robot extends TimedRobot {
 
   /** The method that runs when the robot is turned on. Initializes all subsystems from the map. */
   public static @NotNull RobotMap loadMap() {
-    return IntakeTestMap.createRobotMap();
+    return FullMap.createRobotMap();
   }
 
   /**

src/main/java/frc/team449/RobotMap.java

@@ -2,10 +2,9 @@
 
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import frc.team449.generalInterfaces.MotorContainer;
+import frc.team449.motor.MotorContainer;
 import frc.team449.wrappers.PDP;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;

src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.jetbrains.annotations.NotNull;
 
 public class Cargo2022 extends SubsystemBase {
   /** The leader motor for the intake */
@@ -17,9 +18,9 @@ public class Cargo2022 extends SubsystemBase {
   private final double spitterSpeed;
 
   public Cargo2022(
-      MotorController intakeMotor,
-      MotorController spitterMotor,
-      DoubleSolenoid deployIntake,
+      @NotNull MotorController intakeMotor,
+      @NotNull MotorController spitterMotor,
+      @NotNull DoubleSolenoid deployIntake,
       double intakeSpeed,
       double spitterSpeed) {
     this.intakeMotor = intakeMotor;
@@ -35,8 +36,8 @@ public void runIntake() {
   }
 
   public void runIntakeReverse() {
-    intakeMotor.set(intakeSpeed);
-    spitterMotor.set(spitterSpeed);
+    intakeMotor.set(-intakeSpeed);
+    spitterMotor.set(-spitterSpeed);
   }
 
   public void spit() {
@@ -50,10 +51,11 @@ public void stop() {
   }
 
   public void deployIntake() {
-    deployIntake.set(DoubleSolenoid.Value.kForward);
+    this.deployIntake.set(DoubleSolenoid.Value.kReverse);
   }
 
   public void retractIntake() {
-    deployIntake.set(DoubleSolenoid.Value.kReverse);
+    this.deployIntake.set(DoubleSolenoid.Value.kForward);
   }
+
 }

src/main/java/frc/team449/_2022robot/cargo/IntakeLimelightCommand.java

@@ -0,0 +1,50 @@
+package frc.team449._2022robot.cargo;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.team449.generalInterfaces.limelight.Limelight;
+
+public class IntakeLimelightCommand extends CommandBase {
+  private final Cargo2022 cargo;
+  private final Limelight limelight;
+  private final int bluePipeline;
+  private final int redPipeline;
+
+  /**
+   *
+   * @param cargo
+   * @param limelight
+   * @param bluePipeline Pipeline for detecting blue balls
+   * @param redPipeline Pipeline for detecting red balls
+   */
+  public IntakeLimelightCommand(Cargo2022 cargo, Limelight limelight, int bluePipeline, int redPipeline) {
+    addRequirements(cargo);
+    this.cargo = cargo;
+    this.limelight = limelight;
+    this.bluePipeline = bluePipeline;
+    this.redPipeline = redPipeline;
+  }
+
+  @Override
+  public void initialize() {
+    var isBlue = DriverStation.getAlliance() == DriverStation.Alliance.Blue;
+    if (isBlue) {
+      limelight.setPipeline(redPipeline);
+    } else {
+      limelight.setPipeline(bluePipeline);
+    }
+  }
+
+  @Override
+  public void execute() {
+    if (limelight.hasTarget()) {
+      cargo.stop();
+    } else {
+      cargo.runIntake();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+  }
+}

src/main/java/frc/team449/_2022robot/climber/ClimberArm.java

@@ -1,64 +1,68 @@
 package frc.team449._2022robot.climber;
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj2.command.PIDSubsystem;
-import frc.team449.generalInterfaces.updatable.Updatable;
-import frc.team449.other.Updater;
-import frc.team449.wrappers.WrappedMotor;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+import frc.team449.motor.WrappedMotor;
+import io.github.oblarg.oblog.Loggable;
+import io.github.oblarg.oblog.annotations.Log;
 import org.jetbrains.annotations.NotNull;
 
-public class ClimberArm extends PIDSubsystem {
+public class ClimberArm extends ProfiledPIDSubsystem implements Loggable {
   private final WrappedMotor motor;
   private final ElevatorFeedforward feedforward;
 
   public ClimberArm(
       @NotNull WrappedMotor motor,
-      @NotNull PIDController controller,
-      //todo feedforward isn't needed right now, so maybe only add later?
+      @NotNull ProfiledPIDController controller,
       @NotNull ElevatorFeedforward feedforward) {
     super(controller);
     this.motor = motor;
     this.feedforward = feedforward;
   }
 
   @Override
-  protected void useOutput(double output, double setpoint) {
-    this.motor.setVoltage(output + feedforward.calculate(output));
+  protected void useOutput(double output, TrapezoidProfile.State setpoint) {
+    this.motor.setVoltage(output + feedforward.calculate(setpoint.velocity));
   }
 
   @Override
-  protected double getMeasurement() {
+  public double getMeasurement() {
     return this.motor.getPositionUnits();
   }
 
+  @Log
+  public double getGoal() {
+    return getController().getGoal().position;
+  }
+
+  @Log
+  public double getSetpoint() {
+    return getController().getSetpoint().position;
+  }
+
+  @Log
+  public double getError() {
+    return this.getController().getPositionError();
+  }
+
+  public void stop() {
+    this.getController().reset(getMeasurement());
+  }
   /**
-   * Directly set the velocity. Only for testing/debugging
+   * Only for testing/debugging
    */
-  @Deprecated
   public void set(double velocity) {
     this.motor.set(velocity);
   }
 
-  public static class ClimberArmSim extends ClimberArm implements Updatable {
-    private final ElevatorSim sim;
-
-    public ClimberArmSim(
-        @NotNull WrappedMotor motor,
-        @NotNull PIDController controller,
-        @NotNull ElevatorFeedforward feedforward,
-        double distanceTopBottom) {
-      super(motor, controller, feedforward);
-      this.sim = new ElevatorSim(DCMotor.getNeo550(1), 1.0, 1.0, 1.0, 0.0, distanceTopBottom);
-      Updater.subscribe(this);
-    }
+  @Override
+  public String configureLogName() {
+    return "ClimberArm" + motor.configureLogName();
+  }
 
-    @Override
-    public void update() {
-      this.sim.setInput(RobotController.getBatteryVoltage());
-    }
+  public void sirswagger21() {
+    System.out.println("Sirswagger21 my fav yt");
   }
 }