mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[documentation] Remove more outdated commands examples (#7054)
There are still some examples we'd like to remove here (eg Hatchbot traditional) but this is a good start with not too many changes required in frc-docs.
This commit is contained in:
Jade
@@ -1,74 +0,0 @@
|
||||
// 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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> 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() {
|
||||
m_container.DisablePIDSubsystems();
|
||||
}
|
||||
|
||||
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) {
|
||||
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) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,65 +0,0 @@
|
||||
// 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 "RobotContainer.h"
|
||||
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
namespace ac = AutoConstants;
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::cmd::Run(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
|
||||
m_driverController.A().OnTrue(frc2::cmd::RunOnce(
|
||||
[this] {
|
||||
m_arm.SetGoal(2_rad);
|
||||
m_arm.Enable();
|
||||
},
|
||||
{&m_arm}));
|
||||
|
||||
// Move the arm to neutral position when the 'B' button is pressed.
|
||||
m_driverController.B().OnTrue(frc2::cmd::RunOnce(
|
||||
[this] {
|
||||
m_arm.SetGoal(ArmConstants::kArmOffset);
|
||||
m_arm.Enable();
|
||||
},
|
||||
{&m_arm}));
|
||||
|
||||
// Disable the arm controller when Y is pressed.
|
||||
m_driverController.Y().OnTrue(
|
||||
frc2::cmd::RunOnce([this] { m_arm.Disable(); }, {&m_arm}));
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
m_driverController.RightBumper()
|
||||
.OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }))
|
||||
.OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }));
|
||||
}
|
||||
|
||||
void RobotContainer::DisablePIDSubsystems() {
|
||||
m_arm.Disable();
|
||||
}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
return frc2::cmd::None();
|
||||
}
|
||||
@@ -1,34 +0,0 @@
|
||||
// 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 "subsystems/ArmSubsystem.h"
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace ArmConstants;
|
||||
using State = frc::TrapezoidProfile<units::radians>::State;
|
||||
|
||||
ArmSubsystem::ArmSubsystem()
|
||||
: frc2::ProfiledPIDSubsystem<units::radians>(
|
||||
frc::ProfiledPIDController<units::radians>(
|
||||
kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
|
||||
m_motor(kMotorPort),
|
||||
m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
|
||||
m_feedforward(kS, kG, kV, kA) {
|
||||
m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
|
||||
// Start arm in neutral position
|
||||
SetGoal(State{kArmOffset, 0_rad_per_s});
|
||||
}
|
||||
|
||||
void ArmSubsystem::UseOutput(double output, State setpoint) {
|
||||
// Calculate the feedforward from the sepoint
|
||||
units::volt_t feedforward =
|
||||
m_feedforward.Calculate(setpoint.position, setpoint.velocity);
|
||||
// Add the feedforward to the PID output to get the motor output
|
||||
m_motor.SetVoltage(units::volt_t{output} + feedforward);
|
||||
}
|
||||
|
||||
units::radian_t ArmSubsystem::GetMeasurement() {
|
||||
return units::radian_t{m_encoder.GetDistance()} + kArmOffset;
|
||||
}
|
||||
@@ -1,59 +0,0 @@
|
||||
// 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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_left1{kLeftMotor1Port},
|
||||
m_left2{kLeftMotor2Port},
|
||||
m_right1{kRightMotor1Port},
|
||||
m_right2{kRightMotor2Port},
|
||||
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
|
||||
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_right1.SetInverted(true);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
}
|
||||
|
||||
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
|
||||
m_drive.ArcadeDrive(fwd, rot);
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetAverageEncoderDistance() {
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetLeftEncoder() {
|
||||
return m_leftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRightEncoder() {
|
||||
return m_rightEncoder;
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMaxOutput(double maxOutput) {
|
||||
m_drive.SetMaxOutput(maxOutput);
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/time.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr double kWheelDiameterInches = 6;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameterInches * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace ArmConstants {
|
||||
inline constexpr int kMotorPort = 4;
|
||||
|
||||
inline constexpr double kP = 1;
|
||||
|
||||
// These are fake gains; in actuality these must be determined individually for
|
||||
// each robot
|
||||
inline constexpr auto kS = 1_V;
|
||||
inline constexpr auto kG = 1_V;
|
||||
inline constexpr auto kV = 0.5_V * 1_s / 1_rad;
|
||||
inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
|
||||
|
||||
inline constexpr auto kMaxVelocity = 3_rad_per_s;
|
||||
inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
|
||||
|
||||
inline constexpr int kEncoderPorts[]{4, 5};
|
||||
inline constexpr int kEncoderPPR = 256;
|
||||
inline constexpr auto kEncoderDistancePerPulse =
|
||||
2.0_rad * std::numbers::pi / kEncoderPPR;
|
||||
|
||||
// The offset of the arm from the horizontal in its neutral position,
|
||||
// measured from the horizontal
|
||||
inline constexpr auto kArmOffset = 0.5_rad;
|
||||
} // namespace ArmConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kAutoTimeoutSeconds = 12_s;
|
||||
inline constexpr auto kAutoShootTimeSeconds = 7_s;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,46 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/button/CommandXboxController.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/ArmSubsystem.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();
|
||||
|
||||
/**
|
||||
* Disables all ProfiledPIDSubsystem and PIDSubsystem instances.
|
||||
* This should be called on robot disable to prevent integral windup.
|
||||
*/
|
||||
void DisablePIDSubsystems();
|
||||
|
||||
frc2::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc2::CommandXboxController m_driverController{
|
||||
OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
ArmSubsystem m_arm;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/ArmFeedforward.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/ProfiledPIDSubsystem.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
/**
|
||||
* A robot arm subsystem that moves with a motion profile.
|
||||
*/
|
||||
class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
|
||||
using State = frc::TrapezoidProfile<units::radians>::State;
|
||||
|
||||
public:
|
||||
ArmSubsystem();
|
||||
|
||||
void UseOutput(double output, State setpoint) override;
|
||||
|
||||
units::radian_t GetMeasurement() override;
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_motor;
|
||||
frc::Encoder m_encoder;
|
||||
frc::ArmFeedforward m_feedforward;
|
||||
};
|
||||
@@ -1,86 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
*/
|
||||
void ArcadeDrive(double fwd, double rot);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1;
|
||||
frc::PWMSparkMax m_left2;
|
||||
frc::PWMSparkMax m_right1;
|
||||
frc::PWMSparkMax m_right2;
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder;
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> 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() {}
|
||||
|
||||
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) {
|
||||
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) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,40 +0,0 @@
|
||||
// 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 "RobotContainer.h"
|
||||
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
|
||||
[this] { return -m_driverController.GetLeftY(); },
|
||||
[this] { return -m_driverController.GetRightX(); }));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
|
||||
m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad));
|
||||
|
||||
// Move the arm to neutral position when the 'B' button is pressed.
|
||||
m_driverController.B().OnTrue(
|
||||
m_arm.SetArmGoalCommand(ArmConstants::kArmOffset));
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
m_driverController.RightBumper()
|
||||
.OnTrue(m_drive.SetMaxOutputCommand(0.5))
|
||||
.OnFalse(m_drive.SetMaxOutputCommand(1.0));
|
||||
}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
return frc2::cmd::None();
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
// 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 "subsystems/ArmSubsystem.h"
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace ArmConstants;
|
||||
using State = frc::TrapezoidProfile<units::radians>::State;
|
||||
|
||||
ArmSubsystem::ArmSubsystem()
|
||||
: frc2::TrapezoidProfileSubsystem<units::radians>(
|
||||
{kMaxVelocity, kMaxAcceleration}, kArmOffset),
|
||||
m_motor(kMotorPort),
|
||||
m_feedforward(kS, kG, kV, kA) {
|
||||
m_motor.SetPID(kP, 0, 0);
|
||||
}
|
||||
|
||||
void ArmSubsystem::UseState(State setpoint) {
|
||||
// Calculate the feedforward from the sepoint
|
||||
units::volt_t feedforward =
|
||||
m_feedforward.Calculate(setpoint.position, setpoint.velocity);
|
||||
// Add the feedforward to the PID output to get the motor output
|
||||
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
||||
setpoint.position.value(), feedforward / 12_V);
|
||||
}
|
||||
|
||||
frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) {
|
||||
return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this});
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
// 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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_left1{kLeftMotor1Port},
|
||||
m_left2{kLeftMotor2Port},
|
||||
m_right1{kRightMotor1Port},
|
||||
m_right2{kRightMotor2Port},
|
||||
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
|
||||
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_right1.SetInverted(true);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
frc2::CommandPtr DriveSubsystem::SetMaxOutputCommand(double maxOutput) {
|
||||
return frc2::cmd::RunOnce(
|
||||
[this, maxOutput] { m_drive.SetMaxOutput(maxOutput); });
|
||||
}
|
||||
|
||||
frc2::CommandPtr DriveSubsystem::ArcadeDriveCommand(
|
||||
std::function<double()> fwd, std::function<double()> rot) {
|
||||
return frc2::cmd::Run(
|
||||
[this, fwd = std::move(fwd), rot = std::move(rot)] {
|
||||
m_drive.ArcadeDrive(fwd(), rot());
|
||||
},
|
||||
{this});
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/time.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr double kWheelDiameterInches = 6;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameterInches * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace ArmConstants {
|
||||
inline constexpr int kMotorPort = 4;
|
||||
|
||||
inline constexpr double kP = 1;
|
||||
|
||||
// These are fake gains; in actuality these must be determined individually for
|
||||
// each robot
|
||||
inline constexpr auto kS = 1_V;
|
||||
inline constexpr auto kG = 1_V;
|
||||
inline constexpr auto kV = 0.5_V * 1_s / 1_rad;
|
||||
inline constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
|
||||
|
||||
inline constexpr auto kMaxVelocity = 3_rad_per_s;
|
||||
inline constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
|
||||
|
||||
inline constexpr int kEncoderPorts[]{4, 5};
|
||||
inline constexpr int kEncoderPPR = 256;
|
||||
inline constexpr auto kEncoderDistancePerPulse =
|
||||
2.0_rad * std::numbers::pi / kEncoderPPR;
|
||||
|
||||
// The offset of the arm from the horizontal in its neutral position,
|
||||
// measured from the horizontal
|
||||
inline constexpr auto kArmOffset = 0.5_rad;
|
||||
} // namespace ArmConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kAutoTimeoutSeconds = 12_s;
|
||||
inline constexpr auto kAutoShootTimeSeconds = 7_s;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,80 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* A simplified stub class that simulates the API of a common "smart" motor
|
||||
* controller.
|
||||
*
|
||||
* <p>Has no actual functionality.
|
||||
*/
|
||||
class ExampleSmartMotorController {
|
||||
public:
|
||||
enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
|
||||
|
||||
/**
|
||||
* Creates a new ExampleSmartMotorController.
|
||||
*
|
||||
* @param port The port for the controller.
|
||||
*/
|
||||
explicit ExampleSmartMotorController(int port) {}
|
||||
|
||||
/**
|
||||
* Example method for setting the PID gains of the smart controller.
|
||||
*
|
||||
* @param kp The proportional gain.
|
||||
* @param ki The integral gain.
|
||||
* @param kd The derivative gain.
|
||||
*/
|
||||
void SetPID(double kp, double ki, double kd) {}
|
||||
|
||||
/**
|
||||
* Example method for setting the setpoint of the smart controller in PID
|
||||
* mode.
|
||||
*
|
||||
* @param mode The mode of the PID controller.
|
||||
* @param setpoint The controller setpoint.
|
||||
* @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
|
||||
*/
|
||||
void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
|
||||
|
||||
/**
|
||||
* Places this motor controller in follower mode.
|
||||
*
|
||||
* @param leader The leader to follow.
|
||||
*/
|
||||
void Follow(ExampleSmartMotorController leader) {}
|
||||
|
||||
/**
|
||||
* Returns the encoder distance.
|
||||
*
|
||||
* @return The current encoder distance.
|
||||
*/
|
||||
double GetEncoderDistance() { return 0; }
|
||||
|
||||
/**
|
||||
* Returns the encoder rate.
|
||||
*
|
||||
* @return The current encoder rate.
|
||||
*/
|
||||
double GetEncoderRate() { return 0; }
|
||||
|
||||
/**
|
||||
* Resets the encoder to zero distance.
|
||||
*/
|
||||
void ResetEncoder() {}
|
||||
|
||||
void Set(double speed) {}
|
||||
|
||||
double Get() const { return 0; }
|
||||
|
||||
void SetInverted(bool isInverted) {}
|
||||
|
||||
bool GetInverted() const { return false; }
|
||||
|
||||
void Disable() {}
|
||||
|
||||
void StopMotor() {}
|
||||
};
|
||||
@@ -1,32 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,42 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/button/CommandXboxController.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/ArmSubsystem.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
namespace ac = AutoConstants;
|
||||
|
||||
/**
|
||||
* 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::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc2::CommandXboxController m_driverController{
|
||||
OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
ArmSubsystem m_arm;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/controller/ArmFeedforward.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/TrapezoidProfileSubsystem.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
#include "ExampleSmartMotorController.h"
|
||||
|
||||
/**
|
||||
* A robot arm subsystem that moves with a motion profile.
|
||||
*/
|
||||
class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
|
||||
using State = frc::TrapezoidProfile<units::radians>::State;
|
||||
|
||||
public:
|
||||
ArmSubsystem();
|
||||
|
||||
void UseState(State setpoint) override;
|
||||
|
||||
frc2::CommandPtr SetArmGoalCommand(units::radian_t goal);
|
||||
|
||||
private:
|
||||
ExampleSmartMotorController m_motor;
|
||||
frc::ArmFeedforward m_feedforward;
|
||||
};
|
||||
@@ -1,84 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
frc2::CommandPtr SetMaxOutputCommand(double maxOutput);
|
||||
|
||||
frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
|
||||
std::function<double()> rot);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1;
|
||||
frc::PWMSparkMax m_left2;
|
||||
frc::PWMSparkMax m_right1;
|
||||
frc::PWMSparkMax m_right2;
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder;
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> 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() {}
|
||||
|
||||
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() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,68 +0,0 @@
|
||||
// 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 "RobotContainer.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::cmd::Run(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// We can bind commands while keeping their ownership in RobotContainer
|
||||
|
||||
// Spin up the shooter when the 'A' button is pressed
|
||||
m_driverController.A().OnTrue(m_spinUpShooter.get());
|
||||
|
||||
// Turn off the shooter when the 'B' button is pressed
|
||||
m_driverController.B().OnTrue(m_stopShooter.get());
|
||||
|
||||
// We can also *move* command ownership to the scheduler
|
||||
// Note that we won't be able to access these commands after moving them!
|
||||
|
||||
// Shoots if the shooter wheel has reached the target speed
|
||||
frc2::CommandPtr shoot = frc2::cmd::Either(
|
||||
// Run the feeder
|
||||
frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}),
|
||||
// Do nothing
|
||||
frc2::cmd::None(),
|
||||
// Determine which of the above to do based on whether the shooter has
|
||||
// reached the desired speed
|
||||
[this] { return m_shooter.AtSetpoint(); });
|
||||
|
||||
frc2::CommandPtr stopFeeder =
|
||||
frc2::cmd::RunOnce([this] { m_shooter.StopFeeder(); }, {&m_shooter});
|
||||
|
||||
// Shoot when the 'X' button is pressed
|
||||
m_driverController.X()
|
||||
.OnTrue(std::move(shoot))
|
||||
.OnFalse(std::move(stopFeeder));
|
||||
|
||||
// We can also define commands inline at the binding!
|
||||
// (ownership will be passed to the scheduler)
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
m_driverController.RightBumper()
|
||||
.OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
|
||||
.OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {}));
|
||||
}
|
||||
|
||||
frc2::Command* RobotContainer::GetAutonomousCommand() {
|
||||
// Runs the chosen command in autonomous
|
||||
return m_autonomousCommand.get();
|
||||
}
|
||||
@@ -1,59 +0,0 @@
|
||||
// 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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_left1{kLeftMotor1Port},
|
||||
m_left2{kLeftMotor2Port},
|
||||
m_right1{kRightMotor1Port},
|
||||
m_right2{kRightMotor2Port},
|
||||
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
|
||||
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_left1.SetInverted(true);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
}
|
||||
|
||||
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
|
||||
m_drive.ArcadeDrive(fwd, rot);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -1,44 +0,0 @@
|
||||
// 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 "subsystems/ShooterSubsystem.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <units/angular_velocity.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace ShooterConstants;
|
||||
|
||||
ShooterSubsystem::ShooterSubsystem()
|
||||
: PIDSubsystem{frc::PIDController{kP, kI, kD}},
|
||||
m_shooterMotor(kShooterMotorPort),
|
||||
m_feederMotor(kFeederMotorPort),
|
||||
m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
|
||||
m_shooterFeedforward(kS, kV) {
|
||||
m_controller.SetTolerance(kShooterToleranceRPS.value());
|
||||
m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
SetSetpoint(kShooterTargetRPS.value());
|
||||
}
|
||||
|
||||
void ShooterSubsystem::UseOutput(double output, double setpoint) {
|
||||
m_shooterMotor.SetVoltage(units::volt_t{output} +
|
||||
m_shooterFeedforward.Calculate(kShooterTargetRPS));
|
||||
}
|
||||
|
||||
bool ShooterSubsystem::AtSetpoint() {
|
||||
return m_controller.AtSetpoint();
|
||||
}
|
||||
|
||||
double ShooterSubsystem::GetMeasurement() {
|
||||
return m_shooterEncoder.GetRate();
|
||||
}
|
||||
|
||||
void ShooterSubsystem::RunFeeder() {
|
||||
m_feederMotor.Set(kFeederSpeed);
|
||||
}
|
||||
|
||||
void ShooterSubsystem::StopFeeder() {
|
||||
m_feederMotor.Set(0);
|
||||
}
|
||||
@@ -1,78 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/time.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr double kWheelDiameterInches = 6;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameterInches * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace ShooterConstants {
|
||||
inline constexpr int kEncoderPorts[]{4, 5};
|
||||
inline constexpr bool kEncoderReversed = false;
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Distance units will be rotations
|
||||
1.0 / static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr int kShooterMotorPort = 4;
|
||||
inline constexpr int kFeederMotorPort = 5;
|
||||
|
||||
inline constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
|
||||
inline constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
|
||||
inline constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
|
||||
|
||||
inline constexpr double kP = 1;
|
||||
inline constexpr double kI = 0;
|
||||
inline constexpr double kD = 0;
|
||||
|
||||
// On a real robot the feedforward constants should be empirically determined;
|
||||
// these are reasonable guesses.
|
||||
inline constexpr auto kS = 0.05_V;
|
||||
inline constexpr auto kV =
|
||||
// Should have value 12V at free speed...
|
||||
12_V / kShooterFreeRPS;
|
||||
|
||||
inline constexpr double kFeederSpeed = 0.5;
|
||||
} // namespace ShooterConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kAutoTimeoutSeconds = 12_s;
|
||||
inline constexpr auto kAutoShootTimeSeconds = 7_s;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() 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;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/button/CommandXboxController.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
#include "subsystems/ShooterSubsystem.h"
|
||||
|
||||
namespace ac = AutoConstants;
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
// The chooser for the autonomous routines
|
||||
frc2::Command* GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc2::CommandXboxController m_driverController{
|
||||
OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
ShooterSubsystem m_shooter;
|
||||
|
||||
// RobotContainer-owned commands
|
||||
// (These variables will still be valid after binding, because we don't move
|
||||
// ownership)
|
||||
|
||||
frc2::CommandPtr m_spinUpShooter =
|
||||
frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter});
|
||||
|
||||
frc2::CommandPtr m_stopShooter =
|
||||
frc2::cmd::RunOnce([this] { m_shooter.Disable(); }, {&m_shooter});
|
||||
|
||||
// An autonomous routine that shoots the loaded frisbees
|
||||
frc2::CommandPtr m_autonomousCommand =
|
||||
frc2::cmd::Sequence(
|
||||
// Start the command by spinning up the shooter...
|
||||
frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter}),
|
||||
// Wait until the shooter is at speed before feeding the frisbees
|
||||
frc2::cmd::WaitUntil([this] { return m_shooter.AtSetpoint(); }),
|
||||
// Start running the feeder
|
||||
frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}),
|
||||
// Shoot for the specified time
|
||||
frc2::cmd::Wait(ac::kAutoShootTimeSeconds))
|
||||
// Add a timeout (will end the command if, for instance, the shooter
|
||||
// never gets up to speed)
|
||||
.WithTimeout(ac::kAutoTimeoutSeconds)
|
||||
// When the command ends, turn off the shooter and the feeder
|
||||
.AndThen(frc2::cmd::RunOnce([this] {
|
||||
m_shooter.Disable();
|
||||
m_shooter.StopFeeder();
|
||||
}));
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,86 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
*/
|
||||
void ArcadeDrive(double fwd, double rot);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1;
|
||||
frc::PWMSparkMax m_left2;
|
||||
frc::PWMSparkMax m_right1;
|
||||
frc::PWMSparkMax m_right2;
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder;
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder;
|
||||
};
|
||||
@@ -1,32 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/PIDSubsystem.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
class ShooterSubsystem : public frc2::PIDSubsystem {
|
||||
public:
|
||||
ShooterSubsystem();
|
||||
|
||||
void UseOutput(double output, double setpoint) override;
|
||||
|
||||
double GetMeasurement() override;
|
||||
|
||||
bool AtSetpoint();
|
||||
|
||||
void RunFeeder();
|
||||
|
||||
void StopFeeder();
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_shooterMotor;
|
||||
frc::PWMSparkMax m_feederMotor;
|
||||
frc::Encoder m_shooterEncoder;
|
||||
frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> 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() {}
|
||||
|
||||
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() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,54 +0,0 @@
|
||||
// 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 "RobotContainer.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#include "commands/CloseClaw.h"
|
||||
#include "commands/OpenClaw.h"
|
||||
#include "commands/Pickup.h"
|
||||
#include "commands/Place.h"
|
||||
#include "commands/PrepareToPickup.h"
|
||||
#include "commands/SetElevatorSetpoint.h"
|
||||
#include "commands/TankDrive.h"
|
||||
|
||||
RobotContainer::RobotContainer()
|
||||
: m_autonomousCommand(m_claw, m_wrist, m_elevator, m_drivetrain) {
|
||||
frc::SmartDashboard::PutData(&m_drivetrain);
|
||||
frc::SmartDashboard::PutData(&m_elevator);
|
||||
frc::SmartDashboard::PutData(&m_wrist);
|
||||
frc::SmartDashboard::PutData(&m_claw);
|
||||
|
||||
m_drivetrain.SetDefaultCommand(
|
||||
TankDrive([this] { return -m_joy.GetLeftY(); },
|
||||
[this] { return -m_joy.GetRightY(); }, m_drivetrain));
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
frc2::JoystickButton(&m_joy, 5).OnTrue(
|
||||
SetElevatorSetpoint(0.25, m_elevator).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 6).OnTrue(CloseClaw(m_claw).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 7).OnTrue(
|
||||
SetElevatorSetpoint(0.0, m_elevator).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 8).OnTrue(OpenClaw(m_claw).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 9).OnTrue(
|
||||
Autonomous(m_claw, m_wrist, m_elevator, m_drivetrain).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 10)
|
||||
.OnTrue(Pickup(m_claw, m_wrist, m_elevator).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 11)
|
||||
.OnTrue(Place(m_claw, m_wrist, m_elevator).ToPtr());
|
||||
frc2::JoystickButton(&m_joy, 12)
|
||||
.OnTrue(PrepareToPickup(m_claw, m_wrist, m_elevator).ToPtr());
|
||||
}
|
||||
|
||||
frc2::Command* RobotContainer::GetAutonomousCommand() {
|
||||
// An example command will be run in autonomous
|
||||
return &m_autonomousCommand;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
// 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 "commands/Autonomous.h"
|
||||
|
||||
#include <frc2/command/ParallelCommandGroup.h>
|
||||
|
||||
#include "commands/CloseClaw.h"
|
||||
#include "commands/DriveStraight.h"
|
||||
#include "commands/Pickup.h"
|
||||
#include "commands/Place.h"
|
||||
#include "commands/PrepareToPickup.h"
|
||||
#include "commands/SetDistanceToBox.h"
|
||||
#include "commands/SetWristSetpoint.h"
|
||||
|
||||
Autonomous::Autonomous(Claw& claw, Wrist& wrist, Elevator& elevator,
|
||||
Drivetrain& drivetrain) {
|
||||
SetName("Autonomous");
|
||||
AddCommands(
|
||||
// clang-format off
|
||||
PrepareToPickup(claw, wrist, elevator),
|
||||
Pickup(claw, wrist, elevator),
|
||||
SetDistanceToBox(0.10, drivetrain),
|
||||
// DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken
|
||||
Place(claw, wrist, elevator),
|
||||
SetDistanceToBox(0.6, drivetrain),
|
||||
// DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken
|
||||
frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
|
||||
CloseClaw(claw)));
|
||||
// clang-format on
|
||||
}
|
||||
@@ -1,34 +0,0 @@
|
||||
// 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 "commands/CloseClaw.h"
|
||||
|
||||
#include <frc/RobotBase.h>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
CloseClaw::CloseClaw(Claw& claw) : m_claw(&claw) {
|
||||
SetName("CloseClaw");
|
||||
AddRequirements(m_claw);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void CloseClaw::Initialize() {
|
||||
m_claw->Close();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool CloseClaw::IsFinished() {
|
||||
return m_claw->IsGripping();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void CloseClaw::End(bool) {
|
||||
// NOTE: Doesn't stop in simulation due to lower friction causing the can to
|
||||
// fall out + there is no need to worry about stalling the motor or crushing
|
||||
// the can.
|
||||
if constexpr (frc::RobotBase::IsSimulation()) {
|
||||
m_claw->Stop();
|
||||
}
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
// 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 "commands/DriveStraight.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
DriveStraight::DriveStraight(double distance, Drivetrain& drivetrain)
|
||||
: frc2::CommandHelper<frc2::PIDCommand, DriveStraight>{
|
||||
frc::PIDController{4, 0, 0},
|
||||
[&drivetrain] { return drivetrain.GetDistance(); },
|
||||
distance,
|
||||
[&drivetrain](double output) { drivetrain.Drive(output, output); },
|
||||
{&drivetrain}},
|
||||
m_drivetrain{&drivetrain} {
|
||||
m_controller.SetTolerance(0.01);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void DriveStraight::Initialize() {
|
||||
// Get everything in a safe starting state.
|
||||
m_drivetrain->Reset();
|
||||
frc2::PIDCommand::Initialize();
|
||||
}
|
||||
|
||||
bool DriveStraight::IsFinished() {
|
||||
return m_controller.AtSetpoint();
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
// 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 "commands/OpenClaw.h"
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
OpenClaw::OpenClaw(Claw& claw)
|
||||
: frc2::CommandHelper<frc2::WaitCommand, OpenClaw>(1_s), m_claw(&claw) {
|
||||
SetName("OpenClaw");
|
||||
AddRequirements(m_claw);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void OpenClaw::Initialize() {
|
||||
m_claw->Open();
|
||||
frc2::WaitCommand::Initialize();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void OpenClaw::End(bool) {
|
||||
m_claw->Stop();
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
// 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 "commands/Pickup.h"
|
||||
|
||||
#include <frc2/command/ParallelCommandGroup.h>
|
||||
|
||||
#include "commands/CloseClaw.h"
|
||||
#include "commands/SetElevatorSetpoint.h"
|
||||
#include "commands/SetWristSetpoint.h"
|
||||
|
||||
Pickup::Pickup(Claw& claw, Wrist& wrist, Elevator& elevator) {
|
||||
SetName("Pickup");
|
||||
AddCommands(CloseClaw(claw),
|
||||
frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist),
|
||||
SetElevatorSetpoint(0.25, elevator)));
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
// 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 "commands/Place.h"
|
||||
|
||||
#include "commands/OpenClaw.h"
|
||||
#include "commands/SetElevatorSetpoint.h"
|
||||
#include "commands/SetWristSetpoint.h"
|
||||
|
||||
Place::Place(Claw& claw, Wrist& wrist, Elevator& elevator) {
|
||||
SetName("Place");
|
||||
// clang-format off
|
||||
AddCommands(SetElevatorSetpoint(0.25, elevator),
|
||||
SetWristSetpoint(0, wrist),
|
||||
OpenClaw(claw));
|
||||
// clang-format on
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
// 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 "commands/PrepareToPickup.h"
|
||||
|
||||
#include <frc2/command/ParallelCommandGroup.h>
|
||||
|
||||
#include "commands/OpenClaw.h"
|
||||
#include "commands/SetElevatorSetpoint.h"
|
||||
#include "commands/SetWristSetpoint.h"
|
||||
|
||||
PrepareToPickup::PrepareToPickup(Claw& claw, Wrist& wrist, Elevator& elevator) {
|
||||
SetName("PrepareToPickup");
|
||||
AddCommands(OpenClaw(claw),
|
||||
frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator),
|
||||
SetWristSetpoint(0, wrist)));
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
// 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 "commands/SetDistanceToBox.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
SetDistanceToBox::SetDistanceToBox(double distance, Drivetrain& drivetrain)
|
||||
: frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox>{
|
||||
frc::PIDController{-2, 0, 0},
|
||||
[&drivetrain] { return drivetrain.GetDistanceToObstacle(); },
|
||||
distance,
|
||||
[&drivetrain](double output) { drivetrain.Drive(output, output); },
|
||||
{&drivetrain}},
|
||||
m_drivetrain{&drivetrain} {
|
||||
m_controller.SetTolerance(0.01);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetDistanceToBox::Initialize() {
|
||||
// Get everything in a safe starting state.
|
||||
m_drivetrain->Reset();
|
||||
frc2::PIDCommand::Initialize();
|
||||
}
|
||||
|
||||
bool SetDistanceToBox::IsFinished() {
|
||||
return m_controller.AtSetpoint();
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
// 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 "commands/SetElevatorSetpoint.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator& elevator)
|
||||
: m_setpoint(setpoint), m_elevator(&elevator) {
|
||||
SetName("SetElevatorSetpoint");
|
||||
AddRequirements(m_elevator);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetElevatorSetpoint::Initialize() {
|
||||
m_elevator->SetSetpoint(m_setpoint);
|
||||
m_elevator->Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetElevatorSetpoint::IsFinished() {
|
||||
return m_elevator->GetController().AtSetpoint();
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
// 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 "commands/SetWristSetpoint.h"
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist& wrist)
|
||||
: m_setpoint(setpoint), m_wrist(&wrist) {
|
||||
SetName("SetWristSetpoint");
|
||||
AddRequirements(m_wrist);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetWristSetpoint::Initialize() {
|
||||
m_wrist->SetSetpoint(m_setpoint);
|
||||
m_wrist->Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetWristSetpoint::IsFinished() {
|
||||
return m_wrist->GetController().AtSetpoint();
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
// 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 "commands/TankDrive.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
TankDrive::TankDrive(std::function<double()> left,
|
||||
std::function<double()> right, Drivetrain& drivetrain)
|
||||
: m_left(std::move(left)),
|
||||
m_right(std::move(right)),
|
||||
m_drivetrain(&drivetrain) {
|
||||
SetName("TankDrive");
|
||||
AddRequirements(m_drivetrain);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void TankDrive::Execute() {
|
||||
m_drivetrain->Drive(m_left(), m_right());
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool TankDrive::IsFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void TankDrive::End(bool) {
|
||||
m_drivetrain->Drive(0, 0);
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
// 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 "subsystems/Claw.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
Claw::Claw() {
|
||||
// Let's show everything on the LiveWindow
|
||||
SetName("Claw");
|
||||
AddChild("Motor", &m_motor);
|
||||
}
|
||||
|
||||
void Claw::Open() {
|
||||
m_motor.Set(-1);
|
||||
}
|
||||
|
||||
void Claw::Close() {
|
||||
m_motor.Set(1);
|
||||
}
|
||||
|
||||
void Claw::Stop() {
|
||||
m_motor.Set(0);
|
||||
}
|
||||
|
||||
bool Claw::IsGripping() const {
|
||||
return m_contact.Get();
|
||||
}
|
||||
|
||||
void Claw::Log() {
|
||||
frc::SmartDashboard::PutBoolean("Claw switch", IsGripping());
|
||||
}
|
||||
|
||||
void Claw::Periodic() {
|
||||
Log();
|
||||
}
|
||||
@@ -1,86 +0,0 @@
|
||||
// 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 "subsystems/Drivetrain.h"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Joystick.h>
|
||||
#include <frc/RobotBase.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <units/length.h>
|
||||
|
||||
Drivetrain::Drivetrain() {
|
||||
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
|
||||
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
|
||||
|
||||
m_frontLeft.AddFollower(m_rearLeft);
|
||||
m_frontRight.AddFollower(m_rearRight);
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_frontRight.SetInverted(true);
|
||||
|
||||
// Encoders may measure differently in the real world and in simulation. In
|
||||
// this example the robot moves 0.042 barleycorns per tick in the real world,
|
||||
// but the simulated encoders simulate 360 tick encoders. This if statement
|
||||
// allows for the real robot to handle this difference in devices.
|
||||
if constexpr (frc::RobotBase::IsSimulation()) {
|
||||
m_leftEncoder.SetDistancePerPulse(0.042);
|
||||
m_rightEncoder.SetDistancePerPulse(0.042);
|
||||
} else {
|
||||
// Circumference = diameter * pi. 360 tick simulated encoders.
|
||||
m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
|
||||
std::numbers::pi / 360.0);
|
||||
m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
|
||||
std::numbers::pi / 360.0);
|
||||
}
|
||||
SetName("Drivetrain");
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Front_Left Motor", &m_frontLeft);
|
||||
AddChild("Rear Left Motor", &m_rearLeft);
|
||||
AddChild("Front Right Motor", &m_frontRight);
|
||||
AddChild("Rear Right Motor", &m_rearRight);
|
||||
AddChild("Left Encoder", &m_leftEncoder);
|
||||
AddChild("Right Encoder", &m_rightEncoder);
|
||||
AddChild("Rangefinder", &m_rangefinder);
|
||||
AddChild("Gyro", &m_gyro);
|
||||
}
|
||||
|
||||
void Drivetrain::Log() {
|
||||
frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Right Distance",
|
||||
m_rightEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
|
||||
}
|
||||
|
||||
void Drivetrain::Drive(double left, double right) {
|
||||
m_robotDrive.TankDrive(left, right);
|
||||
}
|
||||
|
||||
double Drivetrain::GetHeading() const {
|
||||
return m_gyro.GetAngle();
|
||||
}
|
||||
|
||||
void Drivetrain::Reset() {
|
||||
m_gyro.Reset();
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double Drivetrain::GetDistance() const {
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
|
||||
}
|
||||
|
||||
double Drivetrain::GetDistanceToObstacle() const {
|
||||
// Really meters in simulation since it's a rangefinder...
|
||||
return m_rangefinder.GetAverageVoltage();
|
||||
}
|
||||
|
||||
void Drivetrain::Periodic() {
|
||||
Log();
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
// 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 "subsystems/Elevator.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/livewindow/LiveWindow.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
Elevator::Elevator()
|
||||
: frc2::PIDSubsystem{frc::PIDController{kP_real, kI_real, 0}} {
|
||||
if constexpr (frc::RobotBase::IsSimulation()) {
|
||||
// Check for simulation and update PID values
|
||||
GetController().SetPID(kP_simulation, kI_simulation, 0);
|
||||
}
|
||||
|
||||
m_controller.SetTolerance(0.005);
|
||||
|
||||
SetName("Elevator");
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", &m_motor);
|
||||
AddChild("Pot", &m_pot);
|
||||
}
|
||||
|
||||
void Elevator::Log() {
|
||||
frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
|
||||
}
|
||||
|
||||
double Elevator::GetMeasurement() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
void Elevator::UseOutput(double output, double setpoint) {
|
||||
m_motor.Set(output);
|
||||
}
|
||||
|
||||
void Elevator::Periodic() {
|
||||
Log();
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
// 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 "subsystems/Wrist.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
Wrist::Wrist() : frc2::PIDSubsystem{frc::PIDController{kP, 0, 0}} {
|
||||
m_controller.SetTolerance(2.5);
|
||||
|
||||
SetName("Wrist");
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", &m_motor);
|
||||
AddChild("Pot", &m_pot);
|
||||
}
|
||||
|
||||
void Wrist::Log() {
|
||||
frc::SmartDashboard::PutNumber("Wrist Angle", GetMeasurement());
|
||||
}
|
||||
|
||||
double Wrist::GetMeasurement() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
void Wrist::UseOutput(double output, double setpoint) {
|
||||
m_motor.Set(output);
|
||||
}
|
||||
|
||||
void Wrist::Periodic() {
|
||||
Log();
|
||||
}
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() 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;
|
||||
};
|
||||
@@ -1,41 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "commands/Autonomous.h"
|
||||
#include "subsystems/Claw.h"
|
||||
#include "subsystems/Drivetrain.h"
|
||||
#include "subsystems/Elevator.h"
|
||||
#include "subsystems/Wrist.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();
|
||||
|
||||
private:
|
||||
// The robot's subsystems and commands are defined here...
|
||||
frc::XboxController m_joy{0};
|
||||
|
||||
Claw m_claw;
|
||||
Wrist m_wrist;
|
||||
Elevator m_elevator;
|
||||
Drivetrain m_drivetrain;
|
||||
|
||||
Autonomous m_autonomousCommand;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,23 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
#include "subsystems/Drivetrain.h"
|
||||
#include "subsystems/Elevator.h"
|
||||
#include "subsystems/Wrist.h"
|
||||
|
||||
/**
|
||||
* The main autonomous command to pickup and deliver the soda to the box.
|
||||
*/
|
||||
class Autonomous
|
||||
: public frc2::CommandHelper<frc2::SequentialCommandGroup, Autonomous> {
|
||||
public:
|
||||
Autonomous(Claw& claw, Wrist& wrist, Elevator& elevator,
|
||||
Drivetrain& drivetrain);
|
||||
};
|
||||
@@ -1,24 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
|
||||
/**
|
||||
* Closes the claw until the limit switch is tripped.
|
||||
*/
|
||||
class CloseClaw : public frc2::CommandHelper<frc2::Command, CloseClaw> {
|
||||
public:
|
||||
explicit CloseClaw(Claw& claw);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End(bool interrupted) override;
|
||||
|
||||
private:
|
||||
Claw* m_claw;
|
||||
};
|
||||
@@ -1,27 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
|
||||
#include "subsystems/Drivetrain.h"
|
||||
|
||||
/**
|
||||
* Drive the given distance straight (negative values go backwards).
|
||||
* Uses a local PID controller to run a simple PID loop that is only
|
||||
* enabled while this command is running. The input is the averaged
|
||||
* values of the left and right encoders.
|
||||
*/
|
||||
class DriveStraight
|
||||
: public frc2::CommandHelper<frc2::PIDCommand, DriveStraight> {
|
||||
public:
|
||||
explicit DriveStraight(double distance, Drivetrain& drivetrain);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
Drivetrain* m_drivetrain;
|
||||
};
|
||||
@@ -1,24 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/WaitCommand.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
|
||||
/**
|
||||
* Opens the claw for one second. Real robots should use sensors, stalling
|
||||
* motors is BAD!
|
||||
*/
|
||||
class OpenClaw : public frc2::CommandHelper<frc2::WaitCommand, OpenClaw> {
|
||||
public:
|
||||
explicit OpenClaw(Claw& claw);
|
||||
void Initialize() override;
|
||||
void End(bool interrupted) override;
|
||||
|
||||
private:
|
||||
Claw* m_claw;
|
||||
};
|
||||
@@ -1,22 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
#include "subsystems/Elevator.h"
|
||||
#include "subsystems/Wrist.h"
|
||||
|
||||
/**
|
||||
* Pickup a soda can (if one is between the open claws) and
|
||||
* get it in a safe state to drive around.
|
||||
*/
|
||||
class Pickup
|
||||
: public frc2::CommandHelper<frc2::SequentialCommandGroup, Pickup> {
|
||||
public:
|
||||
Pickup(Claw& claw, Wrist& wrist, Elevator& elevator);
|
||||
};
|
||||
@@ -1,20 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
#include "subsystems/Elevator.h"
|
||||
#include "subsystems/Wrist.h"
|
||||
|
||||
/**
|
||||
* Place a held soda can onto the platform.
|
||||
*/
|
||||
class Place : public frc2::CommandHelper<frc2::SequentialCommandGroup, Place> {
|
||||
public:
|
||||
Place(Claw& claw, Wrist& wrist, Elevator& elevator);
|
||||
};
|
||||
@@ -1,21 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
|
||||
#include "subsystems/Claw.h"
|
||||
#include "subsystems/Elevator.h"
|
||||
#include "subsystems/Wrist.h"
|
||||
|
||||
/**
|
||||
* Make sure the robot is in a state to pickup soda cans.
|
||||
*/
|
||||
class PrepareToPickup : public frc2::CommandHelper<frc2::SequentialCommandGroup,
|
||||
PrepareToPickup> {
|
||||
public:
|
||||
PrepareToPickup(Claw& claw, Wrist& wrist, Elevator& elevator);
|
||||
};
|
||||
@@ -1,27 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
|
||||
#include "subsystems/Drivetrain.h"
|
||||
|
||||
/**
|
||||
* Drive until the robot is the given distance away from the box. Uses a local
|
||||
* PID controller to run a simple PID loop that is only enabled while this
|
||||
* command is running. The input is the averaged values of the left and right
|
||||
* encoders.
|
||||
*/
|
||||
class SetDistanceToBox
|
||||
: public frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox> {
|
||||
public:
|
||||
explicit SetDistanceToBox(double distance, Drivetrain& drivetrain);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
Drivetrain* m_drivetrain;
|
||||
};
|
||||
@@ -1,29 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
|
||||
#include "subsystems/Elevator.h"
|
||||
|
||||
/**
|
||||
* Move the elevator to a given location. This command finishes when it is
|
||||
* within
|
||||
* the tolerance, but leaves the PID loop running to maintain the position.
|
||||
* Other
|
||||
* commands using the elevator should make sure they disable PID!
|
||||
*/
|
||||
class SetElevatorSetpoint
|
||||
: public frc2::CommandHelper<frc2::Command, SetElevatorSetpoint> {
|
||||
public:
|
||||
explicit SetElevatorSetpoint(double setpoint, Elevator& elevator);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double m_setpoint;
|
||||
Elevator* m_elevator;
|
||||
};
|
||||
@@ -1,27 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
|
||||
#include "subsystems/Wrist.h"
|
||||
|
||||
/**
|
||||
* Move the wrist to a given angle. This command finishes when it is within
|
||||
* the tolerance, but leaves the PID loop running to maintain the position.
|
||||
* Other commands using the wrist should make sure they disable PID!
|
||||
*/
|
||||
class SetWristSetpoint
|
||||
: public frc2::CommandHelper<frc2::Command, SetWristSetpoint> {
|
||||
public:
|
||||
explicit SetWristSetpoint(double setpoint, Wrist& wrist);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double m_setpoint;
|
||||
Wrist* m_wrist;
|
||||
};
|
||||
@@ -1,29 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
|
||||
#include "subsystems/Drivetrain.h"
|
||||
|
||||
/**
|
||||
* Have the robot drive tank style using the PS3 Joystick until interrupted.
|
||||
*/
|
||||
class TankDrive : public frc2::CommandHelper<frc2::Command, TankDrive> {
|
||||
public:
|
||||
TankDrive(std::function<double()> left, std::function<double()> right,
|
||||
Drivetrain& drivetrain);
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End(bool interrupted) override;
|
||||
|
||||
private:
|
||||
std::function<double()> m_left;
|
||||
std::function<double()> m_right;
|
||||
Drivetrain* m_drivetrain;
|
||||
};
|
||||
@@ -1,55 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/DigitalInput.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
/**
|
||||
* The claw subsystem is a simple system with a motor for opening and closing.
|
||||
* If using stronger motors, you should probably use a sensor so that the
|
||||
* motors don't stall.
|
||||
*/
|
||||
class Claw : public frc2::SubsystemBase {
|
||||
public:
|
||||
Claw();
|
||||
|
||||
/**
|
||||
* Set the claw motor to move in the open direction.
|
||||
*/
|
||||
void Open();
|
||||
|
||||
/**
|
||||
* Set the claw motor to move in the close direction.
|
||||
*/
|
||||
void Close();
|
||||
|
||||
/**
|
||||
* Stops the claw motor from moving.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* Return true when the robot is grabbing an object hard enough
|
||||
* to trigger the limit switch.
|
||||
*/
|
||||
bool IsGripping() const;
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Log the data periodically. This method is automatically called
|
||||
* by the subsystem.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_motor{7};
|
||||
frc::DigitalInput m_contact{5};
|
||||
};
|
||||
@@ -1,80 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/AnalogGyro.h>
|
||||
#include <frc/AnalogInput.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
namespace frc {
|
||||
class Joystick;
|
||||
} // namespace frc
|
||||
|
||||
/**
|
||||
* The Drivetrain subsystem incorporates the sensors and actuators attached to
|
||||
* the robots chassis. These include four drive motors, a left and right encoder
|
||||
* and a gyro.
|
||||
*/
|
||||
class Drivetrain : public frc2::SubsystemBase {
|
||||
public:
|
||||
Drivetrain();
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Tank style driving for the Drivetrain.
|
||||
* @param left Speed in range [-1,1]
|
||||
* @param right Speed in range [-1,1]
|
||||
*/
|
||||
void Drive(double left, double right);
|
||||
|
||||
/**
|
||||
* @return The robots heading in degrees.
|
||||
*/
|
||||
double GetHeading() const;
|
||||
|
||||
/**
|
||||
* Reset the robots sensors to the zero states.
|
||||
*/
|
||||
void Reset();
|
||||
|
||||
/**
|
||||
* @return The distance driven (average of left and right encoders).
|
||||
*/
|
||||
double GetDistance() const;
|
||||
|
||||
/**
|
||||
* @return The distance to the obstacle detected by the rangefinder.
|
||||
*/
|
||||
double GetDistanceToObstacle() const;
|
||||
|
||||
/**
|
||||
* Log the data periodically. This method is automatically called
|
||||
* by the subsystem.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_frontLeft{1};
|
||||
frc::PWMSparkMax m_rearLeft{2};
|
||||
|
||||
frc::PWMSparkMax m_frontRight{3};
|
||||
frc::PWMSparkMax m_rearRight{4};
|
||||
|
||||
frc::DifferentialDrive m_robotDrive{
|
||||
[&](double output) { m_frontLeft.Set(output); },
|
||||
[&](double output) { m_frontRight.Set(output); }};
|
||||
|
||||
frc::Encoder m_leftEncoder{1, 2};
|
||||
frc::Encoder m_rightEncoder{3, 4};
|
||||
frc::AnalogInput m_rangefinder{6};
|
||||
frc::AnalogGyro m_gyro{1};
|
||||
};
|
||||
@@ -1,59 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/AnalogPotentiometer.h>
|
||||
#include <frc/RobotBase.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/PIDSubsystem.h>
|
||||
|
||||
/**
|
||||
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
|
||||
* it's current
|
||||
* state PID values for simulation are different than in the real world do to
|
||||
* minor differences.
|
||||
*/
|
||||
class Elevator : public frc2::PIDSubsystem {
|
||||
public:
|
||||
Elevator();
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double GetMeasurement() override;
|
||||
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by
|
||||
* the subsystem.
|
||||
*/
|
||||
void UseOutput(double output, double setpoint) override;
|
||||
|
||||
/**
|
||||
* Log the data periodically. This method is automatically called
|
||||
* by the subsystem.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_motor{5};
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
frc::AnalogPotentiometer m_pot =
|
||||
frc::RobotBase::IsReal()
|
||||
? frc::AnalogPotentiometer{2, -2.0 / 5}
|
||||
: frc::AnalogPotentiometer{2}; // Defaults to meters
|
||||
|
||||
static constexpr double kP_real = 4;
|
||||
static constexpr double kI_real = 0.07;
|
||||
static constexpr double kP_simulation = 18;
|
||||
static constexpr double kI_simulation = 0.2;
|
||||
};
|
||||
@@ -1,53 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/AnalogPotentiometer.h>
|
||||
#include <frc/RobotBase.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/PIDSubsystem.h>
|
||||
|
||||
/**
|
||||
* The wrist subsystem is like the elevator, but with a rotational joint instead
|
||||
* of a linear joint.
|
||||
*/
|
||||
class Wrist : public frc2::PIDSubsystem {
|
||||
public:
|
||||
Wrist();
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double GetMeasurement() override;
|
||||
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by the subsystem.
|
||||
*/
|
||||
void UseOutput(double output, double setpoint) override;
|
||||
|
||||
/**
|
||||
* Log the data periodically. This method is automatically called
|
||||
* by the subsystem.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
private:
|
||||
frc::PWMSparkMax m_motor{6};
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
frc::AnalogPotentiometer m_pot =
|
||||
frc::RobotBase::IsReal()
|
||||
? frc::AnalogPotentiometer{3, -270.0 / 5}
|
||||
: frc::AnalogPotentiometer{3}; // Defaults to degrees
|
||||
|
||||
static constexpr double kP = 1;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// 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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> 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() {}
|
||||
|
||||
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) {
|
||||
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) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,70 +0,0 @@
|
||||
// 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 "RobotContainer.h"
|
||||
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.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.GetLeftY(),
|
||||
-m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Assorted commands to be bound to buttons
|
||||
|
||||
// Stabilize robot to drive straight with gyro when L1 is held
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1)
|
||||
.WhileTrue(
|
||||
frc2::PIDCommand(
|
||||
frc::PIDController{dc::kStabilizationP, dc::kStabilizationI,
|
||||
dc::kStabilizationD},
|
||||
// Close the loop on the turn rate
|
||||
[this] { return m_drive.GetTurnRate(); },
|
||||
// Setpoint is 0
|
||||
0,
|
||||
// Pipe the output to the turning controls
|
||||
[this](double output) {
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(), output);
|
||||
},
|
||||
// Require the robot drive
|
||||
{&m_drive})
|
||||
.ToPtr());
|
||||
|
||||
// Turn to 90 degrees when the 'Cross' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCross)
|
||||
.OnTrue(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
|
||||
|
||||
// Turn to -90 degrees with a profile when the 'Square' button is pressed,
|
||||
// with a 5 second timeout
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare)
|
||||
.OnTrue(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
|
||||
|
||||
// While holding R1, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1)
|
||||
.OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
|
||||
.OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {}));
|
||||
}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// no auto
|
||||
return frc2::cmd::None();
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
// 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 "commands/TurnToAngle.h"
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
|
||||
: CommandHelper{frc::PIDController{kTurnP, kTurnI, kTurnD},
|
||||
// Close loop on heading
|
||||
[drive] { return drive->GetHeading().value(); },
|
||||
// Set reference to target
|
||||
target.value(),
|
||||
// Pipe output to turn robot
|
||||
[drive](double output) { drive->ArcadeDrive(0, output); },
|
||||
// Require the drive
|
||||
{drive}} {
|
||||
// Set the controller to be continuous (because it is an angle controller)
|
||||
m_controller.EnableContinuousInput(-180, 180);
|
||||
// Set the controller tolerance - the delta tolerance ensures the robot is
|
||||
// stationary at the setpoint before it is considered as having reached the
|
||||
// reference
|
||||
m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value());
|
||||
|
||||
AddRequirements(drive);
|
||||
}
|
||||
|
||||
bool TurnToAngle::IsFinished() {
|
||||
return GetController().AtSetpoint();
|
||||
}
|
||||
@@ -1,38 +0,0 @@
|
||||
// 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 "commands/TurnToAngleProfiled.h"
|
||||
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
|
||||
DriveSubsystem* drive)
|
||||
: CommandHelper{
|
||||
frc::ProfiledPIDController<units::radians>{
|
||||
kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}},
|
||||
// Close loop on heading
|
||||
[drive] { return drive->GetHeading(); },
|
||||
// Set reference to target
|
||||
target,
|
||||
// Pipe output to turn robot
|
||||
[drive](double output, auto setpointState) {
|
||||
drive->ArcadeDrive(0, output);
|
||||
},
|
||||
// Require the drive
|
||||
{drive}} {
|
||||
// Set the controller to be continuous (because it is an angle controller)
|
||||
GetController().EnableContinuousInput(-180_deg, 180_deg);
|
||||
// Set the controller tolerance - the delta tolerance ensures the robot is
|
||||
// stationary at the setpoint before it is considered as having reached the
|
||||
// reference
|
||||
GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
|
||||
|
||||
AddRequirements(drive);
|
||||
}
|
||||
|
||||
bool TurnToAngleProfiled::IsFinished() {
|
||||
return GetController().AtGoal();
|
||||
}
|
||||
@@ -1,68 +0,0 @@
|
||||
// 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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_left1{kLeftMotor1Port},
|
||||
m_left2{kLeftMotor2Port},
|
||||
m_right1{kRightMotor1Port},
|
||||
m_right2{kRightMotor2Port},
|
||||
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
|
||||
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_right1.SetInverted(true);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
}
|
||||
|
||||
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
|
||||
m_drive.ArcadeDrive(fwd, rot);
|
||||
}
|
||||
|
||||
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() {
|
||||
return units::degree_t{std::remainder(m_gyro.GetAngle(), 360) *
|
||||
(kGyroReversed ? -1.0 : 1.0)};
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
@@ -1,64 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
|
||||
/**
|
||||
* 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 {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr double kWheelDiameterInches = 6;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameterInches * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr bool kGyroReversed = true;
|
||||
|
||||
inline constexpr double kStabilizationP = 1;
|
||||
inline constexpr double kStabilizationI = 0.5;
|
||||
inline constexpr double kStabilizationD = 0;
|
||||
|
||||
inline constexpr double kTurnP = 1;
|
||||
inline constexpr double kTurnI = 0;
|
||||
inline constexpr double kTurnD = 0;
|
||||
|
||||
inline constexpr auto kTurnTolerance = 5_deg;
|
||||
inline constexpr auto kTurnRateTolerance = 10_deg_per_s;
|
||||
|
||||
inline constexpr auto kMaxTurnRate = 100_deg_per_s;
|
||||
inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr double kAutoDriveDistanceInches = 60;
|
||||
inline constexpr double kAutoBackupDistanceInches = 20;
|
||||
inline constexpr double kAutoDriveSpeed = 0.5;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,44 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/PS4Controller.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "commands/TurnToAngle.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
namespace ac = AutoConstants;
|
||||
namespace dc = DriveConstants;
|
||||
|
||||
/**
|
||||
* 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::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,26 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* A command that will turn the robot to the specified angle.
|
||||
*/
|
||||
class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
|
||||
public:
|
||||
/**
|
||||
* Turns to robot to the specified angle.
|
||||
*
|
||||
* @param targetAngleDegrees The angle to turn to
|
||||
* @param drive The drive subsystem to use
|
||||
*/
|
||||
TurnToAngle(units::degree_t target, DriveSubsystem* drive);
|
||||
|
||||
bool IsFinished() override;
|
||||
};
|
||||
@@ -1,30 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandHelper.h>
|
||||
#include <frc2/command/ProfiledPIDCommand.h>
|
||||
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* A command that will turn the robot to the specified angle using a motion
|
||||
* profile.
|
||||
*/
|
||||
class TurnToAngleProfiled
|
||||
: public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
|
||||
TurnToAngleProfiled> {
|
||||
public:
|
||||
/**
|
||||
* Turns to robot to the specified angle using a motion profile.
|
||||
*
|
||||
* @param targetAngleDegrees The angle to turn to
|
||||
* @param drive The drive subsystem to use
|
||||
*/
|
||||
TurnToAngleProfiled(units::degree_t targetAngleDegrees,
|
||||
DriveSubsystem* drive);
|
||||
|
||||
bool IsFinished() override;
|
||||
};
|
||||
@@ -1,105 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
#include <units/angle.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
*/
|
||||
void ArcadeDrive(double fwd, double rot);
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
double GetTurnRate();
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1;
|
||||
frc::PWMSparkMax m_left2;
|
||||
frc::PWMSparkMax m_right1;
|
||||
frc::PWMSparkMax m_right2;
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder;
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder;
|
||||
|
||||
// The gyro sensor
|
||||
frc::ADXRS450_Gyro m_gyro;
|
||||
};
|
||||
@@ -319,24 +319,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "GearsBot",
|
||||
"description": "A fully functional Command-Based program for WPI's GearsBot robot.",
|
||||
"tags": [
|
||||
"Complete Robot",
|
||||
"Command-based",
|
||||
"Differential Drive",
|
||||
"Elevator",
|
||||
"Arm",
|
||||
"Analog",
|
||||
"Digital Input",
|
||||
"SmartDashboard",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "GearsBot",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "HAL",
|
||||
"description": "Use the low-level HAL C functions. This example is for advanced users.",
|
||||
@@ -428,38 +410,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "Frisbeebot",
|
||||
"description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.",
|
||||
"tags": [
|
||||
"Complete Robot",
|
||||
"Command-based",
|
||||
"Differential Drive",
|
||||
"Flywheel",
|
||||
"Encoder",
|
||||
"XboxController",
|
||||
"PID"
|
||||
],
|
||||
"foldername": "Frisbeebot",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "Gyro Drive Commands",
|
||||
"description": "Control a robot's angle with PID and a gyro, in command-based.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Differential Drive",
|
||||
"Encoder",
|
||||
"PS4Controller",
|
||||
"PID",
|
||||
"Profiled PID",
|
||||
"Gyro"
|
||||
],
|
||||
"foldername": "GyroDriveCommands",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "SwerveBot",
|
||||
"description": "Use kinematics and odometry with a swerve drive.",
|
||||
@@ -609,36 +559,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "ArmBot",
|
||||
"description": "Control an arm with ProfiledPIDSubsystem.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Arm",
|
||||
"Encoder",
|
||||
"Profiled PID",
|
||||
"XboxController",
|
||||
"Differential Drive"
|
||||
],
|
||||
"foldername": "ArmBot",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "ArmBotOffboard",
|
||||
"description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Arm",
|
||||
"Smart Motor Controller",
|
||||
"Trapezoid Profile",
|
||||
"XboxController",
|
||||
"Differential Drive"
|
||||
],
|
||||
"foldername": "ArmBotOffboard",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "DriveDistanceOffboard",
|
||||
"description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",
|
||||
|
||||
Reference in New Issue
Block a user