Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
"""Provide tank steering using the stored robot configuration.
:param leftSpeed: The robot's left side speed along the X axis `[-1.0..1.0]`. Forward is positive.
:param rightSpeed: The robot's right side speed along the X axis`[-1.0..1.0]`. Forward is positive.
:param squareInputs: If set, decreases the input sensitivity at low speeds
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
2,
hal.UsageReporting.kRobotDrive2_DifferentialTank,
)
self.reported = True
leftSpeed = RobotDriveBase.limit(leftSpeed)
leftSpeed = RobotDriveBase.applyDeadband(leftSpeed, self.deadband)
rightSpeed = RobotDriveBase.limit(rightSpeed)
rightSpeed = RobotDriveBase.applyDeadband(rightSpeed, self.deadband)
# square the inputs (while preserving the sign) to increase fine
# control while permitting full power
if squareInputs:
leftSpeed = math.copysign(leftSpeed * leftSpeed, leftSpeed)
rightSpeed = math.copysign(rightSpeed * rightSpeed, rightSpeed)
self.leftMotor.set(leftSpeed * self.maxOutput)
self.rightMotor.set(
rightSpeed * self.maxOutput * self.rightSideInvertMultiplier
)
:param zRotation: The robot's zRotation rate around the Z axis `[-1.0..1.0]`. Clockwise is positive
:param squareInputs: If set, decreases the sensitivity at low speeds.
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
2,
hal.UsageReporting.kRobotDrive_ArcadeStandard,
)
self.reported = True
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
zRotation = RobotDriveBase.limit(zRotation)
zRotation = RobotDriveBase.applyDeadband(zRotation, self.deadband)
if squareInputs:
# Square the inputs (while preserving the sign) to increase fine
# control while permitting full power.
xSpeed = math.copysign(xSpeed * xSpeed, xSpeed)
zRotation = math.copysign(zRotation * zRotation, zRotation)
maxInput = math.copysign(max(abs(xSpeed), abs(zRotation)), xSpeed)
if xSpeed >= 0.0:
if zRotation >= 0.0:
leftMotorSpeed = maxInput
rightMotorSpeed = xSpeed - zRotation
else:
leftMotorSpeed = xSpeed + zRotation
for turn-in-place maneuvers
:param xSpeed: The robot's speed along the X axis `[-1.0..1.0]`. Forward is positive.
:param zRotation: The robot's rotation rate around the Z axis `[-1.0..1.0]`. Clockwise is positive.
:param isQuickTurn: If set, overrides constant-curvature turning for
turn-in-place maneuvers.
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
2,
hal.UsageReporting.kRobotDrive2_DifferentialCurvature,
)
self.reported = True
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
zRotation = RobotDriveBase.limit(zRotation)
zRotation = RobotDriveBase.applyDeadband(zRotation, self.deadband)
if isQuickTurn:
if abs(xSpeed) < self.quickStopThreshold:
self.quickStopAccumulator = (
(1 - self.quickStopAlpha) * self.quickStopAccumulator
+ self.quickStopAlpha * RobotDriveBase.limit(zRotation) * 2
)
overPower = True
angularPower = zRotation
else:
:param ySpeed: The robot's speed along the Y axis `[-1.0..1.0]`. Right is positive.
:param xSpeed: The robot's speed along the X axis `[-1.0..1.0]`. Forward is positive.
:param zRotation: The robot's rotation rate around the Z axis `[-1.0..1.0]`. Clockwise is positive.
:param gyroAngle: The current angle reading from the gyro in degrees around the Z axis. Use
this to implement field-oriented controls.
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
3,
hal.UsageReporting.kRobotDrive2_KilloughCartesian,
)
self.reported = True
ySpeed = RobotDriveBase.limit(ySpeed)
ySpeed = RobotDriveBase.applyDeadband(ySpeed, self.deadband)
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
# Compensate for gyro angle
input = Vector2d(ySpeed, xSpeed)
input.rotate(gyroAngle)
wheelSpeeds = [
input.scalarProject(self.leftVec) + zRotation,
input.scalarProject(self.rightVec) + zRotation,
input.scalarProject(self.backVec) + zRotation,
]
RobotDriveBase.normalize(wheelSpeeds)
:param ySpeed: The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
:param xSpeed: The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
:param zRotation: The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
:param gyroAngle: The current angle reading from the gyro in degrees around the Z axis. Use
this to implement field-oriented controls.
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
4,
hal.UsageReporting.kRobotDrive2_MecanumCartesian,
)
self.reported = True
ySpeed = RobotDriveBase.limit(ySpeed)
ySpeed = RobotDriveBase.applyDeadband(ySpeed, self.deadband)
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
# Compensate for gyro angle
input = Vector2d(ySpeed, xSpeed)
input.rotate(gyroAngle)
wheelSpeeds = [
# Front Left
input.x + input.y + zRotation,
# Rear Left
-input.x + input.y + zRotation,
# Front Right
-input.x + input.y - zRotation,
:param zRotation: The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
:param gyroAngle: The current angle reading from the gyro in degrees around the Z axis. Use
this to implement field-oriented controls.
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
4,
hal.UsageReporting.kRobotDrive2_MecanumCartesian,
)
self.reported = True
ySpeed = RobotDriveBase.limit(ySpeed)
ySpeed = RobotDriveBase.applyDeadband(ySpeed, self.deadband)
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
# Compensate for gyro angle
input = Vector2d(ySpeed, xSpeed)
input.rotate(gyroAngle)
wheelSpeeds = [
# Front Left
input.x + input.y + zRotation,
# Rear Left
-input.x + input.y + zRotation,
# Front Right
-input.x + input.y - zRotation,
# Rear Right
input.x + input.y - zRotation,
]
2,
hal.UsageReporting.kRobotDrive2_DifferentialCurvature,
)
self.reported = True
xSpeed = RobotDriveBase.limit(xSpeed)
xSpeed = RobotDriveBase.applyDeadband(xSpeed, self.deadband)
zRotation = RobotDriveBase.limit(zRotation)
zRotation = RobotDriveBase.applyDeadband(zRotation, self.deadband)
if isQuickTurn:
if abs(xSpeed) < self.quickStopThreshold:
self.quickStopAccumulator = (
(1 - self.quickStopAlpha) * self.quickStopAccumulator
+ self.quickStopAlpha * RobotDriveBase.limit(zRotation) * 2
)
overPower = True
angularPower = zRotation
else:
overPower = False
angularPower = abs(xSpeed) * zRotation - self.quickStopAccumulator
if self.quickStopAccumulator > 1:
self.quickStopAccumulator -= 1
elif self.quickStopAccumulator < -1:
self.quickStopAccumulator += 1
else:
self.quickStopAccumulator = 0
if zRotation >= 0.0:
leftMotorSpeed = maxInput
rightMotorSpeed = xSpeed - zRotation
else:
leftMotorSpeed = xSpeed + zRotation
rightMotorSpeed = maxInput
else:
if zRotation >= 0.0:
leftMotorSpeed = xSpeed + zRotation
rightMotorSpeed = maxInput
else:
leftMotorSpeed = maxInput
rightMotorSpeed = xSpeed - zRotation
leftMotorSpeed = RobotDriveBase.limit(leftMotorSpeed) * self.maxOutput
rightMotorSpeed = RobotDriveBase.limit(rightMotorSpeed) * self.maxOutput
self.leftMotor.set(leftMotorSpeed)
self.rightMotor.set(rightMotorSpeed * self.rightSideInvertMultiplier)
self.feed()
:param rightSpeed: The robot's right side speed along the X axis`[-1.0..1.0]`. Forward is positive.
:param squareInputs: If set, decreases the input sensitivity at low speeds
"""
if not self.reported:
hal.report(
hal.UsageReporting.kResourceType_RobotDrive,
2,
hal.UsageReporting.kRobotDrive2_DifferentialTank,
)
self.reported = True
leftSpeed = RobotDriveBase.limit(leftSpeed)
leftSpeed = RobotDriveBase.applyDeadband(leftSpeed, self.deadband)
rightSpeed = RobotDriveBase.limit(rightSpeed)
rightSpeed = RobotDriveBase.applyDeadband(rightSpeed, self.deadband)
# square the inputs (while preserving the sign) to increase fine
# control while permitting full power
if squareInputs:
leftSpeed = math.copysign(leftSpeed * leftSpeed, leftSpeed)
rightSpeed = math.copysign(rightSpeed * rightSpeed, rightSpeed)
self.leftMotor.set(leftSpeed * self.maxOutput)
self.rightMotor.set(
rightSpeed * self.maxOutput * self.rightSideInvertMultiplier
)
self.feed()