def __init__(self, drive, gyro, encoderY):
self.drive = drive
self.gyro = gyro
self.encoderY = encoderY
self.jDeadband = 0.06
self.sd = NetworkTables.getTable('SmartDashboard')
# PID loop for angle
self.pidAngleDefault = {'p': 0.01, 'i': 0, 'd': 0.004}
self.sd.putNumber('pidAngleP', self.pidAngleDefault['p'])
self.sd.putNumber('pidAngleI', self.pidAngleDefault['i'])
self.sd.putNumber('pidAngleD', self.pidAngleDefault['d'])
self.pidAngle = wpilib.PIDController(self.pidAngleDefault['p'], self.pidAngleDefault['i'], self.pidAngleDefault['d'], self.gyro, self.updateAnglePID)
self.pidAngle.setAbsoluteTolerance(2)
self.pidRotateRate = 0
self.wasRotating = False
# PID loop for Cartesian Y direction
self.pidYDefault = {'p': 0.15, 'i': 0, 'd': 0.05}
self.sd.putNumber('pidYP', self.pidYDefault['p'])
self.sd.putNumber('pidYI', self.pidYDefault['i'])
self.sd.putNumber('pidYD', self.pidYDefault['d'])
self.pidY = wpilib.PIDController(self.pidYDefault['p'], self.pidYDefault['i'], self.pidYDefault['d'], self.encoderY.getDistance, self.updateYPID)
self.pidYRate = 0
self.toDistanceFirstCall = True
self.toAngleFirstCall = True
self.toTimeFirstCall = True
self.lastAngle = 0
self.timer = wpilib.Timer()
def createObjects(self):
self.stager_used = False
# self.pdp = wpilib.PowerDistributionPanel()
self.reverse_stager_used = False
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.ll = NetworkTables.getTable("limelight")
self.controlPanel = wpilib.Joystick(5)
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
self.climbMotor = wpilib.Victor(2)
self.stagerMotor = wpilib.Victor(3)
self.frontShooterMotor = wpilib.Victor(9)
self.elevatorMotor = wpilib.Victor(6)
self.elevatorMotor.setInverted(True)
self.shooterTiltMotor = wpilib.Victor(7)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
self.climbMotor.setInverted(True)
self.stagerMotor.setInverted(True)
self.punchers = wpilib.DoubleSolenoid(0, 0, 1)
self.skis = wpilib.DoubleSolenoid(4, 5)
self.launcherRotate = wpilib.AnalogInput(0)
self.climbLimitSwitch = wpilib.DigitalInput(8)
self.climb_raise_limitswitch = wpilib.DigitalInput(4)
self.ball_center = wpilib.DigitalInput(9)
self.elevator_limit = wpilib.DigitalInput(7)
self.pins = wpilib.DoubleSolenoid(2,3)
self.pins.set(2)
self.tilt_limit = wpilib.DigitalInput(6)
self.tilt_controller = wpilib.PIDController(2,0,0, self.launcherRotate, self.shooterTiltMotor)
# #Practice bot(2,0,0)
# #Comp bot(3.66,0,0)
self.tilt_controller.setOutputRange(-1,.5)
self.tilt_controller.setPercentTolerance(5)
self.elevator_encoder = wpilib.Encoder(0, 1)
self.elevator_controller = wpilib.PIDController(.0025, 0, .001, self.elevator_encoder, self.elevatorMotor)
# #practice bot(0.008,0,0.005)
# #Comp bot(.0025,0,.001)
self.elevator_controller.setOutputRange(-1,.44)
self.elevator_controller.setPercentTolerance(10)
self.gears = wpilib.DoubleSolenoid(6,7)
self.tilt_disabled = True
self.punchers.set(2)
self.skis.set(2)
self.oldtx = 0
self.gears.set(1)
self.tilt_disabled = True
self.controlPanel.setOutputs(False)
self.color_sensor_left = wpilib.DigitalInput(3)
self.color_sensor_mid = wpilib.DigitalInput(5)
self.color_sensor_right = wpilib.DigitalInput(2)
self.ultra = wpilib.AnalogInput(1)
def robotInit(self):
"""initialises robot as a mecanum drive bot w/ 2 joysticks and a camera"""
#want to change this to Xbox 360 controller eventually... probably sooner rather
#than later.
#
#This is for a USB camera. Uncomment it if we aren't using the Axis.
self.camera = wpilib.USBCamera()
self.camera.setExposureManual(50)
self.camera.setBrightness(80)
self.camera.updateSettings()
self.camera.setFPS(10)
self.camera.setSize(320, 240)
self.camera.setWhiteBalanceAuto()
#self.camera.setQuality(30)
server = wpilib.CameraServer.getInstance()
server.startAutomaticCapture(self.camera)
self.drive = wpilib.RobotDrive(3, 1, 2, 0)
self.drive.setExpiration(0.1)
self.stick_left = wpilib.Joystick(0)
self.stick_right = wpilib.Joystick(1)
self.drive.setInvertedMotor(self.drive.MotorType.kFrontRight, True)
self.drive.setInvertedMotor(self.drive.MotorType.kRearRight, True)
#self.gyro = wpilib.Gyro(0)
self.aux_left = wpilib.Jaguar(6)
self.aux_right = wpilib.Jaguar(4)
self.window_motor = wpilib.Jaguar(5)
self.smart_dashboard = NetworkTable.getTable("SmartDashboard")
self.mast_pot = wpilib.AnalogPotentiometer(0)
self.grabba_pot = wpilib.AnalogPotentiometer(1)
self.lift_pot = wpilib.AnalogPotentiometer(2)
def aux_combined(output):
"""use for PID control"""
self.aux_left.pidWrite(output)
self.aux_right.pidWrite(output)
self.grabba_pid = wpilib.PIDController(4, 0.07, 0, self.grabba_pot.pidGet, self.window_motor.pidWrite)
self.grabba_pid.disable()
self.lift_pid = wpilib.PIDController(4, 0.07, 0, self.lift_pot.pidGet, aux_combined)
self.lift_pid.disable()
def robotInit(self):
# Channels for the wheels
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
self.myRobot = wpilib.drive.DifferentialDrive(wpilib.Spark(0), wpilib.Spark(1))
self.myRobot.setExpiration(0.1)
self.stick = wpilib.Joystick(0)
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
#self.ahrs = AHRS.create_spi()
self.ahrs = AHRS.create_i2c(0)
turnController = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.ahrs, output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-0.5, 0.5)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController", turnController)
def __init__(self, robot, distance):
super().__init__()
self.robot = robot
self.requires(self.robot.drivetrain)
self.pid = wpilib.PIDController(-2, 0, 0)
self.pid.setSetpoint(distance)
def __init__(self, speed):
super().__init__(name='FollowCamera')
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.logger = self.getRobot().logger
self.angle = 0.0
self.speed = -speed
self.rotation = 0.0
self.pidOutput = 0.0
self.distance = 0
NetworkTables.addConnectionListener(self.connectionListener,
immediateNotify=True)
self.smartDashboard = NetworkTables.getTable('SmartDashboard')
self.smartDashboard.addEntryListener(self.valueChanged)
kP = robotmap.camera_kP
kD = robotmap.camera_kD
kI = robotmap.camera_kI
self.pid = wpilib.PIDController(kP, kI, kD, lambda: self.getAngle(),
lambda r: self.setRotation(r))
self.pid.setAbsoluteTolerance(0.01)
self.pid.setInputRange(0, 640)
self.pid.setSetpoint(320)
self.pid.setOutputRange(-1.0, 1.0)
self.pid.setContinuous(True)
def __init__(self, elevatorEncoder, elevatorLimitS, jawsLimitS,
metaboxLimitS, jawsM, elevatorM, intakeM, jawsSol):
self.elevatorEncoder = elevatorEncoder
self.elevatorLimit = elevatorLimitS
self.jawsLimitS = jawsLimitS
self.metaboxLimitS = metaboxLimitS
self.jawsM = jawsM
self.elevatorM = elevatorM
self.intakeM = intakeM
self.jawsSol = jawsSol
self.elevatorTravel = 26.4
self.isCalibrated = False
self.sd = NetworkTables.getTable('SmartDashboard')
self.pidDefault = {'p': 0.8, 'i': 0.2, 'd': 0.5}
self.pid = wpilib.PIDController(self.pidDefault['p'],
self.pidDefault['i'],
self.pidDefault['d'],
lambda: self.getEncoder(),
self.setElevator)
self.pid.setAbsoluteTolerance(0.1)
self.sd.putNumber('elevatorP', self.pidDefault['p'])
self.sd.putNumber('elevatorI', self.pidDefault['i'])
self.sd.putNumber('elevatorD', self.pidDefault['d'])
self.timer = wpilib.Timer()
self.autoActionStarted = False
def setup(self):
self.left = 0
self.right = 0
self.sd = NetworkTable.getTable("/SmartDashboard")
#Turn to angle PI values
self.turning_P = self.sd.getAutoUpdateValue("TurnToAngle/P", 1)
self.turning_I = self.sd.getAutoUpdateValue("TurnToAngle/I", 0)
self.turning_D = self.sd.getAutoUpdateValue("TurnToAngle/D", 0)
self.turn_controller = wpilib.PIDController(Kp=self.turning_P.value, Ki=self.turning_I.value, Kd=self.turning_D.value, source=self, output=self)
self.turn_controller.setInputRange(-180, 180)
self.turn_controller.setOutputRange(-1, 1)
self.turn_controller.setContinuous(True)
self.pid_value = 0
self.drive_constant = self.sd.getAutoUpdateValue("Drive/Drive Constant", 0.0001)
self.drive_multiplier = self.sd.getAutoUpdateValue("Drive/Drive Multiplier", 0.75)
SmartDashboard.putBoolean("Reversed", False)
self.reversed = False
self.logger = logging.getLogger("drive")
self.iErr = 0
def robotInit(self):
"""Robot-wide initialization code should go here"""
# Basic robot chassis setup
self.stick = wpilib.Joystick(0)
self.robot_drive = wpilib.RobotDrive(0, 1)
# Position gets automatically updated as robot moves
self.gyro = wpilib.ADXRS450_Gyro()
# Use PIDController to control angle
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.pidGet,
output=self.pidWrite)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
self.i = 0
self.sd = NetworkTables.getTable("SmartDashboard")
self.hephestus = NetworkTables.getTable("hephestus")
def __init__(self, robot, x, y, timeout=None):
super().__init__()
self.robot = robot
self.x = x
self.y = y
self.controller = wpilib.PIDController(-.05, 0, 0, self.returnPIDInput, self.usePIDOutput)
self.requires(self.robot.drivetrain)
self.setTimeout(timeout)
def __init__(self, robot, angle):
super().__init__()
self.robot = robot
self.angle = angle
self.controller = wpilib.PIDController(.008, 0, 0, self.returnPIDInput, self.usePIDOutput)
self.setTimeout(3)
self.controller.setAbsoluteTolerance(2)
self.requires(self.robot.drivetrain)
def __init__(self, top_speed, top_accl, navX, encoder, chassis, distance,
angle):
self.top_speed = top_speed
self.top_accl = top_accl
self.navX = navX
self.distance = distance
self.encoder = encoder
self.angle = angle
self.chassis = chassis
self.kP = 0.0005
self.kI = 0.001
self.kD = 0.00
self.kF = 0.00
self.aP = 0.08
self.aI = 0.02
self.aD = 0.3
self.aF = 0.00
self.kToleranceDegrees = 5
self.aToleranceDegrees = 1
self.fwdWrite = pidWriteObject()
self.turnWrite = pidWriteObject()
self.sum = 0
currentSpeedRate = 0
currentRotationRate = 0
self.control = Distance(self.top_speed, self.top_accl, self.distance)
self.tm = wpilib.Timer()
self.tm.start()
self.firstTime = True
self.fwdController = wpilib.PIDController(self.kP, self.kI, self.kD,
self.kF,
self.encoder.getRate,
self.fwdWrite)
self.fwdController.setInputRange(-500.0, 500.0)
self.fwdController.setOutputRange(-0.008, 0.008)
self.fwdController.setAbsoluteTolerance(self.kToleranceDegrees)
self.fwdController.setContinuous(True)
self.turnController = wpilib.PIDController(self.aP, self.aI, self.aD,
self.aF, navXPID(self.navX),
self.turnWrite)
self.turnController.setInputRange(-180.0, 180.0)
self.turnController.setOutputRange(-0.4, 0.4)
self.turnController.setAbsoluteTolerance(self.aToleranceDegrees)
self.turnController.setContinuous(True)
def __init__(self):
if ShooterConstants.PID_ENABLED:
self.speed_controller_pid = wpilib.PIDController(
ShooterConstants.K_P, ShooterConstants.K_I,
ShooterConstants.K_D, lambda: self.get_linear_velocity(),
lambda x: self.set_motors(x))
self.speed_controller_pid.setOutputRange(-1, 1)
self.enabled = False
def setup(self):
self.pid_controller = wpilib.PIDController(
0.01, 0.0001, 0.0001, self.navx, self
)
self.pid_controller.setInputRange(-180.0, 180.0)
self.pid_controller.setOutputRange(-.75, .75)
self.pid_controller.setContinuous()
self.pid_controller.setAbsoluteTolerance(3)
self.pid_controller.enable()
def __init__(self, robot, distance):
super().__init__()
self.robot = robot
self.requires(self.robot.drivetrain)
self.pid = wpilib.PIDController(
4, 0, 0, lambda: self.robot.drivetrain.getDistance(),
lambda d: self.robot.drivetrain.driveManual(d, d))
self.pid.setAbsoluteTolerance(0.01)
self.pid.setSetpoint(distance)
def setup(self):
self.pid = wpilib.PIDController(self.kP,
self.kI,
self.kD,
source=self.drivetrain.gyro,
output=self)
self.pid.setOutputRange(-0.4, 0.4)
self.pid.setInputRange(-180, 180)
self.pid.setContinuous()
self.pid.setAbsoluteTolerance(2)
def __init__(self,
drive_motor,
rotate_motor,
encoder,
sd_prefix='SwerveModule',
inverted=False,
zero=0.0,
allow_reverse=True,
has_drive_encoder=False):
"""
Swerve drive module was written for a swerve drive train that uses absolute encoders for tracking wheel rotation.
:param drive_motor: Motor object
:param rotate_motor: Motor object
:param encoder: AnalogInput wpilib object
:param sd_prefix: a string used to differentiate modules when debugging
:param zero: The default zero for the encoder
:param inverted: boolean to invert the wheel rotation
:param allow_reverse: If true allows wheels to spin backwards instead of rotating
:param has_drive_encoder: If true allows the module to track wheel position
"""
# SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
self.sd_prefix = sd_prefix
# Motors
self.drive_motor = drive_motor
self.drive_inverted = inverted
self.drive_motor.setInverted(self.drive_inverted)
self.rotate_motor = rotate_motor
self._requested_voltage = 0 # Angle in voltage form
self._requested_speed = 0
# Encoder
self.encoder = encoder
self.encoder_zero = zero
# PID
self._pid_controller = wpilib.PIDController(1.5, 0.0, 0.0,
self.encoder,
self.rotate_motor)
self._pid_controller.setContinuous()
self._pid_controller.setInputRange(0.0, 5.0)
self._pid_controller.enable()
# State variables
self.allow_reverse = allow_reverse
self.debugging = self.sd.getAutoUpdateValue(
'drive/%s/debugging' % self.sd_prefix, False)
self.has_drive_encoder = has_drive_encoder
def setup(self):
self.speed = 0
self.setpoint = 0
self.pid_controller = wpilib.PIDController(
0.00255, 0.0, 0.0, self.lift_encoder, self.lift_motor
)
self.pid_controller.setAbsoluteTolerance(0.5)
self.pid_controller.setContinuous(False)
self.pid_controller.setOutputRange(-0.5, 1.0)
self.pid_controller.setSetpoint(0)
self.pid_controller.enable()
def __init__(self, robot):
super().__init__('climber')
self.robot = robot
# Default max value for climber encoders
self.front_encoder_min = -530225
self.front_encoder_max = 0
# set up motor controllers
self.climber_motor_1 = ctre.WPI_TalonSRX(const.CAN_MOTOR_CLIMBER_1)
self.climber_motor_2 = ctre.WPI_VictorSPX(const.CAN_MOTOR_CLIMBER_2)
self.climber_motor_drive = ctre.WPI_TalonSRX(const.CAN_MOTOR_CLIMBER_DRIVE)
self.climber_back_motor = ctre.WPI_VictorSPX(const.CAN_MOTOR_BACK_CLIMBER)
self.climber_motor_1.setInverted(False)
self.climber_motor_2.setInverted(False)
self.climber_motor_drive.setInverted(False)
# PROBABLY NEEDS TO CHANGE
self.climber_back_motor.setInverted(False)
self.climber_kP = 0.02
self.climber_kI = 0.0
self.climber_kD = 0.0
#self.climber_motor_1.configOpenLoopRamp(1)
self.climber_motor_2.follow(self.climber_motor_1)
self.climber_pid = wpilib.PIDController(
self.climber_kP,
self.climber_kI,
self.climber_kD,
self.get_climber_pid_input,
self.set_climber_pid_output,
)
# add methods for range, continuous, tolerance etc.
self.climber_pid.reset()
self.climber_pid.setInputRange(-90, 90)
self.climber_pid.setOutputRange(-1, 1)
self.climber_pid.setContinuous(False)
self.climber_pid.setAbsoluteTolerance(0)
self.stop_climber()
# set up limit switches
self.front_top_limit = wpilib.DigitalInput(const.DIO_CLIMBER_FRONT_TOP_LIMIT)
self.front_bottom_limit = wpilib.DigitalInput(const.DIO_CLIMBER_FRONT_BOTTOM_LIMIT)
self.stilts_hit_platform_limit = wpilib.DigitalInput(const.DIO_CLIMBER_STILTS_HIT_PLATFORM_LIMIT)
self.back_top_limit = wpilib.DigitalInput(const.DIO_CLIMBER_BACK_TOP_LIMIT)
self.back_bottom_limit = wpilib.DigitalInput(const.DIO_CLIMBER_BACK_BOTTOM_LIMIT)
def rotateLeftUntil(self, func, speed, pre1=None, pre2=None):
u.stage()
if pre1 is not None:
pre1()
total_l, total_r = 0, 0
def inp():
nonlocal total_l, total_r
print(total_l, total_r)
total_l += e.left()
total_r += e.right()
if total_r == 0 or total_l == 0:
i = 1
else:
ratio = total_r / -total_l
adjust = abs(ratio - 1) / (-total_l + total_r)
i = 1 + adjust if ratio > 1 else 1 - adjust
if i > 10:
return 10
return i
f = False
def out(i):
nonlocal f
if f:
print(i)
motors.leftSet(-(int(speed * (1-i)) if i > 0 else speed))
motors.rightSet(int(speed * (1+i)) if i < 0 else speed)
else:
f = True
kp = 5
ki = 1
kd = 1
kf = 1
source = inp
output = out
ctrlr = wpilib.PIDController(kp, ki, kd, kf, source, output)
ctrlr.setInputRange(0, 10)
ctrlr.setOutputRange(-1.0, 1.0)
ctrlr.setAbsoluteTolerance(0.1)
ctrlr.setContinuous()
ctrlr.setSetpoint(1)
inp()
if pre2 is not None:
pre2()
motors.leftSet(-speed)
motors.rightSet(speed)
time.sleep(0.01)
if func():
motors.bothSet(0)
ctrlr.enable()
while not func():
pass
ctrlr.disable()
motors.bothSet(0)
u.unstage()
def setup(self):
self.manual_override = False
self.manual_override_value = 0
self.pid_controller = wpilib.PIDController(0.022, 0.0, 0.0,
self.lift_encoder,
self.lift_master)
self.pid_controller.setAbsoluteTolerance(0.5)
self.pid_controller.setContinuous(False)
self.pid_controller.setOutputRange(-.165, .6)
self.pid_controller.enable()
def __init__(self, robot, can_id):
super().__init__()
self.robot = robot
self.wristMotor = rev.CANSparkMax(can_id, rev.MotorType.kBrushless)
self.encoder = SwiftCanEncoder(self.wristMotor.getEncoder())
self.encoder.setDistancePerPulse(constants.WRIST_DEGREES_FACTOR)
self.encoder.setPosition(constants.WRIST_START_POSITION)
self.pid = wpilib.PIDController(.07, 0, 0, self.encoder, self.wristMotor)
self.pid.setAbsoluteTolerance(3)
self.pid.setEnabled(False)
def __init__(self, turn_motor, drive_motor, encoder, zero, p, i, d):
self.drive_motor = drive_motor
self.zero = zero
self.pid_controller = wpilib.PIDController(p, i, d, encoder,
turn_motor)
self.pid_controller.setInputRange(0, 5)
self.pid_controller.setOutputRange(-1, 1)
self.pid_controller.setContinuous(True)
self.pid_controller.setSetpoint(0 + zero)
self.pid_controller.setPercentTolerance(10)
self.pid_controller.enable()
self.encoder = encoder
class DriveConfig(object):
left_motors = wpilib.Talon(1)
right_motors = wpilib.Talon(2)
left_encoder = wpilib.Encoder(2, 3)
left_PID_encoder = DistanceEncoder(left_encoder)
left_PID_controller = wpilib.PIDController(0, 0, 0, left_PID_encoder, left_motors)
right_encoder = wpilib.Encoder(4, 5)
right_PID_encoder = DistanceEncoder(right_encoder)
right_PID_controller = wpilib.PIDController(0, 0, 0, right_PID_encoder, right_motors)
robot_drive = DriveBase(left_motors, right_motors, True,
left_encoder, right_encoder,
left_PID_controller, right_PID_controller)
#robot_drive = wpilib.RobotDrive(left_motors, right_motors)
left_shifter = wpilib.DoubleSolenoid(1, 2)
right_shifter = wpilib.DoubleSolenoid(3, 4)
forward = wpilib.DoubleSolenoid.kForward
reverse = wpilib.DoubleSolenoid.kReverse
drive_joy = leftJoy
align_button = Button(leftJoy, 6)
front_left_photo_switch = wpilib.DigitalInput(14)
front_right_photo_switch = wpilib.DigitalInput(12)
back_left_photo_switch = wpilib.DigitalInput(13)
back_right_photo_switch = wpilib.DigitalInput(11)
# Buttons
squared_drive_stick = Button(leftJoy, 1)
shift_button = Button(leftJoy, 9)
def create():
l, r = encoder.createLeftRight()
kp = 5
ki = 1
kd = 1
kf = 1
source = inp(l, r)
output = lambda x: motors.bothSetVar(x) or lol.append(x)
ctrlr = wpilib.PIDController(kp, ki, kd, kf, source, output)
ctrlr.setInputRange(-1000, 1000)
ctrlr.setOutputRange(-1.0, 1.0)
ctrlr.setAbsoluteTolerance(10)
ctrlr.setContinuous()
return ctrlr
def create():
l, r = encoder.getLeftRight()
kp = 5
ki = 1
kd = 1
kf = 1
source = inp(l, r)
output = out
ctrlr = wpilib.PIDController(kp, ki, kd, kf, source, output)
ctrlr.setInputRange(0, 10)
ctrlr.setOutputRange(-1.0, 1.0)
ctrlr.setAbsoluteTolerance(0.1)
ctrlr.setContinuous()
return ctrlr
def robotInit(self):
# Channels for the wheels
self.table = NetworkTables.getTable("SmartDashboard")
self.myRobot = wpilib.drive.DifferentialDrive(wpilib.Spark(0),
wpilib.Spark(1))
self.myRobot.setExpiration(0.1)
self.stick = wpilib.Joystick(0)
self.wheel = wpilib.Encoder(0, 1)
self.wheel2 = wpilib.Encoder(2, 3, True)
self.wheel.reset()
self.wheel2.reset()
self.wheel.setDistancePerPulse(0.8922)
self.wheel2.setDistancePerPulse(0.8922)
self.rate = AverageOutRateGen(self.wheel.getRate, self.wheel2.getRate)
self.sum = 0
self.toggle = True
self.control = Distance(300, 20)
self.tm = wpilib.Timer()
self.tm.start()
self.start_time = self.tm.getMsClock()
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
#self.ahrs = AHRS.create_spi()
self.ahrs = AHRS.create_i2c(0)
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
AverageOutRateGen(
self.wheel.getRate,
self.wheel2.getRate),
output=self)
turnController.setInputRange(-500, 500.0)
turnController.setOutputRange(-0.008, 0.008)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController",
turnController)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.open_state = .02
self.closed_state = .35
self.arm = ctre.wpi_talonsrx.WPI_TalonSRX(0)
self.joystick = wpilib.Joystick(0)
self.arm.setInverted(True)
self.arm_pot = wpilib.AnalogPotentiometer(0)
self.arm_pid = wpilib.PIDController(3, 0, 0, self.arm_pot.get,
self.pid_output)
def initialize(self):
kP = 0.01
kI = 0.0001
turnController = wpilib.PIDController(kP,
kI,
0,
0,
self.navx,
output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-.5, .5)
turnController.setContinuous(True)
self.turnController = turnController
self.rotation = 0
def setup(self):
self.speed = 0
self.wrist_setpoint = 0
self.extend = False
self.grab = False
self.pid_controller = wpilib.PIDController(
0.0256, 0.0, 0.0, self.wrist_encoder, self.wrist_motor
)
self.pid_controller.setAbsoluteTolerance(0.5)
self.pid_controller.setContinuous(False)
self.pid_controller.setOutputRange(-1, 1)
self.pid_controller.setSetpoint(209)
# 136-215-220
self.pid_controller.enable()