From 076ed7770c3289bd7df9f545e30f86c3a2ed0dca Mon Sep 17 00:00:00 2001 From: Oblarg Date: Sun, 25 Aug 2019 23:55:59 -0400 Subject: [PATCH] Add new C++ Command framework (#1785) 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. --- .../main/native/cpp/frc2/command/Command.cpp | 100 +++++ .../native/cpp/frc2/command/CommandBase.cpp | 62 +++ .../cpp/frc2/command/CommandGroupBase.cpp | 60 +++ .../cpp/frc2/command/CommandScheduler.cpp | 320 ++++++++++++++++ .../native/cpp/frc2/command/CommandState.cpp | 25 ++ .../cpp/frc2/command/ConditionalCommand.cpp | 52 +++ .../cpp/frc2/command/FunctionalCommand.cpp | 27 ++ .../cpp/frc2/command/InstantCommand.cpp | 22 ++ .../cpp/frc2/command/NotifierCommand.cpp | 32 ++ .../native/cpp/frc2/command/PIDCommand.cpp | 55 +++ .../native/cpp/frc2/command/PIDSubsystem.cpp | 31 ++ .../cpp/frc2/command/ParallelCommandGroup.cpp | 83 ++++ .../frc2/command/ParallelDeadlineGroup.cpp | 86 +++++ .../cpp/frc2/command/ParallelRaceGroup.cpp | 72 ++++ .../cpp/frc2/command/PerpetualCommand.cpp | 25 ++ .../native/cpp/frc2/command/PrintCommand.cpp | 20 +- .../cpp/frc2/command/ProxyScheduleCommand.cpp | 37 ++ .../native/cpp/frc2/command/RunCommand.cpp | 18 + .../cpp/frc2/command/ScheduleCommand.cpp | 24 ++ .../frc2/command/SequentialCommandGroup.cpp | 73 ++++ .../cpp/frc2/command/StartEndCommand.cpp | 27 ++ .../native/cpp/frc2/command/Subsystem.cpp | 27 ++ .../native/cpp/frc2/command/SubsystemBase.cpp | 57 +++ .../native/cpp/frc2/command/WaitCommand.cpp | 27 ++ .../cpp/frc2/command/WaitUntilCommand.cpp | 20 + .../native/cpp/frc2/command/button/Button.cpp | 57 +++ .../cpp/frc2/command/button/Trigger.cpp | 119 ++++++ .../include/frc/smartdashboard/Sendable.h | 4 +- .../native/include/frc2/command/Command.h | 237 ++++++++++++ .../native/include/frc2/command/CommandBase.h | 56 +++ .../include/frc2/command/CommandGroupBase.h | 62 +++ .../include/frc2/command/CommandHelper.h | 37 ++ .../include/frc2/command/CommandScheduler.h | 362 ++++++++++++++++++ .../include/frc2/command/CommandState.h | 33 ++ .../include/frc2/command/ConditionalCommand.h | 92 +++++ .../include/frc2/command/FunctionalCommand.h | 56 +++ .../include/frc2/command/InstantCommand.h | 48 +++ .../include/frc2/command/NotifierCommand.h | 51 +++ .../native/include/frc2/command/PIDCommand.h | 108 ++++++ .../include/frc2/command/PIDSubsystem.h | 73 ++++ .../frc2/command/ParallelCommandGroup.h | 86 +++++ .../frc2/command/ParallelDeadlineGroup.h | 99 +++++ .../include/frc2/command/ParallelRaceGroup.h | 78 ++++ .../include/frc2/command/PerpetualCommand.h | 66 ++++ .../include/frc2/command/PrintCommand.h | 35 ++ .../frc2/command/ProxyScheduleCommand.h | 50 +++ .../native/include/frc2/command/RunCommand.h | 41 ++ .../include/frc2/command/ScheduleCommand.h | 46 +++ .../include/frc2/command/SelectCommand.h | 141 +++++++ .../frc2/command/SequentialCommandGroup.h | 92 +++++ .../include/frc2/command/SetUtilities.h | 29 ++ .../include/frc2/command/StartEndCommand.h | 46 +++ .../native/include/frc2/command/Subsystem.h | 88 +++++ .../include/frc2/command/SubsystemBase.h | 34 ++ .../native/include/frc2/command/WaitCommand.h | 52 +++ .../include/frc2/command/WaitUntilCommand.h | 52 +++ .../include/frc2/command/button/Button.h | 207 ++++++++++ .../frc2/command/button/JoystickButton.h | 37 ++ .../include/frc2/command/button/POVButton.h | 41 ++ .../include/frc2/command/button/Trigger.h | 325 ++++++++++++++++ .../native/cpp/frc2/command/ButtonTest.cpp | 195 ++++++++++ .../cpp/frc2/command/CommandDecoratorTest.cpp | 101 +++++ .../frc2/command/CommandRequirementsTest.cpp | 84 ++++ .../cpp/frc2/command/CommandScheduleTest.cpp | 103 +++++ .../cpp/frc2/command/CommandTestBase.cpp | 37 ++ .../native/cpp/frc2/command/CommandTestBase.h | 102 +++++ .../frc2/command/ConditionalCommandTest.cpp | 56 +++ .../cpp/frc2/command/DefaultCommandTest.cpp | 48 +++ .../cpp/frc2/command/ErrorConfirmer.cpp | 20 + .../native/cpp/frc2/command/ErrorConfirmer.h | 42 ++ .../frc2/command/FunctionalCommandTest.cpp | 31 ++ .../cpp/frc2/command/InstantCommandTest.cpp | 25 ++ .../cpp/frc2/command/NotifierCommandTest.cpp | 30 ++ .../frc2/command/ParallelCommandGroupTest.cpp | 120 ++++++ .../command/ParallelDeadlineGroupTest.cpp | 136 +++++++ .../frc2/command/ParallelRaceGroupTest.cpp | 131 +++++++ .../cpp/frc2/command/PerpetualCommandTest.cpp | 26 ++ .../cpp/frc2/command/PrintCommandTest.cpp | 30 ++ .../frc2/command/ProxyScheduleCommandTest.cpp | 50 +++ .../frc2/command/RobotDisabledCommandTest.cpp | 156 ++++++++ .../cpp/frc2/command/RunCommandTest.cpp | 27 ++ .../cpp/frc2/command/ScheduleCommandTest.cpp | 31 ++ .../native/cpp/frc2/command/SchedulerTest.cpp | 56 +++ .../cpp/frc2/command/SelectCommandTest.cpp | 62 +++ .../command/SequentialCommandGroupTest.cpp | 137 +++++++ .../cpp/frc2/command/StartEndCommandTest.cpp | 28 ++ .../cpp/frc2/command/WaitCommandTest.cpp | 26 ++ .../cpp/frc2/command/WaitUntilCommandTest.cpp | 27 ++ .../native/cpp/frc2/command/make_vector.h | 70 ++++ .../ElevatorProfiledPID/cpp/Robot.cpp | 4 +- .../ElevatorTrapezoidProfile/cpp/Robot.cpp | 4 +- .../cpp/examples/Frisbeebot/cpp/Robot.cpp | 72 ++++ .../Frisbeebot/cpp/RobotContainer.cpp | 53 +++ .../cpp/subsystems/DriveSubsystem.cpp | 47 +++ .../cpp/subsystems/ShooterSubsystem.cpp | 45 +++ .../examples/Frisbeebot/include/Constants.h | 73 ++++ .../cpp/examples/Frisbeebot/include/Robot.h | 34 ++ .../Frisbeebot/include/RobotContainer.h | 102 +++++ .../include/subsystems/DriveSubsystem.h | 96 +++++ .../include/subsystems/ShooterSubsystem.h | 37 ++ .../cpp/examples/GearsBotNew/cpp/Robot.cpp | 72 ++++ .../GearsBotNew/cpp/RobotContainer.cpp | 70 ++++ .../GearsBotNew/cpp/commands/Autonomous.cpp | 35 ++ .../GearsBotNew/cpp/commands/CloseClaw.cpp | 31 ++ .../cpp/commands/DriveStraight.cpp | 31 ++ .../GearsBotNew/cpp/commands/OpenClaw.cpp | 25 ++ .../GearsBotNew/cpp/commands/Pickup.cpp | 21 + .../GearsBotNew/cpp/commands/Place.cpp | 21 + .../cpp/commands/PrepareToPickup.cpp | 21 + .../cpp/commands/SetDistanceToBox.cpp | 31 ++ .../cpp/commands/SetElevatorSetpoint.cpp | 29 ++ .../cpp/commands/SetWristSetpoint.cpp | 27 ++ .../GearsBotNew/cpp/commands/TankDrive.cpp} | 25 +- .../GearsBotNew/cpp/subsystems/Claw.cpp} | 18 +- .../GearsBotNew/cpp/subsystems/DriveTrain.cpp | 69 ++++ .../GearsBotNew/cpp/subsystems/Elevator.cpp | 35 ++ .../GearsBotNew/cpp/subsystems/Wrist.cpp | 35 ++ .../cpp/examples/GearsBotNew/include/Robot.h | 34 ++ .../GearsBotNew/include/RobotContainer.h | 45 +++ .../GearsBotNew/include/commands/Autonomous.h | 26 ++ .../GearsBotNew/include/commands/CloseClaw.h | 28 ++ .../include/commands/DriveStraight.h | 30 ++ .../GearsBotNew/include/commands/OpenClaw.h | 27 ++ .../GearsBotNew/include/commands/Pickup.h | 25 ++ .../GearsBotNew/include/commands/Place.h | 23 ++ .../include/commands/PrepareToPickup.h | 24 ++ .../include/commands/SetDistanceToBox.h | 30 ++ .../include/commands/SetElevatorSetpoint.h | 32 ++ .../include/commands/SetWristSetpoint.h | 30 ++ .../GearsBotNew/include/commands/TankDrive.h | 30 ++ .../GearsBotNew/include/subsystems/Claw.h | 50 +++ .../include/subsystems/DriveTrain.h | 79 ++++ .../GearsBotNew/include/subsystems/Elevator.h | 66 ++++ .../GearsBotNew/include/subsystems/Wrist.h | 62 +++ .../examples/GyroDriveCommands/cpp/Robot.cpp | 72 ++++ .../GyroDriveCommands/cpp/RobotContainer.cpp | 48 +++ .../cpp/commands/TurnToAngle.cpp | 35 ++ .../cpp/subsystems/DriveSubsystem.cpp | 55 +++ .../GyroDriveCommands/include/Constants.h | 58 +++ .../GyroDriveCommands/include/Robot.h | 34 ++ .../include/RobotContainer.h | 80 ++++ .../include/commands/TurnToAngle.h | 29 ++ .../include/subsystems/DriveSubsystem.h | 114 ++++++ .../examples/HatchbotInlined/cpp/Robot.cpp | 72 ++++ .../HatchbotInlined/cpp/RobotContainer.cpp | 53 +++ .../cpp/commands/ComplexAuto.cpp | 37 ++ .../cpp/subsystems/DriveSubsystem.cpp | 47 +++ .../cpp/subsystems/HatchSubsystem.cpp | 21 + .../HatchbotInlined/include/Constants.h | 50 +++ .../examples/HatchbotInlined/include/Robot.h | 34 ++ .../HatchbotInlined/include/RobotContainer.h | 76 ++++ .../include/commands/ComplexAuto.h | 31 ++ .../include/subsystems/DriveSubsystem.h | 96 +++++ .../include/subsystems/HatchSubsystem.h | 36 ++ .../HatchbotTraditional/cpp/Robot.cpp | 72 ++++ .../cpp/RobotContainer.cpp | 61 +++ .../cpp/commands/ComplexAuto.cpp | 20 + .../cpp/commands/DefaultDrive.cpp | 19 + .../cpp/commands/DriveDistance.cpp | 27 ++ .../cpp/commands/GrabHatch.cpp} | 15 +- .../cpp/commands/HalveDriveSpeed.cpp | 15 + .../cpp/commands/ReleaseHatch.cpp | 16 + .../cpp/subsystems/DriveSubsystem.cpp | 47 +++ .../cpp/subsystems/HatchSubsystem.cpp | 21 + .../HatchbotTraditional/include/Constants.h | 50 +++ .../HatchbotTraditional/include/Robot.h | 34 ++ .../include/RobotContainer.h | 54 +++ .../include/commands/ComplexAuto.h | 31 ++ .../include/commands/DefaultDrive.h | 41 ++ .../include/commands/DriveDistance.h | 37 ++ .../include/commands/GrabHatch.h | 32 ++ .../include/commands/HalveDriveSpeed.h | 26 ++ .../include/commands/ReleaseHatch.h | 33 ++ .../include/subsystems/DriveSubsystem.h | 96 +++++ .../include/subsystems/HatchSubsystem.h | 36 ++ .../SchedulerEventLogging/cpp/Robot.cpp | 72 ++++ .../cpp/RobotContainer.cpp | 63 +++ .../SchedulerEventLogging/include/Constants.h | 22 ++ .../SchedulerEventLogging/include/Robot.h | 34 ++ .../include/RobotContainer.h | 43 +++ .../cpp/examples/SelectCommand/cpp/Robot.cpp | 72 ++++ .../SelectCommand/cpp/RobotContainer.cpp | 24 ++ .../SelectCommand/include/Constants.h | 22 ++ .../examples/SelectCommand/include/Robot.h | 34 ++ .../SelectCommand/include/RobotContainer.h | 50 +++ .../src/main/cpp/examples/examples.json | 71 ++++ .../cpp/templates/commandbased/cpp/Robot.cpp | 53 +-- .../commandbased/cpp/RobotContainer.cpp | 24 ++ .../cpp/commands/ExampleCommand.cpp | 26 +- .../cpp/subsystems/ExampleSubsystem.cpp | 16 +- .../commandbased/include/Constants.h | 18 + .../templates/commandbased/include/Robot.h | 23 +- .../commandbased/include/RobotContainer.h | 34 ++ .../templates/commandbased/include/RobotMap.h | 25 -- .../include/commands/ExampleCommand.h | 32 +- .../include/subsystems/ExampleSubsystem.h | 18 +- 196 files changed, 10687 insertions(+), 163 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/frc2/command/Command.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp rename wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h => wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp (56%) create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp create mode 100644 wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp create mode 100644 wpilibc/src/main/native/include/frc2/command/Command.h create mode 100644 wpilibc/src/main/native/include/frc2/command/CommandBase.h create mode 100644 wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h create mode 100644 wpilibc/src/main/native/include/frc2/command/CommandHelper.h create mode 100644 wpilibc/src/main/native/include/frc2/command/CommandScheduler.h create mode 100644 wpilibc/src/main/native/include/frc2/command/CommandState.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/InstantCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/NotifierCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/PIDCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h create mode 100644 wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/PrintCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/RunCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/SelectCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h create mode 100644 wpilibc/src/main/native/include/frc2/command/SetUtilities.h create mode 100644 wpilibc/src/main/native/include/frc2/command/StartEndCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/Subsystem.h create mode 100644 wpilibc/src/main/native/include/frc2/command/SubsystemBase.h create mode 100644 wpilibc/src/main/native/include/frc2/command/WaitCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h create mode 100644 wpilibc/src/main/native/include/frc2/command/button/Button.h create mode 100644 wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h create mode 100644 wpilibc/src/main/native/include/frc2/command/button/POVButton.h create mode 100644 wpilibc/src/main/native/include/frc2/command/button/Trigger.h create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h create mode 100644 wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp create mode 100644 wpilibc/src/test/native/cpp/frc2/command/make_vector.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp rename wpilibcExamples/src/main/cpp/{templates/commandbased/cpp/commands/MyAutoCommand.cpp => examples/GearsBotNew/cpp/commands/TankDrive.cpp} (52%) rename wpilibcExamples/src/main/cpp/{templates/commandbased/cpp/OI.cpp => examples/GearsBotNew/cpp/subsystems/Claw.cpp} (59%) create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h create mode 100644 wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp rename wpilibcExamples/src/main/cpp/{templates/commandbased/include/OI.h => examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp} (58%) create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h create mode 100644 wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp create mode 100644 wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h delete mode 100644 wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp new file mode 100644 index 0000000000..9003338207 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/Command.h" + +#include + +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/PerpetualCommand.h" +#include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; + +Command::~Command() { CommandScheduler::GetInstance().Cancel(this); } + +void Command::Initialize() {} +void Command::Execute() {} +void Command::End(bool interrupted) {} + +ParallelRaceGroup Command::WithTimeout(double seconds) && { + std::vector> temp; + temp.emplace_back(std::make_unique(seconds)); + temp.emplace_back(std::move(*this).TransferOwnership()); + return ParallelRaceGroup(std::move(temp)); +} + +ParallelRaceGroup Command::InterruptOn(std::function condition) && { + std::vector> temp; + temp.emplace_back(std::make_unique(std::move(condition))); + temp.emplace_back(std::move(*this).TransferOwnership()); + return ParallelRaceGroup(std::move(temp)); +} + +SequentialCommandGroup Command::BeforeStarting(std::function toRun) && { + std::vector> temp; + temp.emplace_back(std::make_unique( + std::move(toRun), std::initializer_list{})); + temp.emplace_back(std::move(*this).TransferOwnership()); + return SequentialCommandGroup(std::move(temp)); +} + +SequentialCommandGroup Command::WhenFinished(std::function toRun) && { + std::vector> temp; + temp.emplace_back(std::move(*this).TransferOwnership()); + temp.emplace_back(std::make_unique( + std::move(toRun), std::initializer_list{})); + return SequentialCommandGroup(std::move(temp)); +} + +PerpetualCommand Command::Perpetually() && { + return PerpetualCommand(std::move(*this).TransferOwnership()); +} + +ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); } + +void Command::Schedule(bool interruptible) { + CommandScheduler::GetInstance().Schedule(interruptible, this); +} + +void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); } + +bool Command::IsScheduled() const { + return CommandScheduler::GetInstance().IsScheduled(this); +} + +bool Command::HasRequirement(Subsystem* requirement) const { + bool hasRequirement = false; + for (auto&& subsystem : GetRequirements()) { + hasRequirement |= requirement == subsystem; + } + return hasRequirement; +} + +std::string Command::GetName() const { return GetTypeName(*this); } + +bool Command::IsGrouped() const { return m_isGrouped; } + +void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; } + +namespace frc2 { +bool RequirementsDisjoint(Command* first, Command* second) { + bool disjoint = true; + auto&& requirements = second->GetRequirements(); + for (auto&& requirement : first->GetRequirements()) { + disjoint &= requirements.find(requirement) == requirements.end(); + } + return disjoint; +} +} // namespace frc2 diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp new file mode 100644 index 0000000000..c3cf024ca9 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/CommandBase.h" + +#include +#include +#include + +using namespace frc2; + +CommandBase::CommandBase() { + m_name = Command::GetName(); + m_subsystem = "Unknown"; +} + +CommandBase::CommandBase(const CommandBase& other) : Sendable{}, Command{} { + m_name = other.m_name; + m_subsystem = other.m_subsystem; + m_requirements = other.m_requirements; +} + +void CommandBase::AddRequirements( + std::initializer_list requirements) { + m_requirements.insert(requirements.begin(), requirements.end()); +} + +void CommandBase::AddRequirements(wpi::SmallSet requirements) { + m_requirements.insert(requirements.begin(), requirements.end()); +} + +wpi::SmallSet CommandBase::GetRequirements() const { + return m_requirements; +} + +void CommandBase::SetName(const wpi::Twine& name) { m_name = name.str(); } + +std::string CommandBase::GetName() const { return m_name; } + +std::string CommandBase::GetSubsystem() const { return m_subsystem; } + +void CommandBase::SetSubsystem(const wpi::Twine& subsystem) { + m_subsystem = subsystem.str(); +} + +void CommandBase::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Command"); + builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr); + builder.AddBooleanProperty("running", [this] { return IsScheduled(); }, + [this](bool value) { + bool isScheduled = IsScheduled(); + if (value && !isScheduled) { + Schedule(); + } else if (!value && isScheduled) { + Cancel(); + } + }); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp new file mode 100644 index 0000000000..ba53397039 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/CommandGroupBase.h" + +#include + +#include "frc/WPIErrors.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +template +static bool ContainsKey(const TMap& map, TKey keyToCheck) { + return map.find(keyToCheck) != map.end(); +} +bool CommandGroupBase::RequireUngrouped(Command& command) { + if (command.IsGrouped()) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + return false; + } else { + return true; + } +} + +bool CommandGroupBase::RequireUngrouped( + wpi::ArrayRef> commands) { + bool allUngrouped = true; + for (auto&& command : commands) { + allUngrouped &= !command.get()->IsGrouped(); + } + if (!allUngrouped) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + } + return allUngrouped; +} + +bool CommandGroupBase::RequireUngrouped( + std::initializer_list commands) { + bool allUngrouped = true; + for (auto&& command : commands) { + allUngrouped &= !command->IsGrouped(); + } + if (!allUngrouped) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + } + return allUngrouped; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp new file mode 100644 index 0000000000..f1423d18fa --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -0,0 +1,320 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/CommandScheduler.h" + +#include +#include +#include +#include +#include +#include + +#include + +using namespace frc2; +template +static bool ContainsKey(const TMap& map, TKey keyToCheck) { + return map.find(keyToCheck) != map.end(); +} + +CommandScheduler::CommandScheduler() { SetName("Scheduler"); } + +CommandScheduler& CommandScheduler::GetInstance() { + static CommandScheduler scheduler; + return scheduler; +} + +void CommandScheduler::AddButton(wpi::unique_function button) { + m_buttons.emplace_back(std::move(button)); +} + +void CommandScheduler::ClearButtons() { m_buttons.clear(); } + +void CommandScheduler::Schedule(bool interruptible, Command* command) { + if (command->IsGrouped()) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "A command that is part of a command group " + "cannot be independently scheduled"); + return; + } + if (m_disabled || + (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) || + ContainsKey(m_scheduledCommands, command)) { + return; + } + + const auto& requirements = command->GetRequirements(); + + wpi::SmallVector intersection; + + bool isDisjoint = true; + bool allInterruptible = true; + for (auto&& i1 : m_requirements) { + if (requirements.find(i1.first) != requirements.end()) { + isDisjoint = false; + allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible(); + intersection.emplace_back(i1.second); + } + } + + if (isDisjoint || allInterruptible) { + if (allInterruptible) { + for (auto&& cmdToCancel : intersection) { + Cancel(cmdToCancel); + } + } + command->Initialize(); + m_scheduledCommands[command] = CommandState{interruptible}; + for (auto&& action : m_initActions) { + action(*command); + } + for (auto&& requirement : requirements) { + m_requirements[requirement] = command; + } + } +} + +void CommandScheduler::Schedule(Command* command) { Schedule(true, command); } + +void CommandScheduler::Schedule(bool interruptible, + wpi::ArrayRef commands) { + for (auto command : commands) { + Schedule(interruptible, command); + } +} + +void CommandScheduler::Schedule(bool interruptible, + std::initializer_list commands) { + for (auto command : commands) { + Schedule(interruptible, command); + } +} + +void CommandScheduler::Schedule(wpi::ArrayRef commands) { + for (auto command : commands) { + Schedule(true, command); + } +} + +void CommandScheduler::Schedule(std::initializer_list commands) { + for (auto command : commands) { + Schedule(true, command); + } +} + +void CommandScheduler::Run() { + if (m_disabled) { + return; + } + + // Run the periodic method of all registered subsystems. + for (auto&& subsystem : m_subsystems) { + subsystem.getFirst()->Periodic(); + } + + // Poll buttons for new commands to add. + for (auto&& button : m_buttons) { + button(); + } + + // Run scheduled commands, remove finished commands. + for (auto iterator = m_scheduledCommands.begin(); + iterator != m_scheduledCommands.end(); iterator++) { + Command* command = iterator->getFirst(); + + if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) { + Cancel(command); + continue; + } + + command->Execute(); + for (auto&& action : m_executeActions) { + action(*command); + } + + if (command->IsFinished()) { + command->End(false); + for (auto&& action : m_finishActions) { + action(*command); + } + + for (auto&& requirement : command->GetRequirements()) { + m_requirements.erase(requirement); + } + + m_scheduledCommands.erase(iterator); + } + } + + // Add default commands for un-required registered subsystems. + for (auto&& subsystem : m_subsystems) { + auto s = m_requirements.find(subsystem.getFirst()); + if (s == m_requirements.end()) { + Schedule({subsystem.getSecond().get()}); + } + } +} + +void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) { + m_subsystems[subsystem] = nullptr; +} + +void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) { + auto s = m_subsystems.find(subsystem); + if (s != m_subsystems.end()) { + m_subsystems.erase(s); + } +} + +void CommandScheduler::RegisterSubsystem( + std::initializer_list subsystems) { + for (auto* subsystem : subsystems) { + RegisterSubsystem(subsystem); + } +} + +void CommandScheduler::UnregisterSubsystem( + std::initializer_list subsystems) { + for (auto* subsystem : subsystems) { + UnregisterSubsystem(subsystem); + } +} + +Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { + auto&& find = m_subsystems.find(subsystem); + if (find != m_subsystems.end()) { + return find->second.get(); + } else { + return nullptr; + } +} + +void CommandScheduler::Cancel(Command* command) { + auto find = m_scheduledCommands.find(command); + if (find == m_scheduledCommands.end()) return; + command->End(true); + for (auto&& action : m_interruptActions) { + action(*command); + } + m_scheduledCommands.erase(find); + for (auto&& requirement : m_requirements) { + if (requirement.second == command) { + m_requirements.erase(requirement.first); + } + } +} + +void CommandScheduler::Cancel(wpi::ArrayRef commands) { + for (auto command : commands) { + Cancel(command); + } +} + +void CommandScheduler::Cancel(std::initializer_list commands) { + for (auto command : commands) { + Cancel(command); + } +} + +void CommandScheduler::CancelAll() { + for (auto&& command : m_scheduledCommands) { + Cancel(command.first); + } +} + +double CommandScheduler::TimeSinceScheduled(const Command* command) const { + auto find = m_scheduledCommands.find(command); + if (find != m_scheduledCommands.end()) { + return find->second.TimeSinceInitialized(); + } else { + return -1; + } +} +bool CommandScheduler::IsScheduled( + wpi::ArrayRef commands) const { + for (auto command : commands) { + if (!IsScheduled(command)) { + return false; + } + } + return true; +} + +bool CommandScheduler::IsScheduled( + std::initializer_list commands) const { + for (auto command : commands) { + if (!IsScheduled(command)) { + return false; + } + } + return true; +} + +bool CommandScheduler::IsScheduled(const Command* command) const { + return m_scheduledCommands.find(command) != m_scheduledCommands.end(); +} + +Command* CommandScheduler::Requiring(const Subsystem* subsystem) const { + auto find = m_requirements.find(subsystem); + if (find != m_requirements.end()) { + return find->second; + } else { + return nullptr; + } +} + +void CommandScheduler::Disable() { m_disabled = true; } + +void CommandScheduler::Enable() { m_disabled = false; } + +void CommandScheduler::OnCommandInitialize(Action action) { + m_initActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandExecute(Action action) { + m_executeActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandInterrupt(Action action) { + m_interruptActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandFinish(Action action) { + m_finishActions.emplace_back(std::move(action)); +} + +void CommandScheduler::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Scheduler"); + m_namesEntry = builder.GetEntry("Names"); + m_idsEntry = builder.GetEntry("Ids"); + m_cancelEntry = builder.GetEntry("Cancel"); + + builder.SetUpdateTable([this] { + double tmp[1]; + tmp[0] = 0; + auto toCancel = m_cancelEntry.GetDoubleArray(tmp); + for (auto cancel : toCancel) { + uintptr_t ptrTmp = static_cast(cancel); + Command* command = reinterpret_cast(ptrTmp); + if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) { + Cancel(command); + } + m_cancelEntry.SetDoubleArray(wpi::ArrayRef{}); + } + + wpi::SmallVector names; + wpi::SmallVector ids; + for (auto&& command : m_scheduledCommands) { + names.emplace_back(command.first->GetName()); + uintptr_t ptrTmp = reinterpret_cast(command.first); + ids.emplace_back(static_cast(ptrTmp)); + } + m_namesEntry.SetStringArray(names); + m_idsEntry.SetDoubleArray(ids); + }); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp new file mode 100644 index 0000000000..78ae006e64 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/CommandState.h" + +#include "frc/Timer.h" + +using namespace frc2; +CommandState::CommandState(bool interruptible) + : m_interruptible{interruptible} { + StartTiming(); + StartRunning(); +} + +void CommandState::StartTiming() { + m_startTime = frc::Timer::GetFPGATimestamp(); +} +void CommandState::StartRunning() { m_startTime = -1; } +double CommandState::TimeSinceInitialized() const { + return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp new file mode 100644 index 0000000000..23445135e6 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/ConditionalCommand.h" + +using namespace frc2; + +ConditionalCommand::ConditionalCommand(std::unique_ptr&& onTrue, + std::unique_ptr&& onFalse, + std::function condition) + : m_condition{std::move(condition)} { + if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) { + return; + } + + m_onTrue = std::move(onTrue); + m_onFalse = std::move(onFalse); + + m_onTrue->SetGrouped(true); + m_onFalse->SetGrouped(true); + + m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled(); + m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled(); + + AddRequirements(m_onTrue->GetRequirements()); + AddRequirements(m_onFalse->GetRequirements()); +} + +void ConditionalCommand::Initialize() { + if (m_condition()) { + m_selectedCommand = m_onTrue.get(); + } else { + m_selectedCommand = m_onFalse.get(); + } + m_selectedCommand->Initialize(); +} + +void ConditionalCommand::Execute() { m_selectedCommand->Execute(); } + +void ConditionalCommand::End(bool interrupted) { + m_selectedCommand->End(interrupted); +} + +bool ConditionalCommand::IsFinished() { + return m_selectedCommand->IsFinished(); +} + +bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp new file mode 100644 index 0000000000..63c3179633 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/FunctionalCommand.h" + +using namespace frc2; + +FunctionalCommand::FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished) + : m_onInit{std::move(onInit)}, + m_onExecute{std::move(onExecute)}, + m_onEnd{std::move(onEnd)}, + m_isFinished{std::move(isFinished)} {} + +void FunctionalCommand::Initialize() { m_onInit(); } + +void FunctionalCommand::Execute() { m_onExecute(); } + +void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); } + +bool FunctionalCommand::IsFinished() { return m_isFinished(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp new file mode 100644 index 0000000000..b199074780 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/InstantCommand.h" + +using namespace frc2; + +InstantCommand::InstantCommand(std::function toRun, + std::initializer_list requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + +InstantCommand::InstantCommand() : m_toRun{[] {}} {} + +void InstantCommand::Initialize() { m_toRun(); } + +bool InstantCommand::IsFinished() { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp new file mode 100644 index 0000000000..086a2a1d0e --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/NotifierCommand.h" + +using namespace frc2; + +NotifierCommand::NotifierCommand(std::function toRun, double period, + std::initializer_list requirements) + : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { + AddRequirements(requirements); +} + +NotifierCommand::NotifierCommand(NotifierCommand&& other) + : CommandHelper(std::move(other)), + m_toRun(other.m_toRun), + m_notifier(other.m_toRun), + m_period(other.m_period) {} + +NotifierCommand::NotifierCommand(const NotifierCommand& other) + : CommandHelper(other), + m_toRun(other.m_toRun), + m_notifier(frc::Notifier(other.m_toRun)), + m_period(other.m_period) {} + +void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); } + +void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp new file mode 100644 index 0000000000..03be847d6f --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/PIDCommand.h" + +using namespace frc2; + +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + std::initializer_list requirements) + : m_controller{controller}, + m_measurement{std::move(measurementSource)}, + m_setpoint{std::move(setpointSource)}, + m_useOutput{std::move(useOutput)} { + AddRequirements(requirements); +} + +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + double setpoint, std::function useOutput, + std::initializer_list requirements) + : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; }, + useOutput, requirements) {} + +void PIDCommand::Initialize() { m_controller.Reset(); } + +void PIDCommand::Execute() { + m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint())); +} + +void PIDCommand::End(bool interrupted) { m_useOutput(0); } + +void PIDCommand::SetOutput(std::function useOutput) { + m_useOutput = useOutput; +} + +void PIDCommand::SetSetpoint(std::function setpointSource) { + m_setpoint = setpointSource; +} + +void PIDCommand::SetSetpoint(double setpoint) { + m_setpoint = [setpoint] { return setpoint; }; +} + +void PIDCommand::SetSetpointRelative(double relativeSetpoint) { + SetSetpoint(m_setpoint() + relativeSetpoint); +} + +PIDController& PIDCommand::getController() { return m_controller; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp new file mode 100644 index 0000000000..b81f6f6d96 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/PIDSubsystem.h" + +using namespace frc2; + +PIDSubsystem::PIDSubsystem(PIDController controller) + : m_controller{controller} {} + +void PIDSubsystem::Periodic() { + if (m_enabled) { + UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint())); + } +} + +void PIDSubsystem::Enable() { + m_controller.Reset(); + m_enabled = true; +} + +void PIDSubsystem::Disable() { + UseOutput(0); + m_enabled = false; +} + +PIDController& PIDSubsystem::GetController() { return m_controller; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp new file mode 100644 index 0000000000..d8a4159d23 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/ParallelCommandGroup.h" + +using namespace frc2; + +ParallelCommandGroup::ParallelCommandGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void ParallelCommandGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning.first->Initialize(); + commandRunning.second = true; + } + isRunning = true; +} + +void ParallelCommandGroup::Execute() { + for (auto& commandRunning : m_commands) { + if (!commandRunning.second) continue; + commandRunning.first->Execute(); + if (commandRunning.first->IsFinished()) { + commandRunning.first->End(false); + commandRunning.second = false; + } + } +} + +void ParallelCommandGroup::End(bool interrupted) { + if (interrupted) { + for (auto& commandRunning : m_commands) { + if (commandRunning.second) { + commandRunning.first->End(true); + } + } + } + isRunning = false; +} + +bool ParallelCommandGroup::IsFinished() { + for (auto& command : m_commands) { + if (command.second) return false; + } + return true; +} + +bool ParallelCommandGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void ParallelCommandGroup::AddCommands( + std::vector>&& commands) { + for (auto&& command : commands) { + if (!RequireUngrouped(*command)) return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands[std::move(command)] = false; + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp new file mode 100644 index 0000000000..d967e67b48 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ParallelDeadlineGroup.h" + +using namespace frc2; + +ParallelDeadlineGroup::ParallelDeadlineGroup( + std::unique_ptr&& deadline, + std::vector>&& commands) { + SetDeadline(std::move(deadline)); + AddCommands(std::move(commands)); +} + +void ParallelDeadlineGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning.first->Initialize(); + commandRunning.second = true; + } + isRunning = true; +} + +void ParallelDeadlineGroup::Execute() { + for (auto& commandRunning : m_commands) { + if (!commandRunning.second) continue; + commandRunning.first->Execute(); + if (commandRunning.first->IsFinished()) { + commandRunning.first->End(false); + commandRunning.second = false; + } + } +} + +void ParallelDeadlineGroup::End(bool interrupted) { + for (auto& commandRunning : m_commands) { + if (commandRunning.second) { + commandRunning.first->End(true); + } + } + isRunning = false; +} + +bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); } + +bool ParallelDeadlineGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void ParallelDeadlineGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands[std::move(command)] = false; + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} + +void ParallelDeadlineGroup::SetDeadline(std::unique_ptr&& deadline) { + m_deadline = deadline.get(); + m_deadline->SetGrouped(true); + m_commands[std::move(deadline)] = false; + AddRequirements(m_deadline->GetRequirements()); + m_runWhenDisabled &= m_deadline->RunsWhenDisabled(); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp new file mode 100644 index 0000000000..4f9881b7c5 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/ParallelRaceGroup.h" + +using namespace frc2; + +ParallelRaceGroup::ParallelRaceGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void ParallelRaceGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning->Initialize(); + } + isRunning = true; +} + +void ParallelRaceGroup::Execute() { + for (auto& commandRunning : m_commands) { + commandRunning->Execute(); + if (commandRunning->IsFinished()) { + m_finished = true; + commandRunning->End(false); + } + } +} + +void ParallelRaceGroup::End(bool interrupted) { + for (auto& commandRunning : m_commands) { + if (!commandRunning->IsFinished()) { + commandRunning->End(true); + } + } + isRunning = false; +} + +bool ParallelRaceGroup::IsFinished() { return m_finished; } + +bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; } + +void ParallelRaceGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands.emplace(std::move(command)); + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp new file mode 100644 index 0000000000..f29850bbaf --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/PerpetualCommand.h" + +using namespace frc2; + +PerpetualCommand::PerpetualCommand(std::unique_ptr&& command) { + if (!CommandGroupBase::RequireUngrouped(command)) { + return; + } + m_command = std::move(command); + m_command->SetGrouped(true); + AddRequirements(m_command->GetRequirements()); +} + +void PerpetualCommand::Initialize() { m_command->Initialize(); } + +void PerpetualCommand::Execute() { m_command->Execute(); } + +void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); } diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp similarity index 56% rename from wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h rename to wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp index ce25e76f30..8bd62ea0ff 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h +++ b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp @@ -1,20 +1,16 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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/PrintCommand.h" -#include +using namespace frc2; -class MyAutoCommand : public frc::Command { - public: - MyAutoCommand(); - void Initialize() override; - void Execute() override; - bool IsFinished() override; - void End() override; - void Interrupted() override; -}; +PrintCommand::PrintCommand(const wpi::Twine& message) + : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} { +} + +bool PrintCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp new file mode 100644 index 0000000000..6f96315365 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/ProxyScheduleCommand.h" + +using namespace frc2; + +ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef toSchedule) { + SetInsert(m_toSchedule, toSchedule); +} + +void ProxyScheduleCommand::Initialize() { + for (auto* command : m_toSchedule) { + command->Schedule(); + } +} + +void ProxyScheduleCommand::End(bool interrupted) { + if (interrupted) { + for (auto* command : m_toSchedule) { + command->Cancel(); + } + } +} + +void ProxyScheduleCommand::Execute() { + m_finished = true; + for (auto* command : m_toSchedule) { + m_finished &= !command->IsScheduled(); + } +} + +bool ProxyScheduleCommand::IsFinished() { return m_finished; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp new file mode 100644 index 0000000000..5c2c75534d --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/RunCommand.h" + +using namespace frc2; + +RunCommand::RunCommand(std::function toRun, + std::initializer_list requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + +void RunCommand::Execute() { m_toRun(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp new file mode 100644 index 0000000000..ea1ea8d6d2 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/ScheduleCommand.h" + +using namespace frc2; + +ScheduleCommand::ScheduleCommand(wpi::ArrayRef toSchedule) { + SetInsert(m_toSchedule, toSchedule); +} + +void ScheduleCommand::Initialize() { + for (auto command : m_toSchedule) { + command->Schedule(); + } +} + +bool ScheduleCommand::IsFinished() { return true; } + +bool ScheduleCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp new file mode 100644 index 0000000000..9d51609a4a --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; + +SequentialCommandGroup::SequentialCommandGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void SequentialCommandGroup::Initialize() { + m_currentCommandIndex = 0; + + if (!m_commands.empty()) { + m_commands[0]->Initialize(); + } +} + +void SequentialCommandGroup::Execute() { + if (m_commands.empty()) return; + + auto& currentCommand = m_commands[m_currentCommandIndex]; + + currentCommand->Execute(); + if (currentCommand->IsFinished()) { + currentCommand->End(false); + m_currentCommandIndex++; + if (m_currentCommandIndex < m_commands.size()) { + m_commands[m_currentCommandIndex]->Initialize(); + } + } +} + +void SequentialCommandGroup::End(bool interrupted) { + if (interrupted && !m_commands.empty()) { + m_commands[m_currentCommandIndex]->End(interrupted); + } + m_currentCommandIndex = invalid_index; +} + +bool SequentialCommandGroup::IsFinished() { + return m_currentCommandIndex == m_commands.size(); +} + +bool SequentialCommandGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void SequentialCommandGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (m_currentCommandIndex != invalid_index) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands.emplace_back(std::move(command)); + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp new file mode 100644 index 0000000000..33407bf718 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/StartEndCommand.h" + +using namespace frc2; + +StartEndCommand::StartEndCommand(std::function onInit, + std::function onEnd, + std::initializer_list requirements) + : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} { + AddRequirements(requirements); +} + +StartEndCommand::StartEndCommand(const StartEndCommand& other) + : CommandHelper(other) { + m_onInit = other.m_onInit; + m_onEnd = other.m_onEnd; +} + +void StartEndCommand::Initialize() { m_onInit(); } + +void StartEndCommand::End(bool interrupted) { m_onEnd(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp new file mode 100644 index 0000000000..cccb55bc17 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/Subsystem.h" + +using namespace frc2; +Subsystem::~Subsystem() { + CommandScheduler::GetInstance().UnregisterSubsystem(this); +} + +void Subsystem::Periodic() {} + +Command* Subsystem::GetDefaultCommand() const { + return CommandScheduler::GetInstance().GetDefaultCommand(this); +} + +Command* Subsystem::GetCurrentCommand() const { + return CommandScheduler::GetInstance().Requiring(this); +} + +void Subsystem::Register() { + return CommandScheduler::GetInstance().RegisterSubsystem(this); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp new file mode 100644 index 0000000000..62080435dd --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/SubsystemBase.h" + +#include +#include +#include +#include + +using namespace frc2; + +SubsystemBase::SubsystemBase() { + m_name = GetTypeName(*this); + CommandScheduler::GetInstance().RegisterSubsystem({this}); +} + +void SubsystemBase::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Subsystem"); + builder.AddBooleanProperty(".hasDefault", + [this] { return GetDefaultCommand() != nullptr; }, + nullptr); + builder.AddStringProperty(".default", + [this]() -> std::string { + auto command = GetDefaultCommand(); + if (command == nullptr) return "none"; + return command->GetName(); + }, + nullptr); + builder.AddBooleanProperty(".hasCommand", + [this] { return GetCurrentCommand() != nullptr; }, + nullptr); + builder.AddStringProperty(".command", + [this]() -> std::string { + auto command = GetCurrentCommand(); + if (command == nullptr) return "none"; + return command->GetName(); + }, + nullptr); +} + +std::string SubsystemBase::GetName() const { return m_name; } + +void SubsystemBase::SetName(const wpi::Twine& name) { m_name = name.str(); } + +std::string SubsystemBase::GetSubsystem() const { return GetName(); } + +void SubsystemBase::SetSubsystem(const wpi::Twine& name) { SetName(name); } + +void SubsystemBase::AddChild(std::string name, frc::Sendable* child) { + child->SetName(name); + frc::LiveWindow::GetInstance()->Add(child); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp new file mode 100644 index 0000000000..fa4472018b --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/WaitCommand.h" + +using namespace frc2; + +WaitCommand::WaitCommand(double seconds) : m_duration{seconds} { + auto secondsStr = std::to_string(seconds); + SetName(wpi::Twine(m_name) + ": " + wpi::Twine(secondsStr) + " seconds"); + m_timer = std::make_unique(); +} + +void WaitCommand::Initialize() { + m_timer->Reset(); + m_timer->Start(); +} + +void WaitCommand::End(bool interrupted) { m_timer->Stop(); } + +bool WaitCommand::IsFinished() { return m_timer->HasPeriodPassed(m_duration); } + +bool WaitCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp new file mode 100644 index 0000000000..d7a0daf374 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; + +WaitUntilCommand::WaitUntilCommand(std::function condition) + : m_condition{std::move(condition)} {} + +WaitUntilCommand::WaitUntilCommand(double time) + : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {} + +bool WaitUntilCommand::IsFinished() { return m_condition(); } + +bool WaitUntilCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp new file mode 100644 index 0000000000..e519a9fdaf --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/button/Button.h" + +using namespace frc2; + +Button::Button(std::function isPressed) : Trigger(isPressed) {} + +Button Button::WhenPressed(Command* command, bool interruptible) { + WhenActive(command, interruptible); + return *this; +} + +Button Button::WhenPressed(std::function toRun) { + WhenActive(std::move(toRun)); + return *this; +} + +Button Button::WhileHeld(Command* command, bool interruptible) { + WhileActiveContinous(command, interruptible); + return *this; +} + +Button Button::WhileHeld(std::function toRun) { + WhileActiveContinous(std::move(toRun)); + return *this; +} + +Button Button::WhenHeld(Command* command, bool interruptible) { + WhileActiveOnce(command, interruptible); + return *this; +} + +Button Button::WhenReleased(Command* command, bool interruptible) { + WhenInactive(command, interruptible); + return *this; +} + +Button Button::WhenReleased(std::function toRun) { + WhenInactive(std::move(toRun)); + return *this; +} + +Button Button::ToggleWhenPressed(Command* command, bool interruptible) { + ToggleWhenActive(command, interruptible); + return *this; +} + +Button Button::CancelWhenPressed(Command* command) { + CancelWhenActive(command); + return *this; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp new file mode 100644 index 0000000000..304bf98a49 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc2/command/button/Trigger.h" + +#include + +using namespace frc2; + +Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {} + +Trigger Trigger::WhenActive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + + return *this; +} + +Trigger Trigger::WhenActive(std::function toRun) { + return WhenActive(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhileActiveContinous(std::function toRun) { + return WhileActiveContinous(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhenInactive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (pressedLast && !pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhenInactive(std::function toRun) { + return WhenInactive(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + if (command->IsScheduled()) { + command->Cancel(); + } else { + command->Schedule(interruptible); + } + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::CancelWhenActive(Command* command) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h index 383c169ac6..761b0bbcfe 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2011-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. */ @@ -20,6 +20,8 @@ class Sendable { Sendable() = default; virtual ~Sendable() = default; + Sendable(const Sendable&) = default; + Sendable& operator=(const Sendable&) = default; Sendable(Sendable&&) = default; Sendable& operator=(Sendable&&) = default; diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibc/src/main/native/include/frc2/command/Command.h new file mode 100644 index 0000000000..c9ecb45a01 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/Command.h @@ -0,0 +1,237 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace frc2 { + +template +std::string GetTypeName(const T& type) { + return wpi::Demangle(typeid(type).name()); +} + +class ParallelCommandGroup; +class ParallelRaceGroup; +class ParallelDeadlineGroup; +class SequentialCommandGroup; +class PerpetualCommand; +class ProxyScheduleCommand; + +/** + * A state machine representing a complete action to be performed by the robot. + * Commands are run by the CommandScheduler, and can be composed into + * CommandGroups to allow users to build complicated multi-step actions without + * the need to roll the state machine logic themselves. + * + *

Commands are run synchronously from the main robot loop; no multithreading + * is used, unless specified explicitly from the command implementation. + * + *

Note: ALWAYS create a subclass by extending CommandHelper, + * or decorators will not function! + * + * @see CommandScheduler + * @see CommandHelper + */ +class Command : public frc::ErrorBase { + public: + Command() = default; + Command(Command&& other) = default; + virtual ~Command(); + + /** + * The initial subroutine of a command. Called once when the command is + * initially scheduled. + */ + virtual void Initialize(); + + /** + * The main body of a command. Called repeatedly while the command is + * scheduled. + */ + virtual void Execute(); + + /** + * The action to take when the command ends. Called when either the command + * finishes normally, or when it interrupted/canceled. + * + * @param interrupted whether the command was interrupted/canceled + */ + virtual void End(bool interrupted); + + /** + * Whether the command has finished. Once a command finishes, the scheduler + * will call its end() method and un-schedule it. + * + * @return whether the command has finished. + */ + virtual bool IsFinished() { return false; } + + /** + * Specifies the set of subsystems used by this command. Two commands cannot + * use the same subsystem at the same time. If the command is scheduled as + * interruptible and another command is scheduled that shares a requirement, + * the command will be interrupted. Else, the command will not be scheduled. + * If no subsystems are required, return an empty set. + * + *

Note: it is recommended that user implementations contain the + * requirements as a field, and return that field here, rather than allocating + * a new set every time this is called. + * + * @return the set of subsystems that are required + */ + virtual wpi::SmallSet GetRequirements() const = 0; + + /** + * Decorates this command with a timeout. If the specified timeout is + * exceeded before the command finishes normally, the command will be + * interrupted and un-scheduled. Note that the timeout only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param seconds the timeout duration + * @return the command with the timeout added + */ + ParallelRaceGroup WithTimeout(double seconds) &&; + + /** + * Decorates this command with an interrupt condition. If the specified + * condition becomes true before the command finishes normally, the command + * will be interrupted and un-scheduled. Note that this only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param condition the interrupt condition + * @return the command with the interrupt condition added + */ + ParallelRaceGroup InterruptOn(std::function condition) &&; + + /** + * Decorates this command with a runnable to run before this command starts. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + SequentialCommandGroup BeforeStarting(std::function toRun) &&; + + /** + * Decorates this command with a runnable to run after the command finishes. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + SequentialCommandGroup WhenFinished(std::function toRun) &&; + + /** + * Decorates this command to run perpetually, ignoring its ordinary end + * conditions. The decorated command can still be interrupted or canceled. + * + * @return the decorated command + */ + PerpetualCommand Perpetually() &&; + + /** + * Decorates this command to run "by proxy" by wrapping it in a {@link + * ProxyScheduleCommand}. This is useful for "forking off" from command groups + * when the user does not wish to extend the command's requirements to the + * entire command group. + * + * @return the decorated command + */ + ProxyScheduleCommand AsProxy(); + + /** + * Schedules this command. + * + * @param interruptible whether this command can be interrupted by another + * command that shares one of its requirements + */ + void Schedule(bool interruptible); + + /** + * Schedules this command, defaulting to interruptible. + */ + void Schedule() { Schedule(true); } + + /** + * Cancels this command. Will call the command's interrupted() method. + * Commands will be canceled even if they are not marked as interruptible. + */ + void Cancel(); + + /** + * Whether or not the command is currently scheduled. Note that this does not + * detect whether the command is being run by a CommandGroup, only whether it + * is directly being run by the scheduler. + * + * @return Whether the command is scheduled. + */ + bool IsScheduled() const; + + /** + * Whether the command requires a given subsystem. Named "hasRequirement" + * rather than "requires" to avoid confusion with + * {@link + * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)} + * - this may be able to be changed in a few years. + * + * @param requirement the subsystem to inquire about + * @return whether the subsystem is required + */ + bool HasRequirement(Subsystem* requirement) const; + + /** + * Whether the command is currently grouped in a command group. Used as extra + * insurance to prevent accidental independent use of grouped commands. + */ + bool IsGrouped() const; + + /** + * Sets whether the command is currently grouped in a command group. Can be + * used to "reclaim" a command if a group is no longer going to use it. NOT + * ADVISED! + */ + void SetGrouped(bool grouped); + + /** + * Whether the given command should run when the robot is disabled. Override + * to return true if the command should run when disabled. + * + * @return whether the command should run when the robot is disabled + */ + virtual bool RunsWhenDisabled() const { return false; } + + virtual std::string GetName() const; + + protected: + /** + * Transfers ownership of this command to a unique pointer. Used for + * decorator methods. + */ + virtual std::unique_ptr TransferOwnership() && = 0; + + bool m_isGrouped = false; +}; + +/** + * Checks if two commands have disjoint requirement sets. + * + * @param first The first command to check. + * @param second The second command to check. + * @return False if first and second share a requirement. + */ +bool RequirementsDisjoint(Command* first, Command* second); +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibc/src/main/native/include/frc2/command/CommandBase.h new file mode 100644 index 0000000000..7597d210a1 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandBase.h @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include +#include + +#include "Command.h" + +namespace frc2 { +/** + * A Sendable base class for Commands. + */ +class CommandBase : public frc::Sendable, public Command { + public: + CommandBase(CommandBase&& other) = default; + + CommandBase(const CommandBase& other); + + /** + * Adds the specified requirements to the command. + * + * @param requirements the requirements to add + */ + void AddRequirements(std::initializer_list requirements); + + void AddRequirements(wpi::SmallSet requirements); + + wpi::SmallSet GetRequirements() const override; + + void SetName(const wpi::Twine& name) override; + + std::string GetName() const override; + + std::string GetSubsystem() const override; + + void SetSubsystem(const wpi::Twine& subsystem) override; + + void InitSendable(frc::SendableBuilder& builder) override; + + protected: + CommandBase(); + std::string m_name; + std::string m_subsystem; + wpi::SmallSet m_requirements; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h new file mode 100644 index 0000000000..9d265b493e --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "CommandBase.h" + +namespace frc2 { + +/** + * A base for CommandGroups. Statically tracks commands that have been + * allocated to groups to ensure those commands are not also used independently, + * which can result in inconsistent command state and unpredictable execution. + */ +class CommandGroupBase : public CommandBase { + public: + /** + * Requires that the specified command not have been already allocated to a + * CommandGroup. Reports an error if the command is already grouped. + * + * @param commands The command to check + * @return True if all the command is ungrouped. + */ + static bool RequireUngrouped(Command& command); + + /** + * Requires that the specified commands not have been already allocated to a + * CommandGroup. Reports an error if any of the commands are already grouped. + * + * @param commands The commands to check + * @return True if all the commands are ungrouped. + */ + static bool RequireUngrouped(wpi::ArrayRef>); + + /** + * Requires that the specified commands not have been already allocated to a + * CommandGroup. Reports an error if any of the commands are already grouped. + * + * @param commands The commands to check + * @return True if all the commands are ungrouped. + */ + static bool RequireUngrouped(std::initializer_list); + + /** + * Adds the given commands to the command group. + * + * @param commands The commands to add. + */ + virtual void AddCommands( + std::vector>&& commands) = 0; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h new file mode 100644 index 0000000000..ec15f88020 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +#include "Command.h" + +namespace frc2 { + +/** + * CRTP implementation to allow polymorphic decorator functions in Command. + * + *

Note: ALWAYS create a subclass by extending CommandHelper, + * or decorators will not function! + */ +template >> +class CommandHelper : public Base { + using Base::Base; + + public: + CommandHelper() = default; + + protected: + std::unique_ptr TransferOwnership() && override { + return std::make_unique(std::move(*static_cast(this))); + } +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h new file mode 100644 index 0000000000..ac6179d0b2 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h @@ -0,0 +1,362 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "CommandState.h" + +namespace frc2 { +class Command; +class Subsystem; + +/** + * The scheduler responsible for running Commands. A Command-based robot should + * call Run() on the singleton instance in its periodic block in order to run + * commands synchronously from the main loop. Subsystems should be registered + * with the scheduler using RegisterSubsystem() in order for their Periodic() + * methods to be called and for their default commands to be scheduled. + */ +class CommandScheduler final : public frc::SendableBase, frc::ErrorBase { + public: + /** + * Returns the Scheduler instance. + * + * @return the instance + */ + static CommandScheduler& GetInstance(); + + using Action = std::function; + + /** + * Adds a button binding to the scheduler, which will be polled to schedule + * commands. + * + * @param button The button to add + */ + void AddButton(wpi::unique_function button); + + /** + * Removes all button bindings from the scheduler. + */ + void ClearButtons(); + + /** + * Schedules a command for execution. Does nothing if the command is already + * scheduled. If a command's requirements are not available, it will only be + * started if all the commands currently using those requirements have been + * scheduled as interruptible. If this is the case, they will be interrupted + * and the command will be scheduled. + * + * @param interruptible whether this command can be interrupted + * @param command the command to schedule + */ + void Schedule(bool interruptible, Command* command); + + /** + * Schedules a command for execution, with interruptible defaulted to true. + * Does nothing if the command is already scheduled. + * + * @param command the command to schedule + */ + void Schedule(Command* command); + + /** + * Schedules multiple commands for execution. Does nothing if the command is + * already scheduled. If a command's requirements are not available, it will + * only be started if all the commands currently using those requirements have + * been scheduled as interruptible. If this is the case, they will be + * interrupted and the command will be scheduled. + * + * @param interruptible whether the commands should be interruptible + * @param commands the commands to schedule + */ + void Schedule(bool interruptible, wpi::ArrayRef commands); + + /** + * Schedules multiple commands for execution. Does nothing if the command is + * already scheduled. If a command's requirements are not available, it will + * only be started if all the commands currently using those requirements have + * been scheduled as interruptible. If this is the case, they will be + * interrupted and the command will be scheduled. + * + * @param interruptible whether the commands should be interruptible + * @param commands the commands to schedule + */ + void Schedule(bool interruptible, std::initializer_list commands); + + /** + * Schedules multiple commands for execution, with interruptible defaulted to + * true. Does nothing if the command is already scheduled. + * + * @param commands the commands to schedule + */ + void Schedule(wpi::ArrayRef commands); + + /** + * Schedules multiple commands for execution, with interruptible defaulted to + * true. Does nothing if the command is already scheduled. + * + * @param commands the commands to schedule + */ + void Schedule(std::initializer_list commands); + + /** + * Runs a single iteration of the scheduler. The execution occurs in the + * following order: + * + *

Subsystem periodic methods are called. + * + *

Button bindings are polled, and new commands are scheduled from them. + * + *

Currently-scheduled commands are executed. + * + *

End conditions are checked on currently-scheduled commands, and commands + * that are finished have their end methods called and are removed. + * + *

Any subsystems not being used as requirements have their default methods + * started. + */ + void Run(); + + /** + * Registers subsystems with the scheduler. This must be called for the + * subsystem's periodic block to run when the scheduler is run, and for the + * subsystem's default command to be scheduled. It is recommended to call + * this from the constructor of your subsystem implementations. + * + * @param subsystem the subsystem to register + */ + void RegisterSubsystem(Subsystem* subsystem); + + /** + * Un-registers subsystems with the scheduler. The subsystem will no longer + * have its periodic block called, and will not have its default command + * scheduled. + * + * @param subsystem the subsystem to un-register + */ + void UnregisterSubsystem(Subsystem* subsystem); + + void RegisterSubsystem(std::initializer_list subsystems); + + void UnregisterSubsystem(std::initializer_list subsystems); + + /** + * Sets the default command for a subsystem. Registers that subsystem if it + * is not already registered. Default commands will run whenever there is no + * other command currently scheduled that requires the subsystem. Default + * commands should be written to never end (i.e. their IsFinished() method + * should return false), as they would simply be re-scheduled if they do. + * Default commands must also require their subsystem. + * + * @param subsystem the subsystem whose default command will be set + * @param defaultCommand the default command to associate with the subsystem + */ + template >::value>> + void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) { + if (!defaultCommand.HasRequirement(subsystem)) { + wpi_setWPIErrorWithContext( + CommandIllegalUse, "Default commands must require their subsystem!"); + return; + } + if (defaultCommand.IsFinished()) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Default commands should not end!"); + return; + } + m_subsystems[subsystem] = std::make_unique>( + std::forward(defaultCommand)); + } + + /** + * Gets the default command associated with this subsystem. Null if this + * subsystem has no default command associated with it. + * + * @param subsystem the subsystem to inquire about + * @return the default command associated with the subsystem + */ + Command* GetDefaultCommand(const Subsystem* subsystem) const; + + /** + * Cancels a command. The scheduler will only call the interrupted method of + * a canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param command the command to cancel + */ + void Cancel(Command* command); + + /** + * Cancels commands. The scheduler will only call the interrupted method of a + * canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param commands the commands to cancel + */ + void Cancel(wpi::ArrayRef commands); + + /** + * Cancels commands. The scheduler will only call the interrupted method of a + * canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param commands the commands to cancel + */ + void Cancel(std::initializer_list commands); + + /** + * Cancels all commands that are currently scheduled. + */ + void CancelAll(); + + /** + * Returns the time since a given command was scheduled. Note that this only + * works on commands that are directly scheduled by the scheduler; it will not + * work on commands inside of commandgroups, as the scheduler does not see + * them. + * + * @param command the command to query + * @return the time since the command was scheduled, in seconds + */ + double TimeSinceScheduled(const Command* command) const; + + /** + * Whether the given commands are running. Note that this only works on + * commands that are directly scheduled by the scheduler; it will not work on + * commands inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(wpi::ArrayRef commands) const; + + /** + * Whether the given commands are running. Note that this only works on + * commands that are directly scheduled by the scheduler; it will not work on + * commands inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(std::initializer_list commands) const; + + /** + * Whether a given command is running. Note that this only works on commands + * that are directly scheduled by the scheduler; it will not work on commands + * inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(const Command* command) const; + + /** + * Returns the command currently requiring a given subsystem. Null if no + * command is currently requiring the subsystem + * + * @param subsystem the subsystem to be inquired about + * @return the command currently requiring the subsystem + */ + Command* Requiring(const Subsystem* subsystem) const; + + /** + * Disables the command scheduler. + */ + void Disable(); + + /** + * Enables the command scheduler. + */ + void Enable(); + + /** + * Adds an action to perform on the initialization of any command by the + * scheduler. + * + * @param action the action to perform + */ + void OnCommandInitialize(Action action); + + /** + * Adds an action to perform on the execution of any command by the scheduler. + * + * @param action the action to perform + */ + void OnCommandExecute(Action action); + + /** + * Adds an action to perform on the interruption of any command by the + * scheduler. + * + * @param action the action to perform + */ + void OnCommandInterrupt(Action action); + + /** + * Adds an action to perform on the finishing of any command by the scheduler. + * + * @param action the action to perform + */ + void OnCommandFinish(Action action); + + void InitSendable(frc::SendableBuilder& builder) override; + + private: + // Constructor; private as this is a singleton + CommandScheduler(); + + // A map from commands to their scheduling state. Also used as a set of the + // currently-running commands. + wpi::DenseMap m_scheduledCommands; + + // A map from required subsystems to their requiring commands. Also used as a + // set of the currently-required subsystems. + wpi::DenseMap m_requirements; + + // A map from subsystems registered with the scheduler to their default + // commands. Also used as a list of currently-registered subsystems. + wpi::DenseMap> m_subsystems; + + // The set of currently-registered buttons that will be polled every + // iteration. + wpi::SmallVector, 4> m_buttons; + + bool m_disabled{false}; + + // NetworkTable entries for use in Sendable impl + nt::NetworkTableEntry m_namesEntry; + nt::NetworkTableEntry m_idsEntry; + nt::NetworkTableEntry m_cancelEntry; + + // Lists of user-supplied actions to be executed on scheduling events for + // every command. + wpi::SmallVector m_initActions; + wpi::SmallVector m_executeActions; + wpi::SmallVector m_interruptActions; + wpi::SmallVector m_finishActions; + + friend class CommandTestBase; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibc/src/main/native/include/frc2/command/CommandState.h new file mode 100644 index 0000000000..41fc5d036b --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandState.h @@ -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 + +namespace frc2 { +/** + * Class that holds scheduling state for a command. Used internally by the + * CommandScheduler + */ +class CommandState final { + public: + CommandState() = default; + + explicit CommandState(bool interruptible); + + bool IsInterruptible() const { return m_interruptible; } + + // The time since this command was initialized. + double TimeSinceInitialized() const; + + private: + double m_startTime = -1; + bool m_interruptible; + + void StartTiming(); + void StartRunning(); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h new file mode 100644 index 0000000000..01571861d7 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * Runs one of two commands, depending on the value of the given condition when + * this command is initialized. Does not actually schedule the selected command + * - rather, the command is run through this command; this ensures that the + * command will behave as expected if used as part of a CommandGroup. Requires + * the requirements of both commands, again to ensure proper functioning when + * used in a CommandGroup. If this is undesired, consider using + * ScheduleCommand. + * + *

As this command contains multiple component commands within it, it is + * technically a command group; the command instances that are passed to it + * cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + * + * @see ScheduleCommand + */ +class ConditionalCommand + : public CommandHelper { + public: + /** + * Creates a new ConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + template >::value>, + typename = std::enable_if_t< + std::is_base_of>::value>> + ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function condition) + : ConditionalCommand(std::make_unique>( + std::forward(onTrue)), + std::make_unique>( + std::forward(onFalse)), + condition) {} + + /** + * Creates a new ConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + ConditionalCommand(std::unique_ptr&& onTrue, + std::unique_ptr&& onFalse, + std::function condition); + + ConditionalCommand(ConditionalCommand&& other) = default; + + // No copy constructors for command groups + ConditionalCommand(const ConditionalCommand& other) = delete; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + std::unique_ptr m_onTrue; + std::unique_ptr m_onFalse; + std::function m_condition; + Command* m_selectedCommand{nullptr}; + bool m_runsWhenDisabled = true; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h new file mode 100644 index 0000000000..b47135c52f --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that allows the user to pass in functions for each of the basic + * command methods through the constructor. Useful for inline definitions of + * complex commands - note, however, that if a command is beyond a certain + * complexity it is usually better practice to write a proper class for it than + * to inline it. + */ +class FunctionalCommand : public CommandHelper { + public: + /** + * Creates a new FunctionalCommand. + * + * @param onInit the function to run on command initialization + * @param onExecute the function to run on command execution + * @param onEnd the function to run on command end + * @param isFinished the function that determines whether the command has + * finished + * @param requirements the subsystems required by this command + */ + FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished); + + FunctionalCommand(FunctionalCommand&& other) = default; + + FunctionalCommand(const FunctionalCommand& other) = default; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + std::function m_onInit; + std::function m_onExecute; + std::function m_onEnd; + std::function m_isFinished; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h new file mode 100644 index 0000000000..ec28a11023 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A Command that runs instantly; it will initialize, execute once, and end on + * the same iteration of the scheduler. Users can either pass in a Runnable and + * a set of requirements, or else subclass this command if desired. + */ +class InstantCommand : public CommandHelper { + public: + /** + * Creates a new InstantCommand that runs the given Runnable with the given + * requirements. + * + * @param toRun the Runnable to run + * @param requirements the subsystems required by this command + */ + InstantCommand(std::function toRun, + std::initializer_list requirements); + + InstantCommand(InstantCommand&& other) = default; + + InstantCommand(const InstantCommand& other) = default; + + /** + * Creates a new InstantCommand with a Runnable that does nothing. Useful + * only as a no-arg constructor to call implicitly from subclass constructors. + */ + InstantCommand(); + + void Initialize() override; + + bool IsFinished() final; + + private: + std::function m_toRun; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h new file mode 100644 index 0000000000..037e5c3d71 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that starts a notifier to run the given runnable periodically in a + * separate thread. Has no end condition as-is; either subclass it or use {@link + * Command#withTimeout(double)} or + * {@link Command#interruptOn(BooleanSupplier)} to give it one. + * + *

WARNING: Do not use this class unless you are confident in your ability to + * make the executed code thread-safe. If you do not know what "thread-safe" + * means, that is a good sign that you should not use this class. + */ +class NotifierCommand : public CommandHelper { + public: + /** + * Creates a new NotifierCommand. + * + * @param toRun the runnable for the notifier to run + * @param period the period at which the notifier should run, in seconds + * @param requirements the subsystems required by this command + */ + NotifierCommand(std::function toRun, double period, + std::initializer_list requirements); + + NotifierCommand(NotifierCommand&& other); + + NotifierCommand(const NotifierCommand& other); + + void Initialize() override; + + void End(bool interrupted) override; + + private: + std::function m_toRun; + frc::Notifier m_notifier; + double m_period; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h new file mode 100644 index 0000000000..a54a485d73 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h @@ -0,0 +1,108 @@ +/*----------------------------------------------------------------------------*/ +/* 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/PIDController.h" +#include "frc2/command/CommandBase.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that controls an output with a PIDController. Runs forever by + * default - to add exit conditions and/or other behavior, subclass this class. + * The controller calculation and output are performed synchronously in the + * command's execute() method. + * + * @see PIDController + */ +class PIDCommand : public CommandHelper { + public: + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpointSource the controller's reference (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController with a constant setpoint. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpoint the controller's setpoint (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, double setpoint, + std::function useOutput, + std::initializer_list requirements); + + PIDCommand(PIDCommand&& other) = default; + + PIDCommand(const PIDCommand& other) = default; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + /** + * Sets the function that uses the output of the PIDController. + * + * @param useOutput The function that uses the output. + */ + void SetOutput(std::function useOutput); + + /** + * Sets the setpoint for the controller to track the given source. + * + * @param setpointSource The setpoint source + */ + void SetSetpoint(std::function setpointSource); + + /** + * Sets the setpoint for the controller to a constant value. + * + * @param setpoint The setpoint + */ + void SetSetpoint(double setpoint); + + /** + * Sets the setpoint for the controller to a constant value relative (i.e. + * added to) the current setpoint. + * + * @param relativeReference The change in setpoint + */ + void SetSetpointRelative(double relativeSetpoint); + + /** + * Returns the PIDController used by the command. + * + * @return The PIDController + */ + PIDController& getController(); + + protected: + PIDController m_controller; + std::function m_measurement; + std::function m_setpoint; + std::function m_useOutput; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h new file mode 100644 index 0000000000..7aa21c00fa --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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/PIDController.h" +#include "frc2/command/SubsystemBase.h" + +namespace frc2 { +/** + * A subsystem that uses a PIDController to control an output. The controller + * is run synchronously from the subsystem's periodic() method. + * + * @see PIDController + */ +class PIDSubsystem : public SubsystemBase { + public: + /** + * Creates a new PIDSubsystem. + * + * @param controller the PIDController to use + */ + explicit PIDSubsystem(PIDController controller); + + void Periodic() override; + + /** + * Uses the output from the PIDController. + * + * @param output the output of the PIDController + */ + virtual void UseOutput(double output) = 0; + + /** + * Returns the reference (setpoint) used by the PIDController. + * + * @return the reference (setpoint) to be used by the controller + */ + virtual double GetSetpoint() = 0; + + /** + * Returns the measurement of the process variable used by the PIDController. + * + * @return the measurement of the process variable + */ + virtual double GetMeasurement() = 0; + + /** + * Enables the PID control. Resets the controller. + */ + virtual void Enable(); + + /** + * Disables the PID control. Sets output to zero. + */ + virtual void Disable(); + + /** + * Returns the PIDController. + * + * @return The controller. + */ + PIDController& GetController(); + + protected: + PIDController m_controller; + bool m_enabled; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h new file mode 100644 index 0000000000..53aca10be0 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending when the last + * command ends. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelCommandGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelCommandGroup. The given commands will be executed + * simultaneously. The command group will finish when the last command + * finishes. If the CommandGroup is interrupted, only the commands that are + * still running will be interrupted. + * + * @param commands the commands to include in this group. + */ + explicit ParallelCommandGroup( + std::vector>&& commands); + + /** + * Creates a new ParallelCommandGroup. The given commands will be executed + * simultaneously. The command group will finish when the last command + * finishes. If the CommandGroup is interrupted, only the commands that are + * still running will be interrupted. + * + * @param commands the commands to include in this group. + */ + template >...>>> + explicit ParallelCommandGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + ParallelCommandGroup(ParallelCommandGroup&& other) = default; + + // No copy constructors for commandgroups + ParallelCommandGroup(const ParallelCommandGroup&) = delete; + + template + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + std::unordered_map, bool> m_commands; + bool m_runWhenDisabled{true}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h new file mode 100644 index 0000000000..d76f033841 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h @@ -0,0 +1,99 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending only when a + * specific command (the "deadline") ends, interrupting all other commands that + * are still running at that point. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelDeadlineGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelDeadlineGroup. The given commands (including the + * deadline) will be executed simultaneously. The CommandGroup will finish + * when the deadline finishes, interrupting all other still-running commands. + * If the CommandGroup is interrupted, only the commands still running will be + * interrupted. + * + * @param deadline the command that determines when the group ends + * @param commands the commands to be executed + */ + ParallelDeadlineGroup(std::unique_ptr&& deadline, + std::vector>&& commands); + /** + * Creates a new ParallelDeadlineGroup. The given commands (including the + * deadline) will be executed simultaneously. The CommandGroup will finish + * when the deadline finishes, interrupting all other still-running commands. + * If the CommandGroup is interrupted, only the commands still running will be + * interrupted. + * + * @param deadline the command that determines when the group ends + * @param commands the commands to be executed + */ + template >::value>, + typename = std::enable_if_t>...>>> + explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) { + SetDeadline(std::make_unique>( + std::forward(deadline))); + AddCommands(std::forward(commands)...); + } + + ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default; + + // No copy constructors for command groups + ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete; + + template >...>>> + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + void SetDeadline(std::unique_ptr&& deadline); + + std::unordered_map, bool> m_commands; + Command* m_deadline; + bool m_runWhenDisabled{true}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h new file mode 100644 index 0000000000..ba249cf5a5 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending when any one + * of the commands ends and interrupting all the others. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelRaceGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelCommandRace. The given commands will be executed + * simultaneously, and will "race to the finish" - the first command to finish + * ends the entire command, with all other commands being interrupted. + * + * @param commands the commands to include in this group. + */ + explicit ParallelRaceGroup(std::vector>&& commands); + + template >...>>> + explicit ParallelRaceGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + ParallelRaceGroup(ParallelRaceGroup&& other) = default; + + // No copy constructors for command groups + ParallelRaceGroup(const ParallelRaceGroup&) = delete; + + template + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + std::set> m_commands; + bool m_runWhenDisabled{true}; + bool m_finished{false}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h new file mode 100644 index 0000000000..7ed2dfc2b6 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs another command in perpetuity, ignoring that command's + * end conditions. While this class does not extend {@link CommandGroupBase}, + * it is still considered a CommandGroup, as it allows one to compose another + * command within it; the command instances that are passed to it cannot be + * added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class PerpetualCommand : public CommandHelper { + public: + /** + * Creates a new PerpetualCommand. Will run another command in perpetuity, + * ignoring that command's end conditions, unless this command itself is + * interrupted. + * + * @param command the command to run perpetually + */ + explicit PerpetualCommand(std::unique_ptr&& command); + + /** + * Creates a new PerpetualCommand. Will run another command in perpetuity, + * ignoring that command's end conditions, unless this command itself is + * interrupted. + * + * @param command the command to run perpetually + */ + template >::value>> + explicit PerpetualCommand(T&& command) + : PerpetualCommand(std::make_unique>( + std::forward(command))) {} + + PerpetualCommand(PerpetualCommand&& other) = default; + + // No copy constructors for command groups + PerpetualCommand(const PerpetualCommand& other) = delete; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + private: + std::unique_ptr m_command; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h new file mode 100644 index 0000000000..fb420cb179 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "CommandHelper.h" +#include "InstantCommand.h" + +namespace frc2 { +/** + * A command that prints a string when initialized. + */ +class PrintCommand : public CommandHelper { + public: + /** + * Creates a new a PrintCommand. + * + * @param message the message to print + */ + explicit PrintCommand(const wpi::Twine& message); + + PrintCommand(PrintCommand&& other) = default; + + PrintCommand(const PrintCommand& other) = default; + + bool RunsWhenDisabled() const override; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h new file mode 100644 index 0000000000..8906424d7b --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h @@ -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 + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "SetUtilities.h" + +namespace frc2 { +/** + * Schedules the given commands when this command is initialized, and ends when + * all the commands are no longer scheduled. Useful for forking off from + * CommandGroups. If this command is interrupted, it will cancel all of the + * commands. + */ +class ProxyScheduleCommand + : public CommandHelper { + public: + /** + * Creates a new ProxyScheduleCommand that schedules the given commands when + * initialized, and ends when they are all no longer scheduled. + * + * @param toSchedule the commands to schedule + */ + explicit ProxyScheduleCommand(wpi::ArrayRef toSchedule); + + ProxyScheduleCommand(ProxyScheduleCommand&& other) = default; + + ProxyScheduleCommand(const ProxyScheduleCommand& other) = default; + + void Initialize() override; + + void End(bool interrupted) override; + + void Execute() override; + + bool IsFinished() override; + + private: + wpi::SmallVector m_toSchedule; + bool m_finished{false}; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibc/src/main/native/include/frc2/command/RunCommand.h new file mode 100644 index 0000000000..63b1ab8fc4 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/RunCommand.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs a Runnable continuously. Has no end condition as-is; + * either subclass it or use Command.WithTimeout() or + * Command.InterruptOn() to give it one. If you only wish + * to execute a Runnable once, use InstantCommand. + */ +class RunCommand : public CommandHelper { + public: + /** + * Creates a new RunCommand. The Runnable will be run continuously until the + * command ends. Does not run when disabled. + * + * @param toRun the Runnable to run + * @param requirements the subsystems to require + */ + RunCommand(std::function toRun, + std::initializer_list requirements); + + RunCommand(RunCommand&& other) = default; + + RunCommand(const RunCommand& other) = default; + + void Execute(); + + protected: + std::function m_toRun; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h new file mode 100644 index 0000000000..422823b349 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h @@ -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 + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "SetUtilities.h" + +namespace frc2 { +/** + * Schedules the given commands when this command is initialized. Useful for + * forking off from CommandGroups. Note that if run from a CommandGroup, the + * group will not know about the status of the scheduled commands, and will + * treat this command as finishing instantly. + */ +class ScheduleCommand : public CommandHelper { + public: + /** + * Creates a new ScheduleCommand that schedules the given commands when + * initialized. + * + * @param toSchedule the commands to schedule + */ + explicit ScheduleCommand(wpi::ArrayRef toSchedule); + + ScheduleCommand(ScheduleCommand&& other) = default; + + ScheduleCommand(const ScheduleCommand& other) = default; + + void Initialize() override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + wpi::SmallVector m_toSchedule; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h new file mode 100644 index 0000000000..903b8d159a --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "PrintCommand.h" + +namespace frc2 { +template +/** + * Runs one of a selection of commands, either using a selector and a key to + * command mapping, or a supplier that returns the command directly at runtime. + * Does not actually schedule the selected command - rather, the command is run + * through this command; this ensures that the command will behave as expected + * if used as part of a CommandGroup. Requires the requirements of all included + * commands, again to ensure proper functioning when used in a CommandGroup. If + * this is undesired, consider using ScheduleCommand. + * + *

As this command contains multiple component commands within it, it is + * technically a command group; the command instances that are passed to it + * cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class SelectCommand : public CommandHelper> { + public: + /** + * Creates a new selectcommand. + * + * @param commands the map of commands to choose from + * @param selector the selector to determine which command to run + */ + template >...>>> + SelectCommand(std::function selector, + std::pair... commands) + : m_selector{std::move(selector)} { + std::vector>> foo; + + ((void)foo.emplace_back(commands.first, + std::make_unique>( + std::move(commands.second))), + ...); + + for (auto&& command : foo) { + if (!CommandGroupBase::RequireUngrouped(command.second)) { + return; + } + } + + for (auto&& command : foo) { + this->AddRequirements(command.second->GetRequirements()); + m_runsWhenDisabled &= command.second->RunsWhenDisabled(); + m_commands.emplace(std::move(command.first), std::move(command.second)); + } + } + + SelectCommand( + std::function selector, + std::vector>>&& commands) + : m_selector{std::move(selector)} { + for (auto&& command : commands) { + if (!CommandGroupBase::RequireUngrouped(command.second)) { + return; + } + } + + for (auto&& command : commands) { + this->AddRequirements(command.second->GetRequirements()); + m_runsWhenDisabled &= command.second->RunsWhenDisabled(); + m_commands.emplace(std::move(command.first), std::move(command.second)); + } + } + + // No copy constructors for command groups + SelectCommand(const SelectCommand& other) = delete; + + /** + * Creates a new selectcommand. + * + * @param toRun a supplier providing the command to run + */ + explicit SelectCommand(std::function toRun) : m_toRun{toRun} {} + + SelectCommand(SelectCommand&& other) = default; + + void Initialize() override; + + void Execute() override { m_selectedCommand->Execute(); } + + void End(bool interrupted) override { + return m_selectedCommand->End(interrupted); + } + + bool IsFinished() override { return m_selectedCommand->IsFinished(); } + + bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } + + protected: + std::unique_ptr TransferOwnership() && override { + return std::make_unique(std::move(*this)); + } + + private: + std::unordered_map> m_commands; + std::function m_selector; + std::function m_toRun; + Command* m_selectedCommand; + bool m_runsWhenDisabled = true; +}; + +template +void SelectCommand::Initialize() { + if (m_selector) { + auto find = m_commands.find(m_selector()); + if (find == m_commands.end()) { + m_selectedCommand = new PrintCommand( + "SelectCommand selector value does not correspond to any command!"); + return; + } + m_selectedCommand = find->second.get(); + } else { + m_selectedCommand = m_toRun(); + } + m_selectedCommand->Initialize(); +} + +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h new file mode 100644 index 0000000000..ebb1b4dd43 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include + +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" +#include "frc/ErrorBase.h" +#include "frc/WPIErrors.h" + +namespace frc2 { + +const size_t invalid_index = std::numeric_limits::max(); + +/** + * A CommandGroups that runs a list of commands in sequence. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class SequentialCommandGroup + : public CommandHelper { + public: + /** + * Creates a new SequentialCommandGroup. The given commands will be run + * sequentially, with the CommandGroup finishing when the last command + * finishes. + * + * @param commands the commands to include in this group. + */ + explicit SequentialCommandGroup( + std::vector>&& commands); + + /** + * Creates a new SequentialCommandGroup. The given commands will be run + * sequentially, with the CommandGroup finishing when the last command + * finishes. + * + * @param commands the commands to include in this group. + */ + template >...>>> + explicit SequentialCommandGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + SequentialCommandGroup(SequentialCommandGroup&& other) = default; + + // No copy constructors for command groups + SequentialCommandGroup(const SequentialCommandGroup&) = delete; + + template >...>>> + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) final; + + wpi::SmallVector, 4> m_commands; + size_t m_currentCommandIndex{invalid_index}; + bool m_runWhenDisabled{true}; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h new file mode 100644 index 0000000000..d21f5725c8 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +namespace frc2 { +template +void SetInsert(wpi::SmallVectorImpl& vector, wpi::ArrayRef toAdd) { + for (auto addCommand : toAdd) { + bool exists = false; + for (auto existingCommand : vector) { + if (addCommand == existingCommand) { + exists = true; + break; + } + } + if (!exists) { + vector.emplace_back(addCommand); + } + } +} +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h new file mode 100644 index 0000000000..de59f828ad --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h @@ -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 "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs a given runnable when it is initalized, and another + * runnable when it ends. Useful for running and then stopping a motor, or + * extending and then retracting a solenoid. Has no end condition as-is; either + * subclass it or use Command.WithTimeout() or Command.InterruptOn() to give it + * one. + */ +class StartEndCommand : public CommandHelper { + public: + /** + * Creates a new StartEndCommand. Will run the given runnables when the + * command starts and when it ends. + * + * @param onInit the Runnable to run on command init + * @param onEnd the Runnable to run on command end + * @param requirements the subsystems required by this command + */ + StartEndCommand(std::function onInit, std::function onEnd, + std::initializer_list requirements); + + StartEndCommand(StartEndCommand&& other) = default; + + StartEndCommand(const StartEndCommand& other); + + void Initialize() override; + + void End(bool interrupted) override; + + protected: + std::function m_onInit; + std::function m_onEnd; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibc/src/main/native/include/frc2/command/Subsystem.h new file mode 100644 index 0000000000..f549e64264 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/Subsystem.h @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +namespace frc2 { +class Command; +/** + * A robot subsystem. Subsystems are the basic unit of robot organization in + * the Command-based framework; they encapsulate low-level hardware objects + * (motor controllers, sensors, etc) and provide methods through which they can + * be used by Commands. Subsystems are used by the CommandScheduler's resource + * management system to ensure multiple robot actions are not "fighting" over + * the same hardware; Commands that use a subsystem should include that + * subsystem in their GetRequirements() method, and resources used within a + * subsystem should generally remain encapsulated and not be shared by other + * parts of the robot. + * + *

Subsystems must be registered with the scheduler with the + * CommandScheduler.RegisterSubsystem() method in order for the + * Periodic() method to be called. It is recommended that this method be called + * from the constructor of users' Subsystem implementations. The + * SubsystemBase class offers a simple base for user implementations + * that handles this. + * + * @see Command + * @see CommandScheduler + * @see SubsystemBase + */ +class Subsystem { + public: + ~Subsystem(); + /** + * This method is called periodically by the CommandScheduler. Useful for + * updating subsystem-specific state that you don't want to offload to a + * Command. Teams should try to be consistent within their own codebases + * about which responsibilities will be handled by Commands, and which will be + * handled here. + */ + virtual void Periodic(); + + /** + * Sets the default Command of the subsystem. The default command will be + * automatically scheduled when no other commands are scheduled that require + * the subsystem. Default commands should generally not end on their own, i.e. + * their IsFinished() method should always return false. Will automatically + * register this subsystem with the CommandScheduler. + * + * @param defaultCommand the default command to associate with this subsystem + */ + template >::value>> + void SetDefaultCommand(T&& defaultCommand) { + CommandScheduler::GetInstance().SetDefaultCommand( + this, std::forward(defaultCommand)); + } + + /** + * Gets the default command for this subsystem. Returns null if no default + * command is currently associated with the subsystem. + * + * @return the default command associated with this subsystem + */ + Command* GetDefaultCommand() const; + + /** + * Returns the command currently running on this subsystem. Returns null if + * no command is currently scheduled that requires this subsystem. + * + * @return the scheduled command currently requiring this subsystem + */ + Command* GetCurrentCommand() const; + + /** + * Registers this subsystem with the CommandScheduler, allowing its + * Periodic() method to be called when the scheduler runs. + */ + void Register(); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h new file mode 100644 index 0000000000..2546f5bfab --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "Subsystem.h" + +namespace frc2 { +/** + * A base for subsystems that handles registration in the constructor, and + * provides a more intuitive method for setting the default command. + */ +class SubsystemBase : public Subsystem, public frc::Sendable { + public: + void InitSendable(frc::SendableBuilder& builder) override; + std::string GetName() const override; + void SetName(const wpi::Twine& name) override; + std::string GetSubsystem() const override; + void SetSubsystem(const wpi::Twine& name) override; + void AddChild(std::string name, frc::Sendable* child); + + protected: + SubsystemBase(); + std::string m_name; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h new file mode 100644 index 0000000000..070790b358 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "frc/Timer.h" + +namespace frc2 { +/** + * A command that does nothing but takes a specified amount of time to finish. + * Useful for CommandGroups. Can also be subclassed to make a command with an + * internal timer. + */ +class WaitCommand : public CommandHelper { + public: + /** + * Creates a new WaitCommand. This command will do nothing, and end after the + * specified duration. + * + * @param seconds the time to wait, in seconds + */ + explicit WaitCommand(double seconds); + + WaitCommand(WaitCommand&& other) = default; + + WaitCommand(const WaitCommand& other) = default; + + void Initialize() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + protected: + std::unique_ptr m_timer; + + private: + double m_duration; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h new file mode 100644 index 0000000000..8da18e80c6 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandBase.h" +#include "frc/Timer.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that does nothing but ends after a specified match time or + * condition. Useful for CommandGroups. + */ +class WaitUntilCommand : public CommandHelper { + public: + /** + * Creates a new WaitUntilCommand that ends after a given condition becomes + * true. + * + * @param condition the condition to determine when to end + */ + explicit WaitUntilCommand(std::function condition); + + /** + * Creates a new WaitUntilCommand that ends after a given match time. + * + *

NOTE: The match timer used for this command is UNOFFICIAL. Using this + * command does NOT guarantee that the time at which the action is performed + * will be judged to be legal by the referees. When in doubt, add a safety + * factor or time the action manually. + * + * @param time the match time after which to end, in seconds + */ + explicit WaitUntilCommand(double time); + + WaitUntilCommand(WaitUntilCommand&& other) = default; + + WaitUntilCommand(const WaitUntilCommand& other) = default; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + std::function m_condition; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibc/src/main/native/include/frc2/command/button/Button.h new file mode 100644 index 0000000000..dc4c54a837 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/Button.h @@ -0,0 +1,207 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include "Trigger.h" + +namespace frc2 { +class Command; +/** + * A class used to bind command scheduling to button presses. Can be composed + * with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class Button : public Trigger { + public: + /** + * Create a new button that is pressed when the given condition is true. + * + * @param isActive Whether the button is pressed. + */ + explicit Button(std::function isPressed); + + /** + * Create a new button that is pressed active (default constructor) - activity + * can be further determined by subclass code. + */ + Button() = default; + + /** + * Binds a command to start when the button is pressed. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Button WhenPressed(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Button WhenPressed(T&& command, bool interruptible = true) { + WhenActive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute when the button is pressed. + * + * @param toRun the runnable to execute. + */ + Button WhenPressed(std::function toRun); + + /** + * Binds a command to be started repeatedly while the button is pressed, and + * cancelled when it is released. Takes a raw pointer, and so is non-owning; + * users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhileHeld(Command* command, bool interruptible = true); + + /** + * Binds a command to be started repeatedly while the button is pressed, and + * cancelled when it is released. Transfers command ownership to the button + * scheduler, so the user does not have to worry about lifespan - rvalue refs + * will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhileHeld(T&& command, bool interruptible = true) { + WhileActiveContinous(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute repeatedly while the button is pressed. + * + * @param toRun the runnable to execute. + */ + Button WhileHeld(std::function toRun); + + /** + * Binds a command to be started when the button is pressed, and cancelled + * when it is released. Takes a raw pointer, and so is non-owning; users are + * responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhenHeld(Command* command, bool interruptible = true); + + /** + * Binds a command to be started when the button is pressed, and cancelled + * when it is released. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan - rvalue refs will be + * *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhenHeld(T&& command, bool interruptible = true) { + WhileActiveOnce(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a command to start when the button is released. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhenReleased(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhenReleased(T&& command, bool interruptible = true) { + WhenInactive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute when the button is released. + * + * @param toRun the runnable to execute. + */ + Button WhenReleased(std::function toRun); + + /** + * Binds a command to start when the button is pressed, and be cancelled when + * it is pressed again. Takes a raw pointer, and so is non-owning; users are + * responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button ToggleWhenPressed(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed, and be cancelled when + * it is pessed again. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan - rvalue refs will be + * *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button ToggleWhenPressed(T&& command, bool interruptible = true) { + ToggleWhenActive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a command to be cancelled when the button is pressed. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * and scheduling of the command. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button CancelWhenPressed(Command* command); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h new file mode 100644 index 0000000000..a23738bfa8 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include "Button.h" + +namespace frc2 { +/** + * A class used to bind command scheduling to joystick button presses. Can be + * composed with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class JoystickButton : public Button { + public: + /** + * Creates a JoystickButton that commands can be bound to. + * + * @param joystick The joystick on which the button is located. + * @param buttonNumber The number of the button on the joystic. + */ + explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber) + : m_joystick{joystick}, m_buttonNumber{buttonNumber} {} + + bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); } + + private: + frc::GenericHID* m_joystick; + int m_buttonNumber; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h new file mode 100644 index 0000000000..758cab2056 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include "Button.h" + +namespace frc2 { +/** + * A class used to bind command scheduling to joystick POV presses. Can be + * composed with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class POVButton : public Button { + public: + /** + * Creates a POVButton that commands can be bound to. + * + * @param joystick The joystick on which the button is located. + * @param angle The angle of the POV corresponding to a button press. + * @param povNumber The number of the POV on the joystick. + */ + POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0) + : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {} + + bool Get() const override { + return m_joystick->GetPOV(m_povNumber) == m_angle; + } + + private: + frc::GenericHID* m_joystick; + int m_angle; + int m_povNumber; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h new file mode 100644 index 0000000000..30de38d394 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h @@ -0,0 +1,325 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include +#include +#include + +namespace frc2 { +class Command; +/** + * A class used to bind command scheduling to events. The + * Trigger class is a base for all command-event-binding classes, and so the + * methods are named fairly abstractly; for purpose-specific wrappers, see + * Button. + * + * @see Button + */ +class Trigger { + public: + /** + * Create a new trigger that is active when the given condition is true. + * + * @param isActive Whether the trigger is active. + */ + explicit Trigger(std::function isActive) + : m_isActive{std::move(isActive)} {} + + /** + * Create a new trigger that is never active (default constructor) - activity + * can be further determined by subclass code. + */ + Trigger() { + m_isActive = [] { return false; }; + } + + Trigger(const Trigger& other); + + /** + * Returns whether the trigger is active. Can be overridden by a subclass. + * + * @return Whether the trigger is active. + */ + virtual bool Get() const { return m_isActive(); } + + /** + * Binds a command to start when the trigger becomes active. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhenActive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes active. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhenActive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + + return *this; + } + + /** + * Binds a runnable to execute when the trigger becomes active. + * + * @param toRun the runnable to execute. + */ + Trigger WhenActive(std::function toRun); + + /** + * Binds a command to be started repeatedly while the trigger is active, and + * cancelled when it becomes inactive. Takes a raw pointer, and so is + * non-owning; users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveContinous(Command* command, bool interruptible = true); + + /** + * Binds a command to be started repeatedly while the trigger is active, and + * cancelled when it becomes inactive. Transfers command ownership to the + * button scheduler, so the user does not have to worry about lifespan - + * rvalue refs will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhileActiveContinous(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a runnable to execute repeatedly while the trigger is active. + * + * @param toRun the runnable to execute. + */ + Trigger WhileActiveContinous(std::function toRun); + + /** + * Binds a command to be started when the trigger becomes active, and + * cancelled when it becomes inactive. Takes a raw pointer, and so is + * non-owning; users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveOnce(Command* command, bool interruptible = true); + + /** + * Binds a command to be started when the trigger becomes active, and + * cancelled when it becomes inactive. Transfers command ownership to the + * button scheduler, so the user does not have to worry about lifespan - + * rvalue refs will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhileActiveOnce(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a command to start when the trigger becomes inactive. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhenInactive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes inactive. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhenInactive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (pressedLast && !pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a runnable to execute when the trigger becomes inactive. + * + * @param toRun the runnable to execute. + */ + Trigger WhenInactive(std::function toRun); + + /** + * Binds a command to start when the trigger becomes active, and be cancelled + * when it again becomes active. Takes a raw pointer, and so is non-owning; + * users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger ToggleWhenActive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes active, and be cancelled + * when it again becomes active. Transfers command ownership to the button + * scheduler, so the user does not have to worry about lifespan - rvalue refs + * will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger ToggleWhenActive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + if (command->IsScheduled()) { + command->Cancel(); + } else { + command->Schedule(interruptible); + } + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a command to be cancelled when the trigger becomes active. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * and scheduling of the command. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger CancelWhenActive(Command* command); + + /** + * Composes two triggers with logical AND. + * + * @return A trigger which is active when both component triggers are active. + */ + Trigger operator&&(Trigger rhs) { + return Trigger([*this, rhs] { return Get() && rhs.Get(); }); + } + + /** + * Composes two triggers with logical OR. + * + * @return A trigger which is active when either component trigger is active. + */ + Trigger operator||(Trigger rhs) { + return Trigger([*this, rhs] { return Get() || rhs.Get(); }); + } + + /** + * Composes a trigger with logical NOT. + * + * @return A trigger which is active when the component trigger is inactive, + * and vice-versa. + */ + Trigger operator!() { + return Trigger([*this] { return !Get(); }); + } + + private: + std::function m_isActive; +}; +} // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp new file mode 100644 index 0000000000..829f0d2ae2 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp @@ -0,0 +1,195 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/RunCommand.h" +#include "frc2/command/WaitUntilCommand.h" +#include "frc2/command/button/Trigger.h" +#include "gtest/gtest.h" + +using namespace frc2; +class ButtonTest : public CommandTestBase {}; + +TEST_F(ButtonTest, WhenPressedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + + WaitUntilCommand command([&finished] { return finished; }); + + Trigger([&pressed] { return pressed; }).WhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhenReleasedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = true; + Trigger([&pressed] { return pressed; }).WhenInactive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhileHeldTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + finished = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhenHeldTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + finished = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command); + pressed = true; + scheduler.Run(); + pressed = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, ToggleWhenPressedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + pressed = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, AndTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed1 = false; + bool pressed2 = false; + WaitUntilCommand command([&finished] { return finished; }); + + (Trigger([&pressed1] { return pressed1; }) && + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command); + pressed1 = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed2 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, OrTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed1 = false; + bool pressed2 = false; + WaitUntilCommand command1([&finished] { return finished; }); + WaitUntilCommand command2([&finished] { return finished; }); + + (Trigger([&pressed1] { return pressed1; }) || + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command1); + pressed1 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + + pressed1 = false; + + (Trigger([&pressed1] { return pressed1; }) || + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command2); + pressed2 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); +} + +TEST_F(ButtonTest, NegateTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = true; + WaitUntilCommand command([&finished] { return finished; }); + + (!Trigger([&pressed] { return pressed; })).WhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, RValueButtonTest) { + auto& scheduler = CommandScheduler::GetInstance(); + int counter = 0; + bool pressed = false; + + RunCommand command([&counter] { counter++; }, {}); + + Trigger([&pressed] { return pressed; }).WhenActive(std::move(command)); + scheduler.Run(); + EXPECT_EQ(counter, 0); + pressed = true; + scheduler.Run(); + EXPECT_EQ(counter, 1); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp new file mode 100644 index 0000000000..05f813aef4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/PerpetualCommand.h" +#include "frc2/command/RunCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class CommandDecoratorTest : public CommandTestBase {}; + +TEST_F(CommandDecoratorTest, WithTimeoutTest) { + CommandScheduler scheduler = GetScheduler(); + + auto command = RunCommand([] {}, {}).WithTimeout(.1); + + scheduler.Schedule(&command); + + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, InterruptOnTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = + RunCommand([] {}, {}).InterruptOn([&finished] { return finished; }); + + scheduler.Schedule(&command); + + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + finished = true; + + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, BeforeStartingTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = InstantCommand([] {}, {}).BeforeStarting( + [&finished] { finished = true; }); + + scheduler.Schedule(&command); + + EXPECT_TRUE(finished); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, WhenFinishedTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = + InstantCommand([] {}, {}).WhenFinished([&finished] { finished = true; }); + + scheduler.Schedule(&command); + + EXPECT_FALSE(finished); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(finished); +} + +TEST_F(CommandDecoratorTest, PerpetuallyTest) { + CommandScheduler scheduler = GetScheduler(); + + auto command = InstantCommand([] {}, {}).Perpetually(); + + scheduler.Schedule(&command); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp new file mode 100644 index 0000000000..260043064a --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SelectCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class CommandRequirementsTest : public CommandTestBase {}; + +TEST_F(CommandRequirementsTest, RequirementInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement; + + MockCommand command1({&requirement}); + MockCommand command2({&requirement}); + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()); + EXPECT_CALL(command1, End(true)); + EXPECT_CALL(command1, End(false)).Times(0); + + EXPECT_CALL(command2, Initialize()); + EXPECT_CALL(command2, Execute()); + EXPECT_CALL(command2, End(true)).Times(0); + EXPECT_CALL(command2, End(false)).Times(0); + + scheduler.Schedule(&command1); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + scheduler.Schedule(&command2); + EXPECT_FALSE(scheduler.IsScheduled(&command1)); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); + scheduler.Run(); +} + +TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement; + + MockCommand command1({&requirement}); + MockCommand command2({&requirement}); + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()).Times(2); + EXPECT_CALL(command1, End(true)).Times(0); + EXPECT_CALL(command1, End(false)).Times(0); + + EXPECT_CALL(command2, Initialize()).Times(0); + EXPECT_CALL(command2, Execute()).Times(0); + EXPECT_CALL(command2, End(true)).Times(0); + EXPECT_CALL(command2, End(false)).Times(0); + + scheduler.Schedule(false, &command1); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + scheduler.Schedule(&command2); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + EXPECT_FALSE(scheduler.IsScheduled(&command2)); + scheduler.Run(); +} + +TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) { + TestSubsystem requirement1; + ErrorConfirmer confirmer("require"); + + MockCommand command1; + + requirement1.SetDefaultCommand(std::move(command1)); + + EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp new file mode 100644 index 0000000000..6572a98e3a --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" + +using namespace frc2; +class CommandScheduleTest : public CommandTestBase {}; + +TEST_F(CommandScheduleTest, InstantScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()); + EXPECT_CALL(command, End(false)); + + command.SetFinished(true); + scheduler.Schedule(&command); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, SingleIterationScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()).Times(2); + EXPECT_CALL(command, End(false)); + + scheduler.Schedule(&command); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Run(); + command.SetFinished(true); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, MultiScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command1; + MockCommand command2; + MockCommand command3; + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()).Times(2); + EXPECT_CALL(command1, End(false)); + + EXPECT_CALL(command2, Initialize()); + EXPECT_CALL(command2, Execute()).Times(3); + EXPECT_CALL(command2, End(false)); + + EXPECT_CALL(command3, Initialize()); + EXPECT_CALL(command3, Execute()).Times(4); + EXPECT_CALL(command3, End(false)); + + scheduler.Schedule(&command1); + scheduler.Schedule(&command2); + scheduler.Schedule(&command3); + EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3})); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3})); + command1.SetFinished(true); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3})); + EXPECT_FALSE(scheduler.IsScheduled(&command1)); + command2.SetFinished(true); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2})); + command3.SetFinished(true); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3})); +} + +TEST_F(CommandScheduleTest, SchedulerCancelTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()); + EXPECT_CALL(command, End(false)).Times(0); + EXPECT_CALL(command, End(true)); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Cancel(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, NotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp new file mode 100644 index 0000000000..0429c625d4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" + +using namespace frc2; + +CommandTestBase::CommandTestBase() { + auto& scheduler = CommandScheduler::GetInstance(); + scheduler.CancelAll(); + scheduler.Enable(); + scheduler.ClearButtons(); +} + +CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); } + +void CommandTestBase::SetUp() { + HALSIM_SetDriverStationEnabled(true); + while (!HALSIM_GetDriverStationEnabled()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } +} + +void CommandTestBase::TearDown() { + CommandScheduler::GetInstance().ClearButtons(); +} + +void CommandTestBase::SetDSEnabled(bool enabled) { + HALSIM_SetDriverStationEnabled(enabled); + while (HALSIM_GetDriverStationEnabled() != static_cast(enabled)) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h new file mode 100644 index 0000000000..8b22844dca --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +#include + +#include "ErrorConfirmer.h" +#include "frc2/command/CommandGroupBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/SetUtilities.h" +#include "frc2/command/SubsystemBase.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "make_vector.h" +#include "simulation/DriverStationSim.h" + +namespace frc2 { +class CommandTestBase : public ::testing::Test { + public: + CommandTestBase(); + + class TestSubsystem : public SubsystemBase {}; + + protected: + class MockCommand : public Command { + public: + MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet()); + MOCK_METHOD0(IsFinished, bool()); + MOCK_CONST_METHOD0(RunsWhenDisabled, bool()); + MOCK_METHOD0(Initialize, void()); + MOCK_METHOD0(Execute, void()); + MOCK_METHOD1(End, void(bool interrupted)); + + MockCommand() { + m_requirements = {}; + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false)); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(true)); + } + + MockCommand(std::initializer_list requirements, + bool finished = false, bool runWhenDisabled = true) { + m_requirements.insert(requirements.begin(), requirements.end()); + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(finished)); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(runWhenDisabled)); + } + + MockCommand(MockCommand&& other) { + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(other.IsFinished())); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(other.RunsWhenDisabled())); + std::swap(m_requirements, other.m_requirements); + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + } + + MockCommand(const MockCommand& other) : Command{} {} + + void SetFinished(bool finished) { + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(finished)); + } + + ~MockCommand() { + auto& scheduler = CommandScheduler::GetInstance(); + scheduler.Cancel(this); + } + + protected: + std::unique_ptr TransferOwnership() && { + return std::make_unique(std::move(*this)); + } + + private: + wpi::SmallSet m_requirements; + }; + + CommandScheduler GetScheduler(); + + void SetUp() override; + + void TearDown() override; + + void SetDSEnabled(bool enabled); +}; +} // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp new file mode 100644 index 0000000000..927721ab39 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SelectCommand.h" + +using namespace frc2; +class ConditionalCommandTest : public CommandTestBase {}; + +TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr mock = std::make_unique(); + MockCommand* mockptr = mock.get(); + + EXPECT_CALL(*mock, Initialize()); + EXPECT_CALL(*mock, Execute()).Times(2); + EXPECT_CALL(*mock, End(false)); + + ConditionalCommand conditional( + std::move(mock), std::make_unique(), [] { return true; }); + + scheduler.Schedule(&conditional); + scheduler.Run(); + mockptr->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&conditional)); +} + +TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ConditionalCommand conditional(std::move(command1), std::move(command2), + [] { return true; }); + scheduler.Schedule(&conditional); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&conditional)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp new file mode 100644 index 0000000000..b97cbb69f7 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class DefaultCommandTest : public CommandTestBase {}; + +TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + RunCommand command1([] {}, {&subsystem}); + + scheduler.SetDefaultCommand(&subsystem, std::move(command1)); + auto handle = scheduler.GetDefaultCommand(&subsystem); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(handle)); +} + +TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + RunCommand command1([] {}, {&subsystem}); + RunCommand command2([] {}, {&subsystem}); + + scheduler.SetDefaultCommand(&subsystem, std::move(command1)); + auto handle = scheduler.GetDefaultCommand(&subsystem); + scheduler.Run(); + scheduler.Schedule(&command2); + + EXPECT_TRUE(scheduler.IsScheduled(&command2)); + EXPECT_FALSE(scheduler.IsScheduled(handle)); + + scheduler.Cancel(&command2); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(handle)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp new file mode 100644 index 0000000000..2565a94271 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "ErrorConfirmer.h" + +ErrorConfirmer* ErrorConfirmer::instance; + +int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode, + HAL_Bool isLVCode, const char* details, + const char* location, const char* callStack, + HAL_Bool printMsg) { + if (std::regex_search(details, std::regex(instance->m_msg))) { + instance->ConfirmError(); + } + return 1; +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h new file mode 100644 index 0000000000..011158ca45 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "gmock/gmock.h" +#include "simulation/DriverStationSim.h" + +class ErrorConfirmer { + public: + explicit ErrorConfirmer(const char* msg) : m_msg(msg) { + if (instance != nullptr) return; + HALSIM_SetSendError(HandleError); + EXPECT_CALL(*this, ConfirmError()); + instance = this; + } + + ~ErrorConfirmer() { + HALSIM_SetSendError(nullptr); + instance = nullptr; + } + + MOCK_METHOD0(ConfirmError, void()); + + const char* m_msg; + + static int32_t HandleError(HAL_Bool isError, int32_t errorCode, + HAL_Bool isLVCode, const char* details, + const char* location, const char* callStack, + HAL_Bool printMsg); + + private: + static ErrorConfirmer* instance; +}; diff --git a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp new file mode 100644 index 0000000000..140d4fbe1c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" + +using namespace frc2; +class FunctionalCommandTest : public CommandTestBase {}; + +TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + bool finished = false; + + FunctionalCommand command( + [&counter] { counter++; }, [&counter] { counter++; }, + [&counter](bool) { counter++; }, [&finished] { return finished; }); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_EQ(4, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp new file mode 100644 index 0000000000..e91f6779e7 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" + +using namespace frc2; +class InstantCommandTest : public CommandTestBase {}; + +TEST_F(InstantCommandTest, InstantCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + InstantCommand command([&counter] { counter++; }, {}); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_EQ(counter, 1); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp new file mode 100644 index 0000000000..9f2aefc7d1 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp @@ -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 "CommandTestBase.h" +#include "frc2/command/NotifierCommand.h" + +using namespace frc2; +class NotifierCommandTest : public CommandTestBase {}; + +#ifdef __APPLE__ +TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) { +#else +TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) { +#endif + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + NotifierCommand command([&counter] { counter++; }, 0.01, {}); + + scheduler.Schedule(&command); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + scheduler.Cancel(&command); + + EXPECT_NEAR(.01 * counter, .25, .025); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp new file mode 100644 index 0000000000..55c7b98514 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelCommandGroupTest : public CommandTestBase {}; + +TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + + ParallelCommandGroup group(tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + + ParallelCommandGroup group(tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)).Times(0); + EXPECT_CALL(*command2, End(true)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + scheduler.Run(); + scheduler.Cancel(&group); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelCommandGroup group((InstantCommand(), InstantCommand())); + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelCommandGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelCommandGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp new file mode 100644 index 0000000000..6e3246f8cb --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp @@ -0,0 +1,136 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelDeadlineGroupTest : public CommandTestBase {}; + +TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelDeadlineGroup group( + std::move(command1Holder), + tcb::make_vector>(std::move(command2Holder), + std::move(command3Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(2); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(2); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + command2->SetFinished(true); + scheduler.Run(); + command1->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelDeadlineGroup group( + std::move(command1Holder), + tcb::make_vector>(std::move(command2Holder), + std::move(command3Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelDeadlineGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelDeadlineGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelDeadlineGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp new file mode 100644 index 0000000000..dad02a128c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelRaceGroupTest : public CommandTestBase {}; + +TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelRaceGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(2); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(2); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelRaceGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelRaceGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelRaceGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelRaceGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp new file mode 100644 index 0000000000..b3ef86143e --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/PerpetualCommand.h" + +using namespace frc2; +class PerpetualCommandTest : public CommandTestBase {}; + +TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + bool check = false; + + PerpetualCommand command{InstantCommand([&check] { check = true; }, {})}; + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(check); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp new file mode 100644 index 0000000000..b940566d7e --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp @@ -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 + +#include "CommandTestBase.h" +#include "frc2/command/PrintCommand.h" + +using namespace frc2; +class PrintCommandTest : public CommandTestBase {}; + +TEST_F(PrintCommandTest, PrintCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + PrintCommand command("Test!"); + + testing::internal::CaptureStdout(); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(), + std::regex("Test!"))); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp new file mode 100644 index 0000000000..09a569faba --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ProxyScheduleCommandTest : public CommandTestBase {}; + +TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool scheduled = false; + + InstantCommand toSchedule([&scheduled] { scheduled = true; }, {}); + + ProxyScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduled); +} + +TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool finished = false; + + WaitUntilCommand toSchedule([&finished] { return finished; }); + + ProxyScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp new file mode 100644 index 0000000000..bac40d53d3 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp @@ -0,0 +1,156 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SelectCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class RobotDisabledCommandTest : public CommandTestBase {}; + +TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + MockCommand command({}, false, false); + + EXPECT_CALL(command, End(true)); + + SetDSEnabled(true); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + SetDSEnabled(false); + + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + MockCommand command1; + MockCommand command2; + + scheduler.Schedule(&command1); + + SetDSEnabled(false); + + scheduler.Run(); + + scheduler.Schedule(&command2); + + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); +} + +TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()}; + SequentialCommandGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelCommandGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelRaceGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(), + [] { return true; }}; + ConditionalCommand dontRunWhenDisabled{ + MockCommand(), MockCommand({}, false, false), [] { return true; }}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + SelectCommand runWhenDisabled{[] { return 1; }, + std::pair(1, MockCommand()), + std::pair(1, MockCommand())}; + SelectCommand dontRunWhenDisabled{ + [] { return 1; }, std::pair(1, MockCommand()), + std::pair(1, MockCommand({}, false, false))}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp new file mode 100644 index 0000000000..07eecb3f19 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class RunCommandTest : public CommandTestBase {}; + +TEST_F(RunCommandTest, RunCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + RunCommand command([&counter] { counter++; }, {}); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Run(); + scheduler.Run(); + + EXPECT_EQ(3, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp new file mode 100644 index 0000000000..249561b24c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ScheduleCommand.h" + +using namespace frc2; +class ScheduleCommandTest : public CommandTestBase {}; + +TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool scheduled = false; + + InstantCommand toSchedule([&scheduled] { scheduled = true; }, {}); + + ScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduled); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp new file mode 100644 index 0000000000..f0198c9187 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class SchedulerTest : public CommandTestBase {}; + +TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { + CommandScheduler scheduler = GetScheduler(); + + InstantCommand command; + + int counter = 0; + + scheduler.OnCommandInitialize([&counter](const Command&) { counter++; }); + scheduler.OnCommandExecute([&counter](const Command&) { counter++; }); + scheduler.OnCommandFinish([&counter](const Command&) { counter++; }); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_EQ(counter, 3); +} + +TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + RunCommand command([] {}, {}); + + int counter = 0; + + scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; }); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Cancel(&command); + + EXPECT_EQ(counter, 1); +} + +TEST_F(SchedulerTest, UnregisterSubsystemTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem system; + + scheduler.RegisterSubsystem(&system); + + EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp new file mode 100644 index 0000000000..6be14efcd5 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SelectCommand.h" + +using namespace frc2; +class SelectCommandTest : public CommandTestBase {}; + +TEST_F(SelectCommandTest, SelectCommandTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr mock = std::make_unique(); + MockCommand* mockptr = mock.get(); + + EXPECT_CALL(*mock, Initialize()); + EXPECT_CALL(*mock, Execute()).Times(2); + EXPECT_CALL(*mock, End(false)); + + std::vector>> temp; + + temp.emplace_back(std::pair(1, std::move(mock))); + temp.emplace_back(std::pair(2, std::make_unique())); + temp.emplace_back(std::pair(3, std::make_unique())); + + SelectCommand select([] { return 1; }, std::move(temp)); + + scheduler.Schedule(&select); + scheduler.Run(); + mockptr->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&select)); +} + +TEST_F(SelectCommandTest, SelectCommandRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + SelectCommand select([] { return 1; }, std::pair(1, std::move(command1)), + std::pair(2, std::move(command2))); + + scheduler.Schedule(&select); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&select)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp new file mode 100644 index 0000000000..3a6f8d8ba4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp @@ -0,0 +1,137 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class SequentialCommandGroupTest : public CommandTestBase {}; + +TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + SequentialCommandGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(false)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + command3->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + SequentialCommandGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(0); + EXPECT_CALL(*command2, End(false)).Times(0); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()).Times(0); + EXPECT_CALL(*command3, Execute()).Times(0); + EXPECT_CALL(*command3, End(false)).Times(0); + EXPECT_CALL(*command3, End(true)).Times(0); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + SequentialCommandGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + SequentialCommandGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + SequentialCommandGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp new file mode 100644 index 0000000000..3f2579259b --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/StartEndCommand.h" + +using namespace frc2; +class StartEndCommandTest : public CommandTestBase {}; + +TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; }, + {}); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Run(); + scheduler.Cancel(&command); + + EXPECT_EQ(2, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp new file mode 100644 index 0000000000..40aa959723 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class WaitCommandTest : public CommandTestBase {}; + +TEST_F(WaitCommandTest, WaitCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + WaitCommand command(.1); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + std::this_thread::sleep_for(std::chrono::milliseconds(110)); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp new file mode 100644 index 0000000000..e9728db102 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "CommandTestBase.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class WaitUntilCommandTest : public CommandTestBase {}; + +TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h new file mode 100644 index 0000000000..a15f5a8e5d --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +namespace tcb { + +namespace detail { + +template +struct vec_type_helper { + using type = T; +}; + +template +struct vec_type_helper { + using type = typename std::common_type::type; +}; + +template +using vec_type_helper_t = typename vec_type_helper::type; + +template +struct all_constructible_and_convertible : std::true_type {}; + +template +struct all_constructible_and_convertible + : std::conditional::value && + std::is_convertible::value, + all_constructible_and_convertible, + std::false_type>::type {}; + +template ::value, + int>::type = 0> +std::vector make_vector_impl(Args&&... args) { + std::vector vec; + vec.reserve(sizeof...(Args)); + using arr_t = int[]; + (void)arr_t{0, (vec.emplace_back(std::forward(args)), 0)...}; + return vec; +} + +template ::value, + int>::type = 0> +std::vector make_vector_impl(Args&&... args) { + return std::vector{std::forward(args)...}; +} + +} // namespace detail + +template , + typename std::enable_if< + detail::all_constructible_and_convertible::value, + int>::type = 0> +std::vector make_vector(Args&&... args) { + return detail::make_vector_impl(std::forward(args)...); +} + +} // namespace tcb diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp index f6c3f99a07..d025f06088 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -32,7 +32,7 @@ class Robot : public frc::TimedRobot { private: frc::Joystick m_joystick{1}; frc::Encoder m_encoder{1, 2}; - frc::Spark m_motor{1}; + frc::PWMVictorSPX m_motor{1}; // Create a PID controller whose setpoint's change is subject to maximum // velocity and acceleration constraints. diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index 5dbab38455..9a15f316ce 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ class Robot : public frc::TimedRobot { private: frc::Joystick m_joystick{1}; frc::Encoder m_encoder{1, 2}; - frc::Spark m_motor{1}; + frc::PWMVictorSPX m_motor{1}; frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt}; frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..79f172ec59 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +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 + + // Spin up the shooter when the 'A' button is pressed + frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter); + + // Turn off the shooter when the 'B' button is pressed + frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter); + + // Shoot when the 'X' button is held + frc2::JoystickButton(&m_driverController, 3) + .WhenPressed(&m_shoot) + .WhenReleased(&m_stopFeeder); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp @@ -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.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp new file mode 100644 index 0000000000..f539ce35f2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/ShooterSubsystem.h" + +#include + +#include "Constants.h" + +using namespace ShooterConstants; + +ShooterSubsystem::ShooterSubsystem() + : PIDSubsystem(frc2::PIDController(kP, kI, kD)), + m_shooterMotor(kShooterMotorPort), + m_feederMotor(kFeederMotorPort), + m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) { + m_controller.SetAbsoluteTolerance(kShooterToleranceRPS); + m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void ShooterSubsystem::UseOutput(double output) { + // Use a feedforward of the form kS + kV * velocity + m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS); +} + +void ShooterSubsystem::Disable() { + // Turn off motor when we disable, since useOutput(0) doesn't stop the motor + // due to our feedforward + frc2::PIDSubsystem::Disable(); + m_shooterMotor.Set(0); +} + +bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); } + +double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); } + +double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; } + +void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); } + +void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h new file mode 100644 index 0000000000..13ef6db1c3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +/** + * 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 { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace ShooterConstants { +const int kEncoderPorts[]{4, 5}; +const bool kEncoderReversed = false; +const int kEncoderCPR = 1024; +const double kEncoderDistancePerPulse = + // Distance units will be rotations + 1. / static_cast(kEncoderCPR); + +const int kShooterMotorPort = 4; +const int kFeederMotorPort = 5; + +const double kShooterFreeRPS = 5300; +const double kShooterTargetRPS = 4000; +const double kShooterToleranceRPS = 50; + +const double kP = 1; +const double kI = 0; +const double kD = 0; + +// On a real robot the feedforward constants should be empirically determined; +// these are reasonable guesses. +const double kSFractional = .05; +const double kVFractional = + // Should have value 1 at free speed... + 1. / kShooterFreeRPS; + +const double kFeederSpeed = .5; +} // namespace ShooterConstants + +namespace AutoConstants { +const double kAutoTimeoutSeconds = 12; +const double kAutoShootTimeSeconds = 7; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h new file mode 100644 index 0000000000..0eef0a9c29 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/ShooterSubsystem.h" + +namespace ac = AutoConstants; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + 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; + ShooterSubsystem m_shooter; + + // A simple autonomous routine that shoots the loaded frisbees + frc2::SequentialCommandGroup m_autonomousCommand = + frc2::SequentialCommandGroup{ + // Start the command by spinning up the shooter... + frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}), + // Wait until the shooter is at speed before feeding the frisbees + frc2::WaitUntilCommand([this] { return m_shooter.AtSetpoint(); }), + // Start running the feeder + frc2::InstantCommand([this] { m_shooter.RunFeeder(); }, {&m_shooter}), + // Shoot for the specified time + frc2::WaitCommand(ac::kAutoShootTimeSeconds)} + // Add a timeout (will end the command if, for instance, the shooter + // never gets up to + // speed) + .WithTimeout(ac::kAutoTimeoutSeconds) + // When the command ends, turn off the shooter and the feeder + .WhenFinished([this] { + m_shooter.Disable(); + m_shooter.StopFeeder(); + }); + + // Assorted commands to be bound to buttons + + frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); }, + {&m_shooter}}; + + frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); }, + {&m_shooter}}; + + // Shoots if the shooter wheen has reached the target speed + frc2::ConditionalCommand m_shoot{ + // Run the feeder + frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}}, + // Do nothing + frc2::InstantCommand(), + // Determine which of the above to do based on whether the shooter has + // reached the + // desired speed + [this] { return m_shooter.AtSetpoint(); }}; + + frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); }, + {&m_shooter}}; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include +#include +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h new file mode 100644 index 0000000000..664c4e097d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include + +class ShooterSubsystem : public frc2::PIDSubsystem { + public: + ShooterSubsystem(); + + void UseOutput(double output) override; + + double GetSetpoint() override; + + double GetMeasurement() override; + + void Disable() override; + + bool AtSetpoint(); + + void RunFeeder(); + + void StopFeeder(); + + private: + frc::PWMVictorSPX m_shooterMotor; + frc::PWMVictorSPX m_feederMotor; + frc::Encoder m_shooterEncoder; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c4bbe641cf --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "commands/CloseClaw.h" +#include "commands/OpenClaw.h" +#include "commands/Pickup.h" +#include "commands/Place.h" +#include "commands/PrepareToPickup.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/TankDrive.h" + +RobotContainer::RobotContainer() + : m_autonomousCommand(&m_claw, &m_wrist, &m_elevator, &m_drivetrain) { + frc::SmartDashboard::PutData(&m_drivetrain); + frc::SmartDashboard::PutData(&m_elevator); + frc::SmartDashboard::PutData(&m_wrist); + frc::SmartDashboard::PutData(&m_claw); + + m_claw.Log(); + m_wrist.Log(); + m_elevator.Log(); + m_drivetrain.Log(); + + m_drivetrain.SetDefaultCommand(TankDrive( + &m_drivetrain, + [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); }, + [this] { + return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); + })); + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + frc2::JoystickButton m_dUp{&m_joy, 5}; + frc2::JoystickButton m_dRight{&m_joy, 6}; + frc2::JoystickButton m_dDown{&m_joy, 7}; + frc2::JoystickButton m_dLeft{&m_joy, 8}; + frc2::JoystickButton m_l2{&m_joy, 9}; + frc2::JoystickButton m_r2{&m_joy, 10}; + frc2::JoystickButton m_l1{&m_joy, 11}; + frc2::JoystickButton m_r1{&m_joy, 12}; + + m_dUp.WhenPressed(SetElevatorSetpoint(0.2, &m_elevator)); + m_dDown.WhenPressed(SetElevatorSetpoint(-0.2, &m_elevator)); + m_dRight.WhenPressed(CloseClaw(&m_claw)); + m_dLeft.WhenPressed(OpenClaw(&m_claw)); + + m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator)); + m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator)); + m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator)); + m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain)); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // An example command will be run in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp new file mode 100644 index 0000000000..5db0cdb5f5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/Autonomous.h" + +#include + +#include "commands/CloseClaw.h" +#include "commands/DriveStraight.h" +#include "commands/Pickup.h" +#include "commands/Place.h" +#include "commands/PrepareToPickup.h" +#include "commands/SetDistanceToBox.h" +#include "commands/SetWristSetpoint.h" + +Autonomous::Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator, + DriveTrain* drivetrain) { + SetName("Autonomous"); + AddCommands( + // clang-format off + PrepareToPickup(claw, wrist, elevator), + Pickup(claw, wrist, elevator), + SetDistanceToBox(0.10, drivetrain), + // DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken + Place(claw, wrist, elevator), + SetDistanceToBox(0.6, drivetrain), + // DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken + frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), + CloseClaw(claw))); + // clang-format on +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp new file mode 100644 index 0000000000..97a9ccf0ca --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/CloseClaw.h" + +#include "Robot.h" + +CloseClaw::CloseClaw(Claw* claw) : m_claw(claw) { + SetName("CloseClaw"); + AddRequirements({m_claw}); +} + +// Called just before this Command runs the first time +void CloseClaw::Initialize() { m_claw->Close(); } + +// Make this return true when this Command no longer needs to run execute() +bool CloseClaw::IsFinished() { return m_claw->IsGripping(); } + +// Called once after isFinished returns true +void CloseClaw::End(bool) { +// NOTE: Doesn't stop in simulation due to lower friction causing the can to +// fall out +// + there is no need to worry about stalling the motor or crushing the can. +#ifndef SIMULATION + m_claw->Stop(); +#endif +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp new file mode 100644 index 0000000000..70a6bd71b1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/DriveStraight.h" + +#include + +#include "Robot.h" + +DriveStraight::DriveStraight(double distance, DriveTrain* drivetrain) + : frc2::CommandHelper( + frc2::PIDController(4, 0, 0), + [this]() { return m_drivetrain->GetDistance(); }, distance, + [this](double output) { m_drivetrain->Drive(output, output); }, + {drivetrain}), + m_drivetrain(drivetrain) { + m_controller.SetAbsoluteTolerance(0.01); +} + +// Called just before this Command runs the first time +void DriveStraight::Initialize() { + // Get everything in a safe starting state. + m_drivetrain->Reset(); + frc2::PIDCommand::Initialize(); +} + +bool DriveStraight::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp new file mode 100644 index 0000000000..6ea6e2cad5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/OpenClaw.h" + +#include "Robot.h" + +OpenClaw::OpenClaw(Claw* claw) + : frc2::CommandHelper(1), m_claw(claw) { + SetName("OpenClaw"); + AddRequirements({m_claw}); +} + +// Called just before this Command runs the first time +void OpenClaw::Initialize() { + m_claw->Open(); + frc2::WaitCommand::Initialize(); +} + +// Called once after isFinished returns true +void OpenClaw::End(bool) { m_claw->Stop(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp new file mode 100644 index 0000000000..996414f237 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/Pickup.h" + +#include + +#include "commands/CloseClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +Pickup::Pickup(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("Pickup"); + AddCommands(CloseClaw(claw), + frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), + SetElevatorSetpoint(0.25, elevator))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp new file mode 100644 index 0000000000..764ab0e452 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/Place.h" + +#include "commands/OpenClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +Place::Place(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("Place"); + // clang-format off + AddCommands(SetElevatorSetpoint(0.25, elevator), + SetWristSetpoint(0, wrist), + OpenClaw(claw)); + // clang-format on +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp new file mode 100644 index 0000000000..f1399d0e86 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/PrepareToPickup.h" + +#include + +#include "commands/OpenClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +PrepareToPickup::PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("PrepareToPickup"); + AddCommands(OpenClaw(claw), + frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator), + SetWristSetpoint(0, wrist))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp new file mode 100644 index 0000000000..cbe711e069 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/SetDistanceToBox.h" + +#include + +#include "Robot.h" + +SetDistanceToBox::SetDistanceToBox(double distance, DriveTrain* drivetrain) + : frc2::CommandHelper( + frc2::PIDController(-2, 0, 0), + [this]() { return m_drivetrain->GetDistanceToObstacle(); }, distance, + [this](double output) { m_drivetrain->Drive(output, output); }, + {drivetrain}), + m_drivetrain(drivetrain) { + m_controller.SetAbsoluteTolerance(0.01); +} + +// Called just before this Command runs the first time +void SetDistanceToBox::Initialize() { + // Get everything in a safe starting state. + m_drivetrain->Reset(); + frc2::PIDCommand::Initialize(); +} + +bool SetDistanceToBox::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp new file mode 100644 index 0000000000..161c1ee0f1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "commands/SetElevatorSetpoint.h" + +#include + +#include "Robot.h" + +SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator* elevator) + : m_setpoint(setpoint), m_elevator(elevator) { + SetName("SetElevatorSetpoint"); + AddRequirements({m_elevator}); +} + +// Called just before this Command runs the first time +void SetElevatorSetpoint::Initialize() { + m_elevator->SetSetpoint(m_setpoint); + m_elevator->Enable(); +} + +// Make this return true when this Command no longer needs to run execute() +bool SetElevatorSetpoint::IsFinished() { + return m_elevator->GetController().AtSetpoint(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp new file mode 100644 index 0000000000..50c9d0a05a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/SetWristSetpoint.h" + +#include "Robot.h" + +SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist* wrist) + : m_setpoint(setpoint), m_wrist(wrist) { + SetName("SetWristSetpoint"); + AddRequirements({m_wrist}); +} + +// Called just before this Command runs the first time +void SetWristSetpoint::Initialize() { + m_wrist->SetSetpoint(m_setpoint); + m_wrist->Enable(); +} + +// Make this return true when this Command no longer needs to run execute() +bool SetWristSetpoint::IsFinished() { + return m_wrist->GetController().AtSetpoint(); +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp similarity index 52% rename from wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp rename to wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp index a8e94063dc..fed1cdb714 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp @@ -1,31 +1,26 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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 "commands/MyAutoCommand.h" +#include "commands/TankDrive.h" #include "Robot.h" -MyAutoCommand::MyAutoCommand() { - // Use Requires() here to declare subsystem dependencies - Requires(&Robot::m_subsystem); +TankDrive::TankDrive(DriveTrain* drivetrain, std::function left, + std::function right) + : m_drivetrain(drivetrain), m_left(left), m_right(right) { + SetName("TankDrive"); + AddRequirements({m_drivetrain}); } -// Called just before this Command runs the first time -void MyAutoCommand::Initialize() {} - // Called repeatedly when this Command is scheduled to run -void MyAutoCommand::Execute() {} +void TankDrive::Execute() { m_drivetrain->Drive(m_left(), m_right()); } // Make this return true when this Command no longer needs to run execute() -bool MyAutoCommand::IsFinished() { return false; } +bool TankDrive::IsFinished() { return false; } // Called once after isFinished returns true -void MyAutoCommand::End() {} - -// Called when another command which requires one or more of the same -// subsystems is scheduled to run -void MyAutoCommand::Interrupted() {} +void TankDrive::End(bool) { m_drivetrain->Drive(0, 0); } diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp similarity index 59% rename from wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp rename to wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp index 48afffbc8a..7f2a60f8ac 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp @@ -5,8 +5,20 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "OI.h" +#include "subsystems/Claw.h" -OI::OI() { - // Process operator interface input here. +Claw::Claw() { + // Let's show everything on the LiveWindow + SetName("Claw"); + AddChild("Motor", &m_motor); } + +void Claw::Open() { m_motor.Set(-1); } + +void Claw::Close() { m_motor.Set(1); } + +void Claw::Stop() { m_motor.Set(0); } + +bool Claw::IsGripping() { return m_contact.Get(); } + +void Claw::Log() {} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp new file mode 100644 index 0000000000..8eec077dac --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "subsystems/DriveTrain.h" + +#include +#include + +DriveTrain::DriveTrain() { +// Encoders may measure differently in the real world and in +// simulation. In this example the robot moves 0.042 barleycorns +// per tick in the real world, but the simulated encoders +// simulate 360 tick encoders. This if statement allows for the +// real robot to handle this difference in devices. +#ifndef SIMULATION + m_leftEncoder.SetDistancePerPulse(0.042); + m_rightEncoder.SetDistancePerPulse(0.042); +#else + // Circumference in ft = 4in/12(in/ft)*PI + m_leftEncoder.SetDistancePerPulse(static_cast(4.0 / 12.0 * M_PI) / + 360.0); + m_rightEncoder.SetDistancePerPulse(static_cast(4.0 / 12.0 * M_PI) / + 360.0); +#endif + SetName("DriveTrain"); + // Let's show everything on the LiveWindow + AddChild("Front_Left Motor", &m_frontLeft); + AddChild("Rear Left Motor", &m_rearLeft); + AddChild("Front Right Motor", &m_frontRight); + AddChild("Rear Right Motor", &m_rearRight); + AddChild("Left Encoder", &m_leftEncoder); + AddChild("Right Encoder", &m_rightEncoder); + AddChild("Rangefinder", &m_rangefinder); + AddChild("Gyro", &m_gyro); +} + +void DriveTrain::Log() { + frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance()); + frc::SmartDashboard::PutNumber("Right Distance", + m_rightEncoder.GetDistance()); + frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate()); + frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate()); + frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle()); +} + +void DriveTrain::Drive(double left, double right) { + m_robotDrive.TankDrive(left, right); +} + +double DriveTrain::GetHeading() { return m_gyro.GetAngle(); } + +void DriveTrain::Reset() { + m_gyro.Reset(); + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveTrain::GetDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0; +} + +double DriveTrain::GetDistanceToObstacle() { + // Really meters in simulation since it's a rangefinder... + return m_rangefinder.GetAverageVoltage(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp new file mode 100644 index 0000000000..cfa62f8953 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "subsystems/Elevator.h" + +#include +#include +#include + +Elevator::Elevator() + : frc2::PIDSubsystem(frc2::PIDController(kP_real, kI_real, 0)) { +#ifdef SIMULATION // Check for simulation and update PID values + GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0); +#endif + m_controller.SetAbsoluteTolerance(0.005); + + SetName("Elevator"); + // Let's show everything on the LiveWindow + AddChild("Motor", &m_motor); + AddChild("Pot", &m_pot); +} + +void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); } + +double Elevator::GetMeasurement() { return m_pot.Get(); } + +void Elevator::UseOutput(double d) { m_motor.Set(d); } + +double Elevator::GetSetpoint() { return m_setpoint; } + +void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp new file mode 100644 index 0000000000..953ca9564a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "subsystems/Wrist.h" + +#include +#include + +Wrist::Wrist() : frc2::PIDSubsystem(frc2::PIDController(kP_real, 0, 0)) { +#ifdef SIMULATION // Check for simulation and update PID values + GetPIDController()->SetPID(kP_simulation, 0, 0, 0); +#endif + m_controller.SetAbsoluteTolerance(2.5); + + SetName("Wrist"); + // Let's show everything on the LiveWindow + AddChild("Motor", &m_motor); + AddChild("Pot", &m_pot); +} + +void Wrist::Log() { + // frc::SmartDashboard::PutData("Wrist Angle", &m_pot); +} + +double Wrist::GetSetpoint() { return m_setpoint; } + +void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; } + +double Wrist::GetMeasurement() { return m_pot.Get(); } + +void Wrist::UseOutput(double d) { m_motor.Set(d); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h new file mode 100644 index 0000000000..f201ae6544 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h @@ -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 + +#include + +#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(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h new file mode 100644 index 0000000000..c0d429c23c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h @@ -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 +#include + +#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 { + public: + Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator, + DriveTrain* drivetrain); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h new file mode 100644 index 0000000000..a5e6c0b0dc --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h @@ -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 +#include + +#include "subsystems/Claw.h" + +/** + * Opens the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +class CloseClaw : public frc2::CommandHelper { + public: + explicit CloseClaw(Claw* claw); + void Initialize() override; + bool IsFinished() override; + void End(bool interrupted) override; + + private: + Claw* m_claw; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h new file mode 100644 index 0000000000..1955169efd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h @@ -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 +#include + +#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 { + public: + explicit DriveStraight(double distance, DriveTrain* drivetrain); + void Initialize() override; + bool IsFinished() override; + + private: + DriveTrain* m_drivetrain; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h new file mode 100644 index 0000000000..486f86b715 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h @@ -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 +#include + +#include "subsystems/Claw.h" + +/** + * Opens the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +class OpenClaw : public frc2::CommandHelper { + public: + explicit OpenClaw(Claw* claw); + void Initialize() override; + void End(bool interrupted) override; + + private: + Claw* m_claw; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h new file mode 100644 index 0000000000..4d74588604 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h @@ -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 +#include + +#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 { + public: + Pickup(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h new file mode 100644 index 0000000000..85945c3035 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h @@ -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 +#include + +#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 { + public: + Place(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h new file mode 100644 index 0000000000..b2aa4d1c99 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h @@ -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 +#include + +#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 { + public: + PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h new file mode 100644 index 0000000000..b5b7b1338e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h @@ -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 +#include + +#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 { + public: + explicit SetDistanceToBox(double distance, DriveTrain* drivetrain); + void Initialize() override; + bool IsFinished() override; + + private: + DriveTrain* m_drivetrain; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h new file mode 100644 index 0000000000..c3bcf6f795 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h @@ -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 +#include + +#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 { + public: + explicit SetElevatorSetpoint(double setpoint, Elevator* elevator); + void Initialize() override; + bool IsFinished() override; + + private: + double m_setpoint; + Elevator* m_elevator; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h new file mode 100644 index 0000000000..96c01fabe0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h @@ -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 +#include + +#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 { + public: + explicit SetWristSetpoint(double setpoint, Wrist* wrist); + void Initialize() override; + bool IsFinished() override; + + private: + double m_setpoint; + Wrist* m_wrist; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h new file mode 100644 index 0000000000..94b3392afa --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h @@ -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 +#include + +#include "subsystems/DriveTrain.h" + +/** + * Have the robot drive tank style using the PS3 Joystick until interrupted. + */ +class TankDrive : public frc2::CommandHelper { + public: + TankDrive(DriveTrain* drivetrain, std::function left, + std::function right); + void Execute() override; + bool IsFinished() override; + void End(bool interrupted) override; + + private: + DriveTrain* m_drivetrain; + std::function m_left; + std::function m_right; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h new file mode 100644 index 0000000000..aa898fd2ea --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h @@ -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 + +#include +#include + +/** + * 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}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h new file mode 100644 index 0000000000..0556ecd522 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h @@ -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 + +#include +#include +#include +#include +#include +#include + +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}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h new file mode 100644 index 0000000000..bd63610910 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h @@ -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 + +#include +#include + +/** + * 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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h new file mode 100644 index 0000000000..7091f6779b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h @@ -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 + +#include +#include + +/** + * 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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c67148a310 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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 + + // Stabilize robot to drive straight with gyro when left bumper is held + frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving); + + // Turn to 90 degrees when the 'X' button is pressed + frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // no auto + return nullptr; +} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp new file mode 100644 index 0000000000..1e009f4497 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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/TurnToAngle.h" + +#include + +using namespace DriveConstants; + +TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive) + : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD), + // Close loop on heading + [drive] { return drive->GetHeading(); }, + // Set reference to target + targetAngleDegrees, + // Pipe output to turn robot + [drive](double output) { drive->ArcadeDrive(0, output); }, + // Require the drive + {drive}) { + // Set the controller to be continuous (because it is an angle controller) + m_controller.EnableContinuousInput(-180, 180); + // Set the controller tolerance - the delta tolerance ensures the robot is + // stationary at the setpoint before it is considered as having reached the + // reference + m_controller.SetAbsoluteTolerance(kTurnToleranceDeg, + kTurnRateToleranceDegPerS); + + AddRequirements({drive}); +} + +bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..a5b7a61442 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} + +double DriveSubsystem::GetHeading() { + return std::remainder(m_gyro.GetAngle(), 360); +} + +double DriveSubsystem::GetTurnRate() { + return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h new file mode 100644 index 0000000000..820e8ce8b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +/** + * 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 { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); + +const bool kGyroReversed = false; + +const double kStabilizationP = 1; +const double kStabilizationI = .5; +const double kStabilizationD = 0; + +const double kTurnP = 1; +const double kTurnI = 0; +const double kTurnD = 0; + +const double kTurnToleranceDeg = 5; +const double kTurnRateToleranceDegPerS = 10; // degrees per second +} // namespace DriveConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h new file mode 100644 index 0000000000..4e622ae5c4 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include +#include + +#include +#include +#include + +#include "Constants.h" +#include "commands/TurnToAngle.h" +#include "subsystems/DriveSubsystem.h" + +namespace ac = AutoConstants; +namespace dc = DriveConstants; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::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; + + // 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); + + // 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(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h new file mode 100644 index 0000000000..c7e875e87e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "subsystems/DriveSubsystem.h" + +/** + * A command that will turn the robot to the specified angle. + */ +class TurnToAngle : public frc2::CommandHelper { + public: + /** + * Turns to robot to the specified angle. + * + * @param targetAngleDegrees The angle to turn to + * @param drive The drive subsystem to use + */ + TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive); + + bool IsFinished() override; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..57ab4f559a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h @@ -0,0 +1,114 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + double GetHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::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; + + // The gyro sensor + frc::ADXRS450_Gyro m_gyro; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..f2a9cec655 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Add commands to the autonomous command chooser + m_chooser.AddOption("Simple Auto", &m_simpleAuto); + m_chooser.AddOption("Complex Auto", &m_complexAuto); + + // Put the chooser on the dashboard + frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser); + + // 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 + + // Grab the hatch when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_grabHatch); + // Release the hatch when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_releaseHatch); + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return m_chooser.GetSelected(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp new file mode 100644 index 0000000000..40bd60829b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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/ComplexAuto.h" + +#include +#include +#include + +using namespace AutoConstants; + +ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { + AddCommands( + // Drive forward the specified distance + frc2::StartEndCommand([drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); }, + [drive] { drive->ArcadeDrive(0, 0); }, {drive}) + .BeforeStarting([drive] { drive->ResetEncoders(); }) + .InterruptOn([drive] { + return drive->GetAverageEncoderDistance() >= + kAutoDriveDistanceInches; + }), + // Release the hatch + frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}), + // Drive backward the specified distance + frc2::StartEndCommand( + [drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); }, + [drive] { drive->ArcadeDrive(0, 0); }, {drive}) + .BeforeStarting([drive] { drive->ResetEncoders(); }) + .InterruptOn([drive] { + return drive->GetAverageEncoderDistance() <= + kAutoBackupDistanceInches; + })); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp @@ -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.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp new file mode 100644 index 0000000000..ea7b7961bd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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/HatchSubsystem.h" + +using namespace HatchConstants; + +HatchSubsystem::HatchSubsystem() + : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} + +void HatchSubsystem::GrabHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); +} + +void HatchSubsystem::ReleaseHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h new file mode 100644 index 0000000000..b09572ed0d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h @@ -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 + +/** + * 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 { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace HatchConstants { +const int kHatchSolenoidModule = 0; +const int kHatchSolenoidPorts[]{0, 1}; +} // namespace HatchConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h new file mode 100644 index 0000000000..35edc412f8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include +#include +#include + +#include +#include + +#include "Constants.h" +#include "commands/ComplexAuto.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.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; + HatchSubsystem m_hatch; + + // The autonomous routines + frc2::ParallelRaceGroup m_simpleAuto = + frc2::StartEndCommand( + [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); }, + [this] { m_drive.ArcadeDrive(0, 0); }, {&m_drive}) + .BeforeStarting([this] { m_drive.ResetEncoders(); }) + .InterruptOn([this] { + return m_drive.GetAverageEncoderDistance() >= + ac::kAutoDriveDistanceInches; + }); + ComplexAuto m_complexAuto{&m_drive, &m_hatch}; + + // Assorted commands to be bound to buttons + + frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}}; + frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); }, + {&m_hatch}}; + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h new file mode 100644 index 0000000000..b767f3bbc5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.h" + +/** + * A complex auto command that drives forward, releases a hatch, and then drives + * backward. + */ +class ComplexAuto + : public frc2::CommandHelper { + public: + /** + * Creates a new ComplexAuto. + * + * @param drive The drive subsystem this command will run on + * @param hatch The hatch subsystem this command will run on + */ + ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include +#include +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h new file mode 100644 index 0000000000..a41ae1b0e0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "Constants.h" + +class HatchSubsystem : public frc2::SubsystemBase { + public: + HatchSubsystem(); + + // Subsystem methods go here. + + /** + * Grabs the hatch. + */ + void GrabHatch(); + + /** + * Releases the hatch. + */ + void ReleaseHatch(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + frc::DoubleSolenoid m_hatchSolenoid; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..7c02618904 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "commands/DefaultDrive.h" +#include "commands/GrabHatch.h" +#include "commands/HalveDriveSpeed.h" +#include "commands/ReleaseHatch.h" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Add commands to the autonomous command chooser + m_chooser.AddOption("Simple Auto", &m_simpleAuto); + m_chooser.AddOption("Complex Auto", &m_complexAuto); + + // Put the chooser on the dashboard + frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser); + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(DefaultDrive( + &m_drive, + [this] { return m_driverController.GetY(frc::GenericHID::kLeftHand); }, + [this] { return m_driverController.GetX(frc::GenericHID::kRightHand); })); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // NOTE: Using `new` here will leak these commands if they are ever no longer + // needed. This is usually a non-issue as button-bindings tend to be permanent + // - however, if you wish to avoid this, the commands should be + // stack-allocated and declared as members of RobotContainer. + + // Grab the hatch when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed(new GrabHatch(&m_hatch)); + // Release the hatch when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 2) + .WhenPressed(new ReleaseHatch(&m_hatch)); + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenHeld(new HalveDriveSpeed(&m_drive)); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return m_chooser.GetSelected(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp new file mode 100644 index 0000000000..cb41de6744 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/ComplexAuto.h" + +using namespace AutoConstants; + +ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { + AddCommands( + // Drive forward the specified distance + DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive), + // Release the hatch + ReleaseHatch(hatch), + // Drive backward the specified distance + DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive)); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp new file mode 100644 index 0000000000..3bdee6ef4f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* 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/DefaultDrive.h" + +DefaultDrive::DefaultDrive(DriveSubsystem* subsystem, + std::function forward, + std::function rotation) + : m_drive{subsystem}, m_forward{forward}, m_rotation{rotation} { + AddRequirements({subsystem}); +} + +void DefaultDrive::Execute() { + m_drive->ArcadeDrive(m_forward(), m_rotation()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp new file mode 100644 index 0000000000..6c7ef407e5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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/DriveDistance.h" + +#include + +DriveDistance::DriveDistance(double inches, double speed, + DriveSubsystem* subsystem) + : m_drive(subsystem), m_distance(inches), m_speed(speed) { + AddRequirements({subsystem}); +} + +void DriveDistance::Initialize() { + m_drive->ResetEncoders(); + m_drive->ArcadeDrive(m_speed, 0); +} + +void DriveDistance::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); } + +bool DriveDistance::IsFinished() { + return std::abs(m_drive->GetAverageEncoderDistance()) >= m_distance; +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp similarity index 58% rename from wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h rename to wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp index 0b7713ee42..f665761269 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp @@ -1,13 +1,16 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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 "commands/GrabHatch.h" -class OI { - public: - OI(); -}; +GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) { + AddRequirements({subsystem}); +} + +void GrabHatch::Initialize() { m_hatch->GrabHatch(); } + +bool GrabHatch::IsFinished() { return true; } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp new file mode 100644 index 0000000000..839bb87f86 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp @@ -0,0 +1,15 @@ +/*----------------------------------------------------------------------------*/ +/* 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/HalveDriveSpeed.h" + +HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem) + : m_drive(subsystem) {} + +void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); } + +void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp new file mode 100644 index 0000000000..e8fbd61c8f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* 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/ReleaseHatch.h" + +ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) { + AddRequirements({subsystem}); +} + +void ReleaseHatch::Initialize() { m_hatch->ReleaseHatch(); } + +bool ReleaseHatch::IsFinished() { return true; } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp @@ -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.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp new file mode 100644 index 0000000000..ea7b7961bd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* 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/HatchSubsystem.h" + +using namespace HatchConstants; + +HatchSubsystem::HatchSubsystem() + : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} + +void HatchSubsystem::GrabHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); +} + +void HatchSubsystem::ReleaseHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h new file mode 100644 index 0000000000..b09572ed0d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h @@ -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 + +/** + * 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 { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace HatchConstants { +const int kHatchSolenoidModule = 0; +const int kHatchSolenoidPorts[]{0, 1}; +} // namespace HatchConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h new file mode 100644 index 0000000000..3e663f9bdb --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include + +#include "Constants.h" +#include "commands/ComplexAuto.h" +#include "commands/DefaultDrive.h" +#include "commands/DriveDistance.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.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... + + // The robot's subsystems + DriveSubsystem m_drive; + HatchSubsystem m_hatch; + + // The autonomous routines + DriveDistance m_simpleAuto{AutoConstants::kAutoDriveDistanceInches, + AutoConstants::kAutoDriveSpeed, &m_drive}; + ComplexAuto m_complexAuto{&m_drive, &m_hatch}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h new file mode 100644 index 0000000000..88a246009a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "Constants.h" +#include "commands/DriveDistance.h" +#include "commands/ReleaseHatch.h" + +/** + * A complex auto command that drives forward, releases a hatch, and then drives + * backward. + */ +class ComplexAuto + : public frc2::CommandHelper { + public: + /** + * Creates a new ComplexAuto. + * + * @param drive The drive subsystem this command will run on + * @param hatch The hatch subsystem this command will run on + */ + ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h new file mode 100644 index 0000000000..d42d133af0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "subsystems/DriveSubsystem.h" + +/** + * A command to drive the robot with joystick input passed in through lambdas. + * Written explicitly for pedagogical purposes - actual code should inline a + * command this simple with RunCommand. + * + * @see RunCommand + */ +class DefaultDrive + : public frc2::CommandHelper { + public: + /** + * Creates a new DefaultDrive. + * + * @param subsystem The drive subsystem this command wil run on. + * @param forward The control input for driving forwards/backwards + * @param rotation The control input for turning + */ + DefaultDrive(DriveSubsystem* subsystem, std::function forward, + std::function rotation); + + void Execute() override; + + private: + DriveSubsystem* m_drive; + std::function m_forward; + std::function m_rotation; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h new file mode 100644 index 0000000000..6f350a9049 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "subsystems/DriveSubsystem.h" + +class DriveDistance + : public frc2::CommandHelper { + public: + /** + * Creates a new DriveDistance. + * + * @param inches The number of inches the robot will drive + * @param speed The speed at which the robot will drive + * @param drive The drive subsystem on which this command will run + */ + DriveDistance(double inches, double speed, DriveSubsystem* subsystem); + + void Initialize() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + DriveSubsystem* m_drive; + double m_distance; + double m_speed; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h new file mode 100644 index 0000000000..0ab0c13dff --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "subsystems/HatchSubsystem.h" + +/** + * A simple command that grabs a hatch with the HatchSubsystem. Written + * explicitly for pedagogical purposes. Actual code should inline a command + * this simple with InstantCommand. + * + * @see InstantCommand + */ +class GrabHatch : public frc2::CommandHelper { + public: + explicit GrabHatch(HatchSubsystem* subsystem); + + void Initialize() override; + + bool IsFinished() override; + + private: + HatchSubsystem* m_hatch; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h new file mode 100644 index 0000000000..0b5d7c7980 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include "subsystems/DriveSubsystem.h" + +class HalveDriveSpeed + : public frc2::CommandHelper { + public: + explicit HalveDriveSpeed(DriveSubsystem* subsystem); + + void Initialize() override; + + void End(bool interrupted) override; + + private: + DriveSubsystem* m_drive; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h new file mode 100644 index 0000000000..b98866f5ef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h @@ -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 +#include + +#include "subsystems/HatchSubsystem.h" + +/** + * A simple command that releases a hatch with the HatchSubsystem. Written + * explicitly for pedagogical purposes. Actual code should inline a command + * this simple with InstantCommand. + * + * @see InstantCommand + */ +class ReleaseHatch + : public frc2::CommandHelper { + public: + explicit ReleaseHatch(HatchSubsystem* subsystem); + + void Initialize() override; + + bool IsFinished() override; + + private: + HatchSubsystem* m_hatch; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include +#include +#include +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h new file mode 100644 index 0000000000..a41ae1b0e0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +#include "Constants.h" + +class HatchSubsystem : public frc2::SubsystemBase { + public: + HatchSubsystem(); + + // Subsystem methods go here. + + /** + * Grabs the hatch. + */ + void GrabHatch(); + + /** + * Releases the hatch. + */ + void ReleaseHatch(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + frc::DoubleSolenoid m_hatchSolenoid; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..7d51dc78b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Set names of commands + m_instantCommand1.SetName("Instant Command 1"); + m_instantCommand2.SetName("Instant Command 2"); + m_waitCommand.SetName("Wait 5 Seconds Command"); + + // Set the scheduler to log Shuffleboard events for command initialize, + // interrupt, finish + frc2::CommandScheduler::GetInstance().OnCommandInitialize( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Initialized", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + frc2::CommandScheduler::GetInstance().OnCommandInterrupt( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Interrupted", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + frc2::CommandScheduler::GetInstance().OnCommandFinish( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Finished", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Run instant command 1 when the 'A' button is pressed + frc2::JoystickButton(&m_driverController, 0).WhenPressed(&m_instantCommand1); + // Run instant command 2 when the 'X' button is pressed + frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_instantCommand2); + // Run instant command 3 when the 'Y' button is held; release early to + // interrupt + frc2::JoystickButton(&m_driverController, 4).WhenHeld(&m_waitCommand); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // no auto + return nullptr; +} diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h new file mode 100644 index 0000000000..0a5f832d33 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean 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 OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h new file mode 100644 index 0000000000..fd93be14e3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "Constants.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... + + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // Three commands that do nothing; for demonstration purposes. + frc2::InstantCommand m_instantCommand1; + frc2::InstantCommand m_instantCommand2; + frc2::WaitCommand m_waitCommand{5}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +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. + * + *

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(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..b06845e823 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Run the select command in autonomous + return &m_exampleSelectCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h new file mode 100644 index 0000000000..0a5f832d33 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean 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 OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h @@ -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 + +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h new file mode 100644 index 0000000000..c93e3203be --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h @@ -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 +#include +#include + +/** + * 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 enum used as keys for selecting the command to run. + enum CommandSelector { ONE, TWO, THREE }; + + // An example selector method for the selectcommand. Returns the selector + // that will select which command to run. Can base this choice on logical + // conditions evaluated at runtime. + CommandSelector Select() { return ONE; } + + // The robot's subsystems and commands are defined here... + + // An example selectcommand. Will select from the three commands based on the + // value returned by the selector method at runtime. Note that selectcommand + // takes a generic type, so the selector does not have to be an enum; it could + // be any desired type (string, integer, boolean, double...) + frc2::SelectCommand m_exampleSelectCommand{ + [this] { return Select(); }, + // Maps selector values to commands + std::pair{ONE, frc2::PrintCommand{"Command one was selected!"}}, + std::pair{TWO, frc2::PrintCommand{"Command two was selected!"}}, + std::pair{THREE, frc2::PrintCommand{"Command three was selected!"}}}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index b3564c7ea0..588a9d07a1 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -240,6 +240,16 @@ "foldername": "GearsBot", "gradlebase": "cpp" }, + { + "name": "GearsBotNew", + "description": "A fully functional example CommandBased program for WPIs GearsBot robot, using the new command-based framework. This code can run on your computer if it supports simulation.", + "tags": [ + "CommandBased Robot", + "Complete List" + ], + "foldername": "GearsBotNew", + "gradlebase": "cpp" + }, { "name": "PacGoat", "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.", @@ -267,5 +277,66 @@ ], "foldername": "ShuffleBoard", "gradlebase": "cpp" + }, + { + "name": "'Traditional' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'traditional' style, i.e. commands are given their own classes.", + "tags": [ + "Complete robot", + "Command-based" + ], + "foldername": "HatchbotTraditional", + "gradlebase": "cpp" + }, + { + "name": "'Inlined' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.", + "tags": [ + "Complete robot", + "Command-based", + "Lambdas" + ], + "foldername": "HatchbotInlined", + "gradlebase": "cpp" + }, + { + "name": "Select Command Example", + "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.", + "tags": [ + "Command-based" + ], + "foldername": "SelectCommand", + "gradlebase": "cpp" + }, + { + "name": "Scheduler Event Logging", + "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite", + "tags": [ + "Command-based", + "Shuffleboard" + ], + "foldername": "SchedulerEventLogging", + "gradlebase": "cpp" + }, + { + "name": "Frisbeebot", + "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework", + "tags": [ + "Command-based", + "PID" + ], + "foldername": "Frisbeebot", + "mainclass": "Main" + }, + { + "name": "Gyro Drive Commands", + "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.", + "tags": [ + "Command-based", + "PID", + "Gyro" + ], + "foldername": "GyroDriveCommands", + "mainclass": "Main" } ] diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp index df2a9ccaaf..6c39023fef 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,27 +7,21 @@ #include "Robot.h" -#include +#include + #include -ExampleSubsystem Robot::m_subsystem; -OI Robot::m_oi; - -void Robot::RobotInit() { - m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto); - m_chooser.AddOption("My Auto", &m_myAuto); - frc::SmartDashboard::PutData("Auto Modes", &m_chooser); -} +void Robot::RobotInit() {} /** * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, + * this for items like diagnostics that you want to run during disabled, * autonomous, teleoperated and test. * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ -void Robot::RobotPeriodic() {} +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } /** * This function is called once each time the robot enters Disabled mode. You @@ -36,36 +30,21 @@ void Robot::RobotPeriodic() {} */ void Robot::DisabledInit() {} -void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); } +void Robot::DisabledPeriodic() {} /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable chooser - * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard, - * remove all of the chooser code and uncomment the GetString code to get the - * auto name from the text box below the Gyro. - * - * You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons to - * the if-else structure below with additional strings & commands. + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. */ void Robot::AutonomousInit() { - // std::string autoSelected = frc::SmartDashboard::GetString( - // "Auto Selector", "Default"); - // if (autoSelected == "My Auto") { - // m_autonomousCommand = &m_myAuto; - // } else { - // m_autonomousCommand = &m_defaultAuto; - // } - - m_autonomousCommand = m_chooser.GetSelected(); + m_autonomousCommand = m_container.GetAutonomousCommand(); if (m_autonomousCommand != nullptr) { - m_autonomousCommand->Start(); + m_autonomousCommand->Schedule(); } } -void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); } +void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { // This makes sure that the autonomous stops running when @@ -78,8 +57,14 @@ void Robot::TeleopInit() { } } -void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); } +/** + * 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 diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..8210645c8b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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" + +RobotContainer::RobotContainer() : m_autonomousCommand(&m_subsystem) { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // An example command will be run in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp index fa83682d74..0e709aa6c3 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,25 +7,5 @@ #include "commands/ExampleCommand.h" -#include "Robot.h" - -ExampleCommand::ExampleCommand() { - // Use Requires() here to declare subsystem dependencies - Requires(&Robot::m_subsystem); -} - -// Called just before this Command runs the first time -void ExampleCommand::Initialize() {} - -// Called repeatedly when this Command is scheduled to run -void ExampleCommand::Execute() {} - -// Make this return true when this Command no longer needs to run execute() -bool ExampleCommand::IsFinished() { return false; } - -// Called once after isFinished returns true -void ExampleCommand::End() {} - -// Called when another command which requires one or more of the same -// subsystems is scheduled to run -void ExampleCommand::Interrupted() {} +ExampleCommand::ExampleCommand(ExampleSubsystem* subsystem) + : m_subsystem{subsystem} {} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp index cdde203c22..2e720c9a7c 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,14 +7,10 @@ #include "subsystems/ExampleSubsystem.h" -#include "RobotMap.h" - -ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {} - -void ExampleSubsystem::InitDefaultCommand() { - // Set the default command for a subsystem here. - // SetDefaultCommand(new MySpecialCommand()); +ExampleSubsystem::ExampleSubsystem() { + // Implementation of subsystem constructor goes here. } -// Put methods for controlling this subsystem -// here. Call these from Commands. +void ExampleSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h new file mode 100644 index 0000000000..5ac2de033c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean 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. + */ diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h index 7dd5093369..872d72a57a 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,20 +7,14 @@ #pragma once -#include -#include -#include +#include -#include "OI.h" -#include "commands/ExampleCommand.h" -#include "commands/MyAutoCommand.h" -#include "subsystems/ExampleSubsystem.h" +#include + +#include "RobotContainer.h" class Robot : public frc::TimedRobot { public: - static ExampleSubsystem m_subsystem; - static OI m_oi; - void RobotInit() override; void RobotPeriodic() override; void DisabledInit() override; @@ -34,8 +28,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc::Command* m_autonomousCommand = nullptr; - ExampleCommand m_defaultAuto; - MyAutoCommand m_myAuto; - frc::SendableChooser m_chooser; + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h new file mode 100644 index 0000000000..46609acae7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include "commands/ExampleCommand.h" +#include "subsystems/ExampleSubsystem.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... + ExampleSubsystem m_subsystem; + ExampleCommand m_autonomousCommand; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h deleted file mode 100644 index dd78a21843..0000000000 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h +++ /dev/null @@ -1,25 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 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 - -/** - * The RobotMap is a mapping from the ports sensors and actuators are wired into - * to a variable name. This provides flexibility changing wiring, makes checking - * the wiring easier and significantly reduces the number of magic numbers - * floating around. - */ - -// For example to map the left and right motors, you could define the -// following variables to use with your drivetrain subsystem. -// constexpr int kLeftMotor = 1; -// constexpr int kRightMotor = 2; - -// If you are using multiple modules, make sure to define both the port -// number and the module. For example you with a rangefinder: -// constexpr int kRangeFinderPort = 1; -// constexpr int kRangeFinderModule = 1; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h index 1d11728dad..c36477f930 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,14 +7,28 @@ #pragma once -#include +#include +#include -class ExampleCommand : public frc::Command { +#include "subsystems/ExampleSubsystem.h" + +/** + * An example command that uses an example subsystem. + * + *

Note that this extends CommandHelper, rather extending CommandBase + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ExampleCommand + : public frc2::CommandHelper { public: - ExampleCommand(); - void Initialize() override; - void Execute() override; - bool IsFinished() override; - void End() override; - void Interrupted() override; + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + explicit ExampleCommand(ExampleSubsystem* subsystem); + + private: + ExampleSubsystem* m_subsystem; }; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h index 66bc329cf4..763eafdac0 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* 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. */ @@ -7,14 +7,20 @@ #pragma once -#include +#include -class ExampleSubsystem : public frc::Subsystem { +class ExampleSubsystem : public frc2::SubsystemBase { public: ExampleSubsystem(); - void InitDefaultCommand() override; + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. private: - // It's desirable that everything possible under private except - // for methods that implement subsystem capabilities + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. };