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.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
dt_right = CANTalon(1)
class driveTrain(Component) :
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
# Adding components to the LiveWindow (testing)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor)
'''
def log(self):
#The log method puts interesting information to the SmartDashboard. (like velocity information)
'''
#no longer implemented because of change of hardware
wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity())
'''
wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition())
wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition())
wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition())
wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition())
'''
wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57))
'''
# drive forward function
def drive_forward(self, speed) :
self.drive.tankDrive(speed, speed, True)
# manual drive function for Tank Drive
def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT):
#self.lfmotor.setCloseLoopRampRate(1)
#self.lbmotor.setCloseLoopRampRate(1)
#self.rfmotor.setCloseLoopRampRate(1)
#self.rbmotor.setCloseLoopRampRate(1)
if (leftB == True): #Straight Button
rightSpeed = leftSpeed
if (rightB == True): #Slow Button
#leftSpeed = leftSpeed/1.75
#rightSpeed = rightSpeed/1.75
if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning
leftSpeed = leftSpeed/1.75
rightSpeed = rightSpeed/1.75
# Fast button
if(rightT == True):
#self.lfmotor.setCloseLoopRampRate(24)
#self.lbmotor.setCloseLoopRampRate(24)
#self.rfmotor.setCloseLoopRampRate(24)
#self.rbmotor.setCloseLoopRampRate(24)
leftSpeed = leftSpeed*(1.75)
rightSpeed = rightSpeed*(1.75)
if(leftT == True):
leftSpeed = 0.1
rightSpeed = 0.1
# Creating margin for error when using the joysticks, as they're quite sensitive
if abs(rightSpeed) < 0.04 :
rightSpeed = 0
if abs(leftSpeed) < 0.04 :
leftSpeed = 0
if self.CONTROL_TYPE:
self.pidRightFront.setSetpoint(rightSpeed)
self.pidRightBack.setSetpoint(rightSpeed)
self.pidLeftFront.setSetpoint(leftSpeed)
self.pidLeftBack.setSetpoint(leftSpeed)
else:
self.lfmotor.set(leftSpeed*512)
self.rfmotor.set(rightSpeed*512)
self.lbmotor.set(leftSpeed*512)
self.rbmotor.set(rightSpeed*512)
#autononmous tank drive (to remove a need for a slow, striaght, or fast button)
def autonTankDrive(self, leftSpeed, rightSpeed):
self.log()
#self.drive.tankDrive(leftSpeed, rightSpeed, True)
self.rfmotor.set(rightSpeed)
self.rbmotor.set(rightSpeed*(-1))
self.lfmotor.set(leftSpeed)
self.lbmotor.set(leftSpeed*(-1))
# stop function
def drive_stop(self) :
self.drive.tankDrive(0,0)
# fucntion to reset the PID's and encoder values
def reset(self):
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
if self.CONTROL_TYPE:
self.LEFTFRONTCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE= 0
self.RIGHTBACKCUMULATIVE = 0
self.pidLeftBack.setSetpoint(0)
self.pidLeftFront.setSetpoint(0)
self.pidRightBack.setSetpoint(0)
self.pidRightFront.setSetpoint(0)
# def getDistance(self)
# return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE)))
def turn_angle(self, degrees):
desired_inches = self.INCHES_PER_DEGREE * degrees
if degrees < 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(0.4, -0.4)
elif degrees > 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(-0.4, 0.4)
# Enable PID Controllers
def enablePIDs(self):
'''
#No longer required because we swapped from analog encoders to magnetic encoders
self.pidLeftFront.enable()
self.pidLeftBack.enable()
self.pidRightFront.enable()
self.pidRightBack.enable()
'''
# Disable PID Controllers
def disablePIDs(self):
'''
#see explaination above
self.pidLeftFront.disable()
self.pidLeftBack.disable()
self.pidRightFront.disable()
self.pidRightBack.disable()
'''
def getAutonDistance(self):
return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4
#detirmines how many ticks the encoder has processed
def getMotorDistance(self, motor, cumulativeDistance):
currentRollovers = 0 #number of times the encoder has gone from 1023 to 0
previousValue = cumulativeDistance #variable for comparison
currentValue = motor.getEncPosition() #variable for comparison
if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0
currentRollovers += 1 #notes the rollover
return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks
#converts ticks from getMotorDistance into inches
def convertEncoderRaw(self, selectedEncoderValue):
return selectedEncoderValue * self.INCHES_PER_REV
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)
class Shooter:
left_fly = CANTalon
right_fly = CANTalon
intake_main = CANTalon
intake_mecanum = Talon
ball_limit = DigitalInput
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def warm_up(self):
self.set_rpm(2500) # Warm up flywheels to get ready to shoot
def low_goal(self):
self.set_rpm(500)
def set_rpm(self, rpm_l_set, rpm_r_set=None):
if rpm_r_set is None:
rpm_r_set = rpm_l_set
self.left_fly.set(rpm_l_set)
self.right_fly.set(rpm_r_set)
def get_rpms(self):
return self.left_fly.get(), self.right_fly.get()
def set_fly_off(self):
self.set_rpm(0)
def set_intake(self, power):
self.intake_mecanum.set(-power)
self.intake_main.set(power)
def get_intake(self):
return self.intake_main.get()
def set_intake_off(self):
self.set_intake(0)
def get_ball_limit(self):
return not bool(self.ball_limit.get())
def execute(self):
if int(self.left_fly.getOutputCurrent()) > 20 \
or int(self.left_fly.getOutputCurrent()) > 20:
self.set_rpm(0, 0)
