//where the lazy suzan stops moving to we can make an accurate shot.
lastTime = Timer.getFPGATimestamp();
System.out.println("Targe Diff: "+TrackingCamera.targetDiff);
if (TrackingCamera.targetDiff < 50) {
if (TrackingCamera.targetDiff < 15) {
lazySusan.setRelay(Relay.Value.kOff); //turn off
SmartDashboard.putString("LazySusan", "Off");
} else if (TrackingCamera.targetLocale > TrackingCamera.horCenter) { //and if we are facing right
lazySusan.setRelay(Relay.Value.kForward); //turn left
SmartDashboard.putString("LazySusan", "Right");
} else { //if we face left
lazySusan.setRelay(Relay.Value.kReverse); //turn right
SmartDashboard.putString("LazySusan", "Left");
}
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
//TrackingCamera.angleFinished = true;
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
thisTime = Timer.getFPGATimestamp();
timeLapse = thisTime - lastTime;
if(timeLapse >= 0.01) {
return true;
} else {
return false;
}
}
// Called once after isFinished returns true
protected void end() {
lazySusan.setRelay(Relay.Value.kOff);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
} |