Skip to content

Commit 1b35ebb

Browse files
ds12ads12a
committed
Merge branch 'main' into algae
2 parents 98a24aa + 3216c96 commit 1b35ebb

File tree

10 files changed

+300
-59
lines changed

10 files changed

+300
-59
lines changed

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

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,16 +4,42 @@
44

55
package frc.robot;
66

7+
import edu.wpi.first.wpilibj.Joystick;
8+
import edu.wpi.first.wpilibj.XboxController;
79
import edu.wpi.first.wpilibj2.command.Command;
810
import edu.wpi.first.wpilibj2.command.Commands;
11+
import frc.robot.commands.drive.DriveTeleopCommand;
12+
import frc.robot.controllers.FlyskyJoystick;
13+
import frc.robot.subsystems.drive.DriveSubsystem;
14+
import frc.robot.subsystems.drive.Swerve;
15+
import org.strykeforce.telemetry.TelemetryController;
16+
import org.strykeforce.telemetry.TelemetryService;
917

1018
public class RobotContainer {
19+
private Swerve swerve;
20+
private DriveSubsystem driveSubsystem;
21+
22+
private final XboxController xboxController = new XboxController(1);
23+
private final Joystick driveJoystick = new Joystick(0);
24+
private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
25+
private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
26+
1127
public RobotContainer() {
28+
swerve = new Swerve();
29+
driveSubsystem = new DriveSubsystem(swerve);
30+
1231
configureBindings();
32+
configureDriverBindings();
1333
}
1434

1535
private void configureBindings() {}
1636

37+
private void configureDriverBindings() {
38+
driveSubsystem.setDefaultCommand(
39+
new DriveTeleopCommand(
40+
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
41+
}
42+
1743
public Command getAutonomousCommand() {
1844
return Commands.print("No autonomous command configured");
1945
}

‎src/main/java/frc/robot/commands/drive/DriveTeleopCommand.java‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
package frc.robot.commands.drive;
2+
3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.filter.SlewRateLimiter;
5+
import edu.wpi.first.wpilibj2.command.Command;
6+
import frc.robot.constants.DriveConstants;
7+
import frc.robot.subsystems.drive.DriveSubsystem;
8+
import java.util.function.DoubleSupplier;
9+
import org.strykeforce.thirdcoast.util.ExpoScale;
10+
11+
public class DriveTeleopCommand extends Command {
12+
private DoubleSupplier fwdStick;
13+
private DoubleSupplier strStick;
14+
private DoubleSupplier yawStick;
15+
private final DriveSubsystem driveSubsystem;
16+
// private final RobotStateSubsystem robotStateSubsystem;
17+
private double[] rawValues = new double[3];
18+
private final ExpoScale expoScaleYaw =
19+
new ExpoScale(DriveConstants.kDeadbandAllStick, DriveConstants.kExpoScaleYawFactor);
20+
private final SlewRateLimiter fwdLimiter = new SlewRateLimiter(DriveConstants.kRateLimitFwdStr);
21+
private final SlewRateLimiter strLimiter = new SlewRateLimiter(DriveConstants.kRateLimitFwdStr);
22+
private final SlewRateLimiter yawLimiter = new SlewRateLimiter(DriveConstants.kRateLimitYaw);
23+
24+
public DriveTeleopCommand(
25+
DoubleSupplier fwdStick,
26+
DoubleSupplier strStick,
27+
DoubleSupplier yawStick,
28+
DriveSubsystem driveSubsystem
29+
/*RobotStateSubsystem robotStateSubsystem*/ ) {
30+
addRequirements(driveSubsystem);
31+
this.fwdStick = fwdStick;
32+
this.strStick = strStick;
33+
this.yawStick = yawStick;
34+
this.driveSubsystem = driveSubsystem;
35+
// this.robotStateSubsystem = robotStateSubsystem;
36+
}
37+
38+
@Override
39+
public void execute() {
40+
rawValues[0] = fwdStick.getAsDouble();
41+
rawValues[1] = strStick.getAsDouble();
42+
rawValues[2] = yawStick.getAsDouble();
43+
44+
// if (robotStateSubsystem.getAllianceColor() == Alliance.Blue) {
45+
driveSubsystem.drive(
46+
-DriveConstants.kMaxSpeedMetersPerSecond
47+
* fwdLimiter.calculate(
48+
MathUtil.applyDeadband(fwdStick.getAsDouble(), DriveConstants.kDeadbandAllStick)),
49+
-DriveConstants.kMaxSpeedMetersPerSecond
50+
* strLimiter.calculate(
51+
MathUtil.applyDeadband(strStick.getAsDouble(), DriveConstants.kDeadbandAllStick)),
52+
-DriveConstants.kMaxVelOmega
53+
* yawLimiter.calculate(
54+
MathUtil.applyDeadband(yawStick.getAsDouble(), DriveConstants.kDeadbandAllStick)));
55+
56+
// } else {
57+
// driveSubsystem.drive(
58+
// DriveConstants.kMaxSpeedMetersPerSecond
59+
// * fwdLimiter.calculate(
60+
// MathUtil.applyDeadband(fwdStick.getAsDouble(),
61+
// DriveConstants.kDeadbandAllStick)),
62+
// DriveConstants.kMaxSpeedMetersPerSecond
63+
// * strLimiter.calculate(
64+
// MathUtil.applyDeadband(strStick.getAsDouble(),
65+
// DriveConstants.kDeadbandAllStick)),
66+
// -DriveConstants.kMaxVelOmega
67+
// * yawLimiter.calculate(
68+
// MathUtil.applyDeadband(
69+
// yawStick.getAsDouble(), DriveConstants.kDeadbandAllStick)));
70+
// }
71+
}
72+
73+
@Override
74+
public boolean isFinished() {
75+
return false;
76+
}
77+
78+
@Override
79+
public void end(boolean interrupted) {
80+
driveSubsystem.drive(0, 0, 0);
81+
}
82+
}

‎src/main/java/frc/robot/constants/BiscuitConstants.java‎

Lines changed: 13 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
import com.ctre.phoenix6.configs.MotorOutputConfigs;
99
import com.ctre.phoenix6.configs.Slot0Configs;
1010
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
11-
import com.ctre.phoenix6.configs.TalonFXConfiguration;
11+
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
1212
import com.ctre.phoenix6.configs.VoltageConfigs;
1313
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
1414
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
@@ -19,7 +19,7 @@
1919
import edu.wpi.first.units.measure.Angle;
2020

2121
public class BiscuitConstants {
22-
// These are all wrong right now because we don't have any actual info
22+
// FIXME These are all wrong right now because we don't have any actual info
2323

2424
public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined
2525
public static int talonID = 3;
@@ -28,17 +28,16 @@ public class BiscuitConstants {
2828
public static final Angle kMaxRev = Rotations.of(-100);
2929
// public static Angle Level1 = ;
3030

31-
// Disables the TalonFX by setting it's voltage to zero. Not very shocking.
31+
// Disables the TalonFXS by setting it's voltage to zero. Not very shocking.
3232
public static VoltageConfigs disableTalon() {
3333
VoltageConfigs voltage =
3434
new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0);
35-
getFXConfig().Voltage = voltage;
35+
getFXSConfig().Voltage = voltage;
3636
return voltage;
3737
}
3838

39-
// I copied and pasted this because I'm lazy
40-
public static TalonFXConfiguration getFXConfig() {
41-
TalonFXConfiguration fxConfig = new TalonFXConfiguration();
39+
public static TalonFXSConfiguration getFXSConfig() {
40+
TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
4241

4342
CurrentLimitsConfigs current =
4443
new CurrentLimitsConfigs()
@@ -49,7 +48,7 @@ public static TalonFXConfiguration getFXConfig() {
4948
.withSupplyCurrentLowerLimit(8)
5049
.withSupplyCurrentLowerTime(0.02)
5150
.withSupplyCurrentLimitEnable(true);
52-
fxConfig.CurrentLimits = current;
51+
fxsConfig.CurrentLimits = current;
5352

5453
HardwareLimitSwitchConfigs hwLimit =
5554
new HardwareLimitSwitchConfigs()
@@ -61,15 +60,15 @@ public static TalonFXConfiguration getFXConfig() {
6160
.withReverseLimitEnable(false)
6261
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
6362
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
64-
fxConfig.HardwareLimitSwitch = hwLimit;
63+
fxsConfig.HardwareLimitSwitch = hwLimit;
6564

6665
SoftwareLimitSwitchConfigs swLimit =
6766
new SoftwareLimitSwitchConfigs()
6867
.withForwardSoftLimitEnable(true)
6968
.withForwardSoftLimitThreshold(kMaxFwd)
7069
.withReverseSoftLimitEnable(true)
7170
.withReverseSoftLimitThreshold(kMaxRev);
72-
fxConfig.SoftwareLimitSwitch = swLimit;
71+
fxsConfig.SoftwareLimitSwitch = swLimit;
7372

7473
Slot0Configs slot0 =
7574
new Slot0Configs()
@@ -81,7 +80,7 @@ public static TalonFXConfiguration getFXConfig() {
8180
.withKS(0)
8281
.withKV(0)
8382
.withKA(0);
84-
fxConfig.Slot0 = slot0;
83+
fxsConfig.Slot0 = slot0;
8584

8685
MotionMagicConfigs motionMagic =
8786
new MotionMagicConfigs()
@@ -90,14 +89,14 @@ public static TalonFXConfiguration getFXConfig() {
9089
.withMotionMagicExpo_kA(0)
9190
.withMotionMagicExpo_kV(0)
9291
.withMotionMagicJerk(0);
93-
fxConfig.MotionMagic = motionMagic;
92+
fxsConfig.MotionMagic = motionMagic;
9493

9594
MotorOutputConfigs motorOut =
9695
new MotorOutputConfigs()
9796
.withDutyCycleNeutralDeadband(0.01)
9897
.withNeutralMode(NeutralModeValue.Coast);
99-
fxConfig.MotorOutput = motorOut;
98+
fxsConfig.MotorOutput = motorOut;
10099

101-
return fxConfig;
100+
return fxsConfig;
102101
}
103102
}

‎src/main/java/frc/robot/constants/DriveConstants.java‎

Lines changed: 62 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,11 @@
1313
import edu.wpi.first.math.geometry.Translation2d;
1414

1515
public class DriveConstants {
16+
public static final double kDeadbandAllStick = 0.075;
17+
public static final double kExpoScaleYawFactor = 0.75;
18+
public static final double kRateLimitFwdStr = 3.5;
19+
public static final double kRateLimitYaw = 8.0;
20+
1621
public static final double kDriveGearRatio = 6.5;
1722
public static final double kWheelDiameterInches = 4.0;
1823
public static final double kMaxSpeedMetersPerSecond = 12.0;
@@ -21,9 +26,9 @@ public class DriveConstants {
2126
public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
2227
public static final int kGyroDifferentCount = 3;
2328

24-
public static final double kRobotLength = 22.0;
25-
public static final double kRobotWidth = 22.0;
26-
public static final double kFieldMaxX = 690.0;
29+
public static final double kRobotLength = 0.6223;
30+
public static final double kRobotWidth = 0.6223;
31+
public static final double kFieldMaxX = 17.526;
2732

2833
public static final double kPOmega = 4.5;
2934
public static final double kIOmega = 0.0;
@@ -53,6 +58,60 @@ public static Translation2d[] getWheelLocationMeters() {
5358
public static final double kRecoverTemp = 1290;
5459
public static final double kNotifyTemp = 1295;
5560

61+
// public static TalonFXSConfiguration
62+
// getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
63+
// // constructor sets encoder to Quad/CTRE_MagEncoder_Relative
64+
// TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration();
65+
66+
// HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs();
67+
// hardwareLimitSwitchConfigs.ForwardLimitEnable = false;
68+
// hardwareLimitSwitchConfigs.ReverseLimitEnable = false;
69+
// azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs;
70+
71+
// CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
72+
// currentConfig.SupplyCurrentLowerTime = 0;
73+
// currentConfig.SupplyCurrentLowerLimit = 0;
74+
75+
// currentConfig.SupplyCurrentLimit = 10;
76+
// currentConfig.SupplyCurrentLimitEnable = true;
77+
78+
// azimuthConfig.CurrentLimits = currentConfig;
79+
80+
// Slot0Configs slot0Config = new Slot0Configs();
81+
// slot0Config.kP = 360.35;
82+
// slot0Config.kI = 0.0;
83+
// slot0Config.kD = 3.604;
84+
85+
// azimuthConfig.Slot0 = slot0Config;
86+
87+
// ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs();
88+
// externalFeedbackConfigs.VelocityFilterTimeConstant = 0.1;
89+
// externalFeedbackConfigs.ExternalFeedbackSensorSource =
90+
// ExternalFeedbackSensorSourceValue.PulseWidth;
91+
// azimuthConfig.ExternalFeedback = externalFeedbackConfigs;
92+
93+
// VoltageConfigs voltageConfig = new VoltageConfigs();
94+
// voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long
95+
// azimuthConfig.Voltage = voltageConfig;
96+
97+
// MotionMagicConfigs motionConfig = new MotionMagicConfigs();
98+
// motionConfig.MotionMagicCruiseVelocity = 800;
99+
// motionConfig.MotionMagicAcceleration = 10_000;
100+
// azimuthConfig.MotionMagic = motionConfig;
101+
102+
// MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
103+
// motorConfigs.DutyCycleNeutralDeadband = 0.04;
104+
// motorConfigs.NeutralMode = NeutralModeValue.Coast;
105+
// azimuthConfig.MotorOutput = motorConfigs;
106+
107+
// CommutationConfigs commutationConfigs = new CommutationConfigs();
108+
// commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST;
109+
110+
// azimuthConfig.Commutation = commutationConfigs;
111+
112+
// return azimuthConfig;
113+
// }
114+
56115
public static TalonSRXConfiguration
57116
getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
58117
// constructor sets encoder to Quad/CTRE_MagEncoder_Relative

‎src/main/java/frc/robot/controllers/ControllerInterface.java‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
package frc.robot.controllers;
2+
3+
public interface ControllerInterface {
4+
5+
public double getFwd();
6+
7+
public double getStr();
8+
9+
public double getYaw();
10+
}

‎src/main/java/frc/robot/controllers/FlyskyJoystick.java‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package frc.robot.controllers;
2+
3+
import edu.wpi.first.wpilibj.Joystick;
4+
5+
public class FlyskyJoystick implements ControllerInterface {
6+
private Joystick flysky;
7+
8+
public FlyskyJoystick(Joystick jostick) {
9+
flysky = jostick;
10+
}
11+
12+
@Override
13+
public double getFwd() {
14+
return -flysky.getRawAxis(Axis.FWD.id);
15+
}
16+
17+
@Override
18+
public double getStr() {
19+
return flysky.getRawAxis(Axis.STR.id);
20+
}
21+
22+
@Override
23+
public double getYaw() {
24+
return flysky.getRawAxis(Axis.YAW.id);
25+
}
26+
27+
public enum Axis {
28+
FWD(0),
29+
STR(1),
30+
YAW(2);
31+
32+
public final int id;
33+
34+
Axis(int id) {
35+
this.id = id;
36+
}
37+
}
38+
39+
public enum Button {
40+
SWA(1),
41+
SWB_UP(2),
42+
SWB_DWN(3),
43+
M_SWC(4),
44+
SWD(5),
45+
M_SWE(6),
46+
SWF_UP(7),
47+
SWF_DWN(8),
48+
SWG_UP(9),
49+
SWG_DWN(10),
50+
M_SWH(11),
51+
M_LTRIM_UP(12),
52+
M_LTRIM_DWN(13),
53+
M_LTRIM_L(14),
54+
M_LTRIM_R(15),
55+
M_RTRIM_UP(16),
56+
M_RTRIM_DWN(17),
57+
M_RTRIM_L(18),
58+
M_RTRIM_R(19);
59+
60+
public final int id;
61+
62+
Button(int id) {
63+
this.id = id;
64+
}
65+
}
66+
}

0 commit comments

Comments
 (0)