mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
52 lines
1.6 KiB
Python
52 lines
1.6 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
#
|
||
|
|
# Copyright (c) FIRST and other WPILib contributors.
|
||
|
|
# Open Source Software; you can modify and/or share it under the terms of
|
||
|
|
# the WPILib BSD license file in the root directory of this project.
|
||
|
|
#
|
||
|
|
|
||
|
|
import wpilib
|
||
|
|
|
||
|
|
|
||
|
|
class MyRobot(wpilib.TimedRobot):
|
||
|
|
"""
|
||
|
|
This is a demo program showing how to use Mecanum control with the
|
||
|
|
MecanumDrive class.
|
||
|
|
"""
|
||
|
|
|
||
|
|
# Channels on the roboRIO that the motor controllers are plugged in to
|
||
|
|
kFrontLeftChannel = 2
|
||
|
|
kRearLeftChannel = 3
|
||
|
|
kFrontRightChannel = 1
|
||
|
|
kRearRightChannel = 0
|
||
|
|
|
||
|
|
# The channel on the driver station that the joystick is connected to
|
||
|
|
kJoystickChannel = 0
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
super().__init__()
|
||
|
|
self.frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel)
|
||
|
|
self.rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel)
|
||
|
|
self.frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel)
|
||
|
|
self.rearRight = wpilib.PWMSparkMax(self.kRearRightChannel)
|
||
|
|
|
||
|
|
# invert the right side motors
|
||
|
|
# you may need to change or remove this to match your robot
|
||
|
|
self.frontRight.setInverted(True)
|
||
|
|
self.rearRight.setInverted(True)
|
||
|
|
|
||
|
|
self.robotDrive = wpilib.MecanumDrive(
|
||
|
|
self.frontLeft, self.rearLeft, self.frontRight, self.rearRight
|
||
|
|
)
|
||
|
|
|
||
|
|
self.stick = wpilib.Joystick(self.kJoystickChannel)
|
||
|
|
|
||
|
|
def teleopPeriodic(self):
|
||
|
|
# Use the joystick X axis for lateral movement, Y axis for forward
|
||
|
|
# movement, and Z axis for rotation.
|
||
|
|
self.robotDrive.driveCartesian(
|
||
|
|
-self.stick.getY(),
|
||
|
|
-self.stick.getX(),
|
||
|
|
-self.stick.getZ(),
|
||
|
|
)
|