From 7d58ff84a648162a9e7008d632d531908f862e33 Mon Sep 17 00:00:00 2001 From: NamelessSuperCoder Date: Tue, 20 Feb 2018 19:34:31 -0500 Subject: [PATCH] fixed elevator errors --- PowerUp/.idea/workspace.xml | 152 ++++--- .../org/usfirst/frc/team2974/robot/Robot.java | 2 + .../robot/command/teleop/ElevatorCommand.java | 9 +- .../team2974/robot/subsystems/Elevator.java | 386 +++++++++--------- 4 files changed, 303 insertions(+), 246 deletions(-) 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 @@ + + @@ -20,8 +22,8 @@ - - + + @@ -30,8 +32,8 @@ - - + + @@ -40,8 +42,8 @@ - - + + @@ -49,14 +51,24 @@ - + - - + + - - + + + + + + + + + + + + @@ -72,14 +84,14 @@ - + - - + + - - + + @@ -98,11 +110,11 @@ - - + + - - + + @@ -123,6 +135,9 @@ ... LEVER Point + enable + setPo + system.out.prin null @@ -181,11 +196,11 @@ @@ -198,9 +213,9 @@ - - - @@ -611,6 +626,7 @@ + @@ -644,7 +660,6 @@ - @@ -799,6 +814,11 @@ + + + + + @@ -900,29 +920,18 @@ - - - - - - - - - - - - + - - + + - + - - + + @@ -937,40 +946,61 @@ - - + + - + - - - + + + + + + - + - - + + - - + + + + + + + + + + + + - + + + + + + + + + - - + + - - + + 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 - *

- * TODO: finish me + * The elevator subsystem, which raises and lowers the intake/outtake

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(); + } + } }