New indexer

src/org/usfirst/frc4904/robot/Robot.java

@@ -2,6 +2,7 @@
 
 
 import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain;
+import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator;
 import org.usfirst.frc4904.standard.CommandRobotBase;
 import org.usfirst.frc4904.standard.commands.chassis.ChassisMove;
 
@@ -11,6 +12,7 @@ public class Robot extends CommandRobotBase {
 	@Override
 	public void initialize() {
 		driverChooser.addDefault(new NathanGain());
+		operatorChooser.addDefault(new DefaultOperator());
 	}
 
 	@Override

src/org/usfirst/frc4904/robot/RobotMap.java

@@ -1,8 +1,11 @@
 package org.usfirst.frc4904.robot;
 
 
+import org.usfirst.frc4904.robot.subsystems.CrateIO;
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
 import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick;
 import org.usfirst.frc4904.standard.custom.controllers.CustomXbox;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX;
 import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController;
 import org.usfirst.frc4904.standard.custom.sensors.CANEncoder;
 import org.usfirst.frc4904.standard.custom.sensors.EncoderPair;
@@ -13,6 +16,7 @@
 import org.usfirst.frc4904.standard.subsystems.motor.Motor;
 import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.AccelerationCap;
 import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.EnableableModifier;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.VictorSP;
 
 public class RobotMap {
@@ -22,7 +26,12 @@ public static class HumanInput {
 			public static final int xboxController = 1;
 		}
 
-		public static class CANMotor {}
+		public static class CANMotor {
+			public static final int crateIORollerMotorLeft = 1;
+			public static final int crateIORollerMotorRight = 7;
+			public static final int rollyBOIRollerMotorLeft = 11;
+			public static final int rollyBOIRollerMotorRight = 3;
+		}
 
 		public static class PWM {
 			public static final int leftDriveA = 2;
@@ -39,6 +48,8 @@ public static class CAN {
 		public static class Pneumatics {
 			public static final int shifterUp = 0;
 			public static final int shifterDown = 1;
+			public static final int rollyBOIGrabberClasped = 2;
+			public static final int rollyBOIGrabberReleased = 3;
 		}
 	}
 
@@ -60,6 +71,14 @@ public static class Wheel {
 
 	public static class Component {
 		public static PDP pdp;
+		public static Motor crateIORollerLeft;
+		public static Motor crateIORollerRight;
+		public static Motor rollyBOIRollerLeft;
+		public static Motor rollyBOIRollerRight;
+		public static DoubleSolenoid rollyBOIGrabber;
+		public static CrateIO crateIO;
+		public static RollyBOI rollyBOI;
+		public static CustomJoystick joystick;
 		public static TankDriveShifting chassis;
 		public static Motor leftWheel;
 		public static Motor rightWheel;
@@ -76,7 +95,20 @@ public static class Component {
 	}
 
 	public RobotMap() {
+		Component.joystick = new CustomJoystick(Port.HumanInput.joystick);
 		Component.pdp = new PDP();
+		Component.crateIORollerLeft = new Motor("CrateIORollerLeft", new CANTalonSRX(Port.CANMotor.crateIORollerMotorLeft));
+		Component.crateIORollerRight = new Motor("CrateIORollerRight", new CANTalonSRX(Port.CANMotor.crateIORollerMotorRight));
+		Component.crateIO = new CrateIO(Component.crateIORollerLeft, Component.crateIORollerRight);
+		Component.rollyBOIRollerLeft = new Motor("RollyBOIRollerLeft", new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorLeft));
+		Component.rollyBOIRollerLeft.setInverted(true);
+		Component.rollyBOIRollerRight = new Motor("RollyBOIRollerRight",
+			new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorRight));
+		Component.rollyBOIGrabber = new DoubleSolenoid(RobotMap.Port.Pneumatics.rollyBOIGrabberClasped,
+			RobotMap.Port.Pneumatics.rollyBOIGrabberReleased);
+		Component.rollyBOI = new RollyBOI(Component.rollyBOIRollerLeft, Component.rollyBOIRollerRight,
+			Component.rollyBOIGrabber);
+
 		// Wheels
 		Component.leftWheelEncoder = new CANEncoder("LeftEncoder", Port.CAN.leftEncoder);
 		Component.rightWheelEncoder = new CANEncoder("RightEncoder", Port.CAN.rightEncoder);

src/org/usfirst/frc4904/robot/commands/IndexerGrabberClasp.java

@@ -0,0 +1,10 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
+
+public class IndexerGrabberClasp extends IndexerGrabberSet {
+
+	public IndexerGrabberClasp() {
+		super(RollyBOI.GrabberState.CLASPED);
+	}
+}

src/org/usfirst/frc4904/robot/commands/IndexerGrabberRelease.java

@@ -0,0 +1,10 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
+
+public class IndexerGrabberRelease extends IndexerGrabberSet {
+
+	public IndexerGrabberRelease() {
+		super(RollyBOI.GrabberState.RELEASED);
+	}
+}

src/org/usfirst/frc4904/robot/commands/IndexerGrabberSet.java

@@ -0,0 +1,29 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
+import edu.wpi.first.wpilibj.command.Command;
+
+public class IndexerGrabberSet extends Command {
+	protected final RollyBOI.GrabberState state;
+
+	public IndexerGrabberSet(RollyBOI.GrabberState state) {
+		this.state = state;
+	}
+
+	@Override
+	protected void initialize() {
+		RobotMap.Component.rollyBOI.setState(state);
+	}
+
+	@Override
+	protected void interrupted() {
+		RobotMap.Component.rollyBOI.setState(RollyBOI.GrabberState.CLASPED);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+}

src/org/usfirst/frc4904/robot/commands/IndexerIntake.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
+import org.usfirst.frc4904.standard.commands.motor.MotorConstant;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class IndexerIntake extends CommandGroup {
+	public IndexerIntake() {
+		requires(RobotMap.Component.rollyBOI);
+		addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerLeft, RollyBOI.INTAKE_SPEED));
+		addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerRight, RollyBOI.INTAKE_SPEED));
+	}
+}

src/org/usfirst/frc4904/robot/commands/IndexerOuttake.java

@@ -0,0 +1,19 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.RollyBOI;
+import org.usfirst.frc4904.standard.commands.motor.MotorConstant;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+
+public class IndexerOuttake extends CommandGroup {
+
+	public IndexerOuttake() {
+		super("IndexerRollersOuttake");
+		requires(RobotMap.Component.rollyBOI);
+		addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerLeft, RollyBOI.OUTTAKE_SPEED));
+		addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerRight, RollyBOI.OUTTAKE_SPEED));
+	}
+
+
+}

