Skip to content

Commit 0cdc737

Browse files
author
ryancave282
committed
overhauled elastic implementation; significant code cleanup
1 parent 456370c commit 0cdc737

9 files changed

Lines changed: 68 additions & 71 deletions

File tree

‎src/main/java/frc/robot/subsystems/Climb.java‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
import edu.wpi.first.math.trajectory.TrapezoidProfile;
66
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
77
import frc.robot.DroidRageConstants.Control;
8-
import frc.robot.commands.DisabledCommand;
98
import frc.utility.motor.CANMotorEx;
109
import frc.utility.motor.TalonEx;
1110
import frc.utility.motor.CANMotorEx.Direction;
@@ -59,8 +58,7 @@ public Climb(boolean isEnabled) {
5958
new ArmFeedforward(0, 0.11, 0.3,0.15), //ks: 0.14 kv:0.1
6059
new TrapezoidProfile.Constraints(0, 0),
6160
Constants.MAX_POSITION, Constants.MIN_POSITION, Constants.OFFSET,
62-
Control.FEEDFORWARD, "Climb", "Climb", 0);
63-
motor.setIsEnabled(isEnabled);
61+
Control.FEEDFORWARD, "Climb", "Climb", 0, isEnabled);
6462
SmartDashboard.putData(this.getName() + "/Reset Encoder",runOnce(this::resetEncoder));
6563
// ComplexWidgetBuilder.create(DisabledCommand.create(runOnce(this::resetEncoder)), "Reset Encoder", this.getName());
6664
setTargetPosition(90);

‎src/main/java/frc/robot/subsystems/Elevator.java‎

Lines changed: 1 addition & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,8 @@
33
import edu.wpi.first.math.controller.ElevatorFeedforward;
44
import edu.wpi.first.math.controller.PIDController;
55
import edu.wpi.first.math.trajectory.TrapezoidProfile;
6-
import edu.wpi.first.util.sendable.Sendable;
7-
import edu.wpi.first.util.sendable.SendableBuilder;
8-
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
9-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
106
import edu.wpi.first.wpilibj2.command.Command;
117
import frc.robot.DroidRageConstants.Control;
12-
import frc.robot.commands.DisabledCommand;
13-
import frc.utility.DashboardUtils;
14-
import frc.utility.DashboardUtils.Dashboard;
158
import frc.utility.motor.CANMotorEx;
169
import frc.utility.motor.TalonEx;
1710
import frc.utility.motor.CANMotorEx.Direction;
@@ -71,8 +64,6 @@ private ElevatorValue(double height) {
7164
.withIsEnabled(true)
7265
.withCurrentLimit(50);
7366

74-
private static TalonEx[] motors = {motorRight, motorLeft};
75-
7667
public Elevator(boolean isEnabled) {
7768
super(
7869
new CANMotorEx[]{motorRight, motorLeft},
@@ -81,10 +72,7 @@ public Elevator(boolean isEnabled) {
8172
new TrapezoidProfile.Constraints(.5, 0.5),
8273
Constants.MAX_POSITION,
8374
Constants.MIN_POSITION,
84-
Control.FEEDFORWARD, "Elevator", 0);
85-
for (TalonEx motor: motors) {
86-
motor.setIsEnabled(isEnabled);
87-
}
75+
Control.FEEDFORWARD, "Elevator", 0, isEnabled);
8876
}
8977

9078
@Override

‎src/main/java/frc/robot/subsystems/carriage/Arm.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,6 @@ public Arm(boolean isEnabled) {
4242
new TrapezoidProfile.Constraints(.1, .1),
4343
Constants.MAX_POSITION, Constants.MIN_POSITION,
4444
Constants.OFFSET,
45-
Control.FEEDFORWARD, Carriage.class.getSimpleName(),"Arm", 0, encoder);
46-
motor.setIsEnabled(isEnabled);
45+
Control.FEEDFORWARD, Carriage.class.getSimpleName(),"Arm", 0, encoder, isEnabled);
4746
}
4847
}

‎src/main/java/frc/robot/subsystems/carriage/Intake.java‎

