Example #1
0
    def __init__(self):

        # Initialize all controllers
        self.driveLeftMaster = Talon(kDriveTrain.lmId)
        self.driveLeftSlave = Talon(kDriveTrain.lsId)
        self.driveRightMaster = Talon(kDriveTrain.rmId)
        self.driveRightSlave = Talon(kDriveTrain.rsId)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        # CURRENTLY DISABLED WHEN USING WITH DifferentialDrive
        # self.driveLeftSlave.setInverted(False)
        # self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        # Throw data on the SmartDashboard so we can work with it.
        SD.putNumber('Left Quad Pos.',
                     self.driveLeftMaster.getQuadraturePosition())
        SD.putNumber('Right Quad Pos.',
                     self.driveRightMaster.getQuadraturePosition())

        # these may give the derivitive an integral of the PID once
        # they are set.  For now, they just show 0
        SD.putNumber('Left Derivative',
                     self.driveLeftMaster.getErrorDerivative(0))
        SD.putNumber('Left Integral',
                     self.driveLeftMaster.getIntegralAccumulator(0))

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None

        # self.driveLeftMaster.config_kP(0, .3, 10)
        # kP = self.driveLeftMaster.configGetParameter(
        #     self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)

        # SmartDashboard.putNumber('Left Proportional', kP)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)
Example #2
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)
Example #3
0
    def __init__(self):
        super().__init__('Drive')

        self.debug = False
        self.yawZero = 0

        self.navx = navx.ahrs.AHRS.create_spi()
        self.navx.setName("Drive", "NavX")
        self.accel = wpilib.BuiltInAccelerometer()
        self.accel.setName("Drive", "Accel")
        self.left : Tread = Tread("Left", \
            robotmap.kCanDriveLeft0, robotmap.kCanDriveLeft1, robotmap.kCanDriveLeft2, True, \
            robotmap.kDioDriveLeftEncA, robotmap.kDioDriveLeftEncB, kLeftConv)
        self.right : Tread = Tread("Right", \
            robotmap.kCanDriveRight0, robotmap.kCanDriveRight1, robotmap.kCanDriveRight2, False, \
            robotmap.kDioDriveRightEncA, robotmap.kDioDriveRightEncB, kRightConv)

        self.left.debug = self.debug
        self.right.debug = self.debug

        self.drive: DifferentialDrive = DifferentialDrive(
            self.left.motor, self.right.motor)
        self.drive.setName("Drive", "Differential")
        self.drive.setSafetyEnabled(False)
        self.drive.setDeadband(0.025)
        self.drive.setRightSideInverted(False)
        self.periodic()
        self.zero()
Example #4
0
    def __init__(self):
        '''Instantiates the drivetrain object.'''

        super().__init__('Drivetrain')
        self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.frontLeftMotor)
        self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.frontRightMotor)
        self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.rearLeftMotor)
        self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.rearRightMotor)

        self.frontLeftMotor.setInverted(True)
        self.rearLeftMotor.setInverted(True)
        self.frontRightMotor.setInverted(True)
        self.rearRightMotor.setInverted(True)

        self.leftMotors = wpilib.SpeedControllerGroup(self.frontLeftMotor,
                                                      self.rearLeftMotor)
        self.rightMotors = wpilib.SpeedControllerGroup(self.frontRightMotor,
                                                       self.rearRightMotor)

        self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors)

        self.lastMoveValue = 0
        self.lastRotateValue = 0

        self.gyro = wpilib.ADXRS450_Gyro()
        self.setpoint = 0
        self.angleAcc = 0
        self.anglePreviousTime = time.time()

        self.rangeFinder = wpilib.AnalogInput(0)

        self.didResetRevos = True
        self.startTime = 0
        self.revoVelos = []

        self.gyro.calibrate()