src/org/usfirst/frc4904/robot/commands/IntakeRollersIntake.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.CrateIO;
+import org.usfirst.frc4904.standard.commands.motor.MotorConstant;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class IntakeRollersIntake extends CommandGroup {
+	public IntakeRollersIntake() {
+		super("IntakeRollersIntake");
+		requires(RobotMap.Component.crateIO);
+		addParallel(new MotorConstant(RobotMap.Component.crateIO.rollerLeft, CrateIO.INTAKE_SPEED));
+		addParallel(new MotorConstant(RobotMap.Component.crateIO.rollerRight, CrateIO.INTAKE_SPEED));
+	}
+}

src/org/usfirst/frc4904/robot/commands/IntakeRollersOuttake.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.CrateIO;
+import org.usfirst.frc4904.standard.commands.motor.MotorConstant;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class IntakeRollersOuttake extends CommandGroup {
+	public IntakeRollersOuttake() {
+		super("IntakeRollersOuttake");
+		requires(RobotMap.Component.crateIO);
+		addParallel(new MotorConstant(RobotMap.Component.crateIO.rollerLeft, CrateIO.OUTTAKE_SPEED));
+		addParallel(new MotorConstant(RobotMap.Component.crateIO.rollerRight, CrateIO.OUTTAKE_SPEED));
+	}
+}

