Merge pull request #7 from frc3316/oded/stackerStateMachine

Oded/stacker state machine

src/org/usfirst/frc/team3316/robot/Robot.java

@@ -61,7 +61,6 @@ public void robotInit()
     {
     	logger = new DBugLogger();
     	config = new Config();
-    	timer = new Timer();
 
     	/*
     	 * Human IO (that does not require subsystems)
@@ -87,6 +86,14 @@ public void robotInit()
     	 */
     	joysticks.initButtons();
     	sdb = new SDB();
+
+    	/*
+    	 * Timer
+    	 */
+    	timer = new Timer();
+    	chassis.timerInit();
+    	stacker.timerInit();
+    	sdb.timerInit();
     }
 
 	public void disabledPeriodic() {
@@ -110,8 +117,9 @@ public void teleopInit() {}
      * This function is called when the disabled button is hit.
      * You can use it to reset subsystems before shutting down.
      */
-    public void disabledInit(){
-
+    public void disabledInit()
+    {
+    	Robot.stacker.setSetpointState(null);
     }
 
     /**

src/org/usfirst/frc/team3316/robot/chassis/commands/FieldOrientedDrive.java

@@ -19,8 +19,6 @@ protected void setFieldVector (double x, double y)
 												//clockwise is positive and 
 												//counter-clockwise is negative
 		double robotAngleRad = Math.toRadians(Robot.chassis.getHeading()); //Robot angle
-		// For D-Bugging purposes
-		SmartDashboard.putNumber("Current Heading", Robot.chassis.getHeading()); //TODO: MOVE THIS TO A BETTER PLACE
 
 		double angleDiff = vectorAngle - robotAngleRad; //The angle of the vector oriented to the robot
 

src/org/usfirst/frc/team3316/robot/chassis/commands/RobotOrientedDrive.java

@@ -21,7 +21,6 @@ protected void setRobotVector (double x, double y)
 
 	protected void setRotation (double requiredTurn)
 	{
-		//TODO: figure out PID rotation
 		//NOTE: Required turn cannot be larger than yIn.
 
 		/*

src/org/usfirst/frc/team3316/robot/chassis/commands/StartIntegrator.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc.team3316.robot.chassis.commands;
+
+import org.usfirst.frc.team3316.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class StartIntegrator extends Command {
+
+    public StartIntegrator() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() 
+    {
+    	Robot.chassis.addNavigationIntegrator(Robot.chassis.navigationIntegrator);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java

@@ -9,10 +9,7 @@
 import org.usfirst.frc.team3316.robot.config.Config;
 import org.usfirst.frc.team3316.robot.config.Config.ConfigException;
 import org.usfirst.frc.team3316.robot.logger.DBugLogger;
-import org.usfirst.frc.team3316.robot.rollerGripper.commands.RollIn;
-import org.usfirst.frc.team3316.robot.rollerGripper.commands.RollOut;
-import org.usfirst.frc.team3316.robot.rollerGripper.commands.RollTurnClockwise;
-import org.usfirst.frc.team3316.robot.rollerGripper.commands.RollTurnCounterClockwise;
+import org.usfirst.frc.team3316.robot.sequences.PickupSequence;
 import org.usfirst.frc.team3316.robot.stacker.commands.CloseGripper;
 import org.usfirst.frc.team3316.robot.stacker.commands.HoldContainer;
 import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToFloor;
@@ -70,7 +67,7 @@ public boolean get()
 
 	public JoystickButton moveStackerToFloor, 
 						  moveStackerToStep, 
-						  moveStackerToTote;
+						  moveStackerToTote; //pickup replaces moveStackerToTote
 
 	public JoystickButton holdContainer,
 						  releaseContainer;
@@ -105,24 +102,6 @@ public void initButtons ()
     	 */
 		try
 		{
-	    	/*
-	    	 * Roller Gripper
-	    	 */
-	    	buttonRollIn = new POVButton(joystickOperator, 
-	    			(int) config.get("BUTTON_ROLL_IN"));
-	    	buttonRollIn.whileHeld(new RollIn());
-
-	    	buttonRollOut = new POVButton(joystickOperator, 
-	    			(int) config.get("BUTTON_ROLL_OUT"));
-	    	buttonRollOut.whileHeld(new RollOut());
-
-	    	buttonRollTurnClockwise = new POVButton(joystickOperator, 
-	    			(int) config.get("BUTTON_ROLL_TURN_CLOCKWISE"));
-	    	buttonRollTurnClockwise.whileHeld(new RollTurnClockwise());
-
-	    	buttonRollTurnCounterClockwise = new POVButton(joystickOperator, 
-	    			(int) config.get("BUTTON_ROLL_TURN_COUNTER_CLOCKWISE"));
-	    	buttonRollTurnCounterClockwise.whileHeld(new RollTurnCounterClockwise());
 
 	    	/*
 	    	 * Stacker

src/org/usfirst/frc/team3316/robot/humanIO/SDB.java

@@ -10,19 +10,16 @@
 
 import org.usfirst.frc.team3316.robot.Robot;
 import org.usfirst.frc.team3316.robot.chassis.commands.StrafeDrive;
+import org.usfirst.frc.team3316.robot.chassis.commands.StartIntegrator;
 import org.usfirst.frc.team3316.robot.chassis.commands.TankDrive;
 import org.usfirst.frc.team3316.robot.chassis.heading.SetHeadingPreset;
 import org.usfirst.frc.team3316.robot.chassis.heading.SetHeadingSDB;
 import org.usfirst.frc.team3316.robot.chassis.commands.RobotOrientedDrive;
 import org.usfirst.frc.team3316.robot.config.Config;
 import org.usfirst.frc.team3316.robot.config.Config.ConfigException;
 import org.usfirst.frc.team3316.robot.logger.DBugLogger;
-import org.usfirst.frc.team3316.robot.stacker.commands.HoldContainer;
-import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToFloor;
-import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToStep;
-import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToTote;
-import org.usfirst.frc.team3316.robot.stacker.commands.ReleaseContainer;
 
+import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class SDB 
@@ -33,6 +30,10 @@ public class SDB
 	 */
 	private class UpdateSDBTask extends TimerTask
 	{
+		public UpdateSDBTask ()
+		{
+			logger.info("Created UpdateSDBTask");
+		}
 		public void run ()
 		{
 			put ("Current Heading", Robot.chassis.getHeading());
@@ -41,17 +42,37 @@ public void run ()
 			put ("Current GP distance", Robot.rollerGripper.getGPIRDistance());
 
 			put ("Angular Velocity", Robot.chassis.getAngularVelocity());
+			put ("Angular Velocity Encoders", Robot.chassis.getAngularVelocityEncoders());
 
-			put ("Left Ratchet", Robot.stacker.getSwitchLeft());
-			put ("Right Ratchet", Robot.stacker.getSwitchRight());
+			put ("Left Ratchet", Robot.stacker.getSwitchRatchetLeft());
+			put ("Right Ratchet", Robot.stacker.getSwitchRatchetRight());
 
-			put ("Game Piece Switch", Robot.rollerGripper.getSwitchGP());
+			put ("Game Piece Switch", Robot.rollerGripper.getSwitchGamePiece());
 
 			put ("Distance Left", Robot.chassis.getDistanceLeft());
-			put ("Speed Left", Robot.chassis.getSpeedLeft());
-
 			put ("Distance Right", Robot.chassis.getDistanceRight());
 			put ("Distance Center", Robot.chassis.getDistanceCenter());
+
+			put ("Speed Left", Robot.chassis.getSpeedLeft());
+			put ("Speed Right", Robot.chassis.getSpeedRight());
+			put ("Speed Center", Robot.chassis.getSpeedCenter());
+
+			if (Robot.rollerGripper.getGamePieceCollected() == null)
+			{
+				put ("Game Piece Collected", null);
+			}
+			else
+			{
+				put ("Game Piece Collected", Robot.rollerGripper.getGamePieceCollected().toString());
+			}
+
+			/*
+			 * Integrator testing
+			 * should be removed
+			 */
+			put ("Integrator X", Robot.chassis.navigationIntegrator.getX());
+			put ("Integrator Y", Robot.chassis.navigationIntegrator.getY());
+			put ("Integrator Heading", Robot.chassis.navigationIntegrator.getHeading());
 		}
 
 		private void put (String name, double d)
@@ -68,6 +89,11 @@ private void put (String name, boolean b)
 	    {
 	    	SmartDashboard.putBoolean(name, b);
 	    }
+
+	    private void put (String name, String s)
+	    {
+	    	SmartDashboard.putString(name, s);
+	    }
 	}
 
 	DBugLogger logger = Robot.logger;
@@ -86,7 +112,10 @@ public SDB ()
 
 		initSDB();
 		logger.info("Finished initSDB()");
-
+	}
+
+	public void timerInit ()
+	{
 		updateSDBTask = new UpdateSDBTask();
 		Robot.timer.schedule(updateSDBTask, 0, 20);
 	}
@@ -147,10 +176,14 @@ public Set <Entry <String, Class <?> > > getVariablesInSDB ()
 	private void initSDB ()
 	{	
 		SmartDashboard.putData(new UpdateVariablesInConfig()); // NEVER REMOVE THIS COMMAND
-				
+
 		SmartDashboard.putData(new TankDrive());
 		SmartDashboard.putData(new StrafeDrive());
 		SmartDashboard.putData(new RobotOrientedDrive());
+		SmartDashboard.putData(new StartIntegrator()); //For integrator testing, should be removed
+
+		SmartDashboard.putData(new TankDrive()); //should be removed
+		SmartDashboard.putData(new RobotOrientedDrive()); //should be removed
 
 		/*
 		 * Set Heading SDB
@@ -171,5 +204,29 @@ private void initSDB ()
 
 		putConfigVariableInSDB("rollerGripper_RollJoystick_LowPass");
 
+		//Game Piece IR
+		putConfigVariableInSDB("rollerGripper_ToteDistanceMinimum");
+		putConfigVariableInSDB("rollerGripper_ToteDistanceMaximum");
+
+		putConfigVariableInSDB("rollerGripper_ContainerDistanceMinimum");
+		putConfigVariableInSDB("rollerGripper_ContainerDistanceMaximum");
+
+		putConfigVariableInSDB("rollerGripper_SomethingDistanceThreshold");
+		putConfigVariableInSDB("rollerGripper_UnsureDistanceThreshold");
+
+		/*
+		 * Stacker
+		 */
+		SmartDashboard.putData(Robot.stacker);
+
+		putConfigVariableInSDB("stacker_HeightFloorMinimum");
+		putConfigVariableInSDB("stacker_HeightFloorMaximum");
+
+		putConfigVariableInSDB("stacker_HeightToteMinimum");
+		putConfigVariableInSDB("stacker_HeightToteMaximum");
+
+		putConfigVariableInSDB("stacker_HeightStepMinimum");
+		putConfigVariableInSDB("stacker_HeightStepMaximum");
+
 	}
 }

src/org/usfirst/frc/team3316/robot/robotIO/Actuators.java

@@ -23,7 +23,7 @@ public class Actuators
 	 */
 	public SpeedController chassisMotorControllerLeft1, chassisMotorControllerLeft2;
 	public SpeedController chassisMotorControllerRight1, chassisMotorControllerRight2;
-	public SpeedController chassisMotorControllerCenter1, chassisMotorControllerCenter2;
+	public SpeedController chassisMotorControllerCenter;
 
 	/*
 	 * Stacker
@@ -61,8 +61,7 @@ public Actuators ()
 				chassisMotorControllerRight1 = new VictorSP ((int) config.get("CHASSIS_MOTOR_CONTROLLER_RIGHT_1"));
 				chassisMotorControllerRight2 = new VictorSP ((int) config.get("CHASSIS_MOTOR_CONTROLLER_RIGHT_2"));
 
-				chassisMotorControllerCenter1 = new VictorSP((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER_1"));
-				chassisMotorControllerCenter2 = new VictorSP((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER_2"));
+				chassisMotorControllerCenter = new VictorSP((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER"));
 				/*
 				 * Anschluss
 				 */
@@ -82,8 +81,7 @@ public Actuators ()
 				chassisMotorControllerRight1 = new Talon ((int) config.get("CHASSIS_MOTOR_CONTROLLER_RIGHT_1"));
 				chassisMotorControllerRight2 = new Talon ((int) config.get("CHASSIS_MOTOR_CONTROLLER_RIGHT_2"));
 
-				chassisMotorControllerCenter1 = new Talon((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER_1"));
-				chassisMotorControllerCenter2 = new Talon((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER_2"));
+				chassisMotorControllerCenter = new Talon((int) config.get("CHASSIS_MOTOR_CONTROLLER_CENTER"));
 				/*
 				 * Anschluss
 				 */

src/org/usfirst/frc/team3316/robot/rollerGripper/GamePieceCollected.java

@@ -2,5 +2,15 @@
 
 public enum GamePieceCollected 
 {
-	Tote, Container, Unsure, None;
+	/*
+	 * Ordered from closest to furthest:
+	 
+	 * Tote - a tote is in the gripper
+	 * Container - a container is in the gripper
+	 * Something - something is in the gripper, not sure what
+	 * Unsure - unsure if there's something in the gripper
+	 * None - the gripper is empty
+	 */
+
+	Tote, Container, Something, Unsure, None;
 }