Beispiel #1
0
class DriveTrain(Subsystem):
    """
    The DriveTrain subsytem is used by the driver as well as the Pathfinder and Motion Profile
    controllers.  The default command is DriveJoystick.  Each side of the differential drive is
    connected to CTRE's magnetic encoders.
    """

    ENCODER_TICKS_PER_REV = 4096
    MP_SLOT0_SELECT = 0
    MP_SLOT1_SELECT = 0

    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        # Map the CIM motors to the TalonSRX's
        self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
        self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
        self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
        self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)

        # Set the front motors to be the followers of the rear motors
        self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
                           DRIVETRAIN_REAR_LEFT_MOTOR)
        self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
                            DRIVETRAIN_REAR_RIGHT_MOTOR)

        # Add the motors to the speed controller groups and create the differential drivetrain
        self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
        self.rightDrive = SpeedControllerGroup(self.frontRight,
                                               self.rightTalon)
        self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)

        # Setup the default motor controller setup
        self.initControllerSetup()

        # Map the pigeon.  This will be connected to an unused Talon.
        self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
        self.pigeonIMU = PigeonIMU(self.talonPigeon)

    def initControllerSetup(self):
        """
        This method will setup the default settings of the motor controllers.
        """
        # Feedback sensor phase
        self.leftTalon.setSensorPhase(True)
        self.rightTalon.setSensorPhase(True)

        # Diable the motor-safety
        self.diffDrive.setSafetyEnabled(False)

        # Enable brake/coast mode
        self.leftTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)
        self.rightTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)

        # This function will intiliaze the drivetrain motor controllers to the factory defaults.
        # Only values which do not match the factory default will be written.  Any values which
        # are explicity listed will be skipped (ie any values written prior in this method).

        #  ***** TODO *****  #

    def initiaizeDrivetrainMotionProfileControllers(self, stream_rate):
        """
        This method will initialize the Talon's for motion profiling
        """
        # Invert right motors
        self.rightTalon.setInverted(True)
        self.frontRight.setInverted(True)

        # Enable voltage compensation for 12V
        self.leftTalon.configVoltageCompSaturation(12.0, 10)
        self.leftTalon.enableVoltageCompensation(True)
        self.rightTalon.configVoltageCompSaturation(12.0, 10)
        self.rightTalon.enableVoltageCompensation(True)

        # PIDF slot index 0 is for autonomous wheel postion
        # There are 4096 encoder units per rev.  1 rev of the wheel is pi * diameter.  That
        # evaluates to 2607.6 encoder units per foot.  For the feed-forward system, we expect very
        # tight position control, so use a P-gain which drives full throttle at 8" of error.  This
        # evaluates to 0.588 = (1.0 * 1023) / (8 / 12 * 2607.6)
        self.leftTalon.config_kP(0, 0.0, 10)
        self.leftTalon.config_kI(0, 0.0, 10)
        self.leftTalon.config_kD(0, 0.0, 10)
        self.leftTalon.config_kF(0, 1023 / 12, 10)  # 10-bit ADC / 12 V
        self.leftTalon.config_IntegralZone(0, 100, 10)
        self.leftTalon.configClosedLoopPeakOutput(0, 1.0, 10)
        self.rightTalon.config_kP(0, 0.0, 10)
        self.rightTalon.config_kI(0, 0.0, 10)
        self.rightTalon.config_kD(0, 0.0, 10)
        self.rightTalon.config_kF(0, 1023 / 12, 10)  # 10-bit ADC / 12 V
        self.rightTalon.config_IntegralZone(0, 100, 10)
        self.rightTalon.configClosedLoopPeakOutput(0, 1.0, 10)

        # PIDF slot index 1 is for autonomous heading
        self.leftTalon.config_kP(1, 0, 10)
        self.leftTalon.config_kI(1, 0, 10)
        self.leftTalon.config_kD(1, 0, 10)
        self.leftTalon.config_kF(1, 0, 10)
        self.leftTalon.config_IntegralZone(1, 100, 10)
        self.leftTalon.configClosedLoopPeakOutput(1, 1.0, 10)
        self.rightTalon.config_kP(1, 0, 10)
        self.rightTalon.config_kI(1, 0, 10)
        self.rightTalon.config_kD(1, 0, 10)
        self.rightTalon.config_kF(1, 0, 10)
        self.rightTalon.config_IntegralZone(1, 100, 10)
        self.rightTalon.configClosedLoopPeakOutput(1, 1.0, 10)

        # Change the control frame period
        self.leftTalon.changeMotionControlFramePeriod(stream_rate)
        self.rightTalon.changeMotionControlFramePeriod(stream_rate)

        # Initilaize the quadrature encoders and pigeon IMU
        self.initQuadratureEncoder()
        self.initPigeonIMU()

    def cleanUpDrivetrainMotionProfileControllers(self):
        '''
        This mothod will be called to cleanup the Talon's motion profiling
        '''
        # Invert right motors again so the open-loop joystick driving works
        self.rightTalon.setInverted(False)
        self.frontRight.setInverted(False)

        # Change the control frame period back to the default
        framePeriod = TALON_DEFAULT_MOTION_CONTROL_FRAME_PERIOD_MS
        self.leftTalon.changeMotionControlFramePeriod(framePeriod)
        self.rightTalon.changeMotionControlFramePeriod(framePeriod)

    def initPigeonIMU(self):
        # false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
        # true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
        self.rightTalon.configAuxPIDPolarity(False, 10)
        self.leftTalon.configAuxPIDPolarity(True, 10)

        # select a gadgeteer pigeon for remote 0
        self.rightTalon.configRemoteFeedbackFilter(
            self.talonPigeon.getDeviceID(),
            RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)
        self.leftTalon.configRemoteFeedbackFilter(
            self.talonPigeon.getDeviceID(),
            RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)

        # Select the remote feedback sensor for PID1
        self.rightTalon.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)
        self.leftTalon.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)

        # Using the config feature, scale units to 3600 per rotation.  This is nice as it keeps
        # 0.1 deg resolution, and is fairly intuitive.
        self.rightTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)
        self.leftTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)

        # Zero the sensor
        self.pigeonIMU.setYaw(0, 10)
        self.pigeonIMU.setAccumZAngle(0, 10)

    def initQuadratureEncoder(self):
        """
        This method will initialize the encoders for quadrature feedback.
        """
        self.leftTalon.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
        self.rightTalon.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
        self.leftTalon.getSensorCollection().setQuadraturePosition(0, 10)
        self.rightTalon.getSensorCollection().setQuadraturePosition(0, 10)

    def getLeftQuadraturePosition(self):
        """
        This method will return the left-side sensor quadrature position.  The sign needs to
        manually be handled here since this function is used to provide the sensor postion outide
        of the talon.
        """
        return -self.leftTalon.getSensorCollection().getQuadraturePosition()

    def getRightQuadraturePosition(self):
        """
        This method will return the right-side sensor quadrature position.  The sign needs to
        manually be handled here since this function is used to provide the sensor postion outide
        of the talon.
        """
        return self.rightTalon.getSensorCollection().getQuadraturePosition()

    def setQuadratureStatusFramePeriod(self, sample_period_ms):
        """
        This method will set the status frame persiod of the quadrature encoder
        """
        self.leftTalon.setStatusFramePeriod(
            WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
            sample_period_ms, 10)
        self.rightTalon.setStatusFramePeriod(
            WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
            sample_period_ms, 10)

    def setDefaultQuadratureStatusFramePeriod(self):
        """
        This method will set the status frame persiod of the quadrature encoder back to the factory
        default.
        """
        self.leftTalon.setStatusFramePeriod(
            WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
            TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)
        self.rightTalon.setStatusFramePeriod(
            WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
            TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)

    def pathFinderDrive(self, leftOutput, rightOutput):
        """
        This method will take the Pathfinder Controller motor output and apply them to the
        drivetrain.
        """
        self.leftTalon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftOutput)
        self.rightTalon.set(WPI_TalonSRX.ControlMode.PercentOutput,
                            -rightOutput)

    def getLeftVelocity(self):
        return self.leftTalon.getSensorCollection().getQuadratureVelocity()

    def getRightVelocity(self):
        return self.rightTalon.getSensorCollection().getQuadratureVelocity()

    def getLeftVoltage(self):
        return self.leftTalon.getMotorOutputVoltage()

    def getRightVoltage(self):
        return self.rightTalon.getMotorOutputVoltage()

    def initDefaultCommand(self):
        """
        This method will set the default command for this subsystem.
        """
        self.setDefaultCommand(DriveJoystick(self.robot))
