diff --git a/PowerUp/.idea/workspace.xml b/PowerUp/.idea/workspace.xml index f4db4c4..727d046 100644 --- a/PowerUp/.idea/workspace.xml +++ b/PowerUp/.idea/workspace.xml @@ -2,6 +2,8 @@ <project version="4"> <component name="ChangeListManager"> <list default="true" id="217ff8ac-6cfc-4d31-a1f3-01f35dc8d5e3" name="Default" comment=""> + <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" /> + <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" /> <change beforePath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" afterPath="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" /> </list> <ignored path="$PROJECT_DIR$/bin/" /> @@ -20,8 +22,8 @@ <file leaf-file-name="RobotMap.java" pinned="false" current-in-tab="false"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="510"> - <caret line="46" column="76" lean-forward="false" selection-start-line="46" selection-start-column="76" selection-end-line="46" selection-end-column="76" /> + <state relative-caret-position="631"> + <caret line="50" column="4" lean-forward="false" selection-start-line="50" selection-start-column="4" selection-end-line="50" selection-end-column="4" /> <folding /> </state> </provider> @@ -30,8 +32,8 @@ <file leaf-file-name="Robot.java" pinned="false" current-in-tab="false"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="410"> - <caret line="172" column="93" lean-forward="false" selection-start-line="172" selection-start-column="93" selection-end-line="172" selection-end-column="93" /> + <state relative-caret-position="360"> + <caret line="126" column="52" lean-forward="false" selection-start-line="126" selection-start-column="52" selection-end-line="126" selection-end-column="52" /> <folding /> </state> </provider> @@ -40,8 +42,8 @@ <file leaf-file-name="OI.java" pinned="false" current-in-tab="false"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="52"> - <caret line="31" column="31" lean-forward="false" selection-start-line="31" selection-start-column="31" selection-end-line="31" selection-end-column="31" /> + <state relative-caret-position="781"> + <caret line="62" column="1" lean-forward="false" selection-start-line="62" selection-start-column="1" selection-end-line="62" selection-end-column="1" /> <folding> <element signature="imports" expanded="true" /> </folding> @@ -49,14 +51,24 @@ </provider> </entry> </file> - <file leaf-file-name="Elevator.java" pinned="false" current-in-tab="true"> + <file leaf-file-name="Elevator.java" pinned="false" current-in-tab="false"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="485"> - <caret line="199" column="27" lean-forward="true" selection-start-line="199" selection-start-column="27" selection-end-line="199" selection-end-column="27" /> + <state relative-caret-position="284"> + <caret line="183" column="48" lean-forward="false" selection-start-line="183" selection-start-column="48" selection-end-line="183" selection-end-column="48" /> <folding> - <element signature="e#5346#5347#0" expanded="true" /> - <element signature="e#5411#5412#0" expanded="true" /> + <element signature="e#5035#5036#0" expanded="true" /> + <element signature="e#5055#5056#0" expanded="true" /> + <element signature="e#5086#5087#0" expanded="true" /> + <element signature="e#5107#5108#0" expanded="true" /> + <element signature="e#5147#5148#0" expanded="true" /> + <element signature="e#5179#5180#0" expanded="true" /> + <element signature="e#5469#5470#0" expanded="true" /> + <element signature="e#5525#5526#0" expanded="true" /> + <element signature="e#6251#6252#0" expanded="true" /> + <element signature="e#6305#6306#0" expanded="true" /> + <element signature="e#6342#6343#0" expanded="true" /> + <element signature="e#6396#6397#0" expanded="true" /> </folding> </state> </provider> @@ -72,14 +84,14 @@ </provider> </entry> </file> - <file leaf-file-name="ElevatorCommand.java" pinned="false" current-in-tab="false"> + <file leaf-file-name="ElevatorCommand.java" pinned="false" current-in-tab="true"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="639"> - <caret line="70" column="26" lean-forward="false" selection-start-line="70" selection-start-column="26" selection-end-line="70" selection-end-column="26" /> + <state relative-caret-position="400"> + <caret line="37" column="81" lean-forward="false" selection-start-line="37" selection-start-column="81" selection-end-line="37" selection-end-column="81" /> <folding> - <element signature="e#2272#2273#0" expanded="true" /> - <element signature="e#2291#2292#0" expanded="true" /> + <element signature="e#2542#2543#0" expanded="true" /> + <element signature="e#2561#2562#0" expanded="true" /> </folding> </state> </provider> @@ -98,11 +110,11 @@ <file leaf-file-name="Gamepad.java" pinned="false" current-in-tab="false"> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="175"> - <caret line="7" column="13" lean-forward="false" selection-start-line="7" selection-start-column="13" selection-end-line="7" selection-end-column="13" /> + <state relative-caret-position="277"> + <caret line="21" column="21" lean-forward="false" selection-start-line="21" selection-start-column="21" selection-end-line="21" selection-end-column="21" /> <folding> - <marker date="1519167965201" expanded="true" signature="1946:2296" ph="{...}" /> - <marker date="1519167965201" expanded="true" signature="2367:2838" ph="{...}" /> + <element signature="e#471#472#0" expanded="true" /> + <element signature="e#507#508#0" expanded="true" /> </folding> </state> </provider> @@ -123,6 +135,9 @@ <find>...</find> <find>LEVER</find> <find>Point</find> + <find>enable</find> + <find>setPo</find> + <find>system.out.prin</find> </findStrings> <replaceStrings> <replace>null</replace> @@ -181,11 +196,11 @@ <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java" /> <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java" /> <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java" /> - <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java" /> <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Config.java" /> - <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" /> - <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" /> + <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java" /> <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java" /> + <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java" /> + <option value="$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java" /> </list> </option> </component> @@ -198,9 +213,9 @@ <component name="MavenRunner"> <option name="runMavenInBackground" value="false" /> </component> - <component name="ProjectFrameBounds" extendedState="7"> - <option name="x" value="291" /> - <option name="y" value="-11" /> + <component name="ProjectFrameBounds" extendedState="6"> + <option name="x" value="260" /> + <option name="y" value="40" /> <option name="width" value="1400" /> <option name="height" value="1000" /> </component> @@ -591,7 +606,7 @@ <servers /> </component> <component name="TimeTrackingManager"> - <option name="totallyTimeSpent" value="13347000" /> + <option name="totallyTimeSpent" value="16347000" /> </component> <component name="ToolWindowManager"> <frame x="-11" y="-11" width="1942" height="1102" extended-state="6" /> @@ -611,6 +626,7 @@ <window_info id="Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="4" side_tool="false" content_ui="tabs" /> <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.2308108" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" /> <window_info id="Database" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="false" content_ui="tabs" /> + <window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.3293348" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" /> <window_info id="Structure" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" /> <window_info id="Ant Build" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" /> <window_info id="UI Designer" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="5" side_tool="false" content_ui="tabs" /> @@ -644,7 +660,6 @@ <window_info id="Terminal" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="9" side_tool="false" content_ui="tabs" /> <window_info id="Run with VisualVM" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="10" side_tool="false" content_ui="tabs" /> <window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.16770834" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" /> - <window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.3293348" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" /> <window_info id="Theme Preview" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" /> <window_info id="Favorites" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="true" content_ui="tabs" /> <window_info id="Debug with VisualVM" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="11" side_tool="false" content_ui="tabs" /> @@ -799,6 +814,11 @@ <provider selected="true" editor-type-id="text-editor"> <state relative-caret-position="378"> <caret line="18" column="13" lean-forward="false" selection-start-line="18" selection-start-column="13" selection-end-line="18" selection-end-column="13" /> + <folding> + <element signature="imports" expanded="false" /> + <element signature="e#455#456#0" expanded="false" /> + <element signature="e#503#504#0" expanded="false" /> + </folding> </state> </provider> </entry> @@ -900,29 +920,18 @@ </state> </provider> </entry> - <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java"> - <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="175"> - <caret line="7" column="13" lean-forward="false" selection-start-line="7" selection-start-column="13" selection-end-line="7" selection-end-column="13" /> - <folding> - <marker date="1519167965201" expanded="true" signature="1946:2296" ph="{...}" /> - <marker date="1519167965201" expanded="true" signature="2367:2838" ph="{...}" /> - </folding> - </state> - </provider> - </entry> - <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java"> + <entry file="file://$PROJECT_DIR$/build.xml"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="510"> - <caret line="46" column="76" lean-forward="false" selection-start-line="46" selection-start-column="76" selection-end-line="46" selection-end-column="76" /> + <state relative-caret-position="300"> + <caret line="12" column="28" lean-forward="false" selection-start-line="12" selection-start-column="28" selection-end-line="12" selection-end-column="28" /> <folding /> </state> </provider> </entry> - <entry file="file://$PROJECT_DIR$/build.xml"> + <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/RobotMap.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="300"> - <caret line="12" column="28" lean-forward="false" selection-start-line="12" selection-start-column="28" selection-end-line="12" selection-end-column="28" /> + <state relative-caret-position="631"> + <caret line="50" column="4" lean-forward="false" selection-start-line="50" selection-start-column="4" selection-end-line="50" selection-end-column="4" /> <folding /> </state> </provider> @@ -937,40 +946,61 @@ </entry> <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/OI.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="52"> - <caret line="31" column="31" lean-forward="false" selection-start-line="31" selection-start-column="31" selection-end-line="31" selection-end-column="31" /> + <state relative-caret-position="781"> + <caret line="62" column="1" lean-forward="false" selection-start-line="62" selection-start-column="1" selection-end-line="62" selection-end-column="1" /> <folding> <element signature="imports" expanded="true" /> </folding> </state> </provider> </entry> - <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java"> + <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Gamepad.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="410"> - <caret line="172" column="93" lean-forward="false" selection-start-line="172" selection-start-column="93" selection-end-line="172" selection-end-column="93" /> - <folding /> + <state relative-caret-position="277"> + <caret line="21" column="21" lean-forward="false" selection-start-line="21" selection-start-column="21" selection-end-line="21" selection-end-column="21" /> + <folding> + <element signature="e#471#472#0" expanded="true" /> + <element signature="e#507#508#0" expanded="true" /> + </folding> </state> </provider> </entry> - <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java"> + <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="639"> - <caret line="70" column="26" lean-forward="false" selection-start-line="70" selection-start-column="26" selection-end-line="70" selection-end-column="26" /> + <state relative-caret-position="284"> + <caret line="183" column="48" lean-forward="false" selection-start-line="183" selection-start-column="48" selection-end-line="183" selection-end-column="48" /> <folding> - <element signature="e#2272#2273#0" expanded="true" /> - <element signature="e#2291#2292#0" expanded="true" /> + <element signature="e#5035#5036#0" expanded="true" /> + <element signature="e#5055#5056#0" expanded="true" /> + <element signature="e#5086#5087#0" expanded="true" /> + <element signature="e#5107#5108#0" expanded="true" /> + <element signature="e#5147#5148#0" expanded="true" /> + <element signature="e#5179#5180#0" expanded="true" /> + <element signature="e#5469#5470#0" expanded="true" /> + <element signature="e#5525#5526#0" expanded="true" /> + <element signature="e#6251#6252#0" expanded="true" /> + <element signature="e#6305#6306#0" expanded="true" /> + <element signature="e#6342#6343#0" expanded="true" /> + <element signature="e#6396#6397#0" expanded="true" /> </folding> </state> </provider> </entry> - <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java"> + <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/Robot.java"> + <provider selected="true" editor-type-id="text-editor"> + <state relative-caret-position="360"> + <caret line="126" column="52" lean-forward="false" selection-start-line="126" selection-start-column="52" selection-end-line="126" selection-end-column="52" /> + <folding /> + </state> + </provider> + </entry> + <entry file="file://$PROJECT_DIR$/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java"> <provider selected="true" editor-type-id="text-editor"> - <state relative-caret-position="485"> - <caret line="199" column="27" lean-forward="true" selection-start-line="199" selection-start-column="27" selection-end-line="199" selection-end-column="27" /> + <state relative-caret-position="400"> + <caret line="37" column="81" lean-forward="false" selection-start-line="37" selection-start-column="81" selection-end-line="37" selection-end-column="81" /> <folding> - <element signature="e#5346#5347#0" expanded="true" /> - <element signature="e#5411#5412#0" expanded="true" /> + <element signature="e#2542#2543#0" expanded="true" /> + <element signature="e#2561#2562#0" expanded="true" /> </folding> </state> </provider> diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java index 1baf05b..f44a058 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java @@ -129,6 +129,7 @@ public void teleopInit() { if (autonCommands != null) autonCommands.cancel(); + elevator.enableControl(); drivetrain.shiftUp(); // start in high gear drivetrain.reset(); } @@ -171,6 +172,7 @@ private void updateSmartDashboard() { SmartDashboard.putBoolean("Elevator isZeroing", elevator.isZeroing()); SmartDashboard.putBoolean("Elevator isZeroed", elevator.isZeroed()); SmartDashboard.putString("Elevator power mode", elevatorMotor.getControlMode().name()); + SmartDashboard.putBoolean("Elevator elevator mode", elevator.isMotionControlled()); } /** diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java index bd765be..cd1530e 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java @@ -31,8 +31,11 @@ protected void initialize() { @Override protected void execute() { +// System.out.println(elevator.isMotionControlled()); if (!elevator.isMotionControlled()) { - if (Math.abs(gamepad.getLeftY()) > TOLERANCE) { +// System.out.println("Joystick input receiving"); +// System.out.println("Abs get Y " + Math.abs(gamepad.getLeftY())); + if (Math.abs(gamepad.getLeftY()) /*TODO look at why getLeftY return 0*/ > TOLERANCE) { elevator.setPower(-gamepad.getLeftY()); } else if (Math.abs(gamepad.getRightY()) > TOLERANCE) { elevator.setPower(-gamepad.getRightY()); @@ -41,6 +44,8 @@ protected void execute() { } } else { +// System.out.println("Getting button inputs"); + if (elevatorNudgeUp.get() && !elevator.atTopPosition()) { elevator.nudge(NUDGE_DISTANCE); } @@ -59,7 +64,7 @@ protected void execute() { } if (elevatorToggleControl.get()) { - System.out.println("screeeee toggle pressed!"); +// System.out.println("screeeee toggle pressed!"); if (elevator.isMotionControlled()) { elevator.disableControl(); } else { diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java index cd1074d..b6ccf25 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java @@ -1,5 +1,18 @@ package org.usfirst.frc.team2974.robot.subsystems; +import static org.usfirst.frc.team2974.robot.Config.Elevator.ACCELERATION; +import static org.usfirst.frc.team2974.robot.Config.Elevator.CRUISE_SPEED; +import static org.usfirst.frc.team2974.robot.Config.Elevator.INCHES_TO_NU; +import static org.usfirst.frc.team2974.robot.Config.Elevator.KD; +import static org.usfirst.frc.team2974.robot.Config.Elevator.KF; +import static org.usfirst.frc.team2974.robot.Config.Elevator.KI; +import static org.usfirst.frc.team2974.robot.Config.Elevator.KP; +import static org.usfirst.frc.team2974.robot.Config.Elevator.MAXIMUM_HEIGHT; +import static org.usfirst.frc.team2974.robot.Config.Elevator.MINUMUM_HEIGHT; +import static org.usfirst.frc.team2974.robot.Config.Elevator.TIMEOUT; +import static org.usfirst.frc.team2974.robot.RobotMap.elevatorLimitLower; +import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -10,197 +23,204 @@ import org.usfirst.frc.team2974.robot.command.teleop.ElevatorCommand; import org.usfirst.frc.team2974.robot.util.ElevatorLogger; -import static org.usfirst.frc.team2974.robot.Config.Elevator.*; -import static org.usfirst.frc.team2974.robot.RobotMap.elevatorLimitLower; -import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor; - /** - * The elevator subsystem, which raises and lowers the intake/outtake - * <p> - * TODO: finish me + * The elevator subsystem, which raises and lowers the intake/outtake <p> TODO: finish me */ public class Elevator extends Subsystem { - private ElevatorLogger logger; - private boolean isMotionControlled; - - private double power; - - private boolean zeroing; - private boolean zeroed; - private Timer timer = new Timer(); + private ElevatorLogger logger; + private boolean isMotionControlled; - public Elevator(ElevatorLogger logger) { - initConstants(); - this.logger = logger; - } + private double power; - public double getError() { - if (isMotionControlled) - return elevatorMotor.getClosedLoopError(0); - else - return 0; - } + private boolean zeroing; + private boolean zeroed; + private Timer timer = new Timer(); - @Override - protected void initDefaultCommand() { - setDefaultCommand(new ElevatorCommand()); - } + public Elevator(ElevatorLogger logger) { + zeroing = true; + initConstants(); + this.logger = logger; + } + + public double getError() { + if (isMotionControlled) { + return elevatorMotor.getClosedLoopError(0); + } else { + return 0; + } + } + + @Override + protected void initDefaultCommand() { + setDefaultCommand(new ElevatorCommand()); + } + + @Override + public void periodic() { + logger.addMotionData( + new ElevatorLogger.ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(), + getCurrentPositionNU(), power)); - @Override - public void periodic() { - logger.addMotionData(new ElevatorLogger.ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(), getCurrentPositionNU(), power)); - - if (zeroing) - setPower(-0.1); - if (!elevatorLimitLower.get() || timer.hasPeriodPassed(2.0 /*TODO tune time to be as small as possible*/)) { - zeroing = false; - timer.stop(); - zeroEncoder(); - enableControl(); - } - } - - public void initConstants() { - elevatorMotor.setNeutralMode(NeutralMode.Brake); - elevatorMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, TIMEOUT); - elevatorMotor.setSensorPhase(true); - elevatorMotor.setInverted(false); - - elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT); - elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, TIMEOUT); - - elevatorMotor.configNominalOutputForward(0, TIMEOUT); - elevatorMotor.configNominalOutputReverse(0, TIMEOUT); - elevatorMotor.configPeakOutputForward(1, TIMEOUT); - elevatorMotor.configPeakOutputReverse(-1, TIMEOUT); - - elevatorMotor.selectProfileSlot(0, 0); - - elevatorMotor.config_kP(0, KP, TIMEOUT); - elevatorMotor.config_kI(0, KI, TIMEOUT); - elevatorMotor.config_kD(0, KD, TIMEOUT); - elevatorMotor.config_kF(0, KF, TIMEOUT); - - elevatorMotor.configMotionCruiseVelocity(CRUISE_SPEED, TIMEOUT); - elevatorMotor.configMotionAcceleration(ACCELERATION, TIMEOUT); - - zeroed = false; - - elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); - elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); - - elevatorMotor.configForwardSoftLimitEnable(true, 10); - elevatorMotor.configReverseSoftLimitEnable(true, 10); - - /* pass false to FORCE OFF the feature. Otherwise the enable flags above are honored */ - elevatorMotor.overrideLimitSwitchesEnable(true); - - - } - - public void zeroEncoder() { - elevatorMotor.setSelectedSensorPosition(0, 0, TIMEOUT); // TODO: figure out later - - /* +14 rotations forward when using CTRE Mag encoder */ - elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); // TODO: FIX - /* -15 rotations reverse when using CTRE Mag encoder */ - elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); // TODO: FIX - - elevatorMotor.configForwardSoftLimitEnable(true, 10); - elevatorMotor.configReverseSoftLimitEnable(true, 10); - - /* pass false to FORCE OFF the feature. Otherwise the enable flags above are honored */ - elevatorMotor.overrideLimitSwitchesEnable(true); - - zeroed = true; - } - - public void nudge(double distance) { - if (isMotionControlled && zeroed) { - setTarget(getCurrentPosition() + distance); - } - } - - public void enableControl() { - if (zeroed) { - isMotionControlled = true; - elevatorMotor.set(ControlMode.MotionMagic, elevatorMotor.getSelectedSensorPosition(0)); - } - } - - public void disableControl() { - isMotionControlled = false; - elevatorMotor.set(ControlMode.Disabled, 0); - } - - public boolean isZeroed() { - return zeroed; - } - - public boolean isZeroing() { - return zeroing; - } - - public boolean isMotionControlled() { - return isMotionControlled; - } - - /** - * Gets the current position of the elevator in inches. - * - * @return - */ - public double getCurrentPosition() { - return elevatorMotor.getSelectedSensorPosition(0) / INCHES_TO_NU; - } - - /** - * Gets the current position of the elevator in native units - * - * @return - */ - public double getCurrentPositionNU() { - return elevatorMotor.getSelectedSensorPosition(0); - } - - /** - * Sets the target position from the zero point. - * - * @param inches the position is in inches, duh - */ - public void setTarget(double inches) { - System.out.println("Target: " + inches); - - if (zeroed) - elevatorMotor.set( - ControlMode.MotionMagic, - Math.min(MAXIMUM_HEIGHT, Math.max(MINUMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */ - ); - } - - public void setPower(double percent) { - percent = Math.min(.75, Math.max(-.75, percent)); // throttle power in - - power = percent; - SmartDashboard.putNumber("Elevator Power", percent); - elevatorMotor.set(ControlMode.PercentOutput, percent); - } - - public boolean atTopPosition() { - return getCurrentPositionNU() >= MAXIMUM_HEIGHT; - } - - public boolean atLowerPosition() { - return getCurrentPositionNU() <= MINUMUM_HEIGHT; - } - - public void startZero() { - if (!zeroed) { - zeroing = true; + if (zeroing) { + setPower(-0.2); + + if (!elevatorLimitLower.get() || timer + .hasPeriodPassed(2.0 /*TODO tune time to be as small as possible*/)) { + zeroing = false; + timer.stop(); + enableControl(); + zeroEncoder(); + } + } + } + + public void initConstants() { + elevatorMotor.setNeutralMode(NeutralMode.Brake); + elevatorMotor + .configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, TIMEOUT); + elevatorMotor.setSensorPhase(true); + elevatorMotor.setInverted(false); + + elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT); + elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, TIMEOUT); + + elevatorMotor.configNominalOutputForward(0, TIMEOUT); + elevatorMotor.configNominalOutputReverse(0, TIMEOUT); + elevatorMotor.configPeakOutputForward(1, TIMEOUT); + elevatorMotor.configPeakOutputReverse(-1, TIMEOUT); + + elevatorMotor.selectProfileSlot(0, 0); + + elevatorMotor.config_kP(0, KP, TIMEOUT); + elevatorMotor.config_kI(0, KI, TIMEOUT); + elevatorMotor.config_kD(0, KD, TIMEOUT); + elevatorMotor.config_kF(0, KF, TIMEOUT); + + elevatorMotor.configMotionCruiseVelocity(CRUISE_SPEED, TIMEOUT); + elevatorMotor.configMotionAcceleration(ACCELERATION, TIMEOUT); + + zeroed = false; + + elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); + elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); + + elevatorMotor.configForwardSoftLimitEnable(true, 10); + elevatorMotor.configReverseSoftLimitEnable(false, 10); + + /* pass false to FORCE OFF the feature. Otherwise the enable flags above are honored */ + elevatorMotor.overrideLimitSwitchesEnable(true); + + + } + + public void zeroEncoder() { + elevatorMotor.setSelectedSensorPosition(0, 0, TIMEOUT); // TODO: figure out later + + /* +14 rotations forward when using CTRE Mag encoder */ + elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); // TODO: FIX + /* -15 rotations reverse when using CTRE Mag encoder */ + elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); // TODO: FIX + + elevatorMotor.configForwardSoftLimitEnable(true, 10); + elevatorMotor.configReverseSoftLimitEnable(true, 10); + + /* pass false to FORCE OFF the feature. Otherwise the enable flags above are honored */ + elevatorMotor.overrideLimitSwitchesEnable(true); + + zeroed = true; + } + + public void nudge(double distance) { + if (isMotionControlled && zeroed) { + setTarget(getCurrentPosition() + distance); + } + } + + public void enableControl() { + if (zeroed) { + isMotionControlled = true; + elevatorMotor.set(ControlMode.MotionMagic, elevatorMotor.getSelectedSensorPosition(0)); + } + } + + public void disableControl() { + isMotionControlled = false; + elevatorMotor.set(ControlMode.Disabled, 0); + } + + public boolean isZeroed() { + return zeroed; + } + + public boolean isZeroing() { + return zeroing; + } + + public boolean isMotionControlled() { + return isMotionControlled; + } + + /** + * Gets the current position of the elevator in inches. + */ + public double getCurrentPosition() { + return elevatorMotor.getSelectedSensorPosition(0) / INCHES_TO_NU; + } + + /** + * Gets the current position of the elevator in native units + */ + public double getCurrentPositionNU() { + return elevatorMotor.getSelectedSensorPosition(0); + } + + /** + * Sets the target position from the zero point. + * + * @param inches the position is in inches, duh + */ + public void setTarget(double inches) { +// System.out.println("Target: " + inches); + + if (zeroed) { + elevatorMotor.set( + ControlMode.MotionMagic, + Math.min(MAXIMUM_HEIGHT, + Math.max(MINUMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */ + ); + } + } + + public void setPower(double percent) { + percent = Math.min(.75, Math.max(-.75, percent)); // throttle power in + + power = percent; + SmartDashboard.putNumber("Elevator Power", percent); + +// System.out.println("Setting Power " + percent); + elevatorMotor.set(ControlMode.PercentOutput, percent); + } + + public boolean atTopPosition() { + return getCurrentPositionNU() >= MAXIMUM_HEIGHT; + } + + public boolean atLowerPosition() { + return getCurrentPositionNU() <= MINUMUM_HEIGHT; + } + + public void startZero() { + if (!zeroed) { + zeroing = true; + zeroed = false; + elevatorMotor.configReverseSoftLimitEnable(false, 10); // zeroEncoder(); - timer.start(); - } - } + disableControl(); + +// System.out.println("Heloo "); + timer.start(); + } + } }