Issue 99 (#100)

* started fix

* Continue fix until code compiles

* Have color labels correspond correctly to hatch and cargo

* Set light colors using cargo commands

* Make requested changes

* Attempt to interpret rainbow climbing lights

* Make more requested changes

Author: SaltyJoke

Robot2019/src/main/java/frc/robot/OI.java

@@ -75,18 +75,18 @@ public class OI {
     hatchEjectBtn.whenPressed(new ToggleHatchEject(hp));
 
     cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values
-    cargoIntakeBtn.whenPressed(new IntakeCargo(cargo));
+    cargoIntakeBtn.whenPressed(new IntakeCargo(cargo, lights));
     cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values
     cargoEjectBtn.whenPressed(new EjectCargo(cargo));
 
     climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
     climberRailBtn.whenPressed(new ToggleClimberRails(climber));
 
     autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
-    autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy));
+    autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy, lights));
 
     manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger);
-    manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator));
+    manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights));
 
     toggleCameraBtn = new JoystickButton(leftJoy, 2);
     toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer));

Robot2019/src/main/java/frc/robot/RobotMap.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.VictorSP;
 
@@ -41,7 +42,7 @@ public class RobotMap {
   static VictorSP cargoRoller;
   static Encoder leftEnc, rightEnc;
   static String driveMode;
-  static VictorSP lights;
+  static Relay lights;
   static AHRS ahrs;
   static PowerDistributionPanel pdp;
   static UsbCamera driveCamera, hatchCamera;
@@ -77,7 +78,7 @@ public class RobotMap {
     rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3));
 
     // Initialize lights
-    lights = new VictorSP(3); // TODO: Set this to correct port
+    lights = new Relay(0); // TODO: Set this to correct port
 
     ahrs = new AHRS(SPI.Port.kMXP);
     pdp = new PowerDistributionPanel();

Robot2019/src/main/java/frc/robot/commands/Climb.java

