Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[documentation] Remove more outdated commands examples (#7054)

Browse Source 
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
2024-10-11 14:09:11 +08:00
committed by GitHub
parent 37e7bfe4f9
commit 8f57e4c566
120 changed files with 0 additions and 6615 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
View File

@@ -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

65
wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
View File

@@ -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();
}

34
wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
View File

@@ -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;
}

59
wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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);
}

74
wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
View File

@@ -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

32
wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
View File

@@ -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;
};

46
wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
View File

@@ -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();
};

30
wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
View File

@@ -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;
};

86
wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
View File

@@ -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;
};

72
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
View File

@@ -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

40
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
View File

@@ -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();
}

31
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
View File

@@ -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});
}

67
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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});
}

74
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
View File

@@ -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

80
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
View File

@@ -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() {}
};

32
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
View File

@@ -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;
};

42
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
View File

@@ -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();
};

30
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
View File

@@ -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;
};

84
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
View File

@@ -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;
};

72
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
View File

@@ -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

68
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
View File

@@ -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();
}

59
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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);
}

44
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
View File

@@ -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);
}

78
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
View File

@@ -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

30
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
View File

@@ -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;
};

72
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
View File

@@ -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();
};

86
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
View File

@@ -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;
};

32
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
View File

@@ -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;
};

72
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
View File

@@ -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

54
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
View File

@@ -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;
}

32
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
View File

@@ -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
}

34
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
View File

@@ -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();
}
}

31
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
View File

@@ -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();
}

24
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
View File

@@ -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();
}

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
View File

@@ -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)));
}

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
View File

@@ -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
}

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
View File

@@ -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)));
}

31
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
View File

@@ -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();
}

26
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
View File

@@ -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();
}

24
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
View File

@@ -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();
}

33
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
View File

@@ -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);
}

37
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
View File

@@ -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();
}

86
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
View File

@@ -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();
}

40
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
View File

@@ -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();
}

33
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
View File

@@ -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();
}

30
wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
View File

@@ -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;
};

41
wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
View File

@@ -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();
};

23
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
View File

@@ -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);
};

24
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
View File

@@ -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;
};

27
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
View File

@@ -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;
};

24
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
View File

@@ -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;
};

22
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
View File

@@ -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);
};

20
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
View File

@@ -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);
};

21
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
View File

@@ -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);
};

27
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
View File

@@ -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;
};

29
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
View File

@@ -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;
};

27
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
View File

@@ -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;
};

29
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
View File

@@ -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;
};

55
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
View File

@@ -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};
};

80
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
View File

@@ -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};
};

59
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
View File

@@ -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;
};

53
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
View File

@@ -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;
};

72
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
View File

@@ -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

70
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
View File

@@ -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();
}

33
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
View File

@@ -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();
}

38
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
View File

@@ -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();
}

68
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -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);
}

64
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
View File

@@ -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

32
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
View File

@@ -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;
};

44
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
View File

@@ -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();
};

26
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
View File

@@ -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;
};

30
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
View File

@@ -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;
};

105
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
View File

@@ -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;
};

80
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -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.",

65
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
View File

@@ -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.
package edu.wpi.first.wpilibj.examples.armbot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ArmConstants {
public static final int kMotorPort = 4;
public static final double kP = 1;
// These are fake gains; in actuality these must be determined individually for each robot
public static final double kSVolts = 1;
public static final double kGVolts = 1;
public static final double kVVoltSecondPerRad = 0.5;
public static final double kAVoltSecondSquaredPerRad = 0.1;
public static final double kMaxVelocityRadPerSecond = 3;
public static final double kMaxAccelerationRadPerSecSquared = 10;
public static final int[] kEncoderPorts = new int[] {4, 5};
public static final int kEncoderPPR = 256;
public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
// The offset of the arm from the horizontal in its neutral position,
// measured from the horizontal
public static final double kArmOffsetRads = 0.5;
}
public static final class AutoConstants {
public static final double kAutoTimeoutSeconds = 12;
public static final double kAutoShootTimeSeconds = 7;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
View File

@@ -1,25 +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.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

102
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
View File

@@ -1,102 +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.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically corresponding to each mode, as described in
* the TimedRobot documentation. If you change the name of this class or the package after creating
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {
m_robotContainer.disablePIDSubsystems();
}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void 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 != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

103
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
View File

@@ -1,103 +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.
package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* 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.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* JoystickButton}.
*/
private void configureButtonBindings() {
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
m_driverController
.a()
.onTrue(
Commands.runOnce(
() -> {
m_robotArm.setGoal(2);
m_robotArm.enable();
},
m_robotArm));
// Move the arm to neutral position when the 'B' button is pressed.
m_driverController
.b()
.onTrue(
Commands.runOnce(
() -> {
m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
m_robotArm.enable();
},
m_robotArm));
// Disable the arm controller when Y is pressed.
m_driverController.y().onTrue(Commands.runOnce(m_robotArm::disable));
// Drive at half speed when the bumper is held
m_driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1.0)));
}
/**
* Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This should be called on robot
* disable to prevent integral windup.
*/
public void disablePIDSubsystems() {
m_robotArm.disable();
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return Commands.none();
}
}

