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