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

Add TrapezoidProfile external PID examples (#2131)

Browse Source
This commit is contained in:
Oblarg
2019-12-07 16:37:54 -05:00
committed by Peter Johnson
parent 5c6b8a0f45
commit ccdd0fbdb2
54 changed files with 2734 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
View File

@@ -36,4 +36,4 @@ void PIDCommand::Execute() {
void PIDCommand::End(bool interrupted) { m_useOutput(0); }
PIDController& PIDCommand::getController() { return m_controller; }
PIDController& PIDCommand::GetController() { return m_controller; }

2
wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
View File

@@ -72,7 +72,7 @@ class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
*
* @return The PIDController
*/
PIDController& getController();
PIDController& GetController();
protected:
PIDController m_controller;

71
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp Normal file
View File

@@ -0,0 +1,71 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/CommandScheduler.h>
void Robot::RobotInit() {}
/**
* This function is called every robot packet, 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

51
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp Normal file
View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc2/command/button/JoystickButton.h>
#include <units/units.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.GetY(frc::GenericHID::kLeftHand),
m_driverController.GetX(frc::GenericHID::kRightHand));
},
{&m_drive}));
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
// Move the arm to neutral position when the 'B' button is pressed.
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
{&m_arm});
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
.WhenPressed([this] { m_drive.SetMaxOutput(.5); })
.WhenReleased([this] { m_drive.SetMaxOutput(1); });
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return new frc2::InstantCommand([] {});
}

30
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#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, kCos, 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.to<double>(), feedforward / 12_V);
}

47
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp Normal file
View File

@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#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]} {
// 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);
}

71
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h Normal file
View File

@@ -0,0 +1,71 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include <wpi/math>
/**
* 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 {
constexpr int kLeftMotor1Port = 0;
constexpr int kLeftMotor2Port = 1;
constexpr int kRightMotor1Port = 2;
constexpr int kRightMotor2Port = 3;
constexpr int kLeftEncoderPorts[]{0, 1};
constexpr int kRightEncoderPorts[]{2, 3};
constexpr bool kLeftEncoderReversed = false;
constexpr bool kRightEncoderReversed = true;
constexpr int kEncoderCPR = 1024;
constexpr double kWheelDiameterInches = 6;
constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
} // namespace DriveConstants
namespace ArmConstants {
constexpr int kMotorPort = 4;
constexpr double kP = 1;
// These are fake gains; in actuality these must be determined individually for
// each robot
constexpr auto kS = 1_V;
constexpr auto kCos = 1_V;
constexpr auto kV = 0.5_V * 1_s / 1_rad;
constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
constexpr auto kMaxVelocity = 3_rad_per_s;
constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
constexpr int kEncoderPorts[]{4, 5};
constexpr int kEncoderPPR = 256;
constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
// The offset of the arm from the horizontal in its neutral position,
// measured from the horizontal
constexpr auto kArmOffset = 0.5_rad;
} // namespace ArmConstants
namespace AutoConstants {
constexpr auto kAutoTimeoutSeconds = 12_s;
constexpr auto kAutoShootTimeSeconds = 7_s;
} // namespace AutoConstants
namespace OIConstants {
constexpr int kDriverControllerPort = 1;
} // namespace OIConstants

87
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/SpeedController.h>
/**
* A simplified stub class that simulates the API of a common "smart" motor
* controller.
*
* <p>Has no actual functionality.
*/
class ExampleSmartMotorController : public frc::SpeedController {
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 master The master to follow.
*/
void Follow(ExampleSmartMotorController master) {}
/**
* 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) override {}
double Get() const override { return 0; }
void SetInverted(bool isInverted) override {}
bool GetInverted() const override { return false; }
void Disable() override {}
void StopMotor() override {}
void PIDWrite(double output) override {}
};

33
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
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;
};

53
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/XboxController.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/ConditionalCommand.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
#include <frc2/command/WaitCommand.h>
#include <frc2/command/WaitUntilCommand.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::Command* GetAutonomousCommand();
private:
// The driver's controller
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
ArmSubsystem m_arm;
// The chooser for the autonomous routines
void ConfigureButtonBindings();
};

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

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/controller/ArmFeedforward.h>
#include <frc2/command/TrapezoidProfileSubsystem.h>
#include <units/units.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;
private:
ExampleSmartMotorController m_motor;
frc::ArmFeedforward m_feedforward;
};

95
wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h Normal file
View File

@@ -0,0 +1,95 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.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::PWMVictorSPX m_left1;
frc::PWMVictorSPX m_left2;
frc::PWMVictorSPX m_right1;
frc::PWMVictorSPX m_right2;
// The motors on the left side of the drive
frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
// The motors on the right side of the drive
frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
// The robot's drive
frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
// The left-side drive encoder
frc::Encoder m_leftEncoder;
// The right-side drive encoder
frc::Encoder m_rightEncoder;
};

71
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp Normal file
View File

@@ -0,0 +1,71 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/CommandScheduler.h>
void Robot::RobotInit() {}
/**
* This function is called every robot packet, 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

67
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp Normal file
View File

@@ -0,0 +1,67 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc2/command/button/JoystickButton.h>
#include "commands/DriveDistanceProfiled.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.GetY(frc::GenericHID::kLeftHand),
m_driverController.GetX(frc::GenericHID::kRightHand));
},
{&m_drive}));
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
.WhenPressed(&m_driveHalfSpeed)
.WhenReleased(&m_driveFullSpeed);
// Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
// 10 seconds
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
// Do the same thing as above when the 'B' button is pressed, but defined
// inline
frc2::JoystickButton(&m_driverController, 2)
.WhenPressed(
frc2::TrapezoidProfileCommand<units::meters>(
frc::TrapezoidProfile<units::meters>(
// Limit the max acceleration and velocity
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{3_m, 0_mps}),
// Pipe the profile state to the drive
[this](auto setpointState) {
m_drive.SetDriveStates(setpointState, setpointState);
},
// Require the drive
{&m_drive})
.BeforeStarting([this]() { m_drive.ResetEncoders(); })
.WithTimeout(10_s));
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return new frc2::InstantCommand([] {});
}

30
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/DriveDistanceProfiled.h"
#include "Constants.h"
using namespace DriveConstants;
DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
DriveSubsystem* drive)
: CommandHelper(
frc::TrapezoidProfile<units::meters>(
// Limit the max acceleration and velocity
{kMaxSpeed, kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{distance, 0_mps}),
// Pipe the profile state to the drive
[drive](auto setpointState) {
drive->SetDriveStates(setpointState, setpointState);
},
// Require the drive
{drive}) {
// Reset drive encoders since we're starting at 0
drive->ResetEncoders();
}

59
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp Normal file
View File

@@ -0,0 +1,59 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "subsystems/DriveSubsystem.h"
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
: m_leftMaster{kLeftMotor1Port},
m_leftSlave{kLeftMotor2Port},
m_rightMaster{kRightMotor1Port},
m_rightSlave{kRightMotor2Port},
m_feedforward{ks, kv, ka} {
m_leftSlave.Follow(m_leftMaster);
m_rightSlave.Follow(m_rightMaster);
m_leftMaster.SetPID(kp, 0, 0);
m_rightMaster.SetPID(kp, 0, 0);
}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
}
void DriveSubsystem::SetDriveStates(
frc::TrapezoidProfile<units::meters>::State left,
frc::TrapezoidProfile<units::meters>::State right) {
m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
left.position.to<double>(),
m_feedforward.Calculate(left.velocity) / 12_V);
m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
right.position.to<double>(),
m_feedforward.Calculate(right.velocity) / 12_V);
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::ResetEncoders() {
m_leftMaster.ResetEncoder();
m_rightMaster.ResetEncoder();
}
units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
return units::meter_t(m_leftMaster.GetEncoderDistance());
}
units::meter_t DriveSubsystem::GetRightEncoderDistance() {
return units::meter_t(m_rightMaster.GetEncoderDistance());
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}

46
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h Normal file
View File

@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include <wpi/math>
/**
* 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 {
constexpr int kLeftMotor1Port = 0;
constexpr int kLeftMotor2Port = 1;
constexpr int kRightMotor1Port = 2;
constexpr int kRightMotor2Port = 3;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The RobotPy Characterization
// Toolsuite provides a convenient tool for obtaining these values for your
// robot.
constexpr auto ks = 1_V;
constexpr auto kv = 0.8_V * 1_s / 1_m;
constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
constexpr double kp = 1;
constexpr auto kMaxSpeed = 3_mps;
constexpr auto kMaxAcceleration = 3_mps_sq;
} // namespace DriveConstants
namespace OIConstants {
constexpr int kDriverControllerPort = 1;
} // namespace OIConstants

87
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/SpeedController.h>
/**
* A simplified stub class that simulates the API of a common "smart" motor
* controller.
*
* <p>Has no actual functionality.
*/
class ExampleSmartMotorController : public frc::SpeedController {
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 master The master to follow.
*/
void Follow(ExampleSmartMotorController master) {}
/**
* 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) override {}
double Get() const override { return 0; }
void SetInverted(bool isInverted) override {}
bool GetInverted() const override { return false; }
void Disable() override {}
void StopMotor() override {}
void PIDWrite(double output) override {}
};

33
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include "RobotContainer.h"
class Robot : public frc::TimedRobot {
public:
void RobotInit() override;
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;
};

50
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h Normal file
View File

@@ -0,0 +1,50 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/XboxController.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include <frc2/command/SequentialCommandGroup.h>
#include <frc2/command/StartEndCommand.h>
#include "Constants.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();
frc2::Command* GetAutonomousCommand();
private:
// The driver's controller
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
void ConfigureButtonBindings();
};

20
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h Normal file
View File

@@ -0,0 +1,20 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc2/command/CommandHelper.h>
#include <frc2/command/TrapezoidProfileCommand.h>
#include "subsystems/DriveSubsystem.h"
class DriveDistanceProfiled
: public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
DriveDistanceProfiled> {
public:
DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
};

90
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h Normal file
View File

@@ -0,0 +1,90 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/Encoder.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <frc2/command/SubsystemBase.h>
#include <units/units.h>
#include "Constants.h"
#include "ExampleSmartMotorController.h"
class DriveSubsystem : public frc2::SubsystemBase {
public:
DriveSubsystem();
/**
* Will be called periodically whenever the CommandScheduler runs.
*/
void Periodic() override;
// Subsystem methods go here.
/**
* Attempts to follow the given drive states using offboard PID.
*
* @param left The left wheel state.
* @param right The right wheel state.
*/
void SetDriveStates(frc::TrapezoidProfile<units::meters>::State left,
frc::TrapezoidProfile<units::meters>::State right);
/**
* 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 distance of the left encoder.
*
* @return the average of the TWO encoder readings
*/
units::meter_t GetLeftEncoderDistance();
/**
* Gets the distance of the right encoder.
*
* @return the average of the TWO encoder readings
*/
units::meter_t GetRightEncoderDistance();
/**
* 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
ExampleSmartMotorController m_leftMaster;
ExampleSmartMotorController m_leftSlave;
ExampleSmartMotorController m_rightMaster;
ExampleSmartMotorController m_rightSlave;
// A feedforward component for the drive
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
// The robot's drive
frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
};

25
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -5,20 +5,22 @@
/* the project. */
/*----------------------------------------------------------------------------*/
#include <frc/Encoder.h>
#include <frc/Joystick.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <units/units.h>
#include <wpi/math>
#include "ExampleSmartMotorController.h"
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
Robot() {
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.SetPID(1.3, 0.0, 0.7);
}
void TeleopPeriodic() override {
@@ -38,17 +40,18 @@ class Robot : public frc::TimedRobot {
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
// Run controller with profiled setpoint and update motor output
double output = m_controller.Calculate(m_encoder.GetDistance(),
m_setpoint.position.to<double>());
m_motor.Set(output);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
m_setpoint.position.to<double>(),
m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
}
private:
frc::Joystick m_joystick{1};
frc::Encoder m_encoder{1, 2};
frc::PWMVictorSPX m_motor{1};
frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
ExampleSmartMotorController m_motor{1};
frc::SimpleMotorFeedforward<units::meters> m_feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1.5_V * 1_s / 1_m};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};

87
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/SpeedController.h>
/**
* A simplified stub class that simulates the API of a common "smart" motor
* controller.
*
* <p>Has no actual functionality.
*/
class ExampleSmartMotorController : public frc::SpeedController {
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 master The master to follow.
*/
void Follow(ExampleSmartMotorController master) {}
/**
* 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) override {}
double Get() const override { return 0; }
void SetInverted(bool isInverted) override {}
bool GetInverted() const override { return false; }
void Disable() override {}
void StopMotor() override {}
void PIDWrite(double output) override {}
};

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

@@ -8,6 +8,9 @@
#include "RobotContainer.h"
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc2/command/PIDCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include <frc2/command/button/JoystickButton.h>
RobotContainer::RobotContainer() {
@@ -29,16 +32,39 @@ RobotContainer::RobotContainer() {
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// Assorted commands to be bound to buttons
// Stabilize robot to drive straight with gyro when left bumper is held
frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
frc2::JoystickButton(&m_driverController, 5)
.WhenHeld(frc2::PIDCommand{
frc2::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.GetY(
frc::GenericHID::JoystickHand::kLeftHand),
output);
},
// Require the robot drive
{&m_drive}});
// Turn to 90 degrees when the 'X' button is pressed
frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
frc2::JoystickButton(&m_driverController, 3)
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
// second timeout
frc2::JoystickButton(&m_driverController, 1)
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController, 6)
.WhenPressed(&m_driveHalfSpeed)
.WhenReleased(&m_driveFullSpeed);
.WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
.WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
}
frc2::Command* RobotContainer::GetAutonomousCommand() {

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

@@ -11,12 +11,12 @@
using namespace DriveConstants;
TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
: CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
[drive] { return drive->GetHeading(); },
[drive] { return drive->GetHeading().to<double>(); },
// Set reference to target
targetAngleDegrees,
target.to<double>(),
// Pipe output to turn robot
[drive](double output) { drive->ArcadeDrive(0, output); },
// Require the drive
@@ -26,9 +26,10 @@ TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
// 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(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
m_controller.SetTolerance(kTurnTolerance.to<double>(),
kTurnRateTolerance.to<double>());
AddRequirements({drive});
}
bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }

39
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#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(); }

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

@@ -46,8 +46,9 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
double DriveSubsystem::GetHeading() {
return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
units::degree_t DriveSubsystem::GetHeading() {
return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) *
(kGyroReversed ? -1.0 : 1.0));
}
double DriveSubsystem::GetTurnRate() {

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

@@ -7,6 +7,7 @@
#pragma once
#include <units/units.h>
#include <wpi/math>
/**
@@ -45,8 +46,11 @@ constexpr double kTurnP = 1;
constexpr double kTurnI = 0;
constexpr double kTurnD = 0;
constexpr double kTurnToleranceDeg = 5;
constexpr double kTurnRateToleranceDegPerS = 10; // degrees per second
constexpr auto kTurnTolerance = 5_deg;
constexpr auto kTurnRateTolerance = 10_deg_per_s;
constexpr auto kMaxTurnRate = 100_deg_per_s;
constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
} // namespace DriveConstants
namespace AutoConstants {

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

@@ -12,9 +12,6 @@
#include <frc/smartdashboard/SendableChooser.h>
#include <frc2/command/Command.h>
#include <frc2/command/InstantCommand.h>
#include <frc2/command/PIDCommand.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/RunCommand.h>
#include "Constants.h"
#include "commands/TurnToAngle.h"
@@ -45,34 +42,6 @@ class RobotContainer {
// The robot's subsystems
DriveSubsystem m_drive;
// Assorted commands to be bound to buttons
// Turn to 90 degrees, with a 5 second timeout
frc2::ParallelRaceGroup m_turnTo90 =
TurnToAngle{90, &m_drive}.WithTimeout(5_s);
// Stabilize the robot while driving
frc2::PIDCommand m_stabilizeDriving{
frc2::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.GetY(frc::GenericHID::JoystickHand::kLeftHand),
output);
},
// Require the robot drive
{&m_drive}};
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
{}};
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
{}};
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;

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

@@ -23,7 +23,7 @@ class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
TurnToAngle(units::degree_t target, DriveSubsystem* drive);
bool IsFinished() override;
};

33
wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <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;
};

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

@@ -13,6 +13,7 @@
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc2/command/SubsystemBase.h>
#include <units/units.h>
#include "Constants.h"
@@ -74,7 +75,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
*
* @return the robot's heading in degrees, from 180 to 180
*/
double GetHeading();
units::degree_t GetHeading();
/**
* Returns the turn rate of the robot.

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

@@ -508,5 +508,29 @@
"foldername": "ArmBot",
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "ArmBotOffboard",
"description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
"tags": [
"ArmBotOffboard",
"PID",
"Motion Profile"
],
"foldername": "ArmBotOffboard",
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "DriveDistanceOffboard",
"description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
"tags": [
"DriveDistance",
"PID",
"Motion Profile"
],
"foldername": "DriveDistanceOffboard",
"gradlebase": "cpp",
"commandversion": 2
}
]

59
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java Normal file
View File

@@ -0,0 +1,59 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
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) / (double) 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 kCosVolts = 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 = 1;
}
}

115
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java Normal file
View File

@@ -0,0 +1,115 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.SpeedController;
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
public class ExampleSmartMotorController implements SpeedController {
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 master The master to follow.
*/
public void follow(ExampleSmartMotorController master) {
}
/**
* 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() {
}
@Override
public void set(double speed) {
}
@Override
public double get() {
return 0;
}
@Override
public void setInverted(boolean isInverted) {
}
@Override
public boolean getInverted() {
return false;
}
@Override
public void disable() {
}
@Override
public void stopMotor() {
}
@Override
public void pidWrite(double output) {
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
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);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
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 VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// 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 robot packet, 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() {
}
}

86
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java Normal file
View File

@@ -0,0 +1,86 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants.kDriverControllerPort;
/**
* 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
XboxController m_driverController = new XboxController(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.
new RunCommand(() -> m_robotDrive
.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link 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.
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
// Move the arm to neutral position when the 'B' button is pressed.
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
// Drive at half speed when the bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> 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 new InstantCommand();
}
}

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

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kArmOffsetRads;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kCosVolts;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxVelocityRadPerSecond;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMotorPort;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kP;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kSVolts;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kVVoltSecondPerRad;
/**
* A robot arm subsystem that moves with a motion profile.
*/
public class ArmSubsystem extends TrapezoidProfileSubsystem {
private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(kMotorPort);
private final ArmFeedforward m_feedforward =
new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
/**
* Create a new ArmSubsystem.
*/
public ArmSubsystem() {
super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
kMaxAccelerationRadPerSecSquared),
kArmOffsetRads);
m_motor.setPID(kP, 0, 0);
}
@Override
public void useState(TrapezoidProfile.State setpoint) {
// Calculate the feedforward from the sepoint
double 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,
feedforward / 12.0);
}
}

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

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderPorts;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderReversed;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderPorts;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderReversed;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor2Port;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
new PWMVictorSPX(kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
new PWMVictorSPX(kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(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);
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
/**
* 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;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or theoretically
// for *your* robot's drive.
// The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 1;
public static final double kvVoltSecondsPerMeter = 0.8;
public static final double kaVoltSecondsSquaredPerMeter = 0.15;
public static final double kp = 1;
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
}
public static final class OIConstants {
public static final int kDriverControllerPort = 1;
}
}

115
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java Normal file
View File

@@ -0,0 +1,115 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
import edu.wpi.first.wpilibj.SpeedController;
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
public class ExampleSmartMotorController implements SpeedController {
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 master The master to follow.
*/
public void follow(ExampleSmartMotorController master) {
}
/**
* 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() {
}
@Override
public void set(double speed) {
}
@Override
public double get() {
return 0;
}
@Override
public void setInverted(boolean isInverted) {
}
@Override
public boolean getInverted() {
return false;
}
@Override
public void disable() {
}
@Override
public void stopMotor() {
}
@Override
public void pidWrite(double output) {
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
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);
}
}

121
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// 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 robot packet, 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() {
}
}

103
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java Normal file
View File

@@ -0,0 +1,103 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants.kDriverControllerPort;
/**
* 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();
// The driver's controller
XboxController m_driverController = new XboxController(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.
new RunCommand(() -> m_robotDrive
.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link 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() {
// Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10));
// Do the same thing as above when the 'B' button is pressed, but defined inline
new JoystickButton(m_driverController, Button.kB.value)
.whenPressed(
new TrapezoidProfileCommand(
new TrapezoidProfile(
// Limit the max acceleration and velocity
new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(3, 0)),
// Pipe the profile state to the drive
setpointState -> m_robotDrive.setDriveStates(
setpointState,
setpointState),
// Require the drive
m_robotDrive)
.beforeStarting(m_robotDrive::resetEncoders)
.withTimeout(10));
// Drive at half speed when the bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
.whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> 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 new InstantCommand();
}
}

43
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
/**
* Drives a set distance using a motion profile.
*/
public class DriveDistanceProfiled extends TrapezoidProfileCommand {
/**
* Creates a new DriveDistanceProfiled command.
*
* @param meters The distance to drive.
* @param drive The drive subsystem to use.
*/
public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
super(
new TrapezoidProfile(
// Limit the max acceleration and velocity
new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(meters, 0)),
// Pipe the profile state to the drive
setpointState -> drive.setDriveStates(setpointState, setpointState),
// Require the drive
drive);
// Reset drive encoders since we're starting at 0
drive.resetEncoders();
}
}

118
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java Normal file
View File

@@ -0,0 +1,118 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor1Port;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor2Port;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor1Port;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor2Port;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kp;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.ksVolts;
import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kvVoltSecondsPerMeter;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final ExampleSmartMotorController m_leftMaster =
new ExampleSmartMotorController(kLeftMotor1Port);
private final ExampleSmartMotorController m_leftSlave =
new ExampleSmartMotorController(kLeftMotor2Port);
private final ExampleSmartMotorController m_rightMaster =
new ExampleSmartMotorController(kRightMotor1Port);
private final ExampleSmartMotorController m_rightSlave =
new ExampleSmartMotorController(kRightMotor2Port);
private final SimpleMotorFeedforward m_feedforward =
new SimpleMotorFeedforward(ksVolts,
kvVoltSecondsPerMeter,
kaVoltSecondsSquaredPerMeter);
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster);
/**
* Creates a new DriveSubsystem.
*/
public DriveSubsystem() {
m_leftSlave.follow(m_leftMaster);
m_rightSlave.follow(m_rightMaster);
m_leftMaster.setPID(kp, 0, 0);
m_rightMaster.setPID(kp, 0, 0);
}
/**
* 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);
}
/**
* Attempts to follow the given drive states using offboard PID.
*
* @param left The left wheel state.
* @param right The right wheel state.
*/
public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
m_leftMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
left.position,
m_feedforward.calculate(left.velocity));
m_rightMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
right.position,
m_feedforward.calculate(right.velocity));
}
/**
* Returns the left encoder distance.
*
* @return the left encoder distance
*/
public double getLeftEncoderDistance() {
return m_leftMaster.getEncoderDistance();
}
/**
* Returns the right encoder distance.
*
* @return the right encoder distance
*/
public double getRightEncoderDistance() {
return m_rightMaster.getEncoderDistance();
}
/**
* Resets the drive encoders.
*/
public void resetEncoders() {
m_leftMaster.resetEncoder();
m_rightMaster.resetEncoder();
}
/**
* 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);
}
}

115
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java Normal file
View File

@@ -0,0 +1,115 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import edu.wpi.first.wpilibj.SpeedController;
/**
* A simplified stub class that simulates the API of a common "smart" motor controller.
*
* <p>Has no actual functionality.
*/
public class ExampleSmartMotorController implements SpeedController {
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 master The master to follow.
*/
public void follow(ExampleSmartMotorController master) {
}
/**
* 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() {
}
@Override
public void set(double speed) {
}
@Override
public double get() {
return 0;
}
@Override
public void setInverted(boolean isInverted) {
}
@Override
public boolean getInverted() {
return false;
}
@Override
public void disable() {
}
@Override
public void stopMotor() {
}
@Override
public void pidWrite(double output) {
}
}

22
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -7,21 +7,18 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
private final SpeedController m_motor = new PWMVictorSPX(1);
private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt);
private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
// Note: These gains are fake, and will have to be tuned for your robot.
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
@@ -30,7 +27,8 @@ public class Robot extends TimedRobot {
@Override
public void robotInit() {
m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.setPID(1.3, 0.0, 0.7);
}
@Override
@@ -50,10 +48,8 @@ public class Robot extends TimedRobot {
// toward the goal while obeying the constraints.
m_setpoint = profile.calculate(kDt);
double output = m_controller.calculate(m_encoder.getDistance(),
m_setpoint.position);
// Run controller with profiled setpoint and update motor output
m_motor.set(output);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity) / 12.0);
}
}

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

@@ -485,7 +485,7 @@
},
{
"name": "ArmBot",
"description": "An example command-based robot demonstrating the use of a ProfiledPIDCommand to control an arm.",
"description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
"tags": [
"ArmBot",
"PID",
@@ -496,6 +496,32 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "ArmBotOffboard",
"description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
"tags": [
"ArmBotOffboard",
"PID",
"Motion Profile"
],
"foldername": "armbotoffboard",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "DriveDistanceOffboard",
"description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
"tags": [
"DriveDistance",
"PID",
"Motion Profile"
],
"foldername": "drivedistanceoffboard",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "MecanumControllerCommand",
"description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",

3
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
View File

@@ -43,6 +43,9 @@ public final class Constants {
public static final double kTurnI = 0;
public static final double kTurnD = 0;
public static final double kMaxTurnRateDegPerS = 100;
public static final double kMaxTurnAccelerationDegPerSSquared = 300;
public static final double kTurnToleranceDeg = 5;
public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
View File

@@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
@@ -85,6 +86,10 @@ public class RobotContainer {
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kX.value)
.whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
// Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kA.value)
.whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
}

62
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnRateDegPerS;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
/**
* A command that will turn the robot to the specified angle using a motion profile.
*/
public class TurnToAngleProfiled extends ProfiledPIDCommand {
/**
* 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
*/
public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
super(
new ProfiledPIDController(kTurnP, kTurnI, kTurnD,
new TrapezoidProfile.Constraints(
kMaxTurnRateDegPerS,
kMaxTurnAccelerationDegPerSSquared
)),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
(output, setpoint) -> drive.arcadeDrive(0, output),
// Require the drive
drive);
// Set the controller to be continuous (because it is an angle controller)
getController().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
getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
return getController().atGoal();
}
}