diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LaserToDash.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LaserToDash.java index a379dd2..7113ad7 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LaserToDash.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/LaserToDash.java @@ -12,7 +12,14 @@ import org.usfirst.frc4388.robot.Robot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class LaserToDash extends Command { + String p1 = ""; + String p2 = ""; + + String last1 = "0"; + String last2 = "0"; + public LaserToDash() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); @@ -28,19 +35,27 @@ public class LaserToDash extends Command { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - String p1, p2; + try{ String toParse = Robot.rangefinder.getDistance(); String[] parts = toParse.split("s"); p1 = parts[0]; p2 = parts[1]; + if (!(p1 == "0" || p2 == "0")){ + SmartDashboard.putNumber("Laser 1 raw out", Double.parseDouble(p1)); + SmartDashboard.putNumber("Laser 2 raw out", Double.parseDouble(p2)); + last1 = p1; + last2 = p2; + } + else{ + SmartDashboard.putNumber("Laser 1 raw out", Double.parseDouble(last1)); + SmartDashboard.putNumber("Laser 2 raw out", Double.parseDouble(last2)); + } } catch(Exception nullException){ - p1 = "0"; - p2 = "0"; } - SmartDashboard.putNumber("Laser 1 raw out", Double.parseDouble(p1)); - SmartDashboard.putNumber("Laser 2 raw out", Double.parseDouble(p2)); + + } // Make this return true when this Command no longer needs to run execute()