Example #5
0
class Drivetrain(Subsystem):
    '''
    This is the drivetrain subsystem
    '''
    def __init__(self):
        '''Instantiates the drivetrain object.'''

        super().__init__('Drivetrain')
        self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.frontLeftMotor)
        self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.frontRightMotor)
        self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.rearLeftMotor)
        self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
            RobotMap.drivetrain.rearRightMotor)

        self.frontLeftMotor.setInverted(True)
        self.rearLeftMotor.setInverted(True)
        self.frontRightMotor.setInverted(True)
        self.rearRightMotor.setInverted(True)

        self.leftMotors = wpilib.SpeedControllerGroup(self.frontLeftMotor,
                                                      self.rearLeftMotor)
        self.rightMotors = wpilib.SpeedControllerGroup(self.frontRightMotor,
                                                       self.rearRightMotor)

        self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors)

        self.lastMoveValue = 0
        self.lastRotateValue = 0

        self.gyro = wpilib.ADXRS450_Gyro()
        self.setpoint = 0
        self.angleAcc = 0
        self.anglePreviousTime = time.time()

        self.rangeFinder = wpilib.AnalogInput(0)

        self.didResetRevos = True
        self.startTime = 0
        self.revoVelos = []

        self.gyro.calibrate()

    def drive(self, moveValue, rotateValue):
        '''Arcade drive'''

        # Limit the speed of the robot if it's rekting our battery
        if (wpilib.RobotController.getBatteryVoltage()) < 7:
            if abs(moveValue) > 0:
                moveValue = 0 if moveValue > 0 else -0
            if abs(rotateValue) > 0:
                rotateValue = 0 if rotateValue > 0 else -0
            OI.rumble()
        else:
            OI.stopRumble()

        self.drivetrain.arcadeDrive(moveValue, rotateValue)
        self.lastMoveValue = moveValue
        self.lastRotateValue = rotateValue

    def setSetpoint(self, setpoint):
        self.setpoint = setpoint

    def getError(self):
        return self.getAngle() - self.setpoint

    def stop(self):
        self.drive(0, 0)

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

    def getSpeed(self):
        return self.lastMoveValue

    def getRotate(self):
        return self.lastRotateValue

    def getAngle(self):
        # return self.angleAcc
        return self.gyro.getAngle()

    def resetGyro(self):
        self.anglePreviousTime = time.time()
        self.angleAcc = 0
        self.gyro.reset()

    def getRotationRate(self):
        return self.gyro.getRate()

    def getRangeFinderDistance(self):
        # return self.rangeFinder.getVoltage()
        voltage = self.rangeFinder.getAverageVoltage()
        voltage -= 0.28
        distance = 100 * 1.3168135 * voltage  # Refer to https://www.maxbotix.com/documents/XL-MaxSonar-EZ_Datasheet.pdf
        return distance

    def _getRevPerSec(self):
        return self.rearLeftMotor.getPulseWidthVelocity() / 409.6

    def updateAngleAcc(self):
        # Integrate the rotations per second
        dt = time.time() - self.anglePreviousTime

        y = self.gyro.getRate()
        # Ignore rates less than 1
        if abs(y) < 1:
            y = 0

        self.angleAcc += y * dt
        self.anglePreviousTime = time.time()

    def updateRevolutionCounter(self):
        if self.didResetRevos:
            self.didResetRevos = False
            self.startTime = time.time()
            self.revoVelos = [(0, self._getRevPerSec())]
            return

        self.revoVelos.append(
            (time.time() - self.startTime, self._getRevPerSec()))

    def resetRevolutionCounter(self):
        self.didResetRevos = True

    def getRevolutions(self):
        # Integrate the revoVelos list
        # The first index of the revoVelos list is a time
        # The second index is the revs per sec
        # Integrate to get total revolutions over a certain period
        integral = 0
        for i in range(len(self.revoVelos) - 1):
            dt = self.revoVelos[i + 1][0] - self.revoVelos[i][0]
            y = self.revoVelos[i][1]
            integral += dt * y
        return integral

    def update(self):
        self.updateRevolutionCounter()
        self.updateAngleAcc()

    def log(self):
        wpilib.SmartDashboard.putNumber('Speed Output', self.getSpeed())
        wpilib.SmartDashboard.putNumber('Rotate Output', self.getRotate())
        wpilib.SmartDashboard.putNumber('Absolute Angle', self.getAngle())
        wpilib.SmartDashboard.putNumber('Angle', self.getAngle() % 360)
        wpilib.SmartDashboard.putNumber('Gyro Error', self.getError())
        wpilib.SmartDashboard.putNumber('Gyro Rotation Rate',
                                        self.gyro.getRate())
        wpilib.SmartDashboard.putNumber('Gyro Setpoint', self.setpoint)
        wpilib.SmartDashboard.putNumber('Ranger Finder Distance',
                                        self.getRangeFinderDistance())
        wpilib.SmartDashboard.putNumber('Encoder Rev Per Sec',
                                        self._getRevPerSec())
        wpilib.SmartDashboard.putNumber('Total Revolutions',
                                        self.getRevolutions())
        wpilib.SmartDashboard.putNumber('Encoder Temp',
                                        self.rearLeftMotor.getTemperature())

    def saveOutput(self):
        return 'move: {0}\nturn: {1}\nangle: {2}\n'.format(
            self.getSpeed(), self.getRotate(), self.getAngle())

    def playFromRecording(self, recording):
        '''
        This plays back a certain recording, but only using
        the values that are useful for the drivetrain
        '''

        lines = recording.split('\n')

        for l in lines:
            if l.startswith('move'):
                moveValue = float(l.rstrip()[len('move: '):])
            elif l.startswith('turn'):
                turnValue = float(l.rstrip()[len('turn: '):])
            elif l.startswith('angle'):
                # TODO: Implement turning from recording
                print('TODO: Implement turning from recording')

        self.drive(moveValue, turnValue)
