From 65424d4e84df5de0b64de5189a0c8daa458cf377 Mon Sep 17 00:00:00 2001 From: Meryem Esa Date: Tue, 16 Feb 2016 12:44:12 -0800 Subject: [PATCH] implement PushDownChevalDeFrise command group --- .../intakearm/PushDownChevalDeFrise.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java index bccdc7d0..f877cc64 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/PushDownChevalDeFrise.java @@ -1,6 +1,7 @@ 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; @@ -15,6 +16,14 @@ import edu.wpi.first.wpilibj.command.CommandGroup; * */ public class PushDownChevalDeFrise extends CommandGroup { + // distances are in inches + private final double MIN_ANGLE = 0; // 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 = 0; // TODO: find angle + private final double DEFAULT_ARM_SPEED = 0.4; public PushDownChevalDeFrise() { requires(Robot.intakeArm); @@ -33,7 +42,22 @@ public class PushDownChevalDeFrise extends CommandGroup { * * 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