Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add new C++ Command framework (#1785)

Browse Source 
This is the C++ version of #1682.

The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the frc2 namespace.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main frc namespace.
This commit is contained in:
Oblarg
2019-08-25 23:55:59 -04:00
committed by Peter Johnson
parent a0be07c370
commit 076ed7770c
196 changed files with 10687 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/Command.h>
#include <frc/TimedRobot.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;
};

45
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h Normal file
View File

@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* 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/Command.h>
#include <frc/Joystick.h>
#include "commands/Autonomous.h"
#include "subsystems/Claw.h"
#include "subsystems/DriveTrain.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
/**
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* commands, and button mappings) should be declared here.
*/
class RobotContainer {
public:
RobotContainer();
frc2::Command* GetAutonomousCommand();
private:
// The robot's subsystems and commands are defined here...
frc::Joystick m_joy{0};
Claw m_claw;
Wrist m_wrist;
Elevator m_elevator;
DriveTrain m_drivetrain;
Autonomous m_autonomousCommand;
void ConfigureButtonBindings();
};

26
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h Normal file
View File

@@ -0,0 +1,26 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/SequentialCommandGroup.h>
#include "subsystems/Claw.h"
#include "subsystems/DriveTrain.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
class Autonomous
: public frc2::CommandHelper<frc2::SequentialCommandGroup, Autonomous> {
public:
Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
DriveTrain* drivetrain);
};

28
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h Normal file
View File

@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
#include "subsystems/Claw.h"
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
class CloseClaw : public frc2::CommandHelper<frc2::CommandBase, CloseClaw> {
public:
explicit CloseClaw(Claw* claw);
void Initialize() override;
bool IsFinished() override;
void End(bool interrupted) override;
private:
Claw* m_claw;
};

30
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/PIDCommand.h>
#include "subsystems/DriveTrain.h"
/**
* Drive the given distance straight (negative values go backwards).
* Uses a local PID controller to run a simple PID loop that is only
* enabled while this command is running. The input is the averaged
* values of the left and right encoders.
*/
class DriveStraight
: public frc2::CommandHelper<frc2::PIDCommand, DriveStraight> {
public:
explicit DriveStraight(double distance, DriveTrain* drivetrain);
void Initialize() override;
bool IsFinished() override;
private:
DriveTrain* m_drivetrain;
};

27
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h Normal file
View File

@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/WaitCommand.h>
#include "subsystems/Claw.h"
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
class OpenClaw : public frc2::CommandHelper<frc2::WaitCommand, OpenClaw> {
public:
explicit OpenClaw(Claw* claw);
void Initialize() override;
void End(bool interrupted) override;
private:
Claw* m_claw;
};

25
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h Normal file
View File

@@ -0,0 +1,25 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/SequentialCommandGroup.h>
#include "subsystems/Claw.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
/**
* Pickup a soda can (if one is between the open claws) and
* get it in a safe state to drive around.
*/
class Pickup
: public frc2::CommandHelper<frc2::SequentialCommandGroup, Pickup> {
public:
Pickup(Claw* claw, Wrist* wrist, Elevator* elevator);
};

23
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h Normal file
View File

@@ -0,0 +1,23 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/SequentialCommandGroup.h>
#include "subsystems/Claw.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
/**
* Place a held soda can onto the platform.
*/
class Place : public frc2::CommandHelper<frc2::SequentialCommandGroup, Place> {
public:
Place(Claw* claw, Wrist* wrist, Elevator* elevator);
};

24
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h Normal file
View File

@@ -0,0 +1,24 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/SequentialCommandGroup.h>
#include "subsystems/Claw.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
/**
* Make sure the robot is in a state to pickup soda cans.
*/
class PrepareToPickup : public frc2::CommandHelper<frc2::SequentialCommandGroup,
PrepareToPickup> {
public:
PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator);
};

30
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandHelper.h>
#include <frc2/command/PIDCommand.h>
#include "subsystems/DriveTrain.h"
/**
* Drive until the robot is the given distance away from the box. Uses a local
* PID controller to run a simple PID loop that is only enabled while this
* command is running. The input is the averaged values of the left and right
* encoders.
*/
class SetDistanceToBox
: public frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox> {
public:
explicit SetDistanceToBox(double distance, DriveTrain* drivetrain);
void Initialize() override;
bool IsFinished() override;
private:
DriveTrain* m_drivetrain;
};

32
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h Normal file
View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
#include "subsystems/Elevator.h"
/**
* Move the elevator to a given location. This command finishes when it is
* within
* the tolerance, but leaves the PID loop running to maintain the position.
* Other
* commands using the elevator should make sure they disable PID!
*/
class SetElevatorSetpoint
: public frc2::CommandHelper<frc2::CommandBase, SetElevatorSetpoint> {
public:
explicit SetElevatorSetpoint(double setpoint, Elevator* elevator);
void Initialize() override;
bool IsFinished() override;
private:
double m_setpoint;
Elevator* m_elevator;
};

