Skip to content

Commit ae165d7

Browse files
ControlsNarwhalControlsNarwhal
committed
added Amper constants and PID within subsytem
1 parent 0c1eda6 commit ae165d7

File tree

6 files changed

+79
-59
lines changed

6 files changed

+79
-59
lines changed

‎annotation-processor/build/tmp/compileJava/previous-compilation-data.bin

-10 Bytes
Binary file not shown.

‎src/main/java/frc/team3128/Constants.java

Lines changed: 27 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,15 @@
22

33
import java.util.HashMap;
44

5+
import com.ctre.phoenix6.hardware.CANcoder;
56
import com.pathplanner.lib.path.PathConstraints;
67

78
import common.core.controllers.Controller;
89
import common.core.controllers.PIDFFConfig;
910
import common.core.controllers.Controller.Type;
1011
import common.core.swerve.SwerveConversions;
1112
import common.core.swerve.SwerveModuleConfig;
13+
import common.core.swerve.SwerveModuleConfig.SwerveEncoderConfig;
1214
import common.core.swerve.SwerveModuleConfig.SwerveMotorConfig;
1315
import common.hardware.motorcontroller.NAR_CANSpark;
1416
import common.hardware.motorcontroller.NAR_TalonFX;
@@ -60,7 +62,7 @@ public static class AutoConstants {
6062
public static class SwerveConstants {
6163
public static final double RAMP_TIME = 3;
6264

63-
public static final int pigeonID = 30;
65+
public static final int pigeonID = 15;
6466

6567
/* Drivetrain Constants */
6668
public static final double bumperLength = Units.inchesToMeters(5);
@@ -107,9 +109,9 @@ public static class SwerveConstants {
107109
// Theoretical: v = 4.96824, omega = 11.5
108110
// Real: v = 4.5, omega = 10
109111
// For safety, use less than theoretical and real values
110-
public static final double maxSpeed = 5.87;//4.8; //meters per second - 16.3 ft/sec
112+
public static final double maxSpeed = 3;//4.8; //meters per second - 16.3 ft/sec
111113
public static final double maxAttainableSpeed = maxSpeed; //Stole from citrus.
112-
public static final double maxAcceleration = 5;
114+
public static final double maxAcceleration = 1;
113115
public static final double maxAngularVelocity = 8; //3; //11.5; // citrus: 10 - Mason look at this later wtf
114116
public static final double maxAngularAcceleration = 2 * Math.PI; //I stole from citrus.
115117

@@ -130,38 +132,31 @@ public static class SwerveConstants {
130132

131133
public static final SwerveModuleConfig Mod0 = new SwerveModuleConfig(
132134
0,
133-
new SwerveMotorConfig(new NAR_TalonFX(1), driveMotorConfig, drivePIDConfig),
134-
new SwerveMotorConfig(new NAR_TalonFX(2), angleMotorConfig, anglePIDConfig),
135-
10,
136-
63.017578125 - 180,
137-
canCoderInvert,
138-
maxSpeed);
135+
new SwerveMotorConfig(new NAR_TalonFX(1, "Drivetrain"), driveMotorConfig, drivePIDConfig),
136+
new SwerveMotorConfig(new NAR_TalonFX(2, "Drivetrain"), angleMotorConfig, anglePIDConfig),
137+
new SwerveEncoderConfig(new CANcoder(11, "Drivetrain"), 105.15, canCoderInvert),
138+
maxSpeed
139+
);
139140

140141
public static final SwerveModuleConfig Mod1 = new SwerveModuleConfig(
141142
1,
142-
new SwerveMotorConfig(new NAR_TalonFX(3), driveMotorConfig, drivePIDConfig),
143-
new SwerveMotorConfig(new NAR_TalonFX(4), angleMotorConfig, anglePIDConfig),
144-
11,
145-
110.478515625 - 180,
146-
canCoderInvert,
143+
new SwerveMotorConfig(new NAR_TalonFX(3, "Drivetrain"), driveMotorConfig, drivePIDConfig),
144+
new SwerveMotorConfig(new NAR_TalonFX(4, "Drivetrain"), angleMotorConfig, anglePIDConfig),
145+
new SwerveEncoderConfig(new CANcoder(12, "Drivetrain"), -62.40234375, canCoderInvert),
147146
maxSpeed);
148147

149148
public static final SwerveModuleConfig Mod2 = new SwerveModuleConfig(
150149
2,
151-
new SwerveMotorConfig(new NAR_TalonFX(5), driveMotorConfig, drivePIDConfig),
152-
new SwerveMotorConfig(new NAR_TalonFX(6), angleMotorConfig, anglePIDConfig),
153-
12,
154-
-48.076171875 + 180,
155-
canCoderInvert,
150+
new SwerveMotorConfig(new NAR_TalonFX(5, "Drivetrain"), driveMotorConfig, drivePIDConfig),
151+
new SwerveMotorConfig(new NAR_TalonFX(6, "Drivetrain"), angleMotorConfig, anglePIDConfig),
152+
new SwerveEncoderConfig(new CANcoder(13, "Drivetrain"), -84.287109375, canCoderInvert),
156153
maxSpeed);
157154

158155
public static final SwerveModuleConfig Mod3 = new SwerveModuleConfig(
159156
3,
160-
new SwerveMotorConfig(new NAR_TalonFX(7), driveMotorConfig, drivePIDConfig),
161-
new SwerveMotorConfig(new NAR_TalonFX(8), angleMotorConfig, anglePIDConfig),
162-
13,
163-
-158.37890625000003 + 180,
164-
canCoderInvert,
157+
new SwerveMotorConfig(new NAR_TalonFX(7, "Drivetrain"), driveMotorConfig, drivePIDConfig),
158+
new SwerveMotorConfig(new NAR_TalonFX(8, "Drivetrain"), angleMotorConfig, anglePIDConfig),
159+
new SwerveEncoderConfig(new CANcoder(14, "Drivetrain"), 167.607421875, canCoderInvert),
165160
maxSpeed);
166161

167162
public static final double turnkP = 5;
@@ -462,24 +457,26 @@ public static class AmperConstants {
462457
public static final int ELEV_MOTOR_ID = 21;
463458
public static final NAR_CANSpark ELEV_MOTOR = new NAR_CANSpark(ELEV_MOTOR_ID);
464459

465-
public static final int ROLLER_MOTOR_ID = 20;
466-
public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX);
460+
// public static final int ROLLER_MOTOR_ID = 20;
461+
// public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX);
467462

468-
public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.1, 0, 0, 0.2, 0, 0, 0.3625);
463+
public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.75, 0, 0, 0.21115, 0.00182, 0.00182, 0.0);
469464
public static final double MAX_VELOCTIY = 10000000;
470465
public static final double MAX_ACCELERATION = 100000;
471466
public static final Constraints TRAP_CONSTRAINTS = new Constraints(MAX_VELOCTIY, MAX_ACCELERATION);
472467

473468
public static final double POSITION_TOLERANCE = 0.25;
474469
public static final double MIN_SETPOINT = 0;
475-
public static final double MAX_SETPOINT = 25;
470+
public static final double MAX_SETPOINT = 21.25;
476471
public static final int CURRENT_LIMIT = 40;
477472

478-
public static final double GEAR_RATIO = 1.0 / 15.0;
479-
public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(1.751) * Math.PI;
473+
public static final double GEAR_RATIO = 1.0 / (6 + 2/3);
474+
public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(0.9023) * Math.PI;
480475
public static final double UNIT_CONV_FACTOR = GEAR_RATIO * WHEEL_CIRCUMFERENCE * 100;
481476

482477
public static final double ROLLER_POWER = 0.5;
478+
479+
public static final double AMPER_ANGLE = 31.96;
483480
}
484481
}
485482

