diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 45ef8b3..1024962 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.buttons.JoystickButton; import org.usfirst.frc.team2059.robot.commands.LogEncoder; import org.usfirst.frc.team2059.robot.commands.MoveArm; +import org.usfirst.frc.team2059.robot.commands.SetArmPosition; +import org.usfirst.frc.team2059.robot.RobotMap; /** * This class is the glue that binds the controls on the physical operator * interface to the commands and command groups that allow control of the robot. @@ -27,6 +29,10 @@ public class OI { joystickButtons[0][0].whenPressed(new LogEncoder()); joystickButtons[1][0].whileHeld(new MoveArm(0.5)); joystickButtons[1][1].whileHeld(new MoveArm(-0.5)); + joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect)); + joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse)); + joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot)); + joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetFarShot)); } public Joystick[] getJoysticks() { return joysticks; diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index e350471..88916fd 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -26,8 +26,8 @@ public class RobotMap { public static int armStopSolenoidTwo = 5; //Misc public static int mainArmPresetCollect = 0; - public static int mainArmPresetTraverse = 5; - public static int mainArmPresetCloseShot = 95; - public static int mainArmPresetFarShot = 85; + public static int mainArmPresetTraverse = 11; + public static int mainArmPresetCloseShot = 90; + public static int mainArmPresetFarShot = 70; } // vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java new file mode 100644 index 0000000..035b038 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/SetArmPosition.java @@ -0,0 +1,34 @@ +package org.usfirst.frc.team2059.robot.commands; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class SetArmPosition extends CommandBase { + double pos; + public SetArmPosition(double p) { + requires(mainArm); + pos=p; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + mainArm.setSetpoint(pos); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + // Called once after isFinished returns true + protected void end() { + mainArm.setSetpoint(mainArm.getDegrees()); + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index f0c212e..c075db6 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -11,7 +11,7 @@ public class MainArm extends PIDSubsystem { public MainArm(){ super("MainArm",0.06,0.0,0.002); getPIDController().setContinuous(false); - //setSetpoint(15); + setSetpoint(15); enable(); } public void initDefaultCommand() {