30
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
#include "subsystems/Wrist.h"
/**
* Move the wrist to a given angle. This command finishes when it is within
* the tolerance, but leaves the PID loop running to maintain the position.
* Other commands using the wrist should make sure they disable PID!
*/
class SetWristSetpoint
: public frc2::CommandHelper<frc2::CommandBase, SetWristSetpoint> {
public:
explicit SetWristSetpoint(double setpoint, Wrist* wrist);
void Initialize() override;
bool IsFinished() override;
private:
double m_setpoint;
Wrist* m_wrist;
};

30
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
#include "subsystems/DriveTrain.h"
/**
* Have the robot drive tank style using the PS3 Joystick until interrupted.
*/
class TankDrive : public frc2::CommandHelper<frc2::CommandBase, TankDrive> {
public:
TankDrive(DriveTrain* drivetrain, std::function<double()> left,
std::function<double()> right);
void Execute() override;
bool IsFinished() override;
void End(bool interrupted) override;
private:
DriveTrain* m_drivetrain;
std::function<double()> m_left;
std::function<double()> m_right;
};

50
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h Normal file
View File

@@ -0,0 +1,50 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/SubsystemBase.h>
#include <frc/DigitalInput.h>
#include <frc/PWMVictorSPX.h>
/**
* The claw subsystem is a simple system with a motor for opening and closing.
* If using stronger motors, you should probably use a sensor so that the
* motors don't stall.
*/
class Claw : public frc2::SubsystemBase {
public:
Claw();
/**
* Set the claw motor to move in the open direction.
*/
void Open();
/**
* Set the claw motor to move in the close direction.
*/
void Close();
/**
* Stops the claw motor from moving.
*/
void Stop();
/**
* Return true when the robot is grabbing an object hard enough
* to trigger the limit switch.
*/
bool IsGripping();
void Log();
private:
frc::PWMVictorSPX m_motor{7};
frc::DigitalInput m_contact{5};
};

79
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/SubsystemBase.h>
#include <frc/AnalogGyro.h>
#include <frc/AnalogInput.h>
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/drive/DifferentialDrive.h>
namespace frc {
class Joystick;
} // namespace frc
/**
* The DriveTrain subsystem incorporates the sensors and actuators attached to
* the robots chassis. These include four drive motors, a left and right encoder
* and a gyro.
*/
class DriveTrain : public frc2::SubsystemBase {
public:
DriveTrain();
/**
* The log method puts interesting information to the SmartDashboard.
*/
void Log();
/**
* Tank style driving for the DriveTrain.
* @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
void Drive(double left, double right);
/**
* @return The robots heading in degrees.
*/
double GetHeading();
/**
* Reset the robots sensors to the zero states.
*/
void Reset();
/**
* @return The distance driven (average of left and right encoders).
*/
double GetDistance();
/**
* @return The distance to the obstacle detected by the rangefinder.
*/
double GetDistanceToObstacle();
private:
frc::PWMVictorSPX m_frontLeft{1};
frc::PWMVictorSPX m_rearLeft{2};
frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
frc::PWMVictorSPX m_frontRight{3};
frc::PWMVictorSPX m_rearRight{4};
frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
frc::DifferentialDrive m_robotDrive{m_left, m_right};
frc::Encoder m_leftEncoder{1, 2};
frc::Encoder m_rightEncoder{3, 4};
frc::AnalogInput m_rangefinder{6};
frc::AnalogGyro m_gyro{1};
};

66
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h Normal file
View File

@@ -0,0 +1,66 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/PIDSubsystem.h>
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
* it's current
* state PID values for simulation are different than in the real world do to
* minor differences.
*/
class Elevator : public frc2::PIDSubsystem {
public:
Elevator();
/**
* The log method puts interesting information to the SmartDashboard.
*/
void Log();
/**
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
double GetMeasurement() override;
/**
* Use the motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
void UseOutput(double d) override;
double GetSetpoint() override;
/**
* Sets the setpoint for the subsystem.
*/
void SetSetpoint(double setpoint);
private:
frc::PWMVictorSPX m_motor{5};
double m_setpoint = 0;
// Conversion value of potentiometer varies between the real world and
// simulation
#ifndef SIMULATION
frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
#else
frc::AnalogPotentiometer m_pot{2}; // Defaults to meters
#endif
static constexpr double kP_real = 4;
static constexpr double kI_real = 0.07;
static constexpr double kP_simulation = 18;
static constexpr double kI_simulation = 0.2;
};

62
wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* 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 <frc2/command/PIDSubsystem.h>
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead
* of a linear joint.
*/
class Wrist : public frc2::PIDSubsystem {
public:
Wrist();
/**
* The log method puts interesting information to the SmartDashboard.
*/
void Log();
/**
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
double GetMeasurement() override;
/**
* Use the motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
void UseOutput(double d) override;
double GetSetpoint() override;
/**
* Sets the setpoint for the subsystem.
*/
void SetSetpoint(double setpoint);
private:
frc::PWMVictorSPX m_motor{6};
double m_setpoint = 0;
// Conversion value of potentiometer varies between the real world and
// simulation
#ifndef SIMULATION
frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
#else
frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
#endif
static constexpr double kP_real = 1;
static constexpr double kP_simulation = 0.05;
};