diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 5552693..b0a0450 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/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; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java index 920b755..eee92d4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java index d636586..f53ab86 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java index ec71323..0565226 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java index a78c8f3..39be602 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java index fd85278..e2ac86b 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java index b5660ac..e41e67f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java index 544ba95..e17e9f8 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java +++ b/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()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pneumatics.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pneumatics.java index a4ca5f0..6447ad1 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pneumatics.java +++ b/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() {