@@ -40,7 +40,7 @@ def robotInit(self):
40
40
wpimath .geometry .Pose2d (3 , 0 , wpimath .geometry .Rotation2d .fromDegrees (0 )),
41
41
wpimath .trajectory .TrajectoryConfig (
42
42
wpimath .units .feetToMeters (3.0 ), wpimath .units .feetToMeters (3.0 )
43
- )
43
+ ),
44
44
)
45
45
46
46
# The Ramsete Controller to follow the trajectory.
@@ -78,7 +78,9 @@ def autonomousPeriodic(self):
78
78
desiredPose = self .trajectory .sample (self .timer .get ())
79
79
80
80
# Get the reference chassis speeds from the Ramsete controller.
81
- refChassisSpeeds = self .ramseteController .calculate (self .drive .getPose (), desiredPose )
81
+ refChassisSpeeds = self .ramseteController .calculate (
82
+ self .drive .getPose (), desiredPose
83
+ )
82
84
83
85
# Set the linear and angular speeds.
84
86
self .drive .drive (refChassisSpeeds .vx , refChassisSpeeds .omega )
@@ -88,13 +90,19 @@ def autonomousPeriodic(self):
88
90
def teleopPeriodic (self ):
89
91
# Get the x speed. We are inverting this because Xbox controllers return
90
92
# negative values when we push forward.
91
- xSpeed = - self .speedLimiter .calculate (self .controller .getLeftY ()) * Drivetrain .kMaxSpeed
93
+ xSpeed = (
94
+ - self .speedLimiter .calculate (self .controller .getLeftY ())
95
+ * Drivetrain .kMaxSpeed
96
+ )
92
97
93
98
# Get the rate of angular rotation. We are inverting this because we want a
94
99
# positive value when we pull to the left (remember, CCW is positive in
95
100
# mathematics). Xbox controllers return positive values when you pull to
96
101
# the right by default.
97
- rot = - self .rotLimiter .calculate (self .controller .getRightX ()) * Drivetrain .kMaxAngularSpeed
102
+ rot = (
103
+ - self .rotLimiter .calculate (self .controller .getRightX ())
104
+ * Drivetrain .kMaxAngularSpeed
105
+ )
98
106
99
107
self .drive .drive (xSpeed , rot )
100
108
0 commit comments