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

Don't force public variables to use Hungarian notation (#8774)

Browse Source 
People generally have expressed a dislike for the Hungarian notation
used in member variables, especially in examples/templates, and our
styleguide shouldn't be forced on downstream consumers, so this removes
all Hungarian notation from the examples/templates.

There are _some_ benefits to Hungarian for private member variables
(like knowing what's a member vs. local in a PR review) so we'll keep
private member variables the same for now, but public variables should
no longer use Hungarian notation, since it looks much worse. A new PMD
XPath rule has been added to accomplish this goal. Some other
non-compliant variables were fixed for the new rule.
This commit is contained in:
Gold856
2026-04-25 14:32:08 -04:00
committed by GitHub
parent e7e51c9c05
commit 35e8abedeb
443 changed files with 4584 additions and 4789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrivegamepad/Robot.java
View File

@@ -15,21 +15,21 @@ import org.wpilib.util.sendable.SendableRegistry;
* arcade steering and a gamepad.
*/
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::setThrottle, m_rightMotor::setThrottle);
private final Gamepad m_driverController = new Gamepad(0);
private final PWMSparkMax leftMotor = new PWMSparkMax(0);
private final PWMSparkMax rightMotor = new PWMSparkMax(1);
private final DifferentialDrive robotDrive =
new DifferentialDrive(leftMotor::setThrottle, rightMotor::setThrottle);
private final Gamepad driverController = new Gamepad(0);
/** Called once at the beginning of the robot program. */
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftMotor);
SendableRegistry.addChild(m_robotDrive, m_rightMotor);
SendableRegistry.addChild(robotDrive, leftMotor);
SendableRegistry.addChild(robotDrive, rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
rightMotor.setInverted(true);
}
@Override
@@ -37,6 +37,6 @@ public class Robot extends TimedRobot {
// Drive with split arcade drive.
// That means that the Y axis of the left stick moves forward
// and backward, and the X of the right stick turns left and right.
m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), -m_driverController.getRightX());
robotDrive.arcadeDrive(-driverController.getLeftY(), -driverController.getRightX());
}
}

18
wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/Robot.java
View File

@@ -10,41 +10,41 @@ import org.wpilib.framework.TimedRobot;
/** This is a sample program to demonstrate the use of arm simulation with existing code. */
public class Robot extends TimedRobot {
private final Arm m_arm = new Arm();
private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
private final Arm arm = new Arm();
private final Joystick joystick = new Joystick(Constants.kJoystickPort);
public Robot() {}
@Override
public void simulationPeriodic() {
m_arm.simulationPeriodic();
arm.simulationPeriodic();
}
@Override
public void teleopInit() {
m_arm.loadPreferences();
arm.loadPreferences();
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
if (joystick.getTrigger()) {
// Here, we run PID control like normal.
m_arm.reachSetpoint();
arm.reachSetpoint();
} else {
// Otherwise, we disable the motor.
m_arm.stop();
arm.stop();
}
}
@Override
public void close() {
m_arm.close();
arm.close();
super.close();
}
@Override
public void disabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_arm.stop();
arm.stop();
}
}

79
wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java
View File

@@ -25,24 +25,24 @@ import org.wpilib.util.Preferences;
public class Arm implements AutoCloseable {
// The P gain for the PID controller that drives this arm.
private double m_armKp = Constants.kDefaultArmKp;
private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
private double armKp = Constants.kDefaultArmKp;
private double armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
// The arm gearbox represents a gearbox containing two Vex 775pro motors.
private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
private final DCMotor armGearbox = DCMotor.getVex775Pro(2);
// Standard classes for controlling our arm
private final PIDController m_controller = new PIDController(m_armKp, 0, 0);
private final Encoder m_encoder =
private final PIDController controller = new PIDController(armKp, 0, 0);
private final Encoder encoder =
new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(Constants.kMotorPort);
// Simulation classes help us simulate what's going on, including gravity.
// This arm sim represents an arm that can travel from -75 degrees (rotated down front)
// to 255 degrees (rotated down in the back).
private final SingleJointedArmSim m_armSim =
private final SingleJointedArmSim armSim =
new SingleJointedArmSim(
m_armGearbox,
armGearbox,
Constants.kArmReduction,
SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass),
Constants.kArmLength,
@@ -53,83 +53,82 @@ public class Arm implements AutoCloseable {
Constants.kArmEncoderDistPerPulse,
0.0 // Add noise with a std-dev of 1 tick
);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final EncoderSim encoderSim = new EncoderSim(encoder);
// Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
private final MechanismLigament2d m_armTower =
m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
private final MechanismLigament2d m_arm =
m_armPivot.append(
private final Mechanism2d mech2d = new Mechanism2d(60, 60);
private final MechanismRoot2d armPivot = mech2d.getRoot("ArmPivot", 30, 30);
private final MechanismLigament2d armTower =
armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
private final MechanismLigament2d arm =
armPivot.append(
new MechanismLigament2d(
"Arm",
30,
Units.radiansToDegrees(m_armSim.getAngle()),
Units.radiansToDegrees(armSim.getAngle()),
6,
new Color8Bit(Color.YELLOW)));
/** Subsystem constructor. */
public Arm() {
m_encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
SmartDashboard.putData("Arm Sim", m_mech2d);
m_armTower.setColor(new Color8Bit(Color.BLUE));
SmartDashboard.putData("Arm Sim", mech2d);
armTower.setColor(new Color8Bit(Color.BLUE));
// Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
Preferences.initDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
Preferences.initDouble(Constants.kArmPKey, m_armKp);
Preferences.initDouble(Constants.kArmPositionKey, armSetpointDegrees);
Preferences.initDouble(Constants.kArmPKey, armKp);
}
/** Update the simulation model. */
public void simulationPeriodic() {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.setInput(m_motor.getThrottle() * RobotController.getBatteryVoltage());
armSim.setInput(motor.getThrottle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_armSim.update(0.020);
armSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_armSim.getAngle());
encoderSim.setDistance(armSim.getAngle());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDraw()));
BatterySim.calculateDefaultBatteryLoadedVoltage(armSim.getCurrentDraw()));
// Update the Mechanism Arm angle based on the simulated arm angle
m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngle()));
arm.setAngle(Units.radiansToDegrees(armSim.getAngle()));
}
/** Load setpoint and kP from preferences. */
public void loadPreferences() {
// Read Preferences for Arm setpoint and kP on entering Teleop
m_armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
if (m_armKp != Preferences.getDouble(Constants.kArmPKey, m_armKp)) {
m_armKp = Preferences.getDouble(Constants.kArmPKey, m_armKp);
m_controller.setP(m_armKp);
armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, armSetpointDegrees);
if (armKp != Preferences.getDouble(Constants.kArmPKey, armKp)) {
armKp = Preferences.getDouble(Constants.kArmPKey, armKp);
controller.setP(armKp);
}
}
/** Run the control loop to reach and maintain the setpoint from the preferences. */
public void reachSetpoint() {
var pidOutput =
m_controller.calculate(
m_encoder.getDistance(), Units.degreesToRadians(m_armSetpointDegrees));
m_motor.setVoltage(pidOutput);
controller.calculate(encoder.getDistance(), Units.degreesToRadians(armSetpointDegrees));
motor.setVoltage(pidOutput);
}
public void stop() {
m_motor.setThrottle(0.0);
motor.setThrottle(0.0);
}
@Override
public void close() {
m_motor.close();
m_encoder.close();
m_mech2d.close();
m_armPivot.close();
m_controller.close();
m_arm.close();
motor.close();
encoder.close();
mech2d.close();
armPivot.close();
controller.close();
arm.close();
}
}

63
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Drivetrain.java
View File

@@ -23,54 +23,54 @@ public class Drivetrain {
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final PWMSparkMax leftLeader = new PWMSparkMax(1);
private final PWMSparkMax leftFollower = new PWMSparkMax(2);
private final PWMSparkMax rightLeader = new PWMSparkMax(3);
private final PWMSparkMax rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final Encoder leftEncoder = new Encoder(0, 1);
private final Encoder rightEncoder = new Encoder(2, 3);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final PIDController leftPIDController = new PIDController(1, 0, 0);
private final PIDController rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
private final DifferentialDriveKinematics kinematics =
new DifferentialDriveKinematics(kTrackwidth);
private final DifferentialDriveOdometry m_odometry;
private final DifferentialDriveOdometry odometry;
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
/**
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
* gyro.
*/
public Drivetrain() {
m_imu.resetYaw();
imu.resetYaw();
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
m_odometry =
odometry =
new DifferentialDriveOdometry(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
}
/**
@@ -79,15 +79,14 @@ public class Drivetrain {
* @param velocities The desired wheel velocities.
*/
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
final double leftFeedforward = feedforward.calculate(velocities.left);
final double rightFeedforward = feedforward.calculate(velocities.right);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
final double leftOutput = leftPIDController.calculate(leftEncoder.getRate(), velocities.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
rightPIDController.calculate(rightEncoder.getRate(), velocities.right);
leftLeader.setVoltage(leftOutput + leftFeedforward);
rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
@@ -97,14 +96,12 @@ public class Drivetrain {
* @param rot Angular velocity in rad/s.
*/
public void drive(double xVelocity, double rot) {
var wheelVelocities =
m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
var wheelVelocities = kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
setVelocities(wheelVelocities);
}
/** Updates the field-relative position. */
public void updateOdometry() {
m_odometry.update(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
odometry.update(imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
}
}

17
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Robot.java
View File

@@ -9,17 +9,17 @@ import org.wpilib.framework.TimedRobot;
import org.wpilib.math.filter.SlewRateLimiter;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_drive = new Drivetrain();
private final Gamepad controller = new Gamepad(0);
private final Drivetrain drive = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
teleopPeriodic();
m_drive.updateOdometry();
drive.updateOdometry();
}
@Override
@@ -27,15 +27,14 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
-velocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
final var rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xVelocity, rot);
drive.drive(xVelocity, rot);
}
}

127
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -46,89 +46,89 @@ public class Drivetrain {
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final PWMSparkMax leftLeader = new PWMSparkMax(1);
private final PWMSparkMax leftFollower = new PWMSparkMax(2);
private final PWMSparkMax rightLeader = new PWMSparkMax(3);
private final PWMSparkMax rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final Encoder leftEncoder = new Encoder(0, 1);
private final Encoder rightEncoder = new Encoder(2, 3);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final PIDController leftPIDController = new PIDController(1, 0, 0);
private final PIDController rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
private final DifferentialDriveKinematics kinematics =
new DifferentialDriveKinematics(kTrackwidth);
private final Pose3d m_objectInField;
private final Pose3d objectInField;
private final Transform3d m_robotToCamera =
private final Transform3d robotToCamera =
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, Math.PI / 2));
private final DoubleArrayEntry m_cameraToObjectEntry;
private final DoubleArrayEntry cameraToObjectEntry;
private final double[] m_defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
private final double[] defaultVal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
private final Field2d m_fieldSim = new Field2d();
private final Field2d m_fieldApproximation = new Field2d();
private final Field2d fieldSim = new Field2d();
private final Field2d fieldApproximation = new Field2d();
/* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
numbers used below are robot specific, and should be tuned. */
private final DifferentialDrivePoseEstimator m_poseEstimator =
private final DifferentialDrivePoseEstimator poseEstimator =
new DifferentialDrivePoseEstimator(
m_kinematics,
m_imu.getRotation2d(),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
kinematics,
imu.getRotation2d(),
leftEncoder.getDistance(),
rightEncoder.getDistance(),
Pose2d.kZero,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
// Simulation classes
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
private final EncoderSim leftEncoderSim = new EncoderSim(leftEncoder);
private final EncoderSim rightEncoderSim = new EncoderSim(rightEncoder);
private final LinearSystem<N2, N2, N2> drivetrainSystem =
Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3);
private final DifferentialDrivetrainSim m_drivetrainSimulator =
private final DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
/**
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
* gyro.
*/
public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
m_imu.resetYaw();
imu.resetYaw();
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
cameraToObjectEntry = cameraToObjectTopic.getEntry(defaultVal);
m_objectInField =
objectInField =
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get();
SmartDashboard.putData("Field", m_fieldSim);
SmartDashboard.putData("FieldEstimation", m_fieldApproximation);
SmartDashboard.putData("Field", fieldSim);
SmartDashboard.putData("FieldEstimation", fieldApproximation);
}
/**
@@ -137,15 +137,14 @@ public class Drivetrain {
* @param velocities The desired wheel velocities.
*/
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
final double leftFeedforward = feedforward.calculate(velocities.left);
final double rightFeedforward = feedforward.calculate(velocities.right);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
final double leftOutput = leftPIDController.calculate(leftEncoder.getRate(), velocities.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
rightPIDController.calculate(rightEncoder.getRate(), velocities.right);
leftLeader.setVoltage(leftOutput + leftFeedforward);
rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
@@ -155,8 +154,7 @@ public class Drivetrain {
* @param rot Angular velocity in rad/s.
*/
public void drive(double xVelocity, double rot) {
var wheelVelocities =
m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
var wheelVelocities = kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
setVelocities(wheelVelocities);
}
@@ -221,25 +219,24 @@ public class Drivetrain {
/** Updates the field-relative position. */
public void updateOdometry() {
m_poseEstimator.update(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
poseEstimator.update(
imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
// Publish cameraToObject transformation to networktables --this would normally be handled by
// the
// computer vision solution.
publishCameraToObject(
m_objectInField, m_robotToCamera, m_cameraToObjectEntry, m_drivetrainSimulator);
publishCameraToObject(objectInField, robotToCamera, cameraToObjectEntry, drivetrainSimulator);
// Compute the robot's field-relative position exclusively from vision measurements.
Pose3d visionMeasurement3d =
objectToRobotPose(m_objectInField, m_robotToCamera, m_cameraToObjectEntry);
objectToRobotPose(objectInField, robotToCamera, cameraToObjectEntry);
// Convert robot pose from Pose3d to Pose2d needed to apply vision measurements.
Pose2d visionMeasurement2d = visionMeasurement3d.toPose2d();
// Apply vision measurements. For simulation purposes only, we don't input a latency delay -- on
// a real robot, this must be calculated based either on known latency or timestamps.
m_poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getTimestamp());
poseEstimator.addVisionMeasurement(visionMeasurement2d, Timer.getTimestamp());
}
/** This function is called periodically during simulation. */
@@ -247,23 +244,23 @@ public class Drivetrain {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro.
m_drivetrainSimulator.setInputs(
m_leftLeader.getThrottle() * RobotController.getInputVoltage(),
m_rightLeader.getThrottle() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
drivetrainSimulator.setInputs(
leftLeader.getThrottle() * RobotController.getInputVoltage(),
rightLeader.getThrottle() * RobotController.getInputVoltage());
drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup
leftEncoderSim.setDistance(drivetrainSimulator.getLeftPosition());
leftEncoderSim.setRate(drivetrainSimulator.getLeftVelocity());
rightEncoderSim.setDistance(drivetrainSimulator.getRightPosition());
rightEncoderSim.setRate(drivetrainSimulator.getRightVelocity());
// gyroSim.setAngle(-drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup
// when sim implemented
}
/** This function is called periodically, no matter the mode. */
public void periodic() {
updateOdometry();
m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose());
m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());
fieldSim.setRobotPose(drivetrainSimulator.getPose());
fieldApproximation.setRobotPose(poseEstimator.getEstimatedPosition());
}
}

26
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Robot.java
View File

@@ -11,31 +11,30 @@ import org.wpilib.networktables.DoubleArrayTopic;
import org.wpilib.networktables.NetworkTableInstance;
public class Robot extends TimedRobot {
private final NetworkTableInstance m_inst = NetworkTableInstance.getDefault();
private final DoubleArrayTopic m_doubleArrayTopic =
m_inst.getDoubleArrayTopic("m_doubleArrayTopic");
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
private final DoubleArrayTopic doubleArrayTopic = inst.getDoubleArrayTopic("doubleArrayTopic");
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic);
private final Gamepad controller = new Gamepad(0);
private final Drivetrain drive = new Drivetrain(doubleArrayTopic);
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
teleopPeriodic();
m_drive.updateOdometry();
drive.updateOdometry();
}
@Override
public void simulationPeriodic() {
m_drive.simulationPeriodic();
drive.simulationPeriodic();
}
@Override
public void robotPeriodic() {
m_drive.periodic();
drive.periodic();
}
@Override
@@ -43,15 +42,14 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepad return
// negative values when we push forward.
final var xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
-velocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
final var rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xVelocity, rot);
drive.drive(xVelocity, rot);
}
}

6
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/ExampleSmartMotorController.java
View File

