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 8cec3ba..87ffbc4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -60,10 +60,11 @@ public class OI JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); liftHatchIntake.whenPressed(new LiftHatchDropBall()); - + //liftHatchIntake.toggleWhenActive(new DeployBallIntake()); JoystickButton liftBallIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - //liftBallIntake.whenPressed(new HatchFlip(false)); + //liftBallIntake.toggleWhenActive(new HatchFlip(false)); + //liftBallIntake.t liftBallIntake.whenPressed(new LiftBallDropHatch()); JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); @@ -94,8 +95,11 @@ public class OI JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ - JoystickButton lowheight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - lowheight.whenPressed(new GrabFromLoadingSatation()); + JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON); + lowHeight.whenPressed(new GrabFromLoadingSatation()); + + JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + stow.whenPressed(new StowArm()); SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index 209cac2..ed3813a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -14,10 +14,10 @@ public class StowArm extends CommandGroup { * Add your docs here. */ public StowArm() { - addSequential(new WristSetPositionPID(0), 1); - addParallel(new HatchFlip(false)); + addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addSequential(new ArmSetPositionMM(10)); + addParallel(new WristSetPositionPID(200), 2); + addSequential(new ArmSetPositionMM(50)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 3f08e1f..7114f97 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -87,7 +87,7 @@ public class Wrist extends Subsystem implements ControlLoopable public static final double KF_DOWN = 0.0; public static final double P_Value = 3; public static final double I_Value = 0.001; - public static final double D_Value = 200; + public static final double D_Value = 250; public static final double RampRate = 0.0; private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 150;