Skip to content
This repository was archived by the owner on Jan 10, 2025. It is now read-only.

Commit c3cac1a

Browse files
Owen756Owen756
committed
bring up changes
1 parent 33ff49b commit c3cac1a

File tree

4 files changed

+67
-18
lines changed

4 files changed

+67
-18
lines changed

‎src/main/java/frc/robot/arm/ArmAngle.java‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,11 @@
55
public class ArmAngle {
66
public static final Rotation2d SUBWOOFER =
77
Rotation2d.fromDegrees(25); // NONE OF THESE ACCURATE/TUNED
8-
public static final Rotation2d DROP = Rotation2d.fromDegrees(5);
9-
public static final Rotation2d PODIUM = Rotation2d.fromDegrees(20);
10-
public static final Rotation2d IDLE = Rotation2d.fromDegrees(0);
11-
public static final Rotation2d CLIMBING_1_LINEUP = Rotation2d.fromDegrees(30);
12-
public static final Rotation2d CLIMBING_2_HANGING = Rotation2d.fromDegrees(20);
13-
public static final Rotation2d AMP = Rotation2d.fromDegrees(20);
14-
public static final Rotation2d PASS = Rotation2d.fromDegrees(5);
8+
public static final Rotation2d DROP = Rotation2d.fromDegrees(-60.0);
9+
public static final Rotation2d PODIUM = Rotation2d.fromDegrees(-40.0);
10+
public static final Rotation2d IDLE = Rotation2d.fromDegrees(-60.0);
11+
public static final Rotation2d CLIMBING_1_LINEUP = Rotation2d.fromDegrees(0.0);
12+
public static final Rotation2d CLIMBING_2_HANGING = Rotation2d.fromDegrees(-20.0);
13+
public static final Rotation2d AMP = Rotation2d.fromDegrees(70.0);
14+
public static final Rotation2d PASS = Rotation2d.fromDegrees(-60.0);
1515
}

‎src/main/java/frc/robot/arm/ArmSubsystem.java‎

Lines changed: 11 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.robot.arm;
22