60
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
View File

@@ -1,60 +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.
package edu.wpi.first.wpilibj.examples.armbot.subsystems;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends ProfiledPIDSubsystem {
private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort);
private final Encoder m_encoder =
new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(
ArmConstants.kSVolts, ArmConstants.kGVolts,
ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/** Create a new ArmSubsystem. */
public ArmSubsystem() {
super(
new ProfiledPIDController(
ArmConstants.kP,
0,
0,
new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond,
ArmConstants.kMaxAccelerationRadPerSecSquared)),
0);
m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(ArmConstants.kArmOffsetRads);
}
@Override
public void useOutput(double output, TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward =
m_feedforward
.calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
.in(Volts);
// Add the feedforward to the PID output to get the motor output
m_motor.setVoltage(output + feedforward);
}
@Override
public double getMeasurement() {
return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
View File

@@ -1,110 +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.
package edu.wpi.first.wpilibj.examples.armbot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* 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
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

56
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
View File

@@ -1,56 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ArmConstants {
public static final int kMotorPort = 4;
public static final double kP = 1;
// These are fake gains; in actuality these must be determined individually for each robot
public static final double kSVolts = 1;
public static final double kGVolts = 1;
public static final double kVVoltSecondPerRad = 0.5;
public static final double kAVoltSecondSquaredPerRad = 0.1;
public static final double kMaxVelocityRadPerSecond = 3;
public static final double kMaxAccelerationRadPerSecSquared = 10;
// The offset of the arm from the horizontal in its neutral position,
// measured from the horizontal
public static final double kArmOffsetRads = 0.5;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
}

88
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
View File

@@ -1,88 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
public class ExampleSmartMotorController {
public enum PIDMode {
kPosition,
kVelocity,
kMovementWitchcraft
}
/**
* Creates a new ExampleSmartMotorController.
*
* @param port The port for the controller.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
public 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.
*/
public 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).
*/
public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
/**
* Places this motor controller in follower mode.
*
* @param leader The leader to follow.
*/
public void follow(ExampleSmartMotorController leader) {}
/**
* Returns the encoder distance.
*
* @return The current encoder distance.
*/
public double getEncoderDistance() {
return 0;
}
/**
* Returns the encoder rate.
*
* @return The current encoder rate.
*/
public double getEncoderRate() {
return 0;
}
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
public void set(double speed) {}
public double get() {
return 0;
}
public void setInverted(boolean isInverted) {}
public boolean getInverted() {
return false;
}
public void disable() {}
public void stopMotor() {}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
View File

@@ -1,25 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
View File

@@ -1,100 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically corresponding to each mode, as described in
* the TimedRobot documentation. If you change the name of this class or the package after creating
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void 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 != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

75
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
View File

@@ -1,75 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* 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.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ArmSubsystem m_robotArm = new ArmSubsystem();
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
m_robotDrive.arcadeDriveCommand(
() -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* JoystickButton}.
*/
private void configureButtonBindings() {
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2));
// Move the arm to neutral position when the 'B' button is pressed.
m_driverController
.b()
.onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads));
// Drive at half speed when the bumper is held
m_driverController
.rightBumper()
.onTrue(m_robotDrive.limitOutputCommand(0.5))
.onFalse(m_robotDrive.limitOutputCommand(1));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return Commands.none();
}
}

52
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
View File

