diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp new file mode 100644 index 0000000000..cd19aeb7d8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include +#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/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..aec6b1d7a7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#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 + + // Move the arm to 2 radians above horizontal when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm}); + + // Move the arm to neutral position when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); }, + {&m_arm}); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed([this] { m_drive.SetMaxOutput(.5); }) + .WhenReleased([this] { m_drive.SetMaxOutput(1); }); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return new frc2::InstantCommand([] {}); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp new file mode 100644 index 0000000000..2ac9da1bc3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/ArmSubsystem.h" + +#include "Constants.h" + +using namespace ArmConstants; +using State = frc::TrapezoidProfile::State; + +ArmSubsystem::ArmSubsystem() + : frc2::ProfiledPIDSubsystem( + frc::ProfiledPIDController( + kP, 0, 0, {kMaxVelocity, kMaxAcceleration})), + m_motor(kMotorPort), + m_encoder(kEncoderPorts[0], kEncoderPorts[1]), + m_feedforward(kS, kCos, kV, kA), + // Start arm at rest in neutral position + m_goal{kArmOffset, 0_rad_per_s} { + m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to()); +} + +void ArmSubsystem::UseOutput(double output, State setpoint) { + // Calculate the feedforward from the sepoint + units::volt_t feedforward = + m_feedforward.Calculate(setpoint.position, setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_motor.SetVoltage(units::volt_t(output) + feedforward); +} + +void ArmSubsystem::SetGoal(units::radian_t goal) { + m_goal = State{goal, 0_rad_per_s}; +} + +State ArmSubsystem::GetGoal() { return m_goal; } + +units::radian_t ArmSubsystem::GetMeasurement() { + return units::radian_t(m_encoder.GetDistance()) + kArmOffset; +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/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/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h new file mode 100644 index 0000000000..674633c56d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +constexpr int kLeftMotor1Port = 0; +constexpr int kLeftMotor2Port = 1; +constexpr int kRightMotor1Port = 2; +constexpr int kRightMotor2Port = 3; + +constexpr int kLeftEncoderPorts[]{0, 1}; +constexpr int kRightEncoderPorts[]{2, 3}; +constexpr bool kLeftEncoderReversed = false; +constexpr bool kRightEncoderReversed = true; + +constexpr int kEncoderCPR = 1024; +constexpr double kWheelDiameterInches = 6; +constexpr double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace ArmConstants { +constexpr int kMotorPort = 4; + +constexpr double kP = 1; + +// These are fake gains; in actuality these must be determined individually for +// each robot +constexpr auto kS = 1_V; +constexpr auto kCos = 1_V; +constexpr auto kV = 0.5_V * 1_s / 1_rad; +constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; + +constexpr auto kMaxVelocity = 3_rad_per_s; +constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); + +constexpr int kEncoderPorts[]{4, 5}; +constexpr int kEncoderPPR = 256; +constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR; + +// The offset of the arm from the horizontal in its neutral position, +// measured from the horizontal +constexpr auto kArmOffset = 0.5_rad; +} // namespace ArmConstants + +namespace AutoConstants { +constexpr auto kAutoTimeoutSeconds = 12_s; +constexpr auto kAutoShootTimeSeconds = 7_s; +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h new file mode 100644 index 0000000000..fa173d39e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#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/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h new file mode 100644 index 0000000000..fa07359c84 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/ArmSubsystem.h" +#include "subsystems/DriveSubsystem.h" + +namespace ac = AutoConstants; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +class RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + ArmSubsystem m_arm; + + // The chooser for the autonomous routines + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h new file mode 100644 index 0000000000..5c995cd390 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * A robot arm subsystem that moves with a motion profile. + */ +class ArmSubsystem : public frc2::ProfiledPIDSubsystem { + using State = frc::TrapezoidProfile::State; + + public: + ArmSubsystem(); + + void UseOutput(double output, State setpoint) override; + + void SetGoal(units::radian_t goal); + + State GetGoal() override; + + units::radian_t GetMeasurement() override; + + private: + frc::PWMVictorSPX m_motor; + frc::Encoder m_encoder; + frc::ArmFeedforward m_feedforward; + + State m_goal; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..3ed1357f8f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#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/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 60730dbdad..73ebab526d 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -496,5 +496,17 @@ "foldername": "SwerveControllerCommand", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "ArmBot", + "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.", + "tags": [ + "ArmBot", + "PID", + "Motion Profile" + ], + "foldername": "ArmBot", + "gradlebase": "cpp", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java new file mode 100644 index 0000000000..fda1290b36 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterInches = 6; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR; + } + + public static final class ArmConstants { + public static final int kMotorPort = 4; + + public static final double kP = 1; + + // These are fake gains; in actuality these must be determined individually for each robot + public static final double kSVolts = 1; + public static final double kCosVolts = 1; + public static final double kVVoltSecondPerRad = 0.5; + public static final double kAVoltSecondSquaredPerRad = 0.1; + + public static final double kMaxVelocityRadPerSecond = 3; + public static final double kMaxAccelerationRadPerSecSquared = 10; + + public static final int[] kEncoderPorts = new int[]{4, 5}; + public static final int kEncoderPPR = 256; + public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; + + // The offset of the arm from the horizontal in its neutral position, + // measured from the horizontal + public static final double kArmOffsetRads = 0.5; + } + + public static final class AutoConstants { + public static final double kAutoTimeoutSeconds = 12; + public static final double kAutoShootTimeSeconds = 7; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java new file mode 100644 index 0000000000..681a38ea09 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java new file mode 100644 index 0000000000..a171fc6c99 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java new file mode 100644 index 0000000000..64f44d641a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem; +import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final ArmSubsystem m_robotArm = new ArmSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive + .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a + * {@link JoystickButton}. + */ + private void configureButtonBindings() { + + // Move the arm to 2 radians above horizontal when the 'A' button is pressed. + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm); + + // Move the arm to neutral position when the 'B' button is pressed. + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm); + + // Drive at half speed when the bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(0.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new InstantCommand(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java new file mode 100644 index 0000000000..c6f7a4d5a2 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java @@ -0,0 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.controller.ArmFeedforward; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; + +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kAVoltSecondSquaredPerRad; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kArmOffsetRads; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kCosVolts; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderPorts; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxVelocityRadPerSecond; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMotorPort; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kP; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kSVolts; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kVVoltSecondPerRad; + +/** + * A robot arm subsystem that moves with a motion profile. + */ +public class ArmSubsystem extends ProfiledPIDSubsystem { + private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort); + private final Encoder m_encoder = new Encoder(kEncoderPorts[0], kEncoderPorts[1]); + private final ArmFeedforward m_feedforward = + new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad); + + private TrapezoidProfile.State m_goal; + + /** + * Create a new ArmSubsystem. + */ + public ArmSubsystem() { + super(new ProfiledPIDController( + kP, + 0, + 0, + new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond, + kMaxAccelerationRadPerSecSquared))); + m_encoder.setDistancePerPulse(kEncoderDistancePerPulse); + // Start arm at rest in neutral position + m_goal = new TrapezoidProfile.State(kArmOffsetRads, 0); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_motor.setVoltage(output + feedforward); + } + + /** + * Sets the goal position for the arm. + * + * @param position The goal position, in radians. + */ + public void setGoal(double position) { + m_goal = new TrapezoidProfile.State(position, 0); + } + + @Override + public TrapezoidProfile.State getGoal() { + return m_goal; + } + + @Override + public double getMeasurement() { + return m_encoder.getDistance() + kArmOffsetRads; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..a22079e63b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armbot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the two encoders. + * + * @return the average of the two encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 55b2ebe5a2..e775e20d4f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -483,6 +483,19 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "ArmBot", + "description": "An example command-based robot demonstrating the use of a ProfiledPIDCommand to control an arm.", + "tags": [ + "ArmBot", + "PID", + "Motion Profile" + ], + "foldername": "armbot", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "MecanumControllerCommand", "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",