DriveSubsystem.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/Encoder.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveDriveOdometry.h>
#include <frc/SmartDashBoard/SmartDashboard.h>
#include <frc2/command/SubsystemBase.h>

#include <ctre/phoenix/CANifier.h>

#include "common/Util.h"
#include "common/Gyro.h"

#include "Constants.h"
#include "SwerveModule.h"

// Uncomment to directly set states to each module
//#define MANUAL_MODULE_STATES
// Uncomment to tune Rotation Drive PIDs
//#define TUNE_ROTATION_DRIVE

using namespace ctre::phoenix;
using namespace DriveConstants;
using namespace std;
using namespace frc;

class DriveSubsystem : public frc2::SubsystemBase
{
public:
    enum ModuleLocation    
    {
        kFrontLeft,
        kFrontRight,
        kRearLeft,
        kRearRight
    };

    DriveSubsystem(Gyro *gyro);

    void Periodic() override;

    // Subsystem methods go here.

    void Drive(meters_per_second_t xSpeed, meters_per_second_t ySpeed, radians_per_second_t rot, bool fieldRelative);

    // Drives the robot with the right stick controlling the position angle of the robot
    void RotationDrive(meters_per_second_t xSpeed, meters_per_second_t ySpeed, radian_t rot, bool fieldRelative);

    // Drives the robot with the right stick controlling the position angle of the robot
    void RotationDrive(meters_per_second_t xSpeed, meters_per_second_t ySpeed, double xRot, double yRot, bool fieldRelative);

    void HeadingDrive(meters_per_second_t xSpeed, meters_per_second_t ySpeed, radians_per_second_t rot, bool fieldRelative);

    void UpdateLastHeading();

    void ResetEncoders();

    using SwerveModuleStates = wpi::array<SwerveModuleState, DriveConstants::kNumSwerveModules>;
    void SetModuleStates(SwerveModuleStates desiredStates);

    Pose2d GetPose();

    double PWMToPulseWidth(CANifier::PWMChannel pwmChannel);

    void ResetOdometry(Pose2d pose);

    void WheelsForward();

    void ResetRelativeToAbsolute();

    SwerveDriveKinematics<kNumSwerveModules> kDriveKinematics{
        Translation2d( kWheelBase / 2,  kTrackWidth / 2),    // +x, +y FL
        Translation2d( kWheelBase / 2, -kTrackWidth / 2),    // +x, -y FR
        Translation2d(-kWheelBase / 2,  kTrackWidth / 2),    // -x, +y RL
        Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};   // -x, -y RR

private:    
    SwerveModuleStates getCurrentWheelSpeeds()
    {
        SwerveModuleStates sms({
            m_frontLeft.GetState(),
            m_frontRight.GetState(),
            m_rearLeft.GetState(),
            m_rearRight.GetState()
        });
        return sms;
    }

    SwerveModule m_frontLeft;
    SwerveModule m_frontRight;
    SwerveModule m_rearRight;
    SwerveModule m_rearLeft;

    CANifier m_canifier;
    Gyro *m_gyro;
    SwerveDriveOdometry<DriveConstants::kNumSwerveModules> m_odometry;

    frc2::PIDController m_rotationPIDController{
        DriveConstants::kRotationDriveP,
        DriveConstants::kRotationDriveI,
        DriveConstants::kRotationDriveD
    };
    double m_lastHeading;
    bool m_rotationalInput;
};