Zynh0722/2024-crescendo-cpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

swervin?

Browse source
This commit is contained in:
Zynh Ludwig 2024-03-28 02:18:39 -07:00
parent bbd9e816bc
commit c546e69ca7
2 changed files with 29 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
src/main/cpp/cowlib/SwerveModule.cpp
View file

@ -3,7 +3,11 @@
// the WPILib BSD license file in the root directory of this project. // the WPILib BSD license file in the root directory of this project.
#include "Constants.h" #include "Constants.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "rev/CANSparkMax.h" #include "rev/CANSparkMax.h"
#include "units/angle.h"
#include "units/voltage.h"
#include "cowlib/SwerveModule.h" #include "cowlib/SwerveModule.h"
@ -25,3 +29,22 @@ SwerveModule::SwerveModule(int driveMotorId, int steerMotorId, int encoderId,
this->driveMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit); this->driveMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit);
this->steerMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit); this->steerMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit);
} }
void SwerveModule::drive(double speedMetersPerSecond, double angle) {
units::volt_t drive{(speedMetersPerSecond / maxVelocity) * maxVoltage};
units::volt_t steering{-pid.Calculate(
this->GetEncoder().convert<units::degree>().value(), angle)};
driveMotor.SetVoltage(drive);
steerMotor.SetVoltage(steering);
}
void SwerveModule::drive(frc::SwerveModuleState state) {
frc::SwerveModuleState optimized =
frc::SwerveModuleState::Optimize(state, frc::Rotation2d(GetEncoder()));
this->drive(optimized.speed.value(), optimized.angle.Degrees().value());
};
units::turn_t SwerveModule::GetEncoder() {
return this->encoder.GetAbsolutePosition().GetValue();
}

6
src/main/include/cowlib/SwerveModule.h
View file

@ -4,8 +4,10 @@
#pragma once #pragma once
#include "units/angle.h"
#include <ctre/phoenix6/CANcoder.hpp> #include <ctre/phoenix6/CANcoder.hpp>
#include <frc/controller/PIDController.h> #include <frc/controller/PIDController.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <rev/CANSparkMax.h> #include <rev/CANSparkMax.h>
class SwerveModule { class SwerveModule {
@ -20,4 +22,8 @@ private:
public: public:
SwerveModule(int driveMotorId, int steerMotorId, int encoderId, SwerveModule(int driveMotorId, int steerMotorId, int encoderId,
bool driveInverted, double maxVelocity, double maxVoltage); bool driveInverted, double maxVelocity, double maxVoltage);
void drive(double speedMetersPerSecond, double angle);
void drive(frc::SwerveModuleState state);
units::turn_t GetEncoder();
}; };