Skip to content

Commit

Permalink
more review items
Browse files Browse the repository at this point in the history
  • Loading branch information
jasondaming committed Dec 13, 2023
1 parent 8081306 commit 6a81123
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 15 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ The ``MecanumDriveKinematics`` class accepts four constructor arguments, with ea
backRightLocation = Translation2d(-0.381, -0.381)
# Creating my kinematics object using the wheel locations.
kinematics = MecanumDriveKinematics(
self.kinematics = MecanumDriveKinematics(
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation
)
Expand Down Expand Up @@ -94,7 +94,7 @@ The ``toWheelSpeeds(ChassisSpeeds speeds)`` (Java / Python) / ``ToWheelSpeeds(Ch
speeds = ChassisSpeeds(1.0, 3.0, 1.5)
# Convert to wheel speeds
frontLeft, frontRight, backLeft, backRight = kinematics.toWheelSpeeds(speeds)
frontLeft, frontRight, backLeft, backRight = self.kinematics.toWheelSpeeds(speeds)
Field-oriented drive
~~~~~~~~~~~~~~~~~~~~
Expand Down Expand Up @@ -143,7 +143,7 @@ Field-oriented drive
2.0, 2.0, math.pi / 2.0, Rotation2d.fromDegrees(45.0))
# Now use this in our kinematics
wheelSpeeds = kinematics.toWheelSpeeds(speeds)
wheelSpeeds = self.kinematics.toWheelSpeeds(speeds)
Using custom centers of rotation
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Expand Down Expand Up @@ -190,9 +190,9 @@ One can also use the kinematics object to convert a ``MecanumDriveWheelSpeeds``
wheelSpeeds = MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26)
# Convert to chassis speeds
chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds)
chassisSpeeds = self.kinematics.toChassisSpeeds(wheelSpeeds)
# Getting individual speeds
double forward = chassisSpeeds.vx
double sideways = chassisSpeeds.vy
double angular = chassisSpeeds.omega
forward = chassisSpeeds.vx
sideways = chassisSpeeds.vy
angular = chassisSpeeds.omega
Original file line number Diff line number Diff line change
Expand Up @@ -90,19 +90,19 @@ The fourth optional argument is the starting pose of your robot on the field (as
backRightLocation = Translation2d(-0.381, -0.381)
# Creating my kinematics object using the wheel locations.
kinematics = MecanumDriveKinematics(
self.kinematics = MecanumDriveKinematics(
frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation
)
# Creating my odometry object from the kinematics object and the initial wheel positions.
# Here, our starting pose is 5 meters along the long end of the field and in the
# center of the field along the short end, facing the opposing alliance wall.
odometry = MecanumDriveOdometry(
kinematics,
gyro.getRotation2d(),
self.odometry = MecanumDriveOdometry(
self.kinematics,
self.gyro.getRotation2d(),
MecanumDriveWheelPositions(
frontLeftEncoder.getDistance(), frontRightEncoder.getDistance(),
backLeftEncoder.getDistance(), backRightEncoder.getDistance()
self.frontLeftEncoder.getDistance(), self.frontRightEncoder.getDistance(),
self.backLeftEncoder.getDistance(), self.backRightEncoder.getDistance()
),
Pose2d(5.0, 13.5, Rotation2d())
)
Expand Down Expand Up @@ -153,8 +153,8 @@ The ``update`` method of the odometry class updates the robot position on the fi
def periodic(self):
# Get my wheel positions
wheelPositions = MecanumDriveWheelPositions(
frontLeftEncoder.getDistance(), frontRightEncoder.getDistance(),
backLeftEncoder.getDistance(), backRightEncoder.getDistance())
self.frontLeftEncoder.getDistance(), self.frontRightEncoder.getDistance(),
self.backLeftEncoder.getDistance(), self.backRightEncoder.getDistance())
# Get the rotation of the robot from the gyro.
gyroAngle = gyro.getRotation2d()
Expand Down

0 comments on commit 6a81123

Please sign in to comment.