import wpilib
from wpilib import Timer, DoubleSolenoid
from rev import SparkMax, SparkLowLevel, SparkMaxPIDController
from [Link] import DifferentialDrive
class Robot([Link]):
def robotInit(self):
# Motores del chasis
self.m_leftChassisSpark = SparkMax(1, [Link])
self.m_rightChassisSpark = SparkMax(2, [Link])
# Controladores PID del chasis
self.m_leftChassisClosedLoopController =
self.m_leftChassisSpark.getPIDController()
self.m_rightChassisClosedLoopController =
self.m_rightChassisSpark.getPIDController()
# Motores y PID de subsistemas
[Link] = SparkMax(3, [Link])
[Link] = [Link]()
[Link] = SparkMax(4, [Link])
[Link] = [Link]()
[Link] = [Link]()
[Link] = SparkMax(5, [Link])
[Link] = [Link]()
[Link] = [Link]()
self.m_intakerMotor = SparkMax(6, [Link])
# Solenoide de transmisión
[Link] = DoubleSolenoid(0, 1)
# Controladores
[Link] = [Link](0)
[Link] = [Link](1)
[Link] = DifferentialDrive(self.m_leftChassisSpark,
self.m_rightChassisSpark)
# PIDValues simulados
class PIDValues:
armGearRatio = 100 # Asumido
wristGearRatio = 50 # Asumido
[Link] = PIDValues()
def autonomousPeriodic(self):
self.m_leftChassisClosedLoopController.setReference(-42.5,
[Link], 0)
self.m_rightChassisClosedLoopController.setReference(-42.5,
[Link], 0)
[Link](4.5)
[Link]((46.5*127.6)/51,
[Link], 0)
[Link]((20*[Link])/360,
[Link], 0)
if [Link]() > 0.52:
[Link]((140*[Link])/
360, [Link], 0)
def teleopInit(self):
[Link] = DifferentialDrive(self.m_leftChassisSpark,
self.m_rightChassisSpark)
def teleopPeriodic(self):
# Chasis
if [Link](5) and
[Link]() == [Link]:
self.m_leftChassisSpark.stopMotor()
self.m_rightChassisSpark.stopMotor()
[Link]([Link])
elif [Link](4) and
[Link]() == [Link]:
self.m_leftChassisSpark.stopMotor()
self.m_rightChassisSpark.stopMotor()
[Link]([Link])
else:
[Link](
[Link](1),
[Link](2),
True
)
# Elevator ground level
if [Link](1) or
[Link](9):
[Link](0,
[Link], 0)
[Link](0,
[Link], 0)
[Link](0,
[Link], 0)
elif [Link](2):
[Link]((46.5*127.6)/51,
[Link], 0)
[Link]((20*[Link])/360,
[Link], 0)
if [Link]() > 0.52:
[Link]((75*[Link])/360,
[Link], 0)
elif [Link](3):
[Link]((120*[Link])/360,
[Link], 1)
if [Link]() > 5:
[Link]((100*[Link])/
360, [Link], 1)
elif [Link](4):
[Link]((170*[Link])/360,
[Link], 2)
if [Link]() > 8.5:
[Link]((46.5*127.6)/51,
[Link], 0)
[Link]((150*[Link])/
360, [Link], 1)
# Elevator climber
if -[Link](1) < -0.2:
[Link]((5*127.6)/51,
[Link], 1)
elif -[Link](1) > 0.2:
[Link]((30*127.6)/51,
[Link], 0)
# Floor pickup / processor deposit
if -[Link](5) < -0.2:
[Link]((30*[Link])/360,
[Link], 0)
if [Link]() > 0.5:
[Link]((130*[Link])/
360, [Link], 0)
elif -[Link](5) > 0.2:
[Link]((30*[Link])/360,
[Link], 0)
if [Link]() > 0.5:
[Link]((85*[Link])/360,
[Link], 0)
elif [Link](10):
[Link](0.00,
[Link], 0)
if [Link]() < 6:
[Link](0.00,
[Link], 0)
# Intaker
if [Link](4):
self.m_intakerMotor.set(0.75)
elif [Link](5):
self.m_intakerMotor.set(-1)
else:
self.m_intakerMotor.set(0)
def disabledInit(self):
pass
def disabledPeriodic(self):
pass
def testInit(self):
[Link](0,
[Link], 0)
[Link](0,
[Link], 0)
[Link](0,
[Link], 0)
if __name__ == "__main__":
[Link](Robot)