Example #6
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))
Example #7
0
class DriveTrain(Subsystem):
    '''
    'Tank Drive' system set up with 2 motors per side, one a "master"
    with a mag encoder attached and the other "slave" controller set
    to follow the "master".
    '''
    def __init__(self):

        # Initialize all controllers
        self.driveLeftMaster = Talon(kDriveTrain.lmId)
        self.driveLeftSlave = Talon(kDriveTrain.lsId)
        self.driveRightMaster = Talon(kDriveTrain.rmId)
        self.driveRightSlave = Talon(kDriveTrain.rsId)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        # CURRENTLY DISABLED WHEN USING WITH DifferentialDrive
        # self.driveLeftSlave.setInverted(False)
        # self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        # Throw data on the SmartDashboard so we can work with it.
        SD.putNumber('Left Quad Pos.',
                     self.driveLeftMaster.getQuadraturePosition())
        SD.putNumber('Right Quad Pos.',
                     self.driveRightMaster.getQuadraturePosition())

        # these may give the derivitive an integral of the PID once
        # they are set.  For now, they just show 0
        SD.putNumber('Left Derivative',
                     self.driveLeftMaster.getErrorDerivative(0))
        SD.putNumber('Left Integral',
                     self.driveLeftMaster.getIntegralAccumulator(0))

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None

        # self.driveLeftMaster.config_kP(0, .3, 10)
        # kP = self.driveLeftMaster.configGetParameter(
        #     self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)

        # SmartDashboard.putNumber('Left Proportional', kP)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)

    def moveToPosition(self, position, side='left'):

        if side == 'left':
            self.driveLeftMaster.setSafetyEnabled(False)
            self.driveLeftMaster.set(Talon.ControlMode.Position, position)
        else:
            self.driveRightMaster.set(Talon.ControlMode.Position, position)

    def stop(self):
        # self.driveLeftMaster.set(0)
        # self.driveRightMaster.set(0)
        self.drive.stopMotor()

    def arcade(self, speed, rotation):
        self.updateSD()
        self.drive.arcadeDrive(speed, rotation, True)

    def updateSD(self):

        leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
        leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)

        rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
        rightPos = self.driveRightMaster.getSelectedSensorPosition(0)

        # calculate side deltas
        if self.leftVel:
            leftVelDelta = leftVel - self.leftVel
        else:
            leftVelDelta = 0

        if self.leftPos:
            leftPosDelta = leftPos - self.leftPos
        else:
            leftPosDelta = 0

        if self.rightVel:
            rightVelDelta = rightVel - self.rightVel
        else:
            rightVelDelta = 0

        if self.rightPos:
            rightPosDelta = rightPos - self.rightPos
        else:
            rightPosDelta = 0

        # calculate delta of delta
        differenceVel = leftVelDelta - rightVelDelta
        differencePos = leftPosDelta - rightPosDelta

        SD.putNumber("LeftSensorVel", leftVel)
        SD.putNumber("LeftSensorPos", leftPos)

        SD.putNumber("RightSensorVel", rightVel)
        SD.putNumber("RightSensorPos", rightPos)

        SD.putNumber('LeftVelDelta', leftVelDelta)
        SD.putNumber('LeftPosDelta', leftPosDelta)

        SD.putNumber('RightVelDelta', rightVelDelta)
        SD.putNumber('RightPosDelta', rightPosDelta)

        SD.putNumber('DifferenceVel', differenceVel)
        SD.putNumber('DifferencePos', differencePos)

        self.leftVel = leftVel
        self.leftPos = leftPos
        self.rightVel = rightVel
        self.rightPos = rightPos
Example #8
0
    def __init__(self, robot):

        self.robot = robot

        # Initialize all controllers
        self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
        self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
        self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
        self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])

        wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
                                      self.driveLeftMaster)
        wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
                                      self.driveRightMaster)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        self.driveLeftSlave.setInverted(False)
        self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)
        """
        Initializes the count for toggling which side of the 
        robot will be considered the front when driving.
        """
        self.robotFrontToggleCount = 2

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        # these supposedly aren't part of the WPI_TalonSRX class
        # self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
        # self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)

        # Throw data on the SmartDashboard so we can work with it.
        # SD.putNumber(
        #     'Left Quad Pos.',
        #     self.driveLeftMaster.getQuadraturePosition())
        # SD.putNumber(
        #     'Right Quad Pos.',
        #     self.driveRightMaster.getQuadraturePosition())

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None
        self.leftMaxVel = 0
        self.rightMaxVel = 0
        self.leftMinVel = 0
        self.rightMinVel = 0

        # self.driveLeftMaster.config_kP(0, .3, 10)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)

        super().__init__()
