diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java new file mode 100644 index 0000000..3d54006 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java @@ -0,0 +1,47 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Description + */ +public class WristSetMode extends Command { + + private WristControlMode controlMode; + + public WristSetMode(WristControlMode controlMode) { + this.controlMode = controlMode; + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (controlMode == WristControlMode.PID) { + Robot.wrist.setPositionPID(Robot.wrist.getPositionInches()); + } + else if (controlMode == WristControlMode.JOYSTICK_MANUAL) { + Robot.wrist.setSpeed(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/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index b300670..986c183 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 @@ -154,6 +154,17 @@ public class Wrist extends Subsystem mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); } + public double getPositionInches() + { + return wristRight.getPositionWorld(); + } + + public void setSpeedJoystick(double speed) + { + wristRight.set(ControlMode.PercentOutput, speed); + setWristControlMode(wristControlMode.JOYSTICK_MANUAL); + } + public synchronized boolean isFinished() { return isFinished;