Skip to content

Commit

Permalink
Merge pull request #315 from frc5687/Auto/#297-DefensiveOpposite
Browse files Browse the repository at this point in the history 
Auto/#297 defensive opposite
  • Loading branch information
@BenBernardCIS
BenBernardCIS authored Apr 22, 2018
2 parents e7d269b + e8d8312 commit 4fb0039
Show file tree
Hide file tree
Showing 9 changed files with 3,019 additions and 35 deletions.
315 changes: 307 additions & 8 deletions 2018-robot.iml
View file

Large diffs are not rendered by default.

20 changes: 20 additions & 0 deletions src/main/java/org/frc5687/powerup/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,9 @@ public class Auto {
public static final double MIN_IMU_ANGLE = -180;
public static final double MAX_IMU_ANGLE = 180;

public static final double MAX_PITCH = 7.0;
public static final double MAX_ROLL = 7.0;

public class Align {

public static final double SPEED = 1.0;
Expand Down Expand Up @@ -520,6 +523,23 @@ public class Mode {
public static final int SWITCH_DRIVE = 1001;
public static final int SCALE_DRIVE = 1003;
}

public class Coop {
public static final int DELAY_0 = 0;
public static final int DELAY_1 = 1;
public static final int DELAY_2 = 2;
public static final int DELAY_3 = 3;
public static final int DELAY_4 = 4;
public static final int DELAY_5 = 5;
public static final int DELAY_6 = 6;
public static final int DELAY_7 = 7;
public static final int DELAY_8 = 8;
public static final int DELAY_9 = 9;
public static final int DEFENSE = 10;
public static final int STAY_IN_LANE = 11;
}


}
public class Limits {
/***
Expand Down
24 changes: 18 additions & 6 deletions src/main/java/org/frc5687/powerup/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

public class Robot extends TimedRobot {

// I really don't like the idea of public static refrences to subsystems...
private static Robot _instance;

private Command autoCommand;

Expand Down Expand Up @@ -58,6 +58,7 @@ public void startCompetition() {

@Override
public void robotInit() {
_instance = this;
_identityFlag = new DigitalInput(RobotMap.IDENTITY_FLAG);
_isCompetitionBot = _identityFlag.get();
imu = new AHRS(SPI.Port.kMXP, (byte) 100);
Expand All @@ -78,13 +79,17 @@ public void robotInit() {
setPeriod(1 / Constants.CYCLES_PER_SECOND);

try {
// camera0 = CameraServer.getInstance().startAutomaticCapture(0);
if (isCompetitionBot()) {
camera0 = CameraServer.getInstance().startAutomaticCapture(0);
}
} catch (Exception e) {
DriverStation.reportError(e.getMessage(), true);
}

try {
// camera1 = CameraServer.getInstance().startAutomaticCapture(1);
if (isCompetitionBot()) {
camera1 = CameraServer.getInstance().startAutomaticCapture(1);
}
} catch (Exception e) {
DriverStation.reportError(e.getMessage(), true);
}
Expand Down Expand Up @@ -151,15 +156,16 @@ public void autonomousInit() {
}
int autoPosition = _autoChooser.positionSwitchValue();
int autoMode = _autoChooser.modeSwitchValue();
int coopMode = _autoChooser.coopSwitchValue();
boolean stayInYourOwnLane = _autoChooser.stayInYourOwnLane();
long autoDelayInMillis = _autoChooser.getDelayMillis();
SmartDashboard.putNumber("Auto/SwitchSide", switchSide);
SmartDashboard.putNumber("Auto/ScaleSide", scaleSide);
SmartDashboard.putNumber("Auto/Position", autoPosition);
SmartDashboard.putNumber("Auto/Mode", autoMode);
SmartDashboard.putString("Auto/Delay", Long.toString(autoDelayInMillis) + "ms");
DriverStation.reportError("Running AutoGroup with mode: " + autoMode + ", position: " + autoPosition + ", delay:" + Long.toString(autoDelayInMillis) + "ms, switchSide: " + switchSide + ", scaleSide: " + scaleSide, false);
autoCommand = new AutoGroup(autoMode, autoPosition, switchSide, scaleSide, autoDelayInMillis,stayInYourOwnLane, this);
SmartDashboard.putNumber("Auto/Coop", coopMode);
DriverStation.reportError("Running AutoGroup with mode: " + autoMode + ", position: " + autoPosition + ", delay:" + Long.toString(autoDelayInMillis) + "ms, switchSide: " + switchSide + ", scaleSide: " + scaleSide + ", coop: " + coopMode, false);
autoCommand = new AutoGroup(autoMode, autoPosition, switchSide, scaleSide, autoDelayInMillis, coopMode, this);
autoCommand.start();
}

Expand Down Expand Up @@ -292,5 +298,11 @@ public boolean isManualLightFlashRequested() {

public void requestAbortAuton() {
abortAuton = true;
DriverStation.reportError("Auto abort requested", true);
}

public static void requestAbortAutonStatic() {
_instance.requestAbortAuton();
}

}
2 changes: 1 addition & 1 deletion src/main/java/org/frc5687/powerup/robot/RobotMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ public class Climber {
public class AutoChooser {
public static final int POSITION_SWITCH = ANALOG.RIO2;
public static final int MODE_SWITCH = ANALOG.RIO3;
public static final int DELAY_SWITCH = ANALOG.navX5;
public static final int COOP_SWITCH = ANALOG.navX5;
}

public class PDP {
Expand Down
22 changes: 22 additions & 0 deletions 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 @@ -9,6 +9,7 @@
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.frc5687.powerup.robot.Constants;
import org.frc5687.powerup.robot.Robot;
Expand All @@ -31,6 +32,7 @@ public class AutoAlign extends Command implements PIDOutput {
private long _onTargetSince;
private long startTimeMillis;
private long _endTimeMillis;
private boolean _aborted = false;

private DriveTrain driveTrain;
private AHRS imu;
Expand Down Expand Up @@ -100,6 +102,25 @@ protected void initialize() {
@Override
protected void execute() {
//actOnPidOut();
// Check pitch and tilt
double pitch = imu.getPitch();
double roll = imu.getRoll();

if (Math.abs(pitch) > Constants.Auto.MAX_PITCH) {
DriverStation.reportError("Excessive pitch detected (" + pitch + ")", false);
this.controller.disable();
_aborted = true;
Robot.requestAbortAutonStatic();
}

if (Math.abs(roll) > Constants.Auto.MAX_ROLL) {
DriverStation.reportError("Excessive roll detected (" + roll + ")", false);
this.controller.disable();
_aborted = true;
Robot.requestAbortAutonStatic();
}


SmartDashboard.putBoolean("AutoAlign/onTarget", controller.onTarget());
SmartDashboard.putNumber("AutoAlign/imu", imu.getYaw());
SmartDashboard.putData("AutoAlign/pid", controller);
Expand All @@ -125,6 +146,7 @@ private void actOnPidOut() {

@Override
protected boolean isFinished() {
if (_aborted) { return true; }
if (!controller.onTarget()) {
_onTargetSince = 0;
}
Expand Down
96 changes: 89 additions & 7 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 @@ -7,18 +7,23 @@
import org.frc5687.powerup.robot.Robot;
import org.frc5687.powerup.robot.commands.*;
import org.frc5687.powerup.robot.commands.actions.IntakeToFloor;
import org.frc5687.powerup.robot.commands.actions.IntakeToDrive;
import org.frc5687.powerup.robot.commands.actions.IntakeToScale;
import org.frc5687.powerup.robot.commands.actions.IntakeToSwitch;
import org.frc5687.powerup.robot.commands.actions.IntakeToFloor;
import org.frc5687.powerup.robot.commands.auto.paths.*;
import org.frc5687.powerup.robot.commands.auto.AutoIntake;

/**
* Created by Ben Bernard on 2/2/2018.
*/
public class AutoGroup extends CommandGroup {
public AutoGroup(int mode, int position, int switchSide, int scaleSide, long delayInMillis, boolean stayInYourOwnLane, Robot robot) {
public AutoGroup(int mode, int position, int switchSide, int scaleSide, long delayInMillis, int coopMode, Robot robot) {
super();
int switchFactor = switchSide * (position );
int scaleFactor = scaleSide * (position);
boolean stayInYourOwnLane = coopMode == Constants.AutoChooser.Coop.STAY_IN_LANE;
boolean defensive = coopMode == Constants.AutoChooser.Coop.DEFENSE;
/*
if (robot.getCarriage().isHealthy()) {
addParallel(new AutoZeroCarriage(robot.getCarriage()));
Expand Down Expand Up @@ -126,57 +131,87 @@ public AutoGroup(int mode, int position, int switchSide, int scaleSide, long del
SmartDashboard.putString("Auto/Mode", "Scale Only");
switch (scaleFactor) {
case -Constants.AutoChooser.Position.FAR_LEFT:
DriverStation.reportError("AUTO: Left scale only from far left", false);
farLeftToLeftScale(robot);
break;
case Constants.AutoChooser.Position.FAR_LEFT:
if (!stayInYourOwnLane) { // Traverse allowed
if (!(stayInYourOwnLane || defensive)) { // Traverse allowed
DriverStation.reportError("AUTO: Right scale only from far left", false);
farLeftToRightScale(robot);
} else if (defensive) {
// Drop off the cube...then
DriverStation.reportError("AUTO: Defensive from far left", false);
farLeftDefensive(robot, switchSide == Constants.AutoChooser.LEFT);
} else if (switchSide == Constants.AutoChooser.LEFT) { // Traverse not allowed. Go for switch
DriverStation.reportError("AUTO: Left switch from far left", false);
farLeftToLeftSwitch(robot);
} else {
DriverStation.reportError("AUTO: Auto cross from far left", false);
buildAutoCross(robot);
}
break;
case -Constants.AutoChooser.Position.FAR_RIGHT:
if (!stayInYourOwnLane) { // Traverse allowed
if (!(stayInYourOwnLane || defensive)) { // Traverse allowed
DriverStation.reportError("AUTO: Left scale only from far right", false);
farRightToLeftScale(robot);
} else if (defensive) {
DriverStation.reportError("AUTO: Defensive from far right", false);
farRightDefensive(robot, switchSide == Constants.AutoChooser.RIGHT);
} else if (switchSide == Constants.AutoChooser.RIGHT){ // Traverse !allowed. Go for switch
DriverStation.reportError("AUTO: RIght switch from far right", false);
farRightToRightSwitch(robot);
} else {
DriverStation.reportError("AUTO: Cross the line from far right", false);
buildAutoCross(robot);
}
break;
case Constants.AutoChooser.Position.FAR_RIGHT:
DriverStation.reportError("AUTO: Right scale only from far right", false);
farRightToRightScale(robot);
break;
}
break;
case Constants.AutoChooser.Mode.SCALE_THEN_SCALE:
switch (scaleFactor) {
case -Constants.AutoChooser.Position.FAR_LEFT:
DriverStation.reportError("AUTO: Left scale x 2 from far left", false);
farLeftToLeftScale(robot);
leftScaleToSecondCube(robot);
secondCubeToLeftScale(robot);
break;
case Constants.AutoChooser.Position.FAR_LEFT:
if (!stayInYourOwnLane) { // Traverse allowed
if (!(stayInYourOwnLane || defensive)) { // Traverse allowed
DriverStation.reportError("AUTO: Right scale from far left", false);
farLeftToRightScale(robot);
} else if (switchSide == Constants.AutoChooser.LEFT) { // Traverse !allowed. Get switch.
} else if (defensive) {
// Drop off the cube...then
DriverStation.reportError("AUTO: Defensive from far left", false);
farLeftDefensive(robot, switchSide == Constants.AutoChooser.LEFT);
} else if (switchSide == Constants.AutoChooser.LEFT) { // Traverse not allowed. Go for switch
DriverStation.reportError("AUTO: Left switch from far left", false);
farLeftToLeftSwitch(robot);
} else {
DriverStation.reportError("AUTO: Cross the line from far left", false);
buildAutoCross(robot);
}
break;
case -Constants.AutoChooser.Position.FAR_RIGHT:
if (!stayInYourOwnLane) { // Traverse allowed.
if (!(stayInYourOwnLane || defensive)) { // Traverse allowed
DriverStation.reportError("AUTO: Left scale from far right", false);
farRightToLeftScale(robot);
} else if (switchSide == Constants.AutoChooser.RIGHT) { // Traverse !allowed. Get switch.
} else if (defensive) {
DriverStation.reportError("AUTO: Defensive from far right", false);
farRightDefensive(robot, switchSide == Constants.AutoChooser.RIGHT);
} else if (switchSide == Constants.AutoChooser.RIGHT){ // Traverse !allowed. Go for switch
DriverStation.reportError("AUTO: RIght switch from far right", false);
farRightToRightSwitch(robot);
} else {
DriverStation.reportError("AUTO: Cross the line from far right", false);
buildAutoCross(robot);
}
break;
case Constants.AutoChooser.Position.FAR_RIGHT:
DriverStation.reportError("AUTO: Right scale only from far right", false);
farRightToRightScale(robot);
/*
// Second cube not competition ready, so it is commented out.
Expand Down Expand Up @@ -625,6 +660,53 @@ public RaiseCarriageAfterWaitingNMillis(Robot robot, long millis) {
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -110, Constants.Auto.Align.SPEED, 4000));
}


private void farLeftDefensive(Robot robot, boolean doSwitch) {
addParallel(new AutoZeroCarriageThenLower(robot));
// addSequential(new FarLeftToRightScaleDeadPartOne(robot));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 210, 0.75, true, true, 6000, "pass the switch"));
addParallel(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 90, Constants.Auto.Align.SPEED, 4000));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 52, 0.75, true, true, 3000, "goes halfway accross field"));
addParallel(new AutoEject(robot.getIntake(), Constants.Intake.DROP_SPEED));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), -54, 0.75, true, true, 3000, "moves back"));
addSequential(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), 150, Constants.Auto.Align.SPEED, 3000));
addParallel(new AutoIntake(robot.getIntake()));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 12, 0.6, true, true, 2000, "attack cube"));
if (doSwitch) {
DriverStation.reportError("Attacking swithc", false);
addSequential(new MoveCarriageToSetpointPID(robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP, 2000));
addSequential(new IntakeToSwitch(robot.getCarriage(), robot.getArm()));
addSequential(new AutoEject(robot.getIntake(), Constants.Intake.OUTTAKE_SPEED));
}
addParallel(new IntakeToDrive(robot.getCarriage(), robot.getArm()));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), -24, 0.6, true, true, 2000, "reposition"));
}

private void farRightDefensive(Robot robot, boolean doSwitch) {
addParallel(new AutoZeroCarriageThenLower(robot));
// addSequential(new FarLeftToRightScaleDeadPartOne(robot));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 214, 0.75, true, true, 6000, "pass the switch"));
addParallel(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -90, Constants.Auto.Align.SPEED, 4000));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 78, 0.75, true, true, 3000, "goes halfway accross field"));
addParallel(new AutoEject(robot.getIntake(), Constants.Intake.DROP_SPEED));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), -60, 0.75, true, true, 3000, "moves back"));
addSequential(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
addSequential(new AutoAlign(robot.getDriveTrain(), robot.getIMU(), -150, Constants.Auto.Align.SPEED, 3000));
addParallel(new AutoIntake(robot.getIntake()));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), 24, 0.6, true, true, 2000, "attack cube"));
if (doSwitch) {
DriverStation.reportError("Attacking swithc", false);
addSequential(new MoveCarriageToSetpointPID(robot.getCarriage(), Constants.Carriage.ENCODER_TOP_COMP, 2000));
addSequential(new IntakeToSwitch(robot.getCarriage(), robot.getArm()));
addSequential(new AutoEject(robot.getIntake(), Constants.Intake.OUTTAKE_SPEED));
}
addParallel(new IntakeToDrive(robot.getCarriage(), robot.getArm()));
addSequential(new AutoDrive(robot.getDriveTrain(), robot.getIMU(), -24, 0.6, true, true, 2000, "reposition"));
}

private void farRightToLeftScale(Robot robot) {
addParallel(new AutoZeroCarriageThenLower(robot));
addSequential(new FarRightToLeftScaleDeadPartOne(robot));
Expand Down
Loading

0 comments on commit 4fb0039

Please sign in to comment.