Example #9
0
class DriveTrain(Subsystem):
    '''
    'Tank Drive' system set up with 2 motors per side, one a "master"
    with a mag encoder attached and the other "slave" controller set
    to follow the "master".
    '''
    def __init__(self, robot):

        self.robot = robot

        # Initialize all controllers
        self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
        self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
        self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
        self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])

        wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
                                      self.driveLeftMaster)
        wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
                                      self.driveRightMaster)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        self.driveLeftSlave.setInverted(False)
        self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)
        """
        Initializes the count for toggling which side of the 
        robot will be considered the front when driving.
        """
        self.robotFrontToggleCount = 2

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        # these supposedly aren't part of the WPI_TalonSRX class
        # self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
        # self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)

        # Throw data on the SmartDashboard so we can work with it.
        # SD.putNumber(
        #     'Left Quad Pos.',
        #     self.driveLeftMaster.getQuadraturePosition())
        # SD.putNumber(
        #     'Right Quad Pos.',
        #     self.driveRightMaster.getQuadraturePosition())

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None
        self.leftMaxVel = 0
        self.rightMaxVel = 0
        self.leftMinVel = 0
        self.rightMinVel = 0

        # self.driveLeftMaster.config_kP(0, .3, 10)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)

        super().__init__()

    def moveToPosition(self, position, side='left'):

        if side == 'left':
            self.driveLeftMaster.setSafetyEnabled(False)
            self.driveLeftMaster.set(
                ctre.talonsrx.TalonSRX.ControlMode.Position, position)
        else:
            self.driveRightMaster.set(
                ctre.talonsrx.TalonSRX.ControlMode.Position, position)

    def stop(self):
        self.drive.stopMotor()

    def arcade(self, speed, rotation):
        self.updateSD()

        if self.robot.dStick.getRawButtonReleased(3):
            self.robotFrontToggleCount += 1
        """
        This if statement acts as a toggle to change which motors are 
        inverted, completely changing the "front" of the robot. This is
        useful for when we are about to climb.
        """
        if self.robotFrontToggleCount % 2 == 0:
            self.drive.arcadeDrive(speed, rotation, True)
        else:
            self.drive.arcadeDrive(-speed, rotation, True)

    def arcadeWithRPM(self, speed, rotation, maxRPM):

        if self.robot.dStick.getRawButtonReleased(3):
            self.robotFrontToggleCount += 1

        self.driveLeftMaster.setSafetyEnabled(False)

        XSpeed = wpilib.RobotDrive.limit(speed)
        XSpeed = self.applyDeadband(XSpeed, .02)

        ZRotation = wpilib.RobotDrive.limit(rotation)
        ZRotation = self.applyDeadband(ZRotation, .02)

        if self.robotFrontToggleCount % 2 == 1:
            XSpeed = -XSpeed

        XSpeed = math.copysign(XSpeed * XSpeed, XSpeed)
        ZRotation = math.copysign(ZRotation * ZRotation, ZRotation)

        maxInput = math.copysign(max(abs(XSpeed), abs(ZRotation)), XSpeed)

        if XSpeed >= 0.0:
            if ZRotation >= 0.0:
                leftMotorSpeed = maxInput
                rightMotorSpeed = XSpeed - ZRotation
            else:
                leftMotorSpeed = XSpeed + ZRotation
                rightMotorSpeed = maxInput
        else:
            if ZRotation >= 0.0:
                leftMotorSpeed = XSpeed + ZRotation
                rightMotorSpeed = maxInput
            else:
                leftMotorSpeed = maxInput
                rightMotorSpeed = XSpeed - ZRotation

        leftMotorSpeed = wpilib.RobotDrive.limit(leftMotorSpeed)
        rightMotorSpeed = wpilib.RobotDrive.limit(rightMotorSpeed)

        leftMotorRPM = leftMotorSpeed * maxRPM
        rightMotorRPM = rightMotorSpeed * maxRPM

        self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
                                 leftMotorRPM)
        self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
                                  rightMotorRPM)

    def updateSD(self):

        leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
        leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)

        rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
        rightPos = self.driveRightMaster.getSelectedSensorPosition(0)

        # keep the biggest velocity values
        if self.leftMaxVel < leftVel:
            self.leftMaxVel = leftVel

        if self.rightMaxVel < rightVel:
            self.rightMaxVel = rightVel

        # keep the smallest velocity values
        if self.leftMinVel > leftVel:
            self.leftMinVel = leftVel

        if self.rightMinVel > rightVel:
            self.rightMinVel = rightVel

        # calculate side deltas
        if self.leftVel:
            leftVelDelta = leftVel - self.leftVel
        else:
            leftVelDelta = 0

        if self.leftPos:
            leftPosDelta = leftPos - self.leftPos
        else:
            leftPosDelta = 0

        if self.rightVel:
            rightVelDelta = rightVel - self.rightVel
        else:
            rightVelDelta = 0

        if self.rightPos:
            rightPosDelta = rightPos - self.rightPos
        else:
            rightPosDelta = 0

        # calculate delta of delta
        differenceVel = leftVelDelta - rightVelDelta
        differencePos = leftPosDelta - rightPosDelta

        SD.putNumber("LeftSensorVel", leftVel)
        SD.putNumber("LeftSensorPos", leftPos)

        SD.putNumber("RightSensorVel", rightVel)
        SD.putNumber("RightSensorPos", rightPos)

        SD.putNumber('LeftVelDelta', leftVelDelta)
        SD.putNumber('LeftPosDelta', leftPosDelta)

        SD.putNumber('RightVelDelta', rightVelDelta)
        SD.putNumber('RightPosDelta', rightPosDelta)

        SD.putNumber('DifferenceVel', differenceVel)
        SD.putNumber('DifferencePos', differencePos)

        SD.putNumber('Left Max Vel', self.leftMaxVel)
        SD.putNumber('Right Max Vel', self.rightMaxVel)

        SD.putNumber('Left Min Vel', self.leftMinVel)
        SD.putNumber('Right Min Vel', self.rightMinVel)

        self.leftVel = leftVel
        self.leftPos = leftPos
        self.rightVel = rightVel
        self.rightPos = rightPos

        # kP = self.driveLeftMaster.configGetParameter(
        #     self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)

        # SmartDashboard.putNumber('Left Proportional', kP)

        # these may give the derivitive an integral of the PID once
        # they are set.  For now, they just show 0
        #SD.putNumber(
        #    'Left Derivative',
        #    self.driveLeftMaster.getErrorDerivative(0))
        #SD.putNumber(
        #    'Left Integral',
        #    self.driveLeftMaster.getIntegralAccumulator(0))
    def applyDeadband(self, value, deadband):
        """Returns 0.0 if the given value is within the specified range around zero. The remaining range
        between the deadband and 1.0 is scaled from 0.0 to 1.0.

        :param value: value to clip
        :param deadband: range around zero
        """
        if abs(value) > deadband:
            if value < 0.0:
                return (value - deadband) / (1.0 - deadband)
            else:
                return (value + deadband) / (1.0 - deadband)
        return 0.0
