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 608173b..6996718 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -105,7 +105,7 @@ public class OI JoystickButton help = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON); help.whenPressed(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL)); - help.whenReleased(new ArmSetMode(ArmControlMode.MOTION_MAGIC)); + help.whenReleased(new ArmSetMode(ArmControlMode.JOYSTICK_PID)); help.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); help.whenReleased(new WristSetMode(WristControlMode.JOYSTICK_PID)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index a81b096..a6f70b9 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -134,7 +134,7 @@ public class Arm extends Subsystem implements ControlLoopable // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private ArmControlMode armControlMode = ArmControlMode.SMART_MOTION; + private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; public PlaceMode placeMode = PlaceMode.HATCH; public double targetPositionInchesPID = 0; public double targetPositionInchesSM = 0; @@ -226,7 +226,8 @@ public class Arm extends Subsystem implements ControlLoopable controlManualWithJoystick(); // System.err.println(motorController.getControlMode()); } - else if (!isFinished) { + //else if (!isFinished) { + else { if (armControlMode == ArmControlMode.JOYSTICK_PID){ controlPidWithJoystick(); // System.err.println(motor1.getControlMode()); @@ -331,11 +332,9 @@ public class Arm extends Subsystem implements ControlLoopable resetEncoder(); } double startPositionInches = motorEncoder.getPosition(); - // mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down); motorController.setReference(targetPositionInches, ControlType.kPosition); // motor1.setClosedLoopRampRate(RampRate); - motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down); - // motor1.configClosedloopRamp(0); // motor1.configPeakCurrentLimit(5); @@ -369,8 +368,8 @@ public class Arm extends Subsystem implements ControlLoopable } double startPositionInches = motorEncoder.getPosition(); - motorController.setReference(targetPositionInches, ControlType.kSmartMotion); motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down); + motorController.setReference(targetPositionInches, ControlType.kSmartMotion); } private double limitPosition(double targetPosition) {