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)
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)
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 3
rearRightChannel = 4
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
WHEEL_DIAMETER = 0.5 # 6 inches
ENCODER_COUNTS_PER_REV = 3
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 robotPeriodic(self):
# Odometry Update
# First, get the wheel speeds...
# Next, we need to grab the Gyro angle and send it into the odometry. It must be inverted because gyros v Wpilib are backwards
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.timer = wpilib.Timer()
self.timer.start()
self.f_l_encoder.reset()
self.r_l_encoder.reset()
self.f_r_encoder.reset()
self.r_r_encoder.reset()
self.lastCount = 0
def autonomousPeriodic(self):
preChassis = ChassisSpeeds()
preChassis.vx = 5.0
preChassis.vy = 0.0
preChassis.omega = 0.0
# Convert to wheel speeds
speeds = self.m_kinematics.toWheelSpeeds(preChassis)
self.wheelSpeeds = MecanumDriveWheelSpeeds(
self.f_l_encoder.getRate(),
self.r_l_encoder.getRate(),
self.f_r_encoder.getRate(),
self.r_r_encoder.getRate(),
)
gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle())
# Finally, we can update the pose...
self.m_pose = self.MecanumDriveOdometry.update(gyroAngle,
self.wheelSpeeds)
#For Kinematics, we need to update the wheelspeeds
CurrentChassis = self.m_kinematics.toChassisSpeeds(self.wheelSpeeds)
print(CurrentChassis)
print(self.f_l_encoder.getDistancePerPulse())
print('difference')
print(self.f_l_encoder.get() - self.lastCount)
print('rate')
print(self.r_r_encoder.getRate())
print('lastCount')
self.lastCount = self.f_l_encoder.get()
print(self.lastCount)
'''
left_front = self.feedForward.calculate(speeds.frontLeft)s
right_front = self.feedForward.calculate(speeds.frontRight)
left_rear = self.feedForward.calculate(speeds.rearLeft)
right_rear = self.feedForward.calculate(speeds.rearRight)'''
#print(left_front, right_front, left_rear,right_rear)
if self.timer.get() < 2.0:
# self.drive.driveCartesian(1, 1, 1, 1) #<---- This is using the drive method built into the mecanum dirve.
# Maybe we want to use something with more control, like feedforward...
'''self.frontLeftMotor.set(-left_front)
self.rearLeftMotor.set(right_front)
self.frontRightMotor.set(-left_rear)
self.rearRightMotor.set(right_rear)'''
self.drive.driveCartesian(1, 0, 0, 0)
elif self.timer.get() > 2 and self.timer.get() < 4:
self.drive.driveCartesian(1, 0, 0, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
# self.drive.driveCartesian(
# self.lstick.getX(), -self.lstick.getY(), self.rstick.getX(), 0
# )
self.drive.driveCartesian(self.lstick.getX(), -self.lstick.getY(),
self.lstick.getRawAxis(2), 0)
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
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
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
# Field Oriented Drive
# The desired field relative speed here is 2 meters per second
# toward the opponent's alliance station wall, and 2 meters per
# second toward the left field boundary. The desired rotation
# is a quarter of a rotation per second counterclockwise. The current
# robot angle is 45 degrees.
#FieldOrientedspeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
#2.0, 2.0, (math.pi # 2.0), Rotation2d.fromDegrees(45.0))
# Now use this in our kinematics
#wheelSpeeds = MecanumDriveKinematics.toWheelSpeeds(FieldOrientedspeeds)
# Example wheel speeds
# wheelSpeeds = MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26)
# Convert to chassis speeds
# chassisSpeeds = MecanumDriveKinematics.toChassisSpeeds(wheelSpeeds)
# Getting individual speeds
def autonomousInit(self):
pass
def autonomousPeriodic(self):
# Example chassis speeds: 1 meter per second forward, 3 meters
## per second to the left, and rotation at 1.5 radians per second
## counterclockwise.
# speeds = ChassisSpeeds(self.forward,self.sideways,self.angular)
#wheelSpeeds = self.m_kinematics.toWheelSpeeds(speeds)
self.drive.driveCartesian(.5, 0, 0, 0)
chassisspeeds = self.m_kinematics.toChassisSpeeds(.5, .5, 0, 0)
print(chassisspeeds)
class MecanumDrivetrain:
"""
Four motors, each with a mecanum wheel attached to it.
.. note:: :class:`wpilib.drive.MecanumDrive` assumes that to make
the robot go forward, the left motor outputs are 1, and the
right motor outputs are -1
.. versionadded:: 2018.2.0
"""
#: Use this to compute encoder data after calculate is called
wheelSpeeds: MecanumDriveWheelSpeeds
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 calculate(
self,
lf_motor: float,
lr_motor: float,
rf_motor: float,
rr_motor: float,
) -> ChassisSpeeds:
"""
:param lf_motor: Left front motor value (-1 to 1); 1 is forward
:param lr_motor: Left rear motor value (-1 to 1); 1 is forward
:param rf_motor: Right front motor value (-1 to 1); -1 is forward
:param rr_motor: Right rear motor value (-1 to 1); -1 is forward
:returns: ChassisSpeeds that can be passed to 'drive'
.. versionadded:: 2020.1.0
"""
if self.deadzone:
lf_motor = self.deadzone(lf_motor)
lr_motor = self.deadzone(lr_motor)
rf_motor = self.deadzone(rf_motor)
rr_motor = self.deadzone(rr_motor)
# Calculate speed of each wheel
lr = lr_motor * self.speed
rr = -rr_motor * self.speed
lf = lf_motor * self.speed
rf = -rf_motor * self.speed
self.wheelSpeeds.frontLeft = lf
self.wheelSpeeds.rearLeft = lr
self.wheelSpeeds.frontRight = rf
self.wheelSpeeds.rearRight = rr
return self.kinematics.toChassisSpeeds(self.wheelSpeeds)