From 7bb948ad9cc7afe9ae8bf24cf0a65cfb49e92f69 Mon Sep 17 00:00:00 2001 From: Eric Fletcher Date: Fri, 8 Dec 2023 19:18:20 -0500 Subject: [PATCH] add differential-drive-bot example (#87) --- differential-drive-bot/drivetrain.py | 99 ++++++++++++++++++++++++++++ differential-drive-bot/robot.py | 44 +++++++++++++ run_tests.sh | 1 + 3 files changed, 144 insertions(+) create mode 100755 differential-drive-bot/drivetrain.py create mode 100755 differential-drive-bot/robot.py diff --git a/differential-drive-bot/drivetrain.py b/differential-drive-bot/drivetrain.py new file mode 100755 index 0000000..797a0f0 --- /dev/null +++ b/differential-drive-bot/drivetrain.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 + +import wpilib.drive +import wpimath.controller +import wpimath.kinematics +import wpimath.units + +import math + + +class Drivetrain: + """Represents a differential drive style drivetrain.""" + + MAX_SPEED = 3.0 # meters per second + MAX_ANGULAR_SPEED = 2 * math.pi # one rotation per second + + TRACK_WIDTH = 0.381 * 2 # meters + WHEEL_RADIUS = 0.0508 # meters + ENCODER_RESOLUTION = 4096 # counts per revolution + + def __init__(self): + leftLeader = wpilib.PWMSparkMax(1) + leftFollower = wpilib.PWMSparkMax(2) + rightLeader = wpilib.PWMSparkMax(3) + rightFollower = wpilib.PWMSparkMax(4) + + self.leftEncoder = wpilib.Encoder(0, 1) + self.rightEncoder = wpilib.Encoder(2, 3) + + self.leftGroup = wpilib.MotorControllerGroup(leftLeader, leftFollower) + self.rightGroup = wpilib.MotorControllerGroup(rightLeader, rightFollower) + + self.gyro = wpilib.AnalogGyro(0) + + self.leftPIDController = wpimath.controller.PIDController(1.0, 0.0, 0.0) + self.rightPIDController = wpimath.controller.PIDController(1.0, 0.0, 0.0) + + self.kinematics = wpimath.kinematics.DifferentialDriveKinematics( + self.TRACK_WIDTH + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) + + self.gyro.reset() + + # 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. + self.rightGroup.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. + self.leftEncoder.setDistancePerPulse( + 2 * math.pi * self.WHEEL_RADIUS / self.ENCODER_RESOLUTION + ) + self.rightEncoder.setDistancePerPulse( + 2 * math.pi * self.WHEEL_RADIUS / self.ENCODER_RESOLUTION + ) + + self.leftEncoder.reset() + self.rightEncoder.reset() + + self.odometry = wpimath.kinematics.DifferentialDriveOdometry( + self.gyro.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) + + def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds): + """Sets the desired wheel speeds.""" + leftFeedforward = self.feedforward.calculate(speeds.left) + rightFeedforward = self.feedforward.calculate(speeds.right) + + leftOutput = self.leftPIDController.calculate( + self.leftEncoder.getRate(), speeds.left + ) + rightOutput = self.rightPIDController.calculate( + self.rightEncoder.getRate(), speeds.right + ) + + self.leftGroup.setVoltage(leftOutput + leftFeedforward) + self.rightGroup.setVoltage(rightOutput + rightFeedforward) + + def drive(self, xSpeed, rot): + """Drives the robot with the given linear velocity and angular velocity.""" + wheelSpeeds = self.kinematics.toWheelSpeeds( + wpimath.kinematics.ChassisSpeeds(xSpeed, 0, rot) + ) + self.setSpeeds(wheelSpeeds) + + def updateOdometry(self): + """Updates the field-relative position.""" + self.odometry.update( + self.gyro.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) diff --git a/differential-drive-bot/robot.py b/differential-drive-bot/robot.py new file mode 100755 index 0000000..80482be --- /dev/null +++ b/differential-drive-bot/robot.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 + +import wpilib +import wpilib.drive +import wpimath.filter + +from drivetrain import Drivetrain + + +class MyRobot(wpilib.TimedRobot): + def robotInit(self): + self.controller = wpilib.XboxController(0) + self.drive = Drivetrain() + + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + self.speedLimiter = wpimath.filter.SlewRateLimiter(3) + self.rotLimiter = wpimath.filter.SlewRateLimiter(3) + + def autonomousPeriodic(self): + self.teleopPeriodic() + self.drive.updateOdometry() + + def teleopPeriodic(self): + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.speedLimiter.calculate(self.controller.getLeftY()) + * Drivetrain.MAX_SPEED + ) + + # 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()) + * Drivetrain.MAX_ANGULAR_SPEED + ) + + self.drive.drive(xSpeed, rot) + + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/run_tests.sh b/run_tests.sh index b71a0f2..908e666 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -13,6 +13,7 @@ BASE_TESTS=" hatchbot-inlined cscore-intermediate-vision cscore-quick-vision + differential-drive-bot elevator-profiled-pid elevator-simulation elevator-trapezoid-profile