Issue #14 and added sensorHealth to drive and elevator

>Loops controlled by notifiers
> Drive and elevator encoders are checked if present, speed limiting will limit if no encoder present

src/main/java/frc/robot/Constants.java

@@ -8,6 +8,11 @@
  */
 
 public class Constants {
+	public class kLoop {
+		public static final double LOOP_SPEED = .02;
+		public static final double ENCODER_CHECKER_SPEED = .1;
+	}
+
 	public class kAuton {
 		public static final int COMMAND_ARRAY_SIZE = 41;
 

src/main/java/frc/robot/util/GZLog.java → src/main/java/frc/robot/GZLog.java

@@ -1,10 +1,9 @@
-package frc.robot.util;
+package frc.robot;
 
 import java.util.ArrayList;
 
 import edu.wpi.first.wpilibj.RobotController;
-import frc.robot.Constants;
-import frc.robot.Robot;
+import frc.robot.util.Util;
 
 public class GZLog {
 

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

@@ -43,16 +43,17 @@ public class Robot extends TimedRobot {
 	@Override
 	public void robotInit() {
 		allSubsystems.construct();
+
 		files.fillLogger();
 		auton.fillAutonArray();
-		health.generateHealth();
 
-		// TODO ISSUE #14
+		allSubsystems.startLooping();
+
+		health.generateHealth();
 	}
 
 	@Override
 	public void robotPeriodic() {
-		allSubsystems.loop();
 		elevator.printSensorHealth();
 	}
 

src/main/java/frc/robot/subsystems/Climber.java

@@ -22,12 +22,11 @@ public class Climber extends GZSubsystem {
 	// Construction
 	public Climber() {
 	}
-
-	public synchronized void construct()
-	{
+
+	public synchronized void construct() {
 		climber_1 = new Spark(kClimber.CLIMBER_1);
 		climber_1.setInverted(kClimber.CLIMBER_1_INVERT);
-		
+
 		climber_1.setSubsystem(Climber.class.getName());
 		climber_1.setName("climber_1");
 	}
@@ -59,21 +58,6 @@ public synchronized void loop() {
 		handleStates();
 		in();
 		out();
-
-		switch (mState) {
-
-		case MANUAL:
-			mIO.climber_output = mIO.climber_desired_output;
-			break;
-
-		case NEUTRAL:
-			mIO.climber_output = 0;
-			break;
-
-		default:
-			System.out.println("WARNING: Incorrect climber state " + mState + " reached.");
-			break;
-		}
 	}
 
 	public static class IO {
@@ -86,13 +70,12 @@ public static class IO {
 		public Double climber_desired_output = 0.0;
 	}
 
-	public void runClimber(double percentage)
-	{
+	public void runClimber(double percentage) {
 		if (climbCounter > 3)
 			manual(percentage);
 		else
 			stop();
-			
+
 	}
 
 	private void manual(double percentage) {
@@ -108,11 +91,26 @@ protected synchronized void in() {
 
 	@Override
 	protected synchronized void out() {
+
+		switch (mState) {
+
+		case MANUAL:
+			mIO.climber_output = mIO.climber_desired_output;
+			break;
+
+		case NEUTRAL:
+			mIO.climber_output = 0;
+			break;
+
+		default:
+			System.out.println("WARNING: Incorrect climber state " + mState + " reached.");
+			break;
+		}
 		climber_1.set(Math.abs(mIO.climber_output));
 	}
 
 	public enum ClimberState {
-		NEUTRAL, MANUAL,
+		NEUTRAL, MANUAL;
 	}
 
 	@Override
@@ -124,23 +122,23 @@ public synchronized void setWantedState(ClimberState wantedState) {
 		this.mWantedState = wantedState;
 	}
 
-	private void switchToState(ClimberState s)
-	{
-		if (mState != s)
-		{
+	private void switchToState(ClimberState s) {
+		if (mState != s) {
 			onStateExit(mState);
 			mState = s;
 			onStateStart(mState);
 		}
 	}
-	
+
 	private synchronized void handleStates() {
-
-		//if trying to disable or run demo mode while not connected to field
-		if (((this.isDisabed() || Robot.auton.isDemo()) && !Robot.gzOI.isFMS())
-				|| mWantedState == ClimberState.NEUTRAL) {
+		boolean neutral = false;
+		neutral |= (this.isDisabed() || Robot.auton.isDemo()) && !Robot.gzOI.isFMS();
+		neutral |= mWantedState == ClimberState.NEUTRAL;
+
+		// if trying to disable or run demo mode while not connected to field
+		if (neutral) {
 
-					switchToState(ClimberState.NEUTRAL);
+			switchToState(ClimberState.NEUTRAL);
 
 		} else {