@@ -1,52 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
private final ExampleSmartMotorController m_motor =
new ExampleSmartMotorController(ArmConstants.kMotorPort);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(
ArmConstants.kSVolts, ArmConstants.kGVolts,
ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
/** Create a new ArmSubsystem. */
public ArmSubsystem() {
super(
new TrapezoidProfile.Constraints(
ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
ArmConstants.kArmOffsetRads);
m_motor.setPID(ArmConstants.kP, 0, 0);
}
@Override
public void useState(TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double feedforward =
m_feedforward
.calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity))
.in(Volts);
// Add the feedforward to the PID output to get the motor output
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
}
public Command setArmGoalCommand(double kArmOffsetRads) {
return Commands.runOnce(() -> setGoal(kArmOffsetRads), this);
}
}

114
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
View File

@@ -1,114 +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.
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
}
/**
* A split-stick arcade command, with forward/backward controlled by the left hand, and turning
* controlled by the right.
*
* @param fwd supplier for the commanded forward movement
* @param rot supplier for the commanded rotation
*/
public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
return Commands.run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()), this);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* 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
*/
public Command limitOutputCommand(double maxOutput) {
return Commands.runOnce(() -> m_drive.setMaxOutput(maxOutput));
}
}

85
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -276,25 +276,6 @@
"mainclass": "Main",
"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": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Simple Vision",
"description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
@@ -425,40 +406,6 @@
"mainclass": "Main",
"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": "java",
"mainclass": "Main",
"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": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "SwerveBot",
"description": "Use kinematics and odometry with a swerve drive.",
@@ -583,38 +530,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "ArmBot",
"description": "Control an arm with ProfiledPIDSubsystem.",
"tags": [
"Command-based",
"Arm",
"Encoder",
"Profiled PID",
"XboxController",
"Differential Drive"
],
"foldername": "armbot",
"gradlebase": "java",
"mainclass": "Main",
"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": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "DriveDistanceOffboard",
"description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",

72
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
View File

@@ -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.
package edu.wpi.first.wpilibj.examples.frisbeebot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotor1Port = 0;
public static final int kLeftMotor2Port = 1;
public static final int kRightMotor1Port = 2;
public static final int kRightMotor2Port = 3;
public static final int[] kLeftEncoderPorts = new int[] {0, 1};
public static final int[] kRightEncoderPorts = new int[] {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ShooterConstants {
public static final int[] kEncoderPorts = new int[] {4, 5};
public static final boolean kEncoderReversed = false;
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
// Distance units will be rotations
1.0 / kEncoderCPR;
public static final int kShooterMotorPort = 4;
public static final int kFeederMotorPort = 5;
public static final double kShooterFreeRPS = 5300;
public static final double kShooterTargetRPS = 4000;
public static final double kShooterToleranceRPS = 50;
// These are not real PID gains, and will have to be tuned for your specific robot.
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSVolts = 0.05;
public static final double kVVoltSecondsPerRotation =
// Should have value 12V at free speed...
12.0 / kShooterFreeRPS;
public static final double kFeederSpeed = 0.5;
}
public static final class AutoConstants {
public static final double kAutoTimeoutSeconds = 12;
public static final double kAutoShootTimeSeconds = 7;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 0;
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
View File

@@ -1,25 +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.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
View File

@@ -1,100 +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.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically corresponding to each mode, as described in
* the TimedRobot documentation. If you change the name of this class or the package after creating
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void 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 != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

124
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
View File

@@ -1,124 +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.
package edu.wpi.first.wpilibj.examples.frisbeebot;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
/**
* 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.
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final ShooterSubsystem m_shooter = new ShooterSubsystem();
private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter);
private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter);
// An autonomous routine that shoots the loaded frisbees
Command m_autonomousCommand =
Commands.sequence(
// Start the command by spinning up the shooter...
Commands.runOnce(m_shooter::enable, m_shooter),
// Wait until the shooter is at speed before feeding the frisbees
Commands.waitUntil(m_shooter::atSetpoint),
// Start running the feeder
Commands.runOnce(m_shooter::runFeeder, m_shooter),
// Shoot for the specified time
Commands.waitSeconds(AutoConstants.kAutoShootTimeSeconds))
// Add a timeout (will end the command if, for instance, the shooter
// never gets up to speed)
.withTimeout(AutoConstants.kAutoTimeoutSeconds)
// When the command ends, turn off the shooter and the feeder
.andThen(
Commands.runOnce(
() -> {
m_shooter.disable();
m_shooter.stopFeeder();
}));
// The driver's controller
CommandXboxController m_driverController =
new CommandXboxController(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created via the button
* factories on {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID} or one of its
* subclasses ({@link edu.wpi.first.wpilibj2.command.button.CommandJoystick} or {@link
* CommandXboxController}).
*/
private void configureButtonBindings() {
// Configure your button bindings here
// We can bind commands while retaining references to them in RobotContainer
// Spin up the shooter when the 'A' button is pressed
m_driverController.a().onTrue(m_spinUpShooter);
// Turn off the shooter when the 'B' button is pressed
m_driverController.b().onTrue(m_stopShooter);
// We can also write them as temporary variables outside the bindings
// Shoots if the shooter wheel has reached the target speed
Command shoot =
Commands.either(
// Run the feeder
Commands.runOnce(m_shooter::runFeeder, m_shooter),
// Do nothing
Commands.none(),
// Determine which of the above to do based on whether the shooter has reached the
// desired speed
m_shooter::atSetpoint);
Command stopFeeder = Commands.runOnce(m_shooter::stopFeeder, m_shooter);
// Shoot when the 'X' button is pressed
m_driverController.x().onTrue(shoot).onFalse(stopFeeder);
// We can also define commands inline at the binding!
// While holding the shoulder button, drive at half speed
m_driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1)));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autonomousCommand;
}
}