Beispiel #2
0
class DriveTrain(Subsystem):
    """
    This subsystem controls the drivetrain for the robot.
    """
    def __init__(self):
        super().__init__()
        self.logger = logging.getLogger(self.getName())

        # Configure motors
        self.leftMasterTalon = WPI_TalonSRX(RobotMap.LEFT_MASTER_TALON)
        self.leftSlaveTalon = WPI_TalonSRX(RobotMap.LEFT_SLAVE_TALON)
        self.rightMasterTalon = WPI_TalonSRX(RobotMap.RIGHT_MASTER_TALON)
        self.rightSlaveTalon = WPI_TalonSRX(RobotMap.RIGHT_SLAVE_TALON)

        self.leftSlaveTalon.follow(self.leftMasterTalon)
        self.rightSlaveTalon.follow(self.rightMasterTalon)

        self.leftMasterTalon.configSelectedFeedbackSensor(
            FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
        self.rightMasterTalon.configSelectedFeedbackSensor(
            FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
        self.leftMasterTalon.setInverted(True)
        self.leftSlaveTalon.setInverted(True)

        self.configureDrivePID()

        # Configure shift solenoid
        self.shiftSolenoid = DoubleSolenoid(RobotMap.SHIFT_IN_SOLENOID,
                                            RobotMap.SHIFT_OUT_SOLENOID)

    def configureDrivePID(self):
        # Slot 0 = Distance PID
        # Slot 1 = Velocity PID
        self.leftMasterTalon.config_kF(0, Constants.LEFT_DISTANCE_F,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kP(0, Constants.LEFT_DISTANCE_P,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kI(0, Constants.LEFT_DISTANCE_I,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kD(0, Constants.LEFT_DISTANCE_D,
                                       Constants.TIMEOUT_MS)

        self.leftMasterTalon.config_kF(1, Constants.LEFT_VELOCITY_F,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kP(1, Constants.LEFT_VELOCITY_P,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kI(1, Constants.LEFT_VELOCITY_I,
                                       Constants.TIMEOUT_MS)
        self.leftMasterTalon.config_kD(1, Constants.LEFT_VELOCITY_D,
                                       Constants.TIMEOUT_MS)

        self.rightMasterTalon.config_kF(0, Constants.RIGHT_DISTANCE_F,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kP(0, Constants.RIGHT_DISTANCE_P,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kI(0, Constants.RIGHT_DISTANCE_I,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kD(0, Constants.RIGHT_DISTANCE_D,
                                        Constants.TIMEOUT_MS)

        self.rightMasterTalon.config_kF(1, Constants.RIGHT_VELOCITY_F,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kP(1, Constants.RIGHT_VELOCITY_P,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kI(1, Constants.RIGHT_VELOCITY_I,
                                        Constants.TIMEOUT_MS)
        self.rightMasterTalon.config_kD(1, Constants.RIGHT_VELOCITY_D,
                                        Constants.TIMEOUT_MS)

        self.leftMasterTalon.configMotionAcceleration(6385)
        self.rightMasterTalon.configMotionAcceleration(6385)

        self.leftMasterTalon.configMotionCruiseVelocity(6385)
        self.rightMasterTalon.configMotionCruiseVelocity(6385)

    def initDefaultCommand(self):
        self.setDefaultCommand(JoystickDrive())

    def zeroEncoders(self):
        self.leftMasterTalon.setSelectedSensorPosition(0)
        self.rightMasterTalon.setSelectedSensorPosition(0)

    def set(self, type, leftMagnitude, rightMagnitude):
        assert (isinstance(type, ControlMode))

        if type == ControlMode.PercentOutput:
            self.leftMasterTalon.set(ControlMode.PercentOutput, leftMagnitude)
            self.rightMasterTalon.set(ControlMode.PercentOutput,
                                      rightMagnitude)
        elif type == ControlMode.Velocity:
            self.leftMasterTalon.set(ControlMode.Velocity, leftMagnitude)
            self.rightMasterTalon.set(ControlMode.Velocity, rightMagnitude)
        elif type == ControlMode.MotionMagic:
            self.leftMasterTalon.set(ControlMode.MotionMagic, leftMagnitude)
            self.rightMasterTalon.set(ControlMode.MotionMagic, rightMagnitude)

    def setPIDSlot(self, slot):
        self.leftMasterTalon.selectProfileSlot(slot, 0)
        self.rightMasterTalon.selectProfileSlot(slot, 0)

    def getPositions(self):
        return self.leftMasterTalon.getSelectedSensorPosition(
        ), self.rightMasterTalon.getSelectedSensorPosition()

    def getVelocities(self):
        return self.leftMasterTalon.getSelectedSensorVelocity(
        ), self.rightMasterTalon.getSelectedSensorVelocity()

    def customArcadeDrive(self, xSpeed, zRotation, squareInputs):
        deadband = 0.1
        xSpeed, zRotation = self.limit(xSpeed, zRotation)

        xSpeed, zRotation = self.applyDeadband(deadband, xSpeed, zRotation)

        if squareInputs:
            xSpeed = math.copysign(xSpeed * xSpeed, xSpeed)
            zRotation = math.copysign(zRotation * zRotation, zRotation)

        maxInput = math.copysign(max(abs(xSpeed), abs(zRotation)), xSpeed)

        if xSpeed >= 0:
            if zRotation >= 0:
                leftMotorOutput = maxInput
                rightMotorOutput = xSpeed - zRotation
            else:
                leftMotorOutput = xSpeed + zRotation
                rightMotorOutput = maxInput
        else:
            if zRotation >= 0:
                leftMotorOutput = xSpeed + zRotation
                rightMotorOutput = maxInput
            else:
                leftMotorOutput = maxInput
                rightMotorOutput = xSpeed - zRotation

        # Fine tune control
        if abs(xSpeed) > abs(zRotation):
            self.leftMasterTalon.set(ControlMode.PercentOutput,
                                     self.limit(leftMotorOutput) * 0.55)
            self.rightMasterTalon.set(ControlMode.PercentOutput,
                                      self.limit(rightMotorOutput) * -0.55)
        else:
            self.leftMasterTalon.set(ControlMode.PercentOutput,
                                     self.limit(leftMotorOutput) * 0.55)
            self.rightMasterTalon.set(ControlMode.PercentOutput,
                                      self.limit(rightMotorOutput) * -0.55)

    def limit(self, *args):
        return_list = []
        for arg in args:
            if arg > 1.0:
                return_list.append(1.0)
            elif arg < -1.0:
                return_list.append(1.0)
            else:
                return_list.append(arg)

        return return_list[0] if len(return_list) == 1 else return_list

    def applyDeadband(self, deadband, *args):
        return_list = []
        for arg in args:
            if abs(arg) < deadband:
                return_list.append(0.0)
            else:
                return_list.append(arg)

        return return_list[0] if len(return_list) == 1 else return_list
Beispiel #3
0
class Boom(Subsystem):
    """
    The boom subsystem is used by the operator and also the boom controller.  There is a pot
    mounted to the linear actuator which will provide feedback for the boom controller.
    """

    ENCODER_TICS_PER_REV = 102.4  # 10-turn analog pot on 10-bit ADC: 1024 / 10
    POT_INTAKE_POSITION_DEG = 84 * (3600 / 1023)
    POT_SWITCH_POSITION_DEG = 348 * (3600 / 1023)  # ** TODO **
    POT_SCALE_POSITION_DEG = 848 * (3600 / 1023)  # ** TODO **
    POT_SCALE_POSITION = 825
    POT_SCALE_UPPER_ERROR = 1005
    POT_SCALE_LOWER_ERROR = 645
    POT_ERROR_LIMIT = 360.0

    # FORWARD_SOFT_LIMIT = 840
    # REVERSE_SOFT_LIMIT = 90

    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        # Map the CIM motors to the TalonSRX's
        self.talon = WPI_TalonSRX(BOOM_MOTOR)

        # Setup the default motor controller setup
        self.initOpenLoop()
        self.initClosedLoop()

    def initOpenLoop(self):
        """
        This method will setup the default settings of the motor controllers.
        """
        # Set to percent output mode at neutral
        self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)

        # Add a ramp-rate limiter to joystick control
        self.talon.configOpenLoopRamp(0.2, 10)

        # Add current limiter
        # self.talon.configPeakCurrentLimit(BOOM_MAX_CURRENT, 10)
        # self.talon.configPeakCurrentDuration(0, 10)  # Necessary to avoid errata
        # self.talon.configContinuousCurrentLimit(BOOM_MAX_CURRENT, 10)
        # self.talon.enableCurrentLimit(True)

        # Add soft limits
        # self.talon.configForwardSoftLimitThreshold(self.FORWARD_SOFT_LIMIT, 0)
        # self.talon.configForwardSoftLimitEnable(True, 0)
        # self.talon.configReverseSoftLimitThreshold(self.REVERSE_SOFT_LIMIT, 0)
        # self.talon.configReverseSoftLimitEnable(True, 0)

        # Configured forward and reverse limit switch of Talon to be from a feedback connector and
        # be normally open
        self.talon.configForwardLimitSwitchSource(
            LimitSwitchSource.FeedbackConnector,
            LimitSwitchNormal.NormallyOpen, 0, 10)
        self.talon.configReverseLimitSwitchSource(
            LimitSwitchSource.FeedbackConnector,
            LimitSwitchNormal.NormallyOpen, 0, 10)

        # Set brake/coast mode
        self.talon.setNeutralMode(WPI_TalonSRX.NeutralMode.Brake)

    def initClosedLoop(self):
        # PIDF slot index 0 is for single-position moves
        self.talon.config_kP(0, 5.0, 10)
        self.talon.config_kI(0, 0.0, 10)
        self.talon.config_kD(0, 0.0, 10)
        self.talon.config_kF(0, 20, 10)

        # PIDF slot index 1 is for multi-position moves
        self.talon.config_kP(1, 1.0, 10)
        self.talon.config_kI(1, 0.0, 10)
        self.talon.config_kD(1, 0.0, 10)
        self.talon.config_kF(1, 22, 10)

        # Use analog potentiometer
        self.initAnalogPotentiometer()

    def initAnalogPotentiometer(self):
        """
        This method will initialize the analog potentiometer for position feedback.
        """
        self.talon.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.Analog, 0, 10)

    def getPotPositionInDegrees(self):
        """
        This method will return the potentiometer position.
        """
        # Lead: 0.20", Potentiometer for a 6.85:1 ratio, 12 / 0.2 = 60 => 60 / 6.85 = 8.76
        # The potentiometer will turn 8.76 times during a 12-inch travel
        # 0V - 5V maps to 0 - 1023
        degreesPerADC = 3600 / 1024
        degrees = self.talon.getSensorCollection().getAnalogInRaw(
        ) * degreesPerADC
        # logger.debug('Pot Degrees: %1.2f' % (degrees))
        return degrees

    def getPotPosition(self):
        """
        This method will return the potentiometer position.
        """
        return self.talon.getSensorCollection().getAnalogInRaw()

    def getPotVelocityInDegPer100ms(self):
        """
        This method will return the potentiometer velocity in deg / 100ms
        """
        return self.talon.getSensorCollection().getAnalogInVel()

    def getActiveMPPosition(self):
        """
        This method will return the active motion profile position
        """
        return self.talon.getActiveTrajectoryPosition()

    def getActiveMPVelocity(self):
        """
        This method will return the active motion profile position
        """
        return self.talon.getActiveTrajectoryVelocity()

    def getPrimaryClosedLoopTarget(self):
        """
        This method will return the closed loop target.
        """
        return self.talon.getClosedLoopTarget(0)

    def getPrimaryClosedLoopError(self):
        """
        This method will return the closed loop error for the primary conrol loop.
        """
        return self.talon.getClosedLoopError(0)

    def initDefaultCommand(self):
        """
        This method will set the default command for this subsystem.
        """
        self.setDefaultCommand(BoomJoystick(self.robot))