SwerveModule.h

Go to the documentation of this file.

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#pragma once

#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <frc/SmartDashboard/SmartDashboard.h>
#include <frc/Timer.h>
#include <networktables/NetworkTableEntry.h>
#include <wpi/math>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wattributes"

#include <rev\CANSparkMax.h>

#pragma GCC diagnostic pop

#include <ctre/phoenix/motorcontrol/can/TalonFX.h>
#include <ctre/phoenix/motorcontrol/StatorCurrentLimitConfiguration.h>

#include <string>

#include "Constants.h"
#include "common/Util.h"
#include "common/PIDLoaderFalcon.h"
#include "common/PIDLoaderNEO.h"
 
// Uncomment this to prevent swerve modules from driving
//#define DISABLE_DRIVE

using namespace rev;
using namespace ctre::phoenix::motorcontrol;
using namespace ctre::phoenix::motorcontrol::can;
using namespace ModuleConstants;
using namespace units;
using namespace std;
using namespace frc;

using GetPulseWidthCallback = function<double (CANifier::PWMChannel)>;


class SwerveModule
{
    using radians_per_second_squared_t = compound_unit<radians, inverse<squared<second>>>;

public:
    SwerveModule( int driveMotorChannel
                , int turningMotorChannel
                , GetPulseWidthCallback pulseWidthCallback
                , CANifier::PWMChannel pwmChannel
                , bool driveEncoderReversed
                , double offSet
                , const string& name);

    void Periodic();

    SwerveModuleState GetState();

    void SetDesiredState(SwerveModuleState &state);
    void ResetEncoders();
    void ResetRelativeToAbsolute();

private:
    double CalcAbsoluteAngle();
    double MinTurnRads(double init, double final, bool& bOutputReverse);
    meters_per_second_t CalcMetersPerSec();
    double CalcTicksPer100Ms(meters_per_second_t speed);

    double m_offset;
    string m_name;

    TalonFX m_driveMotor;
    CANSparkMax m_turningMotor;
    CANEncoder m_turnRelativeEncoder = m_turningMotor.GetAlternateEncoder(CANEncoder::AlternateEncoderType::kQuadrature, 
                                                                          ModuleConstants::kTurnEncoderCPR);
    CANPIDController m_turnPIDController = m_turningMotor.GetPIDController();

    PIDLoaderFalcon m_drivePIDLoader;
    PIDLoaderNEO m_turnPIDLoader;

    GetPulseWidthCallback m_pulseWidthCallback;
    CANifier::PWMChannel m_pwmChannel;

    Timer m_timer;
};