Пример #1
0
class SwerveModule():
    def __init__(self,
                 driveCanTalonId,
                 steerCanTalonId,
                 absoluteEncoder=True,
                 reverseDrive=False):
        # Initialise private motor controllers
        self._drive = CANTalon(driveCanTalonId)
        self.reverse_drive = reverseDrive
        self._steer = CANTalon(steerCanTalonId)
        self.absoluteEncoder = absoluteEncoder
        # Set up the motor controllers
        # Different depending on whether we are using absolute encoders or not
        if absoluteEncoder:
            self._steer.changeControlMode(CANTalon.ControlMode.Position)
            self._steer.setFeedbackDevice(
                CANTalon.FeedbackDevice.AnalogEncoder)
        else:
            self._steer.changeControlMode(CANTalon.ControlMode.Position)
            self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
            self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
            self._steer.setPosition(0.0)

        # Private members to store the setpoints
        self._speed = 0.0
        self._direction = 0.0
        self._opposite_direction = math.pi
        # Always in radians. Right hand rule applies - Z is up!
        # Rescale values to the range [-pi, pi)

    def steer(self, direction, speed=0):
        # Set the speed and direction of the swerve module
        # Always choose the direction that minimises movement,
        # even if this means reversing the drive motor
        direction = constrain_angle(direction)  # rescale to +/-pi
        current_heading = constrain_angle(self._direction)

        delta = min_angular_displacement(current_heading, direction)

        self._direction += delta
        if self.reverse_drive:
            speed = -speed
        if abs(constrain_angle(self._direction) - direction) < math.pi / 6.0:
            self._drive.set(speed)
            self._speed = speed
        else:
            self._drive.set(-speed)
            self._speed = -speed
        if self.absoluteEncoder:
            self._steer.set(self._direction / TAU *
                            RobotMap.module_rotation_volts_per_revolution)
        else:
            self._steer.set(self._direction / TAU *
                            RobotMap.module_rotation_counts_per_revolution)

    def getSpeed(self):
        return self._speed

    def getDirection(self):
        return self._direction
Пример #2
0
class SwerveModule():
    def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder = True, reverseDrive = False):
        # Initialise private motor controllers
        self._drive = CANTalon(driveCanTalonId)
        self.reverse_drive = reverseDrive
        self._steer = CANTalon(steerCanTalonId)
        self.absoluteEncoder = absoluteEncoder
        # Set up the motor controllers
        # Different depending on whether we are using absolute encoders or not
        if absoluteEncoder:
            self._steer.changeControlMode(CANTalon.ControlMode.Position)
            self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder)
        else:
            self._steer.changeControlMode(CANTalon.ControlMode.Position)
            self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
            self._steer.setPID(RobotMap.steering_p, 0.0, 0.0)
            self._steer.setPosition(0.0)

        # Private members to store the setpoints
        self._speed = 0.0
        self._direction = 0.0
        self._opposite_direction = math.pi
        # Always in radians. Right hand rule applies - Z is up!
        # Rescale values to the range [-pi, pi)

    def steer(self, direction, speed = 0):
        # Set the speed and direction of the swerve module
        # Always choose the direction that minimises movement,
        # even if this means reversing the drive motor
        direction = constrain_angle(direction) # rescale to +/-pi
        current_heading = constrain_angle(self._direction)

        delta = min_angular_displacement(current_heading, direction)

        self._direction += delta
        if self.reverse_drive:
            speed=-speed
        if abs(constrain_angle(self._direction)-direction)<math.pi/6.0:
            self._drive.set(speed)
            self._speed = speed
        else:
            self._drive.set(-speed)
            self._speed = -speed
        if self.absoluteEncoder:
            self._steer.set(self._direction/TAU*RobotMap.module_rotation_volts_per_revolution)
        else:
            self._steer.set(self._direction/TAU*RobotMap.module_rotation_counts_per_revolution)

    def getSpeed(self):
        return self._speed

    def getDirection(self):
        return self._direction
Пример #3
0
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)
Пример #4
0
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)
Пример #5
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
Пример #6
0
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)
Пример #7
0
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
Пример #8
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)