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 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 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:
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 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 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 = 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