3+
import com.ctre.phoenix6.controls.MotionMagicVoltage;
34
import com.ctre.phoenix6.controls.PositionVoltage;
45
import com.ctre.phoenix6.hardware.TalonFX;
56
import dev.doglog.DogLog;
@@ -22,8 +23,8 @@ public class ArmSubsystem extends StateMachine<ArmState> {
2223
private double lowestSeenAngleRight = Double.MAX_VALUE;
2324
private InterpolatingDoubleTreeMap speakerDistanceToAngle = new InterpolatingDoubleTreeMap();
2425
private InterpolatingDoubleTreeMap feedSpotDistanceToAngle = new InterpolatingDoubleTreeMap();
25-
private final PositionVoltage positionRequest =
26-
new PositionVoltage(0)
26+
private final MotionMagicVoltage positionRequest =
27+
new MotionMagicVoltage(0)
2728
.withEnableFOC(false)
2829
.withLimitReverseMotion(false)
2930
.withOverrideBrakeDurNeutral(true);
@@ -41,7 +42,7 @@ public void setDistanceToFeedSpot(double distance) {
4142
}
4243

4344
public ArmSubsystem(TalonFX leftMotor, TalonFX rightMotor) {
44-
super(SubsystemPriority.ARM, ArmState.IDLE);
45+
super(SubsystemPriority.ARM, ArmState.PRE_MATCH_HOMING);
4546
this.leftMotor = leftMotor;
4647
this.rightMotor = rightMotor;
4748
leftMotor.getConfigurator().apply(RobotConfig.get().arm().leftMotorConfig());
@@ -201,19 +202,24 @@ public void robotPeriodic() {
201202

202203
DogLog.log("Arm/Left/StatorCurrent", leftMotor.getStatorCurrent().getValueAsDouble());
203204
DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
204-
DogLog.log("Arm/Left/ArmAngle", leftMotor.getPosition().getValueAsDouble());
205+
DogLog.log(
206+
"Arm/Left/ArmAngle", Units.rotationsToDegrees(leftMotor.getPosition().getValueAsDouble()));
205207
DogLog.log("Arm/Left/AppliedVoltage", leftMotor.getMotorVoltage().getValueAsDouble());
206208
DogLog.log("Arm/Left/LowestSeenAngle", lowestSeenAngleLeft);
207209

208210
DogLog.log("Arm/Right/StatorCurrent", rightMotor.getStatorCurrent().getValueAsDouble());
209211
DogLog.log("Arm/Right/SupplyCurrent", rightMotor.getSupplyCurrent().getValueAsDouble());
210-
DogLog.log("Arm/Right/ArmAngle", rightMotor.getPosition().getValueAsDouble());
212+
DogLog.log(
213+
"Arm/Right/ArmAngle",
214+
Units.rotationsToDegrees(rightMotor.getPosition().getValueAsDouble()));
211215
DogLog.log("Arm/Right/AppliedVoltage", rightMotor.getMotorVoltage().getValueAsDouble());
212216
DogLog.log("Arm/Right/LowestSeenAngle", lowestSeenAngleRight);
217+
DogLog.log("Arm/ArmState", getState());
213218

214219
if (DriverStation.isEnabled() && getState() == ArmState.PRE_MATCH_HOMING) {
215220
// We are enabled and still in pre match homing
216221
// Reset the motor positions, and then transition to idle state
222+
217223
leftMotor.setPosition(
218224
Units.degreesToRotations(
219225
RobotConfig.get().arm().minAngle() + (leftMotorAngle - lowestSeenAngleLeft)));

‎src/main/java/frc/robot/config/CompConfig.java‎

Lines changed: 48 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,15 @@
22

33
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
44
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
5+
import com.ctre.phoenix6.configs.FeedbackConfigs;
6+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
7+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
58
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
69
import com.ctre.phoenix6.configs.Slot0Configs;
710
import com.ctre.phoenix6.configs.TalonFXConfiguration;
811
import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController;
12+
import com.ctre.phoenix6.signals.GravityTypeValue;
13+
import com.ctre.phoenix6.signals.InvertedValue;
914
import edu.wpi.first.math.filter.Debouncer;
1015
import frc.robot.config.RobotConfig.ArmConfig;
1116
import frc.robot.config.RobotConfig.IntakeConfig;
@@ -95,23 +100,60 @@ class CompConfig {
95100
.withOpenLoopRamps(OPEN_LOOP_RAMP)
96101
.withCurrentLimits(
97102
new CurrentLimitsConfigs()
98-
.withStatorCurrentLimit(20)
99-
.withSupplyCurrentLimit(25)),
103+
.withStatorCurrentLimit(40)
104+
.withSupplyCurrentLimit(40))
105+
.withSlot0(
106+
new Slot0Configs()
107+
.withKV(0)
108+
.withKP(300.0)
109+
.withKI(0)
110+
.withKD(0)
111+
.withKG(0.2)
112+
.withGravityType(GravityTypeValue.Arm_Cosine))
113+
.withFeedback(
114+
new FeedbackConfigs()
115+
.withSensorToMechanismRatio((84 * 84 * 24) / (8 * 20 * 9)))
116+
.withMotorOutput(
117+
new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
118+
.withMotionMagic(
119+
new MotionMagicConfigs()
120+
.withMotionMagicAcceleration(3.0)
121+
.withMotionMagicCruiseVelocity(3.0)
122+
.withMotionMagicJerk(100)),
100123
new TalonFXConfiguration()
101124
.withClosedLoopRamps(CLOSED_LOOP_RAMP)
102125
.withOpenLoopRamps(OPEN_LOOP_RAMP)
103126
.withCurrentLimits(
104127
new CurrentLimitsConfigs()
105-
.withStatorCurrentLimit(20)
106-
.withSupplyCurrentLimit(25)),
128+
.withStatorCurrentLimit(40)
129+
.withSupplyCurrentLimit(40))
130+
.withSlot0(
131+
new Slot0Configs()
132+
.withKV(0)
133+
.withKP(300.0)
134+
.withKI(0)
135+
.withKD(0)
136+
.withKG(0.2)
137+
.withGravityType(GravityTypeValue.Arm_Cosine))
138+
.withFeedback(
139+
new FeedbackConfigs()
140+
.withSensorToMechanismRatio((84 * 84 * 24) / (8 * 20 * 9)))
141+
.withMotorOutput(
142+
new MotorOutputConfigs()
143+
.withInverted(InvertedValue.CounterClockwise_Positive))
144+
.withMotionMagic(
145+
new MotionMagicConfigs()
146+
.withMotionMagicAcceleration(3.0)
147+
.withMotionMagicCruiseVelocity(3.0)
148+
.withMotionMagicJerk(100)),
107149
feedSpotDistanceToAngle -> {
108150
feedSpotDistanceToAngle.put(123.0, 321.0);
109151
},
110152
speakerDistanceToAngle -> {
111153
speakerDistanceToAngle.put(123.0, 321.0);
112154
},
113-
0.0,
114-
30.0),
155+
-76.5,
156+
80.0),
115157
new VisionConfig(4, 0.4, 0.4));
116158

117159
// NOT TUNED

‎src/main/java/frc/robot/swerve/SwerveSubsystem.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -109,6 +109,7 @@ public void setSnapToAngle(double angle) {
109109

110110
public SwerveSubsystem() {
111111
super(SubsystemPriority.SWERVE, SwerveState.TELEOP);
112+
modulePositions = calculateModulePositions();
112113
}
113114

114115
public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {

0 commit comments

Comments
 (0)