mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
69 lines
2.1 KiB
C++
69 lines
2.1 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 <frc/Joystick.h>
|
|
#include <frc/TimedRobot.h>
|
|
#include <frc/controller/SimpleMotorFeedforward.h>
|
|
#include <frc/trajectory/ExponentialProfile.h>
|
|
#include <units/acceleration.h>
|
|
#include <units/length.h>
|
|
#include <units/time.h>
|
|
#include <units/velocity.h>
|
|
#include <units/voltage.h>
|
|
|
|
#include "ExampleSmartMotorController.h"
|
|
|
|
class Robot : public frc::TimedRobot {
|
|
public:
|
|
static constexpr 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.
|
|
auto next = m_profile.Calculate(kDt, m_goal, m_setpoint);
|
|
|
|
// Send setpoint to offboard controller PID
|
|
m_motor.SetSetpoint(
|
|
ExampleSmartMotorController::PIDMode::kPosition,
|
|
m_setpoint.position.value(),
|
|
m_feedforward.Calculate(m_setpoint.velocity, next.velocity, kDt) /
|
|
12_V);
|
|
|
|
m_setpoint = next;
|
|
}
|
|
|
|
private:
|
|
frc::Joystick m_joystick{1};
|
|
ExampleSmartMotorController m_motor{1};
|
|
frc::SimpleMotorFeedforward<units::meters> m_feedforward{
|
|
// Note: These gains are fake, and will have to be tuned for your robot.
|
|
1_V, 1_V / 1_mps, 1_V / 1_mps_sq};
|
|
|
|
// Create a motion profile with the given maximum velocity and maximum
|
|
// acceleration constraints for the next setpoint.
|
|
frc::ExponentialProfile<units::meters, units::volts> m_profile{
|
|
{10_V, 1_V / 1_mps, 1_V / 1_mps_sq}};
|
|
frc::ExponentialProfile<units::meters, units::volts>::State m_goal;
|
|
frc::ExponentialProfile<units::meters, units::volts>::State m_setpoint;
|
|
};
|
|
|
|
#ifndef RUNNING_FRC_TESTS
|
|
int main() {
|
|
return frc::StartRobot<Robot>();
|
|
}
|
|
#endif
|