/*----------------------------------------------------------------------------*/ /* Copyright (c) 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 namespace frc::sim { class DifferentialDrivetrainSim { public: /** * Create a SimDrivetrain. * * @param drivetrainPlant The LinearSystem representing the robot's * drivetrain. This system can be created with * LinearSystemId#createDrivetrainVelocitySystem or * LinearSystemId#identifyDrivetrainSystem. * @param trackWidth The robot's track width. * @param driveMotor A {@link DCMotor} representing the left side of * the drivetrain. * @param gearingRatio The gearingRatio ratio of the left side, as output * over input. This must be the same ratio as the ratio used to identify or * create the drivetrainPlant. * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in * meters. */ DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth, DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius); /** * Create a SimDrivetrain. * * @param driveMotor A {@link DCMotor} representing the left side of the * drivetrain. * @param gearing The gearing on the drive between motor and wheel, as * output over input. This must be the same ratio as the ratio used to * identify or create the drivetrainPlant. * @param J The moment of inertia of the drivetrain about its * center. * @param mass The mass of the drivebase. * @param wheelRadius The radius of the wheels on the drivetrain. * @param trackWidth The robot's track width, or distance between left and * right wheels. */ DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, units::kilogram_t mass, units::meter_t wheelRadius, units::meter_t trackWidth); /** * Set the applied voltage to the drivetrain. Note that positive voltage must * make that side of the drivetrain travel forward (+X). * * @param leftVoltage The left voltage. * @param rightVoltage The right voltage. */ void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage); /** * Set the gearing reduction on the drivetrain. This is commonly used for * shifting drivetrains. * * @param newGearing The new gear ratio, as output over input. */ void SetGearing(double newGearing); /** * Updates the simulation. * * @param dt The time that's passed since the last {@link #update(double)} * call. */ void Update(units::second_t dt); /** * Returns an element of the state vector. * * @param state The row of the state vector. */ double GetState(int state) const; /** * Get the current gearing reduction of the drivetrain, as output over input. */ double GetGearing() const; /** * Returns the current state vector x. */ Eigen::Matrix GetState() const; /** * Get the estimated direction the robot is pointing. Note that this angle is * counterclockwise-positive, while most gyros are clockwise positive. */ Rotation2d GetHeading() const; /** * Returns the current estimated position. */ Pose2d GetEstimatedPosition() const; /** * Returns the currently drawn current. */ units::ampere_t GetCurrentDraw() const; Eigen::Matrix Dynamics(const Eigen::Matrix& x, const Eigen::Matrix& u); class State { public: static constexpr int kX = 0; static constexpr int kY = 1; static constexpr int kHeading = 2; static constexpr int kLeftVelocity = 3; static constexpr int kRightVelocity = 4; static constexpr int kLeftPosition = 5; static constexpr int kRightPosition = 6; }; /** * Represents a gearing option of the Toughbox mini. * 12.75:1 -- 14:50 and 14:50 * 10.71:1 -- 14:50 and 16:48 * 8.45:1 -- 14:50 and 19:45 * 7.31:1 -- 14:50 and 21:43 * 5.95:1 -- 14:50 and 24:40 */ class KitbotGearing { public: static constexpr double k12p75 = 12.75; static constexpr double k10p71 = 10.71; static constexpr double k8p45 = 8.45; static constexpr double k7p31 = 7.31; static constexpr double k5p95 = 5.95; }; class KitbotMotor { public: static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1); static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2); static constexpr frc::DCMotor SingleMiniCIMPerSide = frc::DCMotor::MiniCIM(1); static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2); }; class KitbotWheelSize { public: static constexpr units::meter_t kSixInch = 6_in; static constexpr units::meter_t kEightInch = 8_in; static constexpr units::meter_t kTenInch = 10_in; }; /** * Create a sim for the standard FRC kitbot. * * @param motor The motors installed in the bot. * @param gearing The gearing reduction used. * @param wheelSize The wheel size. */ static DifferentialDrivetrainSim createKitbotSim(frc::DCMotor motor, double gearing, units::meter_t wheelSize) { // MOI estimation -- note that I = m r^2 for point masses units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in; units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) * 2 // CIM plus toughbox per side * (26_in / 2) * (26_in / 2); return DifferentialDrivetrainSim{ motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in}; } /** * Create a sim for the standard FRC kitbot. * * @param motor The motors installed in the bot. * @param gearing The gearing reduction used. * @param wheelSize The wheel size. * @param J The moment of inertia of the drivebase. This can be * calculated using frc-characterization. */ static DifferentialDrivetrainSim createKitbotSim( frc::DCMotor motor, double gearing, units::meter_t wheelSize, units::kilogram_square_meter_t J) { return DifferentialDrivetrainSim{motor, gearing, J, 25_kg, wheelSize / 2.0, 26_in}; } private: const LinearSystem<2, 2, 2>& m_plant; units::meter_t m_rb; units::meter_t m_wheelRadius; DCMotor m_motor; double m_originalGearing; double m_currentGearing; Eigen::Matrix m_x; Eigen::Matrix m_u; }; } // namespace frc::sim