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 Chassis:
correct_range = 1.4 # 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.setPercentTolerance(3.0)
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)
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.4):
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))
logging.getLogger("chassis").info("XYD: %f %f %f" % (x,y,d))
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
distances = (abs(self._modules['c'].distance) + abs(self._modules['d'].distance))
#"""for module in self._modules.values():
# distances += abs(module.distance) / module.drive_counts_per_metre"""
return distances / 2.0 / self._modules['c'].drive_counts_per_metre
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():
# 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()*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.enable()
# 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 name, 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 abs(self.vision.pidGet()) < 0.05
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()
