diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index ed7da9b..d875e9d 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton; import org.usfirst.frc.team2059.robot.commands.drivetrain.LogEncoder; import org.usfirst.frc.team2059.robot.commands.drivetrain.PIDDrive; import org.usfirst.frc.team2059.robot.commands.shooter.MoveArm; +import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower; +import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper; import org.usfirst.frc.team2059.robot.commands.shooter.SetArmPosition; import org.usfirst.frc.team2059.robot.commands.shooter.SetShooterState; import org.usfirst.frc.team2059.robot.commands.shooter.SetArmStopState; @@ -42,6 +44,8 @@ public class OI { joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot)); joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetFarShot)); joystickButtons[1][7].whileHeld(new SetArmStopState(true)); + joystickButtons[1][10].whileHeld(new ResetLower(-1)); + joystickButtons[1][11].whileHeld(new ResetUpper(1)); } public Joystick[] getJoysticks() { return joysticks; diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 3fc9394..c84f040 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -40,7 +40,7 @@ public class Robot extends IterativeRobot { autonomousCommand.cancel(); } CommandBase.pneumatics.setCompressorEnabled(true); - CommandBase.pneumatics.setArmStopState(false); + CommandBase.pneumatics.setArmStopState(true); } public void teleopPeriodic() { Scheduler.getInstance().run(); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 810eb06..0a281fc 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -13,6 +13,8 @@ public class RobotMap { public static int armPot = 0; public static int armLeftMotor = 5; public static int armRightMotor = 6; + public static int armBottomLim = 4; + public static int armTopLim = 5; //Shooter public static int shooterLeftMotor = 7; public static int shooterRightMotor = 8; diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java new file mode 100644 index 0000000..4a709c7 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java @@ -0,0 +1,36 @@ +package org.usfirst.frc.team2059.robot.commands.shooter; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class ResetLower extends CommandBase { + double speed; + public ResetLower(double s) { + requires(mainArm); + speed = s; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + System.out.println("test"); + mainArm.disable(); + mainArm.resetLower(speed); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return mainArm.getBottomPressed(); + } + // Called once after isFinished returns true + protected void end() { + mainArm.moveArm(0); + } + // 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/commands/shooter/ResetUpper.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java new file mode 100644 index 0000000..3c15919 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java @@ -0,0 +1,36 @@ +package org.usfirst.frc.team2059.robot.commands.shooter; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class ResetUpper extends CommandBase { + double speed; + public ResetUpper(double s) { + requires(mainArm); + speed = s; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + System.out.println("testUpper"); + mainArm.disable(); + mainArm.resetUpper(speed); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return mainArm.getTopPressed(); + } + // Called once after isFinished returns true + protected void end() { + mainArm.moveArm(0); + } + // 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 a71ef9f..7a19462 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -4,14 +4,18 @@ import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive; import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; public class MainArm extends PIDSubsystem { CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor); CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); AnalogInput pot = new AnalogInput(RobotMap.armPot); + DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim); + DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim); + private double min = RobotMap.zeroDegrees; + private double max = RobotMap.ninetyDegrees; public MainArm() { super("MainArm", 0.06, 0.0, 0.002); getPIDController().setContinuous(false); - setSetpoint(RobotMap.mainArmPresetTraverse); enable(); } public void initDefaultCommand() { @@ -33,9 +37,41 @@ public class MainArm extends PIDSubsystem { public double getDegrees() { return potToDegrees(getRaw()); } + + public void resetLower(double speed){ + if(!limitSwitchBottom.get()){ + System.out.println("PRESSDE"); + moveArm(0); + return; + }else{ + System.out.println("not pressed"); + moveArm(speed); + } + } + public boolean getBottomPressed(){ + return !limitSwitchBottom.get(); + } + + public void resetUpper(double speed){ + if(!limitSwitchTop.get()){ + System.out.println("PRESSDE"); + moveArm(0); + return; + }else{ + System.out.println("not pressed"); + moveArm(speed); + } + } + public boolean getTopPressed(){ + return !limitSwitchTop.get(); + } private double potToDegrees(double pot) { - double min = RobotMap.zeroDegrees; - double max = RobotMap.ninetyDegrees; + if(!limitSwitchBottom.get()){ + System.out.println("got"); + min = getRaw(); + }else if(!limitSwitchTop.get()){ + max = getRaw(); + } System.out.println((pot - min) / (Math.abs(min - max) / 90)); return (pot - min) / (Math.abs(min - max) / 90); }