Created
January 25, 2019 02:41
-
-
Save codeNinjaDev/5cae39e8eb0b1667b6e8132d0e26096b to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
package frc.robot.controllers; | |
import edu.wpi.first.networktables.NetworkTable; | |
import edu.wpi.first.networktables.NetworkTableEntry; | |
import edu.wpi.first.networktables.NetworkTableInstance; | |
import edu.wpi.first.wpilibj.command.Subsystem; | |
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
import frc.robot.hardware.*; | |
public class VisionController extends Subsystem { | |
private boolean is_enabled, driverVision, tapeVision, cargoVision; | |
private NetworkTableEntry tapeDetected, cargoDetected, tapeYaw, cargoYaw, driveWanted, tapeWanted, cargoWanted, videoTimestamp; | |
private double targetAngle, timestamp; | |
private RemoteControl humanControl; | |
NetworkTableInstance instance; | |
NetworkTable chickenVision; | |
public VisionController(RemoteControl humanControl) { | |
this.humanControl = humanControl; | |
instance = NetworkTableInstance.getDefault(); | |
chickenVision = instance.getTable("ChickenVision"); | |
tapeDetected = chickenVision.getEntry("tapeDetected"); | |
cargoDetected = chickenVision.getEntry("cargoDetected"); | |
tapeYaw = chickenVision.getEntry("tapeYaw"); | |
cargoYaw = chickenVision.getEntry("cargoYaw"); | |
driveWanted = chickenVision.getEntry("Driver"); | |
tapeWanted = chickenVision.getEntry("Tape"); | |
cargoWanted = chickenVision.getEntry("Cargo"); | |
videoTimestamp = chickenVision.getEntry("VideoTimestamp"); | |
tapeVision = cargoVision = false; | |
driverVision = true; | |
} | |
public void reset() { | |
} | |
public void update() { | |
driverVision = false; | |
tapeVision = false; | |
cargoVision = true; | |
driveWanted.setBoolean(driverVision); | |
tapeWanted.setBoolean(tapeVision); | |
cargoWanted.setBoolean(cargoVision); | |
SmartDashboard.putBoolean("Cargo detected", cargoDetected.getBoolean(false)); | |
if(cargoDetected.getBoolean(false)) { | |
targetAngle = cargoYaw.getDouble(0); | |
SmartDashboard.putNumber("Cargo Yaw", targetAngle); | |
} else { | |
targetAngle = 0; | |
} | |
SmartDashboard.putNumber("Cargo Yaw", targetAngle); | |
} | |
public double targetYaw() { | |
update(); | |
return targetAngle; | |
} | |
@Override | |
protected void initDefaultCommand() { | |
// TODO Auto-generated method stub | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment