def compute_speed(
self,
speed: float,
upper_switch: wpilib.DigitalInput,
lower_switch: wpilib.DigitalInput,
) -> float:
if not upper_switch.get() and speed > 0:
return speed
elif not lower_switch.get() and speed < 0:
return speed
else:
return 0
class ClimbSubsystem(Subsystem):
def __init__(self, robot):
super().__init__("Climb")
self.robot = robot
self.winchMotor = Spark(robotmap.CLIMBWINCH)
self.armUpSolenoid = Solenoid(robotmap.CLIMBARM_UP)
self.armDownSolenoid = Solenoid(robotmap.CLIMBARM_DOWN)
self.lockCloseSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_CLOSE)
self.lockOpenSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_OPEN)
self.hookLimit = DigitalInput(robotmap.LIMIT_SWITCH_HOOK)
def pullyclimb(self, speed):
self.winchMotor.set(speed)
def armExtend(self):
self.armUpSolenoid.set(True)
self.armDownSolenoid.set(False)
def armRetract(self):
self.armUpSolenoid.set(False)
self.armDownSolenoid.set(True)
def clampLock(self):
self.lockCloseSolenoid.set(True)
self.lockOpenSolenoid.set(False)
def clampOpen(self):
self.lockCloseSolenoid.set(False)
self.lockOpenSolenoid.set(True)
def isHookPressed(self):
return self.hookLimit.get() == False
class Intake(Component):
def __init__(self):
super().__init__()
self._l_motor = Talon(constants.motor_intake_l)
self._r_motor = Talon(constants.motor_intake_r)
self._intake_piston = Solenoid(constants.solenoid_intake)
self._photosensor = DigitalInput(constants.far_photosensor)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
def update(self):
self._l_motor.set(self._left_pwm)
self._r_motor.set(self._right_pwm)
self._intake_piston.set(not self._open)
def spin(self, power, same_direction=False):
self._left_pwm = power
self._right_pwm = power * (1 if same_direction else -1)
def intake_tote(self):
power = .3 if not self._photosensor.get() else .75
self.spin(power)
def intake_bin(self):
power = .3 if not self._photosensor.get() else .65
self.spin(power)
def open(self):
self._open = True
def close(self):
self._open = False
def stop(self):
"""Disables EVERYTHING. Only use in case of critical failure"""
self._l_motor.set(0)
self._r_motor.set(0)
class Conveyor(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Conveyor')
self.robot = robot
self.blaser = DigitalInput(3)
motors = {}
self.map = self.robot.botMap
for name in self.map.motorMap.motors:
motors[name] = self.robot.Creator.createMotor(
self.map.motorMap.motors[name])
self.cMotors = motors
for name in self.cMotors:
self.cMotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.cMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
def log(self):
wpilib.SmartDashboard.putNumber('Infared Value', self.blaser.get())
def set(self, pw):
self.cMotors['conveyor1'].set(ctre.ControlMode.PercentOutput, pw)
self.cMotors['conveyor2'].set(ctre.ControlMode.PercentOutput, pw)
def stay(self, pw):
self.cMotors['conveyor1'].set(ctre.ControlMode.PercentOutput, pw)
self.cMotors['conveyor2'].set(ctre.ControlMode.PercentOutput, -pw)
def getblaser(self):
y = self.blaser.get()
return y
class Switch(Sensor):
"""
Sensor wrapper for a switch.
Has boolean attribute pressed.
"""
pressed = False
def __init__(self, channel, module=1, reverse=False):
"""
Initializes the switch on some digital channel and module.
Normally assumes switches are active low.
"""
super().__init__()
self.s = DigitalInput(channel)
self.reverse = reverse
def poll(self):
self.pressed = not self.s.get() ^ self.reverse
class Switch(Sensor):
"""
Sensor wrapper for a switch.
Has boolean attribute pressed.
"""
pressed = False
def __init__(self, channel, module=1, reverse=False):
"""
Initializes the switch on some digital channel and module.
Normally assumes switches are active low.
"""
super().__init__()
self.s = DigitalInput(channel)
self.reverse = reverse
def poll(self):
self.pressed = not self.s.get() ^ self.reverse
class Arm(Subsystem):
def __init__(self, robot):
super().__init__("Arm")
self.robot = robot
self.peakCurrentLimit = 30
self.PeaKDuration = 50
self.continuousLimit = 15
motor = {}
for name in self.robot.RobotMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name])
self.motors = motor
for name in self.motors:
self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.motors[name].enableCurrentLimit(True)
self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X)
self.Zero = DigitalInput(6)
self.kp = 0.00035
self.ki = 0.00000000001
self.kd = 0.0000001
self.ArmPID = PID(self.kp, self.ki, self.kd)
def log(self):
wpilib.SmartDashboard.putNumber('armEnc', self.getHeight())
wpilib.SmartDashboard.putNumber('Zero', self.getZeroPos())
"""
Get Functions
"""
def getHeight(self):
# get encoder values
return self.AEnc.get()
def getZeroPos(self):
# get zero position of arm
return self.Zero.get()
"""
set functions
"""
def set(self, power):
self.motors['RTArm'].set(ctre.ControlMode.PercentOutput, power)
self.motors['LTArm'].set(ctre.ControlMode.PercentOutput, power)
def stop(self):
self.set(0)
def resetHeight(self):
self.AEnc.reset()
class Climber():
def initialize(self, robot):
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
if map.robotId == map.astroV1:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10, 0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backBottomSensor)
self.frontSwitch = DigitalInput(map.frontBottomSensor)
self.upSwitch = DigitalInput(map.upSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = 0 #degrees
self.climbSpeed = 0.5
self.wheelSpeed = 0.5
self.backHold = -0.15 #holds back stationary if extended ADJUST**
self.frontHold = -0.1 #holds front stationary if extended
self.kP = 0.4 #proportional gain for angle to power
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def periodic(self):
if self.xbox.getRawButton(map.liftClimber): self.started = True
deadband = 0.50
frontAxis = self.xbox.getRawAxis(map.liftFrontClimber)
backAxis = self.xbox.getRawAxis(map.liftBackClimber)
if abs(frontAxis) > deadband or abs(backAxis) > deadband:
if frontAxis > deadband: self.extend("front")
elif frontAxis < -deadband: self.retract("front")
if backAxis > deadband: self.extend("back")
elif backAxis < -deadband: self.retract("back")
else:
if self.xbox.getRawButton(map.lowerClimber) == True:
self.retract("both")
elif self.xbox.getRawButton(map.liftClimber) == True:
self.extend("both")
else:
if self.xbox.getRawButton(map.driveForwardClimber):
self.wheel("forward")
elif self.xbox.getRawButton(map.driveBackwardClimber):
self.wheel("backward")
else:
self.disable()
def up(self):
return not self.upSwitch.get()
def getLean(self):
if map.robotId == map.astroV1: return -1 * self.robot.drive.getRoll()
else: return self.robot.drive.getPitch()
def getCorrection(self):
return (self.kP * -self.getLean())
def setSpeeds(self, back, front):
self.backLift.set(back * self.climbSpeed)
self.frontLift.set(front * self.climbSpeed)
def retract(self, mode):
correction = self.getCorrection()
if mode == "front": self.setSpeeds(self.backHold, 1)
elif mode == "back": self.setSpeeds(1, 0)
elif mode == "both": self.setSpeeds(1 + correction, 1)
else: self.setSpeeds(0, 0)
def extend(self, mode):
correction = self.getCorrection()
if mode == "front": self.setSpeeds(correction, -1)
elif mode == "back": self.setSpeeds(-1, 0)
elif mode == "both": self.setSpeeds(-1 + correction, -1)
elif self.up() == True: self.setSpeeds(self.backHold, self.frontHold)
else: self.setSpeeds(0, 0)
def wheel(self, direction):
'''FORWARD MOVES ROBOT FORWARD, BACKWARD MOVES ROBOT BACKWARD'''
if direction == "forward":
self.wheelLeft.set(self.wheelSpeed)
self.wheelRight.set(self.wheelSpeed)
elif direction == "backward":
self.wheelLeft.set(-1 * self.wheelSpeed)
self.wheelRight.set(-1 * self.wheelSpeed)
correction = self.getCorrection()
self.setSpeeds(self.backHold + correction, 0)
def stopClimb(self):
self.setSpeeds(0, 0)
def stopDrive(self):
self.wheelLeft.set(0)
self.wheelRight.set(0)
def disable(self):
self.stopClimb()
self.stopDrive()
def dashboardInit(self):
SmartDashboard.putNumber("Climber kP", self.kP)
SmartDashboard.putNumber("ClimbSpeed", self.climbSpeed)
SmartDashboard.putNumber("BackHold", self.backHold)
SmartDashboard.putNumber("FrontHold", self.frontHold)
def dashboardPeriodic(self):
self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed",
self.climbSpeed)
self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
SmartDashboard.putNumber("Lean", self.getLean())
class ArmSubsystem(Subsystem):
def __init__(self):
self.encoderUnit = 4096
self.gearRatio = 93.33
self.armSpeedMultiplier = 1
self.armBottomLimit = 5
self.armUpperLimit = 200
self.resetValue = 0
self.bottomLimitSwitch = DigitalInput(ArmLimitSwitch)
self.armMotor1 = CANSparkMax(armBaseMotor1,MotorType.kBrushless)
self.armMotor2 = CANSparkMax(armBaseMotor2,MotorType.kBrushless)
self.armEncoder1 = self.armMotor1.getEncoder()
self.armEncoder2 = self.armMotor2.getEncoder()
self.currentArmPower = 0
self.isOverride = False
#def initDefaultCommand(self):
# self.setDefaultCommand(AnalogArmCommand())
def getDistanceTicks(self):
return ((self.armEncoder1.getPosition() + self.armEncoder2.getPosition()) / 2) - resetValue
def getArmMotor1Pos(self):
return self.armEncoder1.getPosition
def getArmMotor2Pos(self):
return self.armEncoder2.getPosition
def getRotationAngle(self):
return (getDistanceTicks()/108) * -360
def updateBottomLimit(self):
if not self.bottomLimitSwitch.get():
self.armBottomLimit = getRotationAngle()
def isArmAtBottom(self):
updateBottomLimit()
if (getRotationAngle() >= (armBottomLimit - 2)) and (getRotationAngle() <= (armBottomLimit + 2)):
return True
else:
return False
#def isArmAtTop(self):
#def getLimitSwitch(self):
def setArmPower(self,power):
if isArmAtBottom() and power > 0:
self.currentArmPower = 0
else:
self.currentArmPower = power
def setArmPowerOverride(self,power):
if isOverride:
self.currentArmPower = power
def updateOutputs(self):
self.armMotor1.set(currentArmPower * armSpeedMultiplier)
self.armMotor2.set(currentArmPower * armSpeedMultiplier)
def getGravityCompensation(self):
if getRotationAngle() <= 3: return 0
else: return math.sin(getRotationAngle() + 25)
class arm(Component):
#set up variables
def __init__(self, robot):
super().__init__()
self.robot = robot
self.armMotor = CANTalon(4)
self.wheelMotor = CANTalon(5)
self.frontSwitch = DigitalInput(8)
self.backSwitch = DigitalInput(9)
self.comp = Compressor()
self.comp.enabled()
self.armMotor.enableBrakeMode(True)
self.wheelMotor.enableBrakeMode(True)
self.potentiometer = AnalogPotentiometer(3, 270, -193)
#self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02)
self.position = 0
def armAuto(self, upValue, downValue, target, rate=0.3):
'''
if self.getPOT() <= target:
self.armMotor.set(0)
'''
if upValue == 1:
self.armMotor.set(rate * -1)
elif downValue == 1:
self.armMotor.set(rate)
else:
self.armMotor.set(0)
def armUpDown(self, left, right, controllerA, rate=0.3):
rate2 = rate*1.75
if(self.backSwitch.get() == False or self.frontSwitch.get() == False): #Checking limit switches
self.armMotor.set(0)
if(left >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(rate2)
# else:
self.armMotor.set(rate)
elif(right >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(-rate2)
# else:
self.armMotor.set(rate * -1)
elif(left < 0.75 and right < 0.75):
self.armMotor.set(0)
def wheelSpin(self, speed):
currentSpeed = 0
if (speed > 0.75):
currentSpeed = -1
elif(speed < -0.75):
currentSpeed = 1
self.wheelMotor.set(currentSpeed)
def getPOT(self):
return (self.potentiometer.get()*-1)
def getBackSwitch(self):
return self.backSwitch.get()
def getFrontSwitch(self):
return self.frontSwitch.get()
class Turret:
'''
The object thats responsible for managing the shooter
'''
def __init__(self):
self.clockwise_limit_switch = DigitalInput(
TURRET_CLOCKWISE_LIMIT_SWITCH)
self.counterclockwise_limit_switch = DigitalInput(
TURRET_COUNTERCLOCKWISE_LIMIT_SWITCH)
self.turn_motor = SparkMax(TURRET_TURN_MOTOR)
self.turn_pid = PIDController(0.4, 0.001, 0.02)
self.shoot_motor_1 = Falcon(TURRET_SHOOT_MOTORS[0])
self.shoot_motor_2 = Falcon(TURRET_SHOOT_MOTORS[1])
self.timer = Timer()
self.limelight = Limelight()
def set_target_angle(self, angle):
'''
Sets the target angle of the turret. This will use a PID to turn the
turret to the target angle.
'''
target_encoder = angle_to_encoder(angle)
self.turn_pid.setSetpoint(target_encoder)
def update(self):
'''
This is used to continuously update the turret's event loop.
All this manages as of now is the turrets PID controller.
The shoot motor is constantly running at a low percentage until we need it.
'''
motor_speed = self.turn_pid.calculate(
self.limelight.get_target_screen_x())
if self.clockwise_limit_switch.get() and motor_speed < 0:
self.turn_motor.set_percent_output(motor_speed)
elif self.counterclockwise_limit_switch.get() and motor_speed > 0:
self.turn_motor.set_percent_output(motor_speed)
def shoot(self):
'''
The wheel to shoot will rev up completely before balls start feeding
in from the singulator.
'''
# One of the motors will be reversed, so make sure the shoot motor has the correct ID!
speed = self.shoot_motor_1.get_percent_output()
if speed < 1:
speed += 0.02
elif speed > 1:
speed -= 0.02
self.shoot_motor_1.set_percent_output(speed)
self.shoot_motor_2.set_percent_output(-speed)
def idle(self):
'''
Resets the motors back to their default state.
'''
speed = self.shoot_motor_1.get_percent_output()
if speed < 0.5:
speed += 0.02
elif speed > 0.5:
speed -= 0.02
self.shoot_motor_1.set_percent_output(speed)
self.shoot_motor_2.set_percent_output(-speed)
def is_full_speed(self):
'''
Returns if the motor is at full speed or not.
'''
self.timer.get() > 0.4
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 CargoMech():
def initialize(self):
timeout = 15
self.wristPowerSet = 0
SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet)
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
self.xbox = oi.getJoystick(2)
self.out = 0
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
self.bottomSwitch = DI(map.bottomSwitch)
self.topSwitch = DI(map.topSwitch)
def setIntake(self, mode):
#Intake/Outtake/Stop, based on the mode it changes the speed of the motor
if mode == "intake": self.intake.set(0.9)
elif mode == "outtake": self.intake.set(-0.9)
else: self.intake.set(0)
def setWrist(self, mode):
if mode == "up":
self.out = self.wristUpVolts / self.maxVolts
elif mode == "down":
self.out = self.wristDownVolts / self.maxVolts
else:
self.out = 0
self.setWristPower(self.out)
def setWristPower(self, power):
self.wrist.set(power)
def periodic(self):
if self.xbox.getRawAxis(map.intakeCargo) > 0.4:
self.setIntake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4:
self.setIntake("outtake")
else:
self.setIntake("stop")
if self.xbox.getRawButton(map.wristUp) and not self.topSwitch.get():
self.setWrist("up")
elif self.xbox.getRawButton(
map.wristDown) and not self.bottomSwitch.get():
self.setWrist("down")
else:
self.setWrist("stop")
#disables intake
def disable(self):
self.setIntake("stop")
self.setWrist("stop")
def dashboardInit(self):
pass
def dashboardPeriodic(self):
SmartDashboard.putNumber("Cargomech Wrist Output", self.out)
self.wristPowerSet = SmartDashboard.getNumber("Wrist Power Set", 0)
class Lift:
def __init__(self, Robot):
self._intake = Robot.intake
self._drive = Robot.drive
self._ramp = 0
self._rampSpeed = 6
self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX)
self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX)
self._LiftMotorLeft.setInverted(True)
self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False,
Encoder.EncodingType.k4X)
self._liftEncoder.setDistancePerPulse(1)
self._liftHallaffect = DigitalInput(HALL_DIO)
self.zeroEncoder()
self._liftPID = MiniPID(*LIFT_PID)
self._liftPID.setOutputLimits(-0.45, 1)
self.disableSpeedLimit = False
def setSpeed(self, speed):
self._LiftMotorLeft.set(speed)
self._LiftMotorRight.set(speed)
def rampSpeed(self, speed):
self._ramp += (speed - self._ramp) / self._rampSpeed
if False and speed > 0: #is max limit hit
self._ramp = 0
self._LiftMotorLeft.set(self._ramp)
self._LiftMotorRight.set(self._ramp)
def setLoc(self, target):
target = abs(target)
if target > 1:
target = 1
elif target <= 0.1:
target = 0
SmartDashboard.putNumber("loc dfliusafusd", target)
self._liftPID.setSetpoint(target)
def periodic(self):
if self.isBottom(): #zero switch is active zero encoder
self.zeroEncoder()
else:
if self._intake.getSpeed() >= 0:
self._intake.rampSpeed(0.3)
scaledLift = self.getEncoderVal() / LIFT_HEIGHT
speed = self._liftPID.getOutput(scaledLift)
if not self.disableSpeedLimit:
speedLimit = pow(0.25, scaledLift)
self._drive.setSpeedLimit(speedLimit)
else:
self._drive.setSpeedLimit(1)
self.rampSpeed(speed)
def getEncoderVal(self):
return abs(self._liftEncoder.getDistance())
def zeroEncoder(self):
self._liftEncoder.reset()
def isBottom(self):
return not self._liftHallaffect.get()
class Climber():
def initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def periodic(self):
state = -1
if self.xbox.getRawButton(map.liftClimber): self.started = True
deadband = 0.50
frontAxis = self.xbox.getRawAxis(map.liftFrontClimber)
backAxis = self.xbox.getRawAxis(map.liftBackClimber)
if abs(frontAxis) > deadband or abs(backAxis) > deadband:
if self.state != -1:
self.state = -1
self.disable()
else:
self.stopDrive()
if frontAxis > deadband: self.extend("front")
elif frontAxis < -deadband: self.retract("front")
if backAxis > deadband:
self.extend("back")
elif backAxis < -deadband:
self.retract("back")
return
else:
if self.xbox.getRawButton(map.lowerClimber) == True:
self.retract("both")
return
elif self.xbox.getRawButton(map.liftClimber) == True:
self.extend("both")
return
else:
if self.xbox.getRawButton(map.driveForwardClimber):
self.wheel("forward")
return
elif self.xbox.getRawButton(map.driveBackwardClimber):
self.wheel("backward")
return
else:
if state == -1:
self.extend("hold")
self.stopDrive()
else:
pass
if self.xbox.getRawButton(map.resetAutoClimb):
if self.state == -1:
self.startClimbAuto()
else:
print("already running auto climb")
#elif self.xbox.getRawButton(map.stopAutoClimb):
#self.stopClimbAuto()
else:
state = self.getState()
#if state == 0: self.extend("both")
if state == 1:
now = wpilib.Timer.getFPGATimestamp()
if (now - self.wheelsStart) >= 2:
self.autoClimbIndicator = True
self.stopDrive()
else:
self.wheel("forward")
self.stopClimb()
elif state == 2:
now = wpilib.Timer.getFPGATimestamp()
self.autoClimbIndicator = True
if (now - self.frontRetractStart) >= 3:
self.extend("hold")
if self.isFrontOverGround():
self.wheel('backward', speed=0.4)
else:
self.stopDrive()
else:
self.retract("front")
self.stopDrive()
elif state == 3:
now = wpilib.Timer.getFPGATimestamp()
self.autoClimbIndicator = True
if (now - self.wheelsStart2) >= 2:
self.stopDrive()
else:
self.wheel("forward")
self.stopClimb()
self.wheel("forward")
self.stopClimb()
elif state == 4:
self.retract("back")
self.stopDrive()
elif state == -1:
pass
def frontUp(self):
return not self.frontSwitch.get()
def backUp(self):
return not self.backSwitch.get()
def getLean(self):
if map.robotId == map.astroV1: return self.robot.drive.getRoll()
else: return self.robot.drive.getPitch()
def getCorrection(self):
return (self.kP * -self.getLean())
def setSpeeds(self, back, front):
if self.usingNeo:
self.backLift.set(back * self.climbSpeed)
self.frontLift.set(front * self.climbSpeed)
else:
self.backLift.set(back * self.climbSpeed)
self.frontLift.set(front * self.climbSpeed)
def retract(self, mode):
correction = self.getCorrection()
if mode == "front": self.setSpeeds(self.backHold, 1)
elif mode == "back": self.setSpeeds(0.7, 0)
elif mode == "both": self.setSpeeds(1 + correction, 1)
else: self.setSpeeds(0, 0)
def extend(self, mode):
correction = self.getCorrection()
if mode == "front": self.setSpeeds(correction, -1)
elif mode == "back": self.setSpeeds(-1, 0)
elif mode == "both": self.setSpeeds(-1 + correction, -1)
elif not self.isBackOverGround(
) and not self.isFrontOverGround() and not self.isFullyRetractedFront(
) and not self.isFullyRetractedBack():
self.setSpeeds(self.backHold, self.frontHold)
print('holding both')
elif not self.isFrontOverGround() and not self.isFullyRetractedFront():
self.setSpeeds(0, self.frontHold)
print('holding front')
elif not self.isBackOverGround() and not self.isFullyRetractedBack():
self.setSpeeds(self.backHold, 0)
print('holding back')
else:
self.setSpeeds(0, 0)
print('holding none')
def wheel(self, direction, speed=0):
if (speed == 0): speed = self.wheelSpeed
'''FORWARD MOVES ROBOT FORWARD, BACKWARD MOVES ROBOT BACKWARD'''
if direction == "forward":
self.wheelLeft.set(speed)
self.wheelRight.set(speed)
elif direction == "backward":
self.wheelLeft.set(-1 * speed)
self.wheelRight.set(-1 * speed)
def startClimbAuto(self):
self.state = 1
self.wheelsStart = wpilib.Timer.getFPGATimestamp()
def stopClimbAuto(self):
self.state = -1
def getState(self):
'''
state 0 is robot elevating itself until top sensors trigger
state 1 is robot driving forward until front sensor triggers
state 2 is robot lifting front leg until bottom front sensor triggers
state 3 is robot driving forward until back sensor triggers
state 4 is robot lifting back leg until bottom back sensor triggers
state 5 is robot driving forward until drivers disable method
'''
'''checking any illogical scenarios, if they occur end autoclimb'''
if self.state == 1 and (not self.isFullyExtendedBoth()):
print("STATE 1 Error")
self.state = -1
if self.state == 2 and (not self.isFullyExtendedBack()):
print("STATE 2 Error")
self.state = -1
if self.state == 3 and (not self.isFullyExtendedBack()
or not self.isFullyRetractedFront()):
print("STATE 3 Error")
self.state = -1
if self.state == 4 and (not self.isFullyRetractedFront()):
print("STATE 4 Error")
self.state = -1
if self.state == 5 and (not self.isFullyRetractedBack()):
print("STATE 5 Error")
self.state = -1
'''checking milestones to transition to next steps'''
if self.state == 0 and self.isFullyExtendedBoth(
) and not self.isFrontOverGround() and not self.isBackOverGround():
print("Transition to State 1")
self.state = 1
if self.state == 1 and self.isFrontOverGround():
print("Transition to State 2")
self.state = 2
self.frontRetractStart = wpilib.Timer.getFPGATimestamp()
if self.state == 2 and self.isFullyRetractedFront():
print("Transition to State 3")
self.state = 3
self.wheelsStart2 = wpilib.Timer.getFPGATimestamp()
if self.state == 3 and self.isBackOverGround():
print("Transition to State 4")
self.state = 4
if self.state == 4 and self.isFullyRetractedBack():
print("Transition to State 5")
self.state = 5
return self.state
def stopClimb(self):
self.setSpeeds(0, 0)
def stopDrive(self):
self.wheelLeft.set(0)
self.wheelRight.set(0)
def disable(self):
self.stopClimb()
self.stopDrive()
self.state = -1
def isFullyExtendedFront(self):
""" tells us if the front is fully extended """
#return not self.switchTopFront.get()
'''sensors were removed so it will always return true'''
return True
def isFullyExtendedBack(self):
""" tells us if the back is fully extended, so it can stop """
#return not self.switchTopBack.get()
'''returns true, sensor was removed'''
return True
def isFullyRetractedFront(self):
return not self.switchBottomFront.get()
def isFullyRetractedBack(self):
return not self.switchBottomBack.get()
def isFrontOverGround(self):
return not self.frontSwitch.get()
def isBackOverGround(self):
return not self.backSwitch.get()
def isFullyExtendedBoth(self):
return (self.isFullyExtendedBack() and self.isFullyExtendedFront())
def isFullyRetractedBoth(self):
return (self.isFullyRetractedBack() and self.isFullyRetractedFront())
def dashboardInit(self):
SmartDashboard.putNumber("Climber kP", self.kP)
SmartDashboard.putNumber("ClimbSpeed", self.climbSpeed)
SmartDashboard.putNumber("BackHold", self.backHold)
SmartDashboard.putNumber("FrontHold", self.frontHold)
def dashboardPeriodic(self):
SmartDashboard.putBoolean("Fully Extended Front",
self.isFullyExtendedFront())
SmartDashboard.putBoolean("Fully Extended Back",
self.isFullyExtendedBack())
SmartDashboard.putBoolean("Fully Retracted Front",
self.isFullyRetractedFront())
SmartDashboard.putBoolean("Fully Retracted Back",
self.isFullyRetractedBack())
SmartDashboard.putBoolean("Front Over Ground",
self.isFrontOverGround())
SmartDashboard.putBoolean("Back Over Ground", self.isBackOverGround())
SmartDashboard.putNumber("self.state", self.state)
self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed",
self.climbSpeed)
self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
SmartDashboard.putNumber("Lean", self.getLean())
SmartDashboard.putNumber("FloorSensor", self.backUp())
SmartDashboard.putBoolean("Auto Climb Indicator",
self.autoClimbIndicator)
SmartDashboard.putNumber("Retract Start", self.frontRetractStart)
SmartDashboard.putNumber("Wheels Start", self.wheelsStart2)
class LifterComponent(Events):
class EVENTS:
on_control_move = "on_control_move"
on_manual_move = "on_manual move"
# RPM Multipliers
# ELEVATOR_MULTIPLIER = 1635.15 / 6317.67
ELEVATOR_MULTIPLIER = 2102.35 / 3631.33
CARRIAGE_MULTIPLIER = 1.0 - ELEVATOR_MULTIPLIER
# Max heights of each stage.
ELEVATOR_MAX_HEIGHT = 40
CARRIAGE_MAX_HEIGHT = 40
# Conversion factors (counts to inches)
# CHANGE THESE VALUES
ELEVATOR_CONV_FACTOR = 0.00134
CARRIAGE_CONV_FACTOR = 0.000977
el_down = -1
el_up = 1
carriage_down = -1
carriage_up = 1
MAX_SPEED = 0.5
ELEVATOR_ZERO = 0.0
CARRIAGE_ZERO = 0.0
TIMEOUT_MS = 0
ELEVATOR_kF = 0.0
ELEVATOR_kP = 0.1
ELEVATOR_kI = 0.0
ELEVATOR_kD = 0.0
# ALLOWABLE_ERROR = 2
CARRIAGE_ALLOWABLE_ERROR = int(2 / CARRIAGE_CONV_FACTOR)
ELEVATOR_ALLOWABLE_ERROR = int(2 / ELEVATOR_CONV_FACTOR)
# positions = {
# "floor": 2.0,
# "portal": 34.0,
# "scale_low": 48.0,
# "scale_mid": 60.0,
# "scale_high": 72.0,
# "max_height": 84.0
# }
positions = {
"floor": 0.5,
"portal": 34.0,
"scale_low": 52.0,
"scale_mid": 68.0,
"scale_high": 78.0
}
def __init__(self):
Events.__init__(self)
self.elevator_motor = WPI_TalonSRX(5)
self.elevator_bottom_switch = DigitalInput(9)
self.carriage_motor = WPI_TalonSRX(3)
self.carriage_bottom_switch = DigitalInput(1)
self.carriage_top_switch = DigitalInput(2)
self._create_events([
LifterComponent.EVENTS.on_control_move,
LifterComponent.EVENTS.on_manual_move
])
self._is_reset = False
# configure elevator motor and encoder
self.elevator_motor.setNeutralMode(NeutralMode.Brake)
self.elevator_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSensorPhase(True)
self.elevator_motor.setInverted(True)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configAllowableClosedloopError(
0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configForwardSoftLimitThreshold(
int(LifterComponent.ELEVATOR_MAX_HEIGHT /
LifterComponent.ELEVATOR_CONV_FACTOR), 0)
self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.elevator_motor.setCurrentLimit()
# self.elevator_motor.EnableCurrentLimit()
# self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
# configure carriage motor and encoder
self.carriage_motor.setNeutralMode(NeutralMode.Brake)
self.carriage_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.setSensorPhase(True)
self.carriage_motor.setInverted(True)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configAllowableClosedloopError(
0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configForwardSoftLimitThreshold(
int(LifterComponent.CARRIAGE_MAX_HEIGHT /
LifterComponent.CARRIAGE_CONV_FACTOR), 0)
self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.carriage_motor.setCurrentLimit()
# self.carriage_motor.EnableCurrentLimit()
# self.carriage_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
def set_elevator_speed(self, speed):
if (speed > 0 and self.current_elevator_position >= LifterComponent.ELEVATOR_MAX_HEIGHT - 2) \
or (speed < 0 and self.elevator_bottom_switch.get()):
self.elevator_motor.set(0)
else:
self.elevator_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def set_carriage_speed(self, speed):
if (speed > 0 and self.carriage_top_switch.get()) \
or (speed < 0 and self.carriage_bottom_switch.get()):
self.carriage_motor.set(0)
else:
self.carriage_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def reset_sensors(self):
self.carriage_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self._is_reset = True
@property
def current_elevator_position(self) -> float:
return self.elevator_motor.getSelectedSensorPosition(
0) * LifterComponent.ELEVATOR_CONV_FACTOR
@property
def current_carriage_position(self) -> float:
return self.carriage_motor.getSelectedSensorPosition(
0) * LifterComponent.CARRIAGE_CONV_FACTOR
@property
def current_position(self) -> float:
return self.current_elevator_position + self.current_carriage_position
def stop_lift(self):
self.elevator_motor.stopMotor()
self.carriage_motor.stopMotor()
def elevator_to_target_position(self, inches: float):
if self._is_reset:
self.elevator_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.ELEVATOR_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def carriage_to_target_position(self, inches: float):
if self._is_reset:
self.carriage_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.CARRIAGE_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def lift_to_distance(self, inches):
i = inches + 6
elevator = min(i * LifterComponent.ELEVATOR_MULTIPLIER,
LifterComponent.ELEVATOR_MAX_HEIGHT)
carriage = i - elevator
print("lift_to_distance carriage" + str(carriage))
print("lift_to_distance elevate" + str(elevator))
print("lift_to_distance lifter" + str(carriage + elevator))
self.elevator_to_target_position(elevator)
self.carriage_to_target_position(carriage)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def is_at_position(self, position: str) -> bool:
return self.is_at_distance(LifterComponent.positions[position])
def is_at_distance(self, inches: float) -> bool:
return inches - 5 < self.current_position < inches + 5
def closest_position(self) -> tuple:
# returns the key for the position closest to the current position
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
return min(
positions,
key=lambda position: abs(self.current_position - position[1]))
def next_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == len(positions) - 1:
return position[0]
return positions[index + 1][0]
def prev_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == 0:
return position[0]
return positions[index - 1][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)
class Shooter(Subsystem):
# ----------------- INITIALIZATION -----------------------
def __init__(self, robot):
super().__init__("shooter")
self.robot = robot
self.counter = 0 # used for updating the log
self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional
# motor controllers
self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.spark_hood = Talon(5)
self.spark_feed = Talon(7)
# encoders and PID controllers
self.hood_encoder = Encoder(
4,
5) # generic encoder - we'll have to install one on the 775 motor
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.hood_controller = wpilib.controller.PIDController(Kp=0.005,
Ki=0,
Kd=0.0)
self.hood_setpoint = 5
self.hood_controller.setSetpoint(self.hood_setpoint)
self.flywheel_encoder = self.sparkmax_flywheel.getEncoder(
) # built-in to the sparkmax/neo
self.flywheel_controller = self.sparkmax_flywheel.getPIDController(
) # built-in PID controller in the sparkmax
# limit switches, use is TBD
self.limit_low = DigitalInput(6)
self.limit_high = DigitalInput(7)
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
#self.setDefaultCommand(ShooterHoodAxis(self.robot))
def set_flywheel(self, velocity):
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0, self.feed_forward)
self.flywheel_controller.setReference(velocity,
rev.ControlType.kSmartVelocity,
0)
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0)
def stop_flywheel(self):
self.flywheel_controller.setReference(0, rev.ControlType.kVoltage)
def set_feed_motor(self, speed):
self.spark_feed.set(speed)
def stop_feed_motor(self):
self.spark_feed.set(0)
def set_hood_motor(self, power):
power_limit = 0.2
if power > power_limit:
new_power = power_limit
elif power < -power_limit:
new_power = -power_limit
else:
new_power = power
self.spark_hood.set(new_power)
def change_elevation(
self, power
): # open loop approach - note they were wired to be false when contacted
if power > 0 and self.limit_high.get():
self.set_hood_motor(power)
elif power < 0 and self.limit_low.get():
self.set_hood_motor(power)
else:
self.set_hood_motor(0)
def set_hood_setpoint(self, setpoint):
self.hood_controller.setSetpoint(setpoint)
def get_angle(self):
elevation_minimum = 30 # degrees
conversion_factor = 1 # encoder units to degrees
# return elevation_minimum + conversion_factor * self.hood_encoder.getDistance()
return self.hood_encoder.getDistance()
def periodic(self) -> None:
"""Perform necessary periodic updates"""
self.counter += 1
if self.counter % 5 == 0:
# pass
# ten per second updates
SmartDashboard.putNumber('elevation',
self.hood_encoder.getDistance())
SmartDashboard.putNumber('rpm',
self.flywheel_encoder.getVelocity())
watch_axis = False
if watch_axis:
self.hood_scale = 0.2
self.hood_offset = 0.0
power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) -
0.5) + self.hood_offset
self.robot.shooter.change_elevation(power)
maintain_elevation = True
if maintain_elevation:
self.error = self.get_angle() - self.hood_setpoint
pid_out = self.hood_controller.calculate(self.error)
output = 0.03 + pid_out
SmartDashboard.putNumber('El PID', pid_out)
self.change_elevation(output)
if self.counter % 50 == 0:
SmartDashboard.putBoolean("hood_low", self.limit_low.get())
SmartDashboard.putBoolean("hood_high", self.limit_high.get())
class Elevator(Component):
ON_TARGET_DELTA = 1 / 4
def __init__(self):
super().__init__()
self._motor = SyncGroup(Talon, constants.motor_elevator)
self._position_encoder = Encoder(*constants.encoder_elevator)
self._photosensor = DigitalInput(constants.photosensor)
self._stabilizer_piston = Solenoid(constants.solenoid_dropper)
self._position_encoder.setDistancePerPulse(
(PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION)
# Trajectory controlling stuff
self._follower = TrajectoryFollower()
self.assure_tote = CircularBuffer(5)
self.auto_stacking = True # Do the dew
self._tote_count = 0 # Keep track of totes!
self._has_bin = False # Do we have a bin on top?
self._new_stack = True # starting a new stack?
self._tote_first = False # Override bin first to grab totes before anything else
self._should_drop = False # Are we currently trying to get a bin ?
self._close_stabilizer = True # Opens the stabilizer manually
self.force_stack = False # manually actuates the elevator down and up
self._follower.set_goal(Setpoints.BIN) # Base state
self._follower_thread = Thread(target=self.update_follower)
self._follower_thread.start()
self._auton = False
quickdebug.add_tunables(Setpoints,
["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"])
quickdebug.add_printables(
self,
[('position', self._position_encoder.getDistance),
('photosensor', self._photosensor.get), "has_bin", "tote_count",
"tote_first", "at_goal", "has_game_piece", "auto_stacking"])
def stop(self):
self._motor.set(0)
def update(self):
goal = self._follower.get_goal()
if self.at_goal:
self.assure_tote.append(self.has_game_piece)
if self._should_drop: # Overrides everything else
if self.tote_count < 5 and not self.has_game_piece:
self._follower._max_acc = 100 # Slow down on drop
self._follower.set_goal(Setpoints.DROP)
self._close_stabilizer = False
self._new_stack = True
else:
self._follower._max_acc = 210 # Normal speed
if self._new_stack:
self._new_stack = False
self._close_stabilizer = True
self._tote_count = 0
self._has_bin = False
if goal == Setpoints.STACK: # If we've just gone down to grab something
if self.tote_count == 0 and not self.has_bin and not self.tote_first:
self._has_bin = True # Count the bin
else: # We were waiting for a tote
self.add_tote()
self._follower.set_goal(Setpoints.AUTON if self._auton else
Setpoints.TOTE) # Go back up
if self._tote_count >= 2:
self._close_stabilizer = True
elif (
self.tote_in_long_enough() and self.auto_stacking
) and self.tote_count < 5: # If we try to stack a 6th tote it'll break the robot
if not self.has_bin:
# Without a bin, only stack in these situations:
# - We're picking up a tote, and this is the first tote.
# - We're forcing the elevator to stack (for the bin, usually)
# - We have more than one tote already, so we can pick up more.
if self.tote_first or self.force_stack or self.tote_count > 0:
self._follower.set_goal(Setpoints.STACK)
else: # We have a bin, just auto-stack.
self._follower.set_goal(Setpoints.STACK)
if self.has_bin: # Transfer!
if self._tote_count == 1:
self._close_stabilizer = False
else: # Wait for a game piece & raise the elevator
if self._auton:
self._follower.set_goal(Setpoints.AUTON)
elif self._tote_count == 0 and not self.has_bin:
if self.tote_first:
self._follower.set_goal(Setpoints.FIRST_TOTE)
else:
self._follower.set_goal(Setpoints.BIN)
else:
self._follower.set_goal(Setpoints.TOTE)
self._motor.set(self._follower.output)
self._stabilizer_piston.set(self._close_stabilizer)
self._should_drop = False
self._tote_first = False
self._auton = False
def reset_encoder(self):
self._position_encoder.reset()
self._follower.set_goal(0)
self._follower._reset = True
@property
def has_game_piece(self):
return not self._photosensor.get() or self.force_stack
@property
def position(self):
return self._position_encoder.getDistance()
@property
def at_goal(self):
return self._follower.trajectory_finished(
) # and abs(self._follower.get_goal() - self.position) < 2
def drop_stack(self):
self._should_drop = True
def stack_tote_first(self):
self._tote_first = True
def full(self):
return self._tote_count == 5 and self.has_game_piece
@property
def has_bin(self):
return self._has_bin
@property
def tote_first(self):
return self._tote_first
@property
def tote_count(self):
return self._tote_count
def add_tote(self, number=1):
self._tote_count = max(0, min(5, self._tote_count + number))
def remove_tote(self):
self.add_tote(-1)
def set_bin(self, bin_):
self._has_bin = bin_
def auton(self):
self._auton = True
self._tote_first = True
def tote_in_long_enough(self):
return all(self.assure_tote)
def update_follower(self):
while True:
self._follower.calculate(self.position)
time.sleep(0.005)
