def __init__(
self,
x_wheelbase: units.Quantity = 2 * units.feet,
y_wheelbase: units.Quantity = 3 * units.feet,
speed: units.Quantity = 5 * units.fps,
deadzone: typing.Optional[DeadzoneCallable] = None,
):
"""
:param x_wheelbase: The distance between right and left wheels.
:param y_wheelbase: The distance between forward and rear wheels.
:param speed: Speed of robot (see above)
:param deadzone: A function that adjusts the output of the motor (see :func:`linear_deadzone`)
"""
x2 = units.meters.m_from(x_wheelbase, name="x_wheelbase") / 2.0
y2 = units.meters.m_from(y_wheelbase, name="y_wheelbase") / 2.0
self.kinematics = MecanumDriveKinematics(
Translation2d(x2, y2),
Translation2d(x2, -y2),
Translation2d(-x2, y2),
Translation2d(-x2, -y2),
)
self.speed = units.mps.m_from(speed, name="speed")
self.deadzone = deadzone
self.wheelSpeeds = MecanumDriveWheelSpeeds()
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTables.getTable("SmartDashboard")
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
# Starting positions are relative to the field's top left coordinate facing down the field
class StartingPosition(Enum):
# LEFT is one foot off the LEFT wall relative to the Power port (and facing the power port).
LEFT = Pose2d(3.2, -0.6468, Rotation2d.fromDegrees(180))
CENTER = Pose2d(3.2, 0, Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
RIGHT = Pose2d(3.2, -3.25,
Rotation2d.fromDegrees(180)).relativeTo(POWER_PORT)
# ALL trajectory points should be relative to the field's top left coordinate down the field
# Trajectories that aren't field relative are generated with respect to ALL Starting Positions
# E.x. "charge-LEFT", "charge-CENTER", "charge-RIGHT"
# We do this to allow for trajectory chaining, which requires field-relativity
TRAJECTORIES = {
"charge":
TrajectoryData(Pose2d(), [Translation2d(1, 0)],
Pose2d(2, 0, Rotation2d()),
field_relative=False),
"curve":
TrajectoryData(Pose2d(), [Translation2d(1, 0.3)],
Pose2d(2, 0, Rotation2d()),
field_relative=False),
"trench":
TrajectoryData(Pose2d(3.2, 0, Rotation2d.fromDegrees(180)), [],
Pose2d(6.3, 0, Rotation2d.fromDegrees(180)),
reverse=True),
"trench-far":
TrajectoryData(Pose2d(3.2, 0, Rotation2d.fromDegrees(180)), [],
Pose2d(7.1, 0, Rotation2d.fromDegrees(180)),
reverse=True),
"trench-far-return":
from wpilib.controller import SimpleMotorFeedforwardMeters
from wpilib.geometry import Pose2d, Rotation2d, Translation2d
from wpilib.kinematics import (
ChassisSpeeds,
DifferentialDriveKinematics,
DifferentialDriveOdometry,
)
GEAR_RATIO = 10.75
# measurements in metres
TRACK_WIDTH = 0.579 # measured by characterisation
WHEEL_CIRCUMFERENCE = 0.0254 * 6 * math.pi
POWER_PORT_POSITION = Translation2d(0, 0)
# in field co ordinates
class Chassis:
left_rear: rev.CANSparkMax
left_front: rev.CANSparkMax
right_rear: rev.CANSparkMax
right_front: rev.CANSparkMax
imu: NavX
open_loop = magicbot.tunable(False)
vx = magicbot.will_reset_to(0.0)
vz = magicbot.will_reset_to(0.0)
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
self.sd = NetworkTables.getTable("SmartDashboard")
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
#Create the DriveTrain
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
#Create The Encoders
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_l_encoder.setSimDevice(0)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_l_encoder.setSimDevice(1)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_r_encoder.setSimDevice(2)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_r_encoder.setSimDevice(3)
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
# Now that we have created the ability to see wheel speeds, chassis speeds and our position
# Let us start to use feedforward to try to lock our robot into a specific speed.
self.feedForward = SimpleMotorFeedforwardMeters(kS=0.194,
kV=.5,
kA=0.457)
def s4():
fl = Translation2d(12, 12)
fr = Translation2d(12, -12)
bl = Translation2d(-12, 12)
br = Translation2d(-12, -12)
return SwerveDrive4Kinematics(fl, fr, bl, br)
def robotInit(self):
# Pull in smart dashboard info...
self.sd = NetworkTables.getTable("SmartDashboard")
# Start a timer....
self.timer = wpilib.Timer()
"""Robot initialization function. The Low level is to use the brushless function on the controllers."""
if wpilib.RobotBase.isSimulation():
self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel)
self.frontRightMotor = wpilib.Spark(self.frontRightChannel)
self.rearRightMotor = wpilib.Spark(self.rearRightChannel)
else:
self.frontLeftMotor = rev.CANSparkMax(
self.frontLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearLeftMotor = rev.CANSparkMax(
self.rearLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.frontRightMotor = rev.CANSparkMax(
self.frontRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearRightMotor = rev.CANSparkMax(
self.rearRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
m_frontLeftLocation = Translation2d(0.381, 0.381)
m_frontRightLocation = Translation2d(0.381, -0.381)
m_backLeftLocation = Translation2d(-0.381, 0.381)
m_backRightLocation = Translation2d(-0.381, -0.381)
# Creating my kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
# Example chassis speeds: 1 meter per second forward, 3 meters
# per second to the left, and rotation at 1.5 radians per second
# counterclockwise.
preChassis = ChassisSpeeds()
preChassis.vx = 1.0
preChassis.vy = 0.0
preChassis.omega = 0.0
# Convert to wheel speeds
wheelSpeeds = MecanumDriveKinematics.toWheelSpeeds(preChassis)
# Get the individual wheel speeds
frontLeft = wheelSpeeds.frontLeftMetersPerSecond
frontRight = wheelSpeeds.frontRightMetersPerSecond
backLeft = wheelSpeeds.rearLeftMetersPerSecond
backRight = wheelSpeeds.rearRightMetersPerSecond