Create Hatch Failsafe to Prevent Arm from Hitting the Hatch Intake When Using Preset Positions

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -8,6 +8,7 @@ import org.usfirst.frc4388.controller.IHandController;
 import org.usfirst.frc4388.controller.XboxController;
 import org.usfirst.frc4388.robot.commands.*;
 import org.usfirst.frc4388.robot.commands.presets.SetPositionArmWrist;
+import org.usfirst.frc4388.robot.commands.presets.StowArm;
 import org.usfirst.frc4388.robot.constants.LEDPatterns;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class CargoHigh extends CommandGroup {
    * Add your docs here.
    */
   public CargoHigh() {
-    
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(3243));
     addSequential(new ArmSetPositionMM(4298));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class CargoLow extends CommandGroup {
    * Add your docs here.
    */
   public CargoLow() {
-    
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(2500));
     addSequential(new ArmSetPositionMM(1388));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class CargoMid extends CommandGroup {
    * Add your docs here.
    */
   public CargoMid() {
-
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(2830));
     addSequential(new ArmSetPositionMM(2830));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class HatchHigh extends CommandGroup {
    * Add your docs here.
    */
   public HatchHigh() {
-    
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(1144));
     addSequential(new ArmSetPositionMM(3605));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class HatchLow extends CommandGroup {
    * Add your docs here.
    */
   public HatchLow() {
-    
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(450));
     addSequential(new ArmSetPositionMM(590));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java

@@ -7,7 +7,9 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -17,9 +19,11 @@ public class HatchMid extends CommandGroup {
    * Add your docs here.
    */
   public HatchMid() {
-    
+    boolean HatchFlipStatus = Robot.pnumatics.hatchIntakeState;
+    addSequential(new HatchFlip(false));
     addParallel(new WristSetPositionPID(750));
     addSequential(new ArmSetPositionMM(2000));
+    addSequential(new HatchFlip(HatchFlipStatus));
 
     // Add Commands here:
     // e.g. addSequential(new Command1());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java

@@ -7,6 +7,7 @@
 
 package org.usfirst.frc4388.robot.commands.presets;
 
+import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
 import org.usfirst.frc4388.robot.commands.HatchFlip;
 import org.usfirst.frc4388.robot.commands.WristPlacement;
@@ -19,11 +20,13 @@ public class StowArm extends CommandGroup {
   /**
    * Add your docs here.
    */
+
   public StowArm() {
     addSequential(new HatchFlip(false));
     addParallel(new WristPlacement(true));
     addParallel(new WristSetPositionPID(110), 2);
     addSequential(new ArmSetPositionMM(10));
+
     // Add Commands here:
     // e.g. addSequential(new Command1());
     // addSequential(new Command2());

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pneumatics.java

@@ -14,6 +14,10 @@ public class Pneumatics extends Subsystem {
 	private DoubleSolenoid ballIntake;
 	private DoubleSolenoid wrist;
 
+	public boolean hatchIntakeState = false;
+	public boolean ballIntakeState = false;
+	public boolean speedShiftState = false;
+	public boolean wristState = false;
 	
 	public Pneumatics() {
 		try {
@@ -34,6 +38,7 @@ public class Pneumatics extends Subsystem {
 		if (state==false) {
 			speedShift.set(DoubleSolenoid.Value.kReverse);
 		}
+		speedShiftState = state;
 	}
 	public void setHatchIntakeState(boolean state) {
 		if (state==true) {
@@ -42,6 +47,7 @@ public class Pneumatics extends Subsystem {
 		if (state==false) {
 			hatchIntake.set(DoubleSolenoid.Value.kReverse);
 		}
+		hatchIntakeState = state;
 	}
 	public void setBallIntake(boolean state) {
 		if (state==true) {
@@ -50,6 +56,7 @@ public class Pneumatics extends Subsystem {
 		if (state==false) {
 			ballIntake.set(DoubleSolenoid.Value.kReverse);
 		}
+		ballIntakeState = state;
 	}
 	public void setWrist(boolean state) {
 		if (state==true) {
@@ -58,6 +65,7 @@ public class Pneumatics extends Subsystem {
 		if (state==false) {
 			wrist.set(DoubleSolenoid.Value.kReverse);
 		}
+		wristState = state;
 	}
 
 	public void initDefaultCommand() {