Skip to content

Commit e8ad4c8

Browse files
ds12ads12a
committed
telop driving
1 parent 0a09aa7 commit e8ad4c8

File tree

5 files changed

+192
-3
lines changed

5 files changed

+192
-3
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/DriveConstants.java‎

Lines changed: 8 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;

‎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)