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