@@ -16,7 +16,7 @@ public class ExampleSmartMotorController {
kMovementWitchcraft
}
double m_value;
double value;
/**
* Creates a new ExampleSmartMotorController.
@@ -72,11 +72,11 @@ public class ExampleSmartMotorController {
public void resetEncoder() {}
public void setThrottle(double throttle) {
m_value = throttle;
value = throttle;
}
public double getThrottle() {
return m_value;
return value;
}
public void setInverted(boolean isInverted) {}

16
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/Robot.java
View File

@@ -14,9 +14,9 @@ import org.wpilib.framework.TimedRobot;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final RobotContainer m_robotContainer;
private final RobotContainer robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
@@ -25,7 +25,7 @@ public class Robot extends TimedRobot {
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
robotContainer = new RobotContainer();
}
/**
@@ -54,7 +54,7 @@ public class Robot extends TimedRobot {
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autonomousCommand = robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -64,8 +64,8 @@ public class Robot extends TimedRobot {
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -79,8 +79,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/RobotContainer.java
View File

@@ -19,15 +19,14 @@ import org.wpilib.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final DriveSubsystem robotDrive = new DriveSubsystem();
// Retained command references
private final Command m_driveFullVelocity = Commands.runOnce(() -> m_robotDrive.setMaxOutput(1));
private final Command m_driveHalfVelocity =
Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5));
private final Command driveFullVelocity = Commands.runOnce(() -> robotDrive.setMaxOutput(1));
private final Command driveHalfVelocity = Commands.runOnce(() -> robotDrive.setMaxOutput(0.5));
// The driver's controller
CommandGamepad m_driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
@@ -36,14 +35,13 @@ public class RobotContainer {
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
robotDrive.arcadeDrive(-driverController.getLeftY(), -driverController.getRightX()),
robotDrive));
}
/**
@@ -54,17 +52,15 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
// Drive at half velocity when the bumper is held
m_driverController.rightBumper().onTrue(m_driveHalfVelocity).onFalse(m_driveFullVelocity);
driverController.rightBumper().onTrue(driveHalfVelocity).onFalse(driveFullVelocity);
// Drive forward by 3 meters when the 'South Face' button is pressed, with a timeout of 10
// seconds
m_driverController.southFace().onTrue(m_robotDrive.profiledDriveDistance(3).withTimeout(10));
driverController.southFace().onTrue(robotDrive.profiledDriveDistance(3).withTimeout(10));
// Do the same thing as above when the 'East Face' button is pressed, but without resetting the
// encoders
m_driverController
.eastFace()
.onTrue(m_robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10));
driverController.eastFace().onTrue(robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10));
}
/**

100
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -18,51 +18,51 @@ import org.wpilib.util.sendable.SendableRegistry;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final ExampleSmartMotorController m_leftLeader =
private final ExampleSmartMotorController leftLeader =
new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
private final ExampleSmartMotorController m_leftFollower =
private final ExampleSmartMotorController leftFollower =
new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final ExampleSmartMotorController m_rightLeader =
private final ExampleSmartMotorController rightLeader =
new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
private final ExampleSmartMotorController m_rightFollower =
private final ExampleSmartMotorController rightFollower =
new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
// The feedforward controller.
private final SimpleMotorFeedforward m_feedforward =
private final SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setThrottle, m_rightLeader::setThrottle);
private final DifferentialDrive drive =
new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
// The trapezoid profile
private final TrapezoidProfile m_profile =
private final TrapezoidProfile profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
DriveConstants.kMaxVelocity, DriveConstants.kMaxAcceleration));
// The timer
private final Timer m_timer = new Timer();
private final Timer timer = new Timer();
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
SendableRegistry.addChild(drive, leftLeader);
SendableRegistry.addChild(drive, rightLeader);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
m_leftFollower.follow(m_leftLeader);
m_rightFollower.follow(m_rightLeader);
leftFollower.follow(leftLeader);
rightFollower.follow(rightLeader);
m_leftLeader.setPID(DriveConstants.kp, 0, 0);
m_rightLeader.setPID(DriveConstants.kp, 0, 0);
leftLeader.setPID(DriveConstants.kp, 0, 0);
rightLeader.setPID(DriveConstants.kp, 0, 0);
}
/**
@@ -72,7 +72,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
drive.arcadeDrive(fwd, rot);
}
/**
@@ -89,15 +89,15 @@ public class DriveSubsystem extends SubsystemBase {
TrapezoidProfile.State nextLeft,
TrapezoidProfile.State nextRight) {
// Feedforward is divided by battery voltage to normalize it to [-1, 1]
m_leftLeader.setSetpoint(
leftLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
currentLeft.position,
m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
/ RobotController.getBatteryVoltage());
m_rightLeader.setSetpoint(
rightLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
currentRight.position,
m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
/ RobotController.getBatteryVoltage());
}
@@ -107,7 +107,7 @@ public class DriveSubsystem extends SubsystemBase {
* @return the left encoder distance
*/
public double getLeftEncoderDistance() {
return m_leftLeader.getEncoderDistance();
return leftLeader.getEncoderDistance();
}
/**
@@ -116,13 +116,13 @@ public class DriveSubsystem extends SubsystemBase {
* @return the right encoder distance
*/
public double getRightEncoderDistance() {
return m_rightLeader.getEncoderDistance();
return rightLeader.getEncoderDistance();
}
/** Resets the drive encoders. */
public void resetEncoders() {
m_leftLeader.resetEncoder();
m_rightLeader.resetEncoder();
leftLeader.resetEncoder();
rightLeader.resetEncoder();
}
/**
@@ -131,7 +131,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
drive.setMaxOutput(maxOutput);
}
/**
@@ -144,25 +144,25 @@ public class DriveSubsystem extends SubsystemBase {
return startRun(
() -> {
// Restart timer so profile setpoints start at the beginning
m_timer.restart();
timer.restart();
resetEncoders();
},
() -> {
// Current state never changes, so we need to use a timer to get the setpoints we need
// to be at
var currentTime = m_timer.get();
var currentTime = timer.get();
var currentSetpoint =
m_profile.calculate(currentTime, new State(), new State(distance, 0));
profile.calculate(currentTime, new State(), new State(distance, 0));
var nextSetpoint =
m_profile.calculate(
profile.calculate(
currentTime + DriveConstants.kDt, new State(), new State(distance, 0));
setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint);
})
.until(() -> m_profile.isFinished(0));
.until(() -> profile.isFinished(0));
}
private double m_initialLeftDistance;
private double m_initialRightDistance;
private double initialLeftDistance;
private double initialRightDistance;
/**
* Creates a command to drive forward a specified distance using a motion profile without
@@ -175,38 +175,38 @@ public class DriveSubsystem extends SubsystemBase {
return startRun(
() -> {
// Restart timer so profile setpoints start at the beginning
m_timer.restart();
timer.restart();
// Store distance so we know the target distance for each encoder
m_initialLeftDistance = getLeftEncoderDistance();
m_initialRightDistance = getRightEncoderDistance();
initialLeftDistance = getLeftEncoderDistance();
initialRightDistance = getRightEncoderDistance();
},
() -> {
// Current state never changes for the duration of the command, so we need to use a
// timer to get the setpoints we need to be at
var currentTime = m_timer.get();
var currentTime = timer.get();
var currentLeftSetpoint =
m_profile.calculate(
profile.calculate(
currentTime,
new State(m_initialLeftDistance, 0),
new State(m_initialLeftDistance + distance, 0));
new State(initialLeftDistance, 0),
new State(initialLeftDistance + distance, 0));
var currentRightSetpoint =
m_profile.calculate(
profile.calculate(
currentTime,
new State(m_initialRightDistance, 0),
new State(m_initialRightDistance + distance, 0));
new State(initialRightDistance, 0),
new State(initialRightDistance + distance, 0));
var nextLeftSetpoint =
m_profile.calculate(
profile.calculate(
currentTime + DriveConstants.kDt,
new State(m_initialLeftDistance, 0),
new State(m_initialLeftDistance + distance, 0));
new State(initialLeftDistance, 0),
new State(initialLeftDistance + distance, 0));
var nextRightSetpoint =
m_profile.calculate(
profile.calculate(
currentTime + DriveConstants.kDt,
new State(m_initialRightDistance, 0),
new State(m_initialRightDistance + distance, 0));
new State(initialRightDistance, 0),
new State(initialRightDistance + distance, 0));
setDriveStates(
currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint);
})
.until(() -> m_profile.isFinished(0));
.until(() -> profile.isFinished(0));
}
}

20
wpilibjExamples/src/main/java/org/wpilib/examples/dutycycleencoder/Robot.java
View File

@@ -11,9 +11,9 @@ import org.wpilib.smartdashboard.SmartDashboard;
/** This example shows how to use a duty cycle encoder for devices such as an arm or elevator. */
public class Robot extends TimedRobot {
private final DutyCycleEncoder m_dutyCycleEncoder;
private static final double m_fullRange = 1.3;
private static final double m_expectedZero = 0;
private final DutyCycleEncoder dutyCycleEncoder;
private static final double fullRange = 1.3;
private static final double expectedZero = 0;
/** Called once at the beginning of the robot program. */
public Robot() {
@@ -23,7 +23,7 @@ public class Robot extends TimedRobot {
// to measure this is fairly easy. Set the value to 0, place the mechanism
// where you want "0" to be, and observe the value on the dashboard, That
// is the value to enter for the 3rd parameter.
m_dutyCycleEncoder = new DutyCycleEncoder(0, m_fullRange, m_expectedZero);
dutyCycleEncoder = new DutyCycleEncoder(0, fullRange, expectedZero);
// If you know the frequency of your sensor, uncomment the following
// method, and set the method to the frequency of your sensor.
@@ -36,19 +36,19 @@ public class Robot extends TimedRobot {
// those values. This number doesn't have to be perfect,
// just having a fairly close value will make the output readings
// much more stable.
m_dutyCycleEncoder.setAssumedFrequency(967.8);
dutyCycleEncoder.setAssumedFrequency(967.8);
}
@Override
public void robotPeriodic() {
// Connected can be checked, and uses the frequency of the encoder
boolean connected = m_dutyCycleEncoder.isConnected();
boolean connected = dutyCycleEncoder.isConnected();
// Duty Cycle Frequency in Hz
double frequency = m_dutyCycleEncoder.getFrequency();
double frequency = dutyCycleEncoder.getFrequency();
// Output of encoder
double output = m_dutyCycleEncoder.get();
double output = dutyCycleEncoder.get();
// By default, the output will wrap around to the full range value
// when the sensor goes below 0. However, for moving mechanisms this
@@ -60,9 +60,9 @@ public class Robot extends TimedRobot {
// can go a bit negative before wrapping. Usually 10% or so is fine.
// This does not change where "0" is, so no calibration numbers need
// to be changed.
double percentOfRange = m_fullRange * 0.1;
double percentOfRange = fullRange * 0.1;
double shiftedOutput =
MathUtil.inputModulus(output, 0 - percentOfRange, m_fullRange - percentOfRange);
MathUtil.inputModulus(output, 0 - percentOfRange, fullRange - percentOfRange);
SmartDashboard.putBoolean("Connected", connected);
SmartDashboard.putNumber("Frequency", frequency);

32
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialprofile/Robot.java
View File

@@ -12,41 +12,41 @@ import org.wpilib.math.trajectory.ExponentialProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
private final Joystick joystick = new Joystick(1);
private final ExampleSmartMotorController motor = new ExampleSmartMotorController(1);
// Note: These gains are fake, and will have to be tuned for your robot.
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1, 1);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 1, 1);
// Create a motion profile with the given maximum voltage and characteristics kV, kA
// These gains should match your feedforward kV, kA for best results.
private final ExponentialProfile m_profile =
private final ExponentialProfile profile =
new ExponentialProfile(ExponentialProfile.Constraints.fromCharacteristics(10, 1, 1));
private ExponentialProfile.State m_goal = new ExponentialProfile.State(0, 0);
private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
private ExponentialProfile.State goal = new ExponentialProfile.State(0, 0);
private ExponentialProfile.State setpoint = new ExponentialProfile.State(0, 0);
public Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.setPID(1.3, 0.0, 0.7);
motor.setPID(1.3, 0.0, 0.7);
}
@Override
public void teleopPeriodic() {
if (m_joystick.getRawButtonPressed(2)) {
m_goal = new ExponentialProfile.State(5, 0);
} else if (m_joystick.getRawButtonPressed(3)) {
m_goal = new ExponentialProfile.State(0, 0);
if (joystick.getRawButtonPressed(2)) {
goal = new ExponentialProfile.State(5, 0);
} else if (joystick.getRawButtonPressed(3)) {
goal = new ExponentialProfile.State(0, 0);
}
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
ExponentialProfile.State next = m_profile.calculate(kDt, m_setpoint, m_goal);
ExponentialProfile.State next = profile.calculate(kDt, setpoint, goal);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(
motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(next.velocity) / 12.0);
setpoint.position,
feedforward.calculate(next.velocity) / 12.0);
m_setpoint = next;
setpoint = next;
}
}

20
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/Robot.java
View File

@@ -10,8 +10,8 @@ import org.wpilib.framework.TimedRobot;
/** This is a sample program to demonstrate the use of elevator simulation. */
public class Robot extends TimedRobot {
private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
private final Elevator m_elevator = new Elevator();
private final Joystick joystick = new Joystick(Constants.kJoystickPort);
private final Elevator elevator = new Elevator();
public Robot() {
super(0.020);
@@ -20,40 +20,40 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of mode.
m_elevator.updateTelemetry();
elevator.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// Update the simulation model.
m_elevator.simulationPeriodic();
elevator.simulationPeriodic();
}
@Override
public void teleopInit() {
m_elevator.reset();
elevator.reset();
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
if (joystick.getTrigger()) {
// Here, we set the constant setpoint of 10 meters.
m_elevator.reachGoal(Constants.kSetpoint);
elevator.reachGoal(Constants.kSetpoint);
} else {
// Otherwise, we update the setpoint to 1 meter.
m_elevator.reachGoal(Constants.kLowerkSetpoint);
elevator.reachGoal(Constants.kLowerkSetpoint);
}
}
@Override
public void disabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_elevator.stop();
elevator.stop();
}
@Override
public void close() {
m_elevator.close();
elevator.close();
super.close();
}
}

66
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java
View File

@@ -25,33 +25,33 @@ import org.wpilib.system.RobotController;
public class Elevator implements AutoCloseable {
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
private final DCMotor m_elevatorGearbox = DCMotor.getNEO(2);
private final DCMotor elevatorGearbox = DCMotor.getNEO(2);
private final ExponentialProfile m_profile =
private final ExponentialProfile profile =
new ExponentialProfile(
ExponentialProfile.Constraints.fromCharacteristics(
Constants.kElevatorMaxV, Constants.kElevatorkV, Constants.kElevatorkA));
private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0);
private ExponentialProfile.State setpoint = new ExponentialProfile.State(0, 0);
// Standard classes for controlling our elevator
private final PIDController m_pidController =
private final PIDController pidController =
new PIDController(Constants.kElevatorKp, Constants.kElevatorKi, Constants.kElevatorKd);
ElevatorFeedforward m_feedforward =
ElevatorFeedforward feedforward =
new ElevatorFeedforward(
Constants.kElevatorkS,
Constants.kElevatorkG,
Constants.kElevatorkV,
Constants.kElevatorkA);
private final Encoder m_encoder =
private final Encoder encoder =
new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(Constants.kMotorPort);
// Simulation classes help us simulate what's going on, including gravity.
private final ElevatorSim m_elevatorSim =
private final ElevatorSim elevatorSim =
new ElevatorSim(
m_elevatorGearbox,
elevatorGearbox,
Constants.kElevatorGearing,
Constants.kCarriageMass,
Constants.kElevatorDrumRadius,
@@ -61,40 +61,40 @@ public class Elevator implements AutoCloseable {
0,
0.005,
0.0);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMMotorControllerSim m_motorSim = new PWMMotorControllerSim(m_motor);
private final EncoderSim encoderSim = new EncoderSim(encoder);
private final PWMMotorControllerSim motorSim = new PWMMotorControllerSim(motor);
// Create a Mechanism2d visualization of the elevator
private final Mechanism2d m_mech2d =
private final Mechanism2d mech2d =
new Mechanism2d(Units.inchesToMeters(10), Units.inchesToMeters(51));
private final MechanismRoot2d m_mech2dRoot =
m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5));
private final MechanismLigament2d m_elevatorMech2d =
m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
private final MechanismRoot2d mech2dRoot =
mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5));
private final MechanismLigament2d elevatorMech2d =
mech2dRoot.append(new MechanismLigament2d("Elevator", elevatorSim.getPosition(), 90));
/** Subsystem constructor. */
public Elevator() {
m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
// Publish Mechanism2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
SmartDashboard.putData("Elevator Sim", m_mech2d);
SmartDashboard.putData("Elevator Sim", mech2d);
}
/** Advance the simulation. */
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motorSim.getThrottle() * RobotController.getBatteryVoltage());
elevatorSim.setInput(motorSim.getThrottle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPosition());
encoderSim.setDistance(elevatorSim.getPosition());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.getCurrentDraw()));
}
/**
@@ -105,37 +105,37 @@ public class Elevator implements AutoCloseable {
public void reachGoal(double goal) {
var goalState = new ExponentialProfile.State(goal, 0);
var next = m_profile.calculate(0.020, m_setpoint, goalState);
var next = profile.calculate(0.020, setpoint, goalState);
// With the setpoint value we run PID control like normal
double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position);
double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity);
double pidOutput = pidController.calculate(encoder.getDistance(), setpoint.position);
double feedforwardOutput = feedforward.calculate(setpoint.velocity, next.velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);
motor.setVoltage(pidOutput + feedforwardOutput);
m_setpoint = next;
setpoint = next;
}
/** Stop the control loop and motor output. */
public void stop() {
m_motor.setThrottle(0.0);
motor.setThrottle(0.0);
}
/** Reset Exponential profile to begin from current position on enable. */
public void reset() {
m_setpoint = new ExponentialProfile.State(m_encoder.getDistance(), 0);
setpoint = new ExponentialProfile.State(encoder.getDistance(), 0);
}
/** Update telemetry, including the mechanism visualization. */
public void updateTelemetry() {
// Update elevator visualization with position
m_elevatorMech2d.setLength(m_encoder.getDistance());
elevatorMech2d.setLength(encoder.getDistance());
}
@Override
public void close() {
m_encoder.close();
m_motor.close();
m_mech2d.close();
encoder.close();
motor.close();
mech2d.close();
}
}

30
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorprofiledpid/Robot.java
View File

@@ -23,33 +23,33 @@ public class Robot extends TimedRobot {
private static double kG = 1.2;
private static double kV = 1.3;
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
private final PWMSparkMax m_motor = new PWMSparkMax(1);
private final Joystick joystick = new Joystick(1);
private final Encoder encoder = new Encoder(1, 2);
private final PWMSparkMax motor = new PWMSparkMax(1);
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
private final TrapezoidProfile.Constraints m_constraints =
private final TrapezoidProfile.Constraints constraints =
new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
private final ProfiledPIDController m_controller =
new ProfiledPIDController(kP, kI, kD, m_constraints, kDt);
private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV);
private final ProfiledPIDController controller =
new ProfiledPIDController(kP, kI, kD, constraints, kDt);
private final ElevatorFeedforward feedforward = new ElevatorFeedforward(kS, kG, kV);
public Robot() {
m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
}
@Override
public void teleopPeriodic() {
if (m_joystick.getRawButtonPressed(2)) {
m_controller.setGoal(5);
} else if (m_joystick.getRawButtonPressed(3)) {
m_controller.setGoal(0);
if (joystick.getRawButtonPressed(2)) {
controller.setGoal(5);
} else if (joystick.getRawButtonPressed(3)) {
controller.setGoal(0);
}
// Run controller and update motor output
m_motor.setVoltage(
m_controller.calculate(m_encoder.getDistance())
+ m_feedforward.calculate(m_controller.getSetpoint().velocity));
motor.setVoltage(
controller.calculate(encoder.getDistance())
+ feedforward.calculate(controller.getSetpoint().velocity));
}
}

18
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/Robot.java
View File

@@ -10,43 +10,43 @@ import org.wpilib.framework.TimedRobot;
/** This is a sample program to demonstrate the use of elevator simulation. */
public class Robot extends TimedRobot {
private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
private final Elevator m_elevator = new Elevator();
private final Joystick joystick = new Joystick(Constants.kJoystickPort);
private final Elevator elevator = new Elevator();
public Robot() {}
@Override
public void robotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of mode.
m_elevator.updateTelemetry();
elevator.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// Update the simulation model.
m_elevator.simulationPeriodic();
elevator.simulationPeriodic();
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
if (joystick.getTrigger()) {
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.reachGoal(Constants.kSetpoint);
elevator.reachGoal(Constants.kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_elevator.reachGoal(0.0);
elevator.reachGoal(0.0);
}
}
@Override
public void disabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_elevator.stop();
elevator.stop();
}
@Override
public void close() {
m_elevator.close();
elevator.close();
super.close();
}
}

58
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java
View File

@@ -24,29 +24,29 @@ import org.wpilib.system.RobotController;
public class Elevator implements AutoCloseable {
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
private final DCMotor elevatorGearbox = DCMotor.getVex775Pro(4);
// Standard classes for controlling our elevator
private final ProfiledPIDController m_controller =
private final ProfiledPIDController controller =
new ProfiledPIDController(
Constants.kElevatorKp,
Constants.kElevatorKi,
Constants.kElevatorKd,
new TrapezoidProfile.Constraints(2.45, 2.45));
ElevatorFeedforward m_feedforward =
ElevatorFeedforward feedforward =
new ElevatorFeedforward(
Constants.kElevatorkS,
Constants.kElevatorkG,
Constants.kElevatorkV,
Constants.kElevatorkA);
private final Encoder m_encoder =
private final Encoder encoder =
new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(Constants.kMotorPort);
// Simulation classes help us simulate what's going on, including gravity.
private final ElevatorSim m_elevatorSim =
private final ElevatorSim elevatorSim =
new ElevatorSim(
m_elevatorGearbox,
elevatorGearbox,
Constants.kElevatorGearing,
Constants.kCarriageMass,
Constants.kElevatorDrumRadius,
@@ -56,38 +56,38 @@ public class Elevator implements AutoCloseable {
0,
0.01,
0.0);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMMotorControllerSim m_motorSim = new PWMMotorControllerSim(m_motor);
private final EncoderSim encoderSim = new EncoderSim(encoder);
private final PWMMotorControllerSim motorSim = new PWMMotorControllerSim(motor);
// Create a Mechanism2d visualization of the elevator
private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
private final MechanismLigament2d m_elevatorMech2d =
m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
private final Mechanism2d mech2d = new Mechanism2d(20, 50);
private final MechanismRoot2d mech2dRoot = mech2d.getRoot("Elevator Root", 10, 0);
private final MechanismLigament2d elevatorMech2d =
mech2dRoot.append(new MechanismLigament2d("Elevator", elevatorSim.getPosition(), 90));
/** Subsystem constructor. */
public Elevator() {
m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
// Publish Mechanism2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
SmartDashboard.putData("Elevator Sim", m_mech2d);
SmartDashboard.putData("Elevator Sim", mech2d);
}
/** Advance the simulation. */
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motorSim.getThrottle() * RobotController.getBatteryVoltage());
elevatorSim.setInput(motorSim.getThrottle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPosition());
encoderSim.setDistance(elevatorSim.getPosition());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.getCurrentDraw()));
}
/**
@@ -96,30 +96,30 @@ public class Elevator implements AutoCloseable {
* @param goal the position to maintain
*/
public void reachGoal(double goal) {
m_controller.setGoal(goal);
controller.setGoal(goal);
// With the setpoint value we run PID control like normal
double pidOutput = m_controller.calculate(m_encoder.getDistance());
double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);
double pidOutput = controller.calculate(encoder.getDistance());
double feedforwardOutput = feedforward.calculate(controller.getSetpoint().velocity);
motor.setVoltage(pidOutput + feedforwardOutput);
}
/** Stop the control loop and motor output. */
public void stop() {
m_controller.setGoal(0.0);
m_motor.setThrottle(0.0);
controller.setGoal(0.0);
motor.setThrottle(0.0);
}
/** Update telemetry, including the mechanism visualization. */
public void updateTelemetry() {
// Update elevator visualization with position
m_elevatorMech2d.setLength(m_encoder.getDistance());
elevatorMech2d.setLength(encoder.getDistance());
}
@Override
public void close() {
m_encoder.close();
m_motor.close();
m_mech2d.close();
encoder.close();
motor.close();
mech2d.close();
}
}

30
wpilibjExamples/src/main/java/org/wpilib/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -12,39 +12,39 @@ import org.wpilib.math.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
private final Joystick joystick = new Joystick(1);
private final ExampleSmartMotorController motor = new ExampleSmartMotorController(1);
// Note: These gains are fake, and will have to be tuned for your robot.
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 1.5);
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint.
private final TrapezoidProfile m_profile =
private final TrapezoidProfile profile =
new TrapezoidProfile(new TrapezoidProfile.Constraints(1.75, 0.75));
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
private TrapezoidProfile.State goal = new TrapezoidProfile.State();
private TrapezoidProfile.State setpoint = new TrapezoidProfile.State();
public Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.setPID(1.3, 0.0, 0.7);
motor.setPID(1.3, 0.0, 0.7);
}
@Override
public void teleopPeriodic() {
if (m_joystick.getRawButtonPressed(2)) {
m_goal = new TrapezoidProfile.State(5, 0);
} else if (m_joystick.getRawButtonPressed(3)) {
m_goal = new TrapezoidProfile.State();
if (joystick.getRawButtonPressed(2)) {
goal = new TrapezoidProfile.State(5, 0);
} else if (joystick.getRawButtonPressed(3)) {
goal = new TrapezoidProfile.State();
}
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
setpoint = profile.calculate(kDt, setpoint, goal);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(
motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity) / 12.0);
setpoint.position,
feedforward.calculate(setpoint.velocity) / 12.0);
}
}

12
wpilibjExamples/src/main/java/org/wpilib/examples/encoder/Robot.java
View File

