class Drivetrain:
# We also use injection here to access the l_motor and r_motor declared in BruhBot's
# createObjects method.
m_lfront: WPI_TalonSRX
m_rfront: WPI_TalonSRX
m_lback: WPI_TalonSRX
m_rback: WPI_TalonSRX
xSpeed = 0
ySpeed = 0
zRotation = 0
gyroAngle = 0
# Declare the basic drivetrain setup.
def setup(self):
self.mec_drive = MecanumDrive(self.m_lfront, self.m_lback,
self.m_rfront, self.m_rback)
self.mec_drive.setExpiration(0.1)
self.mec_drive.setSafetyEnabled(True)
# Change x and y variables for movement.
def move(self, x, y, z, gyroAngle):
self.xSpeed = x if abs(x) > 0.03 else 0
self.ySpeed = y if abs(y) > 0.03 else 0
self.zRotation = z
self.gyroAngle = gyroAngle
# Each time move is called, call arcadeDrive with supplied
# x and y coordinates.
def execute(self):
self.mec_drive.driveCartesian(self.xSpeed, self.ySpeed, self.zRotation,
self.gyroAngle)
class Drivetrain(Subsystem):
def __init__(self):
# Verify motor ports when placed on frame
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.motor_lf.setInverted(False)
self.motor_lr.setInverted(False)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class MyRobot(wpilib.SampleRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 3
frontRightChannel = 2
rearRightChannel = 4
# The channel on the driver station that the joystick is connected to
xchannel0 = 0
xchannel1 = 1
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.VictorSP(self.frontLeftChannel)
self.rearLeftMotor = wpilib.VictorSP(self.rearLeftChannel)
self.frontRightMotor = wpilib.VictorSP(self.frontRightChannel)
self.rearRightMotor = wpilib.VictorSP(self.rearRightChannel)
# invert the left side motors
# self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
# self.rearLeftMotor.setInverted(True)
# self.frontRightMotor.setInverted(True)
# self.rearRightMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.xbox0 = wpilib.XboxController(self.xchannel0)
self.xbox1 = wpilib.XboxController(self.xchannel1)
def operatorControl(self):
"""Runs the motors with Mecanum drive."""
self.drive.setSafetyEnabled(True)
while self.isOperatorControl() and self.isEnabled():
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(-self.xbox0.getX(0), -self.xbox0.getY(0),
0)
wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
# Define the Xbox Controller.
self.stick = wpilib.XboxController(self.joystickChannel)
def teleopInit(self):
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with Mecanum drive."""
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
# This Stick configuration is created by K.E. on our team. Left stick Y axis is speed, Left Stick X axis is strafe, and Right Stick Y axis is turn.
self.drive.driveCartesian(
self.stick.getX(self.stick.Hand.kLeftHand),
self.stick.getY(self.stick.Hand.kLeftHand),
self.stick.getY(self.stick.Hand.kRightHand),
0,
)
"""Alternatively, to match the driver station enumeration, you may use ---> self.drive.driveCartesian(
class Drivetrain(Subsystem):
"""
Subsystem that holds everything for the robot's drivetrain.
Provides methods for driving the robot on a Cartesian plane.
"""
def __init__(self):
# Hardware
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def inputNoise(self, input):
"""
This function limits the joystick input noise by increasing the
tolerance for the zero value.
"""
return input if abs(input) < 0.03 else 0
def driveCartesian(self, xSpeed, ySpeed, zRotation, gyroAngle=0.0):
"""
Wrapper method that the subsystem uses to input a truncated number into
the drivetrain, along with any motor inversions.
"""
xSpeed = self.inputNoise(xSpeed) * axes.motor_inversion[0]
ySpeed = self.inputNoise(ySpeed) * axes.motor_inversion[1]
zRotation = zRotation * axes.motor_inversion[2]
self.drive.driveCartesian(xSpeed, ySpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
"""
Shorthand method for driveCartesian.
"""
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class DriveSubsystem(Subsystem):
def __init__(self):
super().__init__('DriveSubsystem')
self.frontRight = WPI_VictorSPX(robotmap.CAN_ID_VICTOR_FRONT_RIGHT)
self.rearRight = WPI_VictorSPX(robotmap.CAN_ID_VICTOR_REAR_RIGHT)
self.frontLeft = WPI_VictorSPX(robotmap.CAN_ID_VICTOR_FRONT_LEFT)
self.rearLeft = WPI_VictorSPX(robotmap.CAN_ID_VICTOR_REAR_LEFT)
self.mecanum = MecanumDrive(self.frontLeft, self.rearLeft,
self.frontRight, self.rearRight)
def initDefaultCommand(self):
self.setDefaultCommand(MecanumDriveCommand())
def drive(self, robotRightSpeed, robotForwardSpeed, robotClockwiseSpeed):
self.mecanum.driveCartesian(robotRightSpeed, robotForwardSpeed,
robotClockwiseSpeed)
class DriveTrain(Subsystem):
'''
This example subsystem controls a single Talon in PercentVBus mode.
'''
def __init__(self):
'''Instantiates the motor object.'''
super().__init__('SingleMotor')
self.frontLeftMotor = WPI_TalonSRX(channels.frontLeftChannel)
self.rearLeftMotor = WPI_TalonSRX(channels.rearLeftChannel)
self.frontRightMotor = WPI_TalonSRX(channels.frontRightChannel)
self.rearRightMotor = WPI_TalonSRX(channels.rearRightChannel)
self.crossbow = wpilib.DoubleSolenoid(0, 1)
self.crossbow.set(wpilib.DoubleSolenoid.Value.kOff)
self.frontLeftMotor.setInverted(False)
# self.rearLeftMotor.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 30)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
# self.motor = wpilib.Talon(1)
def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set_crossbow(self, setting):
self.crossbow.set(setting)
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.stick = wpilib.Joystick(self.joystickChannel)
def teleopInit(self):
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with Mecanum drive."""
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(self.stick.getX(), self.stick.getY(),
self.stick.getZ(), 0)
class MyRobot(wpilib.SampleRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 0
rearLeftChannel = 2
frontRightChannel = 1
rearRightChannel = 3
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
wpilib.CameraServer.launch()
'''Robot initialization function'''
self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel)
self.frontRightMotor = wpilib.Spark(self.frontRightChannel)
self.rearRightMotor = wpilib.Spark(self.rearRightChannel)
# invert the left side motors
#self.frontRightMotor.setInverted(True)
# you may need to change or remove this to match your robot
#self.rearRightMotor.setInverted(True)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.stick = wpilib.Joystick(self.joystickChannel)
def operatorControl(self):
'''Runs the motors with Mecanum drive.'''
self.drive.setSafetyEnabled(True)
while self.isOperatorControl() and self.isEnabled():
self.drive.driveCartesian(
self.stick.getX() * (-self.stick.getZ() + 1) / 2,
-self.stick.getY() * (-self.stick.getZ() + 1) / 2,
self.stick.getThrottle() * (-self.stick.getZ() + 1) / 2)
#Changed "Z" value to "Throttle" value to control turning on the robot
wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.SampleRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
'''Robot initialization function'''
self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel)
self.frontRightMotor = wpilib.Spark(self.frontRightChannel)
self.rearRightMotor = wpilib.Spark(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.stick = wpilib.Joystick(self.joystickChannel)
def operatorControl(self):
'''Runs the motors with Mecanum drive.'''
self.drive.setSafetyEnabled(True)
while self.isOperatorControl() and self.isEnabled():
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Throttle axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(self.stick.getX(), self.stick.getY(),
self.stick.getThrottle(), 0)
#Changed "Z" value to "Throttle" value to control turning on the robot
wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 3
rearRightChannel = 4
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.timer = wpilib.Timer()
self.timer.start()
def autonomousPeriodic(self):
if self.timer.get() < 2.0:
self.drive.driveCartesian(0, -1, 1, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
# self.drive.driveCartesian(
# self.lstick.getX(), -self.lstick.getY(), self.rstick.getX(), 0
# )
self.drive.driveCartesian(self.lstick.getX(), -self.lstick.getY(),
self.lstick.getRawAxis(2), 0)
class DriveTrain(Subsystem):
'''
This example subsystem controls a single Talon in PercentVBus mode.
'''
def __init__(self):
'''Instantiates the motor object.'''
super().__init__('SingleMotor')
self.frontLeftMotor = wpilib.Talon(channels.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(channels.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(channels.frontRightChannel)
self.rearRightMotor = wpilib.Talon(channels.rearRightChannel)
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
# self.motor = wpilib.Talon(1)
def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class Mecanum(Subsystem):
"""
Subsystem to control the motors for MecanumDrive
"""
def __init__(self):
super().__init__("Mecanum")
# motors and the channels they are connected to
self.frontLeftMotor = wpilib.VictorSP(1)
self.rearLeftMotor = wpilib.PWMVictorSPX(2)
self.frontRightMotor = wpilib.VictorSP(0)
self.rearRightMotor = wpilib.PWMVictorSPX(3)
# invert the left side motors
self.frontLeftMotor.setInverted(False)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.rearRightMotor.setInverted(False) #added this to match motor
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(False)
def speed(self, yaxis, xaxis, zaxis, gyro):
self.drive.driveCartesian(yaxis, xaxis, zaxis, gyro)
def initDefaultCommand(self):
self.setDefaultCommand(ThrottleMixer())
class MyRobot(wpilib.TimedRobot):
def __init__(self):
self.ticksPerInchPulley = 30 # 30 ticks per inch is just a guess, the number is probably different
self.ticksPerInchLifter = 30
self.ticksPerInchWheels = 30
self.ticksPerInchCrabbing = 31
self.pulleyDeadbandInches = 0.5 #how many inches we are okay with the pulley being off (to avoid the jiggle)
self.spinBarDeadBand = 400 # ticks, or about 1/10 of a rotation, which is about 36 degrees
self.ticksPerRevolution = 4096
self.queue = Queue()
# switches
self.leftLeadScrewDown = 1
self.leftLeadScrewUp = 2
self.rightLeadScrewDown = 3
self.rightLeadScrewUp = 4
self.spinBarIn = 5
self.spinBarOut = 6
self.manualPulleyUp = 7
self.manualPulleyDown = 8
self.selector1 = 9
self.selector2 = 10
self.selector3 = 11
self.selector4 = 12
# buttons
self.EStop = 10
self.manualHatchDeposit = 1
self.autoHatchDeposit = 2
self.autoCargoDeposit = 3
self.hatchCollectHeight = 4
self.autoCargoShipDeposit = 5
self.pulleyReset = 6
self.hab1to2 = 7
self.hab1to3 = 8
self.hab2to3 = 9
self.buttonsChannel2 = 2
self.buttonsChannel1 = 1
self.driveJoystickChannel = 0
self.ds = wpilib.DriverStation.getInstance()
self.driveStick = wpilib.Joystick(self.driveJoystickChannel)
self.extraHeight = 1 # this is the distance (in inches) that the robot will raise above each hab level before going back down
self.lifter1to2 = 6 + self.extraHeight # these measurements are in inches
self.lifter1to3 = 18 + self.extraHeight
self.lifter2to3 = 12 + self.extraHeight
self.lifterSpeed = 0.5
self.selector0to1voltage = 0.5
self.selector1to2voltage = 1.5
self.selector2to3voltage = 2.5
self.levelSelectorAnalogChannel = 0
self.IRSensorAnalogChannel = 1
self.IRSensor = wpilib.AnalogInput(self.IRSensorAnalogChannel)
self.bottomPulleyHallEffectChannel = 0
self.topPulleyHallEffectChannel = 1
self.topLeftLeadScrewHallEffectChannel = 9
self.topRightLeadScrewHallEffectChannel = 7
self.bottomLeftLeadScrewHallEffectChannel = 8
self.bottomRightLeadScrewHallEffectChannel = 6
self.IRSensorThreshold = 2.5
self.topLeftLeadScrewHallEffect = wpilib.DigitalInput(self.topLeftLeadScrewHallEffectChannel)
self.topRightLeadScrewHallEffect = wpilib.DigitalInput(self.topRightLeadScrewHallEffectChannel)
self.bottomLeftLeadScrewHallEffect = wpilib.DigitalInput(self.bottomLeftLeadScrewHallEffectChannel)
self.bottomRightLeadScrewHallEffect = wpilib.DigitalInput(self.bottomRightLeadScrewHallEffectChannel)
self.crab1 = -1 # these are the distances that the robot will crab onto the different hab levels, in inches
self.crab2 = -2
self.crab3 = -12
self.crab4 = -2
self.crab5 = -4
self.crabSpeed = 0.5
self.hatch1HeightInches = 20 # these are measurements off of the ground, and will change depending on how far the pulley is off the ground
self.hatch2HeightInches = 48 # at the bottom (the measurements are in inches)
self.hatch3HeightInches = 76
self.hatchDepositSpeed = 0.1
self.hatchDepositSpeedForWheels = 1
self.cargo1HeightInches = 28
self.cargo2HeightInches = 56
self.cargo3HeightInches = 84
self.cargoShipHeightInches = 36
self.frontLeftChannel = 2
self.frontRightChannel = 1
self.rearLeftChannel = 3
self.rearRightChannel = 4
self.leftLeadScrewChannel = 6
self.rightLeadScrewChannel = 5
self.pulleyChannel = 7
self.spinBarChannel = 8
self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel)
self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel)
self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel)
self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel)
self.leftLeadScrewMotor = ctre.WPI_TalonSRX(self.leftLeadScrewChannel)
self.rightLeadScrewMotor = ctre.WPI_TalonSRX(self.rightLeadScrewChannel)
self.pulleyMotor = ctre.WPI_TalonSRX(self.pulleyChannel)
self.spinBarMotor = ctre.WPI_TalonSRX(self.spinBarChannel)
self.pulleyMotorModifier = 0.5 # slows down the Pulley motor speed just in case it goes way too fast
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.frontLeftMotor.setSafetyEnabled(False)
self.rearLeftMotor.setSafetyEnabled(False)
self.frontRightMotor.setSafetyEnabled(False)
self.rearRightMotor.setSafetyEnabled(False)
#Last thing in the init function
super().__init__()
def hab(startLevel, goalLevel):
'''This function will '''
hab = Job()
hab.function = 'raiseBase'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'crabLeft'
hab.parameters = '(self.crab1, self.frontLeftMotor)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'raiseLeft'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'crabLeft'
hab.parameters = '(self.crab2, self.rearRightMotor)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'lowerLeft'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'crabLeft'
hab.parameters = '(self.crab3, self.frontLeftMotor)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'raiseRight'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'crabLeft'
hab.parameters = '(self.crab4, self.frontLeftMotor)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'lowerRight'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'crabLeft'
hab.parameters = '(self.crab5, self.frontLeftMotor)'
hab.driveLock = True
self.queue.add(hab)
def raiseBase(startLevel, goalLevel):
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3
if currentPosition < goalPosition and wpilib.DigitalInput(self.bottomLeftLeadScrewHallEffectChannel) < self.bottomLeftLeadScrewHallEffectThreshold and wpilib.DigitalInput(self.bottomRightLeadScrewHallEffectChannel) < self.bottomRightLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(self.lifterSpeed)
self.rightLeadScrewMotor.set(self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
self.rightLeadScrewMotor.set(0)
self.queue.remove()
def lowerLeft():
'''This moves the encoders down, or extends the lead screw.'''
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
goalPosition = self.ticksPerInchLifter * self.extraHeight
if currentPosition < goalPosition and wpilib.DigitalInput(self.bottomLeftLeadScrewHallEffectChannel) < self.bottomLeftLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
self.queue.remove()
def lowerRight():
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
goalPosition = self.ticksPerInchLifter * self.extraHeight
if currentPosition < goalPosition and wpilib.DigitalInput(self.bottomRightLeadScrewHallEffectChannel) < self.bottomRightLeadScrewHallEffectThreshold:
self.rightLeadScrewMotor.set(self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
self.queue.remove()
def raiseLeft(startLevel, goalLevel):
'''This raises the lead screws into the body, and is considered a negative direction.'''
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2 * -1
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3 * -1
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3 * -1
if currentPosition > goalPosition and wpilib.DigitalInput(self.topLeftLeadScrewHallEffectChannel) < self.topLeftLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
self.queue.remove()
def raiseRight(startLevel, goalLevel):
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2 * -1
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3 * -1
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3 * -1
if currentPosition > goalPosition and wpilib.DigitalInput(self.topRightLeadScrewHallEffectChannel) < self.topRightLeadScrewHallEffectThreshold:
self.rightLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
self.queue.remove()
def driveQuadratureReset():
self.frontLeftMotor.setQuadraturePosition(0, 0)
self.frontRightMotor.setQuadraturePosition(0, 0)
self.rearLeftMotor.setQuadraturePosition(0, 0)
self.rearRightMotor.setQuadraturePosition(0, 0)
def crabLeft(distance, encoder):
currentPosition = encoder.getQuadraturePosition()
goalPosition = ticksPerInchCrabbing * distance
if currentPosition > goalPosition:
self.frontLeftMotor.set(-1 * self.crabSpeed)
self.frontRightMotor.set(self.crabSpeed)
self.rearLeftMotor.set(self.crabSpeed)
self.rearRightMotor.set(-1 * self.crabSpeed)
else:
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.queue.remove()
def depositPayload(level, payload):
depositPayload = Job()
depositPayload.function = 'pulleyHeight'
depositPayload.parameters = '(' + str(level) + ', ' + str(payload) + ')'
depositPayload.driveLock = True
self.queue.add(depositPayload)
depositPayload = Job()
depositPayload.function = 'dispense'
depositPayload.parameters = '(' + str(payload) + ')'
depositPayload.driveLock = True
self.queue.add(depositPayload)
def levelSelector():
'''This function returns the level as an integer by checking the rotary switch controlling rocket level.'''
if self.ds.getStickButton(2, self.selector1):
return(0)
elif self.ds.getStickButton(2, self.selector2):
return(1)
elif self.ds.getStickButton(2, self.selector3):
return(2)
else:
return(3)
def Pulley_encoder():
currentPosition= self.pulleyMotor.getQuadraturePosition()
def pulleyHeight(level, payload): # level 0 is the floor, payload 1 is hatch, payload 2 is cargo, payload 3 is cargo ship cargo(not done yet)
'''Moves the Pulley to certain levels, with a certain offset based on hatch or cargo. The cargo ship has a unique offset (supposedly), and the hatch has no offset.'''
currentPosition = self.pulleyMotor.getQuadraturePosition()
if level == 0:
self.resetPulley()
elif payload == 1: # hatch
if level == 1:
goalPosition = self.ticksPerInchPulley * self.hatch1Height
elif level == 2:
goalPosition = self.ticksPerInchPulley * self.hatch2Height
else: # level 3 is the only other option the Pulley has, so the else is for level 3
goalPosition = self.ticksPerInchPulley * self.hatch3Height
elif payload == 2: # cargo
if level == 1:
goalPosition = self.ticksPerInchPulley * self.cargo1Height
elif level == 2:
goalPosition = self.ticksPerInchPulley * self.cargo2Height
else:
goalPosition = self.ticksPerInchPulley * self.cargo3Height
else:
goalPosition = self.ticksPerInchPulley * self.cargoShipHeightInches
if level > 0:
if currentPosition < (goalPosition - (self.ticksPerInchPulley * self.pulleyDeadbandInches)) and wpilib.DigitalInput(topPulleyHallEffectChannel) < self.topPulleyHallEffectThreshold: # this sets a deadband for the encoders on the pulley so that the pulley doesnt go up and down forever
self.pulleyMotor.set(self.pulleyMotorModifier)
elif currentPosition > (goalPosition + (self.ticksPerInchPulley * self.pulleyDeadbandInches)) and wpilib.DigitalInput(topPulleyHallEffectChannel) < self.topPulleyHallEffectThreshold:
self.pulleyMotor.set(-1 * self.pulleyMotorModifier)
else:
self.pulleyMotor.set(0)
self.queue.remove()
def dispense(payload):
currentPosition = self.spinBarMotor.getQuadraturePosition()
goalPositionCargo = self.ticksPerRevolution * 8
goalPositionHatch = self.ticksPerRevolution
if payload == 2 or payload == 3: # a cargo or a cargoship
if currentPosition < (goalPositionCargo - self.spinBarDeadBand):
self.spinBarMotor.set(0.5)
elif currentPosition > (goalPositionCargo + self.spinBarDeadBand):
self.spinBarMotor.set(-0.5)
else:
self.spinBarMotor.set(0)
self.queue.remove()
# This logic requires a job before this one to set the spinBar position to 0 when it is all the way back, without needing the spinBar to
# go backwards multiple rotations to get to quadrature position 0. This is accomplished using the resetSpinBar function.
elif payload == 1: # a hatch
if currentPosition < (goalPositionHatch - self.spinBarDeadBand):
self.spinBarMotor.set(self.hatchDepositSpeed)
self.frontLeftMotor.set(-1 * self.hatchDepositSpeedForWheels)
self.frontRightMotor.set(self.hatchDepositSpeedForWheels)
self.rearLeftMotor.set(self.hatchDepositSpeedForWheels)
self.rearRightMotor.set(-1 * self.hatchDepositSpeedForWheels)
elif currentPosition > (goalPositionHatch + self.spinBarDeadBand):
self.spinBarMotor.set(-1 * self.hatchDepositSpeed)
self.frontLeftMotor.set(-1 * self.hatchDepositSpeedForWheels)
self.frontRightMotor.set(self.hatchDepositSpeedForWheels)
self.rearLeftMotor.set(self.hatchDepositSpeedForWheels)
self.rearRightMotor.set(-1 * self.hatchDepositSpeedForWheels)
else:
self.spinBarMotor.set(0)
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.queue.remove()
def resetSpinBar():
currentPosition = self.spinBarMotor.getQuadraturePosition()
offset = currentPosition % self.ticksPerRevolution
if (offset) > self.spinBarDeadBand:
self.spinBarMotor.set(-1 * self.spinBarResetSpeed)
else:
self.spinBarMotor.set(0)
self.spinBarMotor.setQuadraturePosition(offset, 0)
self.queue.remove()
def spinBar(velocity):
self.spinBarMotor.set(velocity)
def resetPulley(): # to bring the pulley back to its starting height
# go down until the hall effect sensor reads the magnet, then stop and set encoder value to 0
if self.DigitalInput(self.bottomPulleyHallEffectChannel) < self.bottomPulleyHallEffectThreshold:
self.pulleyMotor.set(-1 * self.pulleyMotorModifier)
else:
self.pulleyMotor.set(0)
self.pulleyMotor.setQuadraturePosition(0, 0)
self.queue.remove()
def robotInit(self):
"""Robot initialization function"""
pass
def autonomousInit(self):
pass
def autonomousPeriodic(self):
pass
def teleopInit(self):
pass
def checkSwitches(self):
if self.ds.getStickButton(1, self.EStop) ==1: #E-Stop button pressed, stop all motors and remove all jobs from job queue.
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.leftLeadScrewMotor.set(0)
self.rightLeadScrewMotor.set(0)
self.pulleyMotor.set(0)
self.spinBarMotor.set(0)
#Remove all queued jobs by setting the queue to the blank class
self.queue = Queue()
else: #Check every other switch
# leadscrew buttons aaaaaaaaaaaaaaannnnnnnnd limit switch stuff \/ \/ \/
if self.ds.getStickButton(2, self.leftLeadScrewDown) and self.bottomLeftLeadScrewHallEffect == 1: # left lead screw out manual
self.leftLeadScrewMotor.set(self.lifterSpeed)
elif self.ds.getStickButton(2, self.leftLeadScrewUp) and self.topLeftLeadScrewHallEffect == 1: # left lead screw in manual
self.leftLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
if self.ds.getStickButton(2, self.rightLeadScrewDown) and self.bottomRightLeadScrewHallEffect == 1: # right lead screw out manual
self.rightLeadScrewMotor.set(self.lifterSpeed)
elif self.ds.getStickButton(2, self.rightLeadScrewUp) and self.topRightLeadScrewHallEffect == 1: # right lead screw in manual
self.rightLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
# spinbar buttons
if self.ds.getStickButton(2, self.spinBarIn) ==1: # cargo collecting
if self.IRSensor.getVoltage() < self.IRSensorThreshold: # IR distance sensor stops the spinBar from spinning in when the ball is already in
self.spinBarMotor.set(-1)
else:
self.spinBarMotor.set(0)
elif self.ds.getStickButton(2, self.spinBarOut) ==1: # manual cargo depositing
self.spinBarMotor.set(1)
elif self.ds.getStickButton(1, self.manualHatchDeposit) ==1: # manual hatch depositing
self.spinBarMotor.set(self.hatchDepositSpeed)
else:
self.spinBarMotor.set(0)
# pulley up down buttons
if self.ds.getStickButton(2, self.manualPulleyUp) ==1: # manual pulley up
self.pulleyMotor.set(self.pulleyMotorModifier)
elif self.ds.getStickButton(2, self.manualPulleyDown)==1: # manual pulley down
self.pulleyMotor.set(-1 * self.pulleyMotorModifier)
# hatch buttons
if self.ds.getStickButton(1, self.autoHatchDeposit)==1: # hatch movement and depositing (auto)
Deposit_pl = Job()
Deposit_pl.function = 'depositPayload'
Deposit_pl.parameters = '(self.levelSelector, 1)'
Deposit_pl.driveLock = True
self.queue.add(Deposit_pl)
elif self.ds.getStickButton(1, self.hatchCollectHeight) == 1: # hatch collecting (from player station)
hatchCollectManual = Job()
hatchCollectManual.function = 'pulleyHeight'
hatchCollectManual.parameters = '(1, 1)'
hatchCollectManual.driveLock = True
self.queue.add(hatchCollectManual)
hatchCollectManual = Job()
hatchCollectManual.function = 'resetSpinBar'
hatchCollectManual.parameters = '()'
hatchCollectManual.driveLock = False
self.queue.add(hatchCollectManual)
# cargo buttons
elif self.ds.getStickButton(1, self.autoCargoDeposit) == 1: # cargo movement and depositing
self.depositPayload(self.levelSelector, 2)
elif self.ds.getStickButton(1, self.autoCargoShipDeposit) == 1: # cargo ship depositing
self.depositPayload(self.levelSelector, 3)
# Pulley reset button
elif self.ds.getStickButton(1, self.pulleyReset) == 1: # pulley reset
resetPulley = Job()
resetPulley.function = 'resetPulley'
resetPulley.parameters = '()'
resetPulley.driveLock = False
self.queue.add(resetPulley)
# buttons controlling baseLifter (3 buttons)
if self.ds.getStickButton(1, self.hab1to2) == 1: # hab level 1 to level 2
self.hab(1, 2)
elif self.ds.getStickButton(1, self.hab1to3) ==1: # hab level 1 to level 3
self.hab(1, 3)
elif self.ds.getStickButton(1, self.hab2to3) ==1: # hab level 2 to level 3
self.hab(2, 3)
def teleopPeriodic(self):
# checks switches and sensors, which feed the queue with jobs
self.checkSwitches()
# we are checking if a job is in the queue, and then calling the function that the first job makes using eval
if len(self.queue.queue) > 0:
currentJob = self.queue.peek()
eval(currentJob.function + currentJob.parameters)
# allows the driver to drive the robot when the currentJob allows them to, using the driveLock parameter in the job
if currentJob.drivelock == False:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
else:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
if self.ds.getStickButton(0,2) ==1:
try:
test = sd.getValue('adjust_x', 0)
testy = sd.getValue('adjust_y', 0)
testz = sd.getValue('adjust_z', 0)
print('x ' + str(test))
print('y ' + str(testy))
print('z ' + str(testz))
except Exception as e:
print(str(e.args))
if len(self.queue.queue) == 0 and self.ds.getStickButton(0,2) == 1:
self.drive.driveCartesian(self.driveStick.getX(test), self.driveStick.getY(testy), self.driveStick.getZ(testz), 0)
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.stick = wpilib.Joystick(self.joystickChannel)
def autonomousInit(self):
LS = wpilib.DigitalInput(9)
if LS.get() == 1:
print("Found it")
def operatorControl(self):
"""Runs the motors with Mecanum drive."""
self.drive.setSafetyEnabled(True)
while self.isOperatorControl() and self.isEnabled():
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(self.stick.getX(), self.stick.getY(),
self.stick.getZ(), 0)
wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
self.pop = Camara2.main()
wpilib.CameraServer.launch()
# NetworkTables.startClientTeam(5716)
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize()
self.pc = NetworkTables.getTable("SmartDashboard")
# self.cond = threading.Condition()
# self.notified = [False]
#NetworkTables.initialize(server='roborio-5716-frc.local')
#NetworkTables.initialize()
#self.sd = NetworkTables.getTable('SmartDashboard')
# wpilib.CameraServer.launch()
# cap = cv2.VideoCapture(0)
# self.Video = VideoRecorder()
# wpilib.CameraServer.launch()
# self.chasis_controller = wpilib.Joystick(0)
self.timer = wpilib.Timer()
self.left_cannon_motor = wpilib.Talon(5)
self.right_cannon_motor = wpilib.Talon(6)
self.sucker = wpilib.Talon(7)
self.front_left_motor = wpilib.Talon(3)
self.rear_left_motor = wpilib.Talon(1)
self.front_right_motor = wpilib.Talon(4)
self.rear_right_motor = wpilib.Talon(2)
self.front_left_motor.setInverted(True)
self.color_wheel = wpilib.Talon(8)
self.drive = MecanumDrive(
self.front_left_motor,
self.rear_left_motor,
self.front_right_motor,
self.rear_right_motor)
#led
self.green_led = wpilib.DigitalOutput(0)
#cannon pneumatic
self.Compressor = wpilib.Compressor(0)
self.PSV = self.Compressor.getPressureSwitchValue()
self.cannon_piston = wpilib.Solenoid(0,5)
self.hook1 = wpilib.Solenoid(0,0)
self.hook2 = wpilib.Solenoid(0,7)
# self.navx = navx.AHRS.create_spi()
def autonomousInit(self):
self.timer.reset()
self.timer.start()
def autonomousPeriodic(self):
position = self.pc.getString("Posicion", "Not jalando")
print(position)
wpilib.DriverStation.reportWarning(position,True)
#state 1
print(self.timer.get())
if self.timer.get() < 9:
self.cannon_piston.set(True)
elif self.timer.get() >= 9 and self.timer.get() <= 10:
self.right_cannon_motor.set(-1)
self.left_cannon_motor.set(-1)
elif self.timer.get() >= 10 and self.timer.get() < 13:
self.cannon_piston.set(False)
# elif state["timer.get()_auto"] >= 420 and state["timer.get()_auto"] < 520:
# self.cannon_piston.set(True)
# elif state["timer.get()_auto"] >= 520 and state["timer.get()_auto"] < 540:
# self.cannon_piston.set(False)
elif self.timer.get() >= 13 and self.timer.get() < 15:
self.right_cannon_motor.set(0)
self.left_cannon_motor.set(0)
self.drive.driveCartesian(0, 0.5, 0, 0)
elif self.timer.get() >= 15:
self.drive.driveCartesian(0, 0, 0, 0)
self.right_cannon_motor.set(0)
self.left_cannon_motor.set(0)
self.timer.stop()
# time.sleep(3)
# elif state["timer_auto"] > 40 and state["timer_auto"] <= 80:
# self.cannon_piston.set(False)
# # time.sleep(0.7)
# elif state["timer_auto"] > 80 and state["timer_auto"] <= 150:
# self.cannon_piston.set(True)
# self.sucker.set(1)
# # time.sleep(4)
# # self.drive.driveCartesian(0, 0, 0)
# elif state["timer_auto"] > 150 and state["timer_auto"] < 250:
# self.front_left_motor.set(-1)
# self.rear_left_motor.set(-1)
# self.front_right_motor.set(-1)
# self.rear_right_motor.set(-1)
# state["timer_auto"] == 0
# # time.sleep(5)
#
#-----------------------------------------------------------------------------------------------------------
def teleopPeriodic(self):
wpilib.CameraServer.launch('vision.py:main')
# self.pc = NetworkTables.getTable("Posicion")
oi.ReadControllerInputs()
x = state["x_axis"]
y = state["y_axis"]
z = state["z_axis"]
powerX = 0 if x < 0.20 and x > -0.20 else x
powerY = 0 if y < 0.20 and y > -0.20 else y
powerZ = 0 if z < 0.20 and z > -0.20 else z
self.drive.driveCartesian(powerX * -0.9,powerY * 0.9, powerZ * -0.9, 0)
#----------------------------------------------------------------------------------------------------------
if state["cannon"] > 0.7:
self.right_cannon_motor.set(-1)
self.left_cannon_motor.set(-1)
else:
self.right_cannon_motor.set(0)
self.left_cannon_motor.set(0)
# if state["sucker_manual"]:
# self.sucker.set(0.6)
# elif state["sucker_manual"] == False:
# self.sucker.set(0)
# self.cannon_piston.set(state["piston_cannon"])
#------------------------------------------------------------------------------------------------------------
#Modificaciones para el gancho 10/2/2020
if state["hook"] == True and state["hook_status"] == False:
state["hook_status"] = True
state["solenoid_status"] = True
time.sleep(.3)
elif state["hook"] == True and state["hook_status"] == True:
state["hook_status"] = False
time.sleep(.3)
state["solenoid_status"] = True
if state["solenoid_status"] == True:
if state["hook_status"] == True:
self.hook1.set(True)
self.hook2.set(False)
else:
self.hook1.set(False)
self.hook2.set(True)
else:
self.hook1.set(True)
self.hook2.set(True)
#----------------------------------Semi automatico, cannon de rutina---------------------------------------------------
#cannon rutine
# if state["cannon2"]:
# #enciende los motores del cañón
# self.right_cannon_motor.set(-1)
# self.left_cannon_motor.set(1)
# #ocupas el for para que repita los pistones 5 veces
# i = 0
# for i in range(0,5):
# #enciende el piston
# state["auto_piston"] = True
# self.cannon_piston.set(state["auto_piston"])
# #espera 1 segundo
# time.sleep(1)
# #apaga el piston
# state["auto_piston"] = False
# self.cannon_piston.set(state["auto_piston"])
# #espera un segundo antes de repetir la rutina
# time.sleep(1)
# #acabando la rutina apaga los motores
# self.right_cannon_motor.set(0)
# self.left_cannon_motor.set(0)
# state["auto_piston"] = False
# self.cannon_piston.set(state["auto_piston"])
#------------------------------------Manual cannon motores-----------------------------------------------------
# if state["cannon"] == True and state["cannon_status"] == False:
# state["cannon_status"] = True
# time.sleep(.1)
# elif state["cannon"] == True and state["cannon_status"] == True:
# state["cannon_status"] = False
# time.sleep(.1)
# if state["cannon_status"] == True:
# self.right_cannon_motor.set(-1)
# self.left_cannon_motor.set(-1)
# else:
# self.right_cannon_motor.set(0)
# self.left_cannon_motor.set(0)
#------------------------------------Manual Sucker-----------------------------------------------------
# if state["sucker"] == True and state["sucker_status"] == False:
# state["sucker_status"] = True
# time.sleep(.3)
# elif state["sucker"] == True and state["sucker_status"] == True:
# state["sucker_status"] = False
# time.sleep(.3)
if state["sucker"]:
self.sucker.set(1)
#-------------------------------------Reverse Sucker Provisional------------------------------------------------------
elif state["reverse_sucker"]:
self.sucker.set(-1)
else:
self.sucker.set(0)
#-------------------------------------Manual cannon piston------------------------------------------------------
if state["piston_cannon"] > 0.7:
self.cannon_piston.set(False)
else:
self.cannon_piston.set(True)
#---------------------------------------------------------------------------------------------------------------
# if state["cannon"]:
# self.right_cannon_motor.set(-1)
# self.left_cannon_motor.set(1)
# else:
# self.right_cannon_motor.set(0)
# self.left_cannon_motor.set(0)
# if state["sucker"]:
# self.sucker.set(1)
# else:
# self.sucker.set(0)
#---------------------------------------------------------------------------------------------------------------
if state["aling"]:
print("jalo")
self.green_led.set(0)
if state["target_position"] != "Not Jalando" or "":
print("Jalando")
if state["target_position"] == "Top Left":
self.drive.driveCartesian(-0.5, 0, 0)
elif state["target_position"] == "Top Right":
self.drive.driveCartesian(0.5, 0, 0)
elif state["target_position"] == "Top Center":
self.drive.driveCartesian(0, 0, 0)
print("Shoot!")
else:
self.drive.driveCartesian(0, 0, 0)
print("Not detection!")
else:
# print("No jalo")
self.green_led.set(1)
# if state["navxon"]:
# print("Angle: " + self.navx.getAngle())
#-----------------------------------------------------------------------------------------------
if state["color_wheel"]:
if state["color_wheel_2"]:
self.color_wheel.set(-0.2)
wpilib.wait(0.2)
else:
self.color_wheel.set(1)
else:
self.color_wheel.set(0)
if self.PSV:
self.Compressor.stop()
else:
self.Compressor.start()
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#self.encoder = wpilib.Encoder(0, 6)
self.ir = wpilib.DigitalInput(1)
self.ir2 = wpilib.DigitalInput(2)
self.ir3 = wpilib.DigitalInput(3)
self.ir4 = wpilib.DigitalInput(4)
self.ir5 = wpilib.DigitalInput(5)
self.ir6 = wpilib.DigitalInput(6)
self.timer = wpilib.Timer()
self.front_left_motor = wpilib.Talon(0)
self.rear_left_motor = wpilib.Talon(1)
self.front_right_motor = wpilib.Talon(2)
self.rear_right_motor = wpilib.Talon(3)
self.drive = MecanumDrive(self.front_left_motor, self.rear_left_motor,
self.front_right_motor,
self.rear_right_motor)
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
pass
def autonomousPeriodic(self):
if self.ir3.get() or self.ir4.get():
if self.timer.get() < 3:
self.timer.reset()
self.timer.start()
self.drive.driveCartesian(1, 0, 0, 0)
elif self.ir.get() or self.ir2.get():
self.drive.driveCartesian(0, 1, 0, 0)
elif self.ir5.get() or self.ir6.get():
self.drive.driveCartesian(0, -1, 0, 0)
else:
self.drive.driveCartesian(0, -1, 0, 0)
"""This function is called periodically during autonomous."""
#Rocket right side
"""if self.timer.get() < 7:
self.drive.driveCartesian(1,0,0,0)
elif self.timer.get() > 7 and self.timer.get() < 9:
self.drive.driveCartesian(0,0,1,0)
elif self.timer.get() > 9 and self.timer.get() < 14:
self.drive.driveCartesian(1,0,0,0)
elif self.timer.get() > 14 and self.timer.get() < 17:
self.drive.driveCartesian(0,0,1,0)
"""
#Cargo ship
"""if self.timer.get() < 9:
self.drive.driveCartesian(1,0,0,0)
elif self.timer.get() > 9 and self.timer.get() < 11:
self.drive.driveCartesian(0,0,-1,0)"""
#
"""if self.timer.get() < 5:
self.drive.driveCartesian(1,0,0,0)
elif self.timer.get() > 5 and self.timer.get() < 10:
self.drive.driveCartesian(0,0,1,0)
elif self.timer.get() > 10 and self.timer.get() < 15:
self.drive.driveCartesian(1,0,0,0)
elif self.timer.get() > 15 and self.timer.get( ) < 20:
self.drive.driveCartesian(0,0,-1,0)"""
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
oi.read_all_controller_inputs()
x = state["mov_x"] * .7
y = state["mov_y"] * .7
z = state["mov_z"] * .5
powerX = 0 if x < 0.15 and x > -0.15 else x
powerY = 0 if y < 0.15 and y > -0.15 else y
powerZ = 0 if z < 0.15 and z > -0.15 else z
self.drive.driveCartesian(powerX, -powerY, powerZ, 0)
#print(self.encoder.get())
if state["ir"] == True:
if self.ir5.get() or self.ir6.get():
self.drive.driveCartesian(0, -1, 0, 0)
elif self.ir.get() or self.ir2.get():
self.drive.driveCartesian(0, 1, 0, 0)
elif self.ir3.get() or self.ir4.get():
self.drive.driveCartesian(1, 0, 0, 0)
self.timer.start()
if self.timer.get() <= 3:
"""self.timer.start()"""
self.drive.driveCartesian(1, 0, 0, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
# Pull in smart dashboard info...
self.sd = NetworkTables.getTable("SmartDashboard")
# Start a timer....
self.timer = wpilib.Timer()
"""Robot initialization function. The Low level is to use the brushless function on the controllers."""
if wpilib.RobotBase.isSimulation():
self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel)
self.frontRightMotor = wpilib.Spark(self.frontRightChannel)
self.rearRightMotor = wpilib.Spark(self.rearRightChannel)
else:
self.frontLeftMotor = rev.CANSparkMax(
self.frontLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearLeftMotor = rev.CANSparkMax(
self.rearLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.frontRightMotor = rev.CANSparkMax(
self.frontRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearRightMotor = rev.CANSparkMax(
self.rearRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.stick = wpilib.XboxController(self.joystickChannel)
#
# 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.navx = navx.AHRS.create_spi()
# self.navx = navx.AHRS.create_i2c()
# Analog input
# self.analog = wpilib.AnalogInput(navx.pins.getNavxAnalogInChannel(0)) <--It seems as though the analog channel is not currently supported.
def robotPeriodic(self):
self.timer.reset()
self.timer.start()
while self.isDisabled():
if self.timer.hasPeriodPassed(0.5):
self.sd.putNumber("Displacement X",
self.navx.getDisplacementX())
self.sd.putNumber("Displacement Y",
self.navx.getDisplacementY())
self.sd.putBoolean("IsCalibrating", self.navx.isCalibrating())
self.sd.putBoolean("IsConnected", self.navx.isConnected())
self.sd.putNumber("Angle", self.navx.getAngle())
self.sd.putNumber("Pitch", self.navx.getPitch())
self.sd.putNumber("Yaw", self.navx.getYaw())
self.sd.putNumber("Roll", self.navx.getRoll())
# self.sd.putNumber("Analog", self.analog.getVoltage())
self.sd.putNumber("Timestamp",
self.navx.getLastSensorTimestamp())
def autonomousInit(self):
"""Runs Once during auto"""
def autonomousPeriodic(self):
"""Runs Periodically during auto"""
self.drive.driveCartesian(.5, 0, .5, 0)
def teleopInit(self):
"""Runs Once during teleop"""
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with Mecanum drive."""
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(self.stick.getRawAxis(1),
self.stick.getRawAxis(3),
self.stick.getRawAxis(2),
self.navx.getAngle())
self.sd.putNumber("Field Angle", self.navx.getAngle())
class MyRobot(wpilib.SampleRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomous(self):
"""Called when autonomous mode is enabled"""
timer = wpilib.Timer()
timer.start()
while self.isAutonomous() and self.isEnabled():
if timer.get() < 2.0:
self.drive.driveCartesian(0, -1, 1, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
wpilib.Timer.delay(0.01)
def operatorControl(self):
"""Called when operation control mode is enabled"""
while self.isOperatorControl() and self.isEnabled():
self.drive.driveCartesian(
self.lstick.getX(), self.lstick.getY(), -self.lstick.getZ(), 0
)
wpilib.Timer.delay(0.04)
class MyRobot(wpilib.TimedRobot):
def __init__(self):
wpilib.CameraServer.launch()
self.ticksPerInchPulley = 1735 #for wilsonville pulley, will be inaccurate at higher numbers
self.ticksPerInchLifter = 30
self.ticksPerInchWheels = 30
self.ticksPerInchCrabbing = 31
self.pulleyDeadbandInches = 1 #how many inches we are okay with the pulley being off (to avoid the jiggle)
self.spinBarDeadBand = 400 # ticks, or about 1/10 of a rotation, which is about 36 degrees
self.ticksPerRevolution = 4096
self.queue = Queue()
# switches
self.leftLeadScrewDown = 5
self.leftLeadScrewUp = 4
self.rightLeadScrewDown = 3
self.rightLeadScrewUp = 2
self.spinBarIn = 14
self.spinBarOut = 15
self.manualPulleyUp = 7
self.manualPulleyDown = 6
self.selector1 = 8
self.selector2 = 11
self.selector3 = 12
self.selector4 = 13
# buttons
self.eStop = 1
self.manualHatchDeposit = 1
self.autoHatchDeposit = 4
self.autoCargoDeposit = 3
self.hatchCollectHeight = 7
self.autoCargoShipDeposit = 8
self.pulleyReset = 6
self.hab1to2 = 10
self.hab1to3 = 9
self.hab2to3 = 2
self.buttonsChannel2 = 2
self.buttonsChannel1 = 1
self.driveJoystickChannel = 0
self.ds = wpilib.DriverStation.getInstance()
self.driveStick = wpilib.Joystick(self.driveJoystickChannel)
self.auxiliary1 = wpilib.Joystick(self.buttonsChannel1)
self.auxiliary2 = wpilib.Joystick(self.buttonsChannel2)
self.extraHeight = 1 # this is the distance (in inches) that the robot will raise above each hab level before going back down
self.Lifter1to2 = 6 + self.extraHeight # these measurements are in inches
self.Lifter1to3 = 19 + self.extraHeight
self.Lifter2to3 = 13 + self.extraHeight
self.lifterSpeed = .7
self.selector0to1voltage = 0.5
self.selector1to2voltage = 1.5
self.selector2to3voltage = 2.5
self.levelSelectorAnalogChannel = 0
self.IRSensorAnalogChannel = 1
self.IRSensor = wpilib.AnalogInput(self.IRSensorAnalogChannel)
self.bottomPulleyHallEffectChannel = 0
self.topPulleyHallEffectChannel = 1
self.topLeftLeadScrewHallEffectChannel = 7
self.topRightLeadScrewHallEffectChannel = 3
self.bottomLeftLeadScrewHallEffectChannel = 9
self.bottomRightLeadScrewHallEffectChannel = 5
self.bottomPulleyHallEffect = wpilib.DigitalInput(self.bottomPulleyHallEffectChannel)
self.topPulleyHallEffect = wpilib.DigitalInput(self.topPulleyHallEffectChannel)
self.topLeftLeadScrewHallEffect = wpilib.DigitalInput(self.topLeftLeadScrewHallEffectChannel)
self.topRightLeadScrewHallEffect = wpilib.DigitalInput(self.topRightLeadScrewHallEffectChannel)
self.bottomLeftLeadScrewHallEffect = wpilib.DigitalInput(self.bottomLeftLeadScrewHallEffectChannel)
self.bottomRightLeadScrewHallEffect = wpilib.DigitalInput(self.bottomRightLeadScrewHallEffectChannel)
self.IRSensorThreshold = 2.5
self.bottomPulleyHallEffectThreshold = 1
self.topPulleyHallEffectThreshold = 1
self.topLeftLeadScrewHallEffectThreshold = 1
self.topRightLeadScrewHallEffectThreshold = 1
self.bottomLeftLeadScrewHallEffectThreshold = 1
self.bottomRightLeadScrewHallEffectThreshold = 1
self.crab1 = 1 # these are the distances that the robot will crab onto the different hab levels, in inches
self.crab2 = 2
self.crab3 = 24
self.crab4 = 2
self.crab5 = 4
self.crabSpeed = 0.8
self.crabTolerance = 200
self.fLCrabSpeed = 0.6
self.bLCrabSpeed = 0.6
self.fRCrabSpeed = 0.6
self.bRCrabSpeed = 0.6
self.hatch1Height = 20 # these are measurements off of the ground, and will change depending on how far the pulley is off the ground
self.hatch2Height = 48 # at the bottom (the measurements are in inches)
self.hatch3Height = 76
self.hatchDepositSpeed = 0.1
self.hatchDepositSpeedForWheels = 1
self.cargo1Height = 28
self.cargo2Height = 56
self.cargo3Height = 84
self.cargoShipHeightInches = 36
self.frontLeftChannel = 2
self.frontRightChannel = 1
self.rearLeftChannel = 3
self.rearRightChannel = 4
self.leftLeadScrewChannel = 6
self.rightLeadScrewChannel = 5
self.pulleyChannel = 7
self.spinBarChannel = 8
self.raisedPulley = False
self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel)
self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel)
self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel)
self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel)
self.leftLeadScrewMotor = ctre.WPI_TalonSRX(self.leftLeadScrewChannel)
self.rightLeadScrewMotor = ctre.WPI_TalonSRX(self.rightLeadScrewChannel)
self.pulleyMotor = ctre.WPI_TalonSRX(self.pulleyChannel)
self.spinBarMotor = ctre.WPI_TalonSRX(self.spinBarChannel)
self.pulleyMotorModifier = 0.8 # slows down the Pulley motor speed just in case it goes way too fast
self.frontLeftMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontRightMotor,
self.rearRightMotor,
self.frontLeftMotor,
self.rearLeftMotor,
)
self.frontLeftMotor.setSafetyEnabled(False)
self.rearLeftMotor.setSafetyEnabled(False)
self.frontRightMotor.setSafetyEnabled(False)
self.rearRightMotor.setSafetyEnabled(False)
#this is the IR dist for ball
self.ball_dist = 2.5
self.hatch_dist = 3
self.alignB1 = 15
self.alignH = 35
self.alignCB = 12
self.adjust_ready = False
self.pulleyMotor.setQuadraturePosition(0,0)
#Last thing in the init function
super().__init__()
def hab(self, startLevel, goalLevel):
'''This function will '''
hab = Job()
hab.function = 'self.raiseBase'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.crabRight'
hab.parameters = '(self.crab1)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.raiseLeft'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.crabRight'
hab.parameters = '(self.crab2)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.lowerLeft'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.crabRight'
hab.parameters = '(self.crab3)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.raiseRight'
hab.parameters = '(' + str(startLevel) + ', ' + str(goalLevel) + ')'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.crabRight'
hab.parameters = '(self.crab4)'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.lowerRight'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.driveQuadratureReset'
hab.parameters = '()'
hab.driveLock = True
self.queue.add(hab)
hab = Job()
hab.function = 'self.crabRight'
hab.parameters = '(self.crab5)'
hab.driveLock = True
self.queue.add(hab)
def raiseBase(self, startLevel, goalLevel):
Left_off = 0
Right_off = 0
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3
if currentLeftLeadScrewPosition < goalPosition and (self.bottomLeftLeadScrewHallEffect).get() < self.bottomLeftLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
Left_off = 1
if currentRightLeadScrewPosition < goalPosition and (self.bottomRightLeadScrewHallEffect).get() < self.bottomRightLeadScrewHallEffectThreshold:
self.rightLeadScrewMotor.set(self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
Right_off = 1
self.queue.remove()
if Left_off ==1 and Right_off ==1:
self.queue.remove()
def lowerLeft(self):
'''This moves the encoders down, or extends the lead screw.'''
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
goalPosition = self.ticksPerInchLifter * self.extraHeight
if currentLeftLeadScrewPosition < goalPosition and (self.bottomLeftLeadScrewHallEffect.get()) < self.bottomLeftLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
self.queue.remove()
def lowerRight(self):
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
goalPosition = self.ticksPerInchLifter * self.extraHeight
if currentPosition < goalPosition and (self.bottomRightLeadScrewHallEffect.get()) < self.bottomRightLeadScrewHallEffectThreshold:
self.rightLeadScrewMotor.set(self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
self.queue.remove()
def raiseLeft(self, startLevel, goalLevel):
'''This raises the lead screws into the body, and is considered a negative direction.'''
currentLeftLeadScrewPosition = self.leftLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2 * -1
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3 * -1
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3 * -1
if currentLeftLeadScrewPosition > goalPosition and (self.topLeftLeadScrewHallEffect.get()) < self.topLeftLeadScrewHallEffectThreshold:
self.leftLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.leftLeadScrewMotor.set(0)
self.queue.remove()
def raiseRight(self, startLevel, goalLevel):
currentRightLeadScrewPosition = self.rightLeadScrewMotor.getQuadraturePosition()
if startLevel == 1:
if goalLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter1to2 * -1
elif goalLevel == 3:
goalPosition = self.ticksPerInchLifter * self.Lifter1to3 * -1
elif startLevel == 2:
goalPosition = self.ticksPerInchLifter * self.Lifter2to3 * -1
if currentRightLeadScrewPosition > goalPosition and (self.topRightLeadScrewHallEffect.get()) < self.topRightLeadScrewHallEffectThreshold:
self.rightLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
self.queue.remove()
def driveQuadratureReset(self):
self.frontLeftMotor.setQuadraturePosition(0, 0)
self.frontRightMotor.setQuadraturePosition(0, 0)
self.rearLeftMotor.setQuadraturePosition(0, 0)
self.rearRightMotor.setQuadraturePosition(0, 0)
'''
def crabRight(self, distance):
currentFrontPosition = self.frontLeftMotor.getQuadraturePosition()
currentRearPosition = self.rearLeftMotor.getQuadraturePosition()
currentPosition = (currentFrontPosition + currentRearPosition)/2
goalPosition = self.ticksPerInchCrabbing * distance
#print('function is set up')
#print('current is ' + str(currentPosition))
#print('goal is ' + str(goalPosition))
if currentPosition < goalPosition:
if math.isclose(currentFrontPosition, currentRearPosition, abs_tol=self.crabTolerance):
print(str(currentPosition))
print('motors should be moving the same speed')
elif currentFrontPosition > currentPosition:
print('rear motors should be moving faster')
print(str(currentPosition))
self.bLCrabSpeed += 0.01
self.bRCrabSpeed += 0.01
self.fLCrabSpeed += -0.01
self.fRCrabSpeed += -0.01
else:
print('front motors should be moving faster')
print(str(currentPosition))
self.fLCrabSpeed += 0.01
self.fRCrabSpeed += 0.01
self.bLCrabSpeed += -0.01
self.bRCrabSpeed += -0.01
else:
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.queue.remove()
print('you have reached your goal')
self.frontLeftMotor.set(self.fLCrabSpeed)
self.frontRightMotor.set(self.fRCrabSpeed)
self.rearLeftMotor.set(self.bLCrabSpeed)
self.rearRightMotor.set(self.bRCrabSpeed)
'''
def depositPayload(self, level, payload):
JOB = Job()
JOB.function = 'self.pulleyHeight'
JOB.parameters = '(' + str(level) + ', ' + str(payload) + ')'
JOB.driveLock = True
self.queue.add(JOB)
JOB = Job()
JOB.function = 'self.dispense'
JOB.parameters = '(' + str(payload) + ')'
JOB.driveLock = True
self.queue.add(JOB)
def levelSelector(self):
'''This function returns the level as an integer by checking the rotary switch controlling rocket level.'''
if self.auxiliary2.getRawButton(self.selector1):
return(1)
elif self.auxiliary2.getRawButton(self.selector2):
return(2)
elif self.auxiliary2.getRawButton(self.selector3):
return(3)
else:
pass
def Pulley_encoder(self):
currentPosition= self.pulleyMotor.getQuadraturePosition()
def pulleyHeight(self, level, payload): # level 0 is the floor, payload 1 is hatch, payload 2 is cargo, payload 3 is cargo ship cargo(not done yet)
'''Moves the Pulley to certain levels, with a certain offset based on hatch or cargo. The cargo ship has a unique offset (supposedly), and the hatch has no offset.'''
currentPosition = self.pulleyMotor.getQuadraturePosition()
if payload == 1: # hatch
if level == 1:
goalPosition = self.ticksPerInchPulley * self.hatch1Height
elif level == 2:
goalPosition = self.ticksPerInchPulley * self.hatch2Height
else: # level 3 is the only other option the Pulley has, so the else is for level 3
goalPosition = self.ticksPerInchPulley * self.hatch3Height
elif payload == 2: # cargo
if level == 1:
goalPosition = self.ticksPerInchPulley * self.cargo1Height
elif level == 2:
goalPosition = self.ticksPerInchPulley * self.cargo2Height
else:
goalPosition = self.ticksPerInchPulley * self.cargo3Height
elif payload == 6:
if level==1: #6 is the rocket ball
print('moving at ' + self.pulleyMotorModifier)
goalPosition = self.ticksPerInchPulley * self.alignB1
elif payload == 5: #5 is the rocket hatch
if level == 1:
print('moving at ' + self.pulleyMotorModifier)
goalPosition= self.ticksPerInchPulley * self.alignH
elif payload == 4: #4 is the cargoship ball
if level == 1:
print('moving at ' + self.pulleyMotorModifier)
goalPosition = self.ticksPerInchPulley* self.alignCB
else:
goalPosition = self.ticksPerInchPulley * self.cargoShipHeightInches
if currentPosition < (goalPosition - (self.ticksPerInchPulley * self.pulleyDeadbandInches)): # this sets a deadband for the encoders on the pulley so that the pulley doesnt go up and down forever
self.pulleyMotor.set(self.pulleyMotorModifier)
elif currentPosition > (goalPosition + (self.ticksPerInchPulley * self.pulleyDeadbandInches)): #and (topPulleyHallEffect.get()) < self.topPulleyHallEffectThreshold
self.pulleyMotor.set(-1 * self.pulleyMotorModifier)
else:
self.pulleyMotor.set(0)
self.queue.remove()
def dispense(self, payload):
currentPosition = self.spinBarMotor.getQuadraturePosition()
goalPositionCargo = self.ticksPerRevolution * 8
goalPositionHatch = self.ticksPerRevolution
if payload == 2 or payload == 3: # a cargo or a cargoship
if currentPosition < (goalPositionCargo - self.spinBarDeadBand):
self.spinBarMotor.set(0.5)
elif currentPosition > (goalPositionCargo + self.spinBarDeadBand):
self.spinBarMotor.set(-0.5)
else:
self.spinBarMotor.set(0)
self.queue.remove()
# This logic requires a job before this one to set the spinBar position to 0 when it is all the way back, without needing the spinBar to
# go backwards multiple rotations to get to quadrature position 0. This is accomplished using the resetSpinBar function.
elif payload == 1: # a hatch
if currentPosition < (goalPositionHatch - self.spinBarDeadBand):
self.spinBarMotor.set(self.hatchDepositSpeed)
self.frontLeftMotor.set(-1 * self.hatchDepositSpeedForWheels)
self.frontRightMotor.set(self.hatchDepositSpeedForWheels)
self.rearLeftMotor.set(self.hatchDepositSpeedForWheels)
self.rearRightMotor.set(-1 * self.hatchDepositSpeedForWheels)
elif currentPosition > (goalPositionHatch + self.spinBarDeadBand):
self.spinBarMotor.set(-1 * self.hatchDepositSpeed)
self.frontLeftMotor.set(-1 * self.hatchDepositSpeedForWheels)
self.frontRightMotor.set(self.hatchDepositSpeedForWheels)
self.rearLeftMotor.set(self.hatchDepositSpeedForWheels)
self.rearRightMotor.set(-1 * self.hatchDepositSpeedForWheels)
else:
self.spinBarMotor.set(0)
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.queue.remove()
def resetSpinBar(self):
currentPosition = self.spinBarMotor.getQuadraturePosition()
offset = currentPosition % self.ticksPerRevolution
if (offset) > self.spinBarDeadBand:
self.spinBarMotor.set(-1 * self.spinBarResetSpeed)
else:
self.spinBarMotor.set(0)
self.spinBarMotor.setQuadraturePosition(offset, 0)
self.queue.remove()
def spinBar(self, velocity):
self.spinBarMotor.set(velocity)
def resetPulley(self): # to bring the pulley back to its starting height
# go down until the hall effect sensor reads the magnet, then stop and set encoder value to 0
if (self.bottomPulleyHallEffect.get()) < self.bottomPulleyHallEffectThreshold:
self.pulleyMotor.set(-1 * self.pulleyMotorModifier)
else:
self.pulleyMotor.set(0)
self.pulleyMotor.setQuadraturePosition(0, 0)
self.queue.remove()
def robotInit(self):
"""Robot initialization function"""
def autonomousInit(self):
print('autonomous starting')
self.raisedPulley = False
self.pulleyMotor.setQuadraturePosition(0,0)
self.leftLeadScrewMotor.setQuadraturePosition(0,0)
self.rightLeadScrewMotor.setQuadraturePosition(0,0)
self.frontRightMotor.setQuadraturePosition(0,0)
self.rearRightMotor.setQuadraturePosition(0,0)
self.frontLeftMotor.setQuadraturePosition(0,0)
self.rearLeftMotor.setQuadraturePosition(0,0)
def autonomousPeriodic(self):
if self.raisedPulley == False:
self.pulleyHeight(1,1)
else:
pass
# checks switches and sensors, which feed the queue with jobs
self.checkSwitches()
# we are checking if a job is in the queue, and then calling the function that the first job makes using eval
if len(self.queue.queue) > 0:
currentJob = self.queue.peek()
print(str(currentJob.function))
eval(currentJob.function + currentJob.parameters)
#allows the driver to drive the robot when the currentJob allows them to, using the driveLock parameter in the job
if currentJob.driveLock == False:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
else:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
if self.auxiliary2.getRawButton(1) == True:
self.adjust_ready = False
self.checkSwitches()
print('vision entered')
currentPosition = self.pulleyMotor.getQuadraturePosition()
'''(self.IRSensor.getVoltage() > 0.1) and''' '''(self.IRSensor.getVoltage() < self.ball_dist) and '''
if ((self.adjust_ready == False) and (self.auxiliary2.getRawButton(6) == True)): #rocket ball
print('Quad pos is ' + str(currentPosition))
print('entered the rocket ball')
self.pulleyMotor.set(self.pulleyMotorModifier)
if currentPosition > -3000:
self.pulleyMotorModifier += 0.05
print('moving at ' + str(self.pulleyMotorModifier))
else:
self.pulleyHeight(6,1)
self.pulleyMotorModifier =0.6
self.adjust_ready= 1
'''
if (self.IRSensor.getVoltage() > 0.1) and (self.IRSensor.getVoltage() < self.hatch_dist) and (self.adjust_ready == 0): #rocket hatch
self.pulleyMotorModifier = 0.05
self.pulleyMotor.set(self.pulleyMotorModifier)
if self.pulleyMotor.getQuadraturePosition() > 10:
self.pulleyMotorModifier += 0.05
else:
self.pulleyHeight(5,1)
self.pulleyMotorModifier =0.5
self.adjust_ready= 1
'''
'''(self.IRSensor.getVoltage() > 0.1) and''' '''(self.IRSensor.getVoltage() < self.ball_dist) and'''
if ((self.adjust_ready == False) and (self.auxiliary2.getRawButton(7) == True)): #cargo ball
print('entered the cargo ball')
print('Quad pos is ' + str(currentPosition))
if currentPosition > -3000:
self.pulleyMotorModifier += 0.05
print('moving at ' + str(self.pulleyMotorModifier))
self.pulleyMotor.set(self.pulleyMotorModifier)
else:
self.pulleyHeight(4,1)
self.pulleyMotorModifier =0.6
self.adjust_ready= 1
if self.adjust_ready == True:
test = 0
testy = 0
testz = 0
try:
test = sd.getValue('adjust_x', 0)
testy = sd.getValue('adjust_y', 0)
testz = sd.getValue('adjust_z', 0)
print('x ' + str(test))
print('y ' + str(testy))
print('z ' + str(testz))
except Exception as e:
print(str(e.args))
self.drive.driveCartesian(self.driveStick.getX(test), self.driveStick.getY(testy), self.driveStick.getZ(testz), 0)
def teleopInit(self):
print('teleop starting')
def checkSwitches(self):
if self.auxiliary1.getRawButton(self.eStop): #E-Stop button pressed, stop all motors and remove all jobs from job queue.
self.frontLeftMotor.set(0)
self.frontRightMotor.set(0)
self.rearLeftMotor.set(0)
self.rearRightMotor.set(0)
self.leftLeadScrewMotor.set(0)
self.rightLeadScrewMotor.set(0)
self.pulleyMotor.set(0)
self.spinBarMotor.set(0)
#Remove all queued jobs by setting the queue to the blank class
self.queue = Queue()
else: #Check every other switch
# for testing this button will set lead screw positions to 0
'''if self.driveStick.getRawButton(1):
print('button 1 works')
self.crabRight(self.crab3)
if self.driveStick.getRawButton(2):
print('button 2 works')
self.driveQuadratureReset()'''
# buttons controlling spinBar (3 position momentary switch)
if self.auxiliary2.getRawButton(self.leftLeadScrewDown): # left lead screw out manual
self.leftLeadScrewMotor.set(self.lifterSpeed)
#print(str(self.leftLeadScrewMotor.getQuadraturePosition()))
elif self.auxiliary2.getRawButton(self.leftLeadScrewUp): # left lead screw in manual
self.leftLeadScrewMotor.set(-1 * self.lifterSpeed)
#print(str(self.leftLeadScrewMotor.getQuadraturePosition()))
else:
self.leftLeadScrewMotor.set(0)
#print(str(self.leftLeadScrewMotor.getQuadraturePosition()))
if self.auxiliary2.getRawButton(self.rightLeadScrewDown): # right lead screw out manual
self.rightLeadScrewMotor.set(self.lifterSpeed)
elif self.auxiliary2.getRawButton(self.rightLeadScrewUp): # right lead screw in manual
self.rightLeadScrewMotor.set(-1 * self.lifterSpeed)
else:
self.rightLeadScrewMotor.set(0)
if self.auxiliary2.getRawButton(self.spinBarIn): # cargo collecting
#if self.IRSensor.getVoltage() < self.IRSensorThreshold: # IR distance sensor stops the spinBar from spinning in when the ball is already in
#self.spinBarMotor.set(-1)
#else:
#self.spinBarMotor.set(0)
self.spinBarMotor.set(-1)
elif self.auxiliary2.getRawButton(self.spinBarOut): # manual cargo depositing
self.spinBarMotor.set(1)
elif self.auxiliary1.getRawButton(self.manualHatchDeposit): # manual hatch depositing
self.spinBarMotor.set(self.hatchDepositSpeed)
else:
self.spinBarMotor.set(0)
if self.auxiliary2.getRawButton(self.manualPulleyUp) and self.auxiliary2.getRawButton(1) == False: # manual pulley up
print('you pulled pulley up')
self.pulleyMotor.set(-1*self.pulleyMotorModifier)
elif self.auxiliary2.getRawButton(self.manualPulleyDown) and self.auxiliary2.getRawButton(1) == False: # manual pulley down
print('you pulled pulley down')
self.pulleyMotor.set(self.pulleyMotorModifier)
else:
self.pulleyMotor.set(0)
# buttons controlling Pulley (2 buttons and a rotary switch)
# hatch buttons
if self.auxiliary1.getRawButton(self.autoHatchDeposit): # hatch movement and depositing (auto)
self.depositPayload(self.levelSelector(), 1)
#Liam's thing
elif self.auxiliary1.getRawButton(11):
self.spinBarMotor.set(.2)
elif self.auxiliary1.getRawButton(self.hatchCollectHeight): # hatch collecting (from player station)
hatchCollectManual = Job()
hatchCollectManual.function = 'self.pulleyHeight'
hatchCollectManual.parameters = '(1, 1)'
hatchCollectManual.driveLock = True
self.queue.add(hatchCollectManual)
hatchCollectManual = Job()
hatchCollectManual.function = 'self.resetSpinBar'
hatchCollectManual.parameters = '()'
hatchCollectManual.driveLock = False
self.queue.add(hatchCollectManual)
# cargo buttons
elif self.auxiliary1.getRawButton(self.autoCargoDeposit): # cargo movement and depositing
self.depositPayload(self.levelSelector(), 2)
elif self.auxiliary1.getRawButton(self.autoCargoShipDeposit): # cargo ship depositing
x=self.levelSelector()
self.depositPayload(self.levelSelector(), 3)
# Pulley reset button
elif self.auxiliary1.getRawButton(self.pulleyReset): # pulley reset
resetPulley = Job()
resetPulley.function = 'self.resetPulley'
resetPulley.parameters = '()'
resetPulley.driveLock = False
self.queue.add(resetPulley)
# buttons controlling baseLifter (3 buttons)
elif self.auxiliary1.getRawButton(self.hab1to2): # hab level 1 to level 2
self.hab(1, 2)
elif self.auxiliary1.getRawButton(self.hab1to3): # hab level 1 to level 3
self.hab(1, 3)
elif self.auxiliary1.getRawButton(self.hab2to3): # hab level 2 to level 3
self.hab(2, 3)
else:
pass
#reset of the lead screws to drive height based on a joystick button
while self.driveStick.getRawButton(8):
if self.topRightLeadScrewHallEffect:
self.rightLeadScrewMotor.set(-.3)
else:
self.rightLeadScrewMotor.set(0)
self.rightLeadScrewMotor.setQuadraturePosition(0,0)
if self.topLeftLeadScrewHallEffect:
self.leftLeadScrewMotor.set(-.3)
else:
self.leftLeadScrewMotor.set(0)
self.leftLeadScrewMotor.setQuadraturePosition(0,0)
def teleopPeriodic(self):
# checks switches and sensors, which feed the queue with jobs
self.checkSwitches()
# we are checking if a job is in the queue, and then calling the function that the first job makes using eval
#print('br' + str(self.rearLeftMotor.getQuadraturePosition()))
#print('fl' + str(self.frontLeftMotor.getQuadraturePosition()))
if len(self.queue.queue) > 0:
currentJob = self.queue.peek()
print(str(currentJob.function))
eval(currentJob.function + currentJob.parameters)
#allows the driver to drive the robot when the currentJob allows them to, using the driveLock parameter in the job
if currentJob.driveLock == False:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
else:
self.drive.driveCartesian(self.driveStick.getX(), self.driveStick.getY(), self.driveStick.getZ(), 0)
if self.auxiliary2.getRawButton(1) == True:
self.adjust_ready = False
self.checkSwitches()
#print('vision entered')
currentPosition = self.pulleyMotor.getQuadraturePosition()
'''(self.IRSensor.getVoltage() > 0.1) and''' '''(self.IRSensor.getVoltage() < self.ball_dist) and '''
if ((self.adjust_ready == False) and (self.auxiliary2.getRawButton(6) == True)): #rocket ball
#print('Quad pos is ' + str(currentPosition))
#print('entered the rocket ball')
self.pulleyMotor.set(self.pulleyMotorModifier)
if currentPosition > -3000:
self.pulleyMotorModifier += 0.05
# print('moving at ' + str(self.pulleyMotorModifier))
else:
self.pulleyHeight(6,1)
self.pulleyMotorModifier =0.6
self.adjust_ready= 1
'''
if (self.IRSensor.getVoltage() > 0.1) and (self.IRSensor.getVoltage() < self.hatch_dist) and (self.adjust_ready == 0): #rocket hatch
self.pulleyMotorModifier = 0.05
self.pulleyMotor.set(self.pulleyMotorModifier)
if self.pulleyMotor.getQuadraturePosition() > 10:
self.pulleyMotorModifier += 0.05
else:
self.pulleyHeight(5,1)
self.pulleyMotorModifier =0.5
self.adjust_ready= 1
'''
'''(self.IRSensor.getVoltage() > 0.1) and''' '''(self.IRSensor.getVoltage() < self.ball_dist) and'''
if ((self.adjust_ready == False) and (self.auxiliary2.getRawButton(7) == True)): #cargo ball
#print('entered the cargo ball')
#print('Quad pos is ' + str(currentPosition))
if currentPosition > -3000:
self.pulleyMotorModifier += 0.05
# print('moving at ' + str(self.pulleyMotorModifier))
self.pulleyMotor.set(self.pulleyMotorModifier)
else:
self.pulleyHeight(4,1)
self.pulleyMotorModifier =0.6
self.adjust_ready= 1
if self.adjust_ready == True:
print('vision drive entered')
test = 0
testy = 0
testz = 0
try:
test = sd.getValue('adjust_x', 0)
testy = sd.getValue('adjust_y', 0)
testz = sd.getValue('adjust_z', 0)
print('x ' + str(test))
print('y ' + str(testy))
print('z ' + str(testz))
except Exception as e:
print(str(e.args))
self.drive.driveCartesian(self.driveStick.getX(testx), self.driveStick.getY(testy), self.driveStick.getZ(testz), 0)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
# NetworkTables.initialize()
# self.sd = NetworkTables.getTable('SmartDashboard')
wpilib.CameraServer.launch()
# Inicializadores_de_PCM (en caso de que no arranque el PCM)
# self.Compressor.setClosedLoopControl(True)
# self.enabled = self.Compressor.enabled()
#Solenoides y Compresores
self.Compressor = wpilib.Compressor(0)
self.PSV = self.Compressor.getPressureSwitchValue()
self.piston = wpilib.Solenoid(0, 0)
self.impulsor_frontal = wpilib.DoubleSolenoid(0, 2, 3)
self.impulsor_trasero = wpilib.DoubleSolenoid(0, 4, 5)
# Encoders y otros Sensores
self.encoder = wpilib.Encoder(8, 7)
self.left_sensor = wpilib.DigitalInput(0)
self.principal_sensor = wpilib.DigitalInput(1)
self.right_sensor = wpilib.DigitalInput(2)
self.ultrasonic = wpilib.Ultrasonic(3, 4)
self.prueba_sensor = wpilib.DigitalInput(5)
self.P = 0.2
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
# Contador y Control
self.timer = wpilib.Timer()
# Motores del Chasis
self.front_left_motor = wpilib.Talon(0)
self.rear_left_motor = wpilib.Talon(1)
self.front_right_motor = wpilib.Talon(2)
self.rear_right_motor = wpilib.Talon(3)
#lift and claw motors
self.lift_motor = wpilib.Talon(4)
self.lift_motor_2 = wpilib.Talon(5)
#Union de los motores para su funcionamiento
# en conjunto de mecaunm
self.drive = MecanumDrive(self.front_left_motor, self.rear_left_motor,
self.front_right_motor,
self.rear_right_motor)
#Motor impulsor
self.motor_impulsor = wpilib.Talon(6)
def autonomousInit(self):
self.timer.reset()
self.timer.start()
state["timer_piston"] = 0
def autonomousPeriodic(self):
if self.timer.get() < .8:
self.drive.driveCartesian(0, -0.9, 0, 0)
print("salto de la plataforma hacia atrás")
elif self.timer.get() < 3:
self.drive.driveCartesian(0, -0.1, 0, 0)
print("avanza un poco más, en reversa")
elif self.timer.get() < 4.35:
self.drive.driveCartesian(0, 0, -0.4, 0)
print("gira en su propio eje derecha/izquierda?")
elif self.timer.get() < 6:
self.timer.stop()
# while self.prueba_sensor.get():
while self.ultrasonic.getRangeMM(
) < 20 and self.ultrasonic.getRangeMM() > 0:
print("en modo de infrarrojos")
if self.principal_sensor.get():
self.drive.driveCartesian(0, 0, 0, 0)
self.timer.start()
break
elif self.left_sensor.get():
self.drive.driveCartesian(0.2, 0, 0, 0)
elif self.right_sensor.get():
self.drive.driveCartesian(-0.2, 0, 0, 0)
else:
self.drive.driveCartesian(0, -0.2, 0, 0)
else:
self.drive.driveCartesian(0, 0.3, 0, 0)
print("avanza hacia adelante hasta ultra detect")
elif self.timer.get() < 6.5:
self.piston.set(True)
print("psss lanzar")
elif self.timer.get() < 7:
self.piston.set(False)
print("psss retraer")
else:
print("autonomo terminado")
self.drive.driveCartesian(0, 0, 0, 0)
def teleopPeriodic(self):
#se leen constantemente los botones,joysticks y cambia de modalidades de controles
oi.read_control_inputs(state["Controller"])
self.PID()
self.timer.start()
# Funcion del Encoder
def Encoder(setpoint):
state["setpoint"] = setpoint
if self.rcw >= 660:
state["lift_motor"] = 0.5
elif self.rcw <= 660 and self.rcw >= 460:
state["lift_motor"] = 0.45
elif self.rcw <= 460 and self.rcw >= 300:
state["lift_motor"] = 0.4
elif self.rcw <= 300 and self.rcw >= 200:
state["lift_motor"] = 0.35
elif self.rcw <= 200 and self.rcw >= 102:
state["lift_motor"] = 0.3
elif self.rcw <= 102.00:
state["lift_motor"] = 0
if state["codewide_breaker"] == False:
# Movimiento manual de las mecanum, align y turbo
x = state["mov_x"]
y = state["mov_y"]
z = state["mov_z"]
powerX = 0 if x < 0.10 and x > -0.10 else x
powerY = 0 if y < 0.10 and y > -0.10 else y
powerZ = 0 if z < 0.10 and z > -0.10 else z
if state["align_activated"]:
if self.principal_sensor.get():
self.drive.driveCartesian(0, 0, 0, 0)
elif self.left_sensor.get():
self.drive.driveCartesian(0.2, 0, 0, 0)
elif self.right_sensor.get():
self.drive.driveCartesian(-0.2, 0, 0, 0)
else:
self.drive.driveCartesian(0, -0.2, 0, 0)
elif state["turbo_activated"]:
self.drive.driveCartesian(powerX, -powerY, powerZ, 0)
else:
self.drive.driveCartesian(powerX * 0.6, -powerY * 0.6,
powerZ * 0.5, 0)
# Configuracion para el elevador automaticamente
# Hatch panel medio y piston
if state["position"] == "media" and state["mechanism"] == "piston":
state["timer_lift_middle"] += 1
if state["timer_lift_middle"] < 240:
Encoder(1621)
elif state["timer_lift_middle"] < 275:
state["piston_activated"] = True
elif state["timer_lift_middle"] < 310:
state["piston_activated"] = False
elif state["timer_lift_middle"] < 510:
state["lift_motor"] = -0.5
else:
state["timer_lift_middle"] = 0
state["position"] = "neutral"
state["mechanism"] = "neutral"
if state["position"] == "high" and state["mechanism"] == "piston":
state["timer_lift_taller"] += 1
if state["timer_lift_taller"] < 240:
Encoder(1621)
elif state["timer_lift_taller"] < 275:
state["piston_activated"] = True
elif state["timer_lift_taller"] < 310:
state["piston_activated"] = False
elif state["timer_lift_taller"] < 510:
state["lift_motor"] = -0.5
else:
state["timer_lift_taller"] = 0
state["position"] = "neutral"
state["mechanism"] = "neutral"
# Configuracion para mover el elevador y la claw manualmente
self.lift_motor.set(state["lift_motor"])
self.lift_motor_2.set(state["lift_motor"])
# Pistons (manual) and Compressors (automatico)
self.piston.set(state["piston_activated"])
if self.PSV:
self.Compressor.stop()
else:
self.Compressor.start()
# Immpulsor (Manual y automaticamente)
self.impulsor_frontal.set(state["impulsor_situation_front"])
self.impulsor_trasero.set(state["impulsor_situation_trasero"])
self.motor_impulsor.set(state["impulsor_motor"])
if state["impulsor_on"] or state["timer_impulsor"] != 0:
state["timer_impulsor"] += 1
if state["timer_impulsor"] < 150:
state["impulsor_situation_front"] = 1
state["impulsor_situation_trasero"] = 1
elif state["timer_impulsor"] < 180:
state["impulsor_situation_front"] = 0
state["impulsor_situation_trasero"] = 0
elif state["timer_impulsor"] < 250:
state["impulsor_motor"] = 1
elif state["timer_impulsor"] < 400:
state["impulsor_situation_front"] = 2
state["impulsor_motor"] = 1
self.drive.driveCartesian(0, 0.4, 0, 0)
elif state["timer_impulsor"] < 600:
state["impulsor_situation_trasero"] = 2
state["impulsor_motor"] = 0
elif state["timer_impulsor"] < 700:
self.drive.driveCartesian(0, 0.6, 0, 0)
state["impulsor_situation_trasero"] = 0
else:
state["timer_impulsor"] = 0
state["impulsor_situation_front"] = 0
state["impulsor_situation_trasero"] = 0
state["impulsor_motor"] = 0
self.drive.driveCartesian(0, 0, 0, 0)
else:
pass
else:
self.drive.driveCartesian(0, 0, 0, 0)
self.impulsor_frontal.set(0)
self.impulsor_trasero.set(0)
self.motor_impulsor.set(0)
self.piston.set(False)
self.lift_motor.set(0)
self.lift_motor_2.set(0)
state["impulsor_trasero"] = 0
state["impulsor_frontal"] = 0
state["impulsor_situation_trasero"] = 0
state["impulsor_situation_front"] = 0
state["impulsor_motor"] = 0
state["piston_activated"] = False
state["lift_motor"] = 0
state["position"] = "neutral"
state["mechanism"] = "neutral"
state["timer_piston"] = 0
state["timer_impulsor"] = 0
state["timer_lift_taller"] = 0
state["timer_lift_middle"] = 0
state["align_activated"] = False
state["turbo_activated"] = False
def PID(self):
error = state["setpoint"] - 400 #self.encoder.get()
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
self.rcw = self.P * error + self.I * self.integral + self.D * derivative
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 3
rearRightChannel = 4
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
WHEEL_DIAMETER = 0.5 # 6 inches
ENCODER_COUNTS_PER_REV = 3
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
self.sd = NetworkTables.getTable("SmartDashboard")
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
#Create the DriveTrain
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
#Create The Encoders
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_l_encoder.setSimDevice(0)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_l_encoder.setSimDevice(1)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.f_r_encoder.setSimDevice(2)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse((.0254 * 6 * math.pi) / 1024)
#self.r_r_encoder.setSimDevice(3)
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
# Now that we have created the ability to see wheel speeds, chassis speeds and our position
# Let us start to use feedforward to try to lock our robot into a specific speed.
self.feedForward = SimpleMotorFeedforwardMeters(kS=0.194,
kV=.5,
kA=0.457)
#def robotPeriodic(self):
# Odometry Update
# First, get the wheel speeds...
# Next, we need to grab the Gyro angle and send it into the odometry. It must be inverted because gyros v Wpilib are backwards
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.timer = wpilib.Timer()
self.timer.start()
self.f_l_encoder.reset()
self.r_l_encoder.reset()
self.f_r_encoder.reset()
self.r_r_encoder.reset()
self.lastCount = 0
def autonomousPeriodic(self):
preChassis = ChassisSpeeds()
preChassis.vx = 5.0
preChassis.vy = 0.0
preChassis.omega = 0.0
# Convert to wheel speeds
speeds = self.m_kinematics.toWheelSpeeds(preChassis)
self.wheelSpeeds = MecanumDriveWheelSpeeds(
self.f_l_encoder.getRate(),
self.r_l_encoder.getRate(),
self.f_r_encoder.getRate(),
self.r_r_encoder.getRate(),
)
gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle())
# Finally, we can update the pose...
self.m_pose = self.MecanumDriveOdometry.update(gyroAngle,
self.wheelSpeeds)
#For Kinematics, we need to update the wheelspeeds
CurrentChassis = self.m_kinematics.toChassisSpeeds(self.wheelSpeeds)
print(CurrentChassis)
print(self.f_l_encoder.getDistancePerPulse())
print('difference')
print(self.f_l_encoder.get() - self.lastCount)
print('rate')
print(self.r_r_encoder.getRate())
print('lastCount')
self.lastCount = self.f_l_encoder.get()
print(self.lastCount)
'''
left_front = self.feedForward.calculate(speeds.frontLeft)s
right_front = self.feedForward.calculate(speeds.frontRight)
left_rear = self.feedForward.calculate(speeds.rearLeft)
right_rear = self.feedForward.calculate(speeds.rearRight)'''
#print(left_front, right_front, left_rear,right_rear)
if self.timer.get() < 2.0:
# self.drive.driveCartesian(1, 1, 1, 1) #<---- This is using the drive method built into the mecanum dirve.
# Maybe we want to use something with more control, like feedforward...
'''self.frontLeftMotor.set(-left_front)
self.rearLeftMotor.set(right_front)
self.frontRightMotor.set(-left_rear)
self.rearRightMotor.set(right_rear)'''
self.drive.driveCartesian(1, 0, 0, 0)
elif self.timer.get() > 2 and self.timer.get() < 4:
self.drive.driveCartesian(1, 0, 0, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
# self.drive.driveCartesian(
# self.lstick.getX(), -self.lstick.getY(), self.rstick.getX(), 0
# )
self.drive.driveCartesian(self.lstick.getX(), -self.lstick.getY(),
self.lstick.getRawAxis(2), 0)
class SpootnikDrives(PIDSubsystem):
def __init__(self):
super().__init__(p=0.015, i=0.0001, d=0.0)
self._deadband = 0.1
self._turnOutput = 0.0
# Configure motors
motors = [
WPI_TalonSRX(i)
for i in [RM.DRIVE_LF, RM.DRIVE_LB, RM.DRIVE_RF, RM.DRIVE_RB]
]
for i, motor in enumerate(motors):
# motor.configFactoryDefault()
motor.enableVoltageCompensation(True)
motor.configOpenLoopRamp(1.4, 10)
motor.setNeutralMode(NeutralMode.Brake)
# Invert left side motors?
# if i <= 1:
# motor.setInverted(True)
# Set up PIDSubsystem parameters
self.setInputRange(0.0, 360.0)
self.pidController = self.getPIDController()
self.pidController.setContinuous(True)
self.pidController.setAbsoluteTolerance(1.0)
self.setSetpoint(0.0)
# Enable this is you use the PID functionality
# self.pidController.enable()
self.drive = MecanumDrive(*motors)
self.drive.setExpiration(1)
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0.1)
def returnPIDInput(self):
return 0.0
def usePIDOutput(self, output: float):
self._turnOutput = output
def initDefaultCommand(self):
self.setDefaultCommand(self.DriveWithJoy(self))
# Define commands
class DriveWithJoy(Command):
def __init__(self, spootnikDrives):
super().__init__("DriveWithJoy")
self.logger = logging.getLogger("DriveWithJoy")
self.requires(spootnikDrives)
self._spootnikDrives = spootnikDrives
self._driveSwitcherVal = Command.getRobot(
).driveSwitcher.getSelected()
self._printDriveType()
def execute(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self._spootnikDrives.driveCartesianWithJoy()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self._spootnikDrives.morpheusDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self._spootnikDrives.fieldOrientedDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self._spootnikDrives.noJoyDrive()
def isFinished(self):
return False
def _printDriveType(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self.logger.info("WPI Mecanum Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self.logger.info("Morpheus Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self.logger.info("Field Oriented Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self.logger.info("No Joy Drive selected")
class DriveForTime(TimedCommand):
def __init__(self, spootnikDrives, time):
super().__init__(name="DriveForTime",
timeoutInSeconds=time,
subsystem=spootnikDrives)
self.logger = logging.getLogger("DriveForTime")
self._spootnikDrives = spootnikDrives
self._angle = 0.0
def initialize(self):
self.logger.info("initialize")
# self._spootnikDrives.drivelyMoreBetterer(0.8, 0.0, 0.0, 0.0)
self._spootnikDrives.drive.driveCartesian(0.0, 0.8, 0.0)
def end(self):
self.logger.info("end")
self._spootnikDrives.drive.driveCartesian(0.0, 0.0, 0.0)
# Define support functions
def driveCartesianWithJoy(self):
joy = Command.getRobot().oi.driveJoy
ySpeed = joy.getX(GenericHID.Hand.kLeft)
xSpeed = -joy.getY(GenericHID.Hand.kLeft)
zRotation = joy.getX(GenericHID.Hand.kRight)
self.drive.driveCartesian(ySpeed, xSpeed, zRotation)
def morpheusDrive(self):
print("INFO: morpheusDrive currently unimplemented")
# def fieldOrientedDrive(self):
# lx = Command.getRobot().jml.getLX()
# ly = -Command.getRobot().jml.getLY()
# rx = Command.getRobot().jml.getRX()
lx = 0.0
ly = 0.0
rx = 0.0
lx = self.applyDeadband(lx)
ly = self.applyDeadband(ly)
rx = self.applyDeadband(rx)
self.drivelyMoreBetterer(ly, lx, rx)
def noJoyDrive(self):
print("INFO: noJoyDrive currently unimplemented")
def drivelyMoreBetterer(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
pass
# print("INFO: drivelyMoreBetterer currently unimplemented")
def applyDeadband(self, val):
return val if abs(val) > self._deadband else 0.0
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
"""Robot initialization function. The Low level is to use the brushless function on the controllers."""
self.sd = NetworkTables.getTable("SmartDashboard")
self.timer = wpilib.Timer()
#
# 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.navx = navx.AHRS.create_spi()
# self.navx = navx.AHRS.create_i2c()
if wpilib.RobotBase.isSimulation():
self.frontLeftMotor = wpilib.Jaguar(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Jaguar(self.rearLeftChannel)
self.frontRightMotor = wpilib.Jaguar(self.frontRightChannel)
self.rearRightMotor = wpilib.Jaguar(self.rearRightChannel)
else:
self.frontLeftMotor = rev.CANSparkMax(
self.frontLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearLeftMotor = rev.CANSparkMax(
self.rearLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.frontRightMotor = rev.CANSparkMax(
self.frontRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearRightMotor = rev.CANSparkMax(
self.rearRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
# invert the left side motors
self.rearRightMotor.setInverted(False)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.frontRightMotor,
self.rearLeftMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.stick = wpilib.XboxController(self.joystickChannel)
def robotPeriodic(self):
self.logger.info("Entered disabled mode")
self.timer.reset()
self.timer.start()
if self.timer.hasPeriodPassed(0.5):
self.sd.putNumber("Displacement X", self.navx.getDisplacementX())
self.sd.putNumber("Displacement Y", self.navx.getDisplacementY())
self.sd.putBoolean("IsCalibrating", self.navx.isCalibrating())
self.sd.putBoolean("IsConnected", self.navx.isConnected())
self.sd.putNumber("Angle", self.navx.getAngle())
self.sd.putNumber("Pitch", self.navx.getPitch())
self.sd.putNumber("Yaw", self.navx.getYaw())
self.sd.putNumber("Roll", self.navx.getRoll())
def autonomousInit(self):
"""Runs Once during auto"""
# self.counter = 0
def autonomousPeriodic(self):
"""Runs Periodically during auto"""
self.sd.putNumber("Timestamp", self.navx.getLastSensorTimestamp())
self.drive.driveCartesian(0, 0.5, 0, 0)
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
def robotInit(self):
"""Robot initialization function. The Low level is to use the brushless function on the controllers."""
'''if wpilib.RobotBase.isSimulation():
self.frontLeftMotor = ctre.WPI_VictorSPX(self.frontLeftChannel)
self.rearLeftMotor = ctre.WPI_VictorSPX(self.rearLeftChannel)
self.frontRightMotor = ctre.WPI_VictorSPX(self.frontRightChannel)
self.rearRightMotor = ctre.WPI_VictorSPX(self.rearRightChannel)
else: '''
self.frontLeftMotor = rev.CANSparkMax(
self.frontLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearLeftMotor = rev.CANSparkMax(
self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.frontRightMotor = rev.CANSparkMax(
self.frontRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearRightMotor = rev.CANSparkMax(
self.rearRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
# invert the left side motors
self.rearRightMotor.setInverted(False)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.frontRightMotor,
self.rearLeftMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.stick = wpilib.XboxController(self.joystickChannel)
self.lift = ctre.WPI_VictorSPX(6)
def autonomousInit(self):
"""Runs Once during auto"""
def autonomousPeriodic(self):
"""Runs Periodically during auto"""
self.drive.driveCartesian(0.5, 0, 0, 0)
def teleopInit(self):
"""Runs Once during teleop"""
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with Mecanum drive."""
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
self.drive.driveCartesian(self.stick.getRawAxis(2),
self.stick.getRawAxis(3),
self.stick.getRawAxis(0), 0)
self.lift.set(self.stick.getRawButton(1))
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 3
rearRightChannel = 4
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
WHEEL_DIAMETER = 0.5 # 6 inches
ENCODER_COUNTS_PER_REV = 360
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTables.getTable("SmartDashboard")
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
def robotPeriodic(self):
# Odometry Update
# First, get the wheel speeds...
self.wheelSpeeds = MecanumDriveWheelSpeeds(
self.f_l_encoder.getRate(),
self.r_l_encoder.getRate(),
self.f_r_encoder.getRate(),
self.r_r_encoder.getRate(),
)
# Next, we need to grab the Gyro angle and send it into the odometry. It must be inverted because gyros v Wpilib are backwards
gyroAngle = Rotation2d.fromDegrees(-self.gyro.getAngle())
# Finally, we can update the pose...
self.m_pose = self.MecanumDriveOdometry.update(gyroAngle,
self.wheelSpeeds)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.timer = wpilib.Timer()
self.timer.start()
def autonomousPeriodic(self):
if self.timer.get() < 2.0:
self.drive.driveCartesian(0, 1, 0, 0)
else:
self.drive.driveCartesian(0, 0, 1, 0)
print(float(self.f_l_encoder.getRate()))
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
# self.drive.driveCartesian(
# self.lstick.getX(), -self.lstick.getY(), self.rstick.getX(), 0
# )
self.drive.driveCartesian(self.lstick.getX(), -self.lstick.getY(),
self.lstick.getRawAxis(2), 0)
class MyRobot(wpilib.SampleRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
def robotInit(self):
"""Robot initialization function"""
self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel)
self.frontRightMotor = wpilib.Talon(self.frontRightChannel)
self.rearRightMotor = wpilib.Talon(self.rearRightChannel)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomous(self):
"""Called when autonomous mode is enabled"""
timer = wpilib.Timer()
timer.start()
while self.isAutonomous() and self.isEnabled():
if timer.get() < 2.0:
self.drive.driveCartesian(0, -1, 1, 0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
wpilib.Timer.delay(0.01)
def operatorControl(self):
"""Called when operation control mode is enabled"""
while self.isOperatorControl() and self.isEnabled():
self.drive.driveCartesian(self.lstick.getX(), self.lstick.getY(),
self.rstick.getX(), 0)
wpilib.Timer.delay(0.04)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
#motores
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
self.lift_motor = wpilib.Talon(4)
self.cargo_motor = wpilib.Talon(5)
#sensores
self.sensor_izquierdo = wpilib.DigitalInput(1)
self.sensor_principal = wpilib.DigitalInput(2)
self.sensor_derecho = wpilib.DigitalInput(3)
#invertidores de motores
self.frontLeftMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
#Unión de los motores para su funcionamiento
# en conjunto de mecaunm
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
def teleopPeriodic(self):
def getThrottle(self):
return self._getRawAxis(0)
#se leen constantemente los botones y joysticks
oi.read_all_controller_inputs()
#código para el funcionamiento del movimiento
# de las mecanum a través del control de xbox
x = state["mov_x"]
y = state["mov_y"] * 0.7
z = state["mov_z"] * 0.7
if state["mov_xr"] != 0 and state["mov_xl"] == 0:
x = state["mov_xr"] * 0.7
if state["mov_xl"] != 0 and state["mov_xr"] == 0:
x = state["mov_xl"] * 0.7
powerX = 0 if x < 0.05 and x > -0.05 else x
powerY = 0 if y < 0.05 and y > -0.05 else y
powerZ = 0 if z < 0.05 and z > -0.05 else z
self.drive.driveCartesian(powerX, powerY, powerZ, 0)
#código para el funcionamiento del elevador y la garra
if state["activating_lift"]:
state["timer_lift"] += 1
if state["timer_lift"] <= 100:
self.lift_motor.set(1)
elif state["timer_lift"] <= 200:
self.lift_motor.set(0)
self.cargo_motor.set(-1)
elif state["timer_lift"] <= 300:
self.lift_motor.set(-1)
self.cargo_motor.set(0)
else:
self.lift_motor.set(0)
self.cargo_motor.set(0)
else:
state["timer_lift"] = 0
self.lift_motor.set(0)
self.cargo_motor.set(0)
class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 4
rearRightChannel = 3
ballGrabMotor = 1
ballTickler = 2
# The channel on the driver station that the joystick is connected to
lStickChannel = 0
rStickChannel = 1
# solenoids
PCM_CANID = 6
GEAR_ADJUST_RETRACT_SOLENOID = 0
GEAR_ADJUST_EXTEND_SOLENOID = 1
GEAR_DOOR_DROP_SOLENOID = 3
GEAR_DOOR_RAISE_SOLENOID = 2
GEAR_PUSHER_RETRACT_SOLENOID = 4
GEAR_PUSHER_EXTEND_SOLENOID = 5
BALL_DOOR_OPEN_SOLENOID = 6
BALL_DOOR_CLOSE_SOLENOID = 7
GRABBER_STATE = False
TICKLER_STATE = False
COMPRESSOR_STATE = False
GEAR_DOOR_STATE = False
GEAR_ADJUST_STATE = False
GEAR_PUSHER_STATE = False
AUTO_STATE = 0
WHEEL_CIRCUMFERENCE = 19 #inches
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.auto_timer = wpilib.Timer()
self.gyro = AHRS.create_spi()
self.gyro.reset()
self.compressor = wpilib.Compressor(self.PCM_CANID)
self.compressor.setClosedLoopControl(False)
#self.compressor.setClosedLoopControl(True)
#Solenoids galore
self.gearAdjustExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_EXTEND_SOLENOID)
self.gearAdjustRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_RETRACT_SOLENOID)
self.gearPusherExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_EXTEND_SOLENOID)
self.gearPusherRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_RETRACT_SOLENOID)
self.gearDoorDrop = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_DROP_SOLENOID)
self.gearDoorRaise = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_RAISE_SOLENOID)
self.ballDoorOpen = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_OPEN_SOLENOID)
self.ballDoorClose = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_CLOSE_SOLENOID)
"""Robot initialization function"""
self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel)
self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel)
self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel)
self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel)
self.tickler = Spark(self.ballTickler)
self.grabber = Victor(self.ballGrabMotor)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.talons = [
self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor,
self.rearRightMotor
]
for talon in self.talons:
talon.configNominalOutputForward(0.0, 25)
talon.configNominalOutputReverse(0.0, 25)
talon.configPeakOutputForward(1.0, 25)
talon.configPeakOutputReverse(-1.0, 25)
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(11.5, 25)
talon.configOpenLoopRamp(0.125, 25)
talon.config_kP(0, 0.375, 25)
talon.config_kI(0, 0.0, 25)
talon.config_kD(0, 0.0, 25)
talon.config_kF(0, 0.35, 25)
talon.selectProfileSlot(0, 0)
talon.configClosedLoopPeakOutput(0, 1.0, 25)
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 25)
talon.configSelectedFeedbackCoefficient(1.0, 0, 25)
talon.set(ctre.ControlMode.PercentOutput, 0)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(False)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
def disabled(self):
"""Called when the robot is disabled"""
while self.isDisabled():
wpilib.Timer.delay(0.01)
def DriveStraight(self, distance):
pass
def autonomousInit(self):
self.auto_timer.start()
def autonomousPeriodic(self):
#self.auto_timer.delay(5)
while self.AUTO_STATE != 4:
"""
self.frontLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4)
self.frontRightMotor.set(ctre.ControlMode.PercentOutput, 0.4)
self.rearLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4)
self.rearRightMotor.set(ctre.ControlMode.PercentOutput, 0.4)
"""
if self.AUTO_STATE == 0:
self.drive.driveCartesian(0, .5, -.025)
if self.auto_timer.hasPeriodPassed(1.7):
self.AUTO_STATE = 1
#self.auto_timer.reset()
if self.AUTO_STATE == 1:
self.drive.driveCartesian(0, 0, 0)
if self.auto_timer.hasPeriodPassed(1.5):
self.AUTO_STATE = 2
if self.AUTO_STATE == 2:
self.drive.driveCartesian(0, -.5, 0.025)
if self.auto_timer.hasPeriodPassed(2):
self.AUTO_STATE = 3
if self.AUTO_STATE == 3:
self.drive.driveCartesian(0, 0, 0)
#for talon in self.talons:
#talon.stopMotor()
#self.logger.info("STAWP")
self.AUTO_STATE = 4
#self.drive.setExpiration(0.1)
#pass
def conditonAxis(self, axis, deadband, rate, expo, power, minimum,
maximum):
deadband = min(abs(deadband), 1)
rate = max(0.1, min(abs(rate), 10))
expo = max(0, min(abs(expo), 1))
power = max(1, min(abs(power), 10))
if axis > -deadband and axis < deadband:
axis = 0
else:
axis = rate * (math.copysign(1, axis) * ((abs(axis) - deadband) /
(1 - deadband)))
if expo > 0.01:
axis = ((axis * (abs(axis)**power) * expo) + (axis * (1 - expo)))
axis = max(min(axis, maximum), minimum)
return axis
def teleopInit(self):
self.compressor.setClosedLoopControl(False)
self.gyro.reset()
self.solenoid_timer = wpilib.Timer()
self.solenoid_timer.start()
for talon in self.talons:
talon.setQuadraturePosition(0)
self.GearAdjustRetract()
self.GearPusherRetract()
self.GearDoorDrop()
#Solenoid functions
def BallDoorOpen(self):
self.ballDoorOpen.set(True)
self.ballDoorClose.set(False)
self.tickler.set(-0.4)
def BallDoorClose(self):
self.ballDoorOpen.set(False)
self.ballDoorClose.set(True)
self.tickler.set(0.0)
def GearAdjustExtend(self):
self.gearAdjustExtend.set(True)
self.gearAdjustRetract.set(False)
def GearAdjustRetract(self):
self.gearAdjustExtend.set(False)
self.gearAdjustRetract.set(True)
def GearPusherExtend(self):
self.gearPusherExtend.set(True)
self.gearPusherRetract.set(False)
def GearPusherRetract(self):
self.gearPusherExtend.set(False)
self.gearPusherRetract.set(True)
def GearDoorDrop(self):
self.gearDoorDrop.set(True)
self.gearDoorRaise.set(False)
def GearDoorRaise(self):
self.gearDoorDrop.set(False)
self.gearDoorRaise.set(True)
#Ball Grab
def BallGrabStart(self):
self.grabber.set(-0.2)
def BallGrabStop(self):
self.grabber.set(0.0)
def activate(self, extend, retract, state):
self.solenoid_timer.reset()
self.logger.info(state)
if state:
extend()
else:
retract()
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
while self.isEnabled():
'''
self.logger.info("FrontLeft: " + str(self.frontLeftMotor.getMotorOutputVoltage()))
self.logger.info("RearLeft: " + str(self.rearLeftMotor.getMotorOutputVoltage()))
self.logger.info("FrontRight: " + str(self.frontRightMotor.getMotorOutputVoltage()))
self.logger.info("RearRight: " + str(self.rearRightMotor.getMotorOutputVoltage()))
'''
self.drive.driveCartesian(
-self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5,
-1, 1),
self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1,
1),
-self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5,
-1, 1)
#self.gyro.getYaw()
)
if self.solenoid_timer.hasPeriodPassed(0.5):
if JoystickButton(self.lstick, 3).get():
self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE
self.activate(self.compressor.start, self.compressor.stop,
self.COMPRESSOR_STATE)
if JoystickButton(self.lstick, 2).get():
self.TICKLER_STATE = not self.TICKLER_STATE
self.activate(self.BallDoorOpen, self.BallDoorClose,
self.TICKLER_STATE)
if JoystickButton(self.lstick, 1).get():
self.GRABBER_STATE = not self.GRABBER_STATE
self.activate(self.BallGrabStart, self.BallGrabStop,
self.GRABBER_STATE)
#pilotstick 2
if JoystickButton(self.rstick, 3).get():
self.GEAR_DOOR_STATE = not self.GEAR_DOOR_STATE
self.activate(self.GearDoorRaise, self.GearDoorDrop,
self.GEAR_DOOR_STATE)
if JoystickButton(self.rstick, 5).get():
self.GEAR_ADJUST_STATE = not self.GEAR_ADJUST_STATE
self.activate(self.GearAdjustExtend,
self.GearAdjustRetract,
self.GEAR_ADJUST_STATE)
if JoystickButton(self.rstick, 4).get():
self.GEAR_PUSHER_STATE = not self.GEAR_PUSHER_STATE
self.activate(self.GearPusherExtend,
self.GearPusherRetract,
self.GEAR_PUSHER_STATE)
wpilib.Timer.delay(0.04)
class MyRobot(wpilib.TimedRobot):
# Channels on the roboRIO that the motor controllers are plugged in to
frontLeftChannel = 3
rearLeftChannel = 4
frontRightChannel = 2
rearRightChannel = 1
leftLift = 13
rightLift = 14
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
joystickChannel2 = 1
def robotInit(self):
# Construct the Network Tables Object
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('RobotSpeed', .5)
#self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
"""Robot initialization function"""
self.frontLeftMotor = rev.CANSparkMax(self.frontLeftChannel,
rev.MotorType.kBrushless)
self.frontLeftMotor.restoreFactoryDefaults()
self.rearLeftMotor = rev.CANSparkMax(self.rearLeftChannel,
rev.MotorType.kBrushless)
self.rearLeftMotor.restoreFactoryDefaults()
self.frontRightMotor = rev.CANSparkMax(self.frontRightChannel,
rev.MotorType.kBrushless)
self.frontRightMotor.restoreFactoryDefaults()
self.rearRightMotor = rev.CANSparkMax(self.rearRightChannel,
rev.MotorType.kBrushless)
self.rearRightMotor.restoreFactoryDefaults()
#Servo for the shooter angle
#Lift
self.leftLift = rev.CANSparkMax(self.leftLift,
rev.MotorType.kBrushless)
#lift 1 is the motor that moves the hook up.
self.rightLift = rev.CANSparkMax(self.rightLift,
rev.MotorType.kBrushless)
#self.rightLift.setInverted(True)
self.rightLift.follow(self.leftLift, False)
#lift 2 is the motor that moves the hook down.
self.tilt = wpilib.Servo(0)
self.shooter = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.intake = ctre.WPI_VictorSPX(7)
#intake & shooter
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.stick = wpilib.XboxController(self.joystickChannel)
self.stick2 = wpilib.XboxController(self.joystickChannel2)
self.robotSpeed = self.sd.getNumber('RobotSpeed', .5)
def teleopInit(self):
self.drive.setSafetyEnabled(True)
self.robotSpeed = self.sd.getNumber('RobotSpeed', .5)
self.liftSpeed = self.sd.getNumber('Lift Speed', .5)
def teleopPeriodic(self):
"""Runs the motors with Mecanum drive."""
# Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
# This sample does not use field-oriented drive, so the gyro input is set to zero.
#self.motor.set(-0.47)
# drive code...
self.drive.driveCartesian((self.stick.getRawAxis(0) * self.robotSpeed),
(self.stick.getRawAxis(1) * self.robotSpeed),
(self.stick.getRawAxis(4) * self.robotSpeed))
self.intake.set(self.stick.getRawAxis(2) / 2)
self.shooter.set(self.stick.getRawAxis(3))
self.leftLift.set(self.stick.getRawAxis(5))
#Set the shooter angle
currentAngle = self.tilt.getAngle()
if self.stick.getRawButton(2):
self.tilt.setAngle(currentAngle + 1)
elif self.stick.getRawButton(3):
self.tilt.setAngle(currentAngle - 1)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
#motores
k4X = 2
self.mutex = threading.RLock(1)
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
self.output = 0
self.lift_motor = wpilib.Talon(4)
self.cargo_motor = wpilib.Talon(5)
self.result = 0
#self.rcw = pidcontroller.rcw
#self.
#sensores
#self.encoder_left = Encoder(self.pi, settings.PINS['encoder']['left'])
#self.encoder_right = Encoder(self.pi, settings.PINS['encoder']['right'])
self.encoder = wpilib.Encoder(0, 6, True, k4X)
self.sensor_izquierdo = wpilib.DigitalInput(1)
self.sensor_principal = wpilib.DigitalInput(2)
self.sensor_derecho = wpilib.DigitalInput(3)
self.ir = wpilib.AnalogInput(1)
self.ir2 = wpilib.DigitalInput(4)
#invertidores de motores
#self.pid = wpilib.PIDController(P, I, D, self.TwoEncoders, self.Drive)
self.frontLeftMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.timer = wpilib.Timer()
#Unión de los motores para su funcionamiento
# en conjunto de mecaunm
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.setpoint = 1
#self.PIDController = PIDController()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.timer.reset()
self.timer.start()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
# Avanzar 2.5s girar 1s avanzar 1s girar 1s avanzar 3s girar 1s avanzar 2s
if self.timer.get() < 2.5:
self.drive.driveCartesian(1, 0, 0, 0)
elif self.timer.get() > 2.5 and self.timer.get() < 3.5:
self.drive.driveCartesian(0, 0, 1, 0)
elif self.timer.get() > 3.5 and self.timer.get() < 4.5:
self.drive.driveCartesian(1, 0, 0, 0)
elif self.timer.get() > 4.5 and self.timer.get() < 5.5:
self.drive.driveCartesian(0, 0, 1, 0)
elif self.timer.get() > 5.5 and self.timer.get() < 6.5:
self.cargo_motor.set(1)
elif self.timer.get() > 6.5 and self.timer.get() < 7.5:
self.drive.driveCartesian(1, 0, 0, 0)
self.cargo_motor.set(-1)
elif self.timer.get() > 8.5 and self.timer.get() < 9.5:
self.drive.driveCartesian(1, 0, 0, 0)
self.cargo_motor.set(0)
elif self.timer.get() > 10.5 and self.timer.get() < 11.5:
self.drive.driveCartesian(0, 0, 1, 0)
elif self.timer.get() > 11.5 and self.timer.get() < 12.5:
self.drive.driveCartesian(1, 0, 0, 0)
self.cargo_motor.set(1)
elif self.timer.get() > 12.5 and self.timer.get() < 13.5:
self.cargo_motor.set(-1)
elif self.timer.get() > 13.5:
self.cargo_motor.set(0)
# elif self.timer.get() > 26.5 and self.timer.get() < 29.5:
# self.drive.driveCartesian(1,0,0,0)
# elif self.timer.get() > 29.5 and self.timer.get() < 31.5:
# self.drive.driveCartesian(0,0,-1,0)
else:
self.drive.driveCartesian(0, 0, 0, 0)
def teleopPeriodic(self):
#print(PIDController.get(self))
# print(self.setpoint)
#print(self.rcw)
#print (Encoder.getFPGAIndex(self.encoder))
#self.encoder.reset()
#se leen constantemente los botones y joysticks
#print(self.encoder.get())
oi.read_all_controller_inputs()
#código para el funcionamiento del movimiento
# de las mecanum a través del control de xbox
v = max(self.ir.getVoltage(), 0.00001)
d = 62.28 * math.pow(v, -1.092)
print(self.ir2.get())
# Constrain output
#print(max(min(d, 145.0), 22.5))
x = state["mov_x"] * .7
y = state["mov_y"] * .7
z = state["mov_z"] * .7
powerX = 0 if x < 0.15 and x > -0.15 else x
powerY = 0 if y < 0.15 and y > -0.15 else y
powerZ = 0 if z < 0.15 and z > -0.15 else z
if state["button_x_active"]:
if self.sensor_principal.get():
drive_for()
self.drive.driveCartesian(0, 0, 0, 0)
elif self.sensor_izquierdo.get():
self.drive.driveCartesian(0.4, 0, 0, 0)
elif self.sensor_derecho.get():
self.drive.driveCartesian(-0.4, 0, 0, 0)
else:
self.drive.driveCartesian(0, -0.5, 0, 0)
else:
self.drive.driveCartesian(powerX, powerY, powerZ, 0)
#código para el funcionamiento del elevador y la garra
if state["activating_lift"]:
state["timer_lift"] += 1
if state["timer_lift"] <= 100:
self.lift_motor.set(1)
elif state["timer_lift"] <= 200:
self.lift_motor.set(0)
self.cargo_motor.set(-1)
elif state["timer_lift"] <= 300:
self.lift_motor.set(-1)
self.cargo_motor.set(0)
else:
self.lift_motor.set(0)
self.cargo_motor.set(0)
else:
state["timer_lift"] = 0
self.lift_motor.set(0)
self.cargo_motor.set(0)