‎src/main/java/frc/team3128/RobotContainer.java

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424
import frc.team3128.commands.CmdSwerveDrive;
2525
import common.core.swerve.SwerveModule;
2626
import common.hardware.camera.Camera;
27-
import common.hardware.camera.OffseasonAprilTags;
27+
// import common.hardware.camera.OffseasonAprilTags;
2828
import common.hardware.input.NAR_ButtonBoard;
2929
import common.hardware.input.NAR_XboxController;
3030
import common.hardware.input.NAR_XboxController.XboxButton;
@@ -38,10 +38,11 @@
3838
import common.utility.shuffleboard.NAR_Shuffleboard;
3939
import common.utility.sysid.CmdSysId;
4040
import common.utility.tester.Tester;
41-
import common.utility.tester.Tester.UnitTest;
41+
// import common.utility.tester.Tester.UnitTest;
4242
import frc.team3128.subsystems.Leds;
4343
import frc.team3128.subsystems.Swerve;
4444
import java.util.ArrayList;
45+
import edu.wpi.first.apriltag.AprilTagFields;
4546

4647
/**
4748
* Command-based is a "declarative" paradigm, very little robot logic should
@@ -110,7 +111,7 @@ private void configureButtonBindings() {
110111
@SuppressWarnings("unused")
111112
public void initCameras() {
112113
Camera.disableAll();
113-
Camera.configCameras(OffseasonAprilTags.offSeasonTagMap, PoseStrategy.LOWEST_AMBIGUITY, (pose, time) -> swerve.addVisionMeasurement(pose, time), () -> swerve.getPose());
114+
Camera.configCameras(AprilTagFields.k2024Crescendo, PoseStrategy.LOWEST_AMBIGUITY, (pose, time) -> swerve.addVisionMeasurement(pose, time), () -> swerve.getPose());
114115
Camera.setAmbiguityThreshold(0.3);
115116
Camera.overrideThreshold = 30;
116117
Camera.validDist = 0.5;

‎src/main/java/frc/team3128/subsystems/Amper.java

Lines changed: 45 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,12 @@
55
import common.core.controllers.TrapController;
66
import common.core.subsystems.ElevatorTemplate;
77
import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig;
8+
import common.hardware.motorcontroller.NAR_Motor.Control;
89
import common.hardware.motorcontroller.NAR_Motor.Neutral;
9-
10+
import common.utility.shuffleboard.NAR_Shuffleboard;
1011
import edu.wpi.first.wpilibj2.command.Command;
12+
import edu.wpi.first.wpilibj2.command.InstantCommand;
13+
1114
import static edu.wpi.first.wpilibj2.command.Commands.sequence;
1215

1316
public class Amper extends ElevatorTemplate {
@@ -33,8 +36,9 @@ public static synchronized Amper getInstance() {
3336
private Amper() {
3437
super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), ELEV_MOTOR);
3538

36-
// TODO: figure out kG function
37-
// setkG_Function(());
39+
this.setSafetyThresh(100);
40+
// setkG_Function(() -> getMeasurement()*Math.sin(AMPER_ANGLE));
41+
3842
setTolerance(POSITION_TOLERANCE);
3943
setConstraints(MIN_SETPOINT, MAX_SETPOINT);
4044
initShuffleboard();
@@ -45,42 +49,60 @@ protected void configMotors() {
4549
// TODO: figure unit conversion out
4650
ELEV_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR);
4751
ELEV_MOTOR.setCurrentLimit(CURRENT_LIMIT);
52+
ELEV_MOTOR.setInverted(true);
4853

49-
ELEV_MOTOR.setNeutralMode(Neutral.BRAKE);
50-
ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
54+
ELEV_MOTOR.setNeutralMode(Neutral.COAST);
55+
// ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
5156

52-
ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);
53-
ROLLER_MOTOR.setDefaultStatusFrames();
57+
// ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);
58+
// ROLLER_MOTOR.setDefaultStatusFrames();
5459
}
55-
56-
public Command moveElevator(Setpoint setpoint) {
57-
return moveElevator(setpoint.setpoint);
60+
61+
public void setVoltage(double volts) {
62+
ELEV_MOTOR.set(0, Control.Position);
63+
ELEV_MOTOR.setVolts(volts);
5864
}
59-
60-
public Command runRollers() {
61-
return runRollers(ROLLER_POWER);
65+
public double getVelocity() {
66+
return ELEV_MOTOR.getVelocity();
6267
}
68+
public double getPosition() {
69+
return ELEV_MOTOR.getPosition();
70+
}
71+
72+
@Override
73+
public void initShuffleboard(){
74+
super.initShuffleboard();
6375

64-
public Command stopRollers(){
65-
return runRollers(0);
76+
NAR_Shuffleboard.addData(getName(), "Measurement2", ()-> getPosition(), 5, 1);
77+
NAR_Shuffleboard.addData(getName(), "Current", ()-> ELEV_MOTOR.getStallCurrent(), 5, 2);
78+
NAR_Shuffleboard.addData(getName(), "Voltage", ()-> ELEV_MOTOR.getMotor().getBusVoltage(), 5, 3);
79+
NAR_Shuffleboard.addData(getName(), "Velocity", ()-> getVelocity(), 1,0);
6680
}
6781

68-
public Command runRollers(double power) {
69-
return runOnce(() -> ROLLER_MOTOR.set(power));
70-
}
82+
// public Command runRollers() {
83+
// return runRollers(ROLLER_POWER);
84+
// }
85+
86+
// public Command stopRollers(){
87+
// return runRollers(0);
88+
// }
89+
90+
// public Command runRollers(double power) {
91+
// return runOnce(() -> ROLLER_MOTOR.set(power));
92+
// }
7193

7294
public Command extend() {
7395
return sequence(
74-
moveElevator(Setpoint.EXTENDED),
75-
runRollers()
96+
moveElevator(Setpoint.EXTENDED.setpoint)
97+
// runRollers()
7698
);
7799
}
78100

79101
public Command retract() {
80102
return sequence(
81-
moveElevator(Setpoint.RETRACTED),
82-
stopRollers()
103+
moveElevator(Setpoint.RETRACTED.setpoint)
104+
// stopRollers()
83105
);
84106
}
85107

86-
}
108+
}

‎src/main/java/frc/team3128/subsystems/Swerve.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -193,7 +193,7 @@ public Command turnInPlace(DoubleSupplier setpoint) {
193193

194194
public boolean isConfigured() {
195195
for (final SwerveModule module : modules) {
196-
final double CANCoderAngle = module.getCanCoder().getDegrees();
196+
final double CANCoderAngle = module.getAbsoluteAngle().getDegrees();
197197
final double AngleMotorAngle = module.getAngleMotor().getPosition();
198198
if (CANCoderAngle == 0 || AngleMotorAngle == 0) return false;
199199
}

‎vendordeps/3128-common.json

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
{
22
"name": "3128-common",
3-
"version": "1.7.8",
3+
"version": "1.8.2",
44
"uuid": "ae3fa5a2-78d9-47e8-921a-dba45b889445",
55
"frcYear": "2024",
66
"mavenUrls": [
@@ -12,7 +12,7 @@
1212
{
1313
"groupId": "com.github.Team3128",
1414
"artifactId": "3128-common",
15-
"version": "1.7.8"
15+
"version": "1.8.2"
1616
}
1717
],
1818
"jniDependencies": [],

0 commit comments

Comments
 (0)