110
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
View File

@@ -1,110 +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.
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
// 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_rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
* Drives the robot using arcade controls.
*
* @param fwd the commanded forward movement
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Gets the average distance of the two encoders.
*
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
* Gets the left drive encoder.
*
* @return the left drive encoder
*/
public Encoder getLeftEncoder() {
return m_leftEncoder;
}
/**
* Gets the right drive encoder.
*
* @return the right drive encoder
*/
public Encoder getRightEncoder() {
return m_rightEncoder;
}
/**
* 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
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
}
}

62
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
View File

@@ -1,62 +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.
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final Encoder m_shooterEncoder =
new Encoder(
ShooterConstants.kEncoderPorts[0],
ShooterConstants.kEncoderPorts[1],
ShooterConstants.kEncoderReversed);
private final SimpleMotorFeedforward m_shooterFeedforward =
new SimpleMotorFeedforward(
ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
/** The shooter subsystem for the robot. */
public ShooterSubsystem() {
super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
setSetpoint(ShooterConstants.kShooterTargetRPS);
}
@Override
public void useOutput(double output, double setpoint) {
m_shooterMotor.setVoltage(
output
+ m_shooterFeedforward
.calculate(RadiansPerSecond.of(ShooterConstants.kShooterTargetRPS))
.in(Volts));
}
@Override
public double getMeasurement() {
return m_shooterEncoder.getRate();
}
public boolean atSetpoint() {
return m_controller.atSetpoint();
}
public void runFeeder() {
m_feederMotor.set(ShooterConstants.kFeederSpeed);
}
public void stopFeeder() {
m_feederMotor.set(0);
}
}

94
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java
View File

@@ -1,94 +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.
package edu.wpi.first.wpilibj.examples.gearsbot;
public final class Constants {
public static final class DriveConstants {
public static final int kLeftMotorPort1 = 0;
public static final int kLeftMotorPort2 = 1;
public static final int kRightMotorPort1 = 2;
public static final int kRightMotorPort2 = 3;
public static final int[] kLeftEncoderPorts = {0, 1};
public static final int[] kRightEncoderPorts = {2, 3};
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = false;
public static final int kRangeFinderPort = 6;
public static final int kAnalogGyroPort = 1;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterInches = 6;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * Math.PI) / kEncoderCPR;
}
public static final class ClawConstants {
public static final int kMotorPort = 7;
public static final int kContactPort = 5;
}
public static final class WristConstants {
public static final int kMotorPort = 6;
// these pid constants are not real, and will need to be tuned
public static final double kP = 0.1;
public static final double kI = 0.0;
public static final double kD = 0.0;
public static final double kTolerance = 2.5;
public static final int kPotentiometerPort = 3;
}
public static final class ElevatorConstants {
public static final int kMotorPort = 5;
public static final int kPotentiometerPort = 2;
// these pid constants are not real, and will need to be tuned
public static final double kP_real = 4;
public static final double kI_real = 0.007;
public static final double kP_simulation = 18;
public static final double kI_simulation = 0.2;
public static final double kD = 0.0;
public static final double kTolerance = 0.005;
}
public static final class AutoConstants {
public static final double kDistToBox1 = 0.10;
public static final double kDistToBox2 = 0.60;
public static final double kWristSetpoint = -45.0;
}
public static final class DriveStraightConstants {
// these pid constants are not real, and will need to be tuned
public static final double kP = 4.0;
public static final double kI = 0.0;
public static final double kD = 0.0;
}
public static final class Positions {
public static final class Pickup {
public static final double kWristSetpoint = -45.0;
public static final double kElevatorSetpoint = 0.25;
}
public static final class Place {
public static final double kWristSetpoint = 0.0;
public static final double kElevatorSetpoint = 0.25;
}
public static final class PrepareToPickup {
public static final double kWristSetpoint = 0.0;
public static final double kElevatorSetpoint = 0.0;
}
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
View File

@@ -1,25 +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.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

93
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
View File

@@ -1,93 +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.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The methods in this class are called automatically corresponding to each mode, as described in
* the TimedRobot documentation. If you change the name of this class or the package after creating
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void 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 != null) {
m_autonomousCommand.cancel();
}
}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}

