Files
allwpilib/robotpyExamples/DifferentialDrivePoseEstimator/robot.py
Gold856 35e8abedeb Don't force public variables to use Hungarian notation (#8774)
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.
2026-04-25 11:32:08 -07:00

53 lines
1.8 KiB
Python

#!/usr/bin/env python3
#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#
import ntcore
import wpilib
import wpimath
from drivetrain import Drivetrain
class MyRobot(wpilib.TimedRobot):
def __init__(self) -> None:
super().__init__()
self.inst = ntcore.NetworkTableInstance.getDefault()
self.doubleArrayTopic = self.inst.getDoubleArrayTopic("doubleArrayTopic")
self.controller = wpilib.NiDsXboxController(0)
self.drive = Drivetrain(self.doubleArrayTopic)
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
self.velocityLimiter = wpimath.SlewRateLimiter(3)
self.rotLimiter = wpimath.SlewRateLimiter(3)
def autonomousPeriodic(self) -> None:
self.teleopPeriodic()
self.drive.updateOdometry()
def simulationPeriodic(self) -> None:
self.drive.simulationPeriodic()
def robotPeriodic(self) -> None:
self.drive.periodic()
def teleopPeriodic(self) -> None:
# Get the x velocity. We are inverting this because Xbox controllers return
# negative values when we push forward.
xVelocity = -self.velocityLimiter.calculate(self.controller.getLeftY())
xVelocity *= 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.
rot = -self.rotLimiter.calculate(self.controller.getRightX())
rot *= Drivetrain.kMaxAngularVelocity
self.drive.drive(xVelocity, rot)