Skip to content

Commit

Permalink
Merge branch 'feature-swag' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
omn0mn0m committed Mar 26, 2015
2 parents 45588ed + 7ef3608 commit 93b2a4c
Show file tree
Hide file tree
Showing 11 changed files with 168 additions and 53 deletions.
6 changes: 5 additions & 1 deletion src/org/team708/robot/AutoConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,13 @@ public final class AutoConstants {
*/
public static final double NINETY_DEGREE_TURN = 85.0;
public static final double TURN_SPEED = 0.6;

public static final double CLAW_LENGTH = 22.0; // The length, in inches, of the claw
public static final double ROBOT_LENGTH = 49.0;

public static final double TOTE_TO_AUTOZONE_DISTANCE = 84.0; // inches
public static final double ENCODER_SPEED = 0.45;

public static final double INDEXER_UP_DISTANCE = 13.0;
public static final double INTAKE_ONE_TOTE_TIME = 3.0;
public static final double FIELD_WIDTH = 310;
Expand All @@ -23,7 +27,7 @@ public final class AutoConstants {
* Container Spin Tote
*/
public static final double CONTAINER_TOTE_TURN_ANGLE = 167.0;
public static final double CONTAINER_TOTE_TO_TOTE_DISTANCE = 38.0;
public static final double CONTAINER_TOTE_TO_TOTE_DISTANCE = 18.0;

/*
* Container Tote
Expand Down
4 changes: 3 additions & 1 deletion src/org/team708/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ public final class Constants {
* Game Elements
*/
public static final double TOTE_HEIGHT = 14.0;
public static final double SCORE_TOTE = 1.0;

/*
* Indexer
Expand All @@ -59,6 +58,9 @@ public final class Constants {
public static final double INDEXER_SPROCKET_DIAMETER = 1.8;
public static final int TOTE_LIMIT = 4;

public static final double ADJUST_UP = 1.0;
public static final double ADJUST_DOWN = 0.5;

/*
* Drivetrain
*/
Expand Down
12 changes: 6 additions & 6 deletions src/org/team708/robot/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ public class OI {
* Driver Button Assignment
*/
// Drivetrain Buttons
private static final int TOGGLE_BRAKE_MODE_BUTTON = Gamepad.button_B;
private static final int SCORE_TOTE_BUTTON = Gamepad.button_A;
private static final int ADJUST_DOWN_BUTTON = Gamepad.button_B;
private static final int ADJUST_UP_BUTTON = Gamepad.button_A;
// private static final int HOLD_FOR_NO_PID_BUTTON = Gamepad.button_R_Shoulder;

// Hockey Stick Buttons
Expand All @@ -57,12 +57,12 @@ public class OI {
/*
* Driver Button Commands
*/
private static final Button toggleBrakeMode = new JoystickButton(driverGamepad, TOGGLE_BRAKE_MODE_BUTTON); // Toggles whether the drive is in all brake or all coast
private static final Button adjustDown = new JoystickButton(driverGamepad, ADJUST_DOWN_BUTTON); // Toggles whether the drive is in all brake or all coast
public static final Button toggleHockeyStick = new JoystickButton(driverGamepad, TOGGLE_HOCKEY_STICK_BUTTON); // Toggles the hockey stick
// public static final Button holdForNoPID = new JoystickButton(driverGamepad, HOLD_FOR_NO_PID_BUTTON);
// public static final Button toggleIntakePower = new JoystickButton(driverGamepad, toggleIntakePowerButton); // Toggles the intake power on/off
// public static final Button toggleIntakeDirection = new JoystickButton(driverGamepad, toggleIntakeDirectionButton); // Toggles the intake direction
public static final Button scoreTote = new JoystickButton(driverGamepad, SCORE_TOTE_BUTTON); // Raises Totes up to place on step and/or plateform
public static final Button adjustUp = new JoystickButton(driverGamepad, ADJUST_UP_BUTTON); // Raises Totes up to place on step and/or plateform
/*
* Operator Button Commands
*/
Expand All @@ -81,12 +81,12 @@ public OI() {
/*
* Driver Commands to be called by button
*/
toggleBrakeMode.whenPressed(new ToggleBrakeMode());
adjustDown.whenPressed(new AdjustIndexer(false));
// toggleHockeyStick.whenPressed(new ToggleHockeyStick());
// holdForNoPID.whileHeld(new HoldDisablePID());
// toggleIntakePower.whenPressed(new TogglePower());
// toggleIntakeDirection.whenPressed(new ToggleDirection());
scoreTote.whenPressed(new ScoreTote());
adjustUp.whenPressed(new AdjustIndexer(true));

/*
* Operator Commands to be called by button
Expand Down
3 changes: 2 additions & 1 deletion src/org/team708/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
import org.team708.robot.commands.autonomous.encoder.ContainerToteSpinTote;
import org.team708.robot.commands.autonomous.encoder.ContainerToteTote;
import org.team708.robot.commands.autonomous.encoder.RobotToAutozone;
import org.team708.robot.commands.autonomous.encoder.ThreeTotes;
import org.team708.robot.commands.autonomous.encoder.ToteToAutozone;
import org.team708.robot.commands.autonomous.optical.ContainerToAutoZoneByOptical;
import org.team708.robot.commands.autonomous.optical.ContainerSpinToteByOptical;
Expand Down Expand Up @@ -184,7 +185,7 @@ private void queueAutonomousModes() {
// autonomousMode.addObject("Drive in Square", new DriveInSquare());
// autonomousMode.addObject("Follow Tote", new FollowYellowTote());
// autonomousMode.addObject("Clear Step", new ClearStep());
// autonomousMode.addObject("Three Totes", new ThreeTotes());
autonomousMode.addObject("Three Totes", new ThreeTotes());
// autonomousMode.addObject("Hockey Stick Shove All", new HockeyStickClearToAutoZone());
// autonomousMode.addObject("Three Containers To Auto", new ThreeContainersToAuto());
SmartDashboard.putData("Autonomous Selection", autonomousMode);
Expand Down
20 changes: 11 additions & 9 deletions src/org/team708/robot/commands/autonomous/optical/ContainerSpinToteByOptical.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import org.team708.robot.AutoConstants;
import org.team708.robot.commands.autonomous.steps.DriveOpticalAndEncoder;
import org.team708.robot.commands.claw.CloseClaw;
import org.team708.robot.commands.claw.OpenClaw;
import org.team708.robot.commands.clawElevator.DecrementClawOne;
import org.team708.robot.commands.clawElevator.IncrementClawOne;
import org.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
import org.team708.robot.commands.drivetrain.TurnToDegrees;
Expand All @@ -22,19 +24,19 @@ public ContainerSpinToteByOptical() {
addSequential(new CloseClaw());
addSequential(new IncrementClawOne());

// Gets the tote
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.CONTAINER_TOTE_TURN_ANGLE));
addSequential(new WaitCommand(0.1));
addSequential(new DriveStraightToEncoderDistance(AutoConstants.CONTAINER_TOTE_TO_TOTE_DISTANCE, AutoConstants.ENCODER_SPEED));
addParallel(new IntakeByTime(12));
// addSequential(new DriveToIRDistance(Constants.IR_HAS_TOTE_DISTANCE, IR_TOLERANCE));
addSequential(new IndexerUp(false));
// Drops the container on the tote
addSequential(new IncrementClawOne());
addSequential(new DriveStraightToEncoderDistance(AutoConstants.CONTAINER_TOTE_TO_TOTE_DISTANCE, AutoConstants.ENCODER_SPEED, false));
addSequential(new DecrementClawOne());
addParallel(new OpenClaw());
addSequential(new DecrementClawOne());
addSequential(new CloseClaw());

// Go to auto zone
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.NINETY_DEGREE_TURN));
addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE));
addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE, false));

//Turn 90 degrees counterclockwise
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.NINETY_DEGREE_TURN));
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.NINETY_DEGREE_TURN));
}
}
20 changes: 12 additions & 8 deletions src/org/team708/robot/commands/autonomous/optical/ContainerToteSpinToteByOptical.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import org.team708.robot.AutoConstants;
import org.team708.robot.commands.autonomous.steps.DriveOpticalAndEncoder;
import org.team708.robot.commands.claw.CloseClaw;
import org.team708.robot.commands.claw.OpenClaw;
import org.team708.robot.commands.clawElevator.*;
import org.team708.robot.commands.drivetrain.*;
import org.team708.robot.commands.indexer.*;
Expand All @@ -21,26 +22,29 @@ public ContainerToteSpinToteByOptical() {

// Picks up one container
addSequential(new CloseClaw());
addParallel(new IncrementClawOne());
addSequential(new IncrementClawOne());

// Gets the first tote
// addSequential(new DriveToIRDistance(Constants.IR_HAS_TOTE_DISTANCE, IR_TOLERANCE));
addParallel(new IncrementClawOne());
addSequential(new DriveStraightToEncoderDistance(AutoConstants.TOTE_DISTANCE_ONE, AutoConstants.ENCODER_SPEED));
addSequential(new IndexerUpAuto(AutoConstants.INDEXER_UP_DISTANCE, false));
addParallel(new IndexerUpAuto(AutoConstants.INDEXER_UP_DISTANCE, false));
addSequential(new WaitCommand(0.5));

// Gets the second tote
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.TOTE_TWO_TURN_ANGLE));
addSequential(new WaitCommand(0.1));
addSequential(new DriveStraightToEncoderDistance(AutoConstants.TOTE_DISTANCE_SECOND, AutoConstants.ENCODER_SPEED));
addSequential(new DriveStraightToEncoderDistance(AutoConstants.TOTE_DISTANCE_SECOND, AutoConstants.ENCODER_SPEED, false));
// addSequential(new DriveToIRDistance(Constants.IR_HAS_TOTE_DISTANCE, IR_TOLERANCE));
addParallel(new IndexerUpAuto(AutoConstants.INDEXER_UP_DISTANCE, false));
addSequential(new DecrementClawOne());
addParallel(new OpenClaw());
addSequential(new DecrementClawOne());
addSequential(new CloseClaw());

// Go to auto zone
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.NINETY_DEGREE_TURN));
addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE));
addSequential(new DriveOpticalAndEncoder(AutoConstants.TOTE_TO_AUTOZONE_DISTANCE, false));

//Turn 90 degrees counterclockwise
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, -AutoConstants.NINETY_DEGREE_TURN));
addSequential(new TurnToDegrees(AutoConstants.TURN_SPEED, AutoConstants.NINETY_DEGREE_TURN));

// Add Commands here:
// e.g. addSequential(new Command1());
Expand Down
59 changes: 48 additions & 11 deletions src/org/team708/robot/commands/autonomous/steps/DriveOpticalAndEncoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,28 @@ public class DriveOpticalAndEncoder extends Command {
private boolean leftPlatform = false;

private double distance;

private boolean goForward;

double moveSpeed;

public DriveOpticalAndEncoder(double distance) {
// Use requires() here to declare subsystem dependencies
public DriveOpticalAndEncoder(double distance, boolean goForward) {
// Use requires() here to declare subsystem dependencies
requires(Robot.drivetrain);

this.distance = distance;
if (goForward) {
this.distance = distance;
moveSpeed = AutoConstants.ENCODER_SPEED;
} else {
this.distance = -distance;
moveSpeed = -AutoConstants.ENCODER_SPEED;
}

this.goForward = goForward;
}

public DriveOpticalAndEncoder(double distance) {
this(distance, true);
}

// Called just before this Command runs the first time
Expand All @@ -38,22 +54,43 @@ protected void execute() {
}
}

Robot.drivetrain.haloDrive(AutoConstants.ENCODER_SPEED, 0.0, false);
Robot.drivetrain.haloDrive(moveSpeed, 0.0, false);
}

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return (Robot.drivetrain.getEncoderDistance() >= distance) || (reachedPlatform && leftPlatform);
if (goForward) {
return (Robot.drivetrain.getEncoderDistance() >= distance) || (reachedPlatform && leftPlatform);
} else {
return (Robot.drivetrain.getEncoderDistance() <= distance) || (reachedPlatform && leftPlatform);
}
}

// Called once after isFinished returns true
protected void end() {
if ((reachedPlatform && leftPlatform) && (Robot.drivetrain.getEncoderDistance() < distance)) {
while(Robot.drivetrain.getEncoderDistance() < AutoConstants.CLAW_LENGTH) {
if (Robot.drivetrain.getEncoderDistance() >= distance) {
break;
}
Robot.drivetrain.haloDrive(AutoConstants.ENCODER_SPEED, 0.0, false);
boolean needsEncoderMove;

if (goForward) {
needsEncoderMove = (Robot.drivetrain.getEncoderDistance() < distance);
} else {
needsEncoderMove = (Robot.drivetrain.getEncoderDistance() > distance);
}

if ((reachedPlatform && leftPlatform) && needsEncoderMove) {
if (goForward) {
while(Robot.drivetrain.getEncoderDistance() < AutoConstants.CLAW_LENGTH) {
if (Robot.drivetrain.getEncoderDistance() >= distance) {
break;
}
Robot.drivetrain.haloDrive(moveSpeed, 0.0, false);
}
} else {
while(Robot.drivetrain.getEncoderDistance() > -AutoConstants.CLAW_LENGTH) {
if (Robot.drivetrain.getEncoderDistance() >= distance) {
break;
}
Robot.drivetrain.haloDrive(moveSpeed, 0.0, false);
}
}
Robot.drivetrain.stop();
} else {
Expand Down
39 changes: 39 additions & 0 deletions src/org/team708/robot/commands/claw/OpenClaw.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
package org.team708.robot.commands.claw;

import org.team708.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

/**
*
*/
public class OpenClaw extends Command {

public OpenClaw() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
}

// Called just before this Command runs the first time
protected void initialize() {
Robot.claw.openClaw();
}

// 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() {
}
}
39 changes: 27 additions & 12 deletions src/org/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,33 @@
*/
public class DriveStraightToEncoderDistance extends Command {

private double targetDistance = 0;
private final double rotate = 0;
private double targetDistance;
private final double rotate = 0.0;
private double speed;

private boolean goForward;

public DriveStraightToEncoderDistance(double distance) {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(Robot.drivetrain);
targetDistance = distance;
this.speed = Constants.DRIVE_MOTOR_MAX_SPEED;
this(distance, Constants.DRIVE_MOTOR_MAX_SPEED);
}

public DriveStraightToEncoderDistance(double distance, double speed) {
this(distance);
this.speed = speed;
this(distance, speed, true);
}

public DriveStraightToEncoderDistance(double distance, double speed, boolean goForward) {
// Use requires() here to declare subsystem dependencies
requires(Robot.drivetrain);

if(goForward) {
targetDistance = distance;
this.speed = speed;
} else {
targetDistance = -distance;
this.speed = -speed;
}

this.goForward = goForward;
}

// Called just before this Command runs the first time
Expand All @@ -46,7 +58,11 @@ protected void execute() {

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return (Robot.drivetrain.getEncoderDistance() >= targetDistance);
if(goForward) {
return (Robot.drivetrain.getEncoderDistance() >= targetDistance);
} else {
return (Robot.drivetrain.getEncoderDistance() <= targetDistance);
}
}

// Called once after isFinished returns true
Expand All @@ -59,7 +75,6 @@ protected void end() {
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
Robot.drivetrain.stop();
Robot.drivetrain.resetEncoder();
end();
}
}
Loading

0 comments on commit 93b2a4c

Please sign in to comment.