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 DriveTrain(Subsystem): """DriveTrain: is the subsystem responsible for motors and devices associated with driving subystem. As a subsystem, we represent the single point of contact for all drivetrain-related controls. Specifically, commands that manipulate the drivetrain should 'require' the singleton instance (via require(robot.driveTrain)). Unless overridden, our default command, JoystickDriver, is the means by which driving occurs. """ k_minThrottleScale = 0.5 k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally k_maxDriveSpeed = 150.0 # ~20 ft/sec k_maxTurnSpeed = 40.0 # ~3-4 ft/sec k_fastTurn = -1 k_mediumTurn = -.72 k_slowTurn = -.55 k_quadTicksPerWheelRev = 9830 k_wheelDiameterInInches = 14.0 k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches k_turnKp = .1 k_turnKi = 0 k_turnKd = .3 k_turnKf = .001 k_ControlModeSpeed = 0 k_ControlModeVBus = 1 k_ControlModeDisabled = 2 class _turnHelper(PIDOutput): """a private helper class for PIDController-based imu-guided turning. """ def __init__(self, driveTrain): super().__init__() self.driveTrain = driveTrain def pidWrite(self, output): self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed) def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot # STEP 1: instantiate the motor controllers self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId) self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId) self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId) self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId) # Step 2: Configure the follower Talons: left & right back motors self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID()) self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID()) # STEP 3: Setup speed control mode for the master Talons self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) # STEP 4: Indicate the feedback device used for closed-loop # For speed mode, indicate the ticks per revolution self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) # STEP 5: Set PID values & closed loop error self.leftMasterMotor.setPID(0.22, 0, 0) self.rightMasterMotor.setPID(0.22, 0, 0) # Add ramp up rate self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage # change /sec: reach to # 12V after 1sec self.rightMasterMotor.setVoltageRampRate(48.0) # Add SmartDashboard controls for testing # Add SmartDashboard live windowS LiveWindow.addActuator("DriveTrain", "Left Master %d" % robot.map.k_DtLeftMasterId, self.leftMasterMotor) LiveWindow.addActuator("DriveTrain", "Right Master %d" % robot.map.k_DtRightMasterId, self.rightMasterMotor) # init RobotDrive - all driving should occur through its methods # otherwise we expect motor saftey warnings self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor) self.robotDrive.setSafetyEnabled(True) # init IMU - used for driver & vision feedback as well as for # some autonomous modes. self.visionState = self.robot.visionState self.imu = BNO055() self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf, self.imu, DriveTrain._turnHelper(self)) self.turnPID.setOutputRange(-1, 1) self.turnPID.setInputRange(-180, 180) self.turnPID.setPercentTolerance(2) self.turnMultiplier = DriveTrain.k_mediumTurn self.maxSpeed = DriveTrain.k_defaultDriveSpeed # self.setContinuous() ? robot.info("Initialized DriveTrain") def initForCommand(self, controlMode): self.leftMasterMotor.setEncPosition(0) # async call self.rightMasterMotor.setEncPosition(0) self.robotDrive.stopMotor() self.robotDrive.setMaxOutput(self.maxSpeed) if controlMode == self.k_ControlModeSpeed: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) elif controlMode == self.k_ControlModeVBus: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) elif controlMode == self.k_ControlModeDisabled: # unverified codepath self.leftMasterMotor.disableControl() self.rightMasterMotor.disableControl() else: self.robot.error("Unexpected control mode") self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" % (self.leftMasterMotor.getControlMode(), self.rightMasterMotor.getControlMode())) def initDefaultCommand(self): # control modes are integers values: # 0 percentvbux # 1 position # 2 velocity self.setDefaultCommand(JoystickDriver(self.robot)) self.robotDrive.stopMotor(); def updateDashboard(self): # TODO: additional items? SmartDashboard.putNumber("IMU heading", self.getCurrentHeading()) SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration()) def stop(self): self.robotDrive.stopMotor() def joystickDrive(self, jsY, jsX, throttle): """ joystickDrive - called by JoystickDriver command. Inputs are always on the range [-1, 1]... These values can be scaled for a better "feel", but should always be in a subset of this range. """ if self.robot.isAutonomous or \ (math.fabs(jx) < 0.075 and math.fabs(jy) < .075): # joystick dead zone or auto (where we've been scheduled via # default command machinery) self.robotDrive.stopMotor() else: st = self.scaleThrottle(throttle) self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st) def drive(self, outputmag, curve): """ drive - used by drivestraight command.. """ self.robotDrive.drive(outputmag, curve) def driveStraight(self, speed): """driveStraight: invoked from AutoDriveStraight.. """ # NB: maxOuput isn't applied via set so # speed is supposedly measured in rpm but this depends on our # initialization establishing encoding ticks per revolution. # This is approximate so we rely on the observed values above. # (DEFAULT_SPEED_MAX_OUTPUT) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = speed / self.maxSpeed rotateValue = 0 self.robotDrive.arcadeDrive(moveValue, rotateValue) def startAutoTurn(self, degrees): self.robot.info("start autoturn from %d to %d" % (self.getHeading(), degrees)) self.turnPID.reset() self.turnPID.setSetpoint(degrees) self.turnPID.enable() def endAutoTurn(self): if self.turnPID.isEnabled(): self.turnPID.disable() def isAutoTurning(self): return self.turnPID.isEnabled() def isAutoTurnFinished(self): return self.turnPID.onTarget() def turn(self, speed): """turn takes a speed, not an angle... A negative speed is interpretted as turn left. Note that we bypass application of setMaxOutput Which only applies to calls to robotDrive. """ # In order to turn left, we want the right wheels to go # forward and left wheels to go backward (cf: tankDrive) # Oddly, our right master motor is reversed, so we compensate here. # speed < 0: turn left: rightmotor(negative) (forward), # leftmotor(negative) (backward) # speed > 0: turn right: rightmotor(positive) (backward) # leftmotor(positive) (forward) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = 0 rotateValue = speed / self.maxSpeed self.robotDrive.arcadeDrive(moveValue, rotateValue) def trackVision(self): """presumed called by either autonomous or teleop commands to allow the vision subsystem to guide the robot """ if self.visionState.DriveLockedOnTarget: self.stop() else: if self.isAutoTurning(): if self.isAutoTurnFinished(): self.endAutoTurn() self.visionState.DriveLockedOnTarget = True else: # setup for an auto-turn h = self.getCurrentHeading() tg = self.getTargetHeading(h) self.startAutoTurn(tg) def getCurrentHeading(self): """ getCurrentHeading returns a value between -180 and 180 """ return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi def scaleThrottle(self, rawThrottle): """ scaleThrottle: returns a scaled value between MIN_THROTTLE_SCALE and 1.0 MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation Scale the joystick values by throttle before passing to the driveTrain +1=bottom position; -1=top position """ # Throttle in the range of -1 to 1. We would like to change that to a # range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle # value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1)) # Then, add 1 to make the range 0-2 rather than -1 to +1 # Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale) # Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1 # # Check the results are in the range of k_minThrottleScale to 1, and clip # it in case the math went horribly wrong. result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale if result < self.k_minThrottleScale: # Somehow our math was wrong. Our value was too low, so force it to the minimum result = self.k_mintThrottleScale elif result > 1: # Somehow our math was wrong. Our value was too high, so force it to the maximum result = 1.0 return result def modifyTurnSpeed(self, speedUp): if speedUp: self.turnMultiplier = self.k_mediumTurn else: self.turnMultiplier = self.k_slowTurn def inchesToTicks(self, inches): return int(self.k_quadTicksPerInch * inches) def destinationReached(self, distance): return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \ math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
class IntakeLauncher(Subsystem): """A class to manage/control all aspects of shooting boulders. IntakeLauncher is comprised of: aimer: to raise & lower the mechanism for ball intake and shooting wheels: to suck or push the boulder launcher: to push boulder into the wheels during shooting limit switches: to detect if aimer is at an extreme position potentiometer: to measure our current aim position boulderSwitch: to detect if boulder is present Aiming is controlled via two modes 1: driver/interactive: aimMotor runs in VBUS mode to change angle 2: auto/vision control: aimMotor runs in closed-loop position mode to arrive at a designated position """ k_launchMin = 684.0 # manual calibration value k_launchMinDegrees = -11 # ditto (vestigial) k_launchMax = 1024.0 # manual calibration value k_launchMaxDegrees = 45 # ditto (vestigial) k_launchRange = k_launchMax - k_launchMin k_launchRangeDegrees = k_launchMaxDegrees - k_launchMinDegrees k_aimDegreesSlop = 2 # TODO: tune this k_intakeSpeed = -0.60 # pct vbux k_launchSpeedHigh = 1.0 # pct vbus k_launchSpeedLow = 0.7 k_launchSpeedZero = 0 k_servoLeftLaunchPosition = 0.45 # servo units k_servoRightLaunchPosition = 0.65 k_servoLeftNeutralPosition = 0.75 k_servoRightNeutralPosition = 0.4 k_aimMultiplier = 0.5 k_maxPotentiometerError = 5 # potentiometer units # launcher positions are measured as a percent of the range # of the potentiometer.. # intake: an angle approriate for intaking the boulder # neutral: an angle appropriate for traversing the lowbar # travel: an angle appropriate for traversing the rockwall, enableLimitSwitch # highgoal threshold: above which we shoot harder k_launchIntakeRatio = 0.0 k_launchTravelRatio = 0.51 k_launchNeutralRatio = 0.51 k_launchHighGoalThreshold = 0.69 def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot self.launchMin = self.k_launchMin self.launchMax = self.k_launchMax self.launchRange = self.launchMax - self.launchMin self.controlMode = None self.visionState = robot.visionState self.intakeLeftMotor = CANTalon(robot.map.k_IlLeftMotorId) self.intakeLeftMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeLeftMotor.reverseSensor(True) self.intakeLeftMotor.setSafetyEnabled(False) self.intakeRightMotor = CANTalon(robot.map.k_IlRightMotorId) self.intakeRightMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.intakeRightMotor.setSafetyEnabled(False) self.aimMotor = CANTalon(robot.map.k_IlAimMotorId) # LiveWindow.addActuator("IntakeLauncher", "AimMotor", self.aimMotor) if self.aimMotor.isSensorPresent(CANTalon.FeedbackDevice.AnalogPot): self.aimMotor.setSafetyEnabled(False) self.aimMotor.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogPot) self.aimMotor.enableLimitSwitch(True, True) self.aimMotor.enableBrakeMode(True) self.aimMotor.setAllowableClosedLoopErr(5) self.aimMotor.configPeakOutpuVoltage(12.0, -12.0) self.aimMotor.setVoltageRampRate(150) # TODO: setPID if needed self.boulderSwitch = DigitalInput(robot.map.k_IlBoulderSwitchPort) self.launcherServoLeft = Servo(robot.map.k_IlServoLeftPort) self.launcherServoRight = Servo(robot.map.k_IlServoRightPort) def initDefaultCommand(self): self.setDefaultCommand(ILCmds.DriverControlCommand(self.robot)) def updateDashboard(self): pass def beginDriverControl(self): self.setControlMode("vbus") def driverControl(self, deltaX, deltaY): if self.robot.visionState.wantsControl(): self.trackVision() else: self.setControlMode("vbus") self.aimMotor.set(deltaY * self.k_aimMultiplier) def endDriverControl(self): self.aimMotor.disableControl() # ---------------------------------------------------------------------- def trackVision(self): if not self.visionState.TargetAcquired: return # hopefully someone is guiding the drivetrain to help acquire if not self.visionState.LauncherLockedOnTarget: if math.fabs(self.visionState.TargetY - self.getAngle()) < self.k_aimDegreesSlop: self.visionState.LauncherLockedOnTarget = True else: self.setPosition(self.degreesToTicks(self.TargetY)) elif self.visionState.DriveLockedOnTarget: # here we shoot and then leave AutoAimMode pass else: # do nothing... waiting form driveTrain to reach orientation pass def setControlMode(self, mode): if mode != self.controlMode: self.controlMode = mode if mode == "vbus": self.aimMotor.disableControl() self.aimMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.aimMotor.enableControl() elif mode == "position": self.aimMotor.disableControl() self.aimMotor.changeControlMode(CANTalon.ControlMode.Position) self.aimMotor.enableControl() elif mode == "disabled": self.aimMotor.disableControl() else: self.robot.error("ignoring controlmode " + mode) self.controlMode = None self.aimMotor.disableControl() # launcher aim ------------------------------------------------------------- def getPosition(self): return self.aimMotor.getPosition() def getAngle(self): pos = self.getPosition() return self.ticksToDegress(pos) def setPosition(self, pos): self.setControlMode("position") self.aimMotor.set(pos) def isLauncherAtTop(self): return self.aimMotor.isFwdLimitSwitchClosed() def isLauncherAtBottom(self): return self.aimMotor.isRevLimitSwitchClosed() def isLauncherAtIntake(self): if self.k_intakeRatio == 0.0: return self.isLauncherAtBottom() else: return self.isLauncherNear(self.getIntakePosition()) def isLauncherAtNeutral(self): return self.isLauncherNear(self.getNeutralPosition()) def isLauncherAtTravel(self): return self.isLauncherNear(self.getTravelPosition()) def isLauncherNear(self, pos): return math.fabs(self.getPosition() - pos) < self.k_maxPotentiometerError def getIntakePosition(self): return self.getLauncherPositionFromRatio(self.k_launchIntakeRatio) def getNeutralPosition(self): return self.getLauncherPositionFromRatio(self.k_launchNeutralRatio) def getTravelPosition(self): return self.getLauncherPositionFromRatio(self.k_launchTravelRatio) def getLauncherPositionFromRatio(self, ratio): return self.launchMin + ratio * self.launchRange def degressToTicks(self, deg): ratio = (deg - self.k_launchMinDegrees) / self.k_launchRangeDegrees return self.k_launchMin + ratio * self.k_launchRange def ticksToDegrees(self, t): ratio = (t - self.k_launchMin) / self.k_launchRange return self.k_launchMinDegrees + ratio * self.k_launchRangeDegrees def calibratePotentiometer(self): if self.isLauncherAtBottom(): self.launchMin = self.getPosition() elif self.isLauncherAtTop(): self.launchMax = self.getPosition() self.launchRange = self.launchMax - self.launchMin # intake wheels controls --------------------------------------------------- def setWheelSpeedForLaunch(self): if self.isLauncherAtTop(): speed = self.k_launchSpeedHigh else: speed = self.k_launchSpeedLow self.setSpeed(speed) def setWheelSpeedForIntake(self): self.setSpeed(self.k_intakeSpeed) def stopWheels(self): self.setSpeed(k_launchSpeedZero) def setWheelSpeed(self, speed): self.intakeLeftMotor.set(speed) self.intakeRightMotor.set(-speed) # boulder and launcher servo controls -------------------------------------------------- def hasBoulder(self): return self.boulderSwitch.get() def activateLauncherServos(self): self.robot.info("activateLauncherServos at:" + self.getAimPosition()) self.launcherServoLeft.set(self.k_servoLeftLaunchPosition) self.launcherServoRight.set(self.k_servoRightLaunchPosition) def retractLauncherServos(self): self.launcherServoLeft.set(self.k_servoLeftNeutralPosition) self.launcherServoRight.set(self.k_servoRightNeutralPosition)