def __init__(self): super().__init__() self.l_motor = SyncGroup(Talon, constants.motor_drive_l) self.r_motor = SyncGroup(Talon, constants.motor_drive_r) self.l_encoder = Encoder(*constants.encoder_drive_l) self.r_encoder = Encoder(*constants.encoder_drive_r) DISTANCE_PER_REV = 4 * math.pi TICKS_PER_REV = 128 REDUCTION = 30 / 36 self.l_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV) self.r_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV) self.gyro = Gyro(constants.gyro) self._gyro_p = 0.12 self._gyro_d = 0.005 self._prev_gyro_error = 0 quickdebug.add_printables(self, [ ('gyro angle', self.gyro.getAngle), ('left encoder', self.l_encoder.getDistance), ('right encoder', self.r_encoder.getDistance), 'left_pwm', 'right_pwm', 'encoder_goal' ]) quickdebug.add_tunables(self, ['_gyro_p', '_gyro_d'])
def __init__(self, Robot): #self.navx = Robot.navx self.speedLimit = 0.999 self._leftRamp = 0.0 self._rightRamp = 0.0 self._rampSpeed = 6.0 self._kOonBalanceAngleThresholdDegrees = 5.0 self._autoBalance = True self._quickstop_accumulator = 0.0 self._old_wheel = 0.0 self._driveReversed = True self._drivePool = DataPool("Drive") # setup drive train motors self.rightDrive = SyncGroup(RIGHT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.leftDrive = SyncGroup(LEFT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.rightDrive.setInverted(True) # setup drive train gear shifter self.shifter = Solenoid(DRIVE_SHIFTER_PORT) self.shifter.set(False) # setup drive train encoders self.leftEncoder = Encoder(LEFT_DRIVE_ENCODER_A, LEFT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RIGHT_DRIVE_ENCODER_A, RIGHT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.leftEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) self.leftEncoder.setReverseDirection(True) self.rightEncoder.setReverseDirection(False) self.rightEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK)
def __init__(self, l_a=0, l_b=1, r_a=2, r_b=3): self.left = Encoder(l_a, l_b, reverseDirection=True) self.right = Encoder(r_a, r_b) self.left.setDistancePerPulse(self.dpp) self.right.setDistancePerPulse(self.dpp)
def __init__(self): self.robot = SpartanRobot.getRobotObject(self) self.setDefaultCommand(ArcadeDriveCommand()) #Power output to motors in range of -1 to 1 self.leftPower = 0 self.rightPower = 0 self.leftEncoder = Encoder(RobotMap.leftDriveEncoder1, RobotMap.leftDriveEncoder2, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RobotMap.rightDriveEncoder1, RobotMap.rightDriveEncoder2, True, Encoder.EncodingType.k4X) self.gyro = ADXRS450_Gyro() self.rightDriveMotor1 = VictorSP(RobotMap.rightDriveMotor1) self.rightDriveMotor2 = VictorSP(RobotMap.rightDriveMotor2) # self.rightDriveMotor3 = VictorSP(rightDriveMotor3) self.leftDriveMotor1 = VictorSP(RobotMap.leftDriveMotor1) self.leftDriveMotor2 = VictorSP(RobotMap.leftDriveMotor2) # self.leftDriveMotor3 = VictorSP(leftDriveMotor3) self.leftEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks) self.rightEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks) self.leftEncoder.setMaxPeriod(5) self.rightEncoder.setMaxPeriod(5) self.leftEncoder.setMinRate(0) self.rightEncoder.setMinRate(0) self.leftEncoder.setSamplesToAverage(1) self.rightEncoder.setSamplesToAverage(1) self.gyro.calibrate()
def __init__(self, robot): super().__init__("Drive") self.robot = robot self.peakCurrentLimit = 25 self.PeaKDuration = 50 self.continuousLimit = 15 self.map = robot.RobotMap # drive motor, sensors, and pistons motor = {} pistons = {} # create all drive motors, Sensors, and pistons for name in self.map.motorMap.motors: motor[name] = robot.Creator.createMotor(self.map.motorMap.motors[name]) for name in self.map.PneumaticMap.pistons: if name == 'Shifter': pistons[name] = robot.Creator.createPistons(self.map.PneumaticMap.pistons[name]) self.REnc = Encoder(0, 1, True, Encoder.EncodingType.k4X) self.LEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X) self.Gyro = ADXRS450_Gyro() # make motors, Sensors, and pistons local to subsystem self.Dmotor = motor self.Dpiston = pistons for name in self.Dmotor: self.Dmotor[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.Dmotor[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.Dmotor[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.Dmotor[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.Dmotor[name].enableCurrentLimit(True) self.KtorqueMode = DoubleSolenoid.Value.kReverse self.KspeedMode = DoubleSolenoid.Value.kForward if wpilib.RobotBase.isSimulation(): self.kp = 0.001 self.ki = 0.00001 self.kd = 0.000000001 else: self.kp = 0.001 self.ki = 0.00001 self.kd = 0.000000001 self.DrivePID = PID(self.kp, self.ki, self.kd)
def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag
def __init__(self, robot): super().__init__("shooter") self.robot = robot self.counter = 0 # used for updating the log self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional # motor controllers self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.spark_hood = Talon(5) self.spark_feed = Talon(7) # encoders and PID controllers self.hood_encoder = Encoder( 4, 5) # generic encoder - we'll have to install one on the 775 motor self.hood_encoder.setDistancePerPulse(1 / 1024) self.hood_controller = wpilib.controller.PIDController(Kp=0.005, Ki=0, Kd=0.0) self.hood_setpoint = 5 self.hood_controller.setSetpoint(self.hood_setpoint) self.flywheel_encoder = self.sparkmax_flywheel.getEncoder( ) # built-in to the sparkmax/neo self.flywheel_controller = self.sparkmax_flywheel.getPIDController( ) # built-in PID controller in the sparkmax # limit switches, use is TBD self.limit_low = DigitalInput(6) self.limit_high = DigitalInput(7)
def __init__(self): """Assign ports and save them for use in the move and stop methods.""" super().__init__() self.arm = ctre.WPI_TalonSRX(11) self.armencoder = Encoder(0, 1) self.armencoder.setDistancePerPulse(0.14)
def __init__(self, robot): Subsystem.__init__(self, 'Turret') self.robot = robot self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X) motors = {} self.map = self.robot.botMap self.tmotors = motors for name in self.map.motorMap.motors: motors[name] = robot.Creator.createMotor( self.map.motorMap.motors[name]) for name in self.tmotors: self.tmotors[name].setInverted( self.map.motorMap.motors[name]['inverted']) self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast) self.kP = 0.05 self.kI = 0.000 self.kD = 0.002 self.turretPID = PID(self.kP, self.kI, self.kD) self.turretPID.limitVal(0.3) self.setPow = 0
def __init__(self, robot): super().__init__("Arm") self.robot = robot self.peakCurrentLimit = 30 self.PeaKDuration = 50 self.continuousLimit = 15 motor = {} for name in self.robot.RobotMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name]) self.motors = motor for name in self.motors: self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.motors[name].enableCurrentLimit(True) self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X) self.Zero = DigitalInput(6) self.kp = 0.00035 self.ki = 0.00000000001 self.kd = 0.0000001 self.ArmPID = PID(self.kp, self.ki, self.kd)
def init(): global drive_left_encoder, drive_right_encoder, elevator_encoder, gyro, back_photosensor, side_photosensor, pdp drive_left_encoder = Encoder(6, 7) drive_left_encoder.setDistancePerPulse(_drive_encoder_scale()) drive_right_encoder = Encoder(4, 5) drive_right_encoder.setDistancePerPulse(_drive_encoder_scale()) elevator_encoder = Encoder(0, 1, True) elevator_encoder.setDistancePerPulse(_elevator_encoder_scale()) gyro = Gyro(0) back_photosensor = DigitalInput(8) side_photosensor = DigitalInput(3) pdp = PowerDistributionPanel()
def __init__(self): super().__init__() # Create the motor controllers and their respective speed controllers. self.leftMotors = SpeedControllerGroup( PWMSparkMax(constants.kLeftMotor1Port), PWMSparkMax(constants.kLeftMotor2Port), ) self.rightMotors = SpeedControllerGroup( PWMSparkMax(constants.kRightMotor1Port), PWMSparkMax(constants.kRightMotor2Port), ) # Create the differential drivetrain object, allowing for easy motor control. self.drive = DifferentialDrive(self.leftMotors, self.rightMotors) # Create the encoder objects. self.leftEncoder = Encoder( constants.kLeftEncoderPorts[0], constants.kLeftEncoderPorts[1], constants.kLeftEncoderReversed, ) self.rightEncoder = Encoder( constants.kRightEncoderPorts[0], constants.kRightEncoderPorts[1], constants.kRightEncoderReversed, ) # Configure the encoder so it knows how many encoder units are in one rotation. self.leftEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) self.rightEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) # Create the gyro, a sensor which can indicate the heading of the robot relative # to a customizable position. self.gyro = ADXRS450_Gyro() # Create the an object for our odometry, which will utilize sensor data to # keep a record of our position on the field. self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d()) # Reset the encoders upon the initilization of the robot. self.resetEncoders()
def __init__(self): super().__init__("Chassis") self.spark_L1 = Spark(robotmap.spark_L1) self.spark_L2 = Spark(robotmap.spark_L2) self.spark_R1 = Spark(robotmap.spark_R1) self.spark_R2 = Spark(robotmap.spark_R2) self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2) self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2) self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R) self.gyro = ADXRS450_Gyro(robotmap.gyro) self.encoder_L = Encoder(0, 1) self.encoder_R = Encoder(2, 3) self.dist_pulse_L = pi * 6 / 2048 self.dist_pulse_R = pi * 6 / 425
def __init__(self, robot): Subsystem.__init__(self, 'Drive') self.robot = robot motors = {} pistons = {} self.map = self.robot.botMap self.rEnc = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.lEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X) self.Gyro = ADXRS450_Gyro() for name in self.map.motorMap.motors: motors[name] = robot.Creator.createMotor( self.map.motorMap.motors[name]) for name in self.robot.botMap.PneumaticMap.pistons: if name == 'Shifter': pistons[name] = self.robot.Creator.createPistons( self.robot.botMap.PneumaticMap.pistons[name]) self.dMotors = motors self.dPistons = pistons for name in self.dMotors: self.dMotors[name].setInverted( self.robot.botMap.motorMap.motors[name]['inverted']) self.dMotors[name].setNeutralMode(ctre.NeutralMode.Coast) if self.map.motorMap.motors[name]['CurLimit'] is True: self.dMotors[name].configStatorCurrentLimit( self.robot.Creator.createCurrentConfig( self.robot.botMap.currentConfig['Drive']), 40) self.kP = 0.0 self.kI = 0.0 self.kD = 0.0 self.DrivePID = PID(self.kP, self.kI, self.kD)
def __init__(self, Robot): self._intake = Robot.intake self._drive = Robot.drive self._ramp = 0 self._rampSpeed = 6 self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX) self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX) self._LiftMotorLeft.setInverted(True) self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False, Encoder.EncodingType.k4X) self._liftEncoder.setDistancePerPulse(1) self._liftHallaffect = DigitalInput(HALL_DIO) self.zeroEncoder() self._liftPID = MiniPID(*LIFT_PID) self._liftPID.setOutputLimits(-0.45, 1) self.disableSpeedLimit = False
def __init__(self, operator: OperatorControl): self.operator = operator self.motor = WPI_VictorSPX(7) self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.encoder.setIndexSource(2) self.limit = DigitalInput(4) self.dashboard = NetworkTables.getTable("SmartDashboard") self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1 self.targetValue = 10 self.realValue = 0 self.allowedError = 10 self.hole = 4 self.holeOffset = 36 - 15 self.maxspeed = .25 self.minspeed = .1 self.speedmult = 1/300 self.speed = .1
def __init__(self, robot): Subsystem.__init__(self, 'Flywheel') self.CurPos = 0 self.PasPos = 0 self.robot = robot self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X) self.map = self.robot.botMap motor = {} piston = {} for name in self.robot.botMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor( self.robot.botMap.motorMap.motors[name]) for name in self.robot.botMap.PneumaticMap.pistons: piston[name] = self.robot.Creator.createPistons( self.robot.botMap.PneumaticMap.pistons[name]) self.fmotor = motor self.fpiston = piston for name in self.fmotor: self.fmotor[name].setInverted( self.robot.botMap.motorMap.motors[name]['inverted']) self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast) if self.map.motorMap.motors[name]['CurLimit'] is True: self.fmotor[name].configStatorCurrentLimit( self.robot.Creator.createCurrentConfig( self.robot.botMap.currentConfig['Fly']), 40) self.kP = 0.008 self.kI = 0.00002 self.kD = 0.00018 self.kF = 0.0 # tune me when testing self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF) self.flywheelPID.MaxIn(680) self.flywheelPID.MaxOut(1) self.flywheelPID.limitVal(0.95) # Limit PID output power to 50% self.feetToRPS = 51.111
def robotInit(self): # Right Motors self.RightMotor1 = WPI_TalonFX(1) self.RightMotor2 = WPI_TalonFX(2) self.RightMotor3 = WPI_TalonFX(3) self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1, self.RightMotor2, self.RightMotor3) # Left Motors self.LeftMotor1 = WPI_TalonFX(4) self.LeftMotor2 = WPI_TalonFX(5) self.LeftMotor3 = WPI_TalonFX(6) self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1, self.LeftMotor2, self.LeftMotor3) self.drive = DifferentialDrive(self.leftDriveMotors, self.rightDriveMotors) self.testEncoder = Encoder(1, 2, 3) self.dashboard = NetworkTables.getTable("SmartDashboard")
def __init__(self): super().__init__() self._motor = SyncGroup(Talon, constants.motor_elevator) self._position_encoder = Encoder(*constants.encoder_elevator) self._photosensor = DigitalInput(constants.photosensor) self._stabilizer_piston = Solenoid(constants.solenoid_dropper) self._position_encoder.setDistancePerPulse( (PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION) # Trajectory controlling stuff self._follower = TrajectoryFollower() self.assure_tote = CircularBuffer(5) self.auto_stacking = True # Do the dew self._tote_count = 0 # Keep track of totes! self._has_bin = False # Do we have a bin on top? self._new_stack = True # starting a new stack? self._tote_first = False # Override bin first to grab totes before anything else self._should_drop = False # Are we currently trying to get a bin ? self._close_stabilizer = True # Opens the stabilizer manually self.force_stack = False # manually actuates the elevator down and up self._follower.set_goal(Setpoints.BIN) # Base state self._follower_thread = Thread(target=self.update_follower) self._follower_thread.start() self._auton = False quickdebug.add_tunables(Setpoints, ["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"]) quickdebug.add_printables( self, [('position', self._position_encoder.getDistance), ('photosensor', self._photosensor.get), "has_bin", "tote_count", "tote_first", "at_goal", "has_game_piece", "auto_stacking"])
def __init__(self, robot): super().__init__('Drive') SmartDashboard.putNumber("DriveStraight_P", 0.075) SmartDashboard.putNumber("DriveStraight_I", 0.0) SmartDashboard.putNumber("DriveStraight_D", 0.42) # OLD GAINS 0.075, 0, 0.42 SmartDashboard.putNumber("DriveAngle_P", 0.009) SmartDashboard.putNumber("DriveAngle_I", 0.0) SmartDashboard.putNumber("DriveAngle_D", 0.025) SmartDashboard.putNumber("DriveStraightAngle_P", 0.025) SmartDashboard.putNumber("DriveStraightAngle_I", 0.0) SmartDashboard.putNumber("DriveStraightAngle_D", 0.01) self.driveStyle = "Tank" SmartDashboard.putString("DriveStyle", self.driveStyle) #SmartDashboard.putData("Mode", self.mode) self.robot = robot self.lime = self.robot.limelight self.nominalPID = 0.15 self.nominalPIDAngle = 0.22 # 0.11 - v2 self.preferences = Preferences.getInstance() timeout = 0 TalonLeft = Talon(map.driveLeft1) TalonRight = Talon(map.driveRight1) leftInverted = True rightInverted = False TalonLeft.setInverted(leftInverted) TalonRight.setInverted(rightInverted) VictorLeft1 = Victor(map.driveLeft2) VictorLeft2 = Victor(map.driveLeft3) VictorLeft1.follow(TalonLeft) VictorLeft2.follow(TalonLeft) VictorRight1 = Victor(map.driveRight2) VictorRight2 = Victor(map.driveRight3) VictorRight1.follow(TalonRight) VictorRight2.follow(TalonRight) for motor in [VictorLeft1, VictorLeft2]: motor.clearStickyFaults(timeout) motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.setInverted(leftInverted) for motor in [VictorRight1, VictorRight2]: motor.clearStickyFaults(timeout) motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.setInverted(rightInverted) for motor in [TalonLeft, TalonRight]: motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.clearStickyFaults(timeout) #Clears sticky faults motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor motor.configPeakCurrentLimit( 70, timeout) #20 Amps during Peak Duration motor.configPeakCurrentDuration( 300, timeout) #Peak Current for max 100 ms motor.enableCurrentLimit(True) motor.configVoltageCompSaturation(12, timeout) #Sets saturation value motor.enableVoltageCompensation( True) #Compensates for lower voltages motor.configOpenLoopRamp(0.2, timeout) #number of seconds from 0 to 1 self.left = TalonLeft self.right = TalonRight self.navx = navx.AHRS.create_spi() self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1]) self.leftEncoder.setDistancePerPulse(self.leftConv) self.leftEncoder.setSamplesToAverage(10) self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1]) self.rightEncoder.setDistancePerPulse(self.rightConv) self.rightEncoder.setSamplesToAverage(10) self.zero() #PID for Drive self.TolDist = 0.1 #feet [kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00] if wpilib.RobotBase.isSimulation(): [kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00] distController = PIDController(kP, kI, kD, kF, source=self.__getDistance__, output=self.__setDistance__) distController.setInputRange(0, 50) #feet distController.setOutputRange(-0.6, 0.6) distController.setAbsoluteTolerance(self.TolDist) distController.setContinuous(False) self.distController = distController self.distController.disable() '''PID for Angle''' self.TolAngle = 2 #degrees [kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00] if RobotBase.isSimulation(): [kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00] angleController = PIDController(kP, kI, kD, kF, source=self.__getAngle__, output=self.__setAngle__) angleController.setInputRange(-180, 180) #degrees angleController.setOutputRange(-0.5, 0.5) angleController.setAbsoluteTolerance(self.TolAngle) angleController.setContinuous(True) self.angleController = angleController self.angleController.disable() self.k = 1 self.sensitivity = 1 SmartDashboard.putNumber("K Value", self.k) SmartDashboard.putNumber("sensitivity", self.sensitivity) self.prevLeft = 0 self.prevRight = 0