Add lights code

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

@@ -21,11 +21,13 @@
 import frc.robot.commands.SlowDrive;
 import frc.robot.commands.ToggleCamera;
 import frc.robot.commands.ToggleHatch;
+import frc.robot.commands.ToggleLight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.HatchPanel;
+import frc.robot.subsystems.Lights;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -43,9 +45,10 @@ public class OI {
   JoystickButton climbBtn;
   JoystickButton toggleCameraBtn;
   JoystickButton wobbleDriveBtn;
+  JoystickButton cycleLightBtn;
 
-  OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, UsbCamera driveCamera, UsbCamera hatchCamera,
-      VideoSink cameraServer) {
+  OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, Lights lights, UsbCamera driveCamera,
+      UsbCamera hatchCamera, VideoSink cameraServer) {
     leftJoy = new Joystick(0);
     rightJoy = new Joystick(1);
     manipulator = new Joystick(2);
@@ -79,6 +82,9 @@ public class OI {
 
     toggleCameraBtn = new JoystickButton(leftJoy, 2);
     toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer));
+
+    cycleLightBtn = new JoystickButton(manipulator, Manip.START);
+    cycleLightBtn.whenPressed(new ToggleLight(lights));
   }
 
   private class Manip {

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

@@ -18,13 +18,15 @@
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.HatchPanel;
+import frc.robot.subsystems.Lights;
 
 public class Robot extends TimedRobot {
   private static Drivetrain dt;
   private static HatchPanel hp;
   private static OI oi;
   private static Cargo cargo;
   private static Climber climber;
+  private static Lights lights;
   private static String fname1, fname2, fname3, fname4;
 
   @Override
@@ -34,8 +36,8 @@ public void robotInit() {
     hp = new HatchPanel(RobotMap.hatchPistons);
     cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp, RobotMap.cargoPDPPort);
     climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons);
-
-    oi = new OI(dt, hp, cargo, climber, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
+    lights = new Lights(RobotMap.lights);
+    oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
 
     fname1 = "/home/lvuser/drive_char_linear_for.csv";
     fname2 = "/home/lvuser/drive_char_stepwise_for.csv";

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

@@ -41,6 +41,7 @@ public class RobotMap {
   static VictorSP cargoRoller;
   static Encoder leftEnc, rightEnc;
   static String driveMode;
+  static VictorSP lights;
   static AHRS ahrs;
   static PowerDistributionPanel pdp;
   static UsbCamera driveCamera, hatchCamera;
@@ -74,6 +75,9 @@ public class RobotMap {
     leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1));
     rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3));
 
+    // Initialize lights
+    lights = new VictorSP(3); // TODO: Set this to correct port
+
     ahrs = new AHRS(SPI.Port.kMXP);
     pdp = new PowerDistributionPanel();
 
@@ -146,9 +150,12 @@ private static UsbCamera configureCamera(int port) {
 
   /**
    * Checks an error code and prints it to standard out if it is not ok
+   * 
    * @param ec The error code to check
    */
   private static void catchError(ErrorCode ec) {
-    if(ec != ErrorCode.OK) System.out.println("Error configuring in RobotMap.java at line: " + new Throwable().getStackTrace()[1].getLineNumber());
+    if (ec != ErrorCode.OK)
+      System.out
+          .println("Error configuring in RobotMap.java at line: " + new Throwable().getStackTrace()[1].getLineNumber());
   }
 }

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

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.subsystems.Lights;
+
+public class SetLight extends Command {
+  private Lights lights;
+
+  public SetLight(Lights lights) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    this.lights = lights;
+    requires(lights);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // 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/commands/ToggleLight.java

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.robot.subsystems.Lights;
+
+/**
+ * Add your docs here.
+ */
+public class ToggleLight extends InstantCommand {
+  /**
+   * Add your docs here.
+   */
+  private Lights lights;
+
+  public ToggleLight(Lights lights) {
+    super();
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    this.lights = lights;
+  }
+
+  // Called once when the command executes
+  @Override
+  protected void initialize() {
+    lights.toggleLights();
+  }
+
+}

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

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.SetLight;
+
+/**
+ * Add your docs here.
+ */
+public class Lights extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  private VictorSP lights;
+  private double lightsValue = 0;
+
+  /**
+   * 0 is off 0.5 is orange 1 is yellow
+   */
+
+  public Lights(VictorSP lights) {
+    this.lights = lights;
+  }
+
+  public void setLights() {
+    lights.set(lightsValue);
+    String color;
+    if (lightsValue == 0) {
+      color = "None";
+    } else if (lightsValue == 0.5) {
+      color = "Yellow";
+    } else {
+      color = "Orange";
+    }
+    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;
+    }
+  }
+
+  @Override
+  public void initDefaultCommand() {
+    setDefaultCommand(new SetLight(this));
+  }
+}