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 4fc1133..02fda7b 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.buttons.*; import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.buttons.Button; @@ -59,7 +60,11 @@ public class OI JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); shiftDown.whenPressed(new DriveSpeedShift(false)); - // shiftDown.whenPressed(new LEDIndicators(false)); + // shiftDown.whenPressed(new LEDIndicators(false)); + + //Wrist + JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); + wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); //Operator Xbox 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 986c183..cee418a 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 @@ -86,15 +86,6 @@ public class Wrist extends Subsystem manualOverride = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - if(manualOverride == true) - { - setWristControlMode(wristControlMode.JOYSTICK_MANUAL); - } - else if(manualOverride == false) - { - setWristControlMode(wristControlMode.PID); - } - while(getWristControlMode() == wristControlMode.JOYSTICK_MANUAL) { wristRight.set(wristJoystickInput); @@ -163,6 +154,45 @@ public class Wrist extends Subsystem { wristRight.set(ControlMode.PercentOutput, speed); setWristControlMode(wristControlMode.JOYSTICK_MANUAL); + } + + //@Override + public void onLoop(double timestamp) + { + synchronized (Wrist.this) { + switch(getWristControlMode() ) { + case PID: + controlPidWithJoystick(); + break; + case JOYSTICK_MANUAL: + controlManualWithJoystick(); + break; + default: + break; + } + } + } + + private void controlPidWithJoystick() + { + + + ///CHANGE ACCORDING TO DIFFERENT LEVELS ON SPACESHIP + /* + //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + //double deltaPosition = joystickPosition * joystickInchesPerMs; + + targetPositionInchesPID = targetPositionInchesPID + deltaPosition; + updatePositionPID(targetPositionInchesPID); + */ + } + + private void controlManualWithJoystick() + { + double joystickSpeed; + + joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + setSpeedJoystick(joystickSpeed); } public synchronized boolean isFinished()