def start(self):
# Setting up the left controller
self.left_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_left_encoder,
self.left_output)
self.left_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.left_holder,
output=self.left_holder,
period=self.period)
self.left_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.left_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.left_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.left_pid_contorller.setContinuous(True)
# Setting up the right controller
self.right_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_right_encoder,
self.right_output)
self.right_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.right_holder,
output=self.right_holder,
period=self.period)
self.right_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.right_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.right_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.right_pid_contorller.setContinuous(True)
# Setting up the turn controller
self.turn_holder = IOGyroHolder(self.chassis.get_angle,
self.turn_output)
self.turn_pid_contorller = PIDController(0.02,
self.ki,
self.kd,
self.kf,
source=self.turn_holder,
output=self.turn_holder,
period=self.period)
self.turn_pid_contorller.setInputRange(0, 360)
self.turn_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.turn_pid_contorller.setAbsoluteTolerance(10)
self.turn_pid_contorller.setContinuous(True)
def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(0.75, 0.02, 1.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(self.distance_pid_abs_error)
self.distance_pid.setToleranceBuffer(3)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-5.0, 5.0)
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
self.logger = logging.getLogger("chassis")
def config(self, robot):
self.pid = PIDController(*self.get_pid(robot.dashboard),
self.get_disalignment, self.output)
self.pid.setOutputRange(-1, 1)
self.pid.enable()
self.pid.setSetpoint(0)
self.turn = 0
self.robot = robot
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def __init__(self):
self.angle = None
self.angle_pid_controller = PIDController(Kp=self.kP,
Ki=self.kI,
Kd=self.kD,
Kf=self.kF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.rate, self.rate)
self.nt = NetworkTables.getTable('limelight')
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=3.0,
Ki=0.0,
Kd=5.0,
source=self.imu.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
alphas = []
ls = []
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
alphas.append(module_dist)
module_angle = math.atan2(module.y_pos, module.x_pos)
ls.append(module_angle)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.read_steer_pos()
self.icre = ICREstimator(np.zeros(shape=(3, 1)), np.array(alphas),
np.array(ls), np.zeros(shape=(4, )))
# TODO: re-enable if we end up not using callback method
self.imu.imu.ahrs.registerCallback(self.update_odometry)
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=0.1,
Ki=0.0,
Kd=0.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
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):
super().__init__(self.get_position, 'position_controller')
self.set_abs_output_range(0.18, 0.7)
# Angle correction PID controller - used to maintain a straight
# heading while the encoders track distance traveled.
self._angle_offset = 0
self.angle_pid_controller = PIDController(Kp=self.kAngleP,
Ki=self.kAngleI,
Kd=self.kAngleD,
Kf=self.kAngleF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.kAngleMax,
self.kAngleMax)
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-0.7, 0.7)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0, Ki=0.0, Kd=0.2,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1/50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-3, 3)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [self.module_a, self.module_b, self.module_c, self.module_d]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
logging.debug(
"Turn to angle constructed with angle {}".format(setpoint))
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def __init__(self, pid_values: PIDValue, f_in, f_out, f_feedforwards=None, pid_key=None):
self.ff_enabled = f_feedforwards != None
if self.ff_enabled:
ff = f_feedforwards
else:
ff = lambda x: 0
self.pid_dash_enabled = pid_key != None
if self.pid_dash_enabled:
self.pid_key = pid_key
else:
self.pid_key = ""
self.pid_values = pid_values
self.pid_controller = PIDController(
0, 0, 0,
f_in,
lambda x: f_out(x + ff(self.get_target(), x))
)
self.pid_values.update_controller(self.pid_controller)
def __init__(self, kP, kI, kD, kF, tolerance, encoder_function,
update_function, direction, period, is_enabled):
self.is_enabled = is_enabled
self.direction = direction
self.encoder_function = encoder_function
self.motor_output = 0
self.pid_controller = PIDController(kP,
kI,
kD,
kF,
source=self,
output=self,
period=period)
#self.pid_controller.controlLoop._thread.setDaemon(True)
self.pid_controller.setInputRange(-5, 5)
self.pid_controller.setOutputRange(-1.0, 1.0)
self.pid_controller.setAbsoluteTolerance(tolerance)
self.update_function = update_function
self.pid_controller.setContinuous(True)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.input = 0
self.input2 = 0
self.lastCargoCommand = "unknown"
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
self.F = 0
#should be 0.4
SmartDashboard.putNumber("F Gain", self.F)
[self.kP, self.kI, self.kD, self.kF] = [0, 0, 0, 0]
cargoController = PIDController(self.kP, self.kI, self.kD, self.kF,
self, self)
self.cargoController = cargoController
self.cargoController.disable()
self.pidValuesForMode = {
"resting": [-50, self.kP, self.kI, self.kD, 0.15 / -50],
"cargoship": [-28, self.kP, self.kI, self.kD, 0.0],
"intake": [50, self.kP, self.kI, self.kD, 0.0],
"rocket": [5, self.kP, self.kI, self.kD, 0.19 / 5],
}
def __init__(self):
print('TankDrive: init called')
super().__init__('TankDrive')
self.debug = False
self.logPrefix = "TankDrive: "
# Speed controllers
if robotmap.driveLine.speedControllerType == "VICTORSP":
try:
self.leftSpdCtrl = wpilib.VictorSP(robotmap.driveLine.leftMotorPort)
if robotmap.driveLine.invertLeft:
self.leftSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating left speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightSpdCtrl = wpilib.VictorSP(robotmap.driveLine.rightMotorPort)
if robotmap.driveLine.invertRight:
self.rightSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating right speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
elif robotmap.driveLine.speedControllerType == "TALON":
try:
self.leftSpdCtrl = wpilib.Talon(robotmap.driveLine.leftMotorPort)
if robotmap.driveLine.invertLeft:
self.leftSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating left speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightSpdCtrl = wpilib.Talon(robotmap.driveLine.rightMotorPort)
if robotmap.driveLine.invertRight:
self.rightSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating right speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
else:
print("{}Configured speed controller type in robotmap not recognized - {}".format(self.logPrefix, robotmap.driveLine.speedControllerType))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise RuntimeError('Driveline speed controller specified in robotmap not valid: ' + robotmap.driveLine.speedControllerType)
# Encoders
try:
self.leftEncoder = wpilib.Encoder(robotmap.driveLine.leftEncAPort, robotmap.driveLine.leftEncBPort,
robotmap.driveLine.leftEncReverse, robotmap.driveLine.leftEncType)
self.leftEncoder.setDistancePerPulse(robotmap.driveLine.inchesPerTick)
except Exception as e:
print("{}Exception caught instantiating left encoder. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightEncoder = wpilib.Encoder(robotmap.driveLine.rightEncAPort, robotmap.driveLine.rightEncBPort,
robotmap.driveLine.rightEncReverse, robotmap.driveLine.rightEncType)
self.rightEncoder.setDistancePerPulse(robotmap.driveLine.inchesPerTick)
except Exception as e:
print("{}Exception caught instantiating left encoder. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
# PID Setup
self.tankPID = TankPID()
self.pidController = PIDController(0.0, 0.0, 0.0, self.tankPID, self.tankPID)
self.tankPIDIndirect = TankPIDIndirect()
self.pidControllerIndirect = PIDController(0.0, 0.0, 0.0, self.tankPIDIndirect, self.tankPIDIndirect)
self.turnPID = TankPIDTurn(0.0, 0.5)
self.pidTurnController = PIDController(0.0, 0.0, 0.0, self.turnPID, self.turnPID)
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