src/org/usfirst/frc4904/robot/commands/IntakeSquared.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class IntakeSquared extends CommandGroup {
+	public IntakeSquared() {
+		super("IntakeSquared");
+		requires(RobotMap.Component.crateIO);
+		requires(RobotMap.Component.rollyBOI);
+		addParallel(new IndexerIntake());
+		addParallel(new IntakeRollersIntake());
+	}
+}

src/org/usfirst/frc4904/robot/commands/MotorIdleGroup.java

@@ -0,0 +1,71 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.standard.LogKitten;
+import org.usfirst.frc4904.standard.commands.KittenCommand;
+import org.usfirst.frc4904.standard.commands.motor.MotorIdle;
+import org.usfirst.frc4904.standard.subsystems.motor.Motor;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * Idles motors of a subsystem.
+ * Used as a default command
+ *
+ */
+public class MotorIdleGroup extends CommandGroup{
+
+	/**
+	 * Run MotorIdle (from WPILib) in parallel on every motor
+	 * from a list of motors with unrestricted size.
+	 * 
+	 * @param name
+	 * @param subsystem
+	 * @param motors
+	 */
+	public MotorIdleGroup(String name, Subsystem subsystem,  boolean isDebug, Motor... motors) {
+		super(name);
+		requires(subsystem);
+		String logMessage = "";
+		for (Motor motor : motors) {
+			requires(motor);
+			addParallel(new MotorIdle(motor));
+			if (isDebug){
+				if (motor.getName() != null) {
+					logMessage += motor.getName() + " ";
+				} else {
+					logMessage += "unnamed motor ";
+				}
+				logMessage += "idling";
+				addParallel(new KittenCommand(logMessage, LogKitten.KittenLevel.WTF));
+			}
+		}
+	}
+
+	/**
+	 * Run MotorIdle (from WPILib) in parallel on every motor
+	 * from a list of motors with unrestricted size.
+	 * 
+	 * @param name
+	 * @param subsystem
+	 * @param motors
+	 */
+	public MotorIdleGroup(String name, Subsystem subsystem, Motor... motors) {
+		this(name, subsystem, false, motors);
+	}
+
+	/**
+	 * Run MotorIdle (from WPILib) in parallel on every motor
+	 * from a list of motors with unrestricted size.
+	 * Logs the names of all motors that are being idled
+	 *
+	 * @param subsystem
+	 * @param motors
+	 */
+	public MotorIdleGroup(Subsystem subsystem, Motor... motors) {
+		this("Idling Motors", subsystem, motors);
+	}
+}
+
+
+

src/org/usfirst/frc4904/robot/commands/OuttakeSquared.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4904.robot.commands;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class OuttakeSquared extends CommandGroup {
+
+	public OuttakeSquared() {
+		super("OuttakeSquared");
+		requires(RobotMap.Component.crateIO);
+		requires(RobotMap.Component.rollyBOI);
+		addParallel(new IndexerOuttake());
+		addParallel(new IntakeRollersOuttake());
+	}
+}

src/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java

@@ -0,0 +1,29 @@
+package org.usfirst.frc4904.robot.humaninterface.operators;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.commands.IndexerGrabberClasp;
+import org.usfirst.frc4904.robot.commands.IndexerGrabberRelease;
+import org.usfirst.frc4904.robot.commands.IntakeSquared;
+import org.usfirst.frc4904.robot.commands.OuttakeSquared;
+import org.usfirst.frc4904.standard.humaninput.Operator;
+
+public class DefaultOperator extends Operator {
+	public DefaultOperator() {
+		super("DefaultOperator");
+	}
+
+	public DefaultOperator(String name) {
+		super(name);
+	}
+
+	@Override
+	public void bindCommands() {
+		RobotMap.Component.joystick.button3.onlyWhileHeld(new IntakeSquared());
+		RobotMap.Component.joystick.button3.whenPressed(new IndexerGrabberRelease());
+		RobotMap.Component.joystick.button3.whenReleased(new IndexerGrabberClasp());
+		RobotMap.Component.joystick.button4.onlyWhileHeld(new OuttakeSquared());
+		RobotMap.Component.joystick.button4.whenPressed(new IndexerGrabberRelease());
+		RobotMap.Component.joystick.button4.whenReleased(new IndexerGrabberClasp());
+	}
+}