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 __init__(self):
print('MecDrive: init called')
super().__init__('MecDrive')
self.logPrefix = "MecDrive: "
self.leftFrontSpdCtrl = wpilib.Talon(
robotmap.driveLine.leftFrontMotorPort)
if robotmap.driveLine.invertLeftFront:
self.leftFrontSpdCtrl.setInverted(True)
self.rightFrontSpdCtrl = wpilib.Talon(
robotmap.driveLine.rightFrontMotorPort)
if robotmap.driveLine.invertRightFront:
self.rightFrontSpdCtrl.setInverted(True)
self.leftRearSpdCtrl = wpilib.Talon(
robotmap.driveLine.leftRearMotorPort)
if robotmap.driveLine.invertLeftRear:
self.leftRearSpdCtrl.setInverted(True)
self.rightRearSpdCtrl = wpilib.Talon(
robotmap.driveLine.rightRearMotorPort)
if robotmap.driveLine.invertRightRear:
self.rightRearSpdCtrl.setInverted(True)
#https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib.drive/MecanumDrive.html#wpilib.drive.mecanumdrive.MecanumDrive
self.mecanumDrive = MecanumDrive(self.leftFrontSpdCtrl,
self.leftRearSpdCtrl,
self.rightFrontSpdCtrl,
self.rightRearSpdCtrl)
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 __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 __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)
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 __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 robotInit(self): #Velcrolastick self.lift_motor1 = wpilib.Spark(6) self.lift_motor2 = wpilib.Spark(7) self.sensor1 = wpilib.DigitalInput(7) self.sensor2 = wpilib.DigitalInput(8) # Ismafeeder self.cargo_motor = wpilib.Spark(4) self.lift_motor = wpilib.Spark(5) # Mecanum drive self.frontLeftMotor = wpilib.Talon(0) self.rearLeftMotor = wpilib.Talon(1) self.frontRightMotor = wpilib.Talon(2) self.rearRightMotor = wpilib.Talon(3) self.sensor_1_mec = wpilib.DigitalInput(9) self.frontLeftMotor.setInverted(True) self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor)
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 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 __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 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 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)
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 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)
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.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 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)
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 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 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 __init__(self, robot):
#Motor Setup
self.frontRightMotor = wpilib.Victor(2) #Yellow doesn't
self.frontLeftMotor = wpilib.Victor(3) #Blue
self.backRightMotor = wpilib.Victor(0) #Red
self.backLeftMotor = wpilib.Victor(5) #Orange doesn't
#Mechanum Drive setup
robot.drive = MecanumDrive(self.frontLeftMotor, self.backLeftMotor,
self.frontRightMotor, self.backRightMotor)
def __init__(self, robot):
#Drivetrain Motor Setup
self.rightFrontMotor = wpilib.Talon(7) #Right front
self.rightBackMotor = wpilib.Talon(6) #Right back
self.leftFrontMotor = wpilib.Talon(8) #Left front
self.leftBackMotor = wpilib.Talon(9) #Left back
#Mecanum Drive Setup
robot.drive = MecanumDrive(self.leftFrontMotor, self.leftBackMotor,
self.rightFrontMotor, self.rightBackMotor)
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)
class MecDrive(Subsystem):
def __init__(self):
print('MecDrive: init called')
super().__init__('MecDrive')
self.logPrefix = "MecDrive: "
self.leftFrontSpdCtrl = wpilib.Talon(
robotmap.driveLine.leftFrontMotorPort)
if robotmap.driveLine.invertLeftFront:
self.leftFrontSpdCtrl.setInverted(True)
self.rightFrontSpdCtrl = wpilib.Talon(
robotmap.driveLine.rightFrontMotorPort)
if robotmap.driveLine.invertRightFront:
self.rightFrontSpdCtrl.setInverted(True)
self.leftRearSpdCtrl = wpilib.Talon(
robotmap.driveLine.leftRearMotorPort)
if robotmap.driveLine.invertLeftRear:
self.leftRearSpdCtrl.setInverted(True)
self.rightRearSpdCtrl = wpilib.Talon(
robotmap.driveLine.rightRearMotorPort)
if robotmap.driveLine.invertRightRear:
self.rightRearSpdCtrl.setInverted(True)
#https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib.drive/MecanumDrive.html#wpilib.drive.mecanumdrive.MecanumDrive
self.mecanumDrive = MecanumDrive(self.leftFrontSpdCtrl,
self.leftRearSpdCtrl,
self.rightFrontSpdCtrl,
self.rightRearSpdCtrl)
# ------------------------------------------------------------------------------------------------------------------
def initDefaultCommand(self):
self.setDefaultCommand(MecDriveTeleopDefaultFPS())
print("{}Default command set to MecDriveTeleopDefaultFPS".format(
self.logPrefix))
def stop(self):
self.mecanumDrive.stopMotor()
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)
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
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)
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 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 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)
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)
