Skip to content

Commit b0246cc

Browse files
worked on can bus utilization (#95)
* worked on can bus utilization * ran spotless
1 parent 90da5c3 commit b0246cc

File tree

7 files changed

+53
-31
lines changed

7 files changed

+53
-31
lines changed

src/main/java/frc/robot/climber/Climber.java

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,9 @@ public Climber() {
5252
.maxVelocity(ClimberConstants.kMaxVelocityRPM)
5353
.maxAcceleration(ClimberConstants.kMaxAccelerationRPMPerSecond);
5454

55+
motorConfig.signals
56+
.outputCurrentPeriodMs(100);
57+
5558
motorConfig.softLimit
5659
.reverseSoftLimit(0.0)
5760
.reverseSoftLimitEnabled(true);
@@ -72,10 +75,10 @@ public void periodic() {
7275

7376
SmartDashboard.putNumber("Climber/Servo", servo.get());
7477

75-
SmartDashboard.putNumber("Climber/Motor Current", motor.getOutputCurrent());
78+
// SmartDashboard.putNumber("Climber/Motor Current", motor.getOutputCurrent());
7679
SmartDashboard.putBoolean(
7780
"Climber/Deploy Command is Finished", createDeployCommand().isFinished());
78-
SmartDashboard.putNumber("Climber/Applied Duty Cycle", motor.getAppliedOutput());
81+
// SmartDashboard.putNumber("Climber/Applied Duty Cycle", motor.getAppliedOutput());
7982
}
8083

8184
public void resetEncoder() {

src/main/java/frc/robot/drivetrain/SwerveModule.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,9 @@ private SwerveModule(
9999

100100
m_turningMotorConfig.encoder
101101
.positionConversionFactor(ModuleConstants.kTurnPositionConversionFactor);
102+
103+
m_turningMotorConfig.signals
104+
.primaryEncoderVelocityPeriodMs(100);
102105
// spotless:on
103106

104107
m_driveMotor.configure(
@@ -116,6 +119,7 @@ private SwerveModule(
116119
m_turningAbsEncoderConfig = new CANcoderConfiguration();
117120
m_turningAbsEncoder.getConfigurator().refresh(m_turningAbsEncoderConfig);
118121
m_turningAbsEncoder.getAbsolutePosition().setUpdateFrequency(50, 0.5);
122+
m_turningAbsEncoder.optimizeBusUtilization();
119123
}
120124

121125
/**

src/main/java/frc/robot/elevator/AlgaeRoller.java

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,9 @@ public AlgaeRoller() {
4141
.velocityConversionFactor(AlgaeRollerConstants.kVelocityConversionFactor)
4242
.positionConversionFactor(AlgaeRollerConstants.kPositionConversionFactor);
4343

44+
config.signals
45+
.absoluteEncoderPositionPeriodMs(100);
46+
4447
followerConfig
4548
.apply(config)
4649
.follow(leaderMotor, true);
@@ -56,8 +59,8 @@ public AlgaeRoller() {
5659
@Override
5760
public void periodic() {
5861
SmartDashboard.putNumber("Algae Roller/Velocity", encoder.getVelocity());
59-
SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
60-
SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
62+
// SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
63+
// SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
6164
SmartDashboard.putBoolean("Algae Sensor", algaeSensor.get());
6265
}
6366

src/main/java/frc/robot/elevator/AlgaeWrist.java

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,10 @@ public AlgaeWrist() {
6060
.zeroOffset(AlgaeWristConstants.kZeroOffset.in(Rotations))
6161
.positionConversionFactor(AlgaeWristConstants.kPositionConversionFactor.in(Radians))
6262
.velocityConversionFactor(AlgaeWristConstants.kVelocityConversionFactor.in(RadiansPerSecond));
63+
64+
config.signals
65+
.primaryEncoderVelocityPeriodMs(100)
66+
.outputCurrentPeriodMs(100);
6367

6468
config.softLimit
6569
.reverseSoftLimit(AlgaeWristState.Min.angle.in(Radians))
@@ -81,15 +85,16 @@ public AlgaeWrist() {
8185
@Override
8286
public void periodic() {
8387
SmartDashboard.putNumber("Algae Wrist/Current Angle Degrees", getCurrentAngle().in(Degrees));
84-
SmartDashboard.putNumber("Algae Wrist/Current Angle Radians", encoder.getPosition());
85-
SmartDashboard.putNumber("Algae Wrist/Current Angular Velocity RPS", encoder.getVelocity());
88+
// SmartDashboard.putNumber("Algae Wrist/Current Angle Radians", encoder.getPosition());
89+
// SmartDashboard.putNumber("Algae Wrist/Current Angular Velocity RPS", encoder.getVelocity());
8690
SmartDashboard.putString("Algae Wrist/Target State", getTargetState().name());
8791
SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Degrees", getSetpointAngle().in(Degrees));
88-
SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
89-
SmartDashboard.putNumber(
90-
"Algae Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
91-
SmartDashboard.putNumber("Algae Wrist/Applied Duty Cycle", motor.getAppliedOutput());
92-
SmartDashboard.putNumber("Algae Wrist/Current", motor.getOutputCurrent());
92+
// SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Radians",
93+
// feedback.getSetpoint().position);
94+
// SmartDashboard.putNumber(
95+
// "Algae Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
96+
// SmartDashboard.putNumber("Algae Wrist/Applied Duty Cycle", motor.getAppliedOutput());
97+
// SmartDashboard.putNumber("Algae Wrist/Current", motor.getOutputCurrent());
9398
SmartDashboard.putBoolean("Algae Wrist/At Goal", feedback.atGoal());
9499
}
95100

src/main/java/frc/robot/elevator/CoralRoller.java

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,10 @@ public CoralRoller() {
3232
.smartCurrentLimit(RobotConstants.kDefaultNEO550CurretnLimit)
3333
.inverted(false);
3434

35+
config.signals
36+
.absoluteEncoderPositionPeriodMs(100)
37+
.outputCurrentPeriodMs(100);
38+
3539
config.encoder
3640
.velocityConversionFactor(CoralRollerConstants.kVelocityConversionFactor)
3741
.positionConversionFactor(CoralRollerConstants.kPositionConversionFactor);
@@ -45,8 +49,8 @@ public CoralRoller() {
4549
@Override
4650
public void periodic() {
4751
SmartDashboard.putNumber("Coral Roller/Velocity", encoder.getVelocity());
48-
SmartDashboard.putNumber("Coral Roller/Applied Duty Cycle", motor.getAppliedOutput());
49-
SmartDashboard.putNumber("Coral Roller/Current", motor.getOutputCurrent());
52+
// SmartDashboard.putNumber("Coral Roller/Applied Duty Cycle", motor.getAppliedOutput());
53+
// SmartDashboard.putNumber("Coral Roller/Current", motor.getOutputCurrent());
5054
SmartDashboard.putBoolean("Coral Sensor", coralSensor.get());
5155
}
5256

src/main/java/frc/robot/elevator/CoralWrist.java

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,10 @@ public CoralWrist() {
6262
.positionConversionFactor(CoralWristConstants.kPositionConversionFactor.in(Radians))
6363
.velocityConversionFactor(CoralWristConstants.kVelocityConversionFactor.in(RadiansPerSecond));
6464

65+
config.signals
66+
.primaryEncoderVelocityPeriodMs(100)
67+
.outputCurrentPeriodMs(100);
68+
6569
config.softLimit
6670
.reverseSoftLimit(CoralWristState.Min.angle.in(Radians))
6771
.reverseSoftLimitEnabled(true)
@@ -84,15 +88,16 @@ public CoralWrist() {
8488
@Override
8589
public void periodic() {
8690
SmartDashboard.putNumber("Coral Wrist/Current Angle Degrees", getCurrentAngle().in(Degrees));
87-
SmartDashboard.putNumber("Coral Wrist/Current Angle Radians", encoder.getPosition());
88-
SmartDashboard.putNumber("Coral Wrist/Current Angular Velocity RPS", encoder.getVelocity());
91+
// SmartDashboard.putNumber("Coral Wrist/Current Angle Radians", encoder.getPosition());
92+
// SmartDashboard.putNumber("Coral Wrist/Current Angular Velocity RPS", encoder.getVelocity());
8993
SmartDashboard.putString("Coral Wrist/Target State", getTargetState().name());
9094
SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Degrees", getSetpointAngle().in(Degrees));
91-
SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
92-
SmartDashboard.putNumber(
93-
"Coral Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
94-
SmartDashboard.putNumber("Coral Wrist/Applied Duty Cycle", motor.getAppliedOutput());
95-
SmartDashboard.putNumber("Coral Wrist/Current", motor.getOutputCurrent());
95+
// SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Radians",
96+
// feedback.getSetpoint().position);
97+
// SmartDashboard.putNumber(
98+
// "Coral Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
99+
// SmartDashboard.putNumber("Coral Wrist/Applied Duty Cycle", motor.getAppliedOutput());
100+
// SmartDashboard.putNumber("Coral Wrist/Current", motor.getOutputCurrent());
96101
SmartDashboard.putBoolean("Coral Wrist/At Goal", feedback.atGoal());
97102
}
98103

src/main/java/frc/robot/elevator/Lifter.java

Lines changed: 9 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,9 @@ public Lifter() {
7272
.reverseSoftLimitEnabled(true)
7373
.forwardSoftLimit(LifterState.Max.height.in(Inches))
7474
.forwardSoftLimitEnabled(true);
75+
76+
leaderConfig.signals
77+
.primaryEncoderVelocityPeriodMs(100);
7578
// spotless:on
7679

7780
leaderMotor.configure(
@@ -93,21 +96,16 @@ public void periodic() {
9396
loop.poll();
9497

9598
SmartDashboard.putNumber("Lifter/Height", encoder.getPosition());
96-
SmartDashboard.putNumber("Lifter/Velocity", encoder.getVelocity());
99+
// SmartDashboard.putNumber("Lifter/Velocity", encoder.getVelocity());
97100

98101
SmartDashboard.putString("Lifter/Target State", getTargetState().name());
99102
SmartDashboard.putNumber("Lifter/Target Height", controller.getGoal().position);
100103

101-
if (SmartDashboard.getBoolean("Lifter Reset Encoder", false)) {
102-
SmartDashboard.putBoolean("Lifter Reset Encoder", false);
103-
resetEncoder();
104-
}
105-
106-
SmartDashboard.putNumber("Lifter/Leader Applied Duty Cycle", leaderMotor.getAppliedOutput());
107-
SmartDashboard.putNumber(
108-
"Lifter/Follower Applied Duty Cycle", followerMotor.getAppliedOutput());
109-
SmartDashboard.putNumber("Lifter/Leader Current", leaderMotor.getOutputCurrent());
110-
SmartDashboard.putNumber("Lifter/Follower Current", followerMotor.getOutputCurrent());
104+
// SmartDashboard.putNumber("Lifter/Leader Applied Duty Cycle", leaderMotor.getAppliedOutput());
105+
// SmartDashboard.putNumber(
106+
// "Lifter/Follower Applied Duty Cycle", followerMotor.getAppliedOutput());
107+
// SmartDashboard.putNumber("Lifter/Leader Current", leaderMotor.getOutputCurrent());
108+
// SmartDashboard.putNumber("Lifter/Follower Current", followerMotor.getOutputCurrent());
111109

112110
SmartDashboard.putBoolean("Lifter/Lower Limit Switch", !lowerLimitSwitch.get());
113111
SmartDashboard.putBoolean("Lifter/At Target Height", controller.atGoal());

0 commit comments

Comments
 (0)