From 8659fa28307d76219081442415d3c746a4366660 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Tue, 29 Jan 2019 15:57:45 -0700 Subject: [PATCH] adding in drive with arm pid and man change to oi, arm, add arm set mode --- .../java/org/usfirst/frc4388/robot/OI.java | 14 +++--- .../frc4388/robot/commands/ArmSetMode.java | 47 +++++++++++++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 13 ++--- 3 files changed, 59 insertions(+), 15 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java 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 a12f61d..e2a4baa 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.Arm.ArmControlMode; import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; @@ -43,12 +44,7 @@ public class OI m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); /* XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); - CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); - - XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); - CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); + */ JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); climbUp.whenPressed(new InitiateClimber(true)); @@ -66,7 +62,11 @@ public class OI JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); - + JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); + ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID)); + + + // uncoment the line above diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java new file mode 100644 index 0000000..63b1092 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -0,0 +1,47 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Description + */ +public class ArmSetMode extends Command { + + private ArmControlMode controlMode; + + public ArmSetMode(ArmControlMode controlMode) { + this.controlMode = controlMode; + requires(Robot.arm); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (controlMode == ArmControlMode.PID) { + Robot.wrist.setPositionPID(27); + } + else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { + Robot.wrist.setSpeedJoystick(0); + } + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} 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 7e8d8e7..ffce721 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 @@ -28,8 +28,6 @@ public class Arm extends Subsystem implements Loop // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI); - // Defined speeds - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; @@ -151,19 +149,18 @@ public class Arm extends Subsystem implements Loop case PID: controlPidWithJoystick(); break; - case JOYSTICK_MANUAL: + default: JOYSTICK_MANUAL: controlManualWithJoystick(); break; - default: - break; + } } } private void controlPidWithJoystick() { - double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - double deltaPosition = joystickPosition *.5; - targetPositionInchesPID = targetPositionInchesPID + deltaPosition; + //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + //double deltaPosition = joystickPosition *.5; + targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition; updatePositionPID(targetPositionInchesPID); }