@@ -9,12 +9,14 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.Lights;
 import frc.robot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.Joystick;
 
 public class Climb extends Command {
   private Climber climber;
   private Drivetrain dt;
+  private Lights lights;
   private Joystick dtJoy;
   private State state;
 
@@ -26,20 +28,23 @@ public class Climb extends Command {
   private final double retractGoal = 15; // This only needs to be off the ground. Climb is 19 inches.
   private final double offGroundHeight = 10;
 
-  public Climb(Climber climber, Drivetrain dt, Joystick joy) {
+  public Climb(Climber climber, Drivetrain dt, Joystick joy, Lights lights) {
     // Use requires() here to declare subsystem dependencies
     this.climber = climber;
     this.dt = dt;
+    this.lights = lights;
     dtJoy = joy;
     requires(climber);
     requires(dt);
+    requires(lights);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
     state = State.CLIMBING;
     climber.resetEncoder();
+    lights.setLights(Lights.LightState.HATCH);
   }
 
   // Called repeatedly when this Command is scheduled to run

Robot2019/src/main/java/frc/robot/commands/EjectCargo.java

@@ -11,7 +11,6 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Cargo;
-
 public class EjectCargo extends Command {
 
   final double ejectDuration = 0.5; // seconds

Robot2019/src/main/java/frc/robot/commands/IntakeCargo.java

@@ -11,22 +11,27 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Cargo;
+import frc.robot.subsystems.Lights;
 
 public class IntakeCargo extends Command {
   Timer timer;
   Cargo cargo;
+  Lights lights;
   boolean overdraw;
 
-  public IntakeCargo(Cargo cargo) {
+  public IntakeCargo(Cargo cargo, Lights lights) {
     requires(cargo);
+    requires(lights);
     this.cargo = cargo;
+    this.lights = lights;
     timer = new Timer();
     overdraw = false;
   }
 
   @Override
   protected void initialize() {
     timer.reset();
+    lights.setLights(Lights.LightState.CARGO);
   }
 
   @Override

Robot2019/src/main/java/frc/robot/commands/ManualClimb.java

@@ -11,22 +11,26 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.Lights;
 
 public class ManualClimb extends Command {
   private Climber climber;
+  private Lights lights;
   private Joystick manip;
 
   final private double retractDist = 0;
   final private double climbDist = 24.46; // In inches
 
   final private int climbJoyAxis = 1;
 
-  public ManualClimb(Climber climber, Joystick manip) {
+  public ManualClimb(Climber climber, Joystick manip, Lights lights) {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
     requires(climber);
+    requires(lights);
 
     this.climber = climber;
+    this.lights = lights;
     this.manip = manip;
   }
 
@@ -36,6 +40,7 @@ protected void initialize() {
     SmartDashboard.putNumber("Current Angle", 0);
     SmartDashboard.putNumber("Max Angle", 0);
     SmartDashboard.putNumber("Min Angle", 0);
+    lights.setLights(Lights.LightState.CLIMBER);
   }
 
   // Called repeatedly when this Command is scheduled to run

Robot2019/src/main/java/frc/robot/commands/SetLight.java

@@ -7,45 +7,25 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
 import frc.robot.subsystems.Lights;
 
-public class SetLight extends Command {
+public class SetLight extends InstantCommand {
   private Lights lights;
+  private Lights.LightState state;
 
-  public SetLight(Lights lights) {
+  public SetLight(Lights lights, Lights.LightState state) {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
     this.lights = lights;
     requires(lights);
+    this.state = state;
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    lights.setLights(state);
   }
 
-  // Called repeatedly when this Command is scheduled to run
-  @Override
-  protected void execute() {
-    lights.setLights();
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return false;
-  }
-
-  // Called once after isFinished returns true
-  @Override
-  protected void end() {
-  }
-
-  // Called when another command which requires one or more of the same
-  // subsystems is scheduled to run
-  @Override
-  protected void interrupted() {
-    end();
-  }
 }

Robot2019/src/main/java/frc/robot/subsystems/Lights.java

@@ -7,7 +7,8 @@
 
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.Relay.Value;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.SetLight;
@@ -16,44 +17,66 @@
  * Add your docs here.
  */
 public class Lights extends Subsystem {
+
+  public enum LightState {
+    OFF,
+    CARGO,
+    HATCH,
+    CLIMBER
+  }
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  private VictorSP lights;
-  private double lightsValue = 0;
+  private Relay lights;
+  private LightState lightsState;
 
   /**
    * 0 is off 0.5 is orange 1 is yellow
    */
 
-  public Lights(VictorSP lights) {
+  public Lights(Relay lights) {
     this.lights = lights;
+    lightsState = LightState.HATCH;
   }
 
-  public void setLights() {
-    lights.set(lightsValue);
-    String color;
-    if (lightsValue == 0) {
+  public void setLights(LightState newState) {
+    String color = "";
+    switch (newState) {
+      case OFF:
+      lights.set(Relay.Value.kOff);
       color = "None";
-    } else if (lightsValue == 0.5) {
-      color = "Yellow";
-    } else {
+      break;
+      case CARGO:
+      lights.set(Relay.Value.kForward);
       color = "Orange";
+      break;
+      case HATCH:
+      lights.set(Relay.Value.kReverse);
+      color = "Blue";
+      break;
+      case CLIMBER:
+      lights.set(Relay.Value.kOn);
+      color = "Rainbow";
+      break;
     }
+    lightsState = newState;
     SmartDashboard.putString("Lights Current Color", color);
   }
 
   public void toggleLights() {
-    if (lightsValue == 0) {
-      lightsValue = 0.5;
-    } else if (lightsValue == 0.5) {
-      lightsValue = 1;
-    } else {
-      lightsValue = 0;
+    if (lightsState == LightState.OFF) {
+      lightsState = LightState.CARGO;
+    } else if (lightsState == LightState.CARGO) {
+      lightsState = LightState.HATCH;
+    } else if (lightsState == LightState.HATCH) {
+      lightsState = LightState.CLIMBER;
+    }
+    else {
+      lightsState = LightState.OFF;
     }
   }
 
   @Override
   public void initDefaultCommand() {
-    setDefaultCommand(new SetLight(this));
+    setDefaultCommand(new SetLight(this, LightState.HATCH));
   }
 }