summaryrefslogtreecommitdiff
path: root/src/main/java
diff options
context:
space:
mode:
Diffstat (limited to 'src/main/java')
-rw-r--r--src/main/java/frc/robot/Constants.java37
-rw-r--r--src/main/java/frc/robot/Main.java15
-rw-r--r--src/main/java/frc/robot/Robot.java73
-rw-r--r--src/main/java/frc/robot/RobotContainer.java42
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java92
5 files changed, 259 insertions, 0 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..f517f08
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,37 @@
+package frc.robot;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+public class Constants
+{
+ // other constants
+
+ public static final int PortDriverController = 0;
+
+ public static final double DriveMaxSpeed = 0;
+ public static final double DriveMaxAngular = 0;
+
+ public static final double WheelRadius = 0; // in meters
+ public static final double TrackWidth = 0; // in meters
+
+ public static final double EncoderResolution = 4096;
+
+ // public static final DifferentialDrive drive = new DifferentialDrive(...);
+
+ public static final AnalogGyro gyro = new AnalogGyro(...);
+
+ public static final PWMSparkMax motorL_leader = new PWMSparkMax(...);
+ public static final PWMSparkMax motorL_follower = new PWMSparkMax(...);
+ public static final PWMSparkMax motorR_leader = new PWMSparkMax(...);
+ public static final PWMSparkMax motorR_follower = new PWMSparkMax(...);
+
+
+ public static final Encoder encoderL = new Encoder(...);
+ public static final Encoder encoderR = new Encoder(...);
+
+ public static final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(...);
+}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
new file mode 100644
index 0000000..fe215d7
--- /dev/null
+++ b/src/main/java/frc/robot/Main.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+ private Main() {}
+
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..b68462c
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ @Override
+ public void robotInit() {
+ m_robotContainer = new RobotContainer();
+ }
+
+ @Override
+ public void robotPeriodic() {
+ CommandScheduler.getInstance().run();
+ }
+
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ @Override
+ public void disabledExit() {}
+
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void autonomousExit() {}
+
+ @Override
+ public void teleopInit() {
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ @Override
+ public void teleopPeriodic() {}
+
+ @Override
+ public void teleopExit() {}
+
+ @Override
+ public void testInit() {
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ @Override
+ public void testPeriodic() {}
+
+ @Override
+ public void testExit() {}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644
index 0000000..e08bf8d
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.Drivetrain;
+
+public class RobotContainer
+{
+ private final Drivetrain m_drive = new Drivetrain();
+
+ private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3);
+ private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3);
+
+ private final CommandXboxController m_dcontroller =
+ new CommandXboxController(Constants.PortDriverController);
+
+ public RobotContainer()
+ {
+ configureBindings();
+ }
+
+ private void configureBindings()
+ {
+ // drivetrain controls
+ m_drive.setDefaultCommand(m_drive.run(() ->
+ m_drive.drive(
+ m_speedlimiter.calculate(m_dcontroller.getLeftY()) * Constants.DriveMaxSpeed,
+ m_rotlimiter.calculate(m_dcontroller.getRightX()) * Constants.DriveMaxAngular
+ )));
+ }
+
+ public Command getAutonomousCommand()
+ {
+ return Commands.print("No autonomous command configured");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
new file mode 100644
index 0000000..14c9d50
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -0,0 +1,92 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Drivetrain extends SubsystemBase
+{
+ private AnalogGyro m_gyro = Constants.gyro;
+
+ private PWMSparkMax m_motorL_leader = Constants.motorL_leader;
+ private PWMSparkMax m_motorL_follower = Constants.motorL_follower;
+ private PWMSparkMax m_motorR_leader = Constants.motorR_leader;
+ private PWMSparkMax m_motorR_follower = Constants.motorR_follower;
+
+ private Encoder m_encoderL = Constants.encoderL;
+ private Encoder m_encoderR = Constants.encoderR;
+
+ private SimpleMotorFeedforward m_feedforward = Constants.feedforward;
+
+ private final PIDController m_pidL = new PIDController(1, 0, 0);
+ private final PIDController m_pidR = new PIDController(1, 0, 0);
+
+ private DifferentialDriveKinematics m_kinematics =
+ new DifferentialDriveKinematics(Constants.TrackWidth);
+
+ private DifferentialDriveOdometry m_odometry;
+
+ public Drivetrain()
+ {
+ m_gyro.reset();
+
+ m_motorL_leader.addFollower(m_motorL_follower);
+ m_motorR_leader.addFollower(m_motorR_follower);
+
+ // perhaps invert
+ // m_motorR_leader.setInverted(true);
+
+ m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+ m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+
+ m_encoderL.reset();
+ m_encoderR.reset();
+
+ m_odometry = new DifferentialDriveOdometry(
+ m_gyro.getRotation2d(),
+ Constants.encoderL.getDistance(),
+ Constants.encoderR.getDistance());
+ }
+
+ // speed - linear velocity in m/s
+ // rot - angular velocity in rad/s
+ public void drive(double speed, double rot)
+ {
+ speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot)));
+ }
+
+ public void speed(DifferentialDriveWheelSpeeds speeds)
+ {
+ m_motorL_leader.setVoltage(
+ m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
+ + m_feedforward.calculate(speeds.leftMetersPerSecond)
+ );
+
+ m_motorR_leader.setVoltage(
+ m_pidR.calculate(m_encoderR.getRate(), speeds.rightMetersPerSecond)
+ + m_feedforward.calculate(speeds.rightMetersPerSecond)
+ );
+ }
+
+ @Override
+ public void periodic()
+ {
+ m_odometry.update(
+ Constants.gyro.getRotation2d(),
+ Constants.encoderL.getDistance(),
+ Constants.encoderR.getDistance());
+ }
+
+ @Override
+ public void simulationPeriodic() {
+ // This method will be called once per scheduler run during simulation
+ }
+} \ No newline at end of file
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-23 09:37:56 +0000