Skip to content

Commit

Permalink
Merge branch 'fix/week1' into vision/#161-cameraoverlay
Browse files Browse the repository at this point in the history
  • Loading branch information
BenBernardCIS committed Mar 6, 2016
2 parents 043333b + 67d9d34 commit bca3cfc
Show file tree
Hide file tree
Showing 21 changed files with 156 additions and 75 deletions.
9 changes: 6 additions & 3 deletions src/main/java/org/usfirst/frc/team5687/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public class Deadbands {
*/
public static final double DRIVE_STICK = 0.1;
public static final double SHOOTER_WHEELS = 0.1;
public static final double INTAKE_STICK = 0.2;
public static final double INTAKE_STICK = 0.8;
public static final double ARMS = 0.1;
}

Expand Down Expand Up @@ -106,7 +106,10 @@ public class Shooter {
/**
* Speed to run the shooter wheels to shoot high goal
*/
public static final double SHOOTER_SPEED = .94;
public static final double SHOOTER_SPEED_LOW = 0.92;
public static final double SHOOTER_SPEED = 0.94;
public static final double SHOOTER_SPEED_HIGH = 0.96;
public static final double SHOOTER_SPEED_EXTREME = 0.98;
}

public class Arms {
Expand All @@ -119,7 +122,7 @@ public class Arms {

public class Climber {
public static final double RAISE_SPEED = 0.6;
public static final double LOWER_SPEED = -0.6;
public static final double LOWER_SPEED = -1;
}

public class Encoders {
Expand Down
17 changes: 17 additions & 0 deletions src/main/java/org/usfirst/frc/team5687/robot/OI.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team5687.robot.commands.*;
import org.usfirst.frc.team5687.robot.utils.Gamepad;
import org.usfirst.frc.team5687.robot.utils.Helpers;
Expand All @@ -28,6 +29,11 @@ public class OI {
public static final int UNPRIME = 6;
public static final int FIRE = 1;
public static final int RECOVER = 4;
// Prime Speed Buttons
public static final int LOW_PRIME_SPEED = 8; // 0.92
public static final int DEFAULT_PRIME_SPEED = 7; // 0.94
public static final int HIGH_PRIME_SPEED = 10; // 0.96
public static final int EXTREME_PRIME_SPEED = 9; // 0.98
// Lights Buttons
public static final int SWITCH_RING_LIGHT = 12;
public static final int SWITCH_FLASHLIGHT = 11;
Expand All @@ -54,6 +60,11 @@ public OI() {
JoystickButton visionLightSwitch = new JoystickButton(joystick, SWITCH_RING_LIGHT);
JoystickButton flashlightSwitch = new JoystickButton(joystick, SWITCH_FLASHLIGHT);

JoystickButton lowSpeedButton = new JoystickButton(joystick, LOW_PRIME_SPEED);
JoystickButton normalSpeedButton = new JoystickButton(joystick, DEFAULT_PRIME_SPEED);
JoystickButton highSpeedButton = new JoystickButton(joystick, HIGH_PRIME_SPEED);
JoystickButton extremeSpeedButton = new JoystickButton(joystick, EXTREME_PRIME_SPEED);

// Drive Train Commands
reverseButton.whenPressed(new ReverseDrive());
// Boulder Commands
Expand All @@ -68,6 +79,11 @@ public OI() {
flashlightSwitch.whenPressed(new ToggleFlashlight());
// Reset Camera Command
resetCameraButton.whenPressed(new ResetCamera());

lowSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_LOW));
normalSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED));
highSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_HIGH));
extremeSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_EXTREME));
}