109
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
View File

@@ -1,109 +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.
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
/**
* 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.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final Elevator m_elevator = new Elevator();
private final Wrist m_wrist = new Wrist();
private final Claw m_claw = new Claw();
private final XboxController m_joystick = new XboxController(0);
private final Command m_autonomousCommand =
new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.25, m_elevator));
SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
SmartDashboard.putData(
"Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
// Assign default commands
m_drivetrain.setDefaultCommand(
new TankDrive(() -> -m_joystick.getLeftY(), () -> -m_joystick.getRightY(), m_drivetrain));
// Show what command your subsystem is running on the SmartDashboard
SmartDashboard.putData(m_drivetrain);
SmartDashboard.putData(m_elevator);
SmartDashboard.putData(m_wrist);
SmartDashboard.putData(m_claw);
// Configure the button bindings
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Create some buttons
final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
final JoystickButton l2 = new JoystickButton(m_joystick, 9);
final JoystickButton r2 = new JoystickButton(m_joystick, 10);
final JoystickButton l1 = new JoystickButton(m_joystick, 11);
final JoystickButton r1 = new JoystickButton(m_joystick, 12);
// Connect the buttons to commands
dpadUp.onTrue(new SetElevatorSetpoint(0.25, m_elevator));
dpadDown.onTrue(new SetElevatorSetpoint(0.0, m_elevator));
dpadRight.onTrue(new CloseClaw(m_claw));
dpadLeft.onTrue(new OpenClaw(m_claw));
r1.onTrue(new PrepareToPickup(m_claw, m_wrist, m_elevator));
r2.onTrue(new Pickup(m_claw, m_wrist, m_elevator));
l1.onTrue(new Place(m_claw, m_wrist, m_elevator));
l2.onTrue(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_autonomousCommand;
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
View File

@@ -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.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
/** The main autonomous command to pickup and deliver the soda to the box. */
public class Autonomous extends SequentialCommandGroup {
/** Create a new autonomous command. */
public Autonomous(Drivetrain drive, Claw claw, Wrist wrist, Elevator elevator) {
addCommands(
new PrepareToPickup(claw, wrist, elevator),
new Pickup(claw, wrist, elevator),
new SetDistanceToBox(AutoConstants.kDistToBox1, drive),
// new DriveStraight(4), // Use encoders if ultrasonic is broken
new Place(claw, wrist, elevator),
new SetDistanceToBox(AutoConstants.kDistToBox2, drive),
// new DriveStraight(-2), // Use Encoders if ultrasonic is broken
Commands.parallel(
new SetWristSetpoint(AutoConstants.kWristSetpoint, wrist), new CloseClaw(claw)));
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
View File

@@ -1,43 +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.
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj2.command.Command;
/** Closes the claw until the limit switch is tripped. */
public class CloseClaw extends Command {
private final Claw m_claw;
public CloseClaw(Claw claw) {
m_claw = claw;
addRequirements(m_claw);
}
// Called just before this Command runs the first time
@Override
public void initialize() {
m_claw.close();
}
// Make this return true when this Command no longer needs to run execute()
@Override
public boolean isFinished() {
return m_claw.isGrabbing();
}
// Called once after isFinished returns true
@Override
public void end(boolean interrupted) {
// 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 (!Robot.isSimulation()) {
m_claw.stop();
}
}
}

Some files were not shown because too many files have changed in this diff Show More