From de82bb11825e8bc5087f10605b3fe2fd926f1909 Mon Sep 17 00:00:00 2001 From: kartofen Date: Thu, 18 Jan 2024 00:35:25 +0200 Subject: init --- src/main/java/frc/robot/Constants.java | 37 +++++++++ src/main/java/frc/robot/Main.java | 15 ++++ src/main/java/frc/robot/Robot.java | 73 +++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 42 ++++++++++ src/main/java/frc/robot/subsystems/Drivetrain.java | 92 ++++++++++++++++++++++ 5 files changed, 259 insertions(+) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/Main.java create mode 100644 src/main/java/frc/robot/Robot.java create mode 100644 src/main/java/frc/robot/RobotContainer.java create mode 100644 src/main/java/frc/robot/subsystems/Drivetrain.java (limited to 'src/main/java') 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 -- cgit v1.2.3