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()