mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
64 lines
2.2 KiB
C++
64 lines
2.2 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include <numbers>
|
|
|
|
#include "ExampleSmartMotorController.hpp"
|
|
#include "wpi/driverstation/Joystick.hpp"
|
|
#include "wpi/framework/TimedRobot.hpp"
|
|
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
|
|
#include "wpi/math/trajectory/TrapezoidProfile.hpp"
|
|
#include "wpi/units/acceleration.hpp"
|
|
#include "wpi/units/length.hpp"
|
|
#include "wpi/units/time.hpp"
|
|
#include "wpi/units/velocity.hpp"
|
|
#include "wpi/units/voltage.hpp"
|
|
|
|
class Robot : public wpi::TimedRobot {
|
|
public:
|
|
static constexpr wpi::units::second_t kDt = 20_ms;
|
|
|
|
Robot() {
|
|
// Note: These gains are fake, and will have to be tuned for your robot.
|
|
m_motor.SetPID(1.3, 0.0, 0.7);
|
|
}
|
|
|
|
void TeleopPeriodic() override {
|
|
if (m_joystick.GetRawButtonPressed(2)) {
|
|
m_goal = {5_m, 0_mps};
|
|
} else if (m_joystick.GetRawButtonPressed(3)) {
|
|
m_goal = {0_m, 0_mps};
|
|
}
|
|
|
|
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
|
|
// toward the goal while obeying the constraints.
|
|
m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);
|
|
|
|
// Send setpoint to offboard controller PID
|
|
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
|
m_setpoint.position.value(),
|
|
m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
|
|
}
|
|
|
|
private:
|
|
wpi::Joystick m_joystick{1};
|
|
ExampleSmartMotorController m_motor{1};
|
|
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
|
|
// Note: These gains are fake, and will have to be tuned for your robot.
|
|
1_V, 1.5_V * 1_s / 1_m};
|
|
|
|
// Create a motion profile with the given maximum velocity and maximum
|
|
// acceleration constraints for the next setpoint.
|
|
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{
|
|
{1.75_mps, 0.75_mps_sq}};
|
|
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_goal;
|
|
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_setpoint;
|
|
};
|
|
|
|
#ifndef RUNNING_WPILIB_TESTS
|
|
int main() {
|
|
return wpi::StartRobot<Robot>();
|
|
}
|
|
#endif
|