Add kinematics suite (#1787)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Prateek Machiraju
2019-09-08 00:11:49 -04:00
committed by Peter Johnson
parent 561cbbd144
commit f405582f86
67 changed files with 5060 additions and 0 deletions

View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "Drivetrain.h"
frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
return {units::meters_per_second_t(m_leftEncoder.GetRate()),
units::meters_per_second_t(m_rightEncoder.GetRate())};
}
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const auto leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.to<double>());
const auto rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.to<double>());
m_leftGroup.Set(leftOutput);
m_rightGroup.Set(rightOutput);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(GetAngle(), GetSpeeds());
}

View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include "Drivetrain.h"
class Robot : public frc::TimedRobot {
public:
void AutonomousPeriodic() override {
TeleopPeriodic();
m_drive.UpdateOdometry();
}
void TeleopPeriodic() override {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xSpeed =
-m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
Drivetrain::kMaxAngularSpeed;
m_drive.Drive(xSpeed, rot);
}
private:
frc::XboxController m_controller{0};
Drivetrain m_drive;
};
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* 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 <units/units.h>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/Spark.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/controller/PIDController.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <wpi/math>
/**
* Represents a differential drive style drivetrain.
*/
class Drivetrain {
public:
Drivetrain() {
m_gyro.Reset();
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
kEncoderResolution);
}
/**
* Get the robot angle as a Rotation2d.
*/
frc::Rotation2d GetAngle() const {
// Negating the angle because WPILib Gyros are CW positive.
return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
}
static constexpr units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
wpi::math::pi}; // 1/2 rotation per second
frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
void UpdateOdometry();
private:
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
frc::Spark m_leftMaster{1};
frc::Spark m_leftFollower{2};
frc::Spark m_rightMaster{3};
frc::Spark m_rightFollower{4};
frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
frc::Encoder m_leftEncoder{0, 1};
frc::Encoder m_rightEncoder{0, 1};
frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveOdometry m_odometry{m_kinematics};
};

View File

@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "Drivetrain.h"
frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
units::meters_per_second_t(m_frontRightEncoder.GetRate()),
units::meters_per_second_t(m_backLeftEncoder.GetRate()),
units::meters_per_second_t(m_backRightEncoder.GetRate())};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
const auto frontLeftOutput = m_frontLeftPIDController.Calculate(
m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to<double>());
const auto frontRightOutput = m_frontRightPIDController.Calculate(
m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
const auto backLeftOutput = m_backLeftPIDController.Calculate(
m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
const auto backRightOutput = m_backRightPIDController.Calculate(
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
m_frontLeftMotor.Set(frontLeftOutput);
m_frontRightMotor.Set(frontRightOutput);
m_backLeftMotor.Set(backLeftOutput);
m_backRightMotor.Set(backRightOutput);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, GetAngle())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
wheelSpeeds.Normalize(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(GetAngle(), GetCurrentState());
}

View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include "Drivetrain.h"
class Robot : public frc::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
m_mecanum.UpdateOdometry();
}
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
frc::XboxController m_controller{0};
Drivetrain m_mecanum;
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xSpeed =
-m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
const auto ySpeed =
-m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
Drivetrain::kMaxAngularSpeed;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
}
};
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* 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/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/Spark.h>
#include <frc/controller/PIDController.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/MecanumDriveKinematics.h>
#include <frc/kinematics/MecanumDriveOdometry.h>
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
#include <wpi/math>
/**
* Represents a mecanum drive style drivetrain.
*/
class Drivetrain {
public:
Drivetrain() { m_gyro.Reset(); }
/**
* Get the robot angle as a Rotation2d
*/
frc::Rotation2d GetAngle() const {
// Negating the angle because WPILib Gyros are CW positive.
return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
void UpdateOdometry();
static constexpr units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
wpi::math::pi}; // 1/2 rotation per second
private:
frc::Spark m_frontLeftMotor{1};
frc::Spark m_frontRightMotor{2};
frc::Spark m_backLeftMotor{3};
frc::Spark m_backRightMotor{4};
frc::Encoder m_frontLeftEncoder{0, 1};
frc::Encoder m_frontRightEncoder{0, 1};
frc::Encoder m_backLeftEncoder{0, 1};
frc::Encoder m_backRightEncoder{0, 1};
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::MecanumDriveOdometry m_odometry{m_kinematics};
};

