class MyRobot(wpilib.TimedRobot): def robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = 5e-5 self.kI = 1e-6 self.kD = 0 self.kIz = 0 self.PIDTolerance = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well # Push PID Coefficients to SmartDashboard wpilib.SmartDashboard.putNumber("P Gain", self.kP) wpilib.SmartDashboard.putNumber("I Gain", self.kI) wpilib.SmartDashboard.putNumber("D Gain", self.kD) wpilib.SmartDashboard.putNumber("I Zone", self.kIz) wpilib.SmartDashboard.putNumber("Set Rotations", 0) wpilib.SmartDashboard.putNumber("Tolerance", self.PIDTolerance) wpilib.SmartDashboard.putBoolean("Manual Control", False) wpilib.SmartDashboard.putBoolean("Manual Speed", 0) def encoderBoundedPosition(self, encoder): #I don't know if there's a set continuous for encoders, but it's easy enough to write position = encoder.getPosition() position %= 360 if position < 0: position += 360 return position def brakeMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kBrake) self.turnMotor.setIdleMode(rev.IdleMode.kBrake) def coastMode(self): self.driveMotor.setIdleMode(rev.IdleMode.kCoast) self.turnMotor.setIdleMode(rev.IdleMode.kCoast) def turnSpeedCalculator(self): speed = self.turnController.calculate(self.encoderBoundedPosition(self.turnEncoder)) if abs(speed) > 1: speed /= abs(speed) return speed def autonomousInit(self): self.brakeMode() def autonomousPeriodic(self): pass def teleopInit(self): self.brakeMode() def teleopPeriodic(self): # Read data from SmartDashboard p = wpilib.SmartDashboard.getNumber("P Gain", self.kP) i = wpilib.SmartDashboard.getNumber("I Gain", self.kI) d = wpilib.SmartDashboard.getNumber("D Gain", self.kD) iz = wpilib.SmartDashboard.getNumber("I Zone", self.kIz) tolerance = wpilib.SmartDashboard.getNumber("Tolerance", self.PIDTolerance) test = wpilib.SmartDashboard.getBoolean("Manual Control", False) controlSpeed = wpilib.SmartDashboard.getNumber("Manual Speed", 0) # Update PIDController datapoints with the latest from SmartDashboard if p != self.kP: self.turnController.setP(p) self.kP = p if i != self.kI: self.turnController.setI(i) self.kI = i if d != self.kD: self.turnController.setD(d) self.kD = d if tolerance != self.PIDTolerance: self.turnController.setTolerance(tolerance) self.PIDTolerance = tolerance speed = self.turnSpeedCalculator() wpilib.SmartDashboard.putNumber("Process Variable", self.encoderBoundedPosition(self.turnEncoder)) setpoint = wpilib.SmartDashboard.getNumber("Set Rotations", 0) self.turnController.setSetpoint(setpoint) wpilib.SmartDashboard.putNumber("Motor Input", speed) if test: self.turnMotor.set(controlSpeed) else: self.turnMotor.set(speed) def disabledInit(self): self.coastMode()
class SwerveChassis: WIDTH = 0.75 LENGTH = 0.75 imu: NavX 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_x_vel = tunable(0) # odometry_y_vel = tunable(0) # odometry_z_vel = tunable(0) hold_heading = tunable(True) def __init__(self): self.vx = 0 self.vy = 0 self.vz = 0 self.field_oriented = False self.momentum = False self.automation_running = False def setup(self): # Heading PID controller self.heading_pid = PIDController( Kp=6.0, Ki=0.0, Kd=0.05, measurement_source=self.imu.getAngle, period=1 / 50) # this gain is being changed depending on speed self.heading_pid.setInputRange(-math.pi, math.pi) self.heading_pid.setOutputRange(-3, 3) self.heading_pid.setContinuous() self.modules = [ self.module_a, self.module_b, self.module_c, self.module_d ] self.odometry_x = 0 self.odometry_y = 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, ) self.last_odometry_time = 0 # wpilib.SmartDashboard.putData("heading_pid", self.heading_pid) # 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 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) def set_heading_sp_current(self): self.set_heading_sp(self.imu.getAngle()) def set_heading_sp(self, setpoint): self.heading_pid.setReference(setpoint) def heading_hold_on(self): self.set_heading_sp_current() self.heading_pid.reset() self.hold_heading = True def heading_hold_off(self): self.hold_heading = False def on_enable(self): self.heading_hold_on() self.last_heading = self.imu.getAngle() self.odometry_updated = False for module in self.modules: module.reset_encoder_delta() self.last_odometry_time = time.monotonic() def execute(self): pid_z = 0 if self.hold_heading: pid_z = self.heading_pid.update() 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 self.momentum: self.set_heading_sp_current() pid_z = 0 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, absolute_rotation=False) if abs(math.hypot(self.vx, self.vy)) > 0.5: self.heading_pid.setP(2.0) else: self.heading_pid.setP(6.0) def update_odometry(self, *args): # 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) now = time.monotonic() 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=self.imu.getHeadingRate() # ) delta_t = now - self.last_odometry_time delta_x = vx * delta_t delta_y = vy * delta_t 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 self.set_modules_drive_brake() self.last_odometry_time = now 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.vx = vx self.vy = vy self.vz = vz self.field_oriented = field_oriented @staticmethod def robot_orient(vx: float, vy: float, heading: float) -> Tuple[float, float]: """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: tuple of robot oriented vx and vy """ c_h = math.cos(heading) s_h = math.sin(heading) oriented_vx = vx * c_h + vy * s_h oriented_vy = -vx * s_h + vy * c_h return oriented_vx, oriented_vy @staticmethod def field_orient(vx: float, vy: float, heading: float) -> Tuple[float, float]: """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: tuple of field oriented vx and vy """ c_h = math.cos(heading) s_h = math.sin(heading) oriented_vx = vx * c_h - vy * s_h oriented_vy = vx * s_h + vy * c_h return oriented_vx, oriented_vy @property def position(self): return self.odometry_x, self.odometry_y @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 set_modules_drive_coast(self): for module in self.modules: module.set_drive_coast() def set_modules_drive_brake(self): for module in self.modules: module.set_drive_brake()