diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp index ca5aa93..7ef374f 100644 --- a/src/main/cpp/str/SwerveDrive.cpp +++ b/src/main/cpp/str/SwerveDrive.cpp @@ -72,10 +72,6 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel, units::meters_per_second_t yVel, units::radians_per_second_t omega, bool fieldRelative, bool openLoop) { - units::second_t now = frc::Timer::GetFPGATimestamp(); - units::second_t loopTime = now - lastDriveLoopTime; - loopTimePub.Set((1 / loopTime).value()); - frc::ChassisSpeeds speedsToSend; if (fieldRelative) { speedsToSend = frc::ChassisSpeeds::FromFieldRelativeSpeeds( @@ -86,14 +82,12 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel, speedsToSend.omega = omega; } - speedsToSend = frc::ChassisSpeeds::Discretize(speedsToSend, loopTime); + speedsToSend = frc::ChassisSpeeds::Discretize(speedsToSend, consts::LOOP_PERIOD); SetModuleStates( consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend), true, openLoop, ConvertModuleForcesToTorqueCurrent(xModuleForce, yModuleForce)); - - lastDriveLoopTime = now; } frc::ChassisSpeeds SwerveDrive::GetRobotRelativeSpeeds() const { @@ -190,14 +184,11 @@ void SwerveDrive::UpdateNTEntries() { } void SwerveDrive::SimulationPeriodic() { - units::second_t now = frc::Timer::GetFPGATimestamp(); - units::second_t loopTime = now - lastSimLoopTime; - std::array simState; int i = 0; for (auto &swerveModule : modules) { simState[i] = swerveModule.UpdateSimulation( - loopTime, frc::RobotController::GetBatteryVoltage()); + consts::LOOP_PERIOD, frc::RobotController::GetBatteryVoltage()); i++; } @@ -205,12 +196,10 @@ void SwerveDrive::SimulationPeriodic() { units::radians_per_second_t omega = consts::swerve::physical::KINEMATICS.ToChassisSpeeds(simState).omega; - units::radian_t angleChange = omega * loopTime; + units::radian_t angleChange = omega * consts::LOOP_PERIOD; lastSimAngle = lastSimAngle + frc::Rotation2d{angleChange}; imuSimState.SetRawYaw(lastSimAngle.Degrees()); - - lastSimLoopTime = now; } units::ampere_t SwerveDrive::GetSimulatedCurrentDraw() const { diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp new file mode 100644 index 0000000..7f83e86 --- /dev/null +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "subsystems/IntakeSubsystem.h" +#include + +IntakeSubsystem::IntakeSubsystem() { + + ConfigureIntakeMotor(consts::intake::physical::INVERT_MOTOR, consts::intake::physical::INTAKE_RATIO, consts::intake::current_limits::SUPPLY_CURRENT_LIMIT, consts::intake::current_limits::STATOR_CURRENT_LIMIT); + ConfigureMotorSignals(); + SetName("IntakeSubsystem"); + frc::SmartDashboard::PutData(this); +} + +// This method will be called once per scheduler run +void IntakeSubsystem::Periodic() {} + +bool IntakeSubsystem::ConfigureIntakeMotor(bool invert, + units::scalar_t intakeGearing, + units::ampere_t supplyCurrentLimit, + units::ampere_t statorCurrentLimit) { + return true; + } + + +bool IntakeSubsystem::ConfigureMotorSignals() { + return true; +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index 877a1d1..0b3b6a0 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -5,6 +5,8 @@ #include "subsystems/ShooterSubsystem.h" #include "constants/Constants.h" #include +#include + ShooterSubsystem::ShooterSubsystem() { ConfigureShooterMotors(consts::shooter::physical::BOTTOM_INVERT, @@ -13,6 +15,8 @@ ShooterSubsystem::ShooterSubsystem() { consts::shooter::current_limits::SUPPLY_CURRENT_LIMIT, consts::shooter::current_limits::STATOR_CURRENT_LIMIT); ConfigureMotorSignals(); + SetName("ShooterSubsystem"); + frc::SmartDashboard::PutData(this); } frc2::CommandPtr ShooterSubsystem::RunShooter(std::function preset) { diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index b4aec7f..48f52b2 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -d7b9781 \ No newline at end of file +f5a5c55 \ No newline at end of file diff --git a/src/main/include/constants/IntakeConstants.h b/src/main/include/constants/IntakeConstants.h new file mode 100644 index 0000000..066ffdf --- /dev/null +++ b/src/main/include/constants/IntakeConstants.h @@ -0,0 +1,41 @@ +// Copyright (c) FRC 2053. +// Open Source Software; you can modify and/or share it under the terms of +// the MIT License file in the root of this project + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "str/Gains.h" + +namespace consts { +namespace intake { +namespace can_ids { +inline constexpr int INTAKE = 17; +} // namespace can_ids + +namespace current_limits { +inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A; +inline constexpr units::ampere_t STATOR_CURRENT_LIMIT = 180_A; +} // namespace current_limits + +namespace physical { + +inline constexpr bool INVERT_MOTOR = false; + +inline constexpr units::scalar_t INTAKE_RATIO = (24.0 / 16.0); + +inline constexpr frc::DCMotor SHOOTER_MOTOR = frc::DCMotor::Falcon500(1); + +inline constexpr units::meter_t WHEEL_RADIUS = 1_in; + +//From onshape doc +inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 12.350445 * 1_in * 1_in * 1_lb; +} // namespace physical +} // namespace intake +} // namespace consts diff --git a/src/main/include/str/SwerveDrive.h b/src/main/include/str/SwerveDrive.h index 8ad274c..2cb840a 100644 --- a/src/main/include/str/SwerveDrive.h +++ b/src/main/include/str/SwerveDrive.h @@ -152,17 +152,10 @@ class SwerveDrive { modulePositions, frc::Pose2d{}}; units::radian_t yawLatencyComped; - units::second_t lastDriveLoopTime; - units::second_t lastSimLoopTime; frc::Rotation2d lastSimAngle; std::shared_ptr nt{ nt::NetworkTableInstance::GetDefault().GetTable("SwerveDrive")}; - nt::DoublePublisher loopTimePub{ - nt::NetworkTableInstance::GetDefault() - .GetTable("Metadata") - ->GetDoubleTopic("SwerveDriveOdomLoopRate") - .Publish()}; nt::BooleanPublisher isSlippingPub{ nt->GetBooleanTopic("IsSlipping").Publish()}; nt::StructArrayPublisher desiredStatesPub{ diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h new file mode 100644 index 0000000..c7ef220 --- /dev/null +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -0,0 +1,34 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +class IntakeSubsystem : public frc2::SubsystemBase { + public: + IntakeSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + + private: + void UpdateNTEntries(); + bool ConfigureIntakeMotor(bool invert, + units::scalar_t intakeGearing, + units::ampere_t supplyCurrentLimit, + units::ampere_t statorCurrentLimit); + bool ConfigureMotorSignals(); + + std::shared_ptr nt{ + nt::NetworkTableInstance::GetDefault().GetTable("Intake")}; +}; diff --git a/src/main/include/subsystems/ShooterSubsystem.h b/src/main/include/subsystems/ShooterSubsystem.h index 095bff4..0a662d9 100644 --- a/src/main/include/subsystems/ShooterSubsystem.h +++ b/src/main/include/subsystems/ShooterSubsystem.h @@ -126,11 +126,6 @@ class ShooterSubsystem : public frc2::SubsystemBase { std::shared_ptr nt{ nt::NetworkTableInstance::GetDefault().GetTable("Shooter")}; - nt::DoublePublisher loopTimePub{ - nt::NetworkTableInstance::GetDefault() - .GetTable("Metadata") - ->GetDoubleTopic("ShooterOdomLoopRate") - .Publish()}; nt::DoublePublisher topWheelVelocityPub{ nt->GetDoubleTopic("TopWheelVelocityRPM").Publish() };