Lines changed: 2 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -1,41 +1,20 @@
11
package frc.robot.subsystems.carriage;
22

3-
import java.util.function.Supplier;
4-
53
import edu.wpi.first.math.controller.PIDController;
64
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
75
import edu.wpi.first.math.trajectory.TrapezoidProfile;
8-
import edu.wpi.first.util.sendable.Sendable;
9-
import edu.wpi.first.util.sendable.SendableBuilder;
10-
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
11-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
126
import frc.robot.DroidRageConstants.Control;
137
import frc.utility.motor.CANMotorEx;
148
import frc.utility.motor.TalonEx;
159
import frc.utility.motor.CANMotorEx.Direction;
1610
import frc.utility.motor.CANMotorEx.ZeroPowerMode;
1711
import frc.utility.template.IntakeTemplate;
18-
import lombok.Getter;
19-
import lombok.Setter;
2012

2113
public class Intake extends IntakeTemplate {
2214
private static class Constants {
2315
public static final double MAX_SPEED = 800;
2416
public static final double MIN_SPEED = -800;
2517
}
26-
// private final ShuffleboardValue<Boolean> isElementInWriter =
27-
// ShuffleboardValue.create(false, "IsElement", Carriage.class.getSimpleName())
28-
// .withWidget(BuiltInWidgets.kBooleanBox)
29-
// .build();
30-
// private final ComplexWidgetBuilder builder = ComplexWidgetBuilder
31-
// .create("f", "test", Carriage.class.getSimpleName())
32-
// .withWidget(BuiltInWidgets.kTextView)
33-
// .withSize(2, 1)
34-
// .withPosition(0, 0)
35-
// .withIsEnabled(true)
36-
// .withDefaultValue("Intake");
37-
38-
// private final Supplier<Boolean> isElementIn = () -> (getTargetPosition() - getEncoderPosition() > 40);
3918

4019
private static TalonEx motor = TalonEx.create(31)
4120
.withDirection(Direction.Reversed)
@@ -53,25 +32,10 @@ public Intake(boolean isEnabled) {
5332
new SimpleMotorFeedforward(.0, .6, 0.3),
5433
new TrapezoidProfile.Constraints(0, 0),
5534
Constants.MAX_SPEED, Constants.MIN_SPEED,
56-
Control.FEEDFORWARD, Carriage.class.getSimpleName(), "Intake", 0);
57-
motor.setIsEnabled(isEnabled);
58-
//Change
59-
SmartDashboard.putData("iintakemotor", motor.getMotor());
60-
SmartDashboard.putData("Is Element In", isElementIn);
61-
// isElementIn = this::(getTargetPosition() - getEncoderPosition() > 40);
35+
Control.FEEDFORWARD, Carriage.class.getSimpleName(), "Intake", 0, isEnabled);
36+
6237
}
6338

64-
65-
66-
private final Sendable isElementIn = new Sendable() {
67-
@Override
68-
public void initSendable(SendableBuilder builder) {
69-
builder.setSmartDashboardType("Boolean Box");
70-
71-
builder.addBooleanProperty("Is Element In", () -> (getTargetPosition() - getEncoderPosition() > 40), null);
72-
}
73-
};
74-
7539
// public Command setPowerCommand(double power){
7640
// return new InstantCommand(()->motor.setPower(power));
7741
// }

‎src/main/java/frc/robot/subsystems/carriage/Pivot.java‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -43,8 +43,6 @@ public Pivot(boolean isEnabled) {
4343
//OLD PROGEJCT - 0.079284, 0.12603, 2.3793, 0.05276
4444
new TrapezoidProfile.Constraints(0, 0),
4545
Constants.MAX_POSITION, Constants.MIN_POSITION, Constants.OFFSET,
46-
Control.FEEDFORWARD, Carriage.class.getSimpleName(), "Pivot", 0, encoder);
47-
motor.setIsEnabled(isEnabled);
48-
46+
Control.FEEDFORWARD, Carriage.class.getSimpleName(), "Pivot", 0, encoder, isEnabled);
4947
}
5048
}

