79 lines
2.9 KiB
Python
79 lines
2.9 KiB
Python
import wpilib
|
|
import wpilib.drive
|
|
import math
|
|
import ctre
|
|
|
|
class MyRobot(wpilib.TimedRobot):
|
|
def robotInit(self):
|
|
self.leftFront = ctre.WPI_VictorSPX(0)
|
|
self.leftRear = ctre.WPI_VictorSPX(1)
|
|
self.rightFront = ctre.WPI_VictorSPX(2)
|
|
self.rightRear = ctre.WPI_VictorSPX(3)
|
|
|
|
self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
|
|
self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
|
|
self.right.setInverted(True)
|
|
|
|
self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)
|
|
|
|
self.primary = wpilib.XboxController(0)
|
|
|
|
self.PULSES_PER_REVOLUTION = 512
|
|
self.WHEEL_DIAMETER = 6
|
|
self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION
|
|
|
|
self.leftDrivetrainEncoder = wpilib.Encoder(0, 1)
|
|
self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)
|
|
self.rightDrivetrainEncoder = wpilib.Encoder(2, 3)
|
|
self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)
|
|
|
|
self.gyro = wpilib.ADXRS450_Gyro()
|
|
self.gyro.calibrate()
|
|
|
|
def autonomousInit(self):
|
|
self.leftDrivetrainEncoder.reset()
|
|
self.rightDrivetrainEncoder.reset()
|
|
self.gyro.reset()
|
|
|
|
self.autoState = "DriveForward60"
|
|
|
|
def autonomousPeriodic(self):
|
|
if self.autoState == "DriveForward60":
|
|
if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 60:
|
|
self.drivetrain.tankDrive(.5, .5)
|
|
else:
|
|
self.drivetrain.tankDrive(0, 0)
|
|
self.gyro.reset()
|
|
self.autoState = "Rotate45"
|
|
elif self.autoState == "Rotate45":
|
|
if self.gyro.getAngle() < 45:
|
|
self.drivetrain.tankDrive(.5, -.5)
|
|
else:
|
|
self.drivetrain.tankDrive(0, 0)
|
|
self.leftDrivetrainEncoder.reset()
|
|
self.rightDrivetrainEncoder.reset()
|
|
self.autoState = "DriveForward20"
|
|
elif self.autoState == "DriveForward20":
|
|
if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 20:
|
|
self.drivetrain.tankDrive(.5, .5)
|
|
else:
|
|
self.drivetrain.tankDrive(0, 0)
|
|
self.gyro.reset()
|
|
self.autoState = "RotateNegative90"
|
|
elif self.autoState == "RotateNegative90":
|
|
if self.gyro.getAngle() > -90:
|
|
self.drivetrain.tankDrive(-.5, .5)
|
|
else:
|
|
self.drivetrain.tankDrive(0, 0)
|
|
self.autoState = "End"
|
|
else:
|
|
self.drivetrain.tankDrive(0, 0)
|
|
|
|
def teleopInit(self):
|
|
pass
|
|
|
|
def teleopPeriodic(self):
|
|
self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())
|
|
|
|
if __name__ == "__main__":
|
|
wpilib.run(MyRobot) |