Example #10
0
class DriveTrain(Subsystem):
    '''
    'Tank Drive' system set up with 2 motors per side, one a "master"
    with a mag encoder attached and the other "slave" controller set
    to follow the "master".
    '''
    def __init__(self, robot):

        self.robot = robot

        # Initialize all controllers
        self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
        self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
        self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
        self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])

        wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
                                      self.driveLeftMaster)
        wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
                                      self.driveRightMaster)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        self.driveLeftSlave.setInverted(False)
        self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        self.driveLeftMaster.setQuadraturePosition(0, 0)
        self.driveRightMaster.setQuadraturePosition(0, 0)

        # Throw data on the SmartDashboard so we can work with it.
        # SD.putNumber(
        #     'Left Quad Pos.',
        #     self.driveLeftMaster.getQuadraturePosition())
        # SD.putNumber(
        #     'Right Quad Pos.',
        #     self.driveRightMaster.getQuadraturePosition())

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None
        self.leftMaxVel = 0
        self.rightMaxVel = 0

        # self.driveLeftMaster.config_kP(0, .3, 10)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)
        # self.drive = DifferentialDrive(self.driveLeftMaster,
        #                               self.driveRightMaster)

        wpilib.LiveWindow.addActuator("DriveTrain", "Left Master",
                                      self.driveLeftMaster)
        wpilib.LiveWindow.addActuator("DriveTrain", "Right Master",
                                      self.driveRightMaster)
        #wpilib.LiveWindow.add(self.driveLeftSlave)
        #wpilib.LiveWindow.add(self.driveRightSlave)
        #wpilib.Sendable.setName(self.drive, 'Drive')
        #wpilib.LiveWindow.add(self.drive)
        #wpilib.Sendable.setName(self.driveLeftMaster, 'driveLeftMaster')
        #wpilib.LiveWindow.add(self.driveRightMaster)

        super().__init__()

    def moveToPosition(self, position, side='left'):

        if side == 'left':
            self.driveLeftMaster.setSafetyEnabled(False)
            self.driveLeftMaster.set(
                ctre.talonsrx.TalonSRX.ControlMode.Position, position)
        else:
            self.driveRightMaster.set(
                ctre.talonsrx.TalonSRX.ControlMode.Position, position)

    def stop(self):
        self.drive.stopMotor()

    def arcade(self, speed, rotation):
        self.updateSD()
        self.drive.arcadeDrive(speed, rotation, True)

    def updateSD(self):

        leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
        leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)

        rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
        rightPos = self.driveRightMaster.getSelectedSensorPosition(0)

        # keep the biggest velocity values
        if self.leftMaxVel < leftVel:
            self.leftMaxVel = leftVel

        if self.rightMaxVel < rightVel:
            self.rightMaxVel = rightVel

        # calculate side deltas
        if self.leftVel:
            leftVelDelta = leftVel - self.leftVel
        else:
            leftVelDelta = 0

        if self.leftPos:
            leftPosDelta = leftPos - self.leftPos
        else:
            leftPosDelta = 0

        if self.rightVel:
            rightVelDelta = rightVel - self.rightVel
        else:
            rightVelDelta = 0

        if self.rightPos:
            rightPosDelta = rightPos - self.rightPos
        else:
            rightPosDelta = 0

        # calculate delta of delta
        differenceVel = leftVelDelta - rightVelDelta
        differencePos = leftPosDelta - rightPosDelta

        SD.putNumber("LeftSensorVel", leftVel)
        SD.putNumber("LeftSensorPos", leftPos)

        SD.putNumber("RightSensorVel", rightVel)
        SD.putNumber("RightSensorPos", rightPos)

        SD.putNumber('LeftVelDelta', leftVelDelta)
        SD.putNumber('LeftPosDelta', leftPosDelta)

        SD.putNumber('RightVelDelta', rightVelDelta)
        SD.putNumber('RightPosDelta', rightPosDelta)

        SD.putNumber('DifferenceVel', differenceVel)
        SD.putNumber('DifferencePos', differencePos)

        SD.putNumber('Left Max Vel', self.leftMaxVel)
        SD.putNumber('Right Max Vel', self.rightMaxVel)

        self.leftVel = leftVel
        self.leftPos = leftPos
        self.rightVel = rightVel
        self.rightPos = rightPos
