Skip to content

Commit

Permalink
Use feedforward during driving (#177)
Browse files Browse the repository at this point in the history
* Add advantage-scope lilscoots model configuration

* update offsets and models

* start

* Use FF values

* Add constants for module max acceleration

---------

Co-authored-by: having11 <gatoninja236@gmail.com>
WowMuchDoge and having11 authored Jul 18, 2024
1 parent ca73733 commit a098977
Showing 13 changed files with 145 additions and 25 deletions.
85 changes: 85 additions & 0 deletions advantage-scope/config/config.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
{
"name": "Scoots",
"sourceUrl": "https://esko.onshape.com/documents/ed824b879f12a1905be71511/w/b6d4967fdcca0e3c193bd4dc/e/f9e6220adc6e2fa74aa1afa4",
"rotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "y",
"degrees": 0
},
{
"axis": "z",
"degrees": -90
}
],
"position": [ 0, 0, 0 ],
"cameras": [],
"components": [
{
"zeroedRotations": [
{
"axis": "x",
"degrees": -90
},
{
"axis": "y",
"degrees": -90
},
{
"axis": "z",
"degrees": -90
}
],
"zeroedPosition": [
0.8,
0.2,
0.2
]
},
{
"zeroedRotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "y",
"degrees": -15
},
{
"axis": "z",
"degrees": 180
}
],
"zeroedPosition": [
-0.12,
0.3,
0.07
]
},
{
"zeroedRotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "y",
"degrees": -12
},
{
"axis": "z",
"degrees": 180
}
],
"zeroedPosition": [
-0.1,
-0.3,
0.07
]
}
]
}
Binary file added advantage-scope/config/model.glb
Binary file not shown.
Binary file added advantage-scope/config/model_0.glb
Binary file not shown.
Binary file added advantage-scope/config/model_1.glb
Binary file not shown.
Binary file added advantage-scope/config/model_2.glb
Binary file not shown.
2 changes: 1 addition & 1 deletion src/main/cpp/RobotContainer.cpp
Original file line number Diff line number Diff line change
@@ -144,7 +144,7 @@ void RobotContainer::ConfigureButtonBindings() {
m_leds.Intaking()
.AndThen(m_leds.AngryFace())
.AndThen(IntakingCommands::Intake(&m_intake, &m_scoring))
.AndThen((m_leds.Loaded().AndThen(m_leds.HappyFace())).Unless([this] {
.AndThen((m_leds.Loaded().AndThen(m_leds.OwOFace())).Unless([this] {
return !m_intake.NotePresent();
})));

8 changes: 4 additions & 4 deletions src/main/cpp/subsystems/DriveSubsystem.cpp
Original file line number Diff line number Diff line change
@@ -31,13 +31,13 @@ using namespace DriveConstants;

DriveSubsystem::DriveSubsystem(PhotonVisionEstimators* vision)
: m_frontLeft{kFrontLeftDrivingCanId, kFrontLeftTurningCanId,
kFrontLeftChassisAngularOffset},
kFrontLeftChassisAngularOffset, m_frontLeftFF},
m_rearLeft{kRearLeftDrivingCanId, kRearLeftTurningCanId,
kRearLeftChassisAngularOffset},
kRearLeftChassisAngularOffset, m_rearLeftFF},
m_frontRight{kFrontRightDrivingCanId, kFrontRightTurningCanId,
kFrontRightChassisAngularOffset},
kFrontRightChassisAngularOffset, m_frontRightFF},
m_rearRight{kRearRightDrivingCanId, kRearRightTurningCanId,
kRearRightChassisAngularOffset},
kRearRightChassisAngularOffset, m_rearRightFF},
m_vision{vision} {
// put a field2d in NT
frc::SmartDashboard::PutData("Field", &m_field);
17 changes: 12 additions & 5 deletions src/main/cpp/subsystems/MAXSwerveModule.cpp
Original file line number Diff line number Diff line change
@@ -6,18 +6,21 @@

#include <frc/RobotBase.h>
#include <frc/geometry/Rotation2d.h>
#include <units/acceleration.h>

#include <numbers>

#include "Constants.h"

using namespace ModuleConstants;

MAXSwerveModule::MAXSwerveModule(const int drivingCANId, const int turningCANId,
const double chassisAngularOffset)
MAXSwerveModule::MAXSwerveModule(
const int drivingCANId, const int turningCANId,
const double chassisAngularOffset,
frc::SimpleMotorFeedforward<units::meters>& feedForward)
: m_drivingSparkMax(drivingCANId, rev::CANSparkBase::MotorType::kBrushless),
m_turningSparkMax(turningCANId,
rev::CANSparkBase::MotorType::kBrushless) {
m_turningSparkMax(turningCANId, rev::CANSparkBase::MotorType::kBrushless),
m_feedForward{feedForward} {
// Factory reset, so we get the SPARKS MAX to a known state before configuring
// them. This is useful in case a SPARK MAX is swapped out.

@@ -62,7 +65,11 @@ MAXSwerveModule::MAXSwerveModule(const int drivingCANId, const int turningCANId,
m_drivingPIDController.SetP(kDrivingP);
m_drivingPIDController.SetI(kDrivingI);
m_drivingPIDController.SetD(kDrivingD);
m_drivingPIDController.SetFF(kDrivingFF);

// TODO: might want to change the values here...
auto gain = m_feedForward.Calculate(DriveConstants::kMaxSpeed,
ModuleConstants::kMaxAccel);
m_drivingPIDController.SetFF(kDrivingFF + gain.value());
m_drivingPIDController.SetOutputRange(kDrivingMinOutput, kDrivingMaxOutput);

// Set the PID gains for the turning motor. Note these are example gains, and
27 changes: 15 additions & 12 deletions src/main/include/constants/CharacterizationConstants.h
Original file line number Diff line number Diff line change
@@ -1,27 +1,30 @@
#pragma once

#include <units/length.h>
#include <units/voltage.h>

namespace CharacterizationConstants {
namespace FrontLeftDrive {
constexpr double kS = 0.43779;
constexpr double kV = 2.4439;
constexpr double kA = 0.36441;
constexpr units::volt_t kS = 0.43779_V;
constexpr auto kV = 2.4439_V / 1_mps;
constexpr auto kA = 0.36441_V / 1_mps_sq;
} // namespace FrontLeftDrive

namespace FrontRightDrive {
constexpr double kS = 0.47478;
constexpr double kV = 2.4511;
constexpr double kA = 0.36441;
constexpr units::volt_t kS = 0.47478_V;
constexpr auto kV = 2.4511_V / 1_mps;
constexpr auto kA = 0.36441_V / 1_mps_sq;
} // namespace FrontRightDrive

namespace RearLeftDrive {
constexpr double kS = 0.43972;
constexpr double kV = 2.4352;
constexpr double kA = 0.35057;
constexpr units::volt_t kS = 0.43972_V;
constexpr auto kV = 2.4352_V / 1_mps;
constexpr auto kA = 0.35057_V / 1_mps_sq;
} // namespace RearLeftDrive

namespace RearRightDrive {
constexpr double kS = 0.41185;
constexpr double kV = 2.4218;
constexpr double kA = 0.41332;
constexpr units::volt_t kS = 0.41185_V;
constexpr auto kV = 2.4218_V / 1_mps;
constexpr auto kA = 0.41332_V / 1_mps_sq;
} // namespace RearRightDrive
} // namespace CharacterizationConstants
2 changes: 2 additions & 0 deletions src/main/include/constants/ModuleConstants.h
Original file line number Diff line number Diff line change
@@ -73,4 +73,6 @@ constexpr rev::CANSparkMax::IdleMode kTurningMotorIdleMode =

constexpr units::ampere_t kDrivingMotorCurrentLimit = 50_A;
constexpr units::ampere_t kTurningMotorCurrentLimit = 20_A;

constexpr units::acceleration::meters_per_second_squared_t kMaxAccel = 3_mps_sq;
} // namespace ModuleConstants
20 changes: 20 additions & 0 deletions src/main/include/subsystems/DriveSubsystem.h
Original file line number Diff line number Diff line change
@@ -183,6 +183,26 @@ class DriveSubsystem : public frc2::SubsystemBase {
MAXSwerveModule m_frontRight;
MAXSwerveModule m_rearRight;

frc::SimpleMotorFeedforward<units::meter> m_frontLeftFF{
CharacterizationConstants::FrontLeftDrive::kS,
CharacterizationConstants::FrontLeftDrive::kV,
CharacterizationConstants::FrontLeftDrive::kA};

frc::SimpleMotorFeedforward<units::meter> m_frontRightFF{
CharacterizationConstants::FrontRightDrive::kS,
CharacterizationConstants::FrontRightDrive::kV,
CharacterizationConstants::FrontRightDrive::kA};

frc::SimpleMotorFeedforward<units::meter> m_rearLeftFF{
CharacterizationConstants::RearLeftDrive::kS,
CharacterizationConstants::RearLeftDrive::kV,
CharacterizationConstants::RearLeftDrive::kA};

frc::SimpleMotorFeedforward<units::meter> m_rearRightFF{
CharacterizationConstants::RearRightDrive::kS,
CharacterizationConstants::RearRightDrive::kV,
CharacterizationConstants::RearRightDrive::kA};

uint8_t logCounter = 0;

ctre::phoenix6::hardware::Pigeon2 m_gyro1{CANConstants::kPigeonCanId1, "rio"};
7 changes: 5 additions & 2 deletions src/main/include/subsystems/MAXSwerveModule.h
Original file line number Diff line number Diff line change
@@ -4,6 +4,7 @@

#pragma once

#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
@@ -25,8 +26,8 @@ class MAXSwerveModule {
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
* Encoder.
*/
MAXSwerveModule(int driveCANId, int turningCANId,
double chassisAngularOffset);
MAXSwerveModule(int driveCANId, int turningCANId, double chassisAngularOffset,
frc::SimpleMotorFeedforward<units::meters>&);

/**
* Returns the current state of the module.
@@ -92,6 +93,8 @@ class MAXSwerveModule {
frc::SwerveModuleState m_desiredState{units::meters_per_second_t{0.0},
frc::Rotation2d()};

frc::SimpleMotorFeedforward<units::meters>& m_feedForward;

// Values for simulation
units::meter_t m_simDriveEncoderPosition;
units::meters_per_second_t m_simDriveEncoderVelocity;

0 comments on commit a098977

Please sign in to comment.