class SwerveModule():
def __init__(self,
driveCanTalonId,
steerCanTalonId,
absoluteEncoder=True,
reverseDrive=False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(
CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
# Always in radians. Right hand rule applies - Z is up!
# Rescale values to the range [-pi, pi)
def steer(self, direction, speed=0):
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self._direction)
delta = min_angular_displacement(current_heading, direction)
self._direction += delta
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self._direction) - direction) < math.pi / 6.0:
self._drive.set(speed)
self._speed = speed
else:
self._drive.set(-speed)
self._speed = -speed
if self.absoluteEncoder:
self._steer.set(self._direction / TAU *
RobotMap.module_rotation_volts_per_revolution)
else:
self._steer.set(self._direction / TAU *
RobotMap.module_rotation_counts_per_revolution)
def getSpeed(self):
return self._speed
def getDirection(self):
return self._direction
class SwerveModule():
def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder = True, reverseDrive = False):
# Initialise private motor controllers
self._drive = CANTalon(driveCanTalonId)
self.reverse_drive = reverseDrive
self._steer = CANTalon(steerCanTalonId)
self.absoluteEncoder = absoluteEncoder
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absoluteEncoder:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
self._steer.setPosition(0.0)
# Private members to store the setpoints
self._speed = 0.0
self._direction = 0.0
self._opposite_direction = math.pi
# Always in radians. Right hand rule applies - Z is up!
# Rescale values to the range [-pi, pi)
def steer(self, direction, speed = 0):
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self._direction)
delta = min_angular_displacement(current_heading, direction)
self._direction += delta
if self.reverse_drive:
speed=-speed
if abs(constrain_angle(self._direction)-direction)<math.pi/6.0:
self._drive.set(speed)
self._speed = speed
else:
self._drive.set(-speed)
self._speed = -speed
if self.absoluteEncoder:
self._steer.set(self._direction/TAU*RobotMap.module_rotation_volts_per_revolution)
else:
self._steer.set(self._direction/TAU*RobotMap.module_rotation_counts_per_revolution)
def getSpeed(self):
return self._speed
def getDirection(self):
return self._direction
class GRTTalon:
def __init__(self, channel):
self.t = CANTalon(channel)
self.channel = channel
def set(self, power):
self.t.set(power)
def get(self):
return self.t.get()
print(self.t.Get())
def getDeviceID(self):
return self.t.getDeviceID()
def changeControlMode(self, mode):
self.t.changeControlMode(mode)
def setP(self, value):
self.t.setP(value)
def setI(self, value):
self.t.setI(value)
def setD(self, value):
self.t.setD(value)
def poll(self):
# self.busVoltage = t.getBusVoltage()
# self.closeLoopRampRate = t.getCloseLoopRampRate()
self.controlMode = t.getControlMode()
self.deviceID = t.getDeviceID()
self.encPosition = t.getEncPosition()
self.encVelocity = t.getEncVelocity()
self.outputCurrent = t.getOutputCurrent()
self.outputVoltage = t.getOutputVoltage()
self.closeLoopError = t.getCloseLoopError()
class SwerveModule():
def __init__(self, drive, steer,
absolute=True, reverse_drive=False,
reverse_steer=False, zero_reading=0,
drive_encoder=False, reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian*2.0*math.pi/4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80*6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def changeDriveControlMode(self, control_mode):
if self._drive.getControlMode is not control_mode:
if control_mode == CANTalon.ControlMode.Speed:
self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed)
elif control_mode == CANTalon.ControlMode.Position:
self._drive.setPID(0.1, 0.0, 0.0, 0.0)
self._drive.changeControlMode(control_mode)
@property
def direction(self):
# Read the current direction from the controller setpoint
setpoint = self._steer.getSetpoint()
return float(setpoint - self._offset) / self.counts_per_radian
@property
def speed(self):
# Read the current speed from the controller setpoint
setpoint = self._drive.getSetpoint()
return float(setpoint)
@property
def distance(self):
# Read the current position from the encoder and remove the offset
return self._drive.getEncPosition() - self._distance_offset
def zero_distance(self):
self._distance_offset = self._drive.getEncPosition()
def steer(self, direction, speed=None):
if self.drive_encoder:
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
else:
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
if speed is None:
# Force the modules to the direction specified - don't
# go to the closest one and reverse.
delta = constrain_angle(direction - self.direction) # rescale to +/-pi
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
self._drive.set(0.0)
return
if abs(speed) > 0.05:
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self.direction)
delta = min_angular_displacement(current_heading, direction)
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0:
self._drive.set(speed*self.drive_max_speed)
else:
self._drive.set(-speed*self.drive_max_speed)
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
else:
self._drive.set(0.0)
from grt.sensors.navx import NavX
from grt.macro.straight_macro import StraightMacro
from grt.mechanism.pickup import Pickup
from grt.mechanism.manual_shooter import ManualShooter
from queue import Queue
#Compressor initialization
c = Compressor()
c.start()
#Manual pickup Talons and Objects
pickup_achange_motor1 = CANTalon(45)
pickup_achange_motor2 = CANTalon(46)
pickup_achange_motor1.changeControlMode(CANTalon.ControlMode.Follower)
pickup_achange_motor1.set(47)
pickup_achange_motor1.reverseOutput(True)
pickup_roller_motor = CANTalon(8)
pickup = Pickup(pickup_achange_motor1, pickup_achange_motor2,
pickup_roller_motor)
#Manual shooter Talons and Objects
flywheel_motor = CANTalon(44)
shooter_act = Solenoid(1)
turntable_motor = CANTalon(12)
manual_shooter = ManualShooter(flywheel_motor, shooter_act, turntable_motor)
#DT Talons and Objects
class SwerveModule():
def __init__(self, drive, steer,
absolute=True, reverse_drive=False,
reverse_steer=False, zero_reading=0,
drive_encoder=False, reverse_drive_encoder=False):
# Initialise private motor controllers
self._drive = CANTalon(drive)
self.reverse_drive = reverse_drive
self._steer = CANTalon(steer)
self.drive_encoder = drive_encoder
self._distance_offset = 0 # Offset the drive distance counts
# Set up the motor controllers
# Different depending on whether we are using absolute encoders or not
if absolute:
self.counts_per_radian = 1024.0 / (2.0 * math.pi)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.reverseSensor(reverse_steer)
self._steer.reverseOutput(not reverse_steer)
# Read the current encoder position
self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs
self._offset = zero_reading - 256.0
if reverse_steer:
self._offset = -self._offset
else:
self._steer.changeControlMode(CANTalon.ControlMode.Position)
self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel
self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 /
(2.0 * math.pi))
self._offset = self.counts_per_radian*2.0*math.pi/4.0
self._steer.setPosition(0.0)
if self.drive_encoder:
self.drive_counts_per_rev = 80*6.67
self.drive_counts_per_metre = (self.drive_counts_per_rev /
(math.pi * 0.1016))
self.drive_max_speed = 570
self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
self._drive.reverseSensor(reverse_drive_encoder)
else:
self.drive_counts_per_rev = 0.0
self.drive_max_speed = 1.0
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
self._drive.setVoltageRampRate(150.0)
def changeDriveControlMode(self, control_mode):
if self._drive.getControlMode is not control_mode:
if control_mode == CANTalon.ControlMode.Speed:
self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed)
elif control_mode == CANTalon.ControlMode.Position:
self._drive.setPID(0.1, 0.0, 0.0, 0.0)
self._drive.changeControlMode(control_mode)
@property
def direction(self):
# Read the current direction from the controller setpoint
setpoint = self._steer.getSetpoint()
return float(setpoint - self._offset) / self.counts_per_radian
@property
def speed(self):
# Read the current speed from the controller setpoint
setpoint = self._drive.getSetpoint()
return float(setpoint)
@property
def distance(self):
# Read the current position from the encoder and remove the offset
return self._drive.getEncPosition() - self._distance_offset
def zero_distance(self):
self._distance_offset = self._drive.getEncPosition()
def steer(self, direction, speed=None):
if self.drive_encoder:
self.changeDriveControlMode(CANTalon.ControlMode.Speed)
else:
self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus)
# Set the speed and direction of the swerve module
# Always choose the direction that minimises movement,
# even if this means reversing the drive motor
if speed is None:
# Force the modules to the direction specified - don't
# go to the closest one and reverse.
delta = constrain_angle(direction - self.direction) # rescale to +/-pi
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
self._drive.set(0.0)
return
if abs(speed) > 0.05:
direction = constrain_angle(direction) # rescale to +/-pi
current_heading = constrain_angle(self.direction)
delta = min_angular_displacement(current_heading, direction)
if self.reverse_drive:
speed = -speed
if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0:
self._drive.set(speed*self.drive_max_speed)
else:
self._drive.set(-speed*self.drive_max_speed)
self._steer.set((self.direction + delta) *
self.counts_per_radian + self._offset)
else:
self._drive.set(0.0)
from grt.mechanism.mechcontroller import MechController #DT Talons and Objects dt_right = CANTalon(1) dt_r2 = CANTalon(2) dt_r3 = CANTalon(3) dt_r4 = CANTalon(4) dt_left = CANTalon(7) dt_l2 = CANTalon(8) dt_l3 = CANTalon(9) dt_l4 = CANTalon(10) dt_r2.changeControlMode(CANTalon.ControlMode.Follower) dt_r3.changeControlMode(CANTalon.ControlMode.Follower) dt_r4.changeControlMode(CANTalon.ControlMode.Follower) dt_l2.changeControlMode(CANTalon.ControlMode.Follower) dt_l3.changeControlMode(CANTalon.ControlMode.Follower) dt_l4.changeControlMode(CANTalon.ControlMode.Follower) dt_r2.set(1) dt_r3.set(1) dt_r4.set(1) dt_l2.set(7) dt_l3.set(7) dt_l4.set(7) dt = DriveTrain(dt_left, dt_right, left_encoder=None, right_encoder=None)
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 IntakeLauncher(Subsystem):
"""A class to manage/control all aspects of shooting boulders.
IntakeLauncher is comprised of:
aimer: to raise & lower the mechanism for ball intake and shooting
wheels: to suck or push the boulder
launcher: to push boulder into the wheels during shooting
limit switches: to detect if aimer is at an extreme position
potentiometer: to measure our current aim position
boulderSwitch: to detect if boulder is present
Aiming is controlled via two modes
1: driver/interactive: aimMotor runs in VBUS mode to change angle
2: auto/vision control: aimMotor runs in closed-loop position mode
to arrive at a designated position
"""
k_launchMin = 684.0 # manual calibration value
k_launchMinDegrees = -11 # ditto (vestigial)
k_launchMax = 1024.0 # manual calibration value
k_launchMaxDegrees = 45 # ditto (vestigial)
k_launchRange = k_launchMax - k_launchMin
k_launchRangeDegrees = k_launchMaxDegrees - k_launchMinDegrees
k_aimDegreesSlop = 2 # TODO: tune this
k_intakeSpeed = -0.60 # pct vbux
k_launchSpeedHigh = 1.0 # pct vbus
k_launchSpeedLow = 0.7
k_launchSpeedZero = 0
k_servoLeftLaunchPosition = 0.45 # servo units
k_servoRightLaunchPosition = 0.65
k_servoLeftNeutralPosition = 0.75
k_servoRightNeutralPosition = 0.4
k_aimMultiplier = 0.5
k_maxPotentiometerError = 5 # potentiometer units
# launcher positions are measured as a percent of the range
# of the potentiometer..
# intake: an angle approriate for intaking the boulder
# neutral: an angle appropriate for traversing the lowbar
# travel: an angle appropriate for traversing the rockwall, enableLimitSwitch
# highgoal threshold: above which we shoot harder
k_launchIntakeRatio = 0.0
k_launchTravelRatio = 0.51
k_launchNeutralRatio = 0.51
k_launchHighGoalThreshold = 0.69
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
self.launchMin = self.k_launchMin
self.launchMax = self.k_launchMax
self.launchRange = self.launchMax - self.launchMin
self.controlMode = None
self.visionState = robot.visionState
self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId)
self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeLeftMotor.reverseSensor(True)
self.intakeLeftMotor.setSafetyEnabled(False)
self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId)
self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.intakeRightMotor.setSafetyEnabled(False)
self.aimMotor = CANTalon(robot.map.k_IlAimMotorId)
# LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor)
if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot):
self.aimMotor.setSafetyEnabled(False)
self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot)
self.aimMotor.enableLimitSwitch(True, True)
self.aimMotor.enableBrakeMode(True)
self.aimMotor.setAllowableClosedLoopErr(5)
self.aimMotor.configPeakOutpuVoltage(12.0, -12.0)
self.aimMotor.setVoltageRampRate(150)
# TODO: setPID if needed
self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort)
self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort)
self.launcherServoRight = Servo(robot.map.k_IlServoRightPort)
def initDefaultCommand(self):
self.setDefaultCommand(ILCmds.DriverControlCommand(self.robot))
def updateDashboard(self):
pass
def beginDriverControl(self):
self.setControlMode("vbus")
def driverControl(self, deltaX, deltaY):
if self.robot.visionState.wantsControl():
self.trackVision()
else:
self.setControlMode("vbus")
self.aimMotor.set(deltaY * self.k_aimMultiplier)
def endDriverControl(self):
self.aimMotor.disableControl()
# ----------------------------------------------------------------------
def trackVision(self):
if not self.visionState.TargetAcquired:
return # hopefully someone is guiding the drivetrain to help acquire
if not self.visionState.LauncherLockedOnTarget:
if math.fabs(self.visionState.TargetY - self.getAngle()) < self.k_aimDegreesSlop:
self.visionState.LauncherLockedOnTarget = True
else:
self.setPosition(self.degreesToTicks(self.TargetY))
elif self.visionState.DriveLockedOnTarget:
# here we shoot and then leave AutoAimMode
pass
else:
# do nothing... waiting form driveTrain to reach orientation
pass
def setControlMode(self, mode):
if mode != self.controlMode:
self.controlMode = mode
if mode == "vbus":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.aimMotor.enableControl()
elif mode == "position":
self.aimMotor.disableControl()
self.aimMotor.changeControlMode(CANTalon.ControlMode.Position)
self.aimMotor.enableControl()
elif mode == "disabled":
self.aimMotor.disableControl()
else:
self.robot.error("ignoring controlmode " + mode)
self.controlMode = None
self.aimMotor.disableControl()
# launcher aim -------------------------------------------------------------
def getPosition(self):
return self.aimMotor.getPosition()
def getAngle(self):
pos = self.getPosition()
return self.ticksToDegress(pos)
def setPosition(self, pos):
self.setControlMode("position")
self.aimMotor.set(pos)
def isLauncherAtTop(self):
return self.aimMotor.isFwdLimitSwitchClosed()
def isLauncherAtBottom(self):
return self.aimMotor.isRevLimitSwitchClosed()
def isLauncherAtIntake(self):
if self.k_intakeRatio == 0.0:
return self.isLauncherAtBottom()
else:
return self.isLauncherNear(self.getIntakePosition())
def isLauncherAtNeutral(self):
return self.isLauncherNear(self.getNeutralPosition())
def isLauncherAtTravel(self):
return self.isLauncherNear(self.getTravelPosition())
def isLauncherNear(self, pos):
return math.fabs(self.getPosition() - pos) < self.k_maxPotentiometerError
def getIntakePosition(self):
return self.getLauncherPositionFromRatio(self.k_launchIntakeRatio)
def getNeutralPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchNeutralRatio)
def getTravelPosition(self):
return self.getLauncherPositionFromRatio(self.k_launchTravelRatio)
def getLauncherPositionFromRatio(self, ratio):
return self.launchMin + ratio * self.launchRange
def degressToTicks(self, deg):
ratio = (deg - self.k_launchMinDegrees) / self.k_launchRangeDegrees
return self.k_launchMin + ratio * self.k_launchRange
def ticksToDegrees(self, t):
ratio = (t - self.k_launchMin) / self.k_launchRange
return self.k_launchMinDegrees + ratio * self.k_launchRangeDegrees
def calibratePotentiometer(self):
if self.isLauncherAtBottom():
self.launchMin = self.getPosition()
elif self.isLauncherAtTop():
self.launchMax = self.getPosition()
self.launchRange = self.launchMax - self.launchMin
# intake wheels controls ---------------------------------------------------
def setWheelSpeedForLaunch(self):
if self.isLauncherAtTop():
speed = self.k_launchSpeedHigh
else:
speed = self.k_launchSpeedLow
self.setSpeed(speed)
def setWheelSpeedForIntake(self):
self.setSpeed(self.k_intakeSpeed)
def stopWheels(self):
self.setSpeed(k_launchSpeedZero)
def setWheelSpeed(self, speed):
self.intakeLeftMotor.set(speed)
self.intakeRightMotor.set(-speed)
# boulder and launcher servo controls --------------------------------------------------
def hasBoulder(self):
return self.boulderSwitch.get()
def activateLauncherServos(self):
self.robot.info("activateLauncherServos at:" + self.getAimPosition())
self.launcherServoLeft.set(self.k_servoLeftLaunchPosition)
self.launcherServoRight.set(self.k_servoRightLaunchPosition)
def retractLauncherServos(self):
self.launcherServoLeft.set(self.k_servoLeftNeutralPosition)
self.launcherServoRight.set(self.k_servoRightNeutralPosition)
class Portcullis(Subsystem):
def __init__(self, robot, name = None):
super().__init__(name = name)
self.robot = robot
self.leftMotor = CANTalon(robot.map.k_PcLeftMotorId)
self.leftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.leftMotor.enableLimitSwitch(True, True);
self.leftMotor.enableBrakeMode(True);
self.leftMotor.configPeakOutputVoltage(+12.0, -12.0);
self.leftMotor.setSafetyEnabled(False);
self.rightMotor = CANTalon(robot.map.k_PcRightMotorId)
self.rightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.rightMotor.enableLimitSwitch(True, True);
self.rightMotor.enableBrakeMode(True);
self.rightMotor.configPeakOutputVoltage(+12.0, -12.0);
self.rightMotor.setSafetyEnabled(False);
self.barMotor = CANTalon(robot.map.k_PcBarMotorId)
self.barMotor.changeControlMode(CANTalon.ControlMode.PercentVbus);
self.barMotor.enableBrakeMode(True);
self.barMotor.setSafetyEnabled(False);
# self.leftMotor.setForwardSoftLimit(PORTCULLIS_TOP);
# self.leftMotor.setReverseSoftLimit(PORTCULLIS_BOT);
# self.leftMotor.enableForwardSoftLimit(true);
# self.leftMotor.enableReverseSoftLimit(true);
def initDefaultCommand(self):
# currently we don't have a default command
pass
def updateDashboard(self):
SmartDashboard.putBoolean("Portcullis Upper Right Limit Switch",
self.rightAtTop())
SmartDashboard.putBoolean("Portcullis Upper Left Limit Switch",
self.leftAtTop())
SmartDashboard.putBoolean("Portcullis Lower Right Limit Switch",
self.rightAtBottom())
SmartDashboard.putBoolean("Portcullis Lower Left Limit Switch",
self.leftAtBottom())
def leftAtTop(self):
return self.leftMotor.isFwdLimitSwitchClosed()
def leftAtBottom(self):
return self.leftMotor.isRevLimitSwitchClosed()
def rightAtTop(self):
return self.rightMotor.isFwdLimitSwitchClosed();
def rightAtBottom(self):
return self.rightMotor.isRevLimitSwitchClosed();
def moveRightUp(self):
self.rightMotor.set(s_pctPowerUp)
if self.RightAtTop():
self.StopRight()
self.robot.log("Right Portcullis reached top")
def moveRightDown(self):
self.rightMotor.set(s_pctPowerDown)
if self.RightAtBottom():
self.StopRight()
self.robot.log("Right Portcullis reached bottom")
def stopRight(self):
self.rightMotor.set(0)
def moveLeftUp(self):
self.leftMotor.set(s_pctPowerUp)
if self.LeftAtTop():
self.StopLeft()
self.robot.log("Left Portcullis reached top")
def moveLeftDown(self):
self.leftMotor.set(s_pctPowerDown)
if self.LeftAtBottom():
self.StopLeft()
self.robot.log("Left Portcullis reached bottom")
def stopLeft(self):
self.leftMotor.set(0)
def barIn(self):
if self.LeftAtBottom() and self.RightAtBottom():
self.barMotor.set(s_barIn);
else:
self.StopBar()
def barOut(self):
self.barMotor.set(s_barOut)
def stopBar(self):
self.barMotor.set(0)
