Skip to content

Commit

Permalink
Final Houston Commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@AvidhBavkar
AvidhBavkar committed Apr 21, 2019
1 parent 2ceef6f commit b2c5d0e
Show file tree
Hide file tree
Showing 7 changed files with 60 additions and 40 deletions.
58 changes: 34 additions & 24 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
import frc.robot.subsystems.DustPan;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Elevator.Setpoint;
import frc.robot.subsystems.Intake.IntakeWheelState;
import frc.robot.subsystems.PPintake.ArmState;
import frc.robot.subsystems.PPintake.PPState;
import frc.robot.subsystems.Hand;
Expand Down Expand Up @@ -132,6 +133,7 @@ public void robotInit(){

//Set all system wide variables
activeSide = (Hand.getInstance().hasBall()) ? Side.BALL : Side.PANEL;

respectPerimeter = false;
isClimbing = false;
autoPlace = false;
Expand All @@ -140,12 +142,15 @@ public void robotInit(){

mInitCalled = false;

Ramsete.getInstance().start();
// Ramsete.getInstance().start();

//Import all autonomous paths from filesystem (time intensive)
autonPaths = PathLoader.loadPaths();
autoChooser.setDefaultOption("LEFT CARGO", FieldSide.LEFT);
autoChooser.addOption("RIGHT CARGO", FieldSide.RIGHT);
SmartDashboard.putData(autoChooser);

autonCommand = new CargoShip(FieldSide.RIGHT);


// autonCommand = new PIDTurn(180, 0);
Expand Down Expand Up @@ -182,6 +187,7 @@ public void robotPeriodic() {
VisionHelper.setActiveCam(limePanel);
}


// System.out.println(pdp.getCurrent(RobotMap.DustpanMap.kIntake - 1));

// if(Elevator.getInstance().getSwitch()){
Expand All @@ -195,6 +201,7 @@ public void robotPeriodic() {
// System.out.println(Elevator.getInstance().getPosition());
// SmartDashboard.putNumber("ElevatorPosition", Elevator.getInstance().getPosition());
// VisionHelper.debugMessage();
// limePanel.setCamMode(CAM_MODE.VISION);
// SmartDashboard.putNumber("TEET", (SmartDashboard.getNumber("VisionDriveGain", 0)));
// // VisionHelper.throttleCorrection();
// System.out.println(Hand.getInstance().getSensorVoltage());
Expand All @@ -207,30 +214,31 @@ public void robotPeriodic() {
@Override
public void autonomousInit() {

autonCommand = new CargoShip(autoChooser.getSelected());
// autonCommand = new CargoShip(FieldSide.RIGHT);
// autonCommand = new a

Drivetrain.getInstance().zeroSensor();
// Drivetrain.getInstance().zeroSensor();



Drivetrain.getInstance().setBrakeMode(false);
Drivetrain.getInstance().zeroSensor();
Drivetrain.getInstance().zeroGyro();
Drivetrain.getInstance().startOdometery(0.02);
Elevator.getInstance().checkNeedsZero();
limePanel.setLED(LED_STATE.OFF);
limeBall.setLED(LED_STATE.OFF);
// Drivetrain.getInstance().setBrakeMode(false);
// Drivetrain.getInstance().zeroSensor();
// Drivetrain.getInstance().zeroGyro();
// Drivetrain.getInstance().startOdometery(0.02);
// Elevator.getInstance().checkNeedsZero();
// limePanel.setLED(LED_STATE.OFF);
// limeBall.setLED(LED_STATE.OFF);


// teleopInit(); //Just start teleop in sandstorm
teleopInit(); //Just start teleop in sandstorm

// Elevator.getInstance().setPosition(Setpoint.STOW);

//Start the selected autonomous command.

// Ramsete.getInstance().start();

autonCommand.start();
// autonCommand.start();

Scheduler.getInstance().run();

Expand All @@ -253,19 +261,20 @@ public void autonomousPeriodic() {
// VisionHelper.doDriveIn();
///
// teleopPeriodic();
// teleopPeriodic();
teleopPeriodic();

Scheduler.getInstance().run();

if(Math.abs(OI.getInstance().getForward()) > 0.2){
autonCommand.cancel();
mAutoCancelled = true;
}
// if(Math.abs(OI.getInstance().getForward()) > 0.2){
// autonCommand.cancel();
// mAutoCancelled = true;
// }

if(mAutoCancelled){
if(mInitCalled){
teleopPeriodic();
}
}
// if(mAutoCancelled){
// if(mInitCalled){
// teleopPeriodic();
// }
// }

// System.out.println(Drivetrain.getInstance().getRobotPos().getHeading());

Expand All @@ -275,8 +284,8 @@ public void autonomousPeriodic() {
@Override
public void teleopInit() {

Ramsete.getInstance().stop();
Drivetrain.getInstance().stopOdometery();
// Ramsete.getInstance().stop();
// Drivetrain.getInstance().stopOdometery();
// autonCommand.cancel();

limePanel.setUSBCam(true);
Expand All @@ -291,6 +300,7 @@ public void teleopInit() {


PPintake.getInstance().setPP(PPState.HOLDING);
Intake.getInstance().intake(IntakeWheelState.CLEAR);

// autonCommand.cancel();
mZeroElevatorCommand.start();
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/Unjam.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ public class Unjam extends CommandGroup {

public Unjam() {
addParallel(DustPan.getInstance().intakeCommand(DustpanIntakeState.UNJAM));
addParallel(Elevator.getInstance().jogElevatorCommand(Setpoint.FUEL_LOW));
addParallel(Elevator.getInstance().jogElevatorCommand(Setpoint.HATCH_MID));
addParallel(DustPan.getInstance().deployCommand(true));
addParallel(Intake.getInstance().deployCommand(true));
addParallel(Intake.getInstance().intakeCommand(IntakeWheelState.UNJAM));
Expand All @@ -32,7 +32,7 @@ public void cancel() {
DustPan.getInstance().intake(false);
DustPan.getInstance().deploy(false);
Intake.getInstance().deploy(false);
Intake.getInstance().intake(false);
Intake.getInstance().intake(IntakeWheelState.CLEAR);;
PPintake.getInstance().setPP(PPState.OFF);
Hand.getInstance().set(HandState.OFF);
}
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/commands/balls/IntakeBallSequence.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
public class IntakeBallSequence extends CommandGroup {

public IntakeBallSequence() {
addSequential(new SafeElevatorMove(Setpoint.BOTTOM));
addSequential(new SafeElevatorMove(Setpoint.STOW));
addSequential(Elevator.getInstance().lockElevatorCommand(true));
addSequential(Intake.getInstance().intakeCommand(IntakeWheelState.ON));
addSequential(Intake.getInstance().deployCommand(true));
Expand All @@ -33,7 +33,7 @@ protected void end() {
@Override
protected void interrupted() {
Intake.getInstance().deploy(false);
Intake.getInstance().intake(false);
Intake.getInstance().intake(IntakeWheelState.CLEAR);
Hand.getInstance().set(HandState.OFF);
Intake.getInstance().intake(IntakeWheelState.OFF);
Elevator.getInstance().lockElevator(false);
Expand All @@ -58,15 +58,15 @@ protected boolean isFinished() {
addSequential(Elevator.getInstance().lockElevatorCommand(false));
addSequential(new SafeElevatorMove(Setpoint.FUEL_LOW));
addSequential(Intake.getInstance().deployCommand(false));
addSequential(Intake.getInstance().intakeCommand(IntakeWheelState.OFF));
addSequential(Intake.getInstance().intakeCommand(IntakeWheelState.CLEAR));
addSequential(Robot.limePanel.setLEDCommand(LED_STATE.OFF));
addSequential(Robot.limeBall.setLEDCommand(LED_STATE.OFF));
}

@Override
protected void interrupted() {
Intake.getInstance().deploy(false);
Intake.getInstance().intake(false);
Intake.getInstance().intake(IntakeWheelState.CLEAR);
Hand.getInstance().set(HandState.OFF);
Intake.getInstance().intake(IntakeWheelState.OFF);
Elevator.getInstance().lockElevator(false);
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/balls/ScoreBall.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ protected boolean isFinished() {
}
});
addSequential(Hand.getInstance().setArmCommand(HandArmState.EXTENDED));
addSequential(new WaitCommand(0.2));
// addSequential(new WaitCommand(0.2));
addSequential(Hand.getInstance().setHandCommand(HandState.DROP));
}

Expand All @@ -59,7 +59,7 @@ public synchronized void cancel() {
private class WaitForClaw extends CommandGroup{

public WaitForClaw(){
addSequential(new WaitCommand(1));
// addSequential(new WaitCommand(1));
addSequential(Elevator.getInstance().lockElevatorCommand(false));
}

Expand Down
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,8 @@ public static enum IntakeBoomState{
public static enum IntakeWheelState{
ON,
OFF,
UNJAM
UNJAM,
CLEAR
}

private static final double kIntakePwr = 0.75;
Expand All @@ -43,6 +44,8 @@ private Intake(){
mPiston = new Solenoid(RobotMap.IntakeMap.kPiston);
mIntake = CTREFactory.createVictor(RobotMap.IntakeMap.kIntake);

mIntake.configOpenloopRamp(0.3);

mIntake.setInverted(true);

wheelState = IntakeWheelState.OFF;
Expand Down Expand Up @@ -86,8 +89,10 @@ public void intake (boolean run){
public void intake(IntakeWheelState state){
if (state == IntakeWheelState.ON){
intake(true);
}else if (state == IntakeWheelState.UNJAM){
}else if (state == IntakeWheelState.CLEAR){
setRaw(-0.25);
}else if (state == IntakeWheelState.UNJAM){
setRaw(-1);
}else{
intake(false);
}
Expand Down
15 changes: 9 additions & 6 deletions src/main/java/frc/robot/vision/VisionHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@ public class VisionHelper{
// private static final double kTurnBallIGain = 0.0125;
private static final double kDerivativeGain = -0.2; //-0.2

public static final double kDriveGain = 1.1;
private static final double kDriveDerivGain = 0;//-1.4
public static final double kDriveGain = 0.05; //1/(1/1.5d - 1/2d)
private static final double kDriveDerivGain = -0.4;//-1.4
private static final double kSkewGain = 0;

private static final double kHasTargetRollerThreshold = 0.8;
Expand Down Expand Up @@ -86,19 +86,22 @@ public static double throttleCorrection(){
}
}else{
if (mActiveCam.hasTarget()){
distRoller.add(1 / mActiveCam.tArea());
// distRoller.add(1 / mActiveCam.tArea() - 1/2d);
distRoller.add(mActiveCam.tY());
mDistPastError = mDistError;
mDistError = distRoller.getAverage();
mDistError = (-distRoller.getAverage());
}else{
return 0;
}
}

return Math.max(-0.8, Math.min(-0.2, -((mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain))));
// return Math.max(-0.8, Math.min(-0.2, ((mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain) + 0.2)));
return ((mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain));
}

public static void debugMessage(){
SmartDashboard.putNumber("OUTPUT VEL", Math.max(-0.8, Math.min(-0.2, (mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain))));
getDriveSignal();
SmartDashboard.putNumber("OUTPUT VEL", Math.max(-0.8, -((mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain) + 0.2)));

SmartDashboard.putNumber("Unbounded Output Vel", -(mDistPastError * kDriveGain) + ((mDistPastError - mDistError) * kDriveDerivGain));
}
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/frc/util/CheesyDriveHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@ public class CheesyDriveHelper {
private static final double kQuickStopWeight = 0.1; //0.5
private static final double kQuickStopScalar = 5.0; //5.0

// private static final double kQuickTurnSensitivity = 0.75;

private double mOldWheel = 0.0;
private double mQuickStopAccumlator = 0.0;
private double mNegInertiaAccumlator = 0.0;
Expand Down

0 comments on commit b2c5d0e

Please sign in to comment.