From 3b0abff02f2a75e508622c6ad04cabf00717a864 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 10:05:16 -0600 Subject: [PATCH 1/6] Changed the pivot encoder to be quadrature --- config/Subsystems.yaml | 7 +--- .../levitator/subsystems/Pivot.java | 37 +++++++++++++++---- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/config/Subsystems.yaml b/config/Subsystems.yaml index 1f70a0a0..fb0c46a5 100644 --- a/config/Subsystems.yaml +++ b/config/Subsystems.yaml @@ -111,11 +111,8 @@ lift: homingPower: -0.2 pivot: pivotMotor: { type: VICTOR_SPX, config: { id: 5, inverted: false } } - pivotEncoder: { - channel: 0, fullRange: 360.0, minValue: -180, continuous: true, - averageBits: 4, oversampleBits: 4, - offset: 104 - } + pivotEncoder: { aChannel: 6, bChannel: 7, reverse: false, + distancePerPulse: 0.087890625 } # 360 degrees / 4096 CPR pivotLockSolenoid: { channel: 1 } pivotLockSensor: { channel: 16, type: PNP } diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java index ab193e69..bfaef81f 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java @@ -1,5 +1,6 @@ package org.teamtators.levitator.subsystems; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.SpeedController; @@ -26,7 +27,7 @@ public class Pivot extends Subsystem implements Configurable { private SpeedController pivotMotor; private MotorPowerUpdater pivotMotorUpdater; private AbstractUpdatable pivotUpdatable; - private AnalogPotentiometer pivotEncoder; + private Encoder pivotEncoder; private Solenoid pivotLockSolenoid; private DigitalSensor pivotLockSensor; @@ -34,6 +35,7 @@ public class Pivot extends Subsystem implements Configurable { private InputDerivative pivotVelocity; private BooleanSampler locked = new BooleanSampler(this::isPivotLockedRaw); + private boolean homed = false; private double desiredPivotAngle; private double targetAngle; private double lastAttemptedAngle; @@ -66,8 +68,16 @@ public void setLift(Lift lift) { this.lift = lift; } + public boolean isHomed() { + return homed; + } + public double getCurrentPivotAngle() { - return pivotEncoder.get(); + return pivotEncoder.getDistance(); + } + + private void resetPivotAngle() { + pivotEncoder.reset(); } public double getDesiredPivotAngle() { @@ -75,7 +85,6 @@ public double getDesiredPivotAngle() { } public void setDesiredPivotAngle(double desiredAngle, boolean force) { -// if (getSafePivotAngle(desiredAngle) == desiredAngle) { if (rotationForced && !force) { return; } @@ -86,9 +95,6 @@ public void setDesiredPivotAngle(double desiredAngle, boolean force) { this.desiredPivotAngle = desiredAngle; this.rotationForced = force; } -// } else { -// logger.warn("Rotation to desired angle {} is not allowed at the current height {}!!", desiredAngle, getCurrentHeight()); -// } } public void setDesiredAnglePreset(AnglePreset desiredPivotAngle) { @@ -244,7 +250,7 @@ public void onEnterRobotState(RobotState state) { public ManualTestGroup createManualTests() { ManualTestGroup tests = super.createManualTests(); tests.addTest(new SpeedControllerTest("pivotMotor", pivotMotor)); - tests.addTest(new AnalogPotentiometerTest("pivotEncoder", pivotEncoder)); + tests.addTest(new EncoderTest("pivotEncoder", pivotEncoder)); tests.addTest(new SolenoidTest("pivotLockSolenoid", pivotLockSolenoid)); tests.addTest(new DigitalSensorTest("pivotLockSensor", pivotLockSensor)); @@ -274,6 +280,8 @@ public void configure(Config config) { pivotLockSensor.setName("Pivot", "pivotLockSensor"); pivotMotorUpdater = new MotorPowerUpdater(pivotMotor); + + homed = false; } @Override @@ -289,7 +297,7 @@ public void deconfigure() { @SuppressWarnings("WeakerAccess") public static class Config { public SpeedControllerConfig pivotMotor; - public AnalogPotentiometerConfig pivotEncoder; + public EncoderConfig pivotEncoder; public SolenoidConfig pivotLockSolenoid; public DigitalSensorConfig pivotLockSensor; @@ -381,6 +389,19 @@ public synchronized void start() { @Override public void doUpdate(double delta) { Pivot pivot = Pivot.this; + if (!pivot.isHomed()) { + pivot.disablePivotController(); + pivot.setPivotPower(0.0); + pivot.setPivotLockSolenoid(true); + if (pivot.isPivotLocked()) { + pivot.resetPivotAngle(); + logger.info("Pivot homed"); + pivot.setDesiredAnglePreset(AnglePreset.CENTER); + pivot.homed = true; + } else { + return; + } + } double centerAngle = pivot.getAnglePreset(AnglePreset.CENTER); double currentAngle = pivot.getCurrentPivotAngle(); double safePivotAngle = pivot.getSafePivotAngle(desiredPivotAngle); From bf4d5ce4e86cde61d877e9d19c38ba341735dc5a Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 10:07:18 -0600 Subject: [PATCH 2/6] Chaned the pivot to use a trapezoidal motion follower --- config/Subsystems.yaml | 34 ++++++++----------- .../levitator/subsystems/Pivot.java | 23 ++++++------- 2 files changed, 25 insertions(+), 32 deletions(-) diff --git a/config/Subsystems.yaml b/config/Subsystems.yaml index fb0c46a5..724e9785 100644 --- a/config/Subsystems.yaml +++ b/config/Subsystems.yaml @@ -117,26 +117,20 @@ pivot: pivotLockSensor: { channel: 16, type: PNP } pivotController: -# kpP: 0.04 # 0.04 -# kiP: 0.0 # 0.04 -# kpV: 0.001 -# kfV: 0.002 -# kMinOutput: 0.0537 -# kfA: 0.0005 -# kHoldPower: 0.0 -# minPosition: -90.0 -# maxPosition: 90.0 -# maxOutput: 0.4 -# minOutput: -0.4 -# maxAcceleration: 180.0 -# travelVelocity: 90.0 -# endVelocity: 0.0 - target: { within: 3.0 } - minSetpoint: -90.0 - maxSetpoint: 90.0 - maxAcceleration: 1.2 - movePower: 0.4 - endRamp: 20.0 + kpP: 0.04 # 0.04 + kiP: 0.0 # 0.04 + kpV: 0.001 + kfV: 0.002 + kMinOutput: 0.0537 + kfA: 0.0005 + kHoldPower: 0.0 + minPosition: -90.0 + maxPosition: 90.0 + maxOutput: 0.4 + minOutput: -0.4 + maxAcceleration: 180.0 + travelVelocity: 90.0 + endVelocity: 0.0 # logData: true pivotHoldPower: 0.15 anglePresets: diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java index bfaef81f..250d3251 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java @@ -31,7 +31,7 @@ public class Pivot extends Subsystem implements Configurable { private Solenoid pivotLockSolenoid; private DigitalSensor pivotLockSensor; - private /*TrapezoidalProfileFollower*/ StupidController pivotController; + private TrapezoidalProfileFollower pivotController; private InputDerivative pivotVelocity; private BooleanSampler locked = new BooleanSampler(this::isPivotLockedRaw); @@ -50,12 +50,11 @@ public Pivot() { super("Pivot"); pivotVelocity = new InputDerivative("pivotAngleDerivative", this::getCurrentPivotAngle); - pivotController = new /*TrapezoidalProfileFollower*/StupidController("pivotController"); -// pivotController.setPositionProvider(this::getCurrentPivotAngle); -// pivotController.setVelocityProvider(pivotVelocity); - pivotController.setInputProvider(this::getCurrentPivotAngle); + pivotController = new TrapezoidalProfileFollower("pivotController"); + pivotController.setPositionProvider(this::getCurrentPivotAngle); + pivotController.setVelocityProvider(pivotVelocity); pivotController.setOutputConsumer(this::setPivotPower); -// pivotController.setOnTargetPredicate(ControllerPredicates.alwaysFalse()); + pivotController.setOnTargetPredicate(ControllerPredicates.alwaysFalse()); pivotUpdatable = new PivotUpdatable(); } @@ -128,7 +127,7 @@ public void setTargetAngle(double angle) { logger.debug(String.format("Setting lift target angle to %.3f (degrees to move: %.3f)", angle, distance)); targetAngle = angle; - pivotController./*moveToPosition*/setSetpoint(angle); + pivotController.moveToPosition(angle); pivotController.setHoldPower(Math.signum(angle) * config.pivotHoldPower); } @@ -254,7 +253,7 @@ public ManualTestGroup createManualTests() { tests.addTest(new SolenoidTest("pivotLockSolenoid", pivotLockSolenoid)); tests.addTest(new DigitalSensorTest("pivotLockSensor", pivotLockSensor)); - tests.addTest(new /*MotionCalibrationTest*/ControllerTest(pivotController, 90.0)); + tests.addTest(new MotionCalibrationTest(pivotController)); tests.addTest(new PivotTest()); return tests; @@ -301,7 +300,7 @@ public static class Config { public SolenoidConfig pivotLockSolenoid; public DigitalSensorConfig pivotLockSensor; - public /*TrapezoidalProfileFollower*/ StupidController.Config pivotController; + public TrapezoidalProfileFollower.Config pivotController; public double pivotHoldPower; public Map anglePresets; @@ -344,10 +343,10 @@ public void start() { public void onButtonDown(LogitechF310.Button button) { switch (button) { case B: - double angle = (config.pivotController./*maxPosition*/maxSetpoint - config.pivotController./*minPosition*/minSetpoint) - * ((axisValue + 1) / 2) + config.pivotController./*minPosition*/minSetpoint; + double angle = (config.pivotController.maxPosition - config.pivotController.minPosition) + * ((axisValue + 1) / 2) + config.pivotController.minPosition; logger.info("Moving pivot to angle {}", angle); - pivotController./*moveToPosition*/setSetpoint(angle); + pivotController.moveToPosition(angle); break; case Y: enable(); From 70f229bda74412dc94773a582fd198630602ea46 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 10:15:38 -0600 Subject: [PATCH 3/6] Use quadrature encoder velocity for motion follower --- .../org/teamtators/levitator/subsystems/Pivot.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java index 250d3251..7b875736 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Pivot.java @@ -32,7 +32,6 @@ public class Pivot extends Subsystem implements Configurable { private DigitalSensor pivotLockSensor; private TrapezoidalProfileFollower pivotController; - private InputDerivative pivotVelocity; private BooleanSampler locked = new BooleanSampler(this::isPivotLockedRaw); private boolean homed = false; @@ -49,10 +48,9 @@ public class Pivot extends Subsystem implements Configurable { public Pivot() { super("Pivot"); - pivotVelocity = new InputDerivative("pivotAngleDerivative", this::getCurrentPivotAngle); pivotController = new TrapezoidalProfileFollower("pivotController"); pivotController.setPositionProvider(this::getCurrentPivotAngle); - pivotController.setVelocityProvider(pivotVelocity); + pivotController.setVelocityProvider(this::getCurrentPivotVelocity); pivotController.setOutputConsumer(this::setPivotPower); pivotController.setOnTargetPredicate(ControllerPredicates.alwaysFalse()); @@ -79,6 +77,10 @@ private void resetPivotAngle() { pivotEncoder.reset(); } + public double getCurrentPivotVelocity() { + return pivotEncoder.getRate(); + } + public double getDesiredPivotAngle() { return desiredPivotAngle; } @@ -170,7 +172,7 @@ public Updatable getPivotController() { } public List getUpdatables() { - return Arrays.asList(pivotVelocity, pivotUpdatable, pivotController); + return Arrays.asList(pivotUpdatable, pivotController); } public boolean isPivotLockedRaw() { From 30656cb8d5455ff4ca03da808f96cc025d24cb7c Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 12:22:14 -0600 Subject: [PATCH 4/6] Make climber motors coast when not enabled --- .../levitator/subsystems/Climber.java | 58 +++++++++++++------ 1 file changed, 39 insertions(+), 19 deletions(-) diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java index ed39bd55..185e1bab 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Climber.java @@ -1,5 +1,7 @@ package org.teamtators.levitator.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.Solenoid; @@ -28,9 +30,9 @@ public class Climber extends Subsystem implements Configurable { private final TatorRobot robot; - private SpeedControllerGroup climberMotor; + private SpeedControllerGroup climberMotors; private WPI_TalonSRX masterMotor; -// private MotorPowerUpdater climberMotorUpdater; + // private MotorPowerUpdater climberMotorUpdater; private DigitalSensor topLimit; private DigitalSensor bottomLimit; private Solenoid releaser; @@ -42,14 +44,14 @@ public Climber(TatorRobot robot) { this.robot = robot; } - public void setHomed(boolean homed) { - this.homed = homed; - } - public boolean isHomed() { return homed; } + public void setHomed(boolean homed) { + this.homed = homed; + } + public void setPower(double power, boolean force) { double pow = power; if (!force) { @@ -62,7 +64,15 @@ public void setPower(double power, boolean force) { pow = 0; } } - climberMotor.set(pow); + climberMotors.set(pow); + } + + public void setNeutralMode(NeutralMode neutralMode) { + for (SpeedController climberMotor : climberMotors.getSpeedControllers()) { + if (climberMotor instanceof BaseMotorController) { + ((BaseMotorController) climberMotor).setNeutralMode(neutralMode); + } + } } public double getPosition() { @@ -101,22 +111,32 @@ public void onEnterRobotState(RobotState state) { robot.getScheduler().startCommand(homeCommand); } } + switch (state) { + case DISABLED: + setNeutralMode(NeutralMode.Coast); + break; + default: + setNeutralMode(NeutralMode.Brake); + break; + } } @Override public void configure(Config config) { this.config = config; - climberMotor = config.climberMotor.create(); - masterMotor = (WPI_TalonSRX) climberMotor.getSpeedControllers()[0]; + climberMotors = config.climberMotor.create(); + masterMotor = (WPI_TalonSRX) climberMotors.getSpeedControllers()[0]; topLimit = config.topLimit.create(); bottomLimit = config.bottomLimit.create(); releaser = config.releaser.create(); -// climberMotorUpdater = new MotorPowerUpdater(climberMotor); +// climberMotorUpdater = new MotorPowerUpdater(climberMotors); + + setNeutralMode(NeutralMode.Coast); - climberMotor.setName("Climber", "climberMotor"); - for (int i = 0; i < climberMotor.getSpeedControllers().length; i++) { - SpeedController speedController = climberMotor.getSpeedControllers()[i]; - ((Sendable) speedController).setName("Climber", ("climberMotor(" + i + ")")); + climberMotors.setName("Climber", "climberMotors"); + for (int i = 0; i < climberMotors.getSpeedControllers().length; i++) { + SpeedController speedController = climberMotors.getSpeedControllers()[i]; + ((Sendable) speedController).setName("Climber", ("climberMotors(" + i + ")")); } topLimit.setName("Climber", "topLimit"); bottomLimit.setName("Climber", "bottomLimit"); @@ -127,7 +147,7 @@ public void configure(Config config) { @Override public void deconfigure() { - SpeedControllerConfig.free(climberMotor); + SpeedControllerConfig.free(climberMotors); topLimit.free(); bottomLimit.free(); releaser.free(); @@ -137,10 +157,10 @@ public void deconfigure() { public ManualTestGroup createManualTests() { ManualTestGroup group = super.createManualTests(); group.addTest(new ClimberEncoderTest()); - group.addTest(new SpeedControllerTest("climberMotor", climberMotor)); - for (int i = 0; i < climberMotor.getSpeedControllers().length; i++) { - SpeedController speedController = climberMotor.getSpeedControllers()[i]; - group.addTest(new SpeedControllerTest("climberMotor(" + i + ")", speedController)); + group.addTest(new SpeedControllerTest("climberMotors", climberMotors)); + for (int i = 0; i < climberMotors.getSpeedControllers().length; i++) { + SpeedController speedController = climberMotors.getSpeedControllers()[i]; + group.addTest(new SpeedControllerTest("climberMotors(" + i + ")", speedController)); } group.addTest(new DigitalSensorTest("topLimit", topLimit)); group.addTest(new DigitalSensorTest("bottomLimit", bottomLimit)); From e41c83be551fc232cb8acfe187acaa5a6722c476 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 12:33:05 -0600 Subject: [PATCH 5/6] Default to coast in drive --- .../org/teamtators/levitator/subsystems/Drive.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java index 9b75d9a6..25cf174a 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Drive.java @@ -1,5 +1,7 @@ package org.teamtators.levitator.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import edu.wpi.first.wpilibj.*; import org.teamtators.common.config.Configurable; import org.teamtators.common.config.helpers.EncoderConfig; @@ -204,6 +206,16 @@ public void setLeftPower(double power) { leftMotor.set(power); } + public void setNeutralMode(NeutralMode neutralMode) { + for (SpeedControllerGroup group : Arrays.asList(leftMotor, rightMotor)) { + for (SpeedController motor : group.getSpeedControllers()) { + if (motor instanceof BaseMotorController) { + ((BaseMotorController) motor).setNeutralMode(neutralMode); + } + } + } + } + public double getLeftRate() { return leftEncoder.getRate(); } @@ -353,6 +365,7 @@ public void configure(Config config) { rightEncoder.setName("Drive", "rightEncoder"); gyro.setName("Drive", "gyro"); + setNeutralMode(NeutralMode.Coast); poseEstimator.start(); } From fc69ca96965804c6d33b81e88efbaa527e568e5a Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 12 Apr 2018 12:35:01 -0600 Subject: [PATCH 6/6] Default to coast in lift, except after homed --- .../teamtators/levitator/subsystems/Lift.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java index 0e0a1afb..6910add7 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java @@ -1,6 +1,9 @@ package org.teamtators.levitator.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.teamtators.common.config.Configurable; import org.teamtators.common.config.helpers.*; @@ -20,6 +23,7 @@ import java.util.Map; public class Lift extends Subsystem implements Configurable { + private static final boolean ENABLE_HOMING = false; private Pivot pivot; private SpeedControllerGroup liftMotor; @@ -69,6 +73,14 @@ public void linkTo(Pivot pivot) { this.setPivot(pivot); } + public void setNeutralMode(NeutralMode neutralMode) { + for (SpeedController liftMotor : liftMotor.getSpeedControllers()) { + if (liftMotor instanceof BaseMotorController) { + ((BaseMotorController) liftMotor).setNeutralMode(neutralMode); + } + } + } + /** * @return height in inches */ @@ -347,6 +359,7 @@ public void configure(Config config) { homed = false; homeTimer.stop(); + setNeutralMode(NeutralMode.Coast); } @Override @@ -409,10 +422,11 @@ private void updateHeight() { return; } setLiftPower(config.homingPower); - if (!isAtBottomLimit() && false && !homeTimeout) { + if (!isAtBottomLimit() && ENABLE_HOMING && !homeTimeout) { return; } logger.info("Lift homed"); + setNeutralMode(NeutralMode.Brake); liftEncoder.reset(); enableLiftController(); homed = true;