@@ -32,7 +32,7 @@ public class Robot extends TimedRobot {
* defaults to k4X. Faster (k4X) encoding gives greater positional precision but more noise in the
* rate.
*/
private final Encoder m_encoder = new Encoder(1, 2, false, CounterBase.EncodingType.X4);
private final Encoder encoder = new Encoder(1, 2, false, CounterBase.EncodingType.X4);
/** Called once at the beginning of the robot program. */
public Robot() {
@@ -42,7 +42,7 @@ public class Robot extends TimedRobot {
* larger values result in smoother but potentially
* less accurate rates than lower values.
*/
m_encoder.setSamplesToAverage(5);
encoder.setSamplesToAverage(5);
/*
* Defines how far the mechanism attached to the encoder moves per pulse. In
@@ -50,7 +50,7 @@ public class Robot extends TimedRobot {
* attached to a 3 inch diameter (1.5inch radius) wheel,
* and that we want to measure distance in inches.
*/
m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
/*
* Defines the lowest rate at which the encoder will
@@ -59,12 +59,12 @@ public class Robot extends TimedRobot {
* where distance refers to the units of distance
* that you are using, in this case inches.
*/
m_encoder.setMinRate(1.0);
encoder.setMinRate(1.0);
}
@Override
public void teleopPeriodic() {
SmartDashboard.putNumber("Encoder Distance", m_encoder.getDistance());
SmartDashboard.putNumber("Encoder Rate", m_encoder.getRate());
SmartDashboard.putNumber("Encoder Distance", encoder.getDistance());
SmartDashboard.putNumber("Encoder Rate", encoder.getRate());
}
}

28
wpilibjExamples/src/main/java/org/wpilib/examples/gettingstarted/Robot.java
View File

@@ -17,39 +17,39 @@ import org.wpilib.util.sendable.SendableRegistry;
* this project, you must also update the manifest file in the resource directory.
*/
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftDrive = new PWMSparkMax(0);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::setThrottle, m_rightDrive::setThrottle);
private final Gamepad m_controller = new Gamepad(0);
private final Timer m_timer = new Timer();
private final PWMSparkMax leftDrive = new PWMSparkMax(0);
private final PWMSparkMax rightDrive = new PWMSparkMax(1);
private final DifferentialDrive robotDrive =
new DifferentialDrive(leftDrive::setThrottle, rightDrive::setThrottle);
private final Gamepad controller = new Gamepad(0);
private final Timer timer = new Timer();
/** Called once at the beginning of the robot program. */
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftDrive);
SendableRegistry.addChild(m_robotDrive, m_rightDrive);
SendableRegistry.addChild(robotDrive, leftDrive);
SendableRegistry.addChild(robotDrive, rightDrive);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightDrive.setInverted(true);
rightDrive.setInverted(true);
}
/** This function is run once each time the robot enters autonomous mode. */
@Override
public void autonomousInit() {
m_timer.restart();
timer.restart();
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
// Drive for 2 seconds
if (m_timer.get() < 2.0) {
if (timer.get() < 2.0) {
// Drive forwards half velocity, make sure to turn input squaring off
m_robotDrive.arcadeDrive(0.5, 0.0, false);
robotDrive.arcadeDrive(0.5, 0.0, false);
} else {
m_robotDrive.stopMotor(); // stop robot
robotDrive.stopMotor(); // stop robot
}
}
@@ -60,7 +60,7 @@ public class Robot extends TimedRobot {
/** This function is called periodically during teleoperated mode. */
@Override
public void teleopPeriodic() {
m_robotDrive.arcadeDrive(-m_controller.getLeftY(), -m_controller.getRightX());
robotDrive.arcadeDrive(-controller.getLeftY(), -controller.getRightX());
}
/** This function is called once each time the robot enters utility mode. */

22
wpilibjExamples/src/main/java/org/wpilib/examples/gyro/Robot.java
View File

@@ -26,22 +26,22 @@ public class Robot extends TimedRobot {
OnboardIMU.MountOrientation.FLAT;
private static final int kJoystickPort = 0;
private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::setThrottle, m_rightDrive::setThrottle);
private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final PWMSparkMax leftDrive = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax rightDrive = new PWMSparkMax(kRightMotorPort);
private final DifferentialDrive robotDrive =
new DifferentialDrive(leftDrive::setThrottle, rightDrive::setThrottle);
private final OnboardIMU imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick joystick = new Joystick(kJoystickPort);
/** Called once at the beginning of the robot program. */
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftDrive);
SendableRegistry.addChild(m_robotDrive, m_rightDrive);
SendableRegistry.addChild(robotDrive, leftDrive);
SendableRegistry.addChild(robotDrive, rightDrive);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightDrive.setInverted(true);
rightDrive.setInverted(true);
}
/**
@@ -50,7 +50,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
double turningValue = (kAngleSetpoint - m_imu.getRotation2d().getDegrees()) * kP;
m_robotDrive.arcadeDrive(-m_joystick.getY(), -turningValue);
double turningValue = (kAngleSetpoint - imu.getRotation2d().getDegrees()) * kP;
robotDrive.arcadeDrive(-joystick.getY(), -turningValue);
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/Robot.java
View File

@@ -16,9 +16,9 @@ import org.wpilib.system.DataLogManager;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final RobotContainer m_robotContainer;
private final RobotContainer robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
@@ -27,7 +27,7 @@ public class Robot extends TimedRobot {
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
robotContainer = new RobotContainer();
// Start recording to data log
DataLogManager.start();
@@ -63,11 +63,11 @@ public class Robot extends TimedRobot {
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autonomousCommand = robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -81,8 +81,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

41
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/RobotContainer.java
View File

@@ -23,22 +23,22 @@ import org.wpilib.smartdashboard.SmartDashboard;
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
private final DriveSubsystem robotDrive = new DriveSubsystem();
private final HatchSubsystem hatchSubsystem = new HatchSubsystem();
// Retained command handles
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto = Autos.simpleAuto(m_robotDrive);
private final Command simpleAuto = Autos.simpleAuto(robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = Autos.complexAuto(m_robotDrive, m_hatchSubsystem);
private final Command complexAuto = Autos.complexAuto(robotDrive, hatchSubsystem);
// A chooser for autonomous commands
SendableChooser<Command> m_chooser = new SendableChooser<>();
SendableChooser<Command> chooser = new SendableChooser<>();
// The driver's controller
CommandGamepad m_driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
@@ -47,25 +47,24 @@ public class RobotContainer {
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
Commands.run(
() ->
m_robotDrive.arcadeDrive(
-m_driverController.getLeftY(), -m_driverController.getRightX()),
m_robotDrive));
robotDrive.arcadeDrive(-driverController.getLeftY(), -driverController.getRightX()),
robotDrive));
// Add commands to the autonomous command chooser
m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
m_chooser.addOption("Complex Auto", m_complexAuto);
chooser.setDefaultOption("Simple Auto", simpleAuto);
chooser.addOption("Complex Auto", complexAuto);
// Put the chooser on the dashboard
SmartDashboard.putData("Autonomous", m_chooser);
SmartDashboard.putData("Autonomous", chooser);
// Put subsystems to dashboard.
SmartDashboard.putData("Drivetrain", m_robotDrive);
SmartDashboard.putData("HatchSubsystem", m_hatchSubsystem);
SmartDashboard.putData("Drivetrain", robotDrive);
SmartDashboard.putData("HatchSubsystem", hatchSubsystem);
}
/**
@@ -76,14 +75,14 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
// Grab the hatch when the east face button is pressed.
m_driverController.eastFace().onTrue(m_hatchSubsystem.grabHatchCommand());
driverController.eastFace().onTrue(hatchSubsystem.grabHatchCommand());
// Release the hatch when the west face button is pressed.
m_driverController.westFace().onTrue(m_hatchSubsystem.releaseHatchCommand());
driverController.westFace().onTrue(hatchSubsystem.releaseHatchCommand());
// While holding right bumper, drive at half velocity
m_driverController
driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1)));
.onTrue(Commands.runOnce(() -> robotDrive.setMaxOutput(0.5)))
.onFalse(Commands.runOnce(() -> robotDrive.setMaxOutput(1)));
}
/**
@@ -92,6 +91,6 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
return chooser.getSelected();
}
}

44
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/subsystems/DriveSubsystem.java
View File

@@ -14,26 +14,26 @@ import org.wpilib.util.sendable.SendableRegistry;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
private final PWMSparkMax rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setThrottle, m_rightLeader::setThrottle);
private final DifferentialDrive drive =
new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
private final Encoder leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
private final Encoder rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
@@ -41,20 +41,20 @@ public class DriveSubsystem extends SubsystemBase {
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
SendableRegistry.addChild(drive, leftLeader);
SendableRegistry.addChild(drive, rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
@@ -64,13 +64,13 @@ public class DriveSubsystem extends SubsystemBase {
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
}
/**
@@ -79,7 +79,7 @@ public class DriveSubsystem extends SubsystemBase {
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2.0;
}
/**
@@ -88,14 +88,14 @@ public class DriveSubsystem extends SubsystemBase {
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
drive.setMaxOutput(maxOutput);
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
// Publish encoder distances to telemetry.
builder.addDoubleProperty("leftDistance", m_leftEncoder::getDistance, null);
builder.addDoubleProperty("rightDistance", m_rightEncoder::getDistance, null);
builder.addDoubleProperty("leftDistance", leftEncoder::getDistance, null);
builder.addDoubleProperty("rightDistance", rightEncoder::getDistance, null);
}
}

8
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/subsystems/HatchSubsystem.java
View File

@@ -16,7 +16,7 @@ import org.wpilib.util.sendable.SendableBuilder;
/** A hatch mechanism actuated by a single {@link org.wpilib.hardware.pneumatic.DoubleSolenoid}. */
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
private final DoubleSolenoid hatchSolenoid =
new DoubleSolenoid(
0,
PneumaticsModuleType.CTRE_PCM,
@@ -26,19 +26,19 @@ public class HatchSubsystem extends SubsystemBase {
/** Grabs the hatch. */
public Command grabHatchCommand() {
// implicitly require `this`
return this.runOnce(() -> m_hatchSolenoid.set(FORWARD));
return this.runOnce(() -> hatchSolenoid.set(FORWARD));
}
/** Releases the hatch. */
public Command releaseHatchCommand() {
// implicitly require `this`
return this.runOnce(() -> m_hatchSolenoid.set(REVERSE));
return this.runOnce(() -> hatchSolenoid.set(REVERSE));
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
// Publish the solenoid state to telemetry.
builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == FORWARD, null);
builder.addBooleanProperty("extended", () -> hatchSolenoid.get() == FORWARD, null);
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/Robot.java
View File

@@ -16,9 +16,9 @@ import org.wpilib.system.DataLogManager;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final RobotContainer m_robotContainer;
private final RobotContainer robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
@@ -27,7 +27,7 @@ public class Robot extends TimedRobot {
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
robotContainer = new RobotContainer();
// Start recording to data log
DataLogManager.start();
@@ -63,7 +63,7 @@ public class Robot extends TimedRobot {
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autonomousCommand = robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -73,8 +73,8 @@ public class Robot extends TimedRobot {
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -88,8 +88,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

42
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/RobotContainer.java
View File

@@ -30,24 +30,24 @@ import org.wpilib.smartdashboard.SmartDashboard;
*/
public class RobotContainer {
// The robot's subsystems
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
private final DriveSubsystem robotDrive = new DriveSubsystem();
private final HatchSubsystem hatchSubsystem = new HatchSubsystem();
// The autonomous routines
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
private final Command simpleAuto =
new DriveDistance(
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveVelocity, m_robotDrive);
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveVelocity, robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
private final Command complexAuto = new ComplexAuto(robotDrive, hatchSubsystem);
// A chooser for autonomous commands
SendableChooser<Command> m_chooser = new SendableChooser<>();
SendableChooser<Command> chooser = new SendableChooser<>();
// The driver's controller
Gamepad m_driverController = new Gamepad(OIConstants.kDriverControllerPort);
Gamepad driverController = new Gamepad(OIConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
@@ -56,23 +56,21 @@ public class RobotContainer {
// Configure default commands
// Set the default drive command to split-stick arcade drive
m_robotDrive.setDefaultCommand(
robotDrive.setDefaultCommand(
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new DefaultDrive(
m_robotDrive,
() -> -m_driverController.getLeftY(),
() -> -m_driverController.getRightX()));
robotDrive, () -> -driverController.getLeftY(), () -> -driverController.getRightX()));
// Add commands to the autonomous command chooser
m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
m_chooser.addOption("Complex Auto", m_complexAuto);
chooser.setDefaultOption("Simple Auto", simpleAuto);
chooser.addOption("Complex Auto", complexAuto);
// Put the chooser on the dashboard
SmartDashboard.putData("Autonomous", m_chooser);
SmartDashboard.putData("Autonomous", chooser);
// Put subsystems to dashboard.
SmartDashboard.putData("Drivetrain", m_robotDrive);
SmartDashboard.putData("HatchSubsystem", m_hatchSubsystem);
SmartDashboard.putData("Drivetrain", robotDrive);
SmartDashboard.putData("HatchSubsystem", hatchSubsystem);
}
/**
@@ -83,14 +81,12 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
// Grab the hatch when the 'South Face' button is pressed.
new GamepadButton(m_driverController, Button.SOUTH_FACE)
.onTrue(new GrabHatch(m_hatchSubsystem));
new GamepadButton(driverController, Button.SOUTH_FACE).onTrue(new GrabHatch(hatchSubsystem));
// Release the hatch when the 'East Face' button is pressed.
new GamepadButton(m_driverController, Button.EAST_FACE)
.onTrue(new ReleaseHatch(m_hatchSubsystem));
new GamepadButton(driverController, Button.EAST_FACE).onTrue(new ReleaseHatch(hatchSubsystem));
// While holding the bumper button, drive at half velocity
new GamepadButton(m_driverController, Button.RIGHT_BUMPER)
.whileTrue(new HalveDriveVelocity(m_robotDrive));
new GamepadButton(driverController, Button.RIGHT_BUMPER)
.whileTrue(new HalveDriveVelocity(robotDrive));
}
/**
@@ -99,6 +95,6 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
return chooser.getSelected();
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/DefaultDrive.java
View File

@@ -14,9 +14,9 @@ import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
* org.wpilib.command2.RunCommand}.
*/
public class DefaultDrive extends Command {
private final DriveSubsystem m_drive;
private final DoubleSupplier m_forward;
private final DoubleSupplier m_rotation;
private final DriveSubsystem drive;
private final DoubleSupplier forward;
private final DoubleSupplier rotation;
/**
* Creates a new DefaultDrive.
@@ -26,14 +26,14 @@ public class DefaultDrive extends Command {
* @param rotation The control input for turning
*/
public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) {
m_drive = subsystem;
m_forward = forward;
m_rotation = rotation;
addRequirements(m_drive);
drive = subsystem;
this.forward = forward;
this.rotation = rotation;
addRequirements(drive);
}
@Override
public void execute() {
m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble());
drive.arcadeDrive(forward.getAsDouble(), rotation.getAsDouble());
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/DriveDistance.java
View File

@@ -8,9 +8,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class DriveDistance extends Command {
private final DriveSubsystem m_drive;
private final double m_distance;
private final double m_velocity;
private final DriveSubsystem drive;
private final double distance;
private final double velocity;
/**
* Creates a new DriveDistance.
@@ -20,30 +20,30 @@ public class DriveDistance extends Command {
* @param drive The drive subsystem on which this command will run
*/
public DriveDistance(double inches, double velocity, DriveSubsystem drive) {
m_distance = inches;
m_velocity = velocity;
m_drive = drive;
addRequirements(m_drive);
distance = inches;
this.velocity = velocity;
this.drive = drive;
addRequirements(drive);
}
@Override
public void initialize() {
m_drive.resetEncoders();
m_drive.arcadeDrive(m_velocity, 0);
drive.resetEncoders();
drive.arcadeDrive(velocity, 0);
}
@Override
public void execute() {
m_drive.arcadeDrive(m_velocity, 0);
drive.arcadeDrive(velocity, 0);
}
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
@Override
public boolean isFinished() {
return Math.abs(m_drive.getAverageEncoderDistance()) >= m_distance;
return Math.abs(drive.getAverageEncoderDistance()) >= distance;
}
}

8
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/GrabHatch.java
View File

@@ -14,16 +14,16 @@ import org.wpilib.examples.hatchbottraditional.subsystems.HatchSubsystem;
*/
public class GrabHatch extends Command {
// The subsystem the command runs on
private final HatchSubsystem m_hatchSubsystem;
private final HatchSubsystem hatchSubsystem;
public GrabHatch(HatchSubsystem subsystem) {
m_hatchSubsystem = subsystem;
addRequirements(m_hatchSubsystem);
hatchSubsystem = subsystem;
addRequirements(hatchSubsystem);
}
@Override
public void initialize() {
m_hatchSubsystem.grabHatch();
hatchSubsystem.grabHatch();
}
@Override

8
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/HalveDriveVelocity.java
View File

@@ -8,19 +8,19 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class HalveDriveVelocity extends Command {
private final DriveSubsystem m_drive;
private final DriveSubsystem drive;
public HalveDriveVelocity(DriveSubsystem drive) {
m_drive = drive;
this.drive = drive;
}
@Override
public void initialize() {
m_drive.setMaxOutput(0.5);
drive.setMaxOutput(0.5);
}
@Override
public void end(boolean interrupted) {
m_drive.setMaxOutput(1);
drive.setMaxOutput(1);
}
}

44
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/subsystems/DriveSubsystem.java
View File

@@ -14,26 +14,26 @@ import org.wpilib.util.sendable.SendableRegistry;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
private final PWMSparkMax rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setThrottle, m_rightLeader::setThrottle);
private final DifferentialDrive drive =
new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
private final Encoder leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
private final Encoder rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
@@ -41,20 +41,20 @@ public class DriveSubsystem extends SubsystemBase {
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
SendableRegistry.addChild(drive, leftLeader);
SendableRegistry.addChild(drive, rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
@@ -64,13 +64,13 @@ public class DriveSubsystem extends SubsystemBase {
* @param rot the commanded rotation
*/
public void arcadeDrive(double fwd, double rot) {
m_drive.arcadeDrive(fwd, rot);
drive.arcadeDrive(fwd, rot);
}
/** Resets the drive encoders to currently read a position of 0. */
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
}
/**
@@ -79,7 +79,7 @@ public class DriveSubsystem extends SubsystemBase {
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2.0;
}
/**
@@ -88,14 +88,14 @@ public class DriveSubsystem extends SubsystemBase {
* @param maxOutput the maximum output to which the drive will be constrained
*/
public void setMaxOutput(double maxOutput) {
m_drive.setMaxOutput(maxOutput);
drive.setMaxOutput(maxOutput);
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
// Publish encoder distances to telemetry.
builder.addDoubleProperty("leftDistance", m_leftEncoder::getDistance, null);
builder.addDoubleProperty("rightDistance", m_rightEncoder::getDistance, null);
builder.addDoubleProperty("leftDistance", leftEncoder::getDistance, null);
builder.addDoubleProperty("rightDistance", rightEncoder::getDistance, null);
}
}

8
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/subsystems/HatchSubsystem.java
View File

@@ -15,7 +15,7 @@ import org.wpilib.util.sendable.SendableBuilder;
/** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */
public class HatchSubsystem extends SubsystemBase {
private final DoubleSolenoid m_hatchSolenoid =
private final DoubleSolenoid hatchSolenoid =
new DoubleSolenoid(
0,
PneumaticsModuleType.CTRE_PCM,
@@ -24,18 +24,18 @@ public class HatchSubsystem extends SubsystemBase {
/** Grabs the hatch. */
public void grabHatch() {
m_hatchSolenoid.set(FORWARD);
hatchSolenoid.set(FORWARD);
}
/** Releases the hatch. */
public void releaseHatch() {
m_hatchSolenoid.set(REVERSE);
hatchSolenoid.set(REVERSE);
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
// Publish the solenoid state to telemetry.
builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == FORWARD, null);
builder.addBooleanProperty("extended", () -> hatchSolenoid.get() == FORWARD, null);
}
}

96
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Drivetrain.java
View File

@@ -21,46 +21,46 @@ public class Drivetrain {
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
private final PWMSparkMax frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax frontRightMotor = new PWMSparkMax(2);
private final PWMSparkMax backLeftMotor = new PWMSparkMax(3);
private final PWMSparkMax backRightMotor = new PWMSparkMax(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
private final Encoder m_backLeftEncoder = new Encoder(4, 5);
private final Encoder m_backRightEncoder = new Encoder(6, 7);
private final Encoder frontLeftEncoder = new Encoder(0, 1);
private final Encoder frontRightEncoder = new Encoder(2, 3);
private final Encoder backLeftEncoder = new Encoder(4, 5);
private final Encoder backRightEncoder = new Encoder(6, 7);
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
private final Translation2d frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d backRightLocation = new Translation2d(-0.381, -0.381);
private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
private final PIDController frontLeftPIDController = new PIDController(1, 0, 0);
private final PIDController frontRightPIDController = new PIDController(1, 0, 0);
private final PIDController backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController backRightPIDController = new PIDController(1, 0, 0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final MecanumDriveKinematics m_kinematics =
private final MecanumDriveKinematics kinematics =
new MecanumDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation);
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, m_imu.getRotation2d(), getCurrentWheelDistances());
private final MecanumDriveOdometry odometry =
new MecanumDriveOdometry(kinematics, imu.getRotation2d(), getCurrentWheelDistances());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
/** Constructs a MecanumDrive and resets the gyro. */
public Drivetrain() {
m_imu.resetYaw();
imu.resetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_frontRightMotor.setInverted(true);
m_backRightMotor.setInverted(true);
frontRightMotor.setInverted(true);
backRightMotor.setInverted(true);
}
/**
@@ -70,10 +70,10 @@ public class Drivetrain {
*/
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
frontLeftEncoder.getDistance(),
frontRightEncoder.getDistance(),
backLeftEncoder.getDistance(),
backRightEncoder.getDistance());
}
/**
@@ -83,10 +83,10 @@ public class Drivetrain {
*/
public MecanumDriveWheelVelocities getCurrentWheelVelocities() {
return new MecanumDriveWheelVelocities(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
frontLeftEncoder.getRate(),
frontRightEncoder.getRate(),
backLeftEncoder.getRate(),
backRightEncoder.getRate());
}
/**
@@ -95,24 +95,24 @@ public class Drivetrain {
* @param velocities The desired wheel velocities.
*/
public void setVelocities(MecanumDriveWheelVelocities velocities) {
final double frontLeftFeedforward = m_feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(velocities.rearRight);
final double frontLeftFeedforward = feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = feedforward.calculate(velocities.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), velocities.frontLeft);
frontLeftPIDController.calculate(frontLeftEncoder.getRate(), velocities.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), velocities.frontRight);
frontRightPIDController.calculate(frontRightEncoder.getRate(), velocities.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), velocities.rearLeft);
backLeftPIDController.calculate(backLeftEncoder.getRate(), velocities.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), velocities.rearRight);
backRightPIDController.calculate(backRightEncoder.getRate(), velocities.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
}
/**
@@ -127,16 +127,16 @@ public class Drivetrain {
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisVelocities = chassisVelocities.toRobotRelative(m_imu.getRotation2d());
chassisVelocities = chassisVelocities.toRobotRelative(imu.getRotation2d());
}
setVelocities(
m_kinematics
kinematics
.toWheelVelocities(chassisVelocities.discretize(period))
.desaturate(kMaxVelocity));
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_imu.getRotation2d(), getCurrentWheelDistances());
odometry.update(imu.getRotation2d(), getCurrentWheelDistances());
}
}

21
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Robot.java
View File

@@ -9,18 +9,18 @@ import org.wpilib.framework.TimedRobot;
import org.wpilib.math.filter.SlewRateLimiter;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_mecanum = new Drivetrain();
private final Gamepad controller = new Gamepad(0);
private final Drivetrain mecanum = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
driveWithJoystick(false);
m_mecanum.updateOdometry();
mecanum.updateOdometry();
}
@Override
@@ -32,21 +32,20 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
-xVelocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
-yVelocityLimiter.calculate(controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
final var rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

20
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdrive/Robot.java
View File

@@ -24,9 +24,9 @@ public class Robot extends TimedRobot {
OnboardIMU.MountOrientation.FLAT;
private static final int kJoystickPort = 0;
private final MecanumDrive m_robotDrive;
private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final MecanumDrive robotDrive;
private final OnboardIMU imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick joystick = new Joystick(kJoystickPort);
/** Called once at the beginning of the robot program. */
public Robot() {
@@ -40,17 +40,17 @@ public class Robot extends TimedRobot {
frontRight.setInverted(true);
rearRight.setInverted(true);
m_robotDrive =
robotDrive =
new MecanumDrive(
frontLeft::setThrottle,
rearLeft::setThrottle,
frontRight::setThrottle,
rearRight::setThrottle);
SendableRegistry.addChild(m_robotDrive, frontLeft);
SendableRegistry.addChild(m_robotDrive, rearLeft);
SendableRegistry.addChild(m_robotDrive, frontRight);
SendableRegistry.addChild(m_robotDrive, rearRight);
SendableRegistry.addChild(robotDrive, frontLeft);
SendableRegistry.addChild(robotDrive, rearLeft);
SendableRegistry.addChild(robotDrive, frontRight);
SendableRegistry.addChild(robotDrive, rearRight);
}
/** Mecanum drive is used with the gyro angle as an input. */
@@ -58,7 +58,7 @@ public class Robot extends TimedRobot {
public void teleopPeriodic() {
// Use the joystick Y axis for forward movement, X axis for lateral
// movement, and Z axis for rotation.
m_robotDrive.driveCartesian(
-m_joystick.getY(), -m_joystick.getX(), -m_joystick.getZ(), m_imu.getRotation2d());
robotDrive.driveCartesian(
-joystick.getY(), -joystick.getX(), -joystick.getZ(), imu.getRotation2d());
}
}

103
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -25,54 +25,54 @@ public class Drivetrain {
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
private final PWMSparkMax m_backLeftMotor = new PWMSparkMax(3);
private final PWMSparkMax m_backRightMotor = new PWMSparkMax(4);
private final PWMSparkMax frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax frontRightMotor = new PWMSparkMax(2);
private final PWMSparkMax backLeftMotor = new PWMSparkMax(3);
private final PWMSparkMax backRightMotor = new PWMSparkMax(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
private final Encoder m_frontRightEncoder = new Encoder(2, 3);
private final Encoder m_backLeftEncoder = new Encoder(4, 5);
private final Encoder m_backRightEncoder = new Encoder(6, 7);
private final Encoder frontLeftEncoder = new Encoder(0, 1);
private final Encoder frontRightEncoder = new Encoder(2, 3);
private final Encoder backLeftEncoder = new Encoder(4, 5);
private final Encoder backRightEncoder = new Encoder(6, 7);
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
private final Translation2d frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d backRightLocation = new Translation2d(-0.381, -0.381);
private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
private final PIDController frontLeftPIDController = new PIDController(1, 0, 0);
private final PIDController frontRightPIDController = new PIDController(1, 0, 0);
private final PIDController backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController backRightPIDController = new PIDController(1, 0, 0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final MecanumDriveKinematics m_kinematics =
private final MecanumDriveKinematics kinematics =
new MecanumDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation);
/* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private final MecanumDrivePoseEstimator m_poseEstimator =
private final MecanumDrivePoseEstimator poseEstimator =
new MecanumDrivePoseEstimator(
m_kinematics,
m_imu.getRotation2d(),
kinematics,
imu.getRotation2d(),
getCurrentWheelDistances(),
Pose2d.kZero,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
/** Constructs a MecanumDrive and resets the gyro. */
public Drivetrain() {
m_imu.resetYaw();
imu.resetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_frontRightMotor.setInverted(true);
m_backRightMotor.setInverted(true);
frontRightMotor.setInverted(true);
backRightMotor.setInverted(true);
}
/**
@@ -82,10 +82,10 @@ public class Drivetrain {
*/
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
m_backLeftEncoder.getDistance(),
m_backRightEncoder.getDistance());
frontLeftEncoder.getDistance(),
frontRightEncoder.getDistance(),
backLeftEncoder.getDistance(),
backRightEncoder.getDistance());
}
/**
@@ -95,10 +95,10 @@ public class Drivetrain {
*/
public MecanumDriveWheelVelocities getCurrentWheelVelocities() {
return new MecanumDriveWheelVelocities(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
frontLeftEncoder.getRate(),
frontRightEncoder.getRate(),
backLeftEncoder.getRate(),
backRightEncoder.getRate());
}
/**
@@ -107,24 +107,24 @@ public class Drivetrain {
* @param velocities The desired wheel velocities.
*/
public void setVelocities(MecanumDriveWheelVelocities velocities) {
final double frontLeftFeedforward = m_feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(velocities.rearRight);
final double frontLeftFeedforward = feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = feedforward.calculate(velocities.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), velocities.frontLeft);
frontLeftPIDController.calculate(frontLeftEncoder.getRate(), velocities.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), velocities.frontRight);
frontRightPIDController.calculate(frontRightEncoder.getRate(), velocities.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), velocities.rearLeft);
backLeftPIDController.calculate(backLeftEncoder.getRate(), velocities.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), velocities.rearRight);
backRightPIDController.calculate(backRightEncoder.getRate(), velocities.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
}
/**
@@ -140,23 +140,22 @@ public class Drivetrain {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisVelocities =
chassisVelocities.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
chassisVelocities.toRobotRelative(poseEstimator.getEstimatedPosition().getRotation());
}
setVelocities(
m_kinematics
kinematics
.toWheelVelocities(chassisVelocities.discretize(period))
.desaturate(kMaxVelocity));
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(m_imu.getRotation2d(), getCurrentWheelDistances());
poseEstimator.update(imu.getRotation2d(), getCurrentWheelDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
m_poseEstimator.getEstimatedPosition()),
poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(poseEstimator.getEstimatedPosition()),
Timer.getTimestamp() - 0.3);
}
}

21
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/Robot.java
View File

@@ -9,18 +9,18 @@ import org.wpilib.framework.TimedRobot;
import org.wpilib.math.filter.SlewRateLimiter;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_mecanum = new Drivetrain();
private final Gamepad controller = new Gamepad(0);
private final Drivetrain mecanum = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
driveWithJoystick(false);
m_mecanum.updateOdometry();
mecanum.updateOdometry();
}
@Override
@@ -32,21 +32,20 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
-xVelocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
-yVelocityLimiter.calculate(controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
final var rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

31
wpilibjExamples/src/main/java/org/wpilib/examples/mechanism2d/Robot.java
View File

@@ -27,18 +27,18 @@ public class Robot extends TimedRobot {
private static final double kMetersPerPulse = 0.01;
private static final double kElevatorMinimumLength = 0.5;
private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(0);
private final PWMSparkMax m_wristMotor = new PWMSparkMax(1);
private final AnalogPotentiometer m_wristPot = new AnalogPotentiometer(1, 90);
private final Encoder m_elevatorEncoder = new Encoder(0, 1);
private final Joystick m_joystick = new Joystick(0);
private final PWMSparkMax elevatorMotor = new PWMSparkMax(0);
private final PWMSparkMax wristMotor = new PWMSparkMax(1);
private final AnalogPotentiometer wristPot = new AnalogPotentiometer(1, 90);
private final Encoder elevatorEncoder = new Encoder(0, 1);
private final Joystick joystick = new Joystick(0);
private final MechanismLigament2d m_elevator;
private final MechanismLigament2d m_wrist;
private final MechanismLigament2d elevator;
private final MechanismLigament2d wrist;
/** Called once at the beginning of the robot program. */
public Robot() {
m_elevatorEncoder.setDistancePerPulse(kMetersPerPulse);
elevatorEncoder.setDistancePerPulse(kMetersPerPulse);
// the main mechanism object
Mechanism2d mech = new Mechanism2d(3, 3);
@@ -47,10 +47,9 @@ public class Robot extends TimedRobot {
// MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based
// off the root node or another ligament object
m_elevator = root.append(new MechanismLigament2d("elevator", kElevatorMinimumLength, 90));
m_wrist =
m_elevator.append(
new MechanismLigament2d("wrist", 0.5, 90, 6, new Color8Bit(Color.PURPLE)));
elevator = root.append(new MechanismLigament2d("elevator", kElevatorMinimumLength, 90));
wrist =
elevator.append(new MechanismLigament2d("wrist", 0.5, 90, 6, new Color8Bit(Color.PURPLE)));
// post the mechanism to the dashboard
SmartDashboard.putData("Mech2d", mech);
@@ -59,13 +58,13 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
// update the dashboard mechanism's state
m_elevator.setLength(kElevatorMinimumLength + m_elevatorEncoder.getDistance());
m_wrist.setAngle(m_wristPot.get());
elevator.setLength(kElevatorMinimumLength + elevatorEncoder.getDistance());
wrist.setAngle(wristPot.get());
}
@Override
public void teleopPeriodic() {
m_elevatorMotor.setThrottle(m_joystick.getRawAxis(0));
m_wristMotor.setThrottle(m_joystick.getRawAxis(1));
elevatorMotor.setThrottle(joystick.getRawAxis(0));
wristMotor.setThrottle(joystick.getRawAxis(1));
}
}

36
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/RapidReactCommandBot.java
View File

@@ -28,14 +28,14 @@ import org.wpilib.examples.rapidreactcommandbot.subsystems.Storage;
@Logged(name = "Rapid React Command Robot Container")
public class RapidReactCommandBot {
// The robot's subsystems
private final Drive m_drive = new Drive();
private final Intake m_intake = new Intake();
private final Storage m_storage = new Storage();
private final Shooter m_shooter = new Shooter();
private final Pneumatics m_pneumatics = new Pneumatics();
private final Drive drive = new Drive();
private final Intake intake = new Intake();
private final Storage storage = new Storage();
private final Shooter shooter = new Shooter();
private final Pneumatics pneumatics = new Pneumatics();
// The driver's controller
CommandGamepad m_driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
/**
* Use this method to define bindings between conditions and commands. These are useful for
@@ -49,33 +49,31 @@ public class RapidReactCommandBot {
// Automatically run the storage motor whenever the ball storage is not full,
// and turn it off whenever it fills. Uses subsystem-hosted trigger to
// improve readability and make inter-subsystem communication easier.
m_storage.hasCargo.whileFalse(m_storage.runCommand());
storage.hasCargo.whileFalse(storage.runCommand());
// Automatically disable and retract the intake whenever the ball storage is full.
m_storage.hasCargo.onTrue(m_intake.retractCommand());
storage.hasCargo.onTrue(intake.retractCommand());
// Control the drive with split-stick arcade controls
m_drive.setDefaultCommand(
m_drive.arcadeDriveCommand(
() -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
drive.setDefaultCommand(
drive.arcadeDriveCommand(
() -> -driverController.getLeftY(), () -> -driverController.getRightX()));
// Deploy the intake with the west face button
m_driverController.westFace().onTrue(m_intake.intakeCommand());
driverController.westFace().onTrue(intake.intakeCommand());
// Retract the intake with the north face button
m_driverController.northFace().onTrue(m_intake.retractCommand());
driverController.northFace().onTrue(intake.retractCommand());
// Fire the shooter with the south face button
m_driverController
driverController
.southFace()
.onTrue(
parallel(
m_shooter.shootCommand(ShooterConstants.kShooterTargetRPS),
m_storage.runCommand())
parallel(shooter.shootCommand(ShooterConstants.kShooterTargetRPS), storage.runCommand())
// Since we composed this inline we should give it a name
.withName("Shoot"));
// Toggle compressor with the Start button
m_driverController.start().toggleOnTrue(m_pneumatics.disableCompressorCommand());
driverController.start().toggleOnTrue(pneumatics.disableCompressorCommand());
}
/**
@@ -85,7 +83,7 @@ public class RapidReactCommandBot {
*/
public Command getAutonomousCommand() {
// Drive forward for 2 meters at half velocity with a 3 second timeout
return m_drive
return drive
.driveDistanceCommand(AutoConstants.kDriveDistance, AutoConstants.kDriveVelocity)
.withTimeout(AutoConstants.kTimeout);
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/Robot.java
View File

@@ -18,9 +18,9 @@ import org.wpilib.system.DataLogManager;
*/
@Logged(name = "Rapid React Command Robot")
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final RapidReactCommandBot m_robot = new RapidReactCommandBot();
private final RapidReactCommandBot robot = new RapidReactCommandBot();
/**
* This function is run when the robot is first started up and should be used for any
@@ -28,7 +28,7 @@ public class Robot extends TimedRobot {
*/
public Robot() {
// Configure default commands and condition bindings on robot startup
m_robot.configureBindings();
robot.configureBindings();
// Initialize data logging.
DataLogManager.start();
@@ -60,10 +60,10 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
m_autonomousCommand = m_robot.getAutonomousCommand();
autonomousCommand = robot.getAutonomousCommand();
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -77,8 +77,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

65
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -23,34 +23,34 @@ import org.wpilib.util.sendable.SendableRegistry;
@Logged
public class Drive extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
private final PWMSparkMax rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
// The robot's drive
@NotLogged // Would duplicate motor data, there's no point sending it twice
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setThrottle, m_rightLeader::setThrottle);
private final DifferentialDrive drive =
new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
private final Encoder leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
private final Encoder rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final ProfiledPIDController m_controller =
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final ProfiledPIDController controller =
new ProfiledPIDController(
DriveConstants.kTurnP,
DriveConstants.kTurnI,
@@ -58,31 +58,31 @@ public class Drive extends SubsystemBase {
new TrapezoidProfile.Constraints(
DriveConstants.kMaxTurnRateDegPerS,
DriveConstants.kMaxTurnAccelerationDegPerSSquared));
private final SimpleMotorFeedforward m_feedforward =
private final SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
/** Creates a new Drive subsystem. */
public Drive() {
SendableRegistry.addChild(m_drive, m_leftLeader);
SendableRegistry.addChild(m_drive, m_rightLeader);
SendableRegistry.addChild(drive, leftLeader);
SendableRegistry.addChild(drive, rightLeader);
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
// Set the controller to be continuous (because it is an angle controller)
m_controller.enableContinuousInput(-180, 180);
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.setTolerance(
controller.setTolerance(
DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
}
@@ -95,7 +95,7 @@ public class Drive extends SubsystemBase {
public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
return run(() -> drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
.withName("arcadeDrive");
}
@@ -109,16 +109,15 @@ public class Drive extends SubsystemBase {
return runOnce(
() -> {
// Reset encoders at the start of the command
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
})
// Drive forward at specified velocity
.andThen(run(() -> m_drive.arcadeDrive(velocity, 0)))
.andThen(run(() -> drive.arcadeDrive(velocity, 0)))
// End command when we've traveled the specified distance
.until(
() -> Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) >= distance)
.until(() -> Math.max(leftEncoder.getDistance(), rightEncoder.getDistance()) >= distance)
// Stop the drive when the command ends
.finallyDo(interrupted -> m_drive.stopMotor());
.finallyDo(interrupted -> drive.stopMotor());
}
/**
@@ -129,15 +128,15 @@ public class Drive extends SubsystemBase {
*/
public Command turnToAngleCommand(double angleDeg) {
return startRun(
() -> m_controller.reset(m_imu.getRotation2d().getDegrees()),
() -> controller.reset(imu.getRotation2d().getDegrees()),
() ->
m_drive.arcadeDrive(
drive.arcadeDrive(
0,
m_controller.calculate(m_imu.getRotation2d().getDegrees(), angleDeg)
controller.calculate(imu.getRotation2d().getDegrees(), angleDeg)
// Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
+ m_feedforward.calculate(m_controller.getSetpoint().velocity)
+ feedforward.calculate(controller.getSetpoint().velocity)
/ RobotController.getBatteryVoltage()))
.until(m_controller::atGoal)
.finallyDo(() -> m_drive.arcadeDrive(0, 0));
.until(controller::atGoal)
.finallyDo(() -> drive.arcadeDrive(0, 0));
}
}

12
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Intake.java
View File

@@ -16,10 +16,10 @@ import org.wpilib.hardware.pneumatic.PneumaticsModuleType;
@Logged
public class Intake extends SubsystemBase {
private final PWMSparkMax m_motor = new PWMSparkMax(IntakeConstants.kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(IntakeConstants.kMotorPort);
// Double solenoid connected to two channels of a PCM with the default CAN ID
private final DoubleSolenoid m_pistons =
private final DoubleSolenoid pistons =
new DoubleSolenoid(
0,
PneumaticsModuleType.CTRE_PCM,
@@ -28,8 +28,8 @@ public class Intake extends SubsystemBase {
/** Returns a command that deploys the intake, and then runs the intake motor indefinitely. */
public Command intakeCommand() {
return runOnce(() -> m_pistons.set(DoubleSolenoid.Value.FORWARD))
.andThen(run(() -> m_motor.setThrottle(1.0)))
return runOnce(() -> pistons.set(DoubleSolenoid.Value.FORWARD))
.andThen(run(() -> motor.setThrottle(1.0)))
.withName("Intake");
}
@@ -37,8 +37,8 @@ public class Intake extends SubsystemBase {
public Command retractCommand() {
return runOnce(
() -> {
m_motor.disable();
m_pistons.set(DoubleSolenoid.Value.REVERSE);
motor.disable();
pistons.set(DoubleSolenoid.Value.REVERSE);
})
.withName("Retract");
}

10
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Pneumatics.java
View File

@@ -21,11 +21,11 @@ public class Pneumatics extends SubsystemBase {
// so if r is the raw AnalogPotentiometer output, the pressure is 250r-25
static final double kScale = 250;
static final double kOffset = -25;
private final AnalogPotentiometer m_pressureTransducer =
private final AnalogPotentiometer pressureTransducer =
new AnalogPotentiometer(/* the AnalogIn port*/ 2, kScale, kOffset);
// Compressor connected to a PCM with a default CAN ID (0)
private final Compressor m_compressor = new Compressor(0, PneumaticsModuleType.CTRE_PCM);
private final Compressor compressor = new Compressor(0, PneumaticsModuleType.CTRE_PCM);
/**
* Query the analog pressure sensor.
@@ -34,7 +34,7 @@ public class Pneumatics extends SubsystemBase {
*/
public double getPressure() {
// Get the pressure (in PSI) from an analog pressure sensor connected to the RIO.
return m_pressureTransducer.get();
return pressureTransducer.get();
}
/**
@@ -47,11 +47,11 @@ public class Pneumatics extends SubsystemBase {
public Command disableCompressorCommand() {
return startEnd(
// Disable closed-loop mode on the compressor.
m_compressor::disable,
compressor::disable,
// Enable closed-loop mode based on the digital pressure switch connected to the
// PCM/PH.
// The switch is open when the pressure is over ~120 PSI.
m_compressor::enableDigital)
compressor::enableDigital)
.withName("Compressor Disabled");
}
}

28
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -18,28 +18,28 @@ import org.wpilib.math.controller.SimpleMotorFeedforward;
@Logged
public class Shooter extends SubsystemBase {
private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final Encoder m_shooterEncoder =
private final PWMSparkMax shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
private final PWMSparkMax feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final Encoder shooterEncoder =
new Encoder(
ShooterConstants.kEncoderPorts[0],
ShooterConstants.kEncoderPorts[1],
ShooterConstants.kEncoderReversed);
private final SimpleMotorFeedforward m_shooterFeedforward =
private final SimpleMotorFeedforward shooterFeedforward =
new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV);
private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);
private final PIDController shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);
/** The shooter subsystem for the robot. */
public Shooter() {
m_shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS);
shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
// Set default command to turn off both the shooter and feeder motors, and then idle
setDefaultCommand(
runOnce(
() -> {
m_shooterMotor.disable();
m_feederMotor.disable();
shooterMotor.disable();
feederMotor.disable();
})
.andThen(run(() -> {}))
.withName("Idle"));
@@ -56,14 +56,14 @@ public class Shooter extends SubsystemBase {
// Run the shooter flywheel at the desired setpoint using feedforward and feedback
run(
() -> {
m_shooterMotor.setThrottle(
m_shooterFeedforward.calculate(setpointRotationsPerSecond)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond));
shooterMotor.setThrottle(
shooterFeedforward.calculate(setpointRotationsPerSecond)
+ shooterFeedback.calculate(
shooterEncoder.getRate(), setpointRotationsPerSecond));
}),
// Wait until the shooter has reached the setpoint, and then run the feeder
waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.setThrottle(1)))
waitUntil(shooterFeedback::atSetpoint).andThen(() -> feederMotor.setThrottle(1)))
.withName("Shoot");
}
}

11
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Storage.java
View File

@@ -15,25 +15,24 @@ import org.wpilib.hardware.motor.PWMSparkMax;
@Logged
public class Storage extends SubsystemBase {
private final PWMSparkMax m_motor = new PWMSparkMax(StorageConstants.kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(StorageConstants.kMotorPort);
@NotLogged // We'll log a more meaningful boolean instead
private final DigitalInput m_ballSensor = new DigitalInput(StorageConstants.kBallSensorPort);
private final DigitalInput ballSensor = new DigitalInput(StorageConstants.kBallSensorPort);
// Expose trigger from subsystem to improve readability and ease
// inter-subsystem communications
/** Whether the ball storage is full. */
@Logged(name = "Has Cargo")
@SuppressWarnings("checkstyle:MemberName")
public final Trigger hasCargo = new Trigger(m_ballSensor::get);
public final Trigger hasCargo = new Trigger(ballSensor::get);
/** Create a new Storage subsystem. */
public Storage() {
// Set default command to turn off the storage motor and then idle
setDefaultCommand(runOnce(m_motor::disable).andThen(run(() -> {})).withName("Idle"));
setDefaultCommand(runOnce(motor::disable).andThen(run(() -> {})).withName("Idle"));
}
/** Returns a command that runs the storage motor indefinitely. */
public Command runCommand() {
return run(() -> m_motor.setThrottle(1)).withName("run");
return run(() -> motor.setThrottle(1)).withName("run");
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/Robot.java
View File

@@ -14,8 +14,8 @@ import org.wpilib.framework.TimedRobot;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private final RobotContainer m_robotContainer;
private Command autonomousCommand;
private final RobotContainer robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
@@ -24,7 +24,7 @@ public class Robot extends TimedRobot {
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
robotContainer = new RobotContainer();
}
/**
@@ -54,11 +54,11 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
// Get selected routine from the SmartDashboard
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autonomousCommand = robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -72,8 +72,8 @@ public class Robot extends TimedRobot {
// use the default command which is ArcadeDrive. 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

22
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/RobotContainer.java
View File

@@ -27,14 +27,14 @@ import org.wpilib.smartdashboard.SmartDashboard;
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
private final Drivetrain drivetrain = new Drivetrain();
private final OnBoardIO onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
// Assumes a gamepad plugged into channel 0
private final Joystick m_controller = new Joystick(0);
private final Joystick controller = new Joystick(0);
// Create SmartDashboard chooser for autonomous routines
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
private final SendableChooser<Command> chooser = new SendableChooser<>();
// NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
// that is specified when launching the wpilib-ws server on the Romi raspberry pi.
@@ -62,18 +62,18 @@ public class RobotContainer {
private void configureButtonBindings() {
// Default command is arcade drive. This will run unless another command
// is scheduled over it.
m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
drivetrain.setDefaultCommand(getArcadeDriveCommand());
// Example of how to use the onboard IO
Trigger onboardButtonA = new Trigger(m_onboardIO::getButtonAPressed);
Trigger onboardButtonA = new Trigger(onboardIO::getButtonAPressed);
onboardButtonA
.onTrue(new PrintCommand("Button A Pressed"))
.onFalse(new PrintCommand("Button A Released"));
// Setup SmartDashboard options
m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
SmartDashboard.putData(m_chooser);
chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(drivetrain));
chooser.addOption("Auto Routine Time", new AutonomousTime(drivetrain));
SmartDashboard.putData(chooser);
}
/**
@@ -82,7 +82,7 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
return chooser.getSelected();
}
/**
@@ -92,6 +92,6 @@ public class RobotContainer {
*/
public Command getArcadeDriveCommand() {
return new ArcadeDrive(
m_drivetrain, () -> -m_controller.getRawAxis(1), () -> -m_controller.getRawAxis(2));
drivetrain, () -> -controller.getRawAxis(1), () -> -controller.getRawAxis(2));
}
}

14
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/ArcadeDrive.java
View File

@@ -9,9 +9,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class ArcadeDrive extends Command {
private final Drivetrain m_drivetrain;
private final Supplier<Double> m_xaxisVelocitySupplier;
private final Supplier<Double> m_zaxisRotateSupplier;
private final Drivetrain drivetrain;
private final Supplier<Double> xaxisVelocitySupplier;
private final Supplier<Double> zaxisRotateSupplier;
/**
* Creates a new ArcadeDrive. This command will drive your robot according to the velocity
@@ -25,9 +25,9 @@ public class ArcadeDrive extends Command {
Drivetrain drivetrain,
Supplier<Double> xaxisVelocitySupplier,
Supplier<Double> zaxisRotateSupplier) {
m_drivetrain = drivetrain;
m_xaxisVelocitySupplier = xaxisVelocitySupplier;
m_zaxisRotateSupplier = zaxisRotateSupplier;
this.drivetrain = drivetrain;
this.xaxisVelocitySupplier = xaxisVelocitySupplier;
this.zaxisRotateSupplier = zaxisRotateSupplier;
addRequirements(drivetrain);
}
@@ -38,7 +38,7 @@ public class ArcadeDrive extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drivetrain.arcadeDrive(m_xaxisVelocitySupplier.get(), m_zaxisRotateSupplier.get());
drivetrain.arcadeDrive(xaxisVelocitySupplier.get(), zaxisRotateSupplier.get());
}
// Called once the command ends or is interrupted.

22
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/DriveDistance.java
View File

@@ -8,9 +8,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class DriveDistance extends Command {
private final Drivetrain m_drive;
private final double m_distance;
private final double m_velocity;
private final Drivetrain drive;
private final double distance;
private final double velocity;
/**
* Creates a new DriveDistance. This command will drive your your robot for a desired distance at
@@ -21,35 +21,35 @@ public class DriveDistance extends Command {
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveDistance(double velocity, double inches, Drivetrain drive) {
m_distance = inches;
m_velocity = velocity;
m_drive = drive;
distance = inches;
this.velocity = velocity;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
drive.arcadeDrive(0, 0);
drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_velocity, 0);
drive.arcadeDrive(velocity, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// Compare distance travelled from start to desired distance
return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance;
return Math.abs(drive.getAverageDistanceInch()) >= distance;
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/DriveTime.java
View File

@@ -8,10 +8,10 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class DriveTime extends Command {
private final double m_duration;
private final double m_velocity;
private final Drivetrain m_drive;
private long m_startTime;
private final double duration;
private final double velocity;
private final Drivetrain drive;
private long startTime;
/**
* Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
@@ -21,34 +21,34 @@ public class DriveTime extends Command {
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveTime(double velocity, double time, Drivetrain drive) {
m_velocity = velocity;
m_duration = time * 1000;
m_drive = drive;
this.velocity = velocity;
duration = time * 1000;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
startTime = System.currentTimeMillis();
drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_velocity, 0);
drive.arcadeDrive(velocity, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
return (System.currentTimeMillis() - startTime) >= duration;
}
}

26
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/TurnDegrees.java
View File

@@ -8,9 +8,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class TurnDegrees extends Command {
private final Drivetrain m_drive;
private final double m_degrees;
private final double m_velocity;
private final Drivetrain drive;
private final double degrees;
private final double velocity;
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
@@ -21,9 +21,9 @@ public class TurnDegrees extends Command {
* @param drive The drive subsystem on which this command will run
*/
public TurnDegrees(double velocity, double degrees, Drivetrain drive) {
m_degrees = degrees;
m_velocity = velocity;
m_drive = drive;
this.degrees = degrees;
this.velocity = velocity;
this.drive = drive;
addRequirements(drive);
}
@@ -31,20 +31,20 @@ public class TurnDegrees extends Command {
@Override
public void initialize() {
// Set motors to stop, read encoder values for starting point
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
drive.arcadeDrive(0, 0);
drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_velocity);
drive.arcadeDrive(0, velocity);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@@ -57,12 +57,12 @@ public class TurnDegrees extends Command {
*/
double inchPerDegree = Math.PI * 5.551 / 360;
// Compare distance travelled from start to distance based on degree turn
return getAverageTurningDistance() >= (inchPerDegree * m_degrees);
return getAverageTurningDistance() >= (inchPerDegree * degrees);
}
private double getAverageTurningDistance() {
double leftDistance = Math.abs(m_drive.getLeftDistanceInch());
double rightDistance = Math.abs(m_drive.getRightDistanceInch());
double leftDistance = Math.abs(drive.getLeftDistanceInch());
double rightDistance = Math.abs(drive.getRightDistanceInch());
return (leftDistance + rightDistance) / 2.0;
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/TurnTime.java
View File

@@ -12,10 +12,10 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
* desired rotational velocity and time.
*/
public class TurnTime extends Command {
private final double m_duration;
private final double m_rotationalVelocity;
private final Drivetrain m_drive;
private long m_startTime;
private final double duration;
private final double rotationalVelocity;
private final Drivetrain drive;
private long startTime;
/**
* Creates a new TurnTime.
@@ -25,34 +25,34 @@ public class TurnTime extends Command {
* @param drive The drive subsystem on which this command will run
*/
public TurnTime(double velocity, double time, Drivetrain drive) {
m_rotationalVelocity = velocity;
m_duration = time * 1000;
m_drive = drive;
rotationalVelocity = velocity;
duration = time * 1000;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
startTime = System.currentTimeMillis();
drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_rotationalVelocity);
drive.arcadeDrive(0, rotationalVelocity);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
return (System.currentTimeMillis() - startTime) >= duration;
}
}

46
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/subsystems/Drivetrain.java
View File

@@ -17,60 +17,60 @@ public class Drivetrain extends SubsystemBase {
// 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);
private final Spark leftMotor = new Spark(0);
private final Spark 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);
private final Encoder leftEncoder = new Encoder(4, 5);
private final Encoder rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::setThrottle, m_rightMotor::setThrottle);
private final DifferentialDrive diffDrive =
new DifferentialDrive(leftMotor::setThrottle, rightMotor::setThrottle);
// Set up the RomiGyro
private final RomiGyro m_gyro = new RomiGyro();
private final RomiGyro gyro = new RomiGyro();
/** Creates a new Drivetrain. */
public Drivetrain() {
SendableRegistry.addChild(m_diffDrive, m_leftMotor);
SendableRegistry.addChild(m_diffDrive, m_rightMotor);
SendableRegistry.addChild(diffDrive, leftMotor);
SendableRegistry.addChild(diffDrive, rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
rightMotor.setInverted(true);
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
}
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
return leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
return rightEncoder.get();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
return leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
return rightEncoder.getDistance();
}
public double getAverageDistanceInch() {
@@ -83,7 +83,7 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleX() {
return m_gyro.getAngleX();
return gyro.getAngleX();
}
/**
@@ -92,7 +92,7 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleY() {
return m_gyro.getAngleY();
return gyro.getAngleY();
}
/**
@@ -101,12 +101,12 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleZ() {
return m_gyro.getAngleZ();
return gyro.getAngleZ();
}
/** Reset the gyro. */
public void resetGyro() {
m_gyro.reset();
gyro.reset();
}
@Override

101
wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -34,71 +34,71 @@ public class Drivetrain {
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = -4096;
private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
private final PWMSparkMax leftLeader = new PWMSparkMax(1);
private final PWMSparkMax leftFollower = new PWMSparkMax(2);
private final PWMSparkMax rightLeader = new PWMSparkMax(3);
private final PWMSparkMax rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final Encoder leftEncoder = new Encoder(0, 1);
private final Encoder rightEncoder = new Encoder(2, 3);
private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0);
private final PIDController leftPIDController = new PIDController(8.5, 0, 0);
private final PIDController rightPIDController = new PIDController(8.5, 0, 0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final DifferentialDriveKinematics m_kinematics =
private final DifferentialDriveKinematics kinematics =
new DifferentialDriveKinematics(kTrackwidth);
private final DifferentialDriveOdometry m_odometry =
private final DifferentialDriveOdometry odometry =
new DifferentialDriveOdometry(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
// Gains are for example purposes only - must be determined for your own
// robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
// Simulation classes help us simulate our robot
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
private final Field2d m_fieldSim = new Field2d();
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
private final EncoderSim leftEncoderSim = new EncoderSim(leftEncoder);
private final EncoderSim rightEncoderSim = new EncoderSim(rightEncoder);
private final Field2d fieldSim = new Field2d();
private final LinearSystem<N2, N2, N2> drivetrainSystem =
Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3);
private final DifferentialDrivetrainSim m_drivetrainSimulator =
private final DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
/** Subsystem constructor. */
public Drivetrain() {
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
m_rightLeader.setInverted(true);
SmartDashboard.putData("Field", m_fieldSim);
rightLeader.setInverted(true);
SmartDashboard.putData("Field", fieldSim);
}
/** Sets velocities to the drivetrain motors. */
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
final double leftFeedforward = feedforward.calculate(velocities.left);
final double rightFeedforward = feedforward.calculate(velocities.right);
double leftOutput = leftPIDController.calculate(leftEncoder.getRate(), velocities.left);
double rightOutput = rightPIDController.calculate(rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
leftLeader.setVoltage(leftOutput + leftFeedforward);
rightLeader.setVoltage(rightOutput + rightFeedforward);
}
/**
@@ -108,25 +108,24 @@ public class Drivetrain {
* @param rot the rotation
*/
public void drive(double xVelocity, double rot) {
setVelocities(m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0, rot)));
setVelocities(kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0, rot)));
}
/** Update robot odometry. */
public void updateOdometry() {
m_odometry.update(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
odometry.update(imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance());
}
/** Resets robot odometry. */
public void resetOdometry(Pose2d pose) {
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
drivetrainSimulator.setPose(pose);
odometry.resetPosition(
imu.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose);
}
/** Check the current robot pose. */
public Pose2d getPose() {
return m_odometry.getPose();
return odometry.getPose();
}
/** Update our simulation. This should be run every robot loop in simulation. */
@@ -135,21 +134,21 @@ public class Drivetrain {
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro. We negate the right side so that positive
// voltages make the right side move forward.
m_drivetrainSimulator.setInputs(
m_leftLeader.getThrottle() * RobotController.getInputVoltage(),
m_rightLeader.getThrottle() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
drivetrainSimulator.setInputs(
leftLeader.getThrottle() * RobotController.getInputVoltage(),
rightLeader.getThrottle() * RobotController.getInputVoltage());
drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
leftEncoderSim.setDistance(drivetrainSimulator.getLeftPosition());
leftEncoderSim.setRate(drivetrainSimulator.getLeftVelocity());
rightEncoderSim.setDistance(drivetrainSimulator.getRightPosition());
rightEncoderSim.setRate(drivetrainSimulator.getRightVelocity());
// gyroSim.setAngle(-drivetrainSimulator.getHeading().getDegrees());
}
/** Update odometry - this should be run every robot loop. */
public void periodic() {
updateOdometry();
m_fieldSim.setRobotPose(m_odometry.getPose());
fieldSim.setRobotPose(odometry.getPose());
}
}

39
wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Robot.java
View File

@@ -18,21 +18,21 @@ import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.system.Timer;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Gamepad controller = new Gamepad(0);
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
private final Drivetrain m_drive = new Drivetrain();
private final LTVUnicycleController m_feedback = new LTVUnicycleController(0.020);
private final Timer m_timer = new Timer();
private final Trajectory m_trajectory;
private final Drivetrain drive = new Drivetrain();
private final LTVUnicycleController feedback = new LTVUnicycleController(0.020);
private final Timer timer = new Timer();
private final Trajectory trajectory;
/** Called once at the beginning of the robot program. */
public Robot() {
m_trajectory =
trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(2, 2, Rotation2d.kZero),
List.of(),
@@ -42,40 +42,39 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_drive.periodic();
drive.periodic();
}
@Override
public void autonomousInit() {
m_timer.restart();
m_drive.resetOdometry(m_trajectory.getInitialPose());
timer.restart();
drive.resetOdometry(trajectory.getInitialPose());
}
@Override
public void autonomousPeriodic() {
double elapsed = m_timer.get();
Trajectory.State reference = m_trajectory.sample(elapsed);
ChassisVelocities velocities = m_feedback.calculate(m_drive.getPose(), reference);
m_drive.drive(velocities.vx, velocities.omega);
double elapsed = timer.get();
Trajectory.State reference = trajectory.sample(elapsed);
ChassisVelocities velocities = feedback.calculate(drive.getPose(), reference);
drive.drive(velocities.vx, velocities.omega);
}
@Override
public void teleopPeriodic() {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
double xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
double xVelocity = -velocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
double rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xVelocity, rot);
double rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
drive.drive(xVelocity, rot);
}
@Override
public void simulationPeriodic() {
m_drive.simulationPeriodic();
drive.simulationPeriodic();
}
}

45
wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java
View File

@@ -40,28 +40,28 @@ public class Robot extends TimedRobot {
// the motors, this number should be greater than one.
private static final double kArmGearing = 10.0;
private final TrapezoidProfile m_profile =
private final TrapezoidProfile profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.degreesToRadians(45),
Units.degreesToRadians(90))); // Max arm velocity and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
private TrapezoidProfile.State lastProfiledReference = new TrapezoidProfile.State();
// The plant holds a state-space model of our arm. This system has the following properties:
//
// States: [position, velocity], in radians and radians per second.
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [position], in radians.
private final LinearSystem<N2, N1, N2> m_armPlant =
private final LinearSystem<N2, N1, N2> armPlant =
Models.singleJointedArmFromPhysicalConstants(DCMotor.getNEO(2), kArmMOI, kArmGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
@SuppressWarnings("unchecked")
private final KalmanFilter<N2, N1, N1> m_observer =
private final KalmanFilter<N2, N1, N1> observer =
new KalmanFilter<>(
Nat.N2(),
Nat.N1(),
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0),
(LinearSystem<N2, N1, N1>) armPlant.slice(0),
VecBuilder.fill(0.015, 0.17), // How accurate we
// think our model is, in radians and radians/sec
VecBuilder.fill(0.01), // How accurate we think our encoder position
@@ -70,9 +70,9 @@ public class Robot extends TimedRobot {
// A LQR uses feedback to create voltage commands.
@SuppressWarnings("unchecked")
private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
private final LinearQuadraticRegulator<N2, N1, N1> controller =
new LinearQuadraticRegulator<>(
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0),
(LinearSystem<N2, N1, N1>) armPlant.slice(0),
VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
// Position and velocity error tolerances, in radians and radians per second. Decrease
// this
@@ -89,31 +89,30 @@ public class Robot extends TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
private final LinearSystemLoop<N2, N1, N1> loop =
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0), m_controller, m_observer, 12.0, 0.020);
(LinearSystem<N2, N1, N1>) armPlant.slice(0), controller, observer, 12.0, 0.020);
// An encoder set up to measure arm position in radians.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final Encoder encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final Joystick joystick = new Joystick(kJoystickPort);
public Robot() {
// We go 2 pi radians in 1 rotation, or 4096 counts.
m_encoder.setDistancePerPulse(Math.PI * 2 / 4096.0);
encoder.setDistancePerPulse(Math.PI * 2 / 4096.0);
}
@Override
public void teleopInit() {
// Reset our loop to make sure it's in a known state.
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
loop.reset(VecBuilder.fill(encoder.getDistance(), encoder.getRate()));
// Reset our last reference to the current state.
m_lastProfiledReference =
new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
lastProfiledReference = new TrapezoidProfile.State(encoder.getDistance(), encoder.getRate());
}
@Override
@@ -121,7 +120,7 @@ public class Robot extends TimedRobot {
// Sets the target position of our arm. This is similar to setting the setpoint of a
// PID controller.
TrapezoidProfile.State goal;
if (m_joystick.getTrigger()) {
if (joystick.getTrigger()) {
// the trigger is pressed, so we go to the high goal.
goal = new TrapezoidProfile.State(kRaisedPosition, 0.0);
} else {
@@ -129,19 +128,19 @@ public class Robot extends TimedRobot {
goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
lastProfiledReference = profile.calculate(0.020, lastProfiledReference, goal);
loop.setNextR(lastProfiledReference.position, lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
loop.correct(VecBuilder.fill(encoder.getDistance()));
// Update our LQR to generate new voltage commands and use the voltages to predict the next
// state with out Kalman filter.
m_loop.predict(0.020);
loop.predict(0.020);
// Send the new calculated voltage to the motors.
// voltage = duty cycle * battery voltage, so
// duty cycle = voltage / battery voltage
double nextVoltage = m_loop.getU(0);
m_motor.setVoltage(nextVoltage);
double nextVoltage = loop.getU(0);
motor.setVoltage(nextVoltage);
}
}

49
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java
View File

@@ -42,12 +42,12 @@ public class Robot extends TimedRobot {
// the motors, this number should be greater than one.
private static final double kElevatorGearing = 6.0;
private final TrapezoidProfile m_profile =
private final TrapezoidProfile profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.feetToMeters(3.0),
Units.feetToMeters(6.0))); // Max elevator velocity and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
private TrapezoidProfile.State lastProfiledReference = new TrapezoidProfile.State();
/* The plant holds a state-space model of our elevator. This system has the following properties:
@@ -57,17 +57,17 @@ public class Robot extends TimedRobot {
This elevator is driven by two NEO motors.
*/
private final LinearSystem<N2, N1, N2> m_elevatorPlant =
private final LinearSystem<N2, N1, N2> elevatorPlant =
Models.elevatorFromPhysicalConstants(
DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
@SuppressWarnings("unchecked")
private final KalmanFilter<N2, N1, N1> m_observer =
private final KalmanFilter<N2, N1, N1> observer =
new KalmanFilter<>(
Nat.N2(),
Nat.N1(),
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0),
VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
// think our model is, in meters and meters/second.
VecBuilder.fill(0.001), // How accurate we think our encoder position
@@ -76,9 +76,9 @@ public class Robot extends TimedRobot {
// A LQR uses feedback to create voltage commands.
@SuppressWarnings("unchecked")
private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
private final LinearQuadraticRegulator<N2, N1, N1> controller =
new LinearQuadraticRegulator<>(
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0),
VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
// and velocity error tolerances, in meters and meters per second. Decrease this to more
// heavily penalize state excursion, or make the controller behave more aggressively. In
@@ -93,35 +93,30 @@ public class Robot extends TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
private final LinearSystemLoop<N2, N1, N1> loop =
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
m_controller,
m_observer,
12.0,
0.020);
(LinearSystem<N2, N1, N1>) elevatorPlant.slice(0), controller, observer, 12.0, 0.020);
// An encoder set up to measure elevator height in meters.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final Encoder encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final Joystick joystick = new Joystick(kJoystickPort);
public Robot() {
// Circumference = pi * d, so distance per click = pi * d / counts
m_encoder.setDistancePerPulse(Math.PI * 2 * kDrumRadius / 4096.0);
encoder.setDistancePerPulse(Math.PI * 2 * kDrumRadius / 4096.0);
}
@Override
public void teleopInit() {
// Reset our loop to make sure it's in a known state.
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
loop.reset(VecBuilder.fill(encoder.getDistance(), encoder.getRate()));
// Reset our last reference to the current state.
m_lastProfiledReference =
new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
lastProfiledReference = new TrapezoidProfile.State(encoder.getDistance(), encoder.getRate());
}
@Override
@@ -129,7 +124,7 @@ public class Robot extends TimedRobot {
// Sets the target position of our arm. This is similar to setting the setpoint of a
// PID controller.
TrapezoidProfile.State goal;
if (m_joystick.getTrigger()) {
if (joystick.getTrigger()) {
// the trigger is pressed, so we go to the high goal.
goal = new TrapezoidProfile.State(kHighGoalPosition, 0.0);
} else {
@@ -137,20 +132,20 @@ public class Robot extends TimedRobot {
goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
m_lastProfiledReference = m_profile.calculate(0.020, m_lastProfiledReference, goal);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
lastProfiledReference = profile.calculate(0.020, lastProfiledReference, goal);
loop.setNextR(lastProfiledReference.position, lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
loop.correct(VecBuilder.fill(encoder.getDistance()));
// Update our LQR to generate new voltage commands and use the voltages to predict the next
// state with out Kalman filter.
m_loop.predict(0.020);
loop.predict(0.020);
// Send the new calculated voltage to the motors.
// voltage = duty cycle * battery voltage, so
// duty cycle = voltage / battery voltage
double nextVoltage = m_loop.getU(0);
m_motor.setVoltage(nextVoltage);
double nextVoltage = loop.getU(0);
motor.setVoltage(nextVoltage);
}
}

40
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java
View File

@@ -41,25 +41,25 @@ public class Robot extends TimedRobot {
// States: [velocity], in radians per second.
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [velocity], in radians per second.
private final LinearSystem<N1, N1, N1> m_flywheelPlant =
private final LinearSystem<N1, N1, N1> flywheelPlant =
Models.flywheelFromPhysicalConstants(
DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
private final KalmanFilter<N1, N1, N1> m_observer =
private final KalmanFilter<N1, N1, N1> observer =
new KalmanFilter<>(
Nat.N1(),
Nat.N1(),
m_flywheelPlant,
flywheelPlant,
VecBuilder.fill(3.0), // How accurate we think our model is
VecBuilder.fill(0.01), // How accurate we think our encoder
// data is
0.020);
// A LQR uses feedback to create voltage commands.
private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
private final LinearQuadraticRegulator<N1, N1, N1> controller =
new LinearQuadraticRegulator<>(
m_flywheelPlant,
flywheelPlant,
VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
// this to more heavily penalize state excursion, or make the controller behave more
// aggressively.
@@ -70,50 +70,50 @@ public class Robot extends TimedRobot {
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
private final LinearSystemLoop<N1, N1, N1> m_loop =
new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
private final LinearSystemLoop<N1, N1, N1> loop =
new LinearSystemLoop<>(flywheelPlant, controller, observer, 12.0, 0.020);
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final Encoder encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final Joystick joystick = new Joystick(kJoystickPort);
public Robot() {
// We go 2 pi radians per 4096 clicks.
m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
}
@Override
public void teleopInit() {
m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
loop.reset(VecBuilder.fill(encoder.getRate()));
}
@Override
public void teleopPeriodic() {
// Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
// PID controller.
if (m_joystick.getTriggerPressed()) {
if (joystick.getTriggerPressed()) {
// We just pressed the trigger, so let's set our next reference
m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
} else if (m_joystick.getTriggerReleased()) {
loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
} else if (joystick.getTriggerReleased()) {
// We just released the trigger, so let's spin down
m_loop.setNextR(VecBuilder.fill(0.0));
loop.setNextR(VecBuilder.fill(0.0));
}
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
loop.correct(VecBuilder.fill(encoder.getRate()));
// Update our LQR to generate new voltage commands and use the voltages to predict the next
// state with out Kalman filter.
m_loop.predict(0.020);
loop.predict(0.020);
// Send the new calculated voltage to the motors.
// voltage = duty cycle * battery voltage, so
// duty cycle = voltage / battery voltage
double nextVoltage = m_loop.getU(0);
m_motor.setVoltage(nextVoltage);
double nextVoltage = loop.getU(0);
motor.setVoltage(nextVoltage);
}
}

40
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java
View File

@@ -42,74 +42,74 @@ public class Robot extends TimedRobot {
// Outputs (what we can measure): [velocity], in radians per second.
//
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
private final LinearSystem<N1, N1, N1> m_flywheelPlant =
private final LinearSystem<N1, N1, N1> flywheelPlant =
Models.flywheelFromSysId(kFlywheelKv, kFlywheelKa);
// The observer fuses our encoder data and voltage inputs to reject noise.
private final KalmanFilter<N1, N1, N1> m_observer =
private final KalmanFilter<N1, N1, N1> observer =
new KalmanFilter<>(
Nat.N1(),
Nat.N1(),
m_flywheelPlant,
flywheelPlant,
VecBuilder.fill(3.0), // How accurate we think our model is
VecBuilder.fill(0.01), // How accurate we think our encoder
// data is
0.020);
// A LQR uses feedback to create voltage commands.
private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
private final LinearQuadraticRegulator<N1, N1, N1> controller =
new LinearQuadraticRegulator<>(
m_flywheelPlant,
flywheelPlant,
VecBuilder.fill(8.0), // Velocity error tolerance
VecBuilder.fill(12.0), // Control effort (voltage) tolerance
0.020);
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
private final LinearSystemLoop<N1, N1, N1> m_loop =
new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
private final LinearSystemLoop<N1, N1, N1> loop =
new LinearSystemLoop<>(flywheelPlant, controller, observer, 12.0, 0.020);
// An encoder set up to measure flywheel velocity in radians per second.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final Encoder encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
private final PWMSparkMax motor = new PWMSparkMax(kMotorPort);
// A joystick to read the trigger from.
private final Joystick m_joystick = new Joystick(kJoystickPort);
private final Joystick joystick = new Joystick(kJoystickPort);
public Robot() {
// We go 2 pi radians per 4096 clicks.
m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
}
@Override
public void teleopInit() {
// Reset our loop to make sure it's in a known state.
m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
loop.reset(VecBuilder.fill(encoder.getRate()));
}
@Override
public void teleopPeriodic() {
// Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
// PID controller.
if (m_joystick.getTriggerPressed()) {
if (joystick.getTriggerPressed()) {
// We just pressed the trigger, so let's set our next reference
m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
} else if (m_joystick.getTriggerReleased()) {
loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
} else if (joystick.getTriggerReleased()) {
// We just released the trigger, so let's spin down
m_loop.setNextR(VecBuilder.fill(0.0));
loop.setNextR(VecBuilder.fill(0.0));
}
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
loop.correct(VecBuilder.fill(encoder.getRate()));
// Update our LQR to generate new voltage commands and use the voltages to predict the next
// state with out Kalman filter.
m_loop.predict(0.020);
loop.predict(0.020);
// Send the new calculated voltage to the motors.
// voltage = duty cycle * battery voltage, so
// duty cycle = voltage / battery voltage
double nextVoltage = m_loop.getU(0);
m_motor.setVoltage(nextVoltage);
double nextVoltage = loop.getU(0);
motor.setVoltage(nextVoltage);
}
}

62
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
View File

@@ -16,35 +16,35 @@ public class Drivetrain {
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
private final Translation2d frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d backRightLocation = new Translation2d(-0.381, -0.381);
private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final SwerveModule frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
private final SwerveModule frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
private final SwerveModule backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final SwerveDriveKinematics m_kinematics =
private final SwerveDriveKinematics kinematics =
new SwerveDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation);
private final SwerveDriveOdometry m_odometry =
private final SwerveDriveOdometry odometry =
new SwerveDriveOdometry(
m_kinematics,
m_imu.getRotation2d(),
kinematics,
imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
frontLeft.getPosition(),
frontRight.getPosition(),
backLeft.getPosition(),
backRight.getPosition()
});
public Drivetrain() {
m_imu.resetYaw();
imu.resetYaw();
}
/**
@@ -59,29 +59,29 @@ public class Drivetrain {
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisVelocities = chassisVelocities.toRobotRelative(m_imu.getRotation2d());
chassisVelocities = chassisVelocities.toRobotRelative(imu.getRotation2d());
}
chassisVelocities = chassisVelocities.discretize(period);
var velocities =
SwerveDriveKinematics.desaturateWheelVelocities(
m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
m_frontLeft.setDesiredVelocity(velocities[0]);
m_frontRight.setDesiredVelocity(velocities[1]);
m_backLeft.setDesiredVelocity(velocities[2]);
m_backRight.setDesiredVelocity(velocities[3]);
frontLeft.setDesiredVelocity(velocities[0]);
frontRight.setDesiredVelocity(velocities[1]);
backLeft.setDesiredVelocity(velocities[2]);
backRight.setDesiredVelocity(velocities[3]);
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(
m_imu.getRotation2d(),
odometry.update(
imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
frontLeft.getPosition(),
frontRight.getPosition(),
backLeft.getPosition(),
backRight.getPosition()
});
}
}

20
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Robot.java
View File

@@ -10,18 +10,18 @@ import org.wpilib.math.filter.SlewRateLimiter;
import org.wpilib.math.util.MathUtil;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_swerve = new Drivetrain();
private final Gamepad controller = new Gamepad(0);
private final Drivetrain swerve = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
driveWithJoystick(false);
m_swerve.updateOdometry();
swerve.updateOdometry();
}
@Override
@@ -33,14 +33,14 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xVelocity =
-m_xVelocityLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
-xVelocityLimiter.calculate(MathUtil.applyDeadband(controller.getLeftY(), 0.02))
* Drivetrain.kMaxVelocity;
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
final var yVelocity =
-m_yVelocityLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
-yVelocityLimiter.calculate(MathUtil.applyDeadband(controller.getLeftX(), 0.02))
* Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
@@ -48,9 +48,9 @@ public class Robot extends TimedRobot {
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
-rotLimiter.calculate(MathUtil.applyDeadband(controller.getRightX(), 0.02))
* Drivetrain.kMaxAngularVelocity;
m_swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

56
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
View File

@@ -22,17 +22,17 @@ public class SwerveModule {
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
private final PWMSparkMax m_driveMotor;
private final PWMSparkMax m_turningMotor;
private final PWMSparkMax driveMotor;
private final PWMSparkMax turningMotor;
private final Encoder m_driveEncoder;
private final Encoder m_turningEncoder;
private final Encoder driveEncoder;
private final Encoder turningEncoder;
// Gains are for example purposes only - must be determined for your own robot!
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
private final PIDController drivePIDController = new PIDController(1, 0, 0);
// Gains are for example purposes only - must be determined for your own robot!
private final ProfiledPIDController m_turningPIDController =
private final ProfiledPIDController turningPIDController =
new ProfiledPIDController(
1,
0,
@@ -41,8 +41,8 @@ public class SwerveModule {
kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
/**
* Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
@@ -61,25 +61,25 @@ public class SwerveModule {
int driveEncoderChannelB,
int turningEncoderChannelA,
int turningEncoderChannelB) {
m_driveMotor = new PWMSparkMax(driveMotorChannel);
m_turningMotor = new PWMSparkMax(turningMotorChannel);
driveMotor = new PWMSparkMax(driveMotorChannel);
turningMotor = new PWMSparkMax(turningMotorChannel);
m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
// Set the distance (in this case, angle) in radians per pulse for the turning encoder.
// This is the the angle through an entire rotation (2 * pi) divided by the
// encoder resolution.
m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
}
/**
@@ -89,7 +89,7 @@ public class SwerveModule {
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.getDistance()));
driveEncoder.getDistance(), new Rotation2d(turningEncoder.getDistance()));
}
/**
@@ -99,7 +99,7 @@ public class SwerveModule {
*/
public SwerveModuleVelocity getVelocity() {
return new SwerveModuleVelocity(
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
driveEncoder.getRate(), new Rotation2d(turningEncoder.getDistance()));
}
/**
@@ -108,7 +108,7 @@ public class SwerveModule {
* @param desiredVelocity Desired velocity.
*/
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
var encoderRotation = new Rotation2d(turningEncoder.getDistance());
// Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
// by cosine of angle error. This scales down movement perpendicular to the desired direction of
@@ -116,22 +116,18 @@ public class SwerveModule {
SwerveModuleVelocity velocity =
desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
// Calculate the drive output from the drive PID controller and feedforward.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
drivePIDController.calculate(driveEncoder.getRate(), velocity.velocity)
+ driveFeedforward.calculate(desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
// Calculate the turning motor output from the turning PID controller.
// Calculate the turning motor output from the turning PID controller and feedforward.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), velocity.angle.getRadians());
turningPIDController.calculate(turningEncoder.getDistance(), velocity.angle.getRadians())
+ turnFeedforward.calculate(turningPIDController.getSetpoint().velocity);
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);
driveMotor.setVoltage(driveOutput);
turningMotor.setVoltage(turnOutput);
}
}

67
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -20,40 +20,40 @@ public class Drivetrain {
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
private final Translation2d frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d frontRightLocation = new Translation2d(0.381, -0.381);
private final Translation2d backLeftLocation = new Translation2d(-0.381, 0.381);
private final Translation2d backRightLocation = new Translation2d(-0.381, -0.381);
private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final SwerveModule frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
private final SwerveModule frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
private final SwerveModule backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
private final SwerveDriveKinematics m_kinematics =
private final SwerveDriveKinematics kinematics =
new SwerveDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation);
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private final SwerveDrivePoseEstimator m_poseEstimator =
private final SwerveDrivePoseEstimator poseEstimator =
new SwerveDrivePoseEstimator(
m_kinematics,
m_imu.getRotation2d(),
kinematics,
imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
frontLeft.getPosition(),
frontRight.getPosition(),
backLeft.getPosition(),
backRight.getPosition()
},
Pose2d.kZero,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
public Drivetrain() {
m_imu.resetYaw();
imu.resetYaw();
}
/**
@@ -69,37 +69,36 @@ public class Drivetrain {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisVelocities =
chassisVelocities.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
chassisVelocities.toRobotRelative(poseEstimator.getEstimatedPosition().getRotation());
}
chassisVelocities = chassisVelocities.discretize(period);
var velocities =
SwerveDriveKinematics.desaturateWheelVelocities(
m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
m_frontLeft.setDesiredVelocity(velocities[0]);
m_frontRight.setDesiredVelocity(velocities[1]);
m_backLeft.setDesiredVelocity(velocities[2]);
m_backRight.setDesiredVelocity(velocities[3]);
frontLeft.setDesiredVelocity(velocities[0]);
frontRight.setDesiredVelocity(velocities[1]);
backLeft.setDesiredVelocity(velocities[2]);
backRight.setDesiredVelocity(velocities[3]);
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(
m_imu.getRotation2d(),
poseEstimator.update(
imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
frontLeft.getPosition(),
frontRight.getPosition(),
backLeft.getPosition(),
backRight.getPosition()
});
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
m_poseEstimator.getEstimatedPosition()),
poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(poseEstimator.getEstimatedPosition()),
Timer.getTimestamp() - 0.3);
}
}

21
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Robot.java
View File

@@ -9,18 +9,18 @@ import org.wpilib.framework.TimedRobot;
import org.wpilib.math.filter.SlewRateLimiter;
public class Robot extends TimedRobot {
private final Gamepad m_controller = new Gamepad(0);
private final Drivetrain m_swerve = new Drivetrain();
private final Gamepad controller = new Gamepad(0);
private final Drivetrain swerve = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
@Override
public void autonomousPeriodic() {
driveWithJoystick(false);
m_swerve.updateOdometry();
swerve.updateOdometry();
}
@Override
@@ -32,21 +32,20 @@ public class Robot extends TimedRobot {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
-xVelocityLimiter.calculate(controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
-yVelocityLimiter.calculate(controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
final var rot = -rotLimiter.calculate(controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

56
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -22,17 +22,17 @@ public class SwerveModule {
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
private final PWMSparkMax m_driveMotor;
private final PWMSparkMax m_turningMotor;
private final PWMSparkMax driveMotor;
private final PWMSparkMax turningMotor;
private final Encoder m_driveEncoder;
private final Encoder m_turningEncoder;
private final Encoder driveEncoder;
private final Encoder turningEncoder;
// Gains are for example purposes only - must be determined for your own robot!
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
private final PIDController drivePIDController = new PIDController(1, 0, 0);
// Gains are for example purposes only - must be determined for your own robot!
private final ProfiledPIDController m_turningPIDController =
private final ProfiledPIDController turningPIDController =
new ProfiledPIDController(
1,
0,
@@ -41,8 +41,8 @@ public class SwerveModule {
kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(1, 3);
private final SimpleMotorFeedforward turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
/**
* Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
@@ -61,25 +61,25 @@ public class SwerveModule {
int driveEncoderChannelB,
int turningEncoderChannelA,
int turningEncoderChannelB) {
m_driveMotor = new PWMSparkMax(driveMotorChannel);
m_turningMotor = new PWMSparkMax(turningMotorChannel);
driveMotor = new PWMSparkMax(driveMotorChannel);
turningMotor = new PWMSparkMax(turningMotorChannel);
m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
// Set the distance (in this case, angle) in radians per pulse for the turning encoder.
// This is the the angle through an entire rotation (2 * pi) divided by the
// encoder resolution.
m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
}
/**
@@ -89,7 +89,7 @@ public class SwerveModule {
*/
public SwerveModuleVelocity getState() {
return new SwerveModuleVelocity(
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
driveEncoder.getRate(), new Rotation2d(turningEncoder.getDistance()));
}
/**
@@ -99,7 +99,7 @@ public class SwerveModule {
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.getDistance()));
driveEncoder.getDistance(), new Rotation2d(turningEncoder.getDistance()));
}
/**
@@ -108,7 +108,7 @@ public class SwerveModule {
* @param desiredVelocity Desired velocity.
*/
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
var encoderRotation = new Rotation2d(turningEncoder.getDistance());
// Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
// by cosine of angle error. This scales down movement perpendicular to the desired direction of
@@ -116,21 +116,17 @@ public class SwerveModule {
SwerveModuleVelocity velocity =
desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
// Calculate the drive output from the drive PID controller and feedforward.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
drivePIDController.calculate(driveEncoder.getRate(), velocity.velocity)
+ driveFeedforward.calculate(desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
// Calculate the turning motor output from the turning PID controller.
// Calculate the turning motor output from the turning PID controller and feedforward.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), velocity.angle.getRadians());
turningPIDController.calculate(turningEncoder.getDistance(), velocity.angle.getRadians())
+ turnFeedforward.calculate(turningPIDController.getSetpoint().velocity);
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);
driveMotor.setVoltage(driveOutput);
turningMotor.setVoltage(turnOutput);
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/Robot.java
View File

@@ -14,9 +14,9 @@ import org.wpilib.framework.TimedRobot;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final SysIdRoutineBot m_robot = new SysIdRoutineBot();
private final SysIdRoutineBot robot = new SysIdRoutineBot();
/**
* This function is run when the robot is first started up and should be used for any
@@ -24,7 +24,7 @@ public class Robot extends TimedRobot {
*/
public Robot() {
// Configure default commands and condition bindings on robot startup
m_robot.configureBindings();
robot.configureBindings();
}
/**
@@ -52,10 +52,10 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
m_autonomousCommand = m_robot.getAutonomousCommand();
autonomousCommand = robot.getAutonomousCommand();
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -69,8 +69,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

64
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/SysIdRoutineBot.java
View File

@@ -20,11 +20,11 @@ import org.wpilib.examples.sysidroutine.subsystems.Shooter;
*/
public class SysIdRoutineBot {
// The robot's subsystems
private final Drive m_drive = new Drive();
private final Shooter m_shooter = new Shooter();
private final Drive drive = new Drive();
private final Shooter shooter = new Shooter();
// The driver's controller
CommandGamepad m_driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
/**
* Use this method to define bindings between conditions and commands. These are useful for
@@ -36,50 +36,50 @@ public class SysIdRoutineBot {
*/
public void configureBindings() {
// Control the drive with split-stick arcade controls
m_drive.setDefaultCommand(
m_drive.arcadeDriveCommand(
() -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
drive.setDefaultCommand(
drive.arcadeDriveCommand(
() -> -driverController.getLeftY(), () -> -driverController.getRightX()));
// Bind full set of SysId routine tests to buttons; a complete routine should run each of these
// once.
// Using bumpers as a modifier and combining it with the buttons so that we can have both sets
// of bindings at once
m_driverController
driverController
.southFace()
.and(m_driverController.rightBumper())
.whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
m_driverController
.and(driverController.rightBumper())
.whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
driverController
.eastFace()
.and(m_driverController.rightBumper())
.whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
m_driverController
.and(driverController.rightBumper())
.whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
driverController
.westFace()
.and(m_driverController.rightBumper())
.whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
m_driverController
.and(driverController.rightBumper())
.whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
driverController
.northFace()
.and(m_driverController.rightBumper())
.whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
.and(driverController.rightBumper())
.whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// Control the shooter wheel with the left trigger
m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis));
shooter.setDefaultCommand(shooter.runShooter(driverController::getLeftTriggerAxis));
m_driverController
driverController
.southFace()
.and(m_driverController.leftBumper())
.whileTrue(m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
m_driverController
.and(driverController.leftBumper())
.whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
driverController
.eastFace()
.and(m_driverController.leftBumper())
.whileTrue(m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
m_driverController
.and(driverController.leftBumper())
.whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
driverController
.westFace()
.and(m_driverController.leftBumper())
.whileTrue(m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
m_driverController
.and(driverController.leftBumper())
.whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
driverController
.northFace()
.and(m_driverController.leftBumper())
.whileTrue(m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
.and(driverController.leftBumper())
.whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
}
/**
@@ -89,6 +89,6 @@ public class SysIdRoutineBot {
*/
public Command getAutonomousCommand() {
// Do nothing
return m_drive.run(() -> {});
return drive.run(() -> {});
}
}

46
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/subsystems/Drive.java
View File

@@ -20,39 +20,39 @@ import org.wpilib.system.RobotController;
public class Drive extends SubsystemBase {
// The motors on the left side of the drive.
private final PWMSparkMax m_leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
private final PWMSparkMax leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
// The motors on the right side of the drive.
private final PWMSparkMax m_rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
private final PWMSparkMax rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftMotor::setThrottle, m_rightMotor::setThrottle);
private final DifferentialDrive drive =
new DifferentialDrive(leftMotor::setThrottle, rightMotor::setThrottle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
private final Encoder leftEncoder =
new Encoder(
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
private final Encoder rightEncoder =
new Encoder(
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
// Create a new SysId routine for characterizing the drive.
private final SysIdRoutine m_sysIdRoutine =
private final SysIdRoutine sysIdRoutine =
new SysIdRoutine(
// Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
// Tell SysId how to plumb the driving voltage to the motors.
voltage -> {
m_leftMotor.setVoltage(voltage);
m_rightMotor.setVoltage(voltage);
leftMotor.setVoltage(voltage);
rightMotor.setVoltage(voltage);
},
// Tell SysId how to record a frame of data for each motor on the mechanism being
// characterized.
@@ -61,16 +61,16 @@ public class Drive extends SubsystemBase {
// the entire group to be one motor.
log.motor("drive-left")
.voltage(
Volts.of(m_leftMotor.getThrottle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(m_leftEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(m_leftEncoder.getRate()));
Volts.of(leftMotor.getThrottle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(leftEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(leftEncoder.getRate()));
// Record a frame for the right motors. Since these share an encoder, we consider
// the entire group to be one motor.
log.motor("drive-right")
.voltage(
Volts.of(m_rightMotor.getThrottle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(m_rightEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(m_rightEncoder.getRate()));
Volts.of(rightMotor.getThrottle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(rightEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(rightEncoder.getRate()));
},
// Tell SysId to make generated commands require this subsystem, suffix test state in
// WPILog with this subsystem's name ("drive")
@@ -79,17 +79,17 @@ public class Drive extends SubsystemBase {
/** Creates a new Drive subsystem. */
public Drive() {
// Add the second motors on each side of the drivetrain
m_leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
m_rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
rightMotor.setInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
}
/**
@@ -101,7 +101,7 @@ public class Drive extends SubsystemBase {
public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
return run(() -> drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
.withName("arcadeDrive");
}
@@ -111,7 +111,7 @@ public class Drive extends SubsystemBase {
* @param direction The direction (forward or reverse) to run the test in
*/
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.quasistatic(direction);
return sysIdRoutine.quasistatic(direction);
}
/**
@@ -120,6 +120,6 @@ public class Drive extends SubsystemBase {
* @param direction The direction (forward or reverse) to run the test in
*/
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
return sysIdRoutine.dynamic(direction);
}
}

39
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/subsystems/Shooter.java
View File

@@ -21,52 +21,51 @@ import org.wpilib.system.RobotController;
public class Shooter extends SubsystemBase {
// The motor on the shooter wheel .
private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
private final PWMSparkMax shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
// The motor on the feeder wheels.
private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
private final PWMSparkMax feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
// The shooter wheel encoder
private final Encoder m_shooterEncoder =
private final Encoder shooterEncoder =
new Encoder(
ShooterConstants.kEncoderPorts[0],
ShooterConstants.kEncoderPorts[1],
ShooterConstants.kEncoderReversed);
// Create a new SysId routine for characterizing the shooter.
private final SysIdRoutine m_sysIdRoutine =
private final SysIdRoutine sysIdRoutine =
new SysIdRoutine(
// Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
// Tell SysId how to plumb the driving voltage to the motor(s).
m_shooterMotor::setVoltage,
shooterMotor::setVoltage,
// Tell SysId how to record a frame of data for each motor on the mechanism being
// characterized.
log -> {
// Record a frame for the shooter motor.
log.motor("shooter-wheel")
.voltage(
Volts.of(
m_shooterMotor.getThrottle() * RobotController.getBatteryVoltage()))
.angularPosition(Rotations.of(m_shooterEncoder.getDistance()))
.angularVelocity(RotationsPerSecond.of(m_shooterEncoder.getRate()));
Volts.of(shooterMotor.getThrottle() * RobotController.getBatteryVoltage()))
.angularPosition(Rotations.of(shooterEncoder.getDistance()))
.angularVelocity(RotationsPerSecond.of(shooterEncoder.getRate()));
},
// Tell SysId to make generated commands require this subsystem, suffix test state in
// WPILog with this subsystem's name ("shooter")
this));
// PID controller to run the shooter wheel in closed-loop, set the constants equal to those
// calculated by SysId
private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0);
private final PIDController shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0);
// Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
// those calculated by SysId
private final SimpleMotorFeedforward m_shooterFeedforward =
private final SimpleMotorFeedforward shooterFeedforward =
new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV, ShooterConstants.kA);
/** Creates a new Shooter subsystem. */
public Shooter() {
// Sets the distance per pulse for the encoders
m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
}
/**
@@ -77,15 +76,15 @@ public class Shooter extends SubsystemBase {
public Command runShooter(DoubleSupplier shooterVelocity) {
// Run shooter wheel at the desired velocity using a PID controller and feedforward.
return run(() -> {
m_shooterMotor.setVoltage(
m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterVelocity.getAsDouble())
+ m_shooterFeedforward.calculate(shooterVelocity.getAsDouble()));
m_feederMotor.setThrottle(ShooterConstants.kFeederVelocity);
shooterMotor.setVoltage(
shooterFeedback.calculate(shooterEncoder.getRate(), shooterVelocity.getAsDouble())
+ shooterFeedforward.calculate(shooterVelocity.getAsDouble()));
feederMotor.setThrottle(ShooterConstants.kFeederVelocity);
})
.finallyDo(
() -> {
m_shooterMotor.stopMotor();
m_feederMotor.stopMotor();
shooterMotor.stopMotor();
feederMotor.stopMotor();
})
.withName("runShooter");
}
@@ -96,7 +95,7 @@ public class Shooter extends SubsystemBase {
* @param direction The direction (forward or reverse) to run the test in
*/
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.quasistatic(direction);
return sysIdRoutine.quasistatic(direction);
}
/**
@@ -105,6 +104,6 @@ public class Shooter extends SubsystemBase {
* @param direction The direction (forward or reverse) to run the test in
*/
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutine.dynamic(direction);
return sysIdRoutine.dynamic(direction);
}
}

18
wpilibjExamples/src/main/java/org/wpilib/examples/tankdrivegamepad/Robot.java
View File

@@ -15,21 +15,21 @@ import org.wpilib.util.sendable.SendableRegistry;
* steering and a gamepad.
*/
public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::setThrottle, m_rightMotor::setThrottle);
private final Gamepad m_driverController = new Gamepad(0);
private final PWMSparkMax leftMotor = new PWMSparkMax(0);
private final PWMSparkMax rightMotor = new PWMSparkMax(1);
private final DifferentialDrive robotDrive =
new DifferentialDrive(leftMotor::setThrottle, rightMotor::setThrottle);
private final Gamepad driverController = new Gamepad(0);
/** Called once at the beginning of the robot program. */
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftMotor);
SendableRegistry.addChild(m_robotDrive, m_rightMotor);
SendableRegistry.addChild(robotDrive, leftMotor);
SendableRegistry.addChild(robotDrive, rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
rightMotor.setInverted(true);
}
@Override
@@ -38,6 +38,6 @@ public class Robot extends TimedRobot {
// That means that the Y axis of the left stick moves the left side
// of the robot forward and backward, and the Y axis of the right stick
// moves the right side of the robot forward and backward.
m_robotDrive.tankDrive(-m_driverController.getLeftY(), -m_driverController.getRightY());
robotDrive.tankDrive(-driverController.getLeftY(), -driverController.getRightY());
}
}

18
wpilibjExamples/src/main/java/org/wpilib/examples/unittest/Robot.java
View File

@@ -15,25 +15,25 @@ import org.wpilib.framework.TimedRobot;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private final Intake m_intake = new Intake();
private final Joystick m_joystick = new Joystick(Constants.kJoystickIndex);
private final Intake intake = new Intake();
private final Joystick joystick = new Joystick(Constants.kJoystickIndex);
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
// Activate the intake while the trigger is held
if (m_joystick.getTrigger()) {
m_intake.activate(IntakeConstants.kIntakeVelocity);
if (joystick.getTrigger()) {
intake.activate(IntakeConstants.kIntakeVelocity);
} else {
m_intake.activate(0);
intake.activate(0);
}
// Toggle deploying the intake when the top button is pressed
if (m_joystick.getTop()) {
if (m_intake.isDeployed()) {
m_intake.retract();
if (joystick.getTop()) {
if (intake.isDeployed()) {
intake.retract();
} else {
m_intake.deploy();
intake.deploy();
}
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/unittest/subsystems/Intake.java
View File

@@ -10,12 +10,12 @@ import org.wpilib.hardware.pneumatic.DoubleSolenoid;
import org.wpilib.hardware.pneumatic.PneumaticsModuleType;
public class Intake implements AutoCloseable {
private final PWMSparkMax m_motor;
private final DoubleSolenoid m_piston;
private final PWMSparkMax motor;
private final DoubleSolenoid piston;
public Intake() {
m_motor = new PWMSparkMax(IntakeConstants.kMotorPort);
m_piston =
motor = new PWMSparkMax(IntakeConstants.kMotorPort);
piston =
new DoubleSolenoid(
0,
PneumaticsModuleType.CTRE_PCM,
@@ -24,29 +24,29 @@ public class Intake implements AutoCloseable {
}
public void deploy() {
m_piston.set(DoubleSolenoid.Value.FORWARD);
piston.set(DoubleSolenoid.Value.FORWARD);
}
public void retract() {
m_piston.set(DoubleSolenoid.Value.REVERSE);
m_motor.setThrottle(0); // turn off the motor
piston.set(DoubleSolenoid.Value.REVERSE);
motor.setThrottle(0); // turn off the motor
}
public void activate(double velocity) {
if (isDeployed()) {
m_motor.setThrottle(velocity);
motor.setThrottle(velocity);
} else { // if piston isn't open, do nothing
m_motor.setThrottle(0);
motor.setThrottle(0);
}
}
public boolean isDeployed() {
return m_piston.get() == DoubleSolenoid.Value.FORWARD;
return piston.get() == DoubleSolenoid.Value.FORWARD;
}
@Override
public void close() {
m_piston.close();
m_motor.close();
piston.close();
motor.close();
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/Robot.java
View File

@@ -14,9 +14,9 @@ import org.wpilib.framework.TimedRobot;
* this project, you must also update the Main.java file in the project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autonomousCommand;
private final RobotContainer m_robotContainer;
private final RobotContainer robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
@@ -25,7 +25,7 @@ public class Robot extends TimedRobot {
public Robot() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
robotContainer = new RobotContainer();
}
/**
@@ -54,11 +54,11 @@ public class Robot extends TimedRobot {
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autonomousCommand = robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
@@ -72,8 +72,8 @@ public class Robot extends TimedRobot {
// 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();
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}

36
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/RobotContainer.java
View File

@@ -29,15 +29,15 @@ import org.wpilib.xrp.XRPOnBoardIO;
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final XRPOnBoardIO m_onboardIO = new XRPOnBoardIO();
private final Arm m_arm = new Arm();
private final Drivetrain drivetrain = new Drivetrain();
private final XRPOnBoardIO onboardIO = new XRPOnBoardIO();
private final Arm arm = new Arm();
// Assumes a gamepad plugged into channel 0
private final Joystick m_controller = new Joystick(0);
private final Joystick controller = new Joystick(0);
// Create SmartDashboard chooser for autonomous routines
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
private final SendableChooser<Command> chooser = new SendableChooser<>();
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
@@ -54,28 +54,28 @@ public class RobotContainer {
private void configureButtonBindings() {
// Default command is arcade drive. This will run unless another command
// is scheduled over it.
m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
drivetrain.setDefaultCommand(getArcadeDriveCommand());
// Example of how to use the onboard IO
Trigger userButton = new Trigger(m_onboardIO::getUserButtonPressed);
Trigger userButton = new Trigger(onboardIO::getUserButtonPressed);
userButton
.onTrue(new PrintCommand("USER Button Pressed"))
.onFalse(new PrintCommand("USER Button Released"));
JoystickButton joystickAButton = new JoystickButton(m_controller, 1);
JoystickButton joystickAButton = new JoystickButton(controller, 1);
joystickAButton
.onTrue(new InstantCommand(() -> m_arm.setAngle(45.0), m_arm))
.onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
.onTrue(new InstantCommand(() -> arm.setAngle(45.0), arm))
.onFalse(new InstantCommand(() -> arm.setAngle(0.0), arm));
JoystickButton joystickBButton = new JoystickButton(m_controller, 2);
JoystickButton joystickBButton = new JoystickButton(controller, 2);
joystickBButton
.onTrue(new InstantCommand(() -> m_arm.setAngle(90.0), m_arm))
.onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm));
.onTrue(new InstantCommand(() -> arm.setAngle(90.0), arm))
.onFalse(new InstantCommand(() -> arm.setAngle(0.0), arm));
// Setup SmartDashboard options
m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
SmartDashboard.putData(m_chooser);
chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(drivetrain));
chooser.addOption("Auto Routine Time", new AutonomousTime(drivetrain));
SmartDashboard.putData(chooser);
}
/**
@@ -84,7 +84,7 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return m_chooser.getSelected();
return chooser.getSelected();
}
/**
@@ -94,6 +94,6 @@ public class RobotContainer {
*/
public Command getArcadeDriveCommand() {
return new ArcadeDrive(
m_drivetrain, () -> -m_controller.getRawAxis(1), () -> -m_controller.getRawAxis(2));
drivetrain, () -> -controller.getRawAxis(1), () -> -controller.getRawAxis(2));
}
}

14
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/ArcadeDrive.java
View File

@@ -9,9 +9,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class ArcadeDrive extends Command {
private final Drivetrain m_drivetrain;
private final Supplier<Double> m_xaxisVelocitySupplier;
private final Supplier<Double> m_zaxisRotateSupplier;
private final Drivetrain drivetrain;
private final Supplier<Double> xaxisVelocitySupplier;
private final Supplier<Double> zaxisRotateSupplier;
/**
* Creates a new ArcadeDrive. This command will drive your robot according to the velocity
@@ -25,9 +25,9 @@ public class ArcadeDrive extends Command {
Drivetrain drivetrain,
Supplier<Double> xaxisVelocitySupplier,
Supplier<Double> zaxisRotateSupplier) {
m_drivetrain = drivetrain;
m_xaxisVelocitySupplier = xaxisVelocitySupplier;
m_zaxisRotateSupplier = zaxisRotateSupplier;
this.drivetrain = drivetrain;
this.xaxisVelocitySupplier = xaxisVelocitySupplier;
this.zaxisRotateSupplier = zaxisRotateSupplier;
addRequirements(drivetrain);
}
@@ -38,7 +38,7 @@ public class ArcadeDrive extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drivetrain.arcadeDrive(m_xaxisVelocitySupplier.get(), m_zaxisRotateSupplier.get());
drivetrain.arcadeDrive(xaxisVelocitySupplier.get(), zaxisRotateSupplier.get());
}
// Called once the command ends or is interrupted.

22
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/DriveDistance.java
View File

@@ -8,9 +8,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class DriveDistance extends Command {
private final Drivetrain m_drive;
private final double m_distance;
private final double m_velocity;
private final Drivetrain drive;
private final double distance;
private final double velocity;
/**
* Creates a new DriveDistance. This command will drive your your robot for a desired distance at
@@ -21,35 +21,35 @@ public class DriveDistance extends Command {
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveDistance(double velocity, double inches, Drivetrain drive) {
m_distance = inches;
m_velocity = velocity;
m_drive = drive;
distance = inches;
this.velocity = velocity;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
drive.arcadeDrive(0, 0);
drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_velocity, 0);
drive.arcadeDrive(velocity, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// Compare distance travelled from start to desired distance
return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance;
return Math.abs(drive.getAverageDistanceInch()) >= distance;
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/DriveTime.java
View File

@@ -8,10 +8,10 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class DriveTime extends Command {
private final double m_duration;
private final double m_velocity;
private final Drivetrain m_drive;
private long m_startTime;
private final double duration;
private final double velocity;
private final Drivetrain drive;
private long startTime;
/**
* Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
@@ -21,34 +21,34 @@ public class DriveTime extends Command {
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveTime(double velocity, double time, Drivetrain drive) {
m_velocity = velocity;
m_duration = time * 1000;
m_drive = drive;
this.velocity = velocity;
duration = time * 1000;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
startTime = System.currentTimeMillis();
drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_velocity, 0);
drive.arcadeDrive(velocity, 0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
return (System.currentTimeMillis() - startTime) >= duration;
}
}

26
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/TurnDegrees.java
View File

@@ -8,9 +8,9 @@ import org.wpilib.command2.Command;
import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class TurnDegrees extends Command {
private final Drivetrain m_drive;
private final double m_degrees;
private final double m_velocity;
private final Drivetrain drive;
private final double degrees;
private final double velocity;
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
@@ -21,9 +21,9 @@ public class TurnDegrees extends Command {
* @param drive The drive subsystem on which this command will run
*/
public TurnDegrees(double velocity, double degrees, Drivetrain drive) {
m_degrees = degrees;
m_velocity = velocity;
m_drive = drive;
this.degrees = degrees;
this.velocity = velocity;
this.drive = drive;
addRequirements(drive);
}
@@ -31,20 +31,20 @@ public class TurnDegrees extends Command {
@Override
public void initialize() {
// Set motors to stop, read encoder values for starting point
m_drive.arcadeDrive(0, 0);
m_drive.resetEncoders();
drive.arcadeDrive(0, 0);
drive.resetEncoders();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_velocity);
drive.arcadeDrive(0, velocity);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@@ -57,12 +57,12 @@ public class TurnDegrees extends Command {
*/
double inchPerDegree = Math.PI * 6.102 / 360;
// Compare distance travelled from start to distance based on degree turn
return getAverageTurningDistance() >= (inchPerDegree * m_degrees);
return getAverageTurningDistance() >= (inchPerDegree * degrees);
}
private double getAverageTurningDistance() {
double leftDistance = Math.abs(m_drive.getLeftDistanceInch());
double rightDistance = Math.abs(m_drive.getRightDistanceInch());
double leftDistance = Math.abs(drive.getLeftDistanceInch());
double rightDistance = Math.abs(drive.getRightDistanceInch());
return (leftDistance + rightDistance) / 2.0;
}
}

24
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/TurnTime.java
View File

@@ -12,10 +12,10 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
* desired rotational velocity and time.
*/
public class TurnTime extends Command {
private final double m_duration;
private final double m_rotationalVelocity;
private final Drivetrain m_drive;
private long m_startTime;
private final double duration;
private final double rotationalVelocity;
private final Drivetrain drive;
private long startTime;
/**
* Creates a new TurnTime.
@@ -25,34 +25,34 @@ public class TurnTime extends Command {
* @param drive The drive subsystem on which this command will run
*/
public TurnTime(double velocity, double time, Drivetrain drive) {
m_rotationalVelocity = velocity;
m_duration = time * 1000;
m_drive = drive;
rotationalVelocity = velocity;
duration = time * 1000;
this.drive = drive;
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
m_drive.arcadeDrive(0, 0);
startTime = System.currentTimeMillis();
drive.arcadeDrive(0, 0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_rotationalVelocity);
drive.arcadeDrive(0, rotationalVelocity);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_drive.arcadeDrive(0, 0);
drive.arcadeDrive(0, 0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (System.currentTimeMillis() - m_startTime) >= m_duration;
return (System.currentTimeMillis() - startTime) >= duration;
}
}

6
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/subsystems/Arm.java
View File

@@ -8,12 +8,12 @@ import org.wpilib.command2.SubsystemBase;
import org.wpilib.xrp.XRPServo;
public class Arm extends SubsystemBase {
private final XRPServo m_armServo;
private final XRPServo armServo;
/** Creates a new Arm. */
public Arm() {
// Device number 4 maps to the physical Servo 1 port on the XRP
m_armServo = new XRPServo(4);
armServo = new XRPServo(4);
}
@Override
@@ -27,6 +27,6 @@ public class Arm extends SubsystemBase {
* @param angleDeg Desired arm angle in degrees
*/
public void setAngle(double angleDeg) {
m_armServo.setAngle(angleDeg);
armServo.setAngle(angleDeg);
}
}

46
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/subsystems/Drivetrain.java
View File

@@ -20,60 +20,60 @@ public class Drivetrain extends SubsystemBase {
// The XRP has the left and right motors set to
// channels 0 and 1 respectively
private final XRPMotor m_leftMotor = new XRPMotor(0);
private final XRPMotor m_rightMotor = new XRPMotor(1);
private final XRPMotor leftMotor = new XRPMotor(0);
private final XRPMotor rightMotor = new XRPMotor(1);
// The XRP 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);
private final Encoder leftEncoder = new Encoder(4, 5);
private final Encoder rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::setThrottle, m_rightMotor::setThrottle);
private final DifferentialDrive diffDrive =
new DifferentialDrive(leftMotor::setThrottle, rightMotor::setThrottle);
// Set up the XRPGyro
private final XRPGyro m_gyro = new XRPGyro();
private final XRPGyro gyro = new XRPGyro();
/** Creates a new Drivetrain. */
public Drivetrain() {
SendableRegistry.addChild(m_diffDrive, m_leftMotor);
SendableRegistry.addChild(m_diffDrive, m_rightMotor);
SendableRegistry.addChild(diffDrive, leftMotor);
SendableRegistry.addChild(diffDrive, rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
rightMotor.setInverted(true);
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
}
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
leftEncoder.reset();
rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
return leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
return rightEncoder.get();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
return leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
return rightEncoder.getDistance();
}
public double getAverageDistanceInch() {
@@ -86,7 +86,7 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleX() {
return m_gyro.getAngleX();
return gyro.getAngleX();
}
/**
@@ -95,7 +95,7 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleY() {
return m_gyro.getAngleY();
return gyro.getAngleY();
}
/**
@@ -104,12 +104,12 @@ public class Drivetrain extends SubsystemBase {
* @return The current angle of the XRP in degrees
*/
public double getGyroAngleZ() {
return m_gyro.getAngleZ();
return gyro.getAngleZ();
}
/** Reset the gyro. */
public void resetGyro() {
m_gyro.reset();
gyro.reset();
}
@Override

28
wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java
View File

@@ -17,40 +17,40 @@ import org.wpilib.xrp.XRPMotor;
* this project, you must also update the manifest file in the resource directory.
*/
public class Robot extends TimedRobot {
private final XRPMotor m_leftDrive = new XRPMotor(0);
private final XRPMotor m_rightDrive = new XRPMotor(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::setThrottle, m_rightDrive::setThrottle);
private final XRPMotor leftDrive = new XRPMotor(0);
private final XRPMotor rightDrive = new XRPMotor(1);
private final DifferentialDrive robotDrive =
new DifferentialDrive(leftDrive::setThrottle, rightDrive::setThrottle);
// Assumes a gamepad plugged into channel 0
private final Joystick m_controller = new Joystick(0);
private final Timer m_timer = new Timer();
private final Joystick controller = new Joystick(0);
private final Timer timer = new Timer();
/** Called once at the beginning of the robot program. */
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftDrive);
SendableRegistry.addChild(m_robotDrive, m_rightDrive);
SendableRegistry.addChild(robotDrive, leftDrive);
SendableRegistry.addChild(robotDrive, rightDrive);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightDrive.setInverted(true);
rightDrive.setInverted(true);
}
/** This function is run once each time the robot enters autonomous mode. */
@Override
public void autonomousInit() {
m_timer.restart();
timer.restart();
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
// Drive for 2 seconds
if (m_timer.get() < 2.0) {
if (timer.get() < 2.0) {
// Drive forwards half speed, make sure to turn input squaring off
m_robotDrive.arcadeDrive(0.5, 0.0, false);
robotDrive.arcadeDrive(0.5, 0.0, false);
} else {
m_robotDrive.stopMotor(); // stop robot
robotDrive.stopMotor(); // stop robot
}
}
@@ -61,7 +61,7 @@ public class Robot extends TimedRobot {
/** This function is called periodically during teleoperated mode. */
@Override
public void teleopPeriodic() {
m_robotDrive.arcadeDrive(-m_controller.getRawAxis(2), -m_controller.getRawAxis(1));
robotDrive.arcadeDrive(-controller.getRawAxis(2), -controller.getRawAxis(1));
}
/** This function is called once each time the robot enters utility mode. */

18
wpilibjExamples/src/main/java/org/wpilib/snippets/accelerometercollision/Robot.java
View File

@@ -15,21 +15,21 @@ import org.wpilib.smartdashboard.SmartDashboard;
*/
@SuppressWarnings("checkstyle:VariableDeclarationUsageDistance")
public class Robot extends TimedRobot {
double m_prevXAccel;
double m_prevYAccel;
OnboardIMU m_accelerometer = new OnboardIMU(MountOrientation.FLAT);
double prevXAccel;
double prevYAccel;
OnboardIMU accelerometer = new OnboardIMU(MountOrientation.FLAT);
@Override
public void robotPeriodic() {
// Gets the current accelerations in the X and Y directions
double xAccel = m_accelerometer.getAccelX();
double yAccel = m_accelerometer.getAccelY();
double xAccel = accelerometer.getAccelX();
double yAccel = accelerometer.getAccelY();
// Calculates the jerk in the X and Y directions
// Divides by .02 because default loop timing is 20ms
double xJerk = (xAccel - m_prevXAccel) / 0.02;
double yJerk = (yAccel - m_prevYAccel) / 0.02;
m_prevXAccel = xAccel;
m_prevYAccel = yAccel;
double xJerk = (xAccel - prevXAccel) / 0.02;
double yJerk = (yAccel - prevYAccel) / 0.02;
prevXAccel = xAccel;
prevYAccel = yAccel;
SmartDashboard.putNumber("X Jerk", xJerk);
SmartDashboard.putNumber("Y Jerk", yJerk);

8
wpilibjExamples/src/main/java/org/wpilib/snippets/accelerometerfilter/Robot.java
View File

@@ -15,16 +15,16 @@ import org.wpilib.smartdashboard.SmartDashboard;
* https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html
*/
public class Robot extends TimedRobot {
OnboardIMU m_accelerometer = new OnboardIMU(MountOrientation.FLAT);
OnboardIMU accelerometer = new OnboardIMU(MountOrientation.FLAT);
// Create a LinearFilter that will calculate a moving average of the measured X acceleration over
// the past 10 iterations of the main loop
LinearFilter m_xAccelFilter = LinearFilter.movingAverage(10);
LinearFilter xAccelFilter = LinearFilter.movingAverage(10);
@Override
public void robotPeriodic() {
double xAccel = m_accelerometer.getAccelX();
double xAccel = accelerometer.getAccelX();
// Get the filtered X acceleration
double filteredXAccel = m_xAccelFilter.calculate(xAccel);
double filteredXAccel = xAccelFilter.calculate(xAccel);
SmartDashboard.putNumber("X Acceleration", xAccel);
SmartDashboard.putNumber("Filtered X Acceleration", filteredXAccel);

22
wpilibjExamples/src/main/java/org/wpilib/snippets/addressableled/Robot.java
View File

@@ -14,12 +14,12 @@ import org.wpilib.hardware.led.LEDPattern;
import org.wpilib.units.measure.Distance;
public class Robot extends TimedRobot {
private final AddressableLED m_led;
private final AddressableLEDBuffer m_ledBuffer;
private final AddressableLED led;
private final AddressableLEDBuffer ledBuffer;
// Create an LED pattern that will display a rainbow across
// all hues at maximum saturation and half brightness
private final LEDPattern m_rainbow = LEDPattern.rainbow(255, 128);
private final LEDPattern rainbow = LEDPattern.rainbow(255, 128);
// Our LED strip has a density of 120 LEDs per meter
private static final Distance kLedSpacing = Meters.of(1 / 120.0);
@@ -27,28 +27,28 @@ public class Robot extends TimedRobot {
// Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a
// velocity
// of 1 meter per second.
private final LEDPattern m_scrollingRainbow =
m_rainbow.scrollAtAbsoluteVelocity(MetersPerSecond.of(1), kLedSpacing);
private final LEDPattern scrollingRainbow =
rainbow.scrollAtAbsoluteVelocity(MetersPerSecond.of(1), kLedSpacing);
/** Called once at the beginning of the robot program. */
public Robot() {
// SmartIO port 1
m_led = new AddressableLED(1);
led = new AddressableLED(1);
// Reuse buffer
// Default to a length of 60
m_ledBuffer = new AddressableLEDBuffer(60);
m_led.setLength(m_ledBuffer.getLength());
ledBuffer = new AddressableLEDBuffer(60);
led.setLength(ledBuffer.getLength());
// Set the data
m_led.setData(m_ledBuffer);
led.setData(ledBuffer);
}
@Override
public void robotPeriodic() {
// Update the buffer with the rainbow animation
m_scrollingRainbow.applyTo(m_ledBuffer);
scrollingRainbow.applyTo(ledBuffer);
// Set the LEDs
m_led.setData(m_ledBuffer);
led.setData(ledBuffer);
}
}

8
wpilibjExamples/src/main/java/org/wpilib/snippets/adxlaccelerometers/Robot.java
View File

@@ -14,7 +14,7 @@ import org.wpilib.hardware.bus.I2C;
*/
public class Robot extends TimedRobot {
// Creates an ADXL345 accelerometer object with a measurement range from -8 to 8 G's
ADXL345_I2C m_accelerometer345I2C = new ADXL345_I2C(I2C.Port.PORT_0, 8);
ADXL345_I2C accelerometer345I2C = new ADXL345_I2C(I2C.Port.PORT_0, 8);
/** Called once at the beginning of the robot program. */
public Robot() {}
@@ -22,10 +22,10 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Gets the current acceleration in the X axis
m_accelerometer345I2C.getX();
accelerometer345I2C.getX();
// Gets the current acceleration in the Y axis
m_accelerometer345I2C.getY();
accelerometer345I2C.getY();
// Gets the current acceleration in the Z axis
m_accelerometer345I2C.getZ();
accelerometer345I2C.getZ();
}
}

8
wpilibjExamples/src/main/java/org/wpilib/snippets/analogaccelerometer/Robot.java
View File

@@ -13,19 +13,19 @@ import org.wpilib.hardware.accelerometer.AnalogAccelerometer;
*/
public class Robot extends TimedRobot {
// Creates an analog accelerometer on analog input 0
AnalogAccelerometer m_accelerometer = new AnalogAccelerometer(0);
AnalogAccelerometer accelerometer = new AnalogAccelerometer(0);
/** Called once at the beginning of the robot program. */
public Robot() {
// Sets the sensitivity of the accelerometer to 1 volt per G
m_accelerometer.setSensitivity(1);
accelerometer.setSensitivity(1);
// Sets the zero voltage of the accelerometer to 3 volts
m_accelerometer.setZero(3);
accelerometer.setZero(3);
}
@Override
public void teleopPeriodic() {
// Gets the current acceleration
m_accelerometer.getAcceleration();
accelerometer.getAcceleration();
}
}

8
wpilibjExamples/src/main/java/org/wpilib/snippets/analogencoder/Robot.java
View File

@@ -13,12 +13,12 @@ import org.wpilib.hardware.rotation.AnalogEncoder;
*/
public class Robot extends TimedRobot {
// Initializes an analog encoder on Analog Input pin 0
AnalogEncoder m_encoder = new AnalogEncoder(0);
AnalogEncoder encoder = new AnalogEncoder(0);
// Initializes an analog encoder on DIO pins 0 to return a value of 4 for
// a full rotation, with the encoder reporting 0 half way through rotation (2
// out of 4)
AnalogEncoder m_encoderFR = new AnalogEncoder(0, 4.0, 2.0);
AnalogEncoder encoderFR = new AnalogEncoder(0, 4.0, 2.0);
/** Called once at the beginning of the robot program. */
public Robot() {}
@@ -26,8 +26,8 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Gets the rotation
m_encoder.get();
encoder.get();
m_encoderFR.get();
encoderFR.get();
}
}

6
wpilibjExamples/src/main/java/org/wpilib/snippets/analoginput/Robot.java
View File

@@ -13,7 +13,7 @@ import org.wpilib.hardware.discrete.AnalogInput;
*/
public class Robot extends TimedRobot {
// Initializes an AnalogInput on port 0
AnalogInput m_analog = new AnalogInput(0);
AnalogInput analog = new AnalogInput(0);
/** Called once at the beginning of the robot program. */
public Robot() {}
@@ -23,10 +23,10 @@ public class Robot extends TimedRobot {
// Gets the raw instantaneous measured value from the analog input, without
// applying any calibration and ignoring oversampling and averaging
// settings.
m_analog.getValue();
analog.getValue();
// Gets the instantaneous measured voltage from the analog input.
// Oversampling and averaging settings are ignored
m_analog.getVoltage();
analog.getVoltage();
}
}

10
wpilibjExamples/src/main/java/org/wpilib/snippets/analogpotentiometer/Robot.java
View File

@@ -18,16 +18,16 @@ public class Robot extends TimedRobot {
// instance)
// The "starting point" of the motion, i.e. where the mechanism is located when the potentiometer
// reads 0v, is 30.
AnalogPotentiometer m_pot = new AnalogPotentiometer(0, 180, 30);
AnalogPotentiometer pot = new AnalogPotentiometer(0, 180, 30);
// Initializes an AnalogInput on port 1
AnalogInput m_input = new AnalogInput(0);
AnalogInput input = new AnalogInput(0);
// Initializes an AnalogPotentiometer with the given AnalogInput
// The full range of motion (in meaningful external units) is 0-180 (this could be degrees, for
// instance)
// The "starting point" of the motion, i.e. where the mechanism is located when the potentiometer
// reads 0v, is 30.
AnalogPotentiometer m_pot1 = new AnalogPotentiometer(m_input, 180, 30);
AnalogPotentiometer pot1 = new AnalogPotentiometer(input, 180, 30);
/** Called once at the beginning of the robot program. */
public Robot() {}
@@ -35,8 +35,8 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Get the value of the potentiometer
m_pot.get();
pot.get();
m_pot1.get();
pot1.get();
}
}

16
wpilibjExamples/src/main/java/org/wpilib/snippets/canpdp/Robot.java
View File

@@ -13,11 +13,11 @@ import org.wpilib.smartdashboard.SmartDashboard;
* via CAN. The information will be displayed under variables through the SmartDashboard.
*/
public class Robot extends TimedRobot {
private final PowerDistribution m_pdp = new PowerDistribution(0);
private final PowerDistribution pdp = new PowerDistribution(0);
public Robot() {
// Put the PDP itself to the dashboard
SmartDashboard.putData("PDP", m_pdp);
SmartDashboard.putData("PDP", pdp);
}
@Override
@@ -25,30 +25,30 @@ public class Robot extends TimedRobot {
// Get the current going through channel 7, in Amperes.
// The PDP returns the current in increments of 0.125A.
// At low currents the current readings tend to be less accurate.
double current7 = m_pdp.getCurrent(7);
double current7 = pdp.getCurrent(7);
SmartDashboard.putNumber("Current Channel 7", current7);
// Get the voltage going into the PDP, in Volts.
// The PDP returns the voltage in increments of 0.05 Volts.
double voltage = m_pdp.getVoltage();
double voltage = pdp.getVoltage();
SmartDashboard.putNumber("Voltage", voltage);
// Retrieves the temperature of the PDP, in degrees Celsius.
double temperatureCelsius = m_pdp.getTemperature();
double temperatureCelsius = pdp.getTemperature();
SmartDashboard.putNumber("Temperature", temperatureCelsius);
// Get the total current of all channels.
double totalCurrent = m_pdp.getTotalCurrent();
double totalCurrent = pdp.getTotalCurrent();
SmartDashboard.putNumber("Total Current", totalCurrent);
// Get the total power of all channels.
// Power is the bus voltage multiplied by the current with the units Watts.
double totalPower = m_pdp.getTotalPower();
double totalPower = pdp.getTotalPower();
SmartDashboard.putNumber("Total Power", totalPower);
// Get the total energy of all channels.
// Energy is the power summed over time with units Joules.
double totalEnergy = m_pdp.getTotalEnergy();
double totalEnergy = pdp.getTotalEnergy();
SmartDashboard.putNumber("Total Energy", totalEnergy);
}
}

24
wpilibjExamples/src/main/java/org/wpilib/snippets/digitalcommunication/Robot.java
View File

@@ -22,10 +22,10 @@ public class Robot extends TimedRobot {
static final int kAutonomousPort = 2;
static final int kAlertPort = 3;
private final DigitalOutput m_allianceOutput = new DigitalOutput(kAlliancePort);
private final DigitalOutput m_enabledOutput = new DigitalOutput(kEnabledPort);
private final DigitalOutput m_autonomousOutput = new DigitalOutput(kAutonomousPort);
private final DigitalOutput m_alertOutput = new DigitalOutput(kAlertPort);
private final DigitalOutput allianceOutput = new DigitalOutput(kAlliancePort);
private final DigitalOutput enabledOutput = new DigitalOutput(kEnabledPort);
private final DigitalOutput autonomousOutput = new DigitalOutput(kAutonomousPort);
private final DigitalOutput alertOutput = new DigitalOutput(kAlertPort);
@Override
public void robotPeriodic() {
@@ -36,26 +36,26 @@ public class Robot extends TimedRobot {
}
// pull alliance port high if on red alliance, pull low if on blue alliance
m_allianceOutput.set(setAlliance);
allianceOutput.set(setAlliance);
// pull enabled port high if enabled, low if disabled
m_enabledOutput.set(RobotState.isEnabled());
enabledOutput.set(RobotState.isEnabled());
// pull auto port high if in autonomous, low if in teleop (or disabled)
m_autonomousOutput.set(RobotState.isAutonomous());
autonomousOutput.set(RobotState.isAutonomous());
// pull alert port high if match time remaining is between 30 and 25 seconds
var matchTime = MatchState.getMatchTime();
m_alertOutput.set(matchTime <= 30 && matchTime >= 25);
alertOutput.set(matchTime <= 30 && matchTime >= 25);
}
/** Close all resources. */
@Override
public void close() {
m_allianceOutput.close();
m_enabledOutput.close();
m_autonomousOutput.close();
m_alertOutput.close();
allianceOutput.close();
enabledOutput.close();
autonomousOutput.close();
alertOutput.close();
super.close();
}
}

4
wpilibjExamples/src/main/java/org/wpilib/snippets/digitalinput/Robot.java
View File

@@ -13,11 +13,11 @@ import org.wpilib.hardware.discrete.DigitalInput;
*/
public class Robot extends TimedRobot {
// Initializes a DigitalInput on DIO 0
DigitalInput m_input = new DigitalInput(0);
DigitalInput input = new DigitalInput(0);
@Override
public void teleopPeriodic() {
// Gets the value of the digital input. Returns true if the circuit is open.
m_input.get();
input.get();
}
}

10
wpilibjExamples/src/main/java/org/wpilib/snippets/dutycycleencoder/Robot.java
View File

@@ -13,12 +13,12 @@ import org.wpilib.hardware.rotation.DutyCycleEncoder;
*/
public class Robot extends TimedRobot {
// Initializes a duty cycle encoder on DIO pins 0
DutyCycleEncoder m_encoder = new DutyCycleEncoder(0);
DutyCycleEncoder encoder = new DutyCycleEncoder(0);
// Initializes a duty cycle encoder on DIO pins 0 to return a value of 4 for
// a full rotation, with the encoder reporting 0 half way through rotation (2
// out of 4)
DutyCycleEncoder m_encoderFR = new DutyCycleEncoder(0, 4.0, 2.0);
DutyCycleEncoder encoderFR = new DutyCycleEncoder(0, 4.0, 2.0);
/** Called once at the beginning of the robot program. */
public Robot() {}
@@ -26,11 +26,11 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Gets the rotation
m_encoder.get();
encoder.get();
// Gets if the encoder is connected
m_encoder.isConnected();
encoder.isConnected();
m_encoderFR.get();
encoderFR.get();
}
}

8
wpilibjExamples/src/main/java/org/wpilib/snippets/dutycycleinput/Robot.java
View File

@@ -9,20 +9,20 @@ import org.wpilib.hardware.rotation.DutyCycle;
import org.wpilib.smartdashboard.SmartDashboard;
public class Robot extends TimedRobot {
private final DutyCycle m_dutyCycle;
private final DutyCycle dutyCycle;
public Robot() {
m_dutyCycle = new DutyCycle(0);
dutyCycle = new DutyCycle(0);
}
@Override
public void robotPeriodic() {
// Duty Cycle Frequency in Hz
double frequency = m_dutyCycle.getFrequency();
double frequency = dutyCycle.getFrequency();
// Output of duty cycle, ranging from 0 to 1
// 1 is fully on, 0 is fully off
double output = m_dutyCycle.getOutput();
double output = dutyCycle.getOutput();
SmartDashboard.putNumber("Frequency", frequency);
SmartDashboard.putNumber("Duty Cycle", output);

28
wpilibjExamples/src/main/java/org/wpilib/snippets/encoder/Robot.java
View File

@@ -15,48 +15,48 @@ import org.wpilib.hardware.rotation.Encoder;
public class Robot extends TimedRobot {
// Initializes an encoder on DIO pins 0 and 1
// Defaults to 4X decoding and non-inverted
Encoder m_encoder = new Encoder(0, 1);
Encoder encoder = new Encoder(0, 1);
// Initializes an encoder on DIO pins 0 and 1
// 2X encoding and non-inverted
Encoder m_encoder2x = new Encoder(0, 1, false, Encoder.EncodingType.X2);
Encoder encoder2x = new Encoder(0, 1, false, Encoder.EncodingType.X2);
/** Called once at the beginning of the robot program. */
public Robot() {
// Configures the encoder to return a distance of 4 for every 256 pulses
// Also changes the units of getRate
m_encoder.setDistancePerPulse(4.0 / 256.0);
encoder.setDistancePerPulse(4.0 / 256.0);
// Configures the encoder to consider itself stopped after .1 seconds
m_encoder.setMaxPeriod(0.1);
encoder.setMaxPeriod(0.1);
// Configures the encoder to consider itself stopped when its rate is below 10
m_encoder.setMinRate(10);
encoder.setMinRate(10);
// Reverses the direction of the encoder
m_encoder.setReverseDirection(true);
encoder.setReverseDirection(true);
// Configures an encoder to average its period measurement over 5 samples
// Can be between 1 and 127 samples
m_encoder.setSamplesToAverage(5);
encoder.setSamplesToAverage(5);
m_encoder2x.getRate();
encoder2x.getRate();
}
@Override
public void teleopPeriodic() {
// Gets the distance traveled
m_encoder.getDistance();
encoder.getDistance();
// Gets the current rate of the encoder
m_encoder.getRate();
encoder.getRate();
// Gets whether the encoder is stopped
m_encoder.getStopped();
encoder.getStopped();
// Gets the last direction in which the encoder moved
m_encoder.getDirection();
encoder.getDirection();
// Gets the current period of the encoder
m_encoder.getPeriod();
encoder.getPeriod();
// Resets the encoder to read a distance of zero
m_encoder.reset();
encoder.reset();
}
}

28
wpilibjExamples/src/main/java/org/wpilib/snippets/encoderdrive/Robot.java
View File

@@ -15,35 +15,35 @@ import org.wpilib.hardware.rotation.Encoder;
*/
public class Robot extends TimedRobot {
// Creates an encoder on DIO ports 0 and 1
Encoder m_encoder = new Encoder(0, 1);
Encoder encoder = new Encoder(0, 1);
// Initialize motor controllers and drive
Spark m_leftLeader = new Spark(0);
Spark m_leftFollower = new Spark(1);
Spark m_rightLeader = new Spark(2);
Spark m_rightFollower = new Spark(3);
DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setThrottle, m_rightLeader::setThrottle);
Spark leftLeader = new Spark(0);
Spark leftFollower = new Spark(1);
Spark rightLeader = new Spark(2);
Spark rightFollower = new Spark(3);
DifferentialDrive drive =
new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
/** Called once at the beginning of the robot program. */
public Robot() {
// Configures the encoder's distance-per-pulse
// The robot moves forward 1 foot per encoder rotation
// There are 256 pulses per encoder rotation
m_encoder.setDistancePerPulse(1.0 / 256.0);
encoder.setDistancePerPulse(1.0 / 256.0);
// Invert the right side of the drivetrain. You might have to invert the other side
m_rightLeader.setInverted(true);
rightLeader.setInverted(true);
// Configure the followers to follow the leaders
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
}
/** Drives forward at half velocity until the robot has moved 5 feet, then stops. */
@Override
public void autonomousPeriodic() {
if (m_encoder.getDistance() < 5.0) {
m_drive.tankDrive(0.5, 0.5);
if (encoder.getDistance() < 5.0) {
drive.tankDrive(0.5, 0.5);
} else {
m_drive.tankDrive(0.0, 0.0);
drive.tankDrive(0.0, 0.0);
}
}
}

14
wpilibjExamples/src/main/java/org/wpilib/snippets/encoderhoming/Robot.java
View File

@@ -14,10 +14,10 @@ import org.wpilib.hardware.rotation.Encoder;
* https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
*/
public class Robot extends TimedRobot {
Encoder m_encoder = new Encoder(0, 1);
Spark m_spark = new Spark(0);
Encoder encoder = new Encoder(0, 1);
Spark spark = new Spark(0);
// Limit switch on DIO 2
DigitalInput m_limit = new DigitalInput(2);
DigitalInput limit = new DigitalInput(2);
/**
* Runs the motor backwards at half velocity until the limit switch is pressed then turn off the
@@ -25,11 +25,11 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousPeriodic() {
if (!m_limit.get()) {
m_spark.setThrottle(-0.5);
if (!limit.get()) {
spark.setThrottle(-0.5);
} else {
m_spark.setThrottle(0.0);
m_encoder.reset();
spark.setThrottle(0.0);
encoder.reset();
}
}
}

Some files were not shown because too many files have changed in this diff Show More