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 SuperPIDController:
def __init__(self, pid_values: PIDValue, f_in, f_out, f_feedforwards=None, pid_key=None):
self.ff_enabled = f_feedforwards != None
if self.ff_enabled:
ff = f_feedforwards
else:
ff = lambda x: 0
self.pid_dash_enabled = pid_key != None
if self.pid_dash_enabled:
self.pid_key = pid_key
else:
self.pid_key = ""
self.pid_values = pid_values
self.pid_controller = PIDController(
0, 0, 0,
f_in,
lambda x: f_out(x + ff(self.get_target(), x))
)
self.pid_values.update_controller(self.pid_controller)
def get_target(self):
if self.pid_controller.isEnabled():
return self.pid_controller.getSetpoint()
else:
return 0
def configure_controller(self, output_range=(-1, 1), percent_tolerance=1):
self.pid_controller.setOutputRange(output_range[0], output_range[1])
self.pid_controller.setPercentTolerance(percent_tolerance)
def update_values(self, pid_values: PIDValue):
self.pid_values = pid_values
self.pid_values.update_controller(self.pid_controller)
def update_from_dash(self):
if self.pid_dash_enabled:
self.update_values(get_pid(self.pid_key))
def push_to_dash(self):
if self.pid_dash_enabled:
put_pid(self.pid_key, self.pid_values)
def get_on_target(self):
return self.pid_controller.onTarget() if self.pid_controller.isEnabled() else True
def stop(self):
self.reset()
self.pid_controller.disable()
def start(self):
self.reset()
self.pid_controller.enable()
def reset(self):
self.pid_controller.reset()
def run_setpoint(self, value):
self.pid_controller.setSetpoint(value)
self.start()
