package org.usfirst.frc.team3501.robot;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.SPI;
+
/**
* The Constants stores constant values for all subsystems. This includes the
* port values for motors and sensors, as well as important operational
public final static int TOGGLE_WINCH_PORT = 0;
public final static int TOGGLE_FLYWHEEL_PORT = 0;
public final static int TOGGLE_INDEXWHEEL_PORT = 0;
- public static final int TOGGLE_CAMERA_FEEDS = 4;
+
+ public final static int TOGGLE_GEAR_PORT = 0;
+
+ public static final int TOGGLE_CAMERA_FEEDS = 7;
+
+ public static final int TOGGLE_CAMERA_FEEDS = 4;
+
}
public static class Shooter {
}
public static class DriveTrain {
+ // GEARS
+ public static final int MODULE_NUMBER = 10, LEFT_GEAR_PISTON_FORWARD = 6,
+ LEFT_GEAR_PISTON_REVERSE = 5, RIGHT_GEAR_PISTON_FORWARD = 0,
+ RIGHT_GEAR_PISTON_REVERSE = 1;
+ public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
+ public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
+
// MOTOR CONTROLLERS
public static final int FRONT_LEFT = 1;
public static final int FRONT_RIGHT = 3;
public static final int ENCODER_LEFT_B = 0;
public static final int ENCODER_RIGHT_A = 2;
public static final int ENCODER_RIGHT_B = 3;
- }
- public static class Climber {
- // MOTOR CONTROLLERS
- public static final int MOTOR_VAL = 1;
+ public static final SPI.Port GYRO_PORT = SPI.Port.kOnboardCS0;
}
public static class Intake {
public static final int INTAKE_ROLLER_PORT = 0;
+
}
public static enum Direction {
package org.usfirst.frc.team3501.robot;
+
+
+import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
+=======
+import org.usfirst.frc.team3501.robot.commands.accessories.ToggleCameraFeed;
+>>>>>>> added command for toggling camera feeds
+=======
import org.usfirst.frc.team3501.robot.commands.accessories.ToggleCameraFeed;
+>>>>>>> cb0f662264528bcb0d783fcb00db22eb8d4f4283
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
public static Button toggleIndexWheel;
public static Button toggleFlyWheel;
-
+
+ public static Button toggleCameraFeeds;
+
public static Button toggleCameraFeeds;
+ public static Button toggleGear;
+
public OI() {
leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
Constants.OI.TOGGLE_INDEXWHEEL_PORT);
toggleFlyWheel = new JoystickButton(leftJoystick,
Constants.OI.TOGGLE_FLYWHEEL_PORT);
-
+
+
+ toggleGear = new JoystickButton(leftJoystick,
+ Constants.OI.TOGGLE_GEAR_PORT);
+ toggleGear.whenPressed(new ToggleGear());
+
+
toggleCameraFeeds = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_CAMERA_FEEDS);
-
+
toggleCameraFeeds.whenReleased(new ToggleCameraFeed());
+
+
+
+ toggleCameraFeeds = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_CAMERA_FEEDS);
+
+ toggleCameraFeeds.whenReleased(new ToggleCameraFeed());
+
}
public static OI getOI() {
oi = OI.getOI();
shooter = Shooter.getShooter();
intake = Intake.getIntake();
+
+ // usbCamera = CameraServer.getInstance().startAutomaticCapture();
+
+ // cameraServer2 = CameraServer;getInstance();
+ // axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
+
+ cameraServer2 = CameraServer.getInstance();
+ axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
+
+ cameraFeeds = new CameraFeeds();
+
// usbCamera = CameraServer.getInstance().startAutomaticCapture();
// CameraServer.getInstance().startAutomaticCapture();
// cameraServer2 = CameraServer.getInstance();
// axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
+
+ // cameraFeeds = new CameraFeeds();
+
cameraFeeds = new CameraFeeds();
+
}
public static DriveTrain getDriveTrain() {
return Intake.getIntake();
}
+ // 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();
}
@Override
@Override
public void teleopInit() {
+
cameraFeeds.init();
+
+ cameraFeeds.init();
+
}
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
-
}
}
--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+public class Robot extends IterativeRobot {
+ private static DriveTrain driveTrain;
+ private static Shooter shooter;
+ private static OI oi;
+ private static Intake intake;
+ private static UsbCamera usbCamera;
+ private static CameraServer cameraServer2;
+ private static AxisCamera axisCamera;
+
+ @Override
+ public void robotInit() {
+ driveTrain = DriveTrain.getDriveTrain();
+ oi = OI.getOI();
+ shooter = Shooter.getShooter();
+ intake = Intake.getIntake();
+<<<<<<< HEAD
+
+=======
+ usbCamera = CameraServer.getInstance().startAutomaticCapture();
+ // cameraServer2 = CameraServer;getInstance();
+ // axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
+>>>>>>> Move code to more updated branch.
+ }
+
+ public static DriveTrain getDriveTrain() {
+ return DriveTrain.getDriveTrain();
+ }
+
+ public static Shooter getShooter() {
+ return Shooter.getShooter();
+ }
+
+ public static OI getOI() {
+ return OI.getOI();
+ }
+
+ public static Intake getIntake() {
+ return Intake.getIntake();
+ }
+
+ // 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();
+ }
+
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+
+ @Override
+ public void teleopInit() {
+
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+public class Robot extends IterativeRobot {
+ private static DriveTrain driveTrain;
+ private static Shooter shooter;
+ private static OI oi;
+ private static Intake intake;
+
+ @Override
+ public void robotInit() {
+ driveTrain = DriveTrain.getDriveTrain();
+ oi = OI.getOI();
+ shooter = Shooter.getShooter();
+ intake = Intake.getIntake();
+ }
+
+ public static DriveTrain getDriveTrain() {
+ return DriveTrain.getDriveTrain();
+ }
+
+ public static Shooter getShooter() {
+ return Shooter.getShooter();
+ }
+
+ public static OI getOI() {
+ return OI.getOI();
+ }
+
+ public static Intake getIntake() {
+ return Intake.getIntake();
+ }
+
+ @Override
+ public void autonomousInit() {
+ }
+
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+
+ @Override
+ public void teleopInit() {
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+public class Robot extends IterativeRobot {
+ private static DriveTrain driveTrain;
+ private static Shooter shooter;
+ private static OI oi;
+ private static Intake intake;
+
+ @Override
+ public void robotInit() {
+ driveTrain = DriveTrain.getDriveTrain();
+ oi = OI.getOI();
+ shooter = Shooter.getShooter();
+ intake = Intake.getIntake();
+
+ }
+
+ public static DriveTrain getDriveTrain() {
+ return DriveTrain.getDriveTrain();
+ }
+
+ public static Shooter getShooter() {
+ return Shooter.getShooter();
+ }
+
+ public static OI getOI() {
+ return OI.getOI();
+ }
+
+ public static Intake getIntake() {
+ return Intake.getIntake();
+ }
+
+ // 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();
+ }
+
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+
+ @Override
+ public void teleopInit() {
+
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Scheduler;
+
+public class Robot extends IterativeRobot {
+ private static DriveTrain driveTrain;
+ private static Shooter shooter;
+ private static OI oi;
+ private static Intake intake;
+ private static UsbCamera usbCamera;
+ private static CameraServer cameraServer2;
+ private static AxisCamera axisCamera;
+
+ @Override
+ public void robotInit() {
+ driveTrain = DriveTrain.getDriveTrain();
+ oi = OI.getOI();
+ shooter = Shooter.getShooter();
+ intake = Intake.getIntake();
+ usbCamera = CameraServer.getInstance().startAutomaticCapture();
+ // cameraServer2 = CameraServer;getInstance();
+ // axisCamera = cameraServer2.addAxisCamera("axisCamera", "10.35.1.11");
+ }
+
+ public static DriveTrain getDriveTrain() {
+ return DriveTrain.getDriveTrain();
+ }
+
+ public static Shooter getShooter() {
+ return Shooter.getShooter();
+ }
+
+ public static OI getOI() {
+ return OI.getOI();
+ }
+
+ public static Intake getIntake() {
+ return Intake.getIntake();
+ }
+
+ @Override
+ public void autonomousInit() {
+ }
+
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+
+ @Override
+ public void teleopInit() {
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commandgroups;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- * Example Command group
- */
-public class ExampleCommandGroup extends CommandGroup {
-
- public ExampleCommandGroup() {
- // Add Commands here:
- // e.g. addSequential(new Command1());
- // addSequential(new Command2());
- // these will run in order.
-
- // To run multiple commands at the same time,
- // use addParallel()
- // e.g. addParallel(new Command1());
- // addSequential(new Command2());
- // Command1 and Command2 will run in parallel.
-
- // A command group will require all of the subsystems that each member
- // would require.
- // e.g. if Command1 requires chassis, and Command2 requires arm,
- // a CommandGroup containing them would require both the chassis and the
- // arm.
- }
-}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.climber;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command runs the winch at a specified speed when the robot has completed
+ * the climb and when the button triggering it is pressed. This command also
+ * makes the drive train motors run because the winch is controlled by the drive
+ * train.
+ *
+ * pre-condition: This command is run by a button in OI. The robot must have
+ * completed climbing.
+ *
+ * post-condition: Winch motor set to a specified speed.
+ *
+ * @param motorVal
+ * value range is from -1 to 1
+ *
+ * @author shivanighanta
+ *
+ */
+public class MaintainClimbedPosition extends Command {
+
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param time
+ * time in seconds to run the winch
+ */
+ public MaintainClimbedPosition() {
+ requires(Robot.getDriveTrain());
+ }
+
+ @Override
+ protected void initialize() {
+ }
+
+ @Override
+ protected void execute() {
+ Robot.getDriveTrain().setMotorValues(DriveTrain.MAINTAIN_CLIMBED_POSITION,
+ DriveTrain.MAINTAIN_CLIMBED_POSITION);
+
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ protected void end() {
+ Robot.getDriveTrain().stop();
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
import org.usfirst.frc.team3501.robot.Robot;
-import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
*/
public class RunWinch extends Command {
- Timer timer;
private double time;
private double motorVal;
@Override
protected void initialize() {
- timer.start();
}
@Override
@Override
protected boolean isFinished() {
- return timer.get() >= time;
+ return timeSinceInitialized() >= time;
}
@Override
* This command runs the drive train motors (which runs the winch) continuously
* at a specified speed until the button triggering it is released
*
- * pre-condition: This command must be run by a button in OI. The robot must be
- * attached to the rope.
+ * pre-condition: This command must be run by a button in OI with
+ * button.whileHeld(...). The robot must be attached to the rope.
*
* post-condition: Drive train motors set to a specified speed.
*
public class DriveDistance extends Command {
private DriveTrain driveTrain = Robot.getDriveTrain();
private double maxTimeOut;
- private PIDController driveController;
- private PIDController gyroController;
- private Preferences prefs;
-
private double target;
- private double gyroP;
- private double gyroI;
- private double gyroD;
+ private double zeroAngle;
+ private Preferences prefs;
+ private PIDController driveController;
private double driveP;
private double driveI;
private double driveD;
+ private double gyroP;
- public DriveDistance(double distance, double maxTimeOut) {
+ public DriveDistance(double distance, double motorVal) {
requires(driveTrain);
this.maxTimeOut = maxTimeOut;
this.target = distance;
+ this.zeroAngle = driveTrain.getAngle();
this.driveP = driveTrain.driveP;
this.driveI = driveTrain.driveI;
this.driveD = driveTrain.driveD;
- this.gyroP = driveTrain.defaultGyroP;
- this.gyroI = driveTrain.defaultGyroI;
- this.gyroD = driveTrain.defaultGyroD;
- this.driveController = new PIDController(this.driveP, this.driveI,
- this.driveD);
+ this.gyroP = driveTrain.driveStraightGyroP;
+ this.driveController = new PIDController(driveP, driveI, driveD);
this.driveController.setDoneRange(0.5);
this.driveController.setMaxOutput(1.0);
this.driveController.setMinDoneCycles(5);
-
- this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
- this.gyroController.setDoneRange(1);
- this.gyroController.setMinDoneCycles(5);
}
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
- this.driveTrain.resetGyro();
this.driveController.setSetPoint(this.target);
- this.gyroController.setSetPoint(this.driveTrain.getZeroAngle());
}
@Override
protected void execute() {
- double xVal = 0;
- double yVal = this.driveController
- .calcPID(this.driveTrain.getAvgEncoderDistance());
+ double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+ double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
- if (this.driveTrain.getAngle() - this.driveTrain.getZeroAngle() < 30) {
- xVal = -this.gyroController
- .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
- }
double leftDrive = yVal - xVal;
double rightDrive = yVal + xVal;
-
this.driveTrain.setMotorValues(leftDrive, rightDrive);
-
- driveTrain.printEncoderOutput();
}
@Override
protected boolean isFinished() {
- boolean isDone = this.driveController.isDone();
- if (timeSinceInitialized() >= maxTimeOut || isDone)
- System.out.println("time: " + timeSinceInitialized());
- return timeSinceInitialized() >= maxTimeOut || isDone;
+ return timeSinceInitialized() >= maxTimeOut
+ || this.driveController.isDone();
}
@Override
protected void end() {
- driveTrain.stop();
}
@Override
package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.command.Command;
/**
- * This command toggles the speed at which the drive train runs at
+ * This command toggles the gear(low or high).
*
* post-condition: if the drivetrain is running at high gear, it will be made to
* run at low gear, and vice versa
*/
public class ToggleGear extends Command {
+ DriveTrain driveTrain = Robot.getDriveTrain();
public ToggleGear() {
- requires(Robot.getDriveTrain());
+ requires(driveTrain);
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ Value leftGearPistonValue = driveTrain.getLeftGearPistonValue();
+ Value rightGearPistonValue = driveTrain.getRightGearPistonValue();
+ if (leftGearPistonValue == Constants.DriveTrain.LOW_GEAR) {
+ driveTrain.setHighGear();
+ } else {
+ driveTrain.setLowGear();
+ }
}
- // Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
- return false;
+ return true;
}
- // Called once after isFinished returns true
@Override
protected void end() {
}
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
@Override
protected void interrupted() {
}
private double gyroI;
private double gyroD;
+ private double zeroAngle;
+
public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
this.direction = direction;
this.maxTimeOut = maxTimeOut;
- this.target = angle;
+ this.target = Math.abs(angle);
+ this.zeroAngle = driveTrain.getAngle();
- this.gyroP = driveTrain.defaultGyroP;
- this.gyroI = driveTrain.defaultGyroI;
- this.gyroD = driveTrain.defaultGyroD;
+ this.gyroP = driveTrain.turnP;
+ this.gyroI = driveTrain.turnI;
+ this.gyroD = driveTrain.turnD;
this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
this.gyroController.setDoneRange(1);
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
- this.driveTrain.resetGyro();
this.gyroController.setSetPoint(this.target);
}
double xVal = 0;
xVal = this.gyroController
- .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
+ .calcPID(Math.abs(this.driveTrain.getAngle() - this.zeroAngle));
- double leftDrive;
- double rightDrive;
+ double leftDrive = 0;
+ double rightDrive = 0;
if (direction == Constants.Direction.RIGHT) {
leftDrive = xVal;
rightDrive = -xVal;
- } else {
+ } else if (direction == Constants.Direction.LEFT) {
leftDrive = -xVal;
rightDrive = xVal;
}
/**
* Reverses the intake until the button triggering this command is released
*
- * pre-condition: button is pressed
+ * pre-condition: This command must be run by a button in OI with
+ * button.whileHeld(...).
*/
public class ReverseIntakeContinuous extends Command {
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
import edu.wpi.first.wpilibj.command.Command;
/**
* flywheel is run, it will run at the decreased shooting speed
*/
public class DecreaseShootingSpeed extends Command {
+ private Shooter shooter = Robot.getShooter();
+
public DecreaseShootingSpeed() {
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ shooter.CURRENT_SHOOTING_SPEED -= shooter.SHOOTING_SPEED_INCREMENT;
+
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
}
- // Called once after isFinished returns true
@Override
protected void end() {
}
- // 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() {
- // TODO Auto-generated method stub
- return false;
+ return true;
+
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
import edu.wpi.first.wpilibj.command.Command;
/**
* flywheel is run, it will run at the increased shooting speed
*/
public class IncreaseShootingSpeed extends Command {
+ private Shooter shooter = Robot.getShooter();
+
public IncreaseShootingSpeed() {
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ shooter.CURRENT_SHOOTING_SPEED += shooter.SHOOTING_SPEED_INCREMENT;
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
}
- // Called once after isFinished returns true
@Override
protected void end() {
}
- // 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() {
- // TODO Auto-generated method stub
- return false;
+
+ return true;
}
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command resets the speed at which the flywheel runs to the default
+ * shooting speed
+ *
+ * post-condition: the shooting speed is reset
+ */
+public class ResetShootingSpeed extends Command {
+ private Shooter shooter = Robot.getShooter();
+
+ public ResetShootingSpeed() {
+ requires(shooter);
+ }
+
+ @Override
+ protected void initialize() {
+ shooter.CURRENT_SHOOTING_SPEED = shooter.DEFAULT_SHOOTING_SPEED;
+ }
+
+ @Override
+ protected void execute() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ @Override
+ protected void end() {
+ }
+
+ @Override
+
+ protected void interrupted() {
+ end();
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command reverses the index wheel at a given speed for given time in
+ * seconds.
+ *
+ * @author shivanighanta
+ */
+
+public class ReverseIndexWheel extends Command {
+ private double time;
+ private double motorVal;
+
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range from -1 to 1
+ * @param time
+ * in seconds, amount of time to run index wheel motor
+ */
+
+ public ReverseIndexWheel(double time, double motorVal) {
+ requires(Robot.getDriveTrain());
+ this.time = time;
+ this.motorVal = motorVal;
+ }
+
+ @Override
+ protected void initialize() {
+
+ }
+
+ @Override
+ protected void execute() {
+ Robot.getShooter().setIndexWheelMotorVal(-motorVal);
+
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return timeSinceInitialized() >= time;
+ }
+
+ @Override
+ protected void end() {
+ Robot.getShooter().stopIndexWheel();
+
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
+
package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
-import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
- * This command runs the fly wheel at a given speed for a given time. The fly
- * wheel is intended to shoot balls fed by the intake wheel.
+ * This command runs the fly wheel at a specific speed using a PID Controller
+ * for accuracy for a given time. The fly wheel is intended to shoot balls fed
+ * by the intake wheel.
*
- * @author Shaina
+ * @author Shaina & Chris
*/
public class RunFlyWheel extends Command {
- Timer timer;
- private double motorVal;
- private double time;
+ private Shooter shooter = Robot.getShooter();
+ private double maxTimeOut;
+
+ private PIDController wheelController;
+ private double wheelP;
+ private double wheelI;
+ private double wheelD;
+ private double target;
- /**
- * See JavaDoc comment in class for details
- *
- * @param motorVal
- * value range from -1 to 1
- * @param time
- * in seconds, amount of time to run fly wheel motor
- */
- public RunFlyWheel(double motorVal, double time) {
- requires(Robot.getShooter());
+ public RunFlyWheel(double maxTimeOut) {
+ requires(shooter);
- timer = new Timer();
- this.motorVal = motorVal;
- this.time = time;
+ this.wheelP = this.shooter.wheelP;
+ this.wheelI = this.shooter.wheelI;
+ this.wheelD = this.shooter.wheelD;
+ this.wheelController = new PIDController(this.wheelP, this.wheelI,
+ this.wheelD);
+ this.wheelController.setDoneRange(0.5);
+ this.wheelController.setMaxOutput(1.0);
+ this.wheelController.setMinDoneCycles(3);
+ this.target = this.shooter.CURRENT_SHOOTING_SPEED;
}
// Called just before this Command runs the first time
- @Override
protected void initialize() {
- timer.start();
- Robot.getShooter().setFlyWheelMotorVal(motorVal);
+ this.wheelController.setSetPoint(this.target);
}
// Called repeatedly when this Command is scheduled to run
- @Override
protected void execute() {
+ double shooterSpeed = this.wheelController
+ .calcPID(this.shooter.getShooterSpeed());
+
+ this.shooter.setFlyWheelMotorVal(shooterSpeed);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return timeSinceInitialized() >= maxTimeOut;
}
// Called once after isFinished returns true
- @Override
protected void end() {
- Robot.getShooter().stopFlyWheel();
+ this.shooter.stopFlyWheel();
}
// 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 timer.get() >= time;
- }
-
}
package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
import edu.wpi.first.wpilibj.command.Command;
/**
- * This command runs the fly wheel continuously when OI button managing fly
- * wheel is pressed. The command will run the fly wheel motor until the button
- * triggering it is released.
+ * This command runs the fly wheel continuously at a set speed using a PID
+ * Controller when OI button managing fly wheel is pressed. The command will run
+ * the fly wheel motor until the button triggering it is released.
*
* Should only be run from the operator interface.
*
* pre-condition: This command must be run by a button in OI, with
* button.whileHeld(...).
*
- * @author Shaina
+ * @author Shaina & Chris
*/
public class RunFlyWheelContinuous extends Command {
- private double motorVal;
-
- /**
- * See JavaDoc comment in class for details
- *
- * @param motorVal
- * value range from -1 to 1
- */
- public RunFlyWheelContinuous(double motorVal) {
- this.motorVal = motorVal;
+ private Shooter shooter = Robot.getShooter();
+
+ private PIDController wheelController;
+ private double wheelP;
+ private double wheelI;
+ private double wheelD;
+ private double target;
+
+ public RunFlyWheelContinuous() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(shooter);
+
+ this.wheelP = this.shooter.wheelP;
+ this.wheelI = this.shooter.wheelI;
+ this.wheelD = this.shooter.wheelD;
+ this.wheelController = new PIDController(this.wheelP, this.wheelI,
+ this.wheelD);
+ this.wheelController.setDoneRange(0.5);
+ this.wheelController.setMaxOutput(1.0);
+ this.wheelController.setMinDoneCycles(3);
+ this.target = this.shooter.CURRENT_SHOOTING_SPEED;
}
// Called just before this Command runs the first time
- @Override
protected void initialize() {
- Robot.getShooter().setFlyWheelMotorVal(motorVal);
+ this.wheelController.setSetPoint(this.target);
}
// Called repeatedly when this Command is scheduled to run
- @Override
protected void execute() {
+ double shooterSpeed = this.wheelController
+ .calcPID(this.shooter.getShooterSpeed());
+
+ this.shooter.setFlyWheelMotorVal(shooterSpeed);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
}
// Called once after isFinished returns true
- @Override
protected void end() {
+ this.shooter.stopFlyWheel();
}
// 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 !Robot.getOI().toggleFlyWheel.get();
- }
-
}
package org.usfirst.frc.team3501.robot.commands.shooter;
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;
/**
* @author Shaina
*/
public class RunIndexWheel extends Command {
- Timer timer;
+ private Shooter shooter = Robot.getShooter();
private double time;
- private double motorVal;
/**
* See JavaDoc comment in class for details
* @param time
* in seconds, amount of time to run index wheel motor
*/
- public RunIndexWheel(double motorVal, double time) {
- requires(Robot.getShooter());
-
- timer = new Timer();
- this.motorVal = motorVal;
+ public RunIndexWheel(double time) {
+ requires(shooter);
this.time = time;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
- timer.start();
- Robot.getShooter().setIndexWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ shooter.setIndexWheelMotorVal(shooter.DEFAULT_INDEXING_SPEED);
}
// Called once after isFinished returns true
@Override
protected void end() {
- Robot.getShooter().stopIndexWheel();
+ shooter.stopIndexWheel();
}
// Called when another command which requires one or more of the same
@Override
protected boolean isFinished() {
- return timer.get() >= time;
+ return timeSinceInitialized() >= time;
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
import edu.wpi.first.wpilibj.command.Command;
* @author Shaina
*/
public class RunIndexWheelContinuous extends Command {
- private double motorVal;
+ private Shooter shooter = Robot.getShooter();
/**
* See JavaDoc comment in class for details
* @param motorVal
* value range from -1 to 1
*/
- public RunIndexWheelContinuous(double motorVal) {
- this.motorVal = motorVal;
+ public RunIndexWheelContinuous() {
+ requires(shooter);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
- Robot.getShooter().setIndexWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ shooter.setIndexWheelMotorVal(shooter.DEFAULT_INDEXING_SPEED);
}
// Called once after isFinished returns true
@Override
protected boolean isFinished() {
- return Robot.getOI().toggleIndexWheel.get();
+ return false;
}
import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
-import org.usfirst.frc.team3501.robot.utils.BNO055;
import com.ctre.CANTalon;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- public static double driveP = 0.008, driveI = 0.001, driveD = -0.002;
- public static double defaultGyroP = 0.006, defaultGyroI = 0.00000,
- defaultGyroD = -0.000;
- private double gyroZero = 0;
+ public static double driveP = 0.006, driveI = 0.001, driveD = -0.002;
+ public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005;
+ public static double driveStraightGyroP = 0.01;
public static final double WHEEL_DIAMETER = 6; // inches
public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
/ ENCODER_PULSES_PER_REVOLUTION;
-
+ public static final int MAINTAIN_CLIMBED_POSITION = 0;
+ public static final int TIME_TO_CLIMB_FOR = 0;
private static DriveTrain driveTrain;
+
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
+ private final DoubleSolenoid leftGearPiston, rightGearPiston;
- private BNO055 imu;
+ private ADXRS450_Gyro imu;
private DriveTrain() {
// MOTOR CONTROLLERS
// ROBOT DRIVE
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
- this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
- BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28);
- gyroZero = imu.getHeading();
+ this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
+
+ // TODO: Not sure if MODULE_NUMBER should be the same for both
+ leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
+ rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
}
public static DriveTrain getDriveTrain() {
}
public void printEncoderOutput() {
- // System.out.println("left: " + getLeftEncoderDistance());
- // System.out.println("right: " + getRightEncoderDistance());
- System.out.println(getAvgEncoderDistance());
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
+ // System.out.println(getAvgEncoderDistance());
}
public double getAvgEncoderDistance() {
// ------Gyro------//
public double getAngle() {
- if (!this.imu.isInitialized())
- return -1;
- return this.imu.getHeading() - this.gyroZero;
+ return this.imu.getAngle();
}
public void resetGyro() {
- this.gyroZero = this.getAngle();
+ this.imu.reset();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+ /*
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
}
- public double getZeroAngle() {
- return this.gyroZero;
+ /*
+ * Changes the gear to a DoubleSolenoid.Value
+ */
+ private void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
}
@Override
import edu.wpi.first.wpilibj.command.Subsystem;
public class Shooter extends Subsystem {
+ public double wheelP = 0, wheelI = 0, wheelD = -0;
private static Shooter shooter;
private final CANTalon flyWheel, indexWheel;
+ public static final double DEFAULT_INDEXING_SPEED = 0;
+ public static final double DEFAULT_SHOOTING_SPEED = 0;
+ public static double CURRENT_SHOOTING_SPEED = DEFAULT_SHOOTING_SPEED;
+
+ public static final double SHOOTING_SPEED_INCREMENT = 0;
+
private Shooter() {
flyWheel = new CANTalon(Constants.Shooter.FLY_WHEEL);
indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL);
+
}
/**
protected void initDefaultCommand() {
}
+
+ public double getShooterSpeed() {
+ return 0.0;
+ }
}
// close enough to target
if (currError <= this.doneRange) {
this.doneCycleCount++;
- System.out.println(doneCycleCount);
}
// not close enough to target
else {
projects / 3501 / 2017steamworks / commitdiff