diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp deleted file mode 100644 index 712bc16683..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019-2020 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" - -#include - -void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { - auto leftFeedforward = m_feedforward.Calculate(speeds.left); - auto rightFeedforward = m_feedforward.Calculate(speeds.right); - double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), - speeds.left.to()); - double rightOutput = m_rightPIDController.Calculate( - m_rightEncoder.GetRate(), speeds.right.to()); - - m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); - m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); -} - -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(m_gyro.GetRotation2d(), - units::meter_t(m_leftEncoder.GetDistance()), - units::meter_t(m_rightEncoder.GetDistance())); -} - -void Drivetrain::SimulationPeriodic() { - // To update our simulation, we set motor voltage inputs, update the - // simulation, and write the simulated positions and velocities to our - // simulated encoder and gyro. We negate the right side so that positive - // voltages make the right side move forward. - m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} * - frc::RobotController::GetInputVoltage(), - units::volt_t{-m_rightLeader.Get()} * - frc::RobotController::GetInputVoltage()); - m_drivetrainSimulator.Update(20_ms); - - m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kLeftPosition)); - m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity)); - m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kRightPosition)); - m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kRightVelocity)); - m_gyroSim.SetAngle( - -m_drivetrainSimulator.GetHeading().Degrees().to()); - - m_fieldSim.SetRobotPose(m_odometry.GetPose()); -} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp deleted file mode 100644 index 195e1cdde9..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019-2020 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 -#include -#include - -#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_speedLimiter.Calculate( - 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. - auto rot = -m_rotLimiter.Calculate( - m_controller.GetX(frc::GenericHID::kRightHand)) * - Drivetrain::kMaxAngularSpeed; - - m_drive.Drive(xSpeed, rot); - } - - void SimulationPeriodic() override { m_drive.SimulationPeriodic(); } - - private: - frc::XboxController m_controller{0}; - - // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 - // to 1. - frc::SlewRateLimiter m_speedLimiter{3 / 1_s}; - frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; - - Drivetrain m_drive; -}; - -#ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h deleted file mode 100644 index 48efbc6df9..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h +++ /dev/null @@ -1,99 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019-2020 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * 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); - - m_leftEncoder.Reset(); - m_rightEncoder.Reset(); - } - - 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 - - void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); - void Drive(units::meters_per_second_t xSpeed, - units::radians_per_second_t rot); - void UpdateOdometry(); - - void SimulationPeriodic(); - - private: - static constexpr units::meter_t kTrackWidth = 0.381_m * 2; - static constexpr double kWheelRadius = 0.0508; // meters - static constexpr int kEncoderResolution = 4096; - - frc::PWMVictorSPX m_leftLeader{1}; - frc::PWMVictorSPX m_leftFollower{2}; - frc::PWMVictorSPX m_rightLeader{3}; - frc::PWMVictorSPX m_rightFollower{4}; - - frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; - frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; - - frc::Encoder m_leftEncoder{0, 1}; - frc::Encoder m_rightEncoder{2, 3}; - - 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_gyro.GetRotation2d()}; - - // Gains are for example purposes only - must be determined for your own - // robot! - frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; - - // Simulation classes help us simulate our robot - frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; - frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; - frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; - frc::Field2d m_fieldSim{}; - frc::LinearSystem<2, 2, 2> m_drivetrainSystem = - frc::LinearSystemId::IdentifyDrivetrainSystem( - 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_rad_per_s, - 0.3_V / 1_rad_per_s_sq); - frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp new file mode 100644 index 0000000000..75849f12db --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "Constants.h" + +using namespace DriveConstants; + +const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics( + kTrackwidth); + +const frc::LinearSystem<2, 2, 2> DriveConstants::kDrivetrainPlant = + frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..26d7499969 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp @@ -0,0 +1,85 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 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 "Robot.h" + +#include +#include +#include +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() { + frc2::CommandScheduler::GetInstance().CancelAll(); +} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Schedule(); + } +} + +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +void Robot::SimulationPeriodic() { + // Here we calculate the battery voltage based on drawn current. + // As our robot draws more power from the battery its voltage drops. + // The estimated voltage is highly dependent on the battery's internal + // resistance. + auto current = m_container.GetRobotDrive().GetCurrentDraw(); + auto loadedVoltage = frc::sim::BatterySim::Calculate({current}); + frc::sim::RoboRioSim::SetVInVoltage(loadedVoltage); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..760812278e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "RobotContainer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +const DriveSubsystem& RobotContainer::GetRobotDrive() const { return m_drive; } + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Create a voltage constraint to ensure we don't accelerate too fast + frc::DifferentialDriveVoltageConstraint autoVoltageConstraint( + frc::SimpleMotorFeedforward( + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka), + DriveConstants::kDriveKinematics, 10_V); + + // Set up config for trajectory + frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, + AutoConstants::kMaxAcceleration); + // Add kinematics to ensure max speed is actually obeyed + config.SetKinematics(DriveConstants::kDriveKinematics); + // Apply the voltage constraint + config.AddConstraint(autoVoltageConstraint); + + // An example trajectory to follow. All units in meters. + auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( + // Start at the origin facing the +X direction + frc::Pose2d(), + // Pass through these two interior waypoints, making an 's' curve path + {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)}, + // End 3 meters straight ahead of where we started, facing forward + frc::Pose2d(3_m, 0_m, 0_deg), + // Pass the config + config); + + frc2::RamseteCommand ramseteCommand( + exampleTrajectory, [this] { return m_drive.GetPose(); }, + frc::RamseteController(AutoConstants::kRamseteB, + AutoConstants::kRamseteZeta), + frc::SimpleMotorFeedforward( + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka), + DriveConstants::kDriveKinematics, + [this] { return m_drive.GetWheelSpeeds(); }, + frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), + frc2::PIDController(DriveConstants::kPDriveVel, 0, 0), + [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); }, + {&m_drive}); + + // Reset odometry to starting pose of trajectory. + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + + // no auto + return new frc2::SequentialCommandGroup( + std::move(ramseteCommand), + frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {})); +} diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..d8cc462bac --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 "subsystems/DriveSubsystem.h" + +#include +#include +#include +#include +#include + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() { + // Set the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + + ResetEncoders(); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. + m_odometry.Update(m_gyro.GetRotation2d(), + units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); +} + +void DriveSubsystem::SimulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} * + frc::RobotController::GetInputVoltage(), + units::volt_t{-m_rightMotors.Get()} * + frc::RobotController::GetInputVoltage()); + m_drivetrainSimulator.Update(20_ms); + + m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState( + frc::sim::DifferentialDrivetrainSim::State::kLeftPosition)); + m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState( + frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity)); + m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState( + frc::sim::DifferentialDrivetrainSim::State::kRightPosition)); + m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState( + frc::sim::DifferentialDrivetrainSim::State::kRightVelocity)); + m_gyroAngleSim.SetAngle( + -m_drivetrainSimulator.GetHeading().Degrees().to()); + + m_fieldSim.SetRobotPose(m_odometry.GetPose()); +} + +units::ampere_t DriveSubsystem::GetCurrentDraw() const { + return m_drivetrainSimulator.GetCurrentDraw(); +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) { + m_leftMotors.SetVoltage(left); + m_rightMotors.SetVoltage(-right); + m_drive.Feed(); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} + +units::degree_t DriveSubsystem::GetHeading() const { + return m_gyro.GetRotation2d().Degrees(); +} + +double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); } + +frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } + +frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() { + return {units::meters_per_second_t(m_leftEncoder.GetRate()), + units::meters_per_second_t(m_rightEncoder.GetRate())}; +} + +void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + ResetEncoders(); + m_drivetrainSimulator.SetPose(pose); + m_odometry.ResetPosition(pose, m_gyro.GetRotation2d()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h new file mode 100644 index 0000000000..77e11841f3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +constexpr int kLeftMotor1Port = 0; +constexpr int kLeftMotor2Port = 1; +constexpr int kRightMotor1Port = 2; +constexpr int kRightMotor2Port = 3; + +constexpr int kLeftEncoderPorts[]{0, 1}; +constexpr int kRightEncoderPorts[]{2, 3}; +constexpr bool kLeftEncoderReversed = false; +constexpr bool kRightEncoderReversed = true; + +constexpr auto kTrackwidth = 0.69_m; +extern const frc::DifferentialDriveKinematics kDriveKinematics; + +constexpr int kEncoderCPR = 1024; +constexpr auto kWheelDiameter = 6_in; +constexpr double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameter.to() * wpi::math::pi) / + static_cast(kEncoderCPR); + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The Robot Characterization +// Toolsuite provides a convenient tool for obtaining these values for your +// robot. +constexpr auto ks = 0.22_V; +constexpr auto kv = 1.98 * 1_V * 1_s / 1_m; +constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m; + +constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad; +constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad; + +extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant; + +// Example values only -- use what's on your physical robot! +constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2); +constexpr auto kDrivetrainGearing = 8.0; + +// Example value only - as above, this must be tuned for your drive! +constexpr double kPDriveVel = 8.5; +} // namespace DriveConstants + +namespace AutoConstants { +constexpr auto kMaxSpeed = 3_mps; +constexpr auto kMaxAcceleration = 3_mps_sq; + +// Reasonable baseline values for a RAMSETE follower in units of meters and +// seconds +constexpr double kRamseteB = 2; +constexpr double kRamseteZeta = 0.7; +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h new file mode 100644 index 0000000000..50509994db --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 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 +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TestPeriodic() override; + void SimulationPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h new file mode 100644 index 0000000000..06cbf7cfc2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + const DriveSubsystem& GetRobotDrive() const; + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..2cbc9908a2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h @@ -0,0 +1,175 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + /** + * Will be called periodically during simulation. + */ + void SimulationPeriodic() override; + + // Subsystem methods go here. + + units::ampere_t GetCurrentDraw() const; + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Controls each side of the drive directly with a voltage. + * + * @param left the commanded left output + * @param right the commanded right output + */ + void TankDriveVolts(units::volt_t left, units::volt_t right); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from -180 to 180 + */ + units::degree_t GetHeading() const; + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + frc::Pose2d GetPose(); + + /** + * Returns the current wheel speeds of the robot. + * + * @return The current wheel speeds. + */ + frc::DifferentialDriveWheelSpeeds GetWheelSpeeds(); + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + void ResetOdometry(frc::Pose2d pose); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1{DriveConstants::kLeftMotor1Port}; + frc::PWMVictorSPX m_left2{DriveConstants::kLeftMotor2Port}; + frc::PWMVictorSPX m_right1{DriveConstants::kRightMotor1Port}; + frc::PWMVictorSPX m_right2{DriveConstants::kRightMotor2Port}; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // The left-side drive encoder + frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0], + DriveConstants::kLeftEncoderPorts[1]}; + + // The right-side drive encoder + frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0], + DriveConstants::kRightEncoderPorts[1]}; + + // The gyro sensor + frc::AnalogGyro m_gyro{0}; + + // Odometry class for tracking robot pose + frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()}; + + // These classes help simulate our drivetrain. + frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ + DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth, + DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing, + DriveConstants::kWheelDiameter / 2}; + + frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; + frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; + frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro}; + + // The Field2d class simulates the field in the sim GUI. Note that we can have + // only one instance! + frc::Field2d m_fieldSim; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index d3d2e67bd8..93bfad462a 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -657,7 +657,7 @@ "Drivetrain", "Field2d" ], - "foldername": "DifferentialDriveSimulation", + "foldername": "StateSpaceDifferentialDriveSimulation", "gradlebase": "cpp", "mainclass": "Main", "commandversion": 2