[examples] Enhance Romi templates (#2931)

Add motors and encoders so they are more usable out of the box.
This commit is contained in:
Zhiquan Yeo
2020-12-11 00:38:27 -05:00
committed by GitHub
parent 00b9ae77f9
commit b27d33675d
10 changed files with 572 additions and 2 deletions

View File

@@ -0,0 +1,19 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
}

View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

View File

@@ -0,0 +1,113 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// 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() {
}
}

View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.templates.romicommandbased.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
import edu.wpi.first.wpilibj2.command.Command;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
* (including subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final RomiDrivetrain m_romiDrivetrain = new RomiDrivetrain();
private final ExampleCommand m_autoCommand = new ExampleCommand(m_romiDrivetrain);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
}
/**
* 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 edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return m_autoCommand;
}
}

View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased.commands;
import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
import edu.wpi.first.wpilibj2.command.CommandBase;
/**
* An example command that uses an example subsystem.
*/
public class ExampleCommand extends CommandBase {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final RomiDrivetrain m_subsystem;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ExampleCommand(RomiDrivetrain subsystem) {
m_subsystem = subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

View File

@@ -0,0 +1,77 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romicommandbased.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class RomiDrivetrain extends SubsystemBase {
private static final double kCountsPerRevolution = 1440.0;
private static final double kWheelDiameterInch = 2.75;
// The Romi has the left and right motors set to
// PWM channels 0 and 1 respectively
private final Spark m_leftMotor = new Spark(0);
private final Spark m_rightMotor = new Spark(1);
// The Romi has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
/**
* Creates a new RomiDrivetrain.
*/
public RomiDrivetrain() {
// DifferentialDrive defaults to having the right side flipped
// We don't need to do this because the Romi has accounted for this
// in firmware/hardware
m_diffDrive.setRightSideInverted(false);
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
}
public double getLeftDistanceInch() {
return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution);
}
public double getRightDistanceInch() {
return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
}
}

View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romitimed;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

View File

@@ -0,0 +1,129 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romitimed;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* 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 static final String kDefaultAuto = "Default";
private static final String kCustomAuto = "My Auto";
private String m_autoSelected;
private final SendableChooser<String> m_chooser = new SendableChooser<>();
private final RomiDrivetrain m_drivetrain = new RomiDrivetrain();
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
m_chooser.addOption("My Auto", kCustomAuto);
SmartDashboard.putData("Auto choices", m_chooser);
}
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
}
/**
* 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 line to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional comparisons to
* the switch structure below with additional strings. If using the
* SendableChooser make sure to add them to the chooser code above as well.
*/
@Override
public void autonomousInit() {
m_autoSelected = m_chooser.getSelected();
// m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
System.out.println("Auto selected: " + m_autoSelected);
m_drivetrain.resetEncoders();
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
switch (m_autoSelected) {
case kCustomAuto:
// Put custom auto code here
break;
case kDefaultAuto:
default:
// Put default auto code here
break;
}
}
/**
* This function is called once when teleop is enabled.
*/
@Override
public void teleopInit() {
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
/**
* This function is called once when the robot is disabled.
*/
@Override
public void disabledInit() {
}
/**
* This function is called periodically when disabled.
*/
@Override
public void disabledPeriodic() {
}
/**
* This function is called once when test mode is enabled.
*/
@Override
public void testInit() {
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}

View File

@@ -0,0 +1,66 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be 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.templates.romitimed;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
public class RomiDrivetrain {
private static final double kCountsPerRevolution = 1440.0;
private static final double kWheelDiameterInch = 2.75;
// The Romi has the left and right motors set to
// PWM channels 0 and 1 respectively
private final Spark m_leftMotor = new Spark(0);
private final Spark m_rightMotor = new Spark(1);
// The Romi has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
/**
* Creates a new RomiDrivetrain.
*/
public RomiDrivetrain() {
// DifferentialDrive defaults to having the right side flipped
// We don't need to do this because the Romi has accounted for this
// in firmware/hardware
m_diffDrive.setRightSideInverted(false);
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
}
public double getLeftDistanceInch() {
return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution);
}
public double getRightDistanceInch() {
return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution);
}
}

View File

@@ -60,7 +60,7 @@
"tags": [
"Timed", "Romi"
],
"foldername": "timed",
"foldername": "romitimed",
"gradlebase": "javaromi",
"mainclass": "Main",
"commandversion": 2
@@ -71,7 +71,7 @@
"tags": [
"Command", "Romi"
],
"foldername": "commandbased",
"foldername": "romicommandbased",
"gradlebase": "javaromi",
"mainclass": "Main",
"commandversion": 2