‎src/main/java/frc/utility/template/ArmAbsoluteTemplate.java‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,12 @@ public ArmAbsoluteTemplate(
2323
String tabName,
2424
String subsystemName,
2525
int mainNum,
26-
EncoderEx encoder
26+
EncoderEx encoder,
27+
boolean isEnabled
2728
){
2829
super(motors, controller, feedforward, constraints,
2930
maxPosition, minPosition, offset, control, tabName,
30-
subsystemName, mainNum);
31+
subsystemName, mainNum, isEnabled);
3132
this.encoder=encoder;
3233

3334
}
@@ -45,11 +46,12 @@ public ArmAbsoluteTemplate(
4546
String tabName,
4647
String subsystemName,
4748
int mainNum,
48-
EncoderEx encoder
49+
EncoderEx encoder,
50+
boolean isEnabled
4951
){
5052
super(motors, controller, feedforward, limitSwitch, constraints,
5153
maxPosition, minPosition, offset, control, tabName,
52-
subsystemName, mainNum);
54+
subsystemName, mainNum, isEnabled);
5355
this.encoder=encoder;
5456

5557
}

‎src/main/java/frc/utility/template/ArmTemplate.java‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,10 @@
1212
import edu.wpi.first.wpilibj2.command.InstantCommand;
1313
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1414
import frc.robot.DroidRageConstants.Control;
15+
import frc.utility.DashboardUtils.Dashboard;
1516
import frc.utility.motor.CANMotorEx;
1617

