From 027452180cc75fa58cd6305d9eb351ec64b2345f Mon Sep 17 00:00:00 2001 From: Meryem Esa Date: Sat, 5 Mar 2016 11:20:24 -0800 Subject: [PATCH] delete commands that moved intake arm to a specific angle --- .../robot/commands/auton/CompactRobot.java | 7 +- .../intakearm/MoveIntakeArmToAngle.java | 68 ------------------- .../intakearm/MoveIntakeArmToLevel.java | 31 --------- .../intakearm/PushDownChevalDeFrise.java | 66 ------------------ 4 files changed, 2 insertions(+), 170 deletions(-) delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java delete mode 100755 src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java b/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java index 7e3ba9dc..8596cd3b 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/CompactRobot.java @@ -1,8 +1,5 @@ package org.usfirst.frc.team3501.robot.commands.auton; -import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArmToAngle; -import org.usfirst.frc.team3501.robot.subsystems.IntakeArm; - import edu.wpi.first.wpilibj.command.CommandGroup; /** @@ -10,8 +7,8 @@ import edu.wpi.first.wpilibj.command.CommandGroup; */ public class CompactRobot extends CommandGroup { + // TODO: implement CompactRobot with updated Robot capabilities public CompactRobot() { - addSequential(new MoveIntakeArmToAngle(IntakeArm.potAngles[3], - IntakeArm.moveIntakeArmSpeed)); + } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java deleted file mode 100644 index 510146cc..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java +++ /dev/null @@ -1,68 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class MoveIntakeArmToAngle extends Command { - private double currentAngle; - private double targetAngle; - private double targetSpeed; - private double calculatedSpeed; - private double SENSITIVITY_THRESHOLD = 0.1; - private double MIN_SPEED = .3; - - public MoveIntakeArmToAngle(double angle, double speed) { - requires(Robot.intakeArm); - targetAngle = angle; - targetSpeed = speed; - - } - - @Override - protected void initialize() { - - // set the arm speed to the calculated angle - double speed = getCalculatedSpeed(); - - if (speed < MIN_SPEED && speed > 0) - speed = Math.signum(getCalculatedSpeed()) * MIN_SPEED; - Robot.intakeArm.setArmSpeed(speed); - } - - @Override - protected void execute() { - // set the arm speed to the calculated angle - Robot.intakeArm.setArmSpeed(calculatedSpeed); - } - - private double getError() { - // targetAngle - currentAngle = error - return Robot.intakeArm.getArmAngle() - targetAngle; - } - - private double getCalculatedSpeed() { - // if the arm has to move up -- the speed will be + - // if the arm has to move down -- the speed will be - - - return (getError() / targetAngle) * targetSpeed; - } - - @Override - protected boolean isFinished() { - // if the arm's angle is close enough to the target value return true - // else return false - return (Math.abs(getError()) <= SENSITIVITY_THRESHOLD); - } - - @Override - protected void end() { - Robot.intakeArm.stop(); - } - - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java deleted file mode 100755 index 11a95270..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToLevel.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Constants.IntakeArm; -import org.usfirst.frc.team3501.robot.Robot; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * This command group will move the arm to the specified level. Levels - * correspond to the angles stored in the potAngles array list saved in the - * IntakeArm subsystem. (starting from level 0) - * - * pre-condition: level >= 0 && level < potAngles.length - * - * post-condition: arm has moved to specified level - * - * @author Meryem - */ -public class MoveIntakeArmToLevel extends CommandGroup { - - public MoveIntakeArmToLevel(int level) { - requires(Robot.intakeArm); - - if (level < 0 || level >= Robot.intakeArm.potAngles.length) - this.end(); - - addSequential(new MoveIntakeArmToAngle(Robot.intakeArm.potAngles[level], - IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); - - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java deleted file mode 100755 index 9852a1c0..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.intakearm; - -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -/** - * This command group uses the intake arm to push down the moving parts of the - * cheval de frise so that the robot can drive over it - * - * pre-condition: the robot is flush against the border of the outerworks (flush - * against the ramp) - * - * post condition: intake arm has pushed the cheval de frise down - * - */ -public class PushDownChevalDeFrise extends CommandGroup { - // distances are in inches - private final double MIN_ANGLE = Robot.intakeArm.potAngles[2]; // TODO: find - // min angle - // the intake - // arm has - // to be to be over the cheval de frise - private final double RAMP_DIST = 12; - private final double MAX_TIMEOUT_RAMP = 1; // TODO: check that one second is - // enough - private final double LOWEST_ANGLE = Robot.intakeArm.potAngles[0]; - private final double DEFAULT_ARM_SPEED = 0.4; - - public PushDownChevalDeFrise() { - requires(Robot.intakeArm); - /** - * if the arm is high enough - above cheval de frise height (find this from - * the potangle) - * - * go forward until arm is over cheval de frise - * - * move arm down to floor level -- or as much as possible - * ------------------------------------------------------------------------- - * - * if the arm is beneath the cheval de frise - * - * move the arm up so it's high enough - * - * move forward a little - * - * move arm down - **/ - - if (Robot.intakeArm.getArmAngle() > MIN_ANGLE) { - addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP)); - addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED)); - - } else { - addSequential(new MoveIntakeArmToAngle(MIN_ANGLE + 3, DEFAULT_ARM_SPEED)); // TODO: - // check that adding - // 3 is correct - - addSequential(new DriveDistance(RAMP_DIST, MAX_TIMEOUT_RAMP)); - addSequential(new MoveIntakeArmToAngle(LOWEST_ANGLE, DEFAULT_ARM_SPEED)); - - } - - } -} -- 2.30.2