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 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 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.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.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 = 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 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 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"""
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.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):
# 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):
# 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.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.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)
