Skip to content

Commit

Permalink
Merge branch 'master' into All/#305-SelfTestFramework
Browse files Browse the repository at this point in the history
  • Loading branch information
@BenBernardCIS
BenBernardCIS committed Apr 21, 2018
2 parents 319cd38 + 3884fa7 commit 7ec5c7e
Show file tree
Hide file tree
Showing 25 changed files with 19,605 additions and 6,130 deletions.
33 changes: 24 additions & 9 deletions src/main/java/org/frc5687/powerup/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,12 @@ public enum typeOfTurn {
rightOnly
}

public enum DriveTrainBehavior {
bothSides,
leftOnly,
rightOnly
}


public class Lights {
// Values obtained from page 16- of http://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf
Expand Down Expand Up @@ -113,7 +119,7 @@ public class Intake {
public static final double SCALE_DROP_SPEED = -0.9;
public static final double SCALE_SHOOT_SPEED = -0.70;
public static final double SCALE_SHOOT_SPEED_SECOND_CUBE = -0.75;
public static final double SWITCH_DROP_SPEED = -0.42;
public static final double SWITCH_DROP_SPEED = -0.22;

public static final double HC_MIN_SPEED = 0.1;
public static final double HC_MIN_CURRENT = 0.5;
Expand Down Expand Up @@ -190,7 +196,7 @@ public class MaxJerk {

public class TrajectoryFollowing {
public class Talon {
public static final double kP = 0.15; // Talon doesn't use kP
public static final double kP = 0.3; // Talon doesn't use kP
public static final double kI = 0.001;//02;
public static final double kD = 0.0;
public static final double kF = 0.2; // 0.28 works well
Expand Down Expand Up @@ -317,21 +323,24 @@ public class Carriage {
public static final boolean MOTOR_INVERTED = true;
public static final double HC_MIN_SPEED = 0.1; // Minimum speed to triggr the amp/isHalthy check
public class HoldSpeeds {
public static final double PAST_TOP_PROTO = 0.42;
public static final double PAST_TOP_PROTO = 0.5;
public static final double PAST_TOP_GRETA = 0.5;

public static final double PAST_NEG_20_PROTO = 0.42;
public static final double PAST_NEG_20_PROTO = 0.6;
public static final double PAST_NEG_20_GRETA = 0.5;

public static final double PAST_NEG_50_PROTO = 0.42;
public static final double PAST_NEG_50_PROTO = 0.6;
public static final double PAST_NEG_50_GRETA = 0.5;

public static final double PAST_NEG_100_PROTO = 0.42;
public static final double PAST_NEG_100_PROTO = 0.5;
public static final double PAST_NEG_100_GRETA = 0.5;

public static final double PAST_NEG_200_PROTO = 0.3;
public static final double PAST_NEG_200_PROTO = 0.42;
public static final double PAST_NEG_200_GRETA = 0.0;

public static final double PAST_NEG_300_PROTO = 0.42;
public static final double PAST_NEG_300_GRETA = 0.0;

public static final double PAST_NEG_400_PROTO = 0.2;
public static final double PAST_NEG_400_GRETA = 0.0;

Expand Down Expand Up @@ -434,6 +443,7 @@ public class Pot {
public static final double DRIVE = 33.0;

public static final double SCALE = 150.0;
public static final double SCALE_MAX = 163.0;
public static final double SWITCH_HEIGHT_WITH_CARRIAGE = 100;
public static final double SWITCH_HEIGHT_BROKEN_CARRIAGE = 72; // I guess this shouldn't be lower, but I'm just removing a magic number..
public static final double switchHeightWithCarriageHalfwayUp = 91;
Expand All @@ -443,11 +453,16 @@ public class Pot {
public static final double LENGTH = 34.0;

public class HoldSpeeds {
public static final double PAST_160_CUBE_PROTO = 0.0;
public static final double PAST_160_CUBE_PROTO = 0.2;
public static final double PAST_160_CUBE_GRETA = 0.25;
public static final double PAST_160_NO_CUBE_PROTO = 0.0;
public static final double PAST_160_NO_CUBE_PROTO = 0.2;
public static final double PAST_160_NO_CUBE_GRETA = 0.25;

public static final double PAST_150_CUBE_PROTO = 0.2;
public static final double PAST_150_CUBE_GRETA = 0.25;
public static final double PAST_150_NO_CUBE_PROTO = 0.2;
public static final double PAST_150_NO_CUBE_GRETA = 0.25;

public static final double PAST_90_CUBE_PROTO = 0.0;
public static final double PAST_90_CUBE_GRETA = 0.1;
public static final double PAST_90_NO_CUBE_PROTO = 0.0;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/frc5687/powerup/robot/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@ public double getCarriageSpeed() {
}

public double getArmSpeed() {
double driver = driverArmUp.get() ? -0.75 : (driverArmDown.get() ? 0.5 : 0);
double driver = driverArmUp.get() ? -0.75 : (driverArmDown.get() ? 1.0 : 0);
double operator = getSpeedFromAxis(operatorGamepad, ButtonNumbers.RIGHT_AXIS);
double speed = Helpers.absMax(operator, driver);
speed = Helpers.applySensitivityFactor(speed,Constants.Arm.SENSITIVITY);
Expand Down
17 changes: 16 additions & 1 deletion src/main/java/org/frc5687/powerup/robot/commands/auto/AutoAlign.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@ public class AutoAlign extends Command implements PIDOutput {
private DriveTrain driveTrain;
private AHRS imu;

private Constants.DriveTrainBehavior _driveTrainBehavior;

private double _tolerance;

public AutoAlign(Robot robot, double angle) {
Expand Down Expand Up @@ -62,13 +64,18 @@ public AutoAlign(Robot robot, double angle, double speed, long timeout, double t
}

public AutoAlign(DriveTrain driveTrain, AHRS imu, double angle, double speed, long timeout, double tolerance) {
this(driveTrain, imu, angle, speed, timeout, tolerance, Constants.DriveTrainBehavior.bothSides);
}

public AutoAlign(DriveTrain driveTrain, AHRS imu, double angle, double speed, long timeout, double tolerance, Constants.DriveTrainBehavior driveTrainBehavior) {
requires(driveTrain);
this.angle = angle;
this.speed = speed;
this.driveTrain = driveTrain;
this.imu = imu;
_timeout = timeout;
_tolerance = tolerance;
_driveTrainBehavior = driveTrainBehavior;
}

@Override
Expand Down Expand Up @@ -105,7 +112,15 @@ private void actOnPidOut() {
if (pidOut < 0 && pidOut > -Align.MINIMUM_SPEED) {
pidOut = -Align.MINIMUM_SPEED;
}
driveTrain.setPower(pidOut, -pidOut, true); // positive output is clockwise
if (_driveTrainBehavior == Constants.DriveTrainBehavior.bothSides) {
driveTrain.setPower(pidOut, -pidOut, true); // positive output is clockwise
} else if (_driveTrainBehavior == Constants.DriveTrainBehavior.rightOnly) {
driveTrain.setLeftVelocityIPS(0);
driveTrain.setRightPower(-pidOut);
} else if (_driveTrainBehavior == Constants.DriveTrainBehavior.leftOnly) {
driveTrain.setLeftPower(pidOut);
driveTrain.setRightVelocityIPS(0);
}
}

@Override
Expand Down
64 changes: 44 additions & 20 deletions src/main/java/org/frc5687/powerup/robot/commands/auto/AutoGroup.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,9 @@ public AutoGroup(int mode, int position, int switchSide, int scaleSide, long del
int carriageTopPosition;
MoveArmToSetpointPID armPid;

addSequential(new AutoWaitForMillis(delayInMillis));
if (delayInMillis != 0) {
addSequential(new AutoWaitForMillis(delayInMillis));
}

switch (mode) {
case Constants.AutoChooser.Mode.STAY_PUT:
Expand Down Expand Up @@ -348,6 +350,9 @@ public AutoGroup(int mode, int position, int switchSide, int scaleSide, long del
break;
}
break;
case 10:
addSequential(new AggressiveCrossAutoLine(robot));
break;
case 11:
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 90, 1.0, 2500, 2.0));
break;
Expand Down Expand Up @@ -449,7 +454,7 @@ private void centerLeftToRightSwitchThenPickupSecondCube(Robot robot) {
double armSwitchAngle = robot.getCarriage().isHealthy() ? Constants.Arm.Pot.SWITCH_HEIGHT_WITH_CARRIAGE : Constants.Arm.Pot.SWITCH_HEIGHT_BROKEN_CARRIAGE;
double carriageTopPosition = Constants.Carriage.ENCODER_TOP_COMP;
addParallel(new MoveArmToSetpointPID(robot.getArm(), armSwitchAngle, true));
addParallel(new AutoEjectAfterNMillis(robot.getIntake(), Constants.Intake.SWITCH_DROP_SPEED, CenterLeftToRightSwitchForSecondCube.duration - 100));
addParallel(new AutoEjectAfterNMillis(robot.getIntake(), Constants.Intake.SWITCH_DROP_SPEED, CenterLeftToRightSwitchForSecondCube.duration - 200));
addSequential(new CenterLeftToRightSwitchForSecondCube(robot));
/*
Move Carriage Down and backup
Expand All @@ -460,7 +465,8 @@ private void centerLeftToRightSwitchThenPickupSecondCube(Robot robot) {
Move Arm Down while aligning
*/
addParallel(new MoveArmToSetpointPID(robot.getArm(), armIntakeAngle));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -20, Constants.Auto.Align.SPEED));
// -12.9
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -12.9, Constants.Auto.Align.SPEED));
/*
Intake second cube
*/
Expand Down Expand Up @@ -524,33 +530,42 @@ private void rightScaleBackup(Robot robot) {

private void farLeftToLeftScale(Robot robot) {
//addParallel(new PrepIntakeForScale(robot, 100, 3000, true));
addParallel(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.SCALE));
addParallel(new MoveCarriageToSetpointPIDButWaitForNInchesFirst(robot.getDriveTrain(), robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP, 140));
addSequential(new FarLeftToLeftScale(robot));
// Faster path makes it so we don't need auto aline, except if we exclude it we need to turn to 105deg to get 2nd cube
addParallel(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.SCALE_MAX));
addParallel(new MoveCarriageToSetpointPIDButWaitForNInchesFirst(robot.getDriveTrain(), robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP, 136));
//addSequential(new FarLeftToLeftScale(robot));
addSequential(new FarLeftToLeftScaleWithTightTurnFour(robot));
addSequential(new AutoEject(robot.getIntake(), Constants.Intake.SCALE_DROP_SPEED));
//addParallel(new AutoEjectAfterNMillis(robot.getIntake(), Constants.Intake.SCALE_DROP_SPEED, FarLeftToLeftScaleWithTightTurn.duration - 20));
// Faster path makes it so we don't need auto align, except if we exclude it we need to turn to 105deg to get 2nd cube
// Timeout used to be 1000, but because of too high scrub we would time out.
// We changed the min. speed for auto align, so we don't "need" a greater timeout, but we haven't been able to test
// it, so if it appears that we're stalling for too long, bump up the min. speed and or decrease this timeout
addSequential(new AutoAlign(robot, 40, Constants.Auto.Align.SPEED, 2500, 1.0));
addSequential(new AutoEject(robot.getIntake(), Constants.Intake.SCALE_DROP_SPEED));
//addSequential(new AutoAlign(robot, 40, Constants.Auto.Align.SPEED, 2500, 1.0));
}

private void leftScaleToSecondCube(Robot robot) {
/*
Align towards second cube
*/
addParallel(new MoveCarriageToSetpointPID(robot.getCarriage(), Constants.Carriage.ENCODER_BOTTOM_COMP));
// should be 149
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 160, Constants.Auto.Align.SPEED));
//addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 135, Constants.Auto.Align.SPEED));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 90, Constants.Auto.Align.SPEED, 2000, Constants.Auto.Align.TOLERANCE, Constants.DriveTrainBehavior.rightOnly));
/*
Prepare intake
*/
addSequential(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.INTAKE));
class AlignAndPrepareIntake extends CommandGroup {
AlignAndPrepareIntake(Robot robot) {
addParallel(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.INTAKE));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 158, Constants.Auto.Align.SPEED, 2000, Constants.Auto.Align.TOLERANCE, Constants.DriveTrainBehavior.bothSides));
}
}
addSequential(new AlignAndPrepareIntake(robot));
/*
Approach second cube and intake
*/
addParallel(new AutoIntake(robot.getIntake()));
addSequential(new LeftScaleToCube(robot));
addSequential(new LeftScaleToCubeAlternativeTwo(robot));
//addSequential(new LeftScaleToCube(robot));
addSequential(new AbortIfNoCubeDetected(robot));
}

Expand All @@ -560,7 +575,8 @@ private void secondCubeToLeftScale(Robot robot) {
*/
addParallel(new MoveCarriageToSetpointPID(robot.getCarriage(), Constants.Carriage.ENCODER_DRIVE_COMP));
addParallel(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.SCALE));
addSequential(new LeftScaleToCubeReversed(robot));
//addSequential(new LeftScaleToCubeReversed(robot));
addSequential(new LeftScaleToCubeAlternativeTwoReverse(robot));
/*
Prepare intake
*/
Expand All @@ -570,7 +586,8 @@ private void secondCubeToLeftScale(Robot robot) {
//addSequential(new AutoAlign(robot, -140, 1500, 7));
addParallel(new MoveCarriageToSetpointPID(robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP));
addSequential(new AbortIfCubeNotSecured(robot));
addSequential(new AutoAlign(robot, 22.8));
//addSequential(new AutoAlign(robot, 22.8));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 45, Constants.Auto.Align.SPEED, 2000, Constants.Auto.Align.TOLERANCE, Constants.DriveTrainBehavior.rightOnly));
addSequential(new AutoEject(robot.getIntake(), Constants.Intake.SCALE_SHOOT_SPEED_SECOND_CUBE));
}

Expand All @@ -584,13 +601,20 @@ private void secondCubeToLeftSwitch(Robot robot) {
}

private void farLeftToRightScale(Robot robot) {
addParallel(new AutoZeroCarriageThenLower(robot));
addSequential(new FarLeftToRightScaleDeadPartOne(robot));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 88, Constants.Auto.Align.SPEED, 5000, 1.0));
addParallel(new PrepIntakeForScale(robot, 1600, false));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 88, Constants.Auto.Align.SPEED, 5000, 1.0, Constants.DriveTrainBehavior.leftOnly));
addParallel(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Pot.SCALE_MAX));
//addParallel(new MoveCarriageToSetpointPIDButWaitForNInchesFirst(robot.getDriveTrain(), robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP, 100));
addSequential(new FarLeftToRightScaleDeadPartTwo(robot));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -25, Constants.Auto.Align.SPEED, 3000));
addParallel(new AutoEjectAfterNMillis(robot.getIntake(), Constants.Intake.DROP_SPEED, FarLeftToRightScaleDeadPartThree.duration - 340));
class RaiseCarriageAfterWaitingNMillis extends CommandGroup {
public RaiseCarriageAfterWaitingNMillis(Robot robot, long millis) {
addSequential(new AutoWaitForMillis(millis));
addSequential(new AutoZeroCarriage(robot.getCarriage()));
}
}
addParallel(new RaiseCarriageAfterWaitingNMillis(robot, 300));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -25, Constants.Auto.Align.SPEED, 3000, 1.0, Constants.DriveTrainBehavior.bothSides));
addParallel(new AutoEjectAfterNMillis(robot.getIntake(), Constants.Intake.SCALE_DROP_SPEED, FarLeftToRightScaleDeadPartThree.duration - 220));
addSequential(new FarLeftToRightScaleDeadPartThree(robot));
/*
Go to intake position and turn towards second cube
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/frc5687/powerup/robot/commands/auto/DynamicPathCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ protected boolean loadPath() {

@Override
protected void initialize() {
DriverStation.reportError("Starting DynamicPathCommand", false);
DriverStation.reportError("Starting DynamicPathCommand (" + path.getName() + ")", false);
_driveTrain.resetDriveEncoders();
//_imu.reset();
endMillis = System.currentTimeMillis() + 15000;
Expand Down
Loading

0 comments on commit 7ec5c7e

Please sign in to comment.