17-
public class ArmTemplate extends SubsystemBase {
18+
public class ArmTemplate extends SubsystemBase implements Dashboard {
1819
protected final CANMotorEx[] motors;
1920
protected final PIDController controller;
2021
protected final ArmFeedforward feedforward;
@@ -29,6 +30,7 @@ public class ArmTemplate extends SubsystemBase {
2930
protected final TrapezoidProfile profile;
3031
protected TrapezoidProfile.State current = new TrapezoidProfile.State(0,0); //initial
3132
protected TrapezoidProfile.State goal = new TrapezoidProfile.State(0,0);
33+
protected final String name;
3234

3335
public ArmTemplate(
3436
CANMotorEx[] motors,
@@ -41,7 +43,8 @@ public ArmTemplate(
4143
Control control,
4244
String tabName,
4345
String subsystemName,
44-
int mainNum
46+
int mainNum,
47+
boolean isEnabled
4548
){
4649
this.motors=motors;
4750
this.controller=controller;
@@ -51,6 +54,7 @@ public ArmTemplate(
5154
this.minPosition=minPosition;
5255
this.offset=offset;
5356
this.mainNum=mainNum;
57+
this.name=subsystemName;
5458

5559
profile = new TrapezoidProfile(constraints);
5660

@@ -72,7 +76,8 @@ public ArmTemplate(
7276
Control control,
7377
String tabName,
7478
String subsystemName,
75-
int mainNum
79+
int mainNum,
80+
boolean isEnabled
7681
){
7782
this.motors=motors;
7883
this.controller=controller;
@@ -83,13 +88,21 @@ public ArmTemplate(
8388
this.minPosition=minPosition;
8489
this.offset=offset;
8590
this.mainNum=mainNum;
91+
this.name=subsystemName;
92+
93+
for (CANMotorEx motor: motors) {
94+
motor.setIsEnabled(isEnabled);
95+
}
8696

8797
profile = new TrapezoidProfile(constraints);
8898

8999
positionRadian = () -> motors[mainNum].getPosition()+offset;
90100
targetRadian = controller::getSetpoint;
101+
}
91102

92-
SmartDashboard.putData(subsystemName, this);
103+
@Override
104+
public void elasticInit() {
105+
SmartDashboard.putData(name, this);
93106
}
94107

95108
@Override

‎src/main/java/frc/utility/template/ElevatorTemplate.java‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,8 @@ public ElevatorTemplate(
4949
double minPosition,
5050
Control control,
5151
String name,
52-
int mainNum
52+
int mainNum,
53+
boolean isEnabled
5354
){
5455
this.motors=motors;
5556
this.controller=controller;
@@ -61,6 +62,10 @@ public ElevatorTemplate(
6162

6263
profile = new TrapezoidProfile(constraints);
6364

65+
for (CANMotorEx motor: motors) {
66+
motor.setIsEnabled(isEnabled);
67+
}
68+
6469
DashboardUtils.register(this);
6570

6671
// controller.setTolerance(.3);
@@ -88,7 +93,8 @@ public ElevatorTemplate(
8893
double minPosition,
8994
Control control,
9095
String name,
91-
int mainNum
96+
int mainNum,
97+
boolean isEnabled
9298
){
9399
this.motors=motors;
94100
this.controller=controller;
@@ -99,6 +105,10 @@ public ElevatorTemplate(
99105
this.minPosition=minPosition;
100106
this.mainNum=mainNum;
101107

108+
for (CANMotorEx motor: motors) {
109+
motor.setIsEnabled(isEnabled);
110+
}
111+
102112
profile = new TrapezoidProfile(constraints);
103113
// controller.setTolerance(.3);
104114

‎src/main/java/frc/utility/template/IntakeTemplate.java‎

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,18 @@
33
import edu.wpi.first.math.controller.PIDController;
44
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
55
import edu.wpi.first.math.trajectory.TrapezoidProfile;
6+
import edu.wpi.first.util.sendable.Sendable;
67
import edu.wpi.first.util.sendable.SendableBuilder;
78
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
89
import edu.wpi.first.wpilibj2.command.Command;
910
import edu.wpi.first.wpilibj2.command.Commands;
1011
import edu.wpi.first.wpilibj2.command.InstantCommand;
1112
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1213
import frc.robot.DroidRageConstants.Control;
14+
import frc.utility.DashboardUtils.Dashboard;
1315
import frc.utility.motor.CANMotorEx;
1416

15-
public class IntakeTemplate extends SubsystemBase{
17+
public class IntakeTemplate extends SubsystemBase implements Dashboard {
1618
private final CANMotorEx[] motors;
1719
private final PIDController controller;
1820
private final SimpleMotorFeedforward feedforward;
@@ -21,6 +23,7 @@ public class IntakeTemplate extends SubsystemBase{
2123
private final double minSpeed;
2224
// private final ShuffleboardValue<Double> errorWriter;
2325
private final int mainNum;
26+
private final String name;
2427
private final TrapezoidProfile profile;
2528
private TrapezoidProfile.State current = new TrapezoidProfile.State(0,0); //initial
2629
private final TrapezoidProfile.State goal = new TrapezoidProfile.State(0,0);
@@ -36,7 +39,8 @@ public IntakeTemplate(
3639
Control control,
3740
String tabName,
3841
String name,
39-
int mainNum
42+
int mainNum,
43+
boolean isEnabled
4044
){
4145
this.motors=motors;
4246
this.controller=controller;
@@ -45,10 +49,21 @@ public IntakeTemplate(
4549
this.maxSpeed=maxSpeed;
4650
this.minSpeed=minSpeed;
4751
this.mainNum=mainNum;
52+
this.name=name;
53+
54+
for (CANMotorEx motor: motors) {
55+
motor.setIsEnabled(isEnabled);
56+
}
4857

4958
profile = new TrapezoidProfile(constraints);
5059

60+
}
61+
62+
@Override
63+
public void elasticInit() {
5164
SmartDashboard.putData(name, this);
65+
SmartDashboard.putData("Is Element In", isElementIn);
66+
5267
}
5368

5469
@Override
@@ -58,6 +73,16 @@ public void initSendable(SendableBuilder builder) {
5873
builder.addDoubleProperty("Applied Voltage", motors[mainNum]::getVoltage, null);
5974
}
6075

76+
// isElementIn = this::(getTargetPosition() - getEncoderPosition() > 40);
77+
78+
private final Sendable isElementIn = new Sendable() {
79+
@Override
80+
public void initSendable(SendableBuilder builder) {
81+
builder.setSmartDashboardType("Boolean Box");
82+
builder.addBooleanProperty("Is Element In", () -> (getTargetPosition() - getEncoderPosition() > 40), null);
83+
}
84+
};
85+
6186
@Override
6287
public void periodic() {
6388
switch(control){

0 commit comments

Comments
 (0)