/**
Expand Down Expand Up @@ -117,6 +133,7 @@ public double getIntakeSpeed() {
return Helpers.applyDeadband(joystick.getRawAxis(1), Constants.Deadbands.INTAKE_STICK);
}


/**
* Gets the desired speed for the arms
* @return the control value for the arms' motor
Expand Down
8 changes: 6 additions & 2 deletions src/main/java/org/usfirst/frc/team5687/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -145,16 +145,19 @@ public void robotInit() {
SmartDashboard.putData("Start Position", positionChooser);

autoChooser.addDefault("Do Nothing At All", new AutonomousDoNothing());
autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
autoChooser.addObject("Traverse Defense", new AutoTraverseBuilder());
autoChooser.addObject("Traverse And Shoot", new AutoTraverseAndShootBuilder());
autoChooser.addDefault("---Below are for Testing---", new AutonomousDoNothing());
autoChooser.addObject("Target and Shoot", new AutoShootOnly());
autoChooser.addObject("Chase Target", new AutoChaseTarget());
autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
autoChooser.addObject("Left 90", new AutoAlign(-90));
autoChooser.addObject("Right 90", new AutoAlign(90));
autoChooser.addObject("Drive 12", new AutoDrive(0.4, 12f));
autoChooser.addObject("Drive 24", new AutoDrive(0.4, 24f));
autoChooser.addObject("Drive 48", new AutoDrive(0.4, 48f));
autoChooser.addObject("Drive 96", new AutoDrive(0.4, 96f));
autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
SmartDashboard.putData("Autonomous mode", autoChooser);

}
Expand Down Expand Up @@ -198,7 +201,7 @@ public void autonomousPeriodic() {
driveTrain.sendAmpDraw();
intake.updateDashboard();
arms.updateDashboard();

lights.updateDashboard();
}

public void teleopInit() {
Expand All @@ -219,6 +222,7 @@ public void teleopPeriodic() {
Scheduler.getInstance().run();
intake.updateDashboard();
arms.updateDashboard();
lights.updateDashboard();
}

/**
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/org/usfirst/frc/team5687/robot/RobotMap.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,11 @@ public static class Drive {
public static final int PDP_RIGHT_MOTOR_FRONT = 12;
public static final int PDP_RIGHT_MOTOR_REAR = 13;

public static final int LEFT_ENCODER_CHANNEL_A = 8;
public static final int LEFT_ENCODER_CHANNEL_B = 9;
public static final int RIGHT_ENCODER_CHANNEL_A = 6;
public static final int RIGHT_ENCODER_CHANNEL_B = 7;
// Encoder channel ports as of 03/02, left reversed with right
public static final int LEFT_ENCODER_CHANNEL_A = 6;
public static final int LEFT_ENCODER_CHANNEL_B = 7;
public static final int RIGHT_ENCODER_CHANNEL_A = 8;
public static final int RIGHT_ENCODER_CHANNEL_B = 9;
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ public class AutoChaseTarget extends Command {
private static final double deadbandWidth = 50;
private static final double deadbandX = 20;

private static final double baseSpeed = 0.6;
private static final double twist = .2;
private static final double baseSpeed = 0.4;
private static final double twist = .1;

private boolean centered = false;
private boolean inRange = false;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
package org.usfirst.frc.team5687.robot.commands;

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

/**
* Created by Ben Bernard on 3/5/2016.
*/
public class AutoShootOnly extends CommandGroup {

public AutoShootOnly() {

addParallel(new Prime());
addSequential(new AutoChaseTarget());
addSequential(new Fire());
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ public abstract class AutoTraverse extends Command{
*
*/
static final long inchesToTraverseDefense = 90;
static final long inchesToClear = 3;
static final long inchesToClear = 9; //TODO! back to 3 with Rotation
//static final long inchesToCross = 20;
//static final double rollThreshold = 8.0;
static final double rollErrorMargin = 1.0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@ public class AutoTraverseAndShoot extends CommandGroup {
public AutoTraverseAndShoot(String defense, int position) {

addSequential(new AutoTraverseOnly(defense, position));
// addSequential(new AutoChaseTarget());
addSequential(new Prime());
addParallel(new Prime());
addSequential(new AutoChaseTarget());
addSequential(new Fire());


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ protected void initialize() {
DriverStation.reportError("Building autotraverse for defense " + defense + " in position " + position,false);

// Traverse the selected defense
Scheduler.getInstance().add(new AutoTraverseOnly(defense, position));
Scheduler.getInstance().add(new AutoTraverseOnly(defense, 0));

}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,11 @@ public AutoTraverseOnly(String defense, int position) {
break;
}


switch (position){
case 1:

// Run forward 112 inches
addSequential(new AutoDrive(.7, 124.0f));
// Run forward 100 inches
addSequential(new AutoDrive(.7, 100.0f));

// Turn towards the tower
addSequential(new AutoAlign(50));
Expand Down Expand Up @@ -97,9 +96,6 @@ public AutoTraverseOnly(String defense, int position) {
break;
}




}

}
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
public class CancelPrime extends CommandGroup{

public CancelPrime() {
addSequential(new SetShooterSpeed(0, Constants.Shooter.UNPRIME_TIME)); // Stops the shooter
addSequential(new ToggleShooter(false)); // Stops the shooter
addSequential(new UnprimeBoulder()); // Unprimes the shooter to captured position
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ public class Fire extends CommandGroup{

public Fire() {
addSequential(new FireBoulder());
addSequential(new SetShooterSpeed(0, Constants.Shooter.UNPRIME_TIME));
addSequential(new ToggleShooter(false));
addSequential(new DisableFlashlight());
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,16 @@
* @author wil
*/
public class FireBoulder extends Command {
private long endTime;


public FireBoulder() {
requires(intake);
}

@Override
protected void initialize() {
endTime = System.currentTimeMillis() + Constants.Shooter.UNPRIME_TIME;
}

@Override
Expand All @@ -27,7 +30,7 @@ protected void execute() {

@Override
protected boolean isFinished() {
return !intake.isPrimed();
return !intake.isPrimed() && System.currentTimeMillis() > endTime;
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc.team5687.robot.Constants;

import static org.usfirst.frc.team5687.robot.Robot.oi;

/**
* Command group to prime the shooter subsystem
* @author wil
Expand All @@ -12,6 +14,6 @@ public class Prime extends CommandGroup{
public Prime() {
addSequential(new EnableFlashlight());
addSequential(new PrimeBoulder());
addSequential(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED, Constants.Shooter.PRIME_TIME));
addSequential(new ToggleShooter(true));
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -2,18 +2,17 @@

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

import static org.usfirst.frc.team5687.robot.Robot.intake;
import static org.usfirst.frc.team5687.robot.Robot.shooter;

/**
* Command to try to suckset speed of shooter motor
* @author wil
*/
public class RecoverBoulder extends Command {
private static final double speed = -0.8f;
private static final long time = 5000;
private static final double speed = -1f;
private static final long time = 40;
private long endTime;

private double previous;
/**
* Constructor for RecoverBoulder
*/
Expand All @@ -23,25 +22,29 @@ public RecoverBoulder() {

@Override
protected void initialize() {
previous = shooter.getSpeed();
endTime = System.currentTimeMillis() + time;
shooter.setSpeed(speed);
}

@Override
protected void execute() {
shooter.setSpeed(speed);
shooter.toggle(true);
}

@Override
protected boolean isFinished() {
return /*intake.isCaptured() ||*/ System.currentTimeMillis() > endTime;
return System.currentTimeMillis() > endTime;
}

@Override
protected void end() {
shooter.stop();
shooter.toggle(false);
shooter.setSpeed(previous);
}

@Override
protected void interrupted() {
end();
}
}
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.usfirst.frc.team5687.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc.team5687.robot.Constants;

import static org.usfirst.frc.team5687.robot.Robot.shooter;

Expand All @@ -10,34 +9,26 @@
* @author wil
*/
public class SetShooterSpeed extends Command {
private double speed;
private long time;
private long endTime;

/**
* Constructor for SetShooterSpeed
* @param speed motor speed to run the shooter
* @param time duration of time to run the shooter at speed
*/
public SetShooterSpeed(double speed, long time) {
private double _speed;

public SetShooterSpeed(double speed) {
requires(shooter);
this.speed = speed;
this.time = time;
_speed = speed;
}


@Override
protected void initialize() {
endTime = System.currentTimeMillis() + time;
shooter.setSpeed(_speed);
}

@Override
protected void execute() {
shooter.setSpeed(speed);
}

@Override
protected boolean isFinished() {
return System.currentTimeMillis() > endTime;
return true;
}

@Override
Expand Down
Loading

0 comments on commit bca3cfc

Please sign in to comment.