class AlignmentController:
drivetrain = Drivetrain
targeting = Targeting
rate = .5
kP = 0.05
kI = 0
kD = 0.005
kF = 0
def __init__(self):
self.angle = None
self.angle_pid_controller = PIDController(Kp=self.kP,
Ki=self.kI,
Kd=self.kD,
Kf=self.kF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.rate, self.rate)
self.nt = NetworkTables.getTable('limelight')
def get_position(self):
return self.drivetrain.get_position()
def get_angle(self):
angle = self.targeting.get_data().x
if angle != 0.0:
self.angle = angle
return self.angle
def found(self):
fnd = self.targeting.get_data().found
if fnd == 1:
return True
else:
return False
def move_to(self, position):
self.setpoint = position
self.angle_pid_controller.enable()
def pidWriteAngle(self, rate):
self.rate = rate
def execute(self):
if self.rate is not None:
if self.found():
self.drivetrain.differential_drive(0)
self.stop()
else:
self.drivetrain.manual_drive(-self.rate, self.rate)
def stop(self):
self.angle_pid_controller.disable()
def on_disable(self):
self.stop()
class DriveController:
wheel_radius = 0.1 # meters
encoder_ticks_per_revolution = 256 * 3 * 1.8
def __init__(self, kP, kI, kD, kF, tolerance, encoder_function,
update_function, direction, period, is_enabled):
self.is_enabled = is_enabled
self.direction = direction
self.encoder_function = encoder_function
self.motor_output = 0
self.pid_controller = PIDController(kP,
kI,
kD,
kF,
source=self,
output=self,
period=period)
#self.pid_controller.controlLoop._thread.setDaemon(True)
self.pid_controller.setInputRange(-5, 5)
self.pid_controller.setOutputRange(-1.0, 1.0)
self.pid_controller.setAbsoluteTolerance(tolerance)
self.update_function = update_function
self.pid_controller.setContinuous(True)
def start(self, setpoint):
self.setpoint = setpoint
self.pid_controller.setSetpoint(setpoint)
self.pid_controller.enable()
def pidWrite(self, output):
print("output voltage", output)
# print("distance for cotroller", self.pid_controller.getError())
self.pid_controller.setSetpoint(self.setpoint)
self.update_function(self.direction * output)
def is_finished(self):
return self.pid_controller.onTarget()
def close(self):
self.pid_controller.close()
def pidGet(self):
distance_in_encoder = self.encoder_function()
distance_in_meters = (
distance_in_encoder /
self.encoder_ticks_per_revolution) * self.wheel_radius
print("dist", distance_in_meters)
return distance_in_meters
def getPIDSourceType(self):
return "meter"
def getPIDOutputType(self):
return "output"
class TurnToAngle(Command):
def _setMotors(self, signal):
signal = signal if abs(
signal) > Constants.TURN_TO_ANGLE_MIN_OUTPUT else math.copysign(
Constants.TURN_TO_ANGLE_MIN_OUTPUT, signal)
Dash.putNumber("Turn To Angle Output", signal)
self.drive.setPercentOutput(signal, -signal, signal, -signal)
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
logging.debug(
"Turn to angle constructed with angle {}".format(setpoint))
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def initialize(self):
self.PID.setP(Constants.TURN_TO_ANGLE_KP)
self.PID.setI(Constants.TURN_TO_ANGLE_KI)
self.PID.setD(Constants.TURN_TO_ANGLE_KD)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setSetpoint(self.setpoint)
self.PID.enable()
def execute(self):
Dash.putNumber("Turn To Angle Error", self.PID.getError())
# logging.info(f"Turn To Angle Error {self.PID.getError()}")
def isFinished(self):
return self.PID.onTarget()
def end(self):
logging.debug("Finished turning to angle {}".format(self.setpoint))
self.PID.disable()
snaplistener.SnapListener(0).start()
def interrupted(self):
self.end()
class Chassis:
correct_range = 1.65 # m
length = 498.0 # mm
width = 600.0 # mm
motor_dist = math.sqrt((width / 2) ** 2 + (length / 2) ** 2) # distance of motors from the center of the robot
# x component y component
vz_components = {'x': (width / 2) / motor_dist, 'y': (length / 2) / motor_dist} # multiply both by vz and the
# the number that you need to multiply the vz components by to get them in the appropriate directions
# vx vy
"""module_params = {'a': {'args': {'drive':13, 'steer':14, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}"""
module_params = {'a': {'args': {'drive':13, 'steer':11, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}
# Use the magic here!
bno055 = BNO055
vision = Vision
range_finder = RangeFinder
heading_hold_pid_output = BlankPIDOutput
heading_hold_pid = PIDController
def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(1.0, 0.02, 0.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(0.05)
self.distance_pid.setToleranceBuffer(5)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-10.0, 10.0) # TODO check that this range is good for us
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold = True
self.field_oriented = True
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.heading_hold_pid.reset()
# Update the current module steer setpoint to be the current position
# Stops the unwind problem
for module in self._modules.values():
module._steer.set(module._steer.getPosition())
def onTarget(self):
for module in self._modules.values():
if not abs(module._steer.getError()) < 50:
return False
return True
def toggle_field_oriented(self):
self.field_oriented = not self.field_oriented
def toggle_vision_tracking(self):
self.track_vision = not self.track_vision
if self.track_vision:
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
def toggle_range_holding(self, setpoint=1.65):
if not self.range_setpoint:
self.range_setpoint = setpoint
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
else:
self.range_setpoint = 0.0
def zero_encoders(self):
for module in self._modules.values():
module.zero_distance()
def field_displace(self, x, y):
'''Use the distance PID to displace the robot by x,y
in field reference frame.'''
d = math.sqrt((x ** 2 + y ** 2))
fx, fy = field_orient(x, y, self.bno055.getHeading())
self.distance_pid_heading = math.atan2(fy, fx)
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid.setSetpoint(d)
self.distance_pid.reset()
self.distance_pid.enable()
def pidGet(self):
return self.distance
def getPIDSourceType(self):
return PIDSource.PIDSourceType.kDisplacement
@property
def distance(self):
distances = 0.0
for module in self._modules.values():
distances += abs(module.distance) / module.drive_counts_per_metre
return distances / 4.0
def drive(self, vX, vY, vZ, absolute=False):
motor_vectors = {}
for name, params in Chassis.module_params.items():
motor_vectors[name] = {'x': vX + vZ * params['vz']['x'],
'y': vY + vZ * params['vz']['y']
}
# convert the vectors to polar coordinates
polar_vectors = {}
max_mag = 1.0
for name, motor_vector in motor_vectors.items():
polar_vectors[name] = {'dir': math.atan2(motor_vector['y'],
motor_vector['x']
),
'mag': math.sqrt(motor_vector['x'] ** 2
+ motor_vector['y'] ** 2
)
}
if abs(polar_vectors[name]['mag']) > max_mag:
max_mag = polar_vectors[name]['mag']
for name in polar_vectors.keys():
polar_vectors[name]['mag'] /= max_mag
if absolute:
polar_vectors[name]['mag'] = None
continue
for name, polar_vector in polar_vectors.items():
self._modules[name].steer(polar_vector['dir'], polar_vector['mag'])
def execute(self):
if self.field_oriented and self.inputs[3] is not None:
self.inputs[0:2] = field_orient(self.inputs[0], self.inputs[1], self.bno055.getHeading())
# Are we in setpoint displacement mode?
if self.distance_pid.isEnable():
if self.distance_pid.onTarget():
if self.pid_counter > 10:
self.reset_distance_pid = False
# Let's see if we need to move further
x = y = 0.0
if self.range_setpoint:
x = self.range_finder.pidGet() - self.range_setpoint
if x > 0.5:
x = 0.5
elif x < -0.5:
x = -0.5
if self.track_vision:
y = self.vision.pidGet()*0.3#*self.range_finder.pidGet()*0.3 # TODO we need proper scaling factors here
if y > 0.3:
y = 0.3
elif y < -0.3:
y = -0.3
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid_heading = constrain_angle(math.atan2(y, x)+self.bno055.getAngle())
self.distance_pid.setSetpoint(math.sqrt(x**2+y**2))
self.distance_pid.reset()
self.distance_pid.enable()
self.pid_counter = 0
else:
self.pid_counter += 1
# Keep driving
self.vx = math.cos(self.distance_pid_heading) * self.distance_pid_output.output
self.vy = math.sin(self.distance_pid_heading) * self.distance_pid_output.output
else:
self.vx = self.inputs[0] * self.inputs[3] # multiply by throttle
self.vy = self.inputs[1] * self.inputs[3] # multiply by throttle
if self.heading_hold:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.inputs[2] != 0.0:
self.momentum = True
if not self.momentum:
self.heading_hold_pid.enable()
self.vz = self.heading_hold_pid_output.output
else:
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.vz = self.inputs[2] * self.inputs[3] # multiply by throttle
if self.lock_wheels:
for _, params, module in zip(Chassis.module_params.items(),
self._modules):
direction = constrain_angle(math.atan2(params['vz']['y'],
params['vz']['x']) +
math.pi / 2.0)
module.steer(direction, 0.0)
else:
self.drive(self.vx, self.vy, self.vz)
def toggle_heading_hold(self):
self.heading_hold = not self.heading_hold
def set_heading_setpoint(self, setpoint):
self.heading_hold_pid.setSetpoint(constrain_angle(setpoint))
def on_range_target(self):
return abs(self.range_finder.getDistance() - self.range_setpoint) < 0.1
def on_vision_target(self):
return (self.vision.no_vision_counter == 0.0 and
abs(self.vision.pidGet()) < 0.035)
class Chassis:
correct_range = 1.65 # m
length = 498.0 # mm
width = 600.0 # mm
vision_scale_factor = 0.3 # units of m/(vision unit)
distance_pid_abs_error = 0.05 # metres
motor_dist = math.sqrt((width / 2) ** 2 + (length / 2) ** 2) # distance of motors from the center of the robot
# x component y component
vz_components = {'x': (width / 2) / motor_dist, 'y': (length / 2) / motor_dist} # multiply both by vz and the
# the number that you need to multiply the vz components by to get them in the appropriate directions
# vx vy
"""module_params = {'a': {'args': {'drive':13, 'steer':14, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}"""
module_params = {'a': {'args': {'drive':13, 'steer':11, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}
# Use the magic here!
bno055 = BNO055
vision = Vision
range_finder = RangeFinder
heading_hold_pid_output = BlankPIDOutput
heading_hold_pid = PIDController
def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(0.75, 0.02, 1.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(self.distance_pid_abs_error)
self.distance_pid.setToleranceBuffer(3)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-5.0, 5.0)
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
self.logger = logging.getLogger("chassis")
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold = True
self.field_oriented = True
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.heading_hold_pid.reset()
# Update the current module steer setpoint to be the current position
# Stops the unwind problem
for module in self._modules.values():
module._steer.set(module._steer.getPosition())
def onTarget(self):
for module in self._modules.values():
if not abs(module._steer.getError()) < 50:
return False
return True
def toggle_field_oriented(self):
self.field_oriented = not self.field_oriented
def toggle_vision_tracking(self):
self.track_vision = not self.track_vision
self.logger.info("Vision Tracking: " + str(self.track_vision))
if self.track_vision:
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
def toggle_range_holding(self, setpoint=1.65):
if not self.range_setpoint:
self.range_setpoint = setpoint
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
else:
self.range_setpoint = 0.0
def zero_encoders(self):
for module in self._modules.values():
module.zero_distance()
def field_displace(self, x, y):
'''Use the distance PID to displace the robot by x,y
in field reference frame.'''
d = math.sqrt((x ** 2 + y ** 2))
fx, fy = field_orient(x, y, self.bno055.getHeading())
self.distance_pid_heading = math.atan2(fy, fx)
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid.setSetpoint(d)
self.distance_pid.reset()
self.distance_pid.enable()
def pidGet(self):
return self.distance
def getPIDSourceType(self):
return PIDSource.PIDSourceType.kDisplacement
@property
def distance(self):
distances = 0.0
for module in self._modules.values():
distances += abs(module.distance) / module.drive_counts_per_metre
return distances / 4.0
def drive(self, vX, vY, vZ, absolute=False):
motor_vectors = {}
for name, params in Chassis.module_params.items():
motor_vectors[name] = {'x': vX + vZ * params['vz']['x'],
'y': vY + vZ * params['vz']['y']
}
# convert the vectors to polar coordinates
polar_vectors = {}
max_mag = 1.0
for name, motor_vector in motor_vectors.items():
polar_vectors[name] = {'dir': math.atan2(motor_vector['y'],
motor_vector['x']
),
'mag': math.sqrt(motor_vector['x'] ** 2
+ motor_vector['y'] ** 2
)
}
if abs(polar_vectors[name]['mag']) > max_mag:
max_mag = polar_vectors[name]['mag']
for name in polar_vectors.keys():
polar_vectors[name]['mag'] /= max_mag
if absolute:
polar_vectors[name]['mag'] = None
continue
for name, polar_vector in polar_vectors.items():
self._modules[name].steer(polar_vector['dir'], polar_vector['mag'])
def execute(self):
if self.field_oriented and self.inputs[3] is not None:
self.inputs[0:2] = field_orient(self.inputs[0], self.inputs[1], self.bno055.getHeading())
# Are we in setpoint displacement mode?
if self.distance_pid.isEnable():
if self.distance_pid.onTarget():
if self.pid_counter > 10:
self.reset_distance_pid = False
# Let's see if we need to move further
x = y = 0.0
if self.range_setpoint and not self.on_range_target():
x = self.range_finder.pidGet() - self.range_setpoint
if x > 0.5:
x = 0.5
elif x < -0.5:
x = -0.5
self.logger.info("X: " + str(x))
if self.track_vision and not self.on_vision_target():
self.logger.info("Tracking Vision")
y = self.vision.pidGet() * self.vision_scale_factor
if y > 0.5:
y = 0.5
elif y < -0.5:
y = -0.5
self.logger.info("Y: " + str(y))
elif self.on_vision_target():
self.track_vision = False
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid_heading = constrain_angle(math.atan2(y, x)+self.bno055.getAngle())
self.distance_pid.setSetpoint(math.sqrt(x**2+y**2))
self.distance_pid.reset()
self.distance_pid.enable()
self.pid_counter = 0
else:
self.pid_counter += 1
# Keep driving
self.vx = math.cos(self.distance_pid_heading) * self.distance_pid_output.output
self.vy = math.sin(self.distance_pid_heading) * self.distance_pid_output.output
else:
self.vx = self.inputs[0] * self.inputs[3] # multiply by throttle
self.vy = self.inputs[1] * self.inputs[3] # multiply by throttle
if self.heading_hold:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.inputs[2] != 0.0:
self.momentum = True
if not self.momentum:
self.heading_hold_pid.enable()
self.vz = self.heading_hold_pid_output.output
else:
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.vz = self.inputs[2] * self.inputs[3] # multiply by throttle
self.logger.info("Vision: %s Rangefinder: %s Distance: %s Setpoint: %s" % (self.vision.pidGet(), self.range_finder.pidGet(), self.distance, self.distance_pid.getSetpoint()))
if self.lock_wheels:
for _, params, module in zip(Chassis.module_params.items(),
self._modules):
direction = constrain_angle(math.atan2(params['vz']['y'],
params['vz']['x']) +
math.pi / 2.0)
module.steer(direction, 0.0)
else:
self.drive(self.vx, self.vy, self.vz)
def toggle_heading_hold(self):
self.heading_hold = not self.heading_hold
def set_heading_setpoint(self, setpoint):
self.heading_hold_pid.setSetpoint(constrain_angle(setpoint))
def on_range_target(self):
return abs(self.range_finder.pidGet() - self.range_setpoint) < self.distance_pid_abs_error * 2.0
def on_vision_target(self):
return (self.vision.no_vision_counter == 0.0 and
abs(self.vision.pidGet() * self.vision_scale_factor) < self.distance_pid_abs_error * 2.0)
class SwerveChassis:
WIDTH = 1
LENGTH = 0.88
imu: IMU
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
# tunables here purely for debugging
odometry_x = tunable(0)
odometry_y = tunable(0)
# odometry_theta = tunable(0)
# odometry_x_vel = tunable(0)
# odometry_y_vel = tunable(0)
# odometry_z_vel = tunable(0)
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.last_vx, self.last_vy = self.vx, self.vy
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=3.0,
Ki=0.0,
Kd=5.0,
source=self.imu.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
alphas = []
ls = []
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
alphas.append(module_dist)
module_angle = math.atan2(module.y_pos, module.x_pos)
ls.append(module_angle)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.read_steer_pos()
self.icre = ICREstimator(np.zeros(shape=(3, 1)), np.array(alphas),
np.array(ls), np.zeros(shape=(4, )))
# TODO: re-enable if we end up not using callback method
self.imu.imu.ahrs.registerCallback(self.update_odometry)
def set_heading_sp_current(self):
self.set_heading_sp(self.imu.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.heading_hold_on()
self.last_heading = self.imu.getAngle()
self.odometry_updated = False
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.imu.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if not self.momentum:
pid_z = self.heading_pid_out.output
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
if abs(pid_z) < 0.1 and math.hypot(self.vx, self.vy) < 0.01:
pid_z = 0
vz = input_vz + pid_z
angle = self.imu.getAngle()
# TODO: re-enable if we end up not using callback method
# self.update_odometry()
# self.odometry_updated = False # reset for next timestep
for module in self.modules:
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module.dist * vz * math.sin(module.angle)
vz_y = module.dist * vz * math.cos(module.angle)
if self.field_oriented:
vx, vy = self.robot_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
def update_odometry(self, o, sensor_timestamp):
# TODO: re-enable if we end up not using callback method
# if self.odometry_updated:
# return
heading = self.imu.getAngle()
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
betas = []
phi_dots = []
for i, module in enumerate(self.modules):
module.update_odometry()
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
betas.append(module.measured_azimuth)
phi_dots.append(module.wheel_angular_vel)
q = np.array(betas)
lambda_e = self.icre.estimate_lmda(q)
print(lambda_e)
vx, vy, vz = self.robot_movement_from_odometry(velocity_outputs,
heading)
delta_x, delta_y, delta_z = self.robot_movement_from_odometry(
odometry_outputs, heading, z_vel=vz)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_x_vel = vx
self.odometry_y_vel = vy
self.odometry_z_vel = vz
self.last_heading = heading
self.odometry_updated = True
def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0):
lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None)
x, y, theta = lstsq_ret[0].reshape(3)
# TODO: re-enable if we move back to running in the same thread
x_field, y_field = self.field_orient(x, y, angle + z_vel * (1 / 200))
# x_field, y_field = self.field_orient(x, y, angle)
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self,
vx: float,
vy: float,
vz: float,
*,
field_oriented: bool = True):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
field_oriented: Whether the inputs are field or robot oriented.
"""
self.last_vx, self.last_vy = self.vx, self.vy
self.vx = vx
self.vy = vy
self.vz = vz
self.field_oriented = field_oriented
@staticmethod
def robot_orient(vx, vy, heading):
"""Turn a vx and vy relative to the field into a vx and vy based on the
robot.
Args:
vx: vx to robot orient
vy: vy to robot orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: robot oriented vx speed
float: robot oriented vy speed
"""
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@staticmethod
def field_orient(vx, vy, heading):
"""Turn a vx and vy relative to the robot into a vx and vy based on the
field.
Args:
vx: vx to field orient
vy: vy to field orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: field oriented vx speed
float: field oriented vy speed
"""
oriented_vx = vx * math.cos(heading) - vy * math.sin(heading)
oriented_vy = vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@property
def position(self):
return np.array([[self.odometry_x], [self.odometry_y]], dtype=float)
@property
def speed(self):
return math.hypot(self.odometry_x_vel, self.odometry_y_vel)
@property
def all_aligned(self):
return all(module.aligned for module in self.modules)
def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.driveStyle = "Tank"
SmartDashboard.putString("DriveStyle", self.driveStyle)
#SmartDashboard.putData("Mode", self.mode)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
self.zero()
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
self.k = 1
self.sensitivity = 1
SmartDashboard.putNumber("K Value", self.k)
SmartDashboard.putNumber("sensitivity", self.sensitivity)
self.prevLeft = 0
self.prevRight = 0
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0,
Ki=0.0,
Kd=1.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold_on()
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.reset_steer_setpoint()
self.last_heading = self.bno055.getAngle()
self.odometry_updated = False
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if not self.momentum:
pid_z = self.heading_pid_out.output
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
vz = input_vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
angle = self.bno055.getAngle()
vx, vy = self.robot_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
self.update_odometry()
self.odometry_updated = False # reset for next timestep
SmartDashboard.putNumber('module_a_speed',
self.modules[0].current_speed)
SmartDashboard.putNumber('module_b_speed',
self.modules[1].current_speed)
SmartDashboard.putNumber('module_c_speed',
self.modules[2].current_speed)
SmartDashboard.putNumber('module_d_speed',
self.modules[3].current_speed)
SmartDashboard.putNumber('module_a_pos',
self.modules[0].current_measured_azimuth)
SmartDashboard.putNumber('module_b_pos',
self.modules[1].current_measured_azimuth)
SmartDashboard.putNumber('module_c_pos',
self.modules[2].current_measured_azimuth)
SmartDashboard.putNumber('module_d_pos',
self.modules[3].current_measured_azimuth)
SmartDashboard.putNumber('odometry_x', self.odometry_x)
SmartDashboard.putNumber('odometry_y', self.odometry_y)
SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel)
SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel)
SmartDashboard.putNumber('odometry_z_vel', self.odometry_z_vel)
NetworkTables.flush()
def update_odometry(self):
if self.odometry_updated:
return
heading = self.bno055.getAngle()
heading_delta = constrain_angle(heading - self.last_heading)
heading_adjustment_factor = 1
adjusted_heading = heading - heading_adjustment_factor * heading_delta
timestep_average_heading = adjusted_heading - heading_delta / 2
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
v_x, v_y, v_z = self.robot_movement_from_odometry(
velocity_outputs, heading)
delta_x, delta_y, delta_z = self.robot_movement_from_odometry(
odometry_outputs, heading, z_vel=v_z)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
self.last_heading = heading
SmartDashboard.putNumber('odometry_delta_x', delta_x)
SmartDashboard.putNumber('odometry_delta_y', delta_y)
SmartDashboard.putNumber('imu_heading', heading)
SmartDashboard.putNumber('heading_delta', heading_delta)
SmartDashboard.putNumber('average_heading', timestep_average_heading)
self.odometry_updated = True
def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0):
lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None)
x, y, theta = lstsq_ret[0].reshape(3)
x_field, y_field = self.field_orient(x, y, angle + z_vel * (2.5 / 50))
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
"""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def robot_orient(vx, vy, heading):
"""Turn a vx and vy relative to the field into a vx and vy based on the
robot.
Args:
vx: vx to robot orient
vy: vy to robot orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: robot oriented vx speed
float: robot oriented vy speed
"""
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@staticmethod
def field_orient(vx, vy, heading):
"""Turn a vx and vy relative to the robot into a vx and vy based on the
field.
Args:
vx: vx to field orient
vy: vy to field orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: field oriented vx speed
float: field oriented vy speed
"""
oriented_vx = vx * math.cos(heading) - vy * math.sin(heading)
oriented_vy = vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@property
def position(self):
return np.array([[self.odometry_x], [self.odometry_y]], dtype=float)
@property
def speed(self):
return math.hypot(self.odometry_x_vel, self.odometry_y_vel)
class PositionController(BasePIDComponent):
drivetrain = Drivetrain
kP = tunable(0.05)
kI = tunable(0)
kD = tunable(0.005)
kF = tunable(0.0)
kToleranceInches = tunable(0.2 if hal.HALIsSimulation() else 0.75)
kIzone = tunable(0)
angle_controller = AngleController
kAngleP = 0.05 if hal.HALIsSimulation() else 0.1
kAngleI = 0
kAngleD = 0
kAngleF = 0
kAngleMax = 0.18
# Angle correction factor
angle = 0
def __init__(self):
super().__init__(self.get_position, 'position_controller')
self.set_abs_output_range(0.18, 0.7)
# Angle correction PID controller - used to maintain a straight
# heading while the encoders track distance traveled.
self._angle_offset = 0
self.angle_pid_controller = PIDController(Kp=self.kAngleP,
Ki=self.kAngleI,
Kd=self.kAngleD,
Kf=self.kAngleF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.kAngleMax,
self.kAngleMax)
def get_position(self):
return self.drivetrain.get_position()
def get_angle(self):
return self.angle_controller.get_angle() - self._angle_offset
def reset_position_and_heading(self):
self.drivetrain.shift_low_gear()
self.drivetrain.reset_position()
self._angle_offset = self.angle_controller.get_angle()
self.angle_pid_controller.setSetpoint(0)
def move_to(self, position):
self.setpoint = position
self.angle_pid_controller.enable()
def is_at_location(self):
return self.enabled and \
abs(self.get_position() - self.setpoint) < self.kToleranceInches
def pidWrite(self, output):
self.rate = -output
def pidWriteAngle(self, angle):
self.angle = angle
def execute(self):
super().execute()
if self.rate is not None:
if self.is_at_location():
self.stop()
else:
self.drivetrain.differential_drive(self.rate,
self.angle,
squared=False,
force=True)
def stop(self):
self.drivetrain.differential_drive(0)
self.angle_pid_controller.disable()
def on_disable(self):
self.stop()
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = False
self.hold_heading = True
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=0.1,
Ki=0.0,
Kd=0.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
def on_enable(self):
self.heading_pid.reset()
self.set_heading_sp_current()
# matrix which translates column vector of [x, y, z] in robot frame of
# reference to module [x, y] movement
self.A_matrix = np.array([[1, 0, -1], [0, 1, 1], [1, 0, -1],
[0, 1, -1], [1, 0, 1], [0, 1, -1], [1, 0, 1],
[0, 1, 1]])
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in our
# matrix
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
z_comp = module_dist / 2
# third column in A matrix already encodes direction of robot's
# vz index upon the module's axis, just need to multiply to
# encode magnitude
self.A_matrix[i * 2, 2] = z_comp * self.A_matrix[i * 2, 2]
self.A_matrix[i * 2 + 1, 2] = z_comp * self.A_matrix[i * 2 + 1, 2]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
for module in self.modules:
module.reset_steer_setpoint()
def execute(self):
pid_z = 0
if self.hold_heading:
pid_z = self.heading_pid.get()
vz = self.vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
if hal.isSimulation():
from hal_impl.data import hal_data
angle = math.radians(-hal_data['robot']['bno055'])
else:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
delta_x, delta_y, delta_theta = self.robot_movement_from_odometry(
odometry_outputs)
v_x, v_y, v_z = self.robot_movement_from_odometry(velocity_outputs)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_theta += delta_theta
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
def robot_movement_from_odometry(self, odometry_outputs):
lstsq_ret = np.linalg.lstsq(self.A_matrix, odometry_outputs, rcond=-1)
x, y, theta = lstsq_ret[0].reshape(3)
angle = self.bno055.getAngle()
x_field, y_field = self.field_orient(x, y, angle)
return x_field, y_field, theta
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
:param vx: The vx (forward) component of the robot's desired velocity. In m/s.
:param vy: The vy (leftward) component of the robot's desired velocity. In m/s.
:param vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s."""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def field_orient(vx, vy, heading):
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0,
Ki=0.0,
Kd=0.2,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-3, 3)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
self.momentum = False
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold_on()
# matrix which translates column vector of [x, y, z] in robot frame of
# reference to module [x, y] movement
self.A_matrix = np.array([[1, 0, -1], [0, 1, 1], [1, 0, -1],
[0, 1, -1], [1, 0, 1], [0, 1, -1], [1, 0, 1],
[0, 1, 1]])
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in our
# matrix
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
z_comp = module_dist / 2
# third column in A matrix already encodes direction of robot's
# vz index upon the module's axis, just need to multiply to
# encode magnitude
self.A_matrix[i * 2, 2] = z_comp * self.A_matrix[i * 2, 2]
self.A_matrix[i * 2 + 1, 2] = z_comp * self.A_matrix[i * 2 + 1, 2]
for module in self.modules:
module.reset_steer_setpoint()
self.last_heading = self.bno055.getAngle()
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if self.vz is None:
self.momentum = False
if not self.momentum:
pid_z = self.heading_pid.get()
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
vz = input_vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
heading = self.bno055.getAngle()
heading_delta = heading - self.last_heading
timestep_average_heading = heading - heading_delta / 2
delta_x, delta_y, delta_theta = self.robot_movement_from_odometry(
odometry_outputs, timestep_average_heading)
v_x, v_y, v_z = self.robot_movement_from_odometry(
velocity_outputs, heading)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_theta += delta_theta
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
SmartDashboard.putNumber('module_a_speed',
self.modules[0].current_speed)
SmartDashboard.putNumber('module_b_speed',
self.modules[1].current_speed)
SmartDashboard.putNumber('module_c_speed',
self.modules[2].current_speed)
SmartDashboard.putNumber('module_d_speed',
self.modules[3].current_speed)
SmartDashboard.putNumber('module_a_pos',
self.modules[0].current_measured_azimuth)
SmartDashboard.putNumber('module_b_pos',
self.modules[1].current_measured_azimuth)
SmartDashboard.putNumber('module_c_pos',
self.modules[2].current_measured_azimuth)
SmartDashboard.putNumber('module_d_pos',
self.modules[3].current_measured_azimuth)
SmartDashboard.putNumber('odometry_x', self.odometry_x)
SmartDashboard.putNumber('odometry_y', self.odometry_y)
SmartDashboard.putNumber('odometry_theta', self.odometry_theta)
SmartDashboard.putNumber('odometry_delta_x', delta_x)
SmartDashboard.putNumber('odometry_delta_y', delta_y)
SmartDashboard.putNumber('odometry_delta_theta', delta_theta)
SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel)
SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel)
SmartDashboard.putNumber('odometry_z_vel', self.odometry_z_vel)
NetworkTables.flush()
self.last_heading = heading
def robot_movement_from_odometry(self, odometry_outputs, angle):
lstsq_ret = np.linalg.lstsq(self.A_matrix, odometry_outputs, rcond=-1)
x, y, theta = lstsq_ret[0].reshape(3)
x_field, y_field = self.field_orient(x, y, 2 * math.pi - angle)
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
"""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def field_orient(vx, vy, heading):
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
class DriveController(StateMachine):
chassis: Chassis
wheel_diameter = 0.1 # wheel diameter in meters
encoder_ticks_per_revolution = 256 * 1.8 * 3 * 5 # encoder ticks per rev
distance_setpoint = 0 # Distance setpoint
angle_setpoint = 0 # Angle setpoint
left_speed = 0 # Current left speed
right_speed = 0 # Current right speed
turn_speed = 0 # Current turn speed
loop_counter = 0 # A counter that represents the number of times the drive function has been called.
finished = False # Is finished?
enabled = False # Is enable?
def setup(self):
self.motor_updater = Notifier(
self.update_motors) # A motor updater thread
self.setup_values()
self.start()
def setup_values(self,
input_range=15,
output_range=1,
tolerance=0.05,
period=0.02,
kp=0.08,
ki=0,
kd=0,
kf=0):
""" Sets up all the different values that will be used during the course of the PID control loop. """
self.input_range = input_range
self.output_range = output_range
self.tolerance = tolerance
self.period = period
self.kp = kp
self.ki = ki
self.kd = kd
self.kf = kf
def start(self):
# Setting up the left controller
self.left_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_left_encoder,
self.left_output)
self.left_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.left_holder,
output=self.left_holder,
period=self.period)
self.left_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.left_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.left_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.left_pid_contorller.setContinuous(True)
# Setting up the right controller
self.right_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_right_encoder,
self.right_output)
self.right_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.right_holder,
output=self.right_holder,
period=self.period)
self.right_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.right_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.right_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.right_pid_contorller.setContinuous(True)
# Setting up the turn controller
self.turn_holder = IOGyroHolder(self.chassis.get_angle,
self.turn_output)
self.turn_pid_contorller = PIDController(0.02,
self.ki,
self.kd,
self.kf,
source=self.turn_holder,
output=self.turn_holder,
period=self.period)
self.turn_pid_contorller.setInputRange(0, 360)
self.turn_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.turn_pid_contorller.setAbsoluteTolerance(10)
self.turn_pid_contorller.setContinuous(True)
# put different pidf values for angle control
def enable(self, distance: float, angle: float):
self.distance_setpoint = distance
self.angle_setpoint = angle
# Setting the setpoints
self.left_pid_contorller.setSetpoint(distance)
self.right_pid_contorller.setSetpoint(distance)
self.turn_pid_contorller.setSetpoint(angle)
# Enabling the PID Controllers
self.left_pid_contorller.enable()
self.right_pid_contorller.enable()
self.turn_pid_contorller.enable()
self.motor_updater.startPeriodic(self.period)
self.enabled = True
def left_output(self, speed):
self.left_speed = speed
def right_output(self, speed):
self.right_speed = speed
def turn_output(self, linear_speed):
self.turn_speed = linear_speed
def update_motors(self):
# print("distance for cotroller", self.left_pid_contorller.getError(), self.right_pid_contorller.getError(),
# self.turn_pid_contorller.getError())
self.left_pid_contorller.setSetpoint(self.distance_setpoint)
self.right_pid_contorller.setSetpoint(self.distance_setpoint)
self.turn_pid_contorller.setSetpoint(self.angle_setpoint)
self.chassis.set_motors_values(self.left_speed - self.turn_speed,
self.right_speed + self.turn_speed)
def set_input_range(self, range):
self.input_range = range
def set_tolerance(self, tolerance):
self.tolerance = tolerance
def set_kp(self, kp):
self.kp = kp
def set_ki(self, ki):
self.ki = ki
def set_kd(self, kd):
self.kd = kd
def set_kf(self, kf):
self.kf = kf
def set_period(self, period):
self.period = period
def restart(self):
self.chassis.reset_encoders()
self.chassis.reset_navx()
self.left_speed = 0
self.right_speed = 0
self.turn_speed = 0
def is_finished(self):
return self.left_pid_contorller.onTarget(
) and self.right_pid_contorller.onTarget(
) and self.turn_pid_contorller.onTarget()
def close(self):
self.left_speed = 0
self.right_speed = 0
self.turn_speed = 0
self.motor_updater.close()
self.left_pid_contorller.close()
self.right_pid_contorller.close()
self.turn_pid_contorller.close()
@state(first=True)
def drive(self, initial_call):
print(self.loop_counter)
self.loop_counter += 1
if initial_call:
self.restart()
self.enable(1, 90)
elif self.is_finished():
self.close()
self.finished = True
self.enabled = False
self.done()
