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] in robot frame of # reference to module [x, y] movement self.A_matrix = np.array([ [1, 0], [0, 1], [1, 0], [0, 1], [1, 0], [0, 1], [1, 0], [0, 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 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) 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.robot_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)) heading = self.bno055.getAngle() heading_delta = constrain_angle(heading - self.last_heading) timestep_average_heading = heading - heading_delta / 2 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() z_adj_delta = self.z_axis_adjustment * heading_delta z_adj_vel = self.z_axis_adjustment * self.bno055.getHeadingRate() odometry_outputs = odometry_outputs - z_adj_delta velocity_outputs = velocity_outputs - z_adj_vel delta_x, delta_y = self.robot_movement_from_odometry(odometry_outputs, timestep_average_heading) v_x, v_y = self.robot_movement_from_odometry(velocity_outputs, heading) self.odometry_x += delta_x self.odometry_y += delta_y self.odometry_x_vel = v_x self.odometry_y_vel = v_y 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_delta_x', delta_x) SmartDashboard.putNumber('odometry_delta_y', delta_y) SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel) SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel) SmartDashboard.putNumber('imu_heading', heading) SmartDashboard.putNumber('heading_delta', heading_delta) SmartDashboard.putNumber('average_heading', timestep_average_heading) 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 = lstsq_ret[0].reshape(2) x_field, y_field = self.field_orient(x, y, angle) return x_field, y_field 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
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