diff --git a/src/org/usfirst/frc/team2854/PID/DummyPIDOutput.java b/src/org/usfirst/frc/team2854/PID/DummyPIDOutput.java old mode 100755 new mode 100644 index cb554da..37d0e3e --- a/src/org/usfirst/frc/team2854/PID/DummyPIDOutput.java +++ b/src/org/usfirst/frc/team2854/PID/DummyPIDOutput.java @@ -2,19 +2,31 @@ import edu.wpi.first.wpilibj.PIDOutput; +/** + * @author Kevin Palani + * Class that represents a Dummy PID Output + */ public class DummyPIDOutput implements PIDOutput { - private double output; + private double output; + + /** + * Default constructor + */ + public DummyPIDOutput() { + output = 0; + } + + /** + * Sets PID output value to output. + */ + @Override + public void pidWrite(double output) { + //System.out.println("PId writing"); + this.output = output; + } - public DummyPIDOutput() { - output = 0; - } - @Override - public void pidWrite(double output) { - // System.out.println("PId writing"); - this.output = output; - } public double getOutput() { return output; diff --git a/src/org/usfirst/frc/team2854/robot/Robot.java b/src/org/usfirst/frc/team2854/robot/Robot.java index 54b920e..90b7f97 100644 --- a/src/org/usfirst/frc/team2854/robot/Robot.java +++ b/src/org/usfirst/frc/team2854/robot/Robot.java @@ -18,20 +18,20 @@ import org.usfirst.frc.team2854.robot.subsystems.Restartabale; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the IterativeRobot documentation. If you change the name of this class - * or the package after creating this project, you must also update the manifest file in the - * resource directory. + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the manifest file in the resource + * directory. */ public class Robot extends IterativeRobot { public static OI oi; Command autonomousCommand; SendableChooser chooser = new SendableChooser<>(); - private static HashMap subsystems; + private static HashMap subsystems; private static SensorBoard sensors; - /** * This function is run when the robot is first started up and should be used * for any initialization code. @@ -43,21 +43,23 @@ public void robotInit() { subsystems.put(SubsystemNames.DRIVE_TRAIN, new DriveTrain()); sensors = new SensorBoard(); - - for(Subsystem s : subsystems.values()) { - if(s instanceof Restartabale) { + + for (Subsystem s : subsystems.values()) { + if (s instanceof Restartabale) { ((Restartabale) s).enable(); } + } - -// double fieldWidth = 5; -// double fieldHeight = 5; -// FieldMap map = new FieldMap(fieldWidth, fieldHeight); -// MapInput input = new EncoderBased(); -// FieldMapDriver mapDrive = new FieldMapDriver(map, 720, 720, input); -// + + // double fieldWidth = 5; + // double fieldHeight = 5; + // FieldMap map = new FieldMap(fieldWidth, fieldHeight); + // MapInput input = new EncoderBased(); + // FieldMapDriver mapDrive = new FieldMapDriver(map, 720, 720, input); + // } + /** * This function is called once each time the robot enters Disabled mode. You * can use it to reset any subsystem information you want to clear when the @@ -65,8 +67,8 @@ public void robotInit() { */ @Override public void disabledInit() { - for(Subsystem s : subsystems.values()) { - if(s instanceof Restartabale) { + for (Subsystem s : subsystems.values()) { + if (s instanceof Restartabale) { ((Restartabale) s).disable(); } } @@ -91,12 +93,12 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - for(Subsystem s : subsystems.values()) { - if(s instanceof Restartabale) { + for (Subsystem s : subsystems.values()) { + if (s instanceof Restartabale) { ((Restartabale) s).enable(); } } - + } /** @@ -109,13 +111,13 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - for(Subsystem s : subsystems.values()) { - if(s instanceof Restartabale) { + for (Subsystem s : subsystems.values()) { + if (s instanceof Restartabale) { ((Restartabale) s).enable(); } } - - //OI.buttonA.whenPressed(new DriveMotionMagik()); + + // OI.buttonA.whenPressed(new DriveMotionMagik()); } @@ -127,15 +129,14 @@ public void teleopPeriodic() { SmartDashboard.putBoolean("NavX is Connected", sensors.getNavX().isConnected()); SmartDashboard.putBoolean("NavX is Calibrating", sensors.getNavX().isCalibrating()); - ((DriveTrain)getSubsystem(SubsystemNames.DRIVE_TRAIN)).writeToDashBoard(); -// double angle = sensors.getNavX().getAngle(); -// while(angle < 0) { -// angle += 360; -// } -// SmartDashboard.putNumber("Gyro", angle % 360); + ((DriveTrain) getSubsystem(SubsystemNames.DRIVE_TRAIN)).writeToDashBoard(); + // double angle = sensors.getNavX().getAngle(); + // while(angle < 0) { + // angle += 360; + // } + // SmartDashboard.putNumber("Gyro", angle % 360); - - Scheduler.getInstance().run(); + Scheduler.getInstance().run(); } /** @@ -149,10 +150,9 @@ public void testPeriodic() { public static Subsystem getSubsystem(SubsystemNames name) { return subsystems.get(name); } + public static SensorBoard getSensors() { return sensors; } - - } diff --git a/src/org/usfirst/frc/team2854/robot/SensorBoard.java b/src/org/usfirst/frc/team2854/robot/SensorBoard.java index 0011155..d64d5e7 100644 --- a/src/org/usfirst/frc/team2854/robot/SensorBoard.java +++ b/src/org/usfirst/frc/team2854/robot/SensorBoard.java @@ -18,9 +18,16 @@ public class SensorBoard { private AHRS navX; private ADXRS450_Gyro spiGyro; private BuiltInAccelerometer builtInacc; + private DriveTrain drive; - public SensorBoard() { + + /** + * Default constructor. + */ + + +public SensorBoard() { // navX = new AHRS(I2C.Port.kMXP); String type = null; @@ -73,37 +80,61 @@ public SensorBoard() { // forwardAccel = new DualSensor("Forward Acceleration"); drive = (DriveTrain) Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN); } + + /** + * Calibrates gyro and forward accelerometer. + */ +// public void calibrate(long time) { +// long startTime = System.nanoTime(); +// while(System.nanoTime() - startTime < time) { +// gyro.addValue(spiGyro.getRate(), navX.getRate()); +// forwardAccel.addValue(builtInacc.getX(), navX.getRawAccelY()); +// } +// gyro.calibrate(); +// forwardAccel.calibrate(); +// } + /** + * Gets the value of the gyro. + * @return - value of the gyro + */ +// public double getGyroValue() { +// return gyro.calculateValue(spiGyro.getRate(), navX.getRate()); +// } + /** + * Gets the value of the forward acceleration. + * @return - value of the forward acceleration + */ +// public double getForwardAccelValue() { +// return forwardAccel.calculateValue(builtInacc.getX(), navX.getRawAccelY()); +// } + /** + * Gets gyro. + * @return - gyro + */ +// public DualSensor getGyro() { +// return gyro; +// } + /** + * Gets the forward acceleration. + * @return - forward acceleration. + */ +// public DualSensor getForwardAccel() { +// return forwardAccel; +// } - // public void calibrate(long time) { - // long startTime = System.nanoTime(); - // while(System.nanoTime() - startTime < time) { - // gyro.addValue(spiGyro.getRate(), navX.getRate()); - // forwardAccel.addValue(builtInacc.getX(), navX.getRawAccelY()); - // } - // gyro.calibrate(); - // forwardAccel.calibrate(); - // } - - // public double getGyroValue() { - // return gyro.calculateValue(spiGyro.getRate(), navX.getRate()); - // } - // - // public double getForwardAccelValue() { - // return forwardAccel.calculateValue(builtInacc.getX(), navX.getRawAccelY()); - // } - // - // public DualSensor getGyro() { - // return gyro; - // } - // - // public DualSensor getForwardAccel() { - // return forwardAccel; - // } + /** + * Getter method for the Accelerometer. + * @return - built in accelerometer. + */ public BuiltInAccelerometer getAccelerometer() { return builtInacc; } + /** + * Getter method for NavX object. + * @return - NavX object + */ public AHRS getNavX() { return navX; } diff --git a/src/org/usfirst/frc/team2854/robot/SubsystemNames.java b/src/org/usfirst/frc/team2854/robot/SubsystemNames.java index 3a2db65..4dd3384 100755 --- a/src/org/usfirst/frc/team2854/robot/SubsystemNames.java +++ b/src/org/usfirst/frc/team2854/robot/SubsystemNames.java @@ -1,5 +1,9 @@ package org.usfirst.frc.team2854.robot; +/** + * @author Kevin Palani + * SubsystemNames + */ public enum SubsystemNames { DRIVE_TRAIN, SHIFTER; diff --git a/src/org/usfirst/frc/team2854/robot/commands/DriveMotionMagik.java b/src/org/usfirst/frc/team2854/robot/commands/DriveMotionMagik.java old mode 100755 new mode 100644 index b77c0d4..632435b --- a/src/org/usfirst/frc/team2854/robot/commands/DriveMotionMagik.java +++ b/src/org/usfirst/frc/team2854/robot/commands/DriveMotionMagik.java @@ -9,6 +9,12 @@ import org.usfirst.frc.team2854.robot.SubsystemNames; import org.usfirst.frc.team2854.robot.subsystems.DriveTrain; +import com.ctre.phoenix.motion.MotionProfileStatus; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.command.Command; + + /** */ public class DriveMotionMagik extends Command { diff --git a/src/org/usfirst/frc/team2854/robot/commands/DriveThottle.java b/src/org/usfirst/frc/team2854/robot/commands/DriveThottle.java old mode 100755 new mode 100644 index 9cbd091..44661fc --- a/src/org/usfirst/frc/team2854/robot/commands/DriveThottle.java +++ b/src/org/usfirst/frc/team2854/robot/commands/DriveThottle.java @@ -9,40 +9,40 @@ /** */ public class DriveThottle extends Command { - private double percent; - private DriveTrain drive; - - public DriveThottle(double percent) { - requires(Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN)); - this.percent = percent; - } - - // Called just before this Command runs the first time - protected void initialize() { - drive = ((DriveTrain) Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN)); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - System.out.println("Driving"); - drive.drive(percent, percent, ControlMode.Velocity); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - System.out.println("Stopping"); - drive.stop(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - System.out.println("Interupted"); - drive.stop(); - } + private double percent; + private DriveTrain drive; + + public DriveThottle(double percent) { + requires(Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN)); + this.percent = percent; + } + + // Called just before this Command runs the first time + protected void initialize() { + drive = ((DriveTrain) Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN)); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + System.out.println("Driving"); + drive.drive(percent, percent, ControlMode.Velocity); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + System.out.println("Stopping"); + drive.stop(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + System.out.println("Interupted"); + drive.stop(); + } } diff --git a/src/org/usfirst/frc/team2854/robot/commands/JoystickDrive.java b/src/org/usfirst/frc/team2854/robot/commands/JoystickDrive.java old mode 100755 new mode 100644 index da12028..3cdf236 --- a/src/org/usfirst/frc/team2854/robot/commands/JoystickDrive.java +++ b/src/org/usfirst/frc/team2854/robot/commands/JoystickDrive.java @@ -8,21 +8,31 @@ import org.usfirst.frc.team2854.robot.SubsystemNames; import org.usfirst.frc.team2854.robot.subsystems.DriveTrain; -/** */ +/** + * Drives the robot using joystick drive. + */ + public class JoystickDrive extends Command { private DriveTrain drive; + /** + * Default constructor. + */ public JoystickDrive() { requires(Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN)); } - // Called just before this Command runs the first time + /** + * Initializes the drivetrain. + */ protected void initialize() { drive = (DriveTrain) Robot.getSubsystem(SubsystemNames.DRIVE_TRAIN); } - // Called repeatedly when this Command is scheduled to run + /** + * Executes the driving. + */ protected void execute() { double speed = OI.joystick.getRawAxis(2) - OI.joystick.getRawAxis(3); // SmartDashboard.putNumber("target val", speed * Config.manuelSpeedMultiplier); @@ -43,18 +53,23 @@ protected void execute() { // drive.drive(speed, speed, ControlMode.PercentOutput); } - // Make this return true when this Command no longer needs to run execute() + /** + * Stops drive train. + */ protected boolean isFinished() { return false; } - // Called once after isFinished returns true + /** + * Stops driving. + */ protected void end() { drive.stop(); } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run + /** + * Stops drive when interrupted. + */ protected void interrupted() { drive.stop(); } diff --git a/src/org/usfirst/frc/team2854/robot/commands/RunAllTalons.java b/src/org/usfirst/frc/team2854/robot/commands/RunAllTalons.java old mode 100755 new mode 100644 index 00e7a31..b7e041e --- a/src/org/usfirst/frc/team2854/robot/commands/RunAllTalons.java +++ b/src/org/usfirst/frc/team2854/robot/commands/RunAllTalons.java @@ -5,42 +5,60 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; -/** */ +/** + * Runs all the Talons. + */ public class RunAllTalons extends Command { - public RunAllTalons() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() {} - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - int i; - for (i = 0; i < 6; i++) { - try { - TalonSRX talon = new TalonSRX(i); - talon.set(ControlMode.PercentOutput, 1); - Timer.delay(2); - talon.set(ControlMode.PercentOutput, 0); - System.out.println("Talon #" + i + " alive"); - } catch (Exception e) { - System.out.println("Talon #" + i + " died"); - } - } - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - protected void end() {} - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() {} + /** + * Default constructor + */ + public RunAllTalons() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + /** + * Initializes the Talons. + */ + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + /** + * Checks if Talon is functional. + */ + protected void execute() { + int i; + for (i = 0; i < 6; i++) { + try { + TalonSRX talon = new TalonSRX(i); + talon.set(ControlMode.PercentOutput, 1); + Timer.delay(2); + talon.set(ControlMode.PercentOutput, 0); + System.out.println("Talon #" + i + " alive"); + } catch (Exception e) { + System.out.println("Talon #" + i + " died"); + } + + } + + } + + // Make this return true when this Command no longer needs to run execute() + + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + } diff --git a/src/org/usfirst/frc/team2854/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team2854/robot/subsystems/DriveTrain.java old mode 100755 new mode 100644 index 55f772f..a3f23d0 --- a/src/org/usfirst/frc/team2854/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team2854/robot/subsystems/DriveTrain.java @@ -35,7 +35,7 @@ public class DriveTrain extends Subsystem implements Restartabale { private DoubleSolenoid shifter; private GearState gear; - + private boolean autoShift = true; enum GearState { @@ -71,6 +71,9 @@ public void initDefaultCommand() { setDefaultCommand(new JoystickDrive()); } + /** + * Default constructor. + */ public DriveTrain() { leftT1 = new TalonSRX(RobotMap.leftTalonID1); leftT1.setInverted(side); @@ -96,8 +99,6 @@ public DriveTrain() { leftT1.set(ControlMode.Follower, leftT2.getDeviceID()); rightT1.set(ControlMode.Follower, rightT2.getDeviceID()); - // PIDConstant.startSmartDashboardInput(PIDConstant.highDrive, leftT2, rightT2); - } public void enable() { @@ -109,8 +110,21 @@ public void enable() { public void disable() { System.out.println("Disableing drive train"); // turnController.disable(); + } + /** + * Initializes PID values when turning + * + * @param P + * - proportional gain + * @param I + * - integral gain + * @param D + * - derivative gain + * @param F + * - feed forward term + */ public void initTurnPID(double P, double I, double D, double F) { PIDSource turnSource = new PIDSource() { @@ -145,6 +159,9 @@ public PIDSourceType getPIDSourceType() { turnController.enable(); } + /** + * Writes to SmartDashboard + */ public void writeToDashBoard() { SmartDashboard.putNumber("Left Velocity", leftT2.getSelectedSensorVelocity(0)); @@ -163,6 +180,9 @@ public void writeToDashBoard() { } + /** + * Shifts the gears on the robot. + */ private void applyShift(GearState desiredState, int attempt) { if (attempt > 10) { System.err.println("Shifter is not shifting to " + desiredState + " at attempt " + attempt); @@ -212,6 +232,16 @@ public void toggleShift() { } } + /** + * Method that drives the robot at the specified ControlMode. + * + * @param left + * - input percentage + * @param right + * - input percentage + * @param mode + * - ControlMode object that specifies ControlMode + */ public void drive(double left, double right, ControlMode mode) { // System.out.println(mode.toString() + " " + ControlMode.Velocity + " " + // mode.equals(ControlMode.Velocity)); @@ -231,28 +261,49 @@ public void drive(double left, double right, ControlMode mode) { left += leftT2.getSelectedSensorPosition(0); right += rightT2.getSelectedSensorPosition(0); } - //System.out.println("target left + " + left); + // System.out.println("target left + " + left); // System.out.println(left + " " + right); leftT2.set(mode, left * Config.totalDriveSpeedMultiplier); rightT2.set(mode, right * Config.totalDriveSpeedMultiplier); - + } + /** + * Drives the robot straight. + * + * @param left + * - input percentage + * @param right + * - input percentage + * @param mode + * - ControlMode object that specifies ControlMode + */ public void driveStraight(double left, double right, ControlMode mode) { double output = turnController.get(); System.out.println(output); turnController.setSetpoint(0); } - + public void setNeutralMode(NeutralMode mode) { rightT2.setNeutralMode(mode); leftT2.setNeutralMode(mode); } + /** + * Drives the robot at the PercentOutput Control Mode. + * + * @param left + * - input percentage + * @param right + * - input percentage + */ public void drive(double left, double right) { drive(left, right, ControlMode.PercentOutput); } + /** + * Stops the robot. + */ public void stop() { drive(0, 0); } @@ -265,8 +316,8 @@ public double getAvgVelocity() { return (rightT2.getSelectedSensorVelocity(0) + leftT2.getSelectedSensorVelocity(0)) / 2d; } - public double inchesToCycles(double d) { //TODO finish this - return (d+.997)/6.96d; + public double inchesToCycles(double d) { + return (d + .997) / 6.96d; } public boolean isAutoShift() { @@ -276,7 +327,7 @@ public boolean isAutoShift() { public void setAutoShift(boolean autoShift) { this.autoShift = autoShift; } - + public double getDriveConstant() { return (gear == GearState.HIGH ? Config.highTarget : Config.lowTarget); }