# Imports ---------------------------------------------------------------------
import sys
import vex
from vex import *
import math
# Robot Setup -----------------------------------------------------------------
wheelTravel = 310 # circumference of the wheel (mm)
trackWidth = 370 # width of the chassis (mm)
turnSpeed = 25 # how fast the robot will turn (%)
movementSpeed = 50 # how fast the robot will go forwards and back (%)
brain = vex.Brain()
controller = vex.Controller(vex.ControllerType.PRIMARY)
twoBar = vex.Motor(vex.Ports.PORT1, vex.GearSetting.RATIO18_1, False)
leftMotor = vex.Motor(vex.Ports.PORT14, vex.GearSetting.RATIO18_1, False)
rightMotor = vex.Motor(vex.Ports.PORT15, vex.GearSetting.RATIO18_1, True)
flywheelOne = vex.Motor(vex.Ports.PORT9, vex.GearSetting.RATIO18_1, True)
flywheelTwo = vex.Motor(vex.Ports.PORT10, vex.GearSetting.RATIO18_1, False)
intake = vex.Motor(vex.Ports.PORT16, vex.GearSetting.RATIO18_1, False)
loader = vex.Motor(vex.Ports.PORT11, vex.GearSetting.RATIO18_1, False)
sonar = vex.Sonar(brain.three_wire_port.c)
sonarLeft = vex.Sonar(brain.three_wire_port.e)
sonarRight = vex.Sonar(brain.three_wire_port.g)
dt = vex.Drivetrain(leftMotor, rightMotor, wheelTravel, trackWidth,
vex.DistanceUnits.MM)
import sys
import vex
import math
brain = vex.Brain()
competition = vex.Competition()
controller1 = vex.Controller(vex.ControllerType.PRIMARY)
controller2 = vex.Controller(vex.ControllerType.PARTNER)
leftFront = vex.Motor(vex.Ports.PORT17, vex.GearSetting.RATIO18_1, False)
leftBack = vex.Motor(vex.Ports.PORT2, vex.GearSetting.RATIO18_1, False)
rightBack = vex.Motor(vex.Ports.PORT1, vex.GearSetting.RATIO18_1, True)
rightFront = vex.Motor(vex.Ports.PORT18, vex.GearSetting.RATIO18_1, True)
swerve = vex.Motor(vex.Ports.PORT15, vex.GearSetting.RATIO18_1, True)
catapult = vex.Motor(vex.Ports.PORT13, vex.GearSetting.RATIO36_1, True)
intake = vex.Motor(vex.Ports.PORT14, vex.GearSetting.RATIO18_1, True)
flipper = vex.Motor(vex.Ports.PORT20, vex.GearSetting.RATIO18_1, True)
vision1 = vex.Vision(vex.Ports.PORT11)
green = vex.VisionSignature(1, -1375, -1011, -1193, -5539, -4449, -4994, 3, 0)
class PID:
kP = 0
kI = 0
kD = 0
prevTime = 0