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
TheMightyWarPig committed Apr 19, 2018
2 parents 2d2cde2 + d85eff6 commit afb84d5
Show file tree
Hide file tree
Showing 33 changed files with 14,489 additions and 8,706 deletions.
315 changes: 8 additions & 307 deletions 2018-robot.iml

Large diffs are not rendered by default.

18 changes: 13 additions & 5 deletions src/main/java/org/frc5687/powerup/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,12 @@ public class Constants {
public static final double END_ALERT = 28;
public static final int HEALTH_CHECK_CYCLES = 10;

public enum typeOfTurn {
shortest,
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 @@ -99,8 +105,9 @@ public class Intake {
public static final double SENSITIVITY = 0.5;
public static final long SETTLE_TIME = 1500;
public static final double PLATE_MINIMUM_CLARANCE = 24.0;
public static final double SCALE_DROP_SPEED = -0.6;
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 HC_MIN_SPEED = 0.1;
Expand Down Expand Up @@ -143,7 +150,7 @@ public class Align {
public static final double kI = 0.0;
public static final double kD = 0.3;
public static final double TOLERANCE = 1.0; // 0.5
public static final double MINIMUM_SPEED = 0.35;
public static final double MINIMUM_SPEED = 0.5;
public static final double MAX_OUTPUT = 0;
/*
*time the angle must be on target for to be considered steady
Expand Down Expand Up @@ -404,7 +411,7 @@ public class Pot {
public static final double BOTTOM_COMP = 33.0;
public static final double BOTTOM_PROTO = 33.0; // TODO: Tune

public static final double INTAKE_COMP = 50.0;
public static final double INTAKE_COMP = 49.0;
public static final double INTAKE_PROTO = 47.0;

public static final double DRIVE_COMP = 41.0;
Expand Down Expand Up @@ -487,10 +494,11 @@ public class Mode {
public static final int SCALE_THEN_SCALE = 3;
public static final int SWITCH_THEN_SWITCH = 4;
public static final int SCALE_THEN_SWITCH = 5;
public static final int SCALE_THEN_BACKOFF = 6;
public static final int SWITCH_ONLY = 7;
public static final int SCALE_ONLY = 8;
public static final int SWITCH_DRIVE = 1001;
public static final int SWITCH_ONLY = 10002;
public static final int SCALE_DRIVE = 1003;
public static final int SCALE_ONLY = 1004;
}
}
public class Limits {
Expand Down
13 changes: 11 additions & 2 deletions src/main/java/org/frc5687/powerup/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,11 @@ public class Robot extends TimedRobot {
private DigitalInput _identityFlag;
private boolean _isCompetitionBot;
private long lastPeriod;
private int ticksPerUpdate = 5;
private int ticksPerUpdate = 20;
private int updateTick = 0;
private boolean hasRumbledForEndgame;
private boolean _manualLightFlashRequested;
private boolean abortAuton;


public Robot() {
Expand All @@ -70,7 +71,7 @@ public void robotInit() {
_arm = new Arm(oi, pdp, intake, _isCompetitionBot);
intake.setArm(_arm);
_lights = new Lights(this);
_climber = new Climber(oi, pdp, _isCompetitionBot);
_climber = new Climber(oi, pdp, intake, _isCompetitionBot);
_autoChooser = new AutoChooser(_isCompetitionBot);
SmartDashboard.putString("Identity", (_isCompetitionBot ? "Diana" : "Jitterbug"));
lastPeriod = System.currentTimeMillis();
Expand Down Expand Up @@ -120,6 +121,7 @@ public void autonomousInit() {
carriage.zeroEncoder();
_manualLightFlashRequested = false;
hasRumbledForEndgame = false;
abortAuton = false;
// Reset the lights slider in case it was left on
SmartDashboard.putNumber("DB/Slider 0", 0.0);

Expand Down Expand Up @@ -191,6 +193,9 @@ public void disabledPeriodic() {
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
if (abortAuton && autoCommand != null) {
autoCommand.cancel();
}
}

@Override
Expand Down Expand Up @@ -283,4 +288,8 @@ public boolean isInWarningPeriod() {
public boolean isManualLightFlashRequested() {
return _manualLightFlashRequested;
}

public void requestAbortAuton() {
abortAuton = true;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@ public DriveCarriage(Carriage carriage, OI oi) {
protected void execute() {
double oiSpeed = DriverStation.getInstance().isAutonomous() ? 0 : oi.getCarriageSpeed();
if (oiSpeed != 0.0) {
carriage.disable();
if (carriage.getPIDController().isEnabled()) {
carriage.disable();
}
//DriverStation.reportError("DriveCarriage requesting oiSpeed: " + Double.toString(oiSpeed), false);
carriage.drive(oiSpeed);
} else if (!carriage.getPIDController().isEnabled()) {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.frc5687.powerup.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.frc5687.powerup.robot.Constants;
import org.frc5687.powerup.robot.OI;
import org.frc5687.powerup.robot.subsystems.Climber;
Expand Down Expand Up @@ -31,7 +30,6 @@ protected void execute() {
} else if (_holdOn) {
speed = Constants.Climber.HOLD_SPEED;
}

climber.drive(speed);
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.frc5687.powerup.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.frc5687.powerup.robot.Constants;
import org.frc5687.powerup.robot.OI;
import org.frc5687.powerup.robot.subsystems.Intake;
Expand All @@ -24,11 +25,17 @@ protected void execute() {
double right = oi.getRightIntakeSpeed();

intake.drive(left, right);

if(intake.isEjecting()){
intake.driveServo(1);
} else{
intake.driveServo(0);
}
/*
double val = SmartDashboard.getNumber("DB/Slider 2", 0.5);
intake.driveServo(val);
*/

}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
package org.frc5687.powerup.robot.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;
import org.frc5687.powerup.robot.commands.auto.AutoWaitForDistance;
import org.frc5687.powerup.robot.commands.auto.AutoWaitForMillis;
import org.frc5687.powerup.robot.subsystems.Carriage;
import org.frc5687.powerup.robot.subsystems.DriveTrain;

public class MoveCarriageToSetpointPIDButWaitForNInchesFirst extends CommandGroup {
public MoveCarriageToSetpointPIDButWaitForNInchesFirst(DriveTrain drivetrain, Carriage carriage, int target, double inches) {
addSequential(new AutoWaitForDistance(drivetrain, inches, 5000));
addSequential(new MoveCarriageToSetpointPIDButFirstZeroIt(carriage, target));
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
package org.frc5687.powerup.robot.commands.auto;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.Command;
import org.frc5687.powerup.robot.Robot;
import org.frc5687.powerup.robot.subsystems.Intake;

public class AbortIfCubeNotSecured extends Command {
private Robot _robot;
private Intake _intake;
private boolean _isFinished;

public AbortIfCubeNotSecured(Robot robot) {
_robot = robot;
_intake = robot.getIntake();
}

@Override
protected void initialize() {
DriverStation.reportError("AbortIfCubeNotSecured initialized", false);
_isFinished = true;
if (!_intake.cubeIsSecured()) {
DriverStation.reportError("AbortIfCubeNotSecured cube not secured!!.. calling robot.requestAbortAuton()", false);
_robot.requestAbortAuton();
}
}

@Override
protected boolean isFinished() {
return _isFinished;
}

@Override
protected void end() {
DriverStation.reportError("AbortIfNoCubeDetected ending", false);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
package org.frc5687.powerup.robot.commands.auto;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.Command;
import org.frc5687.powerup.robot.Robot;
import org.frc5687.powerup.robot.subsystems.Intake;

public class AbortIfNoCubeDetected extends Command {
private Robot _robot;
private Intake _intake;
private boolean _isFinished;

public AbortIfNoCubeDetected(Robot robot) {
_robot = robot;
_intake = robot.getIntake();
}

@Override
protected void initialize() {
DriverStation.reportError("AbortIfNoCubeDetected initialized", false);
_isFinished = true;
if (!_intake.cubeIsDetected()) {
DriverStation.reportError("AbortIfNoCubeDetected no cube detected.. calling robot.requestAbortAuton()", false);
_robot.requestAbortAuton();
}
}

@Override
protected boolean isFinished() {
return _isFinished;
}

@Override
protected void end() {
DriverStation.reportError("AbortIfNoCubeDetected ending", false);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -92,16 +92,20 @@ protected void initialize() {

@Override
protected void execute() {
//actOnPidOut();
SmartDashboard.putBoolean("AutoAlign/onTarget", controller.onTarget());
SmartDashboard.putNumber("AutoAlign/imu", imu.getYaw());
SmartDashboard.putData("AutoAlign/pid", controller);
}

private void actOnPidOut() {
if (pidOut > 0 && pidOut < Align.MINIMUM_SPEED) {
pidOut = Align.MINIMUM_SPEED;
}
if (pidOut < 0 && pidOut > -Align.MINIMUM_SPEED) {
pidOut = -Align.MINIMUM_SPEED;
}
driveTrain.setPower(pidOut, -pidOut, true); // positive output is clockwise
SmartDashboard.putBoolean("AutoAlign/onTarget", controller.onTarget());
SmartDashboard.putNumber("AutoAlign/imu", imu.getYaw());
SmartDashboard.putData("AutoAlign/pid", controller);
}

@Override
Expand All @@ -111,7 +115,7 @@ protected boolean isFinished() {
}

if(System.currentTimeMillis() >= _endTimeMillis){
DriverStation.reportError("AutoAlign timed out after " + _timeout + "ms", false);
DriverStation.reportError("AutoAlign timed out after " + _timeout + "ms at " + imu.getYaw(), false);
return true;
}

Expand Down Expand Up @@ -141,6 +145,7 @@ protected void end() {
@Override
public void pidWrite(double output) {
pidOut = output;
actOnPidOut();
//SmartDashboard.putNumber("AutoAlign/pidOut", pidOut);
}

Expand Down
Loading

0 comments on commit afb84d5

Please sign in to comment.