public final static int RIGHT_STICK_PORT = 1;
// Need to fill in the port numbers of the following buttons
- public final static int TOGGLE_WINCH_PORT = 0;
public final static int TOGGLE_FLYWHEEL_PORT = 4;
public final static int RUN_INDEXWHEEL_PORT = 1;
public final static int REVERSE_INDEXWHEEL_PORT = 2;
public final static int REVERSE_INTAKE_PORT = 4;
public static final int INCREASE_SHOOTER_SPEED_PORT = 6;
public static final int DECREASE_SHOOTER_SPEED_PORT = 2;
++
+ public static final int CHANGE_CAMERA_VIEW = 6;
++
+ public static final int BRAKE_CANTALONS_PORT = 5;
+ public static final int COAST_CANTALONS_PORT = 3;
++
}
public static class Shooter {
public final static int HALL_EFFECT_PORT = 9;
- public static final int PISTON_MODULE = 10, PISTON_FORWARD = 4,
+ public final static int TOGGLE_INDEXER = 8;
+
+ public static final int MODULE_NUMBER = 10, PISTON_FORWARD = 4,
PISTON_REVERSE = 5;
++
public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
}
public static class DriveTrain {
// GEARS
public static final int PISTON_MODULE = 10, LEFT_GEAR_PISTON_FORWARD = 0,
- LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 2,
- RIGHT_GEAR_PISTON_REVERSE = 3;
+ LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 3,
+ RIGHT_GEAR_PISTON_REVERSE = 2;
public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
+ public final static int TOGGLE_DRIVE_PISTON = 7;
+
// MOTOR CONTROLLERS
public static final int FRONT_LEFT = 1;
public static final int FRONT_RIGHT = 3;
package org.usfirst.frc.team3501.robot;
- import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
-import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons;
-import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons;
import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous;
+import org.usfirst.frc.team3501.robot.commands.shooter.ToggleIndexerPiston;
+import org.usfirst.frc.team3501.robot.utils.ChangeCameraView;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
-public class OI {
+public class OI /* implements KeyListener */ {
private static OI oi;
public static Joystick leftJoystick;
public static Joystick rightJoystick;
- public static Button toggleWinch;
public static Button runIndexWheel;
public static Button reverseIndexWheel;
public static Button increaseShooterSpeed;
public static Button decreaseShooterSpeed;
+ private static Button changeCam;
+
+ private static Button togglePiston;
+ private static Button toggleDriveTrainPiston;
+
+ public static Button brakeCANTalons;
+ public static Button coastCANTalons;
+
public OI() {
+
leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
Constants.OI.REVERSE_INTAKE_PORT);
reverseIntake.whileHeld(new ReverseIntakeContinuous());
- toggleWinch = new JoystickButton(leftJoystick,
- Constants.OI.TOGGLE_WINCH_PORT);
- toggleWinch.whenPressed(new ToggleWinch());
-
increaseShooterSpeed = new JoystickButton(leftJoystick,
Constants.OI.INCREASE_SHOOTER_SPEED_PORT);
increaseShooterSpeed.whenPressed(new IncreaseShootingSpeed());
Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
+ changeCam = new JoystickButton(rightJoystick,
+ Constants.OI.CHANGE_CAMERA_VIEW);
+ changeCam.toggleWhenPressed(new ChangeCameraView());
+
+ togglePiston = new JoystickButton(rightJoystick,
+ Constants.Shooter.TOGGLE_INDEXER);
+ togglePiston.whenPressed(new ToggleIndexerPiston());
+
+ toggleDriveTrainPiston = new JoystickButton(rightJoystick,
+ Constants.DriveTrain.TOGGLE_DRIVE_PISTON);
+ toggleDriveTrainPiston.whenPressed(new ToggleGear());
++
+ brakeCANTalons = new JoystickButton(rightJoystick,
+ Constants.OI.BRAKE_CANTALONS_PORT);
+ brakeCANTalons.whenPressed(new BrakeCANTalons());
+
+ coastCANTalons = new JoystickButton(rightJoystick,
+ Constants.OI.COAST_CANTALONS_PORT);
+ coastCANTalons.whenPressed(new CoastCANTalons());
}
public static OI getOI() {
oi = new OI();
return oi;
}
+
+ /*
+ * @Override public void keyPressed(KeyEvent e) { if (e.getKeyCode() ==
+ * KeyEvent.VK_Z) { // Robot.swapCameraFeed();
+ * Robot.getShooter().runIndexWheel(); } }
+ *
+ * @Override public void keyReleased(KeyEvent e) { if (e.getKeyCode() ==
+ * KeyEvent.VK_Z) { // Robot.swapCameraFeed();
+ * Robot.getShooter().stopIndexWheel(); } }
+ *
+ * @Override public void keyTyped(KeyEvent e) { }
+ */
+
}
private static Shooter shooter;
private static OI oi;
private static Intake intake;
+ private static CameraServer server;
@Override
public void robotInit() {
oi = OI.getOI();
shooter = Shooter.getShooter();
intake = Intake.getIntake();
- CameraServer server = CameraServer.getInstance();
+
+ server = CameraServer.getInstance();
UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
+
+ driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
}
public static DriveTrain getDriveTrain() {
+
return DriveTrain.getDriveTrain();
}
return Intake.getIntake();
}
+ public static void swapCameraFeed() {
+ UsbCamera climberCam = server.startAutomaticCapture("climbercam", 1);
+ UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 0);
+ }
+
// If the gear values do not match in the left and right piston, then they are
// both set to high gear
@Override
public void autonomousInit() {
driveTrain.setHighGear();
+ driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
}
@Override
@Override
public void teleopInit() {
+ driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
}
@Override
public void teleopPeriodic() {
+ // driveTrain.printEncoderOutput();
Scheduler.getInstance().run();
updateSmartDashboard();
}
+ @Override
+ public void disabledInit() {
+ driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE);
+ }
+ //
+ // @Override
+ // public void disabledPeriodic() {
+ // Scheduler.getInstance().add(new RunFlyWheel(2));
+ // }
+
public void updateSmartDashboard() {
+ SmartDashboard.putNumber("left encode ",
+ driveTrain.getLeftEncoderDistance());
+ SmartDashboard.putNumber("right encoder",
+ driveTrain.getRightEncoderDistance());
SmartDashboard.putNumber("angle", driveTrain.getAngle());
SmartDashboard.putNumber("voltage",
DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putNumber("rpm", shooter.getShooterRPM());
- SmartDashboard.putNumber("motor value", shooter.getCurrentShootingSpeed());
+ SmartDashboard.putNumber("target shooting",
+ shooter.getTargetShootingSpeed());
}
}
import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
-
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
*/
public class RunIndexWheelContinuous extends Command {
private Shooter shooter = Robot.getShooter();
+ private Timer t = new Timer();
/**
* See JavaDoc comment in class for details
- *
- * @param motorVal
- * value range from -1 to 1
*/
public RunIndexWheelContinuous() {
requires(shooter);
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ t.start();
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
-
- if (timeSinceInitialized() % 0.5 <= 0.02) {
+ if (t.get() >= 1) {
if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) {
Shooter.getShooter().setHighGear();
} else {
Shooter.getShooter().setLowGear();
}
+ t.reset();
}
- double shooterSpeed = shooter.getShooterRPM();
- double targetShooterSpeed = shooter.getTargetShootingSpeed();
- double threshold = shooter.getRPMThreshold();
- if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold)
+ if (shooter.isShooterRPMWithinRangeOfTargetSpeed(25))
shooter.runIndexWheel();
++
}
- // Called once after isFinished returns true
@Override
protected void end() {
shooter.stopIndexWheel();
}
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
@Override
protected void interrupted() {
end();
@Override
protected boolean isFinished() {
return false;
-
}
}
import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.MathLib;
import org.usfirst.frc.team3501.robot.utils.HallEffectSensor;
+ import org.usfirst.frc.team3501.robot.utils.PIDController;
import com.ctre.CANTalon;
import edu.wpi.first.wpilibj.command.Subsystem;
public class Shooter extends Subsystem {
- public double wheelP = 0.0001, wheelI = 0, wheelD = 0.0004;
+ public double wheelP = 0.00007, wheelI = 0, wheelD = 0.0008;
private static Shooter shooter;
private static HallEffectSensor hallEffect;
private final CANTalon flyWheel1, flyWheel2, indexWheel;
- private static final double DEFAULT_INDEXING_SPEED = -0.75;
- private static final double DEFAULT_SHOOTING_SPEED = 2800; // rpm
- private static final double SHOOTING_SPEED_INCREMENT = 25;
+ private PIDController wheelController;
+
+ private static final double RPM_THRESHOLD = 10;
+ private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75;
+ private static final double DEFAULT_SHOOTING_SPEED = 3100; // rpm
+ private static final double SHOOTING_SPEED_INCREMENT = 50;
+ private static final int ACCEPTABLE_SHOOTING_DEVIATION = 300;
- private double currentShootingSpeed = DEFAULT_SHOOTING_SPEED;
+ private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
+ private double currentShooterMotorValue = 0;
private final DoubleSolenoid piston;
hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1);
- piston = new DoubleSolenoid(Constants.Shooter.PISTON_MODULE,
+ piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER,
Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE);
}
}
+ public double getRPMThreshold() {
+ return RPM_THRESHOLD;
+ }
+
public double getShooterRPM() {
return hallEffect.getRPM();
}
- public void setCurrentShootingSpeed(double Value) {
- currentShootingSpeed = Value;
+ public void setTargetShootingSpeed(double Value) {
+ targetShootingSpeed = Value;
}
- public void decrementCurrentShootingSpeed() {
- this.currentShootingSpeed -= this.SHOOTING_SPEED_INCREMENT;
+ public void decrementTargetShootingSpeed() {
+ this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT;
}
- public void incrementCurrentShootingSpeed() {
- this.currentShootingSpeed += this.SHOOTING_SPEED_INCREMENT;
+ public void incrementTargetShootingSpeed() {
+ this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT;
}
- public void resetCurrentShootingSpeed() {
- this.currentShootingSpeed = this.DEFAULT_SHOOTING_SPEED;
+ public void resetTargetShootingSpeed() {
+ this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED;
}
- public double getCurrentShootingSpeed() {
- return currentShootingSpeed;
+ public double getTargetShootingSpeed() {
+ return targetShootingSpeed;
}
public void reverseIndexWheel() {
- this.setIndexWheelMotorVal(-DEFAULT_INDEXING_SPEED);
+ this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE);
}
public void runIndexWheel() {
- this.setIndexWheelMotorVal(DEFAULT_INDEXING_SPEED);
+ this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE);
+ }
+
+ public double calculateShooterSpeed() {
+ this.wheelController.setSetPoint(targetShootingSpeed);
+ double calculatedShooterIncrement = this.wheelController
+ .calcPID(this.getShooterRPM());
+ currentShooterMotorValue += calculatedShooterIncrement;
+ return currentShooterMotorValue;
+ }
+
+ public void initializePIDController() {
+ this.wheelController = new PIDController(wheelP, wheelI, wheelD);
+ this.wheelController.setDoneRange(10);
+ this.wheelController.setMaxOutput(1.0);
+ this.wheelController.setMinDoneCycles(3);
+ this.wheelController.setSetPoint(this.targetShootingSpeed);
+ this.currentShooterMotorValue = 0;
}
public Value getPistonValue() {
private void changeGear(DoubleSolenoid.Value gear) {
piston.set(gear);
}
+
+ public boolean isShooterRPMAtTargetSpeed() {
+ return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION);
+ }
+
+ public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError) {
+ double shooterSpeed = getShooterRPM();
+ if (shooterSpeed > DEFAULT_SHOOTING_SPEED - acceptableRPMError
+ && shooterSpeed < DEFAULT_SHOOTING_SPEED + acceptableRPMError) {
+ return true;
+ }
+ return false;
+ }
}
projects / 3501 / 2017steamworks / commitdiff