View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "Drivetrain.h"
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto states = m_kinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, GetAngle())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_backLeft.SetDesiredState(bl);
m_backRight.SetDesiredState(br);
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(),
m_backLeft.GetState(), m_backRight.GetState());
}

View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include "Drivetrain.h"
class Robot : public frc::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
m_swerve.UpdateOdometry();
}
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
frc::XboxController m_controller{0};
Drivetrain m_swerve;
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xSpeed =
-m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
const auto ySpeed =
-m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
}
};
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

View File

@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "SwerveModule.h"
#include <frc/geometry/Rotation2d.h>
#include <wpi/math>
SwerveModule::SwerveModule(const int driveMotorChannel,
const int turningMotorChannel)
: m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) {
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_driveEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
kEncoderResolution);
// Set the distance (in this case, angle) per pulse for the turning encoder.
// This is the the angle through an entire rotation (2 * wpi::math::pi)
// divided by the encoder resolution.
m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution);
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
}
frc::SwerveModuleState SwerveModule::GetState() const {
return {units::meters_per_second_t{m_driveEncoder.GetRate()},
frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
}
void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), state.speed.to<double>());
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
m_turningEncoder.Get(),
// We have to convert to the meters type here because that's what
// ProfiledPIDController wants.
units::meter_t(state.angle.Radians().to<double>()));
// Set the motor outputs.
m_driveMotor.Set(driveOutput);
m_turningMotor.Set(turnOutput);
}

View File

@@ -0,0 +1,61 @@
/*----------------------------------------------------------------------------*/
/* 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/AnalogGyro.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveDriveOdometry.h>
#include <wpi/math>
#include "SwerveModule.h"
/**
* Represents a swerve drive style drivetrain.
*/
class Drivetrain {
public:
Drivetrain() { m_gyro.Reset(); }
/**
* Get the robot angle as a Rotation2d.
*/
frc::Rotation2d GetAngle() const {
// Negating the angle because WPILib Gyros are CW positive.
return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
}
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
void UpdateOdometry();
static constexpr units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
wpi::math::pi}; // 1/2 rotation per second
private:
frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m};
frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
SwerveModule m_frontLeft{1, 2};
SwerveModule m_frontRight{2, 3};
SwerveModule m_backLeft{5, 6};
SwerveModule m_backRight{7, 8};
frc::AnalogGyro m_gyro{0};
frc::SwerveDriveKinematics<4> m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
};

View File

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* 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/Spark.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <wpi/math>
class SwerveModule {
public:
SwerveModule(int driveMotorChannel, int turningMotorChannel);
frc::SwerveModuleState GetState() const;
void SetDesiredState(const frc::SwerveModuleState& state);
private:
static constexpr double kWheelRadius = 0.0508;
static constexpr int kEncoderResolution = 4096;
// We have to use meters here instead of radians because of the fact that
// ProfiledPIDController's constraints only take in meters per second and
// meters per second squared.
static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
units::meters_per_second_t(wpi::math::pi); // radians per second
static constexpr units::meters_per_second_squared_t
kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
wpi::math::pi * 2.0); // radians per second squared
frc::Spark m_driveMotor;
frc::Spark m_turningMotor;
frc::Encoder m_driveEncoder{0, 1};
frc::Encoder m_turningEncoder{0, 1};
frc2::PIDController m_drivePIDController{1.0, 0, 0};
frc::ProfiledPIDController m_turningPIDController{
1.0,
0.0,
0.0,
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
};

View File

@@ -328,5 +328,32 @@
],
"foldername": "GyroDriveCommands",
"mainclass": "Main"
},
{
"name": "SwerveBot",
"description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
"tags": [
"SwerveBot"
],
"foldername": "SwerveBot",
"gradlebase": "cpp"
},
{
"name": "MecanumBot",
"description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
"tags": [
"MecanumBot"
],
"foldername": "MecanumBot",
"gradlebase": "cpp"
},
{
"name": "DifferentialDriveBot",
"description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
"tags": [
"DifferentialDriveBot"
],
"foldername": "DifferentialDriveBot",
"gradlebase": "cpp"
}
]