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 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)
class DriveTrain(Subsystem): """DriveTrain: is the subsystem responsible for motors and devices associated with driving subystem. As a subsystem, we represent the single point of contact for all drivetrain-related controls. Specifically, commands that manipulate the drivetrain should 'require' the singleton instance (via require(robot.driveTrain)). Unless overridden, our default command, JoystickDriver, is the means by which driving occurs. """ k_minThrottleScale = 0.5 k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally k_maxDriveSpeed = 150.0 # ~20 ft/sec k_maxTurnSpeed = 40.0 # ~3-4 ft/sec k_fastTurn = -1 k_mediumTurn = -.72 k_slowTurn = -.55 k_quadTicksPerWheelRev = 9830 k_wheelDiameterInInches = 14.0 k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches k_turnKp = .1 k_turnKi = 0 k_turnKd = .3 k_turnKf = .001 k_ControlModeSpeed = 0 k_ControlModeVBus = 1 k_ControlModeDisabled = 2 class _turnHelper(PIDOutput): """a private helper class for PIDController-based imu-guided turning. """ def __init__(self, driveTrain): super().__init__() self.driveTrain = driveTrain def pidWrite(self, output): self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed) def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot # STEP 1: instantiate the motor controllers self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId) self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId) self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId) self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId) # Step 2: Configure the follower Talons: left & right back motors self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID()) self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID()) # STEP 3: Setup speed control mode for the master Talons self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) # STEP 4: Indicate the feedback device used for closed-loop # For speed mode, indicate the ticks per revolution self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) # STEP 5: Set PID values & closed loop error self.leftMasterMotor.setPID(0.22, 0, 0) self.rightMasterMotor.setPID(0.22, 0, 0) # Add ramp up rate self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage # change /sec: reach to # 12V after 1sec self.rightMasterMotor.setVoltageRampRate(48.0) # Add SmartDashboard controls for testing # Add SmartDashboard live windowS LiveWindow.addActuator("DriveTrain", "Left Master %d" % robot.map.k_DtLeftMasterId, self.leftMasterMotor) LiveWindow.addActuator("DriveTrain", "Right Master %d" % robot.map.k_DtRightMasterId, self.rightMasterMotor) # init RobotDrive - all driving should occur through its methods # otherwise we expect motor saftey warnings self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor) self.robotDrive.setSafetyEnabled(True) # init IMU - used for driver & vision feedback as well as for # some autonomous modes. self.visionState = self.robot.visionState self.imu = BNO055() self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf, self.imu, DriveTrain._turnHelper(self)) self.turnPID.setOutputRange(-1, 1) self.turnPID.setInputRange(-180, 180) self.turnPID.setPercentTolerance(2) self.turnMultiplier = DriveTrain.k_mediumTurn self.maxSpeed = DriveTrain.k_defaultDriveSpeed # self.setContinuous() ? robot.info("Initialized DriveTrain") def initForCommand(self, controlMode): self.leftMasterMotor.setEncPosition(0) # async call self.rightMasterMotor.setEncPosition(0) self.robotDrive.stopMotor() self.robotDrive.setMaxOutput(self.maxSpeed) if controlMode == self.k_ControlModeSpeed: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) elif controlMode == self.k_ControlModeVBus: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) elif controlMode == self.k_ControlModeDisabled: # unverified codepath self.leftMasterMotor.disableControl() self.rightMasterMotor.disableControl() else: self.robot.error("Unexpected control mode") self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" % (self.leftMasterMotor.getControlMode(), self.rightMasterMotor.getControlMode())) def initDefaultCommand(self): # control modes are integers values: # 0 percentvbux # 1 position # 2 velocity self.setDefaultCommand(JoystickDriver(self.robot)) self.robotDrive.stopMotor(); def updateDashboard(self): # TODO: additional items? SmartDashboard.putNumber("IMU heading", self.getCurrentHeading()) SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration()) def stop(self): self.robotDrive.stopMotor() def joystickDrive(self, jsY, jsX, throttle): """ joystickDrive - called by JoystickDriver command. Inputs are always on the range [-1, 1]... These values can be scaled for a better "feel", but should always be in a subset of this range. """ if self.robot.isAutonomous or \ (math.fabs(jx) < 0.075 and math.fabs(jy) < .075): # joystick dead zone or auto (where we've been scheduled via # default command machinery) self.robotDrive.stopMotor() else: st = self.scaleThrottle(throttle) self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st) def drive(self, outputmag, curve): """ drive - used by drivestraight command.. """ self.robotDrive.drive(outputmag, curve) def driveStraight(self, speed): """driveStraight: invoked from AutoDriveStraight.. """ # NB: maxOuput isn't applied via set so # speed is supposedly measured in rpm but this depends on our # initialization establishing encoding ticks per revolution. # This is approximate so we rely on the observed values above. # (DEFAULT_SPEED_MAX_OUTPUT) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = speed / self.maxSpeed rotateValue = 0 self.robotDrive.arcadeDrive(moveValue, rotateValue) def startAutoTurn(self, degrees): self.robot.info("start autoturn from %d to %d" % (self.getHeading(), degrees)) self.turnPID.reset() self.turnPID.setSetpoint(degrees) self.turnPID.enable() def endAutoTurn(self): if self.turnPID.isEnabled(): self.turnPID.disable() def isAutoTurning(self): return self.turnPID.isEnabled() def isAutoTurnFinished(self): return self.turnPID.onTarget() def turn(self, speed): """turn takes a speed, not an angle... A negative speed is interpretted as turn left. Note that we bypass application of setMaxOutput Which only applies to calls to robotDrive. """ # In order to turn left, we want the right wheels to go # forward and left wheels to go backward (cf: tankDrive) # Oddly, our right master motor is reversed, so we compensate here. # speed < 0: turn left: rightmotor(negative) (forward), # leftmotor(negative) (backward) # speed > 0: turn right: rightmotor(positive) (backward) # leftmotor(positive) (forward) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = 0 rotateValue = speed / self.maxSpeed self.robotDrive.arcadeDrive(moveValue, rotateValue) def trackVision(self): """presumed called by either autonomous or teleop commands to allow the vision subsystem to guide the robot """ if self.visionState.DriveLockedOnTarget: self.stop() else: if self.isAutoTurning(): if self.isAutoTurnFinished(): self.endAutoTurn() self.visionState.DriveLockedOnTarget = True else: # setup for an auto-turn h = self.getCurrentHeading() tg = self.getTargetHeading(h) self.startAutoTurn(tg) def getCurrentHeading(self): """ getCurrentHeading returns a value between -180 and 180 """ return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi def scaleThrottle(self, rawThrottle): """ scaleThrottle: returns a scaled value between MIN_THROTTLE_SCALE and 1.0 MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation Scale the joystick values by throttle before passing to the driveTrain +1=bottom position; -1=top position """ # Throttle in the range of -1 to 1. We would like to change that to a # range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle # value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1)) # Then, add 1 to make the range 0-2 rather than -1 to +1 # Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale) # Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1 # # Check the results are in the range of k_minThrottleScale to 1, and clip # it in case the math went horribly wrong. result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale if result < self.k_minThrottleScale: # Somehow our math was wrong. Our value was too low, so force it to the minimum result = self.k_mintThrottleScale elif result > 1: # Somehow our math was wrong. Our value was too high, so force it to the maximum result = 1.0 return result def modifyTurnSpeed(self, speedUp): if speedUp: self.turnMultiplier = self.k_mediumTurn else: self.turnMultiplier = self.k_slowTurn def inchesToTicks(self, inches): return int(self.k_quadTicksPerInch * inches) def destinationReached(self, distance): return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \ math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
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 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()
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 DriveBase: left_motor_one = CANTalon left_motor_two = CANTalon right_motor_one = CANTalon right_motor_two = CANTalon left_encoder = Encoder right_encoder = Encoder navx = AHRS def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag def drive(self, left_power, right_power): self.base.tankDrive(left_power, right_power) def execute(self): if int(self.base.getNumMotors()) < 3: self.base.drive(0, 0) def get_drive_distance(self): return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance()) def rest_base(self): self.left_encoder.reset() self.right_encoder.reset() def pid_drive(self, speed, distance, to_angle=None): self.navx.isCalibrating() self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F) self.pid_d_controller.setOutputRange(speed - distance, speed + distance) if to_angle is None: set_angle = self.navx.getYaw() else: set_angle = to_angle self.pid_d_controller.setSetpoint(float(set_angle)) self.drive(speed + 0.03, speed) self.pid_d_controller.enable() self.current_flag = self.type_flag[0] self.auto_pid_out.current_flag = self.current_flag def pid_turn(self, angle): self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.turn_P, sensor_map.turn_I, sensor_map.turn_D, sensor_map.turn_F) self.pid_d_controller.setOutputRange(sensor_map.output_range_min, sensor_map.output_range_max) self.pid_d_controller.setSetpoint(float(angle)) self.pid_d_controller.enable() self.current_flag = self.type_flag[1] self.auto_pid_out.current_flag = self.current_flag def stop_pid(self): self.pid_d_controller.disable() self.pid_d_controller.reset()
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