Added Launcher PID Reset at init & Enabled Pixy Debug mode

CircuitOfLifeRobotics · Apr 6, 2016 · 9c68501 · 9c68501
1 parent 374dcc8
commit 9c68501
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 6 deletions.
diff --git a/src/com/team3925/robot2016/Robot.java b/src/com/team3925/robot2016/Robot.java
@@ -123,7 +123,7 @@ private void reset() {
 		maxVel = 0;
 		maxRotationVel = 0;
 		maxRotationAccel = 0; 
-//		launcherPID.reset();
+//		launcher.resetPID();
 	}
 
 	/**
@@ -195,11 +195,11 @@ public void teleopPeriodic() {
 
 		if (oi.getVisionShoot_GyroTurnEnable()) {
 			if (!visionGyroTurn.isRunning()) {
-				if (launcher.getTurnAngle() != Double.NaN) {
+				if (Double.isNaN(launcher.getTurnAngle())) {
 					visionGyroTurn.setSetpointRelative(launcher.getTurnAngle());
 					visionGyroTurn.start();
 				} else {
-					DriverStation.reportWarning("Camera cannot detect object! GyroDrive not starting!", false);
+					DriverStation.reportWarning("Camera cannot detect object! Thou shall not runneth or beginneth a GyroDrive!", false);
 				}
 			}
 		} else {

diff --git a/src/com/team3925/robot2016/subsystems/Launcher.java b/src/com/team3925/robot2016/subsystems/Launcher.java
@@ -37,7 +37,7 @@ public final class Launcher extends Subsystem implements SmartdashBoardLoggable,
     private SynchronousPID aimPidLoop = new SynchronousPID();
     private final PixyCmu5 pixy = Robot.pixy;
     private PixyFrame latestFrame = null;
-    private double turnAngle = 0d;
+    private double turnAngle = Double.NaN;
 
     private boolean aimEnabled = false,
     		aimOnTarget = false;
@@ -51,6 +51,8 @@ public final class Launcher extends Subsystem implements SmartdashBoardLoggable,
 
 	public void init() {
 		aimPidLoop.setPID(LAUNCHER_AIM_KP, LAUNCHER_AIM_KI, LAUNCHER_AIM_KD);
+		resetPID();
+
 		// TODO: If the limits are this high, can this be replaced with a normal PID controller?
 //		aimPidLoop.setPIDLimits(10000, 10000, 10000, 10000, -10000, -10000, -10000, -10000);
 
@@ -168,7 +170,11 @@ public void enableAim(boolean isEnable) {
 	public void setIntakeSpeed(double speed) {
 		intakeSpeed = speed;
 	}
-
+
+
+	public void resetPID() {
+		aimPidLoop.reset();
+	}
 
 	public void setPuncher(boolean isHigh) {
 		//reverse isHigh for practice bot
@@ -232,6 +238,7 @@ public void logData() {
 
 			if (latestFrame != null) {
 				putBooleanSD("Vision_CanShoot", pixy.isInXCenter(latestFrame));
+				putNumberSD("AngleToTarget", turnAngle);
 			}
 		}
 

diff --git a/src/com/team3925/robot2016/util/PixyCmu5.java b/src/com/team3925/robot2016/util/PixyCmu5.java
@@ -106,7 +106,7 @@ public class PixyFrame
     /**********************************************************
 	 *  Debug variables
 	 **********************************************************/
-    private static boolean flg_debug = false;
+    private static boolean flg_debug = true;
 
 	/**
 	 * PixyTask is the private scheduler within PixyCMU5 that