Example #11
0
class DriveTrain(Subsystem):
    '''
    'Tank Drive' system set up with 2 motors per side, one a "master"
    with a mag encoder attached and the other "slave" controller set
    to follow the "master".
    '''
    def __init__(self, robot):

        self.robot = robot

        self.ahrs = AHRS.create_spi()
        self.ahrs.reset()

        #         self.angleAdjustment = self.ahrs.getAngle()
        #         self.ahrs.setAngleAdjustment(self.angleAdjustment)
        # Initialize all controllers
        self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
        self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
        self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
        self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])

        wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
                                      self.driveLeftMaster)
        wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
                                      self.driveRightMaster)

        # Connect the slaves to the masters on each side
        self.driveLeftSlave.follow(self.driveLeftMaster)
        self.driveRightSlave.follow(self.driveRightMaster)

        self.driveLeftMaster.configNominalOutputForward(0, 0)
        self.driveLeftMaster.configNominalOutputReverse(0, 0)

        self.driveRightMaster.configNominalOutputForward(0, 0)
        self.driveRightMaster.configNominalOutputReverse(0, 0)

        self.speed = .4
        self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
        self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveRightMaster.configPeakOutputForward(self.speed, 0)
        self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveLeftMaster.configClosedLoopRamp(.2, 0)
        self.driveRightMaster.configClosedLoopRamp(.2, 0)

        self.driveLeftMaster.setSafetyEnabled(False)
        self.driveRightMaster.setSafetyEnabled(False)

        # Makes sure both sides' controllers show green and use positive
        # values to move the bot forward.
        self.driveLeftSlave.setInverted(False)
        self.driveLeftMaster.setInverted(False)
        self.driveRightSlave.setInverted(True)
        self.driveRightMaster.setInverted(True)

        self.PID()
        """
        Initializes the count for toggling which side of the 
        robot will be considered the front when driving.
        """
        self.robotFrontToggleCount = 2

        # Configures each master to use the attached Mag Encoders
        self.driveLeftMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)
        self.driveRightMaster.configSelectedFeedbackSensor(
            ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
            0)

        # Reverses the encoder direction so forward movement always
        # results in a positive increase in the encoder ticks.
        self.driveLeftMaster.setSensorPhase(True)
        self.driveRightMaster.setSensorPhase(True)

        self.driveLeftMaster.setSelectedSensorPosition(0, 0, 0)
        self.driveRightMaster.setSelectedSensorPosition(0, 0, 0)

        # these supposedly aren't part of the WPI_TalonSRX class
        # self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
        # self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)

        # Throw data on the SmartDashboard so we can work with it.
        # SD.putNumber(
        #     'Left Quad Pos.',
        #     self.driveLeftMaster.getQuadraturePosition())
        # SD.putNumber(
        #     'Right Quad Pos.',
        #     self.driveRightMaster.getQuadraturePosition())

        self.leftVel = None
        self.leftPos = None
        self.rightVel = None
        self.rightPos = None

        # self.driveLeftMaster.config_kP(0, .3, 10)

        self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
        self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
        self.driveControllerRight.setInverted(True)
        self.drive = DifferentialDrive(self.driveControllerLeft,
                                       self.driveControllerRight)
        self.drive.setSafetyEnabled(False)

        self.previousError = 0

        super().__init__()

    def autoInit(self):
        self.speed = .5
        self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
        self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveRightMaster.configPeakOutputForward(self.speed, 0)
        self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveLeftMaster.config_kP(0, .115, 0)
        self.driveRightMaster.config_kP(0, .115, 0)

        #         self.driveLeftMaster.config_kP(0, .185, 0)
        #         self.driveRightMaster.config_kP(0, .185, 0)

        #         self.driveLeftMaster.config_kP(0, 20, 0)
        #         self.driveRightMaster.config_kP(0, 20, 0)

        self.driveLeftMaster.config_kF(0, 0.0, 0)
        self.driveRightMaster.config_kF(0, 0.0, 0)

    def teleInit(self):
        self.speed = .55
        self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
        self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveRightMaster.configPeakOutputForward(self.speed, 0)
        self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)

        self.driveLeftMaster.config_kP(0, 0.0, 0)
        self.driveRightMaster.config_kP(0, 0.0, 0)

        self.driveLeftMaster.config_kF(0, 0.313, 0)
        self.driveRightMaster.config_kF(0, 0.313, 0)

    def moveToPosition(self, position):
        self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
                                 position)

        self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
                                  position)

    def stop(self):
        self.drive.stopMotor()

    def arcade(self, speed, rotation):
        #         self.updateSD()

        if self.robot.dStick.getRawButtonReleased(3):
            self.robotFrontToggleCount += 1
        """
        This if statement acts as a toggle to change which motors are 
        inverted, completely changing the "front" of the robot. This is
        useful for when we are about to climb.
        """
        if self.robotFrontToggleCount % 2 == 0:
            self.drive.arcadeDrive(speed, rotation, True)
        else:
            self.drive.arcadeDrive(-speed, rotation, True)

    def arcadeWithRPM(self, speed, rotation, maxRPM):
        #         self.updateSD()
        self.driveLeftMaster.setSafetyEnabled(False)

        if self.robot.dStick.getRawButtonReleased(3):
            self.robotFrontToggleCount += 1

        if self.robotFrontToggleCount % 2 == 0:
            XSpeed = wpilib.RobotDrive.limit(speed)
        else:
            XSpeed = wpilib.RobotDrive.limit(-speed)

        XSpeed = self.applyDeadband(XSpeed, .02)

        ZRotation = wpilib.RobotDrive.limit(rotation)
        ZRotation = self.applyDeadband(ZRotation, .02)

        XSpeed = math.copysign(XSpeed * XSpeed, XSpeed)
        ZRotation = math.copysign(ZRotation * ZRotation, ZRotation)

        maxInput = math.copysign(max(abs(XSpeed), abs(ZRotation)), XSpeed)

        if XSpeed >= 0.0:
            if ZRotation >= 0.0:
                leftMotorSpeed = maxInput
                rightMotorSpeed = XSpeed - ZRotation
            else:
                leftMotorSpeed = XSpeed + ZRotation
                rightMotorSpeed = maxInput
        else:
            if ZRotation >= 0.0:
                leftMotorSpeed = XSpeed + ZRotation
                rightMotorSpeed = maxInput
            else:
                leftMotorSpeed = maxInput
                rightMotorSpeed = XSpeed - ZRotation

        leftMotorSpeed = wpilib.RobotDrive.limit(leftMotorSpeed)
        rightMotorSpeed = wpilib.RobotDrive.limit(rightMotorSpeed)

        leftMotorRPM = leftMotorSpeed * maxRPM
        rightMotorRPM = rightMotorSpeed * maxRPM

        self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
                                 leftMotorRPM)
        self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
                                  rightMotorRPM)

    def updateSD(self):

        leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
        leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)

        rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
        rightPos = self.driveRightMaster.getSelectedSensorPosition(0)

        # calculate side deltas
        if self.leftVel:
            leftVelDelta = leftVel - self.leftVel
        else:
            leftVelDelta = 0

        if self.leftPos:
            leftPosDelta = leftPos - self.leftPos
        else:
            leftPosDelta = 0

        if self.rightVel:
            rightVelDelta = rightVel - self.rightVel
        else:
            rightVelDelta = 0

        if self.rightPos:
            rightPosDelta = rightPos - self.rightPos
        else:
            rightPosDelta = 0

        # calculate delta of delta
        differenceVel = leftVelDelta - rightVelDelta
        differencePos = leftPosDelta - rightPosDelta

        SD.putNumber("LeftSensorVel", leftVel)
        SD.putNumber("LeftSensorPos", leftPos)

        SD.putNumber("RightSensorVel", rightVel)
        SD.putNumber("RightSensorPos", rightPos)

        SD.putNumber('LeftVelDelta', leftVelDelta)
        SD.putNumber('LeftPosDelta', leftPosDelta)

        SD.putNumber('RightVelDelta', rightVelDelta)
        SD.putNumber('RightPosDelta', rightPosDelta)

        SD.putNumber('DifferenceVel', differenceVel)
        SD.putNumber('DifferencePos', differencePos)

        SD.putNumber('Angle', self.ahrs.getAngle())
        SD.putNumber('Angle Adjustment', self.ahrs.getAngleAdjustment())

        self.leftVel = leftVel
        self.leftPos = leftPos
        self.rightVel = rightVel
        self.rightPos = rightPos

        # kP = self.driveLeftMaster.configGetParameter(
        #     self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)

        # SmartDashboard.putNumber('Left Proportional', kP)

        # these may give the derivitive an integral of the PID once
        # they are set.  For now, they just show 0
        #SD.putNumber(
        #    'Left Derivative',
        #    self.driveLeftMaster.getErrorDerivative(0))
        #SD.putNumber(
        #    'Left Integral',
        #    self.driveLeftMaster.getIntegralAccumulator(0))

    def applyDeadband(self, value, deadband):
        """Returns 0.0 if the given value is within the specified range around zero. The remaining range
        between the deadband and 1.0 is scaled from 0.0 to 1.0.

        :param value: value to clip
        :param deadband: range around zero
        """
        if abs(value) > deadband:
            if value < 0.0:
                return (value - deadband) / (1.0 - deadband)
            else:
                return (value + deadband) / (1.0 - deadband)
        return 0.0

    def setAngle(self, angle, tolerance):
        #self.tolerance = tolerance
        #self.calculateAdjustedSetpoint(angle)
        self.turnController.setSetpoint(angle)

        if ((self.ahrs.getYaw() <= (angle + tolerance))
                and (self.ahrs.getYaw() >= (angle - tolerance))):
            self.turnController.disable()

            self.driveLeftMaster.set(0)
            self.driveRightMaster.set(0)

        else:
            self.turnController.enable()

            self.drive.arcadeDrive(0, self.output)

            #self.leftTurnController.setSetpoint(angle)

    def isInGyroPosition(self):
        SD.putNumber('Is in gyro position',
                     ((self.ahrs.getYaw() <=
                       (self.turnController.getSetpoint() +
                        self.robot.autonomous.ANGLE_TOLERANCE)) and
                      (self.ahrs.getYaw() >=
                       (self.turnController.getSetpoint() -
                        self.robot.autonomous.ANGLE_TOLERANCE))))
        return ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() +
                                        self.robot.autonomous.ANGLE_TOLERANCE))
                and (self.ahrs.getYaw() >=
                     (self.turnController.getSetpoint() -
                      self.robot.autonomous.ANGLE_TOLERANCE)))

    def calculateAdjustedSetpoint(self, angle):
        self.startingYaw = self.robot.autonomous.startingYaw
        adjustedAngle = angle + self.startingYaw

        if adjustedAngle < -180:
            undershot = adjustedAngle + 180
            adjustedAngle = 180 + undershot

        elif adjustedAngle > 180:
            overshot = adjustedAngle - 180
            adjustedAngle = -180 + overshot

        self.adjustedSetpoint = adjustedAngle

    def PID(self):
        self.kP = .045
        self.kI = 0.00
        self.kD = 0.00
        self.kF = 0.00

        self.turnController = wpilib.PIDController(self.kP,
                                                   self.kI,
                                                   self.kD,
                                                   self.kF,
                                                   self.ahrs,
                                                   output=self)

        self.turnController.setInputRange(-180, 180)
        self.turnController.setOutputRange(-0.55, 0.55)

        self.turnController.disable()

    def pidWrite(self, output):
        self.output = output