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