def execute(self):
reset = True
for i in range(4, 8):
if not oi.OI().drive_buttons[i].get():
reset = False
break
if reset:
self.odemetry.reset()
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
# self.drive.setDirectionOutput(x_speed, y_speed, rotation)
epsilon = 1e-3
if Constants.TANK_PERCENT_OUTPUT:
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
else:
if abs(x_speed) <= epsilon and abs(y_speed) <= epsilon and abs(
rotation) <= epsilon:
self.drive.setPercentOutput(0, 0, 0, 0)
else:
self.drive.setDirectionVelocity(x_speed * self.multiplier,
y_speed * self.multiplier,
rotation * self.multiplier)
def execute(self):
power = -math.pow(oi.OI().getJoystick().getY(),
Constants.TANK_DRIVE_EXPONENT)
rotation = -oi.OI().getJoystick().getZ()
rotation = (rotation, 0)[abs(rotation) < 0.05]
left = power - rotation
right = power + rotation
drive.Drive().setPercentOutput(left, right)
def execute(self):
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
def robotInit(self):
Command.getRobot = lambda x=0: self
wpilib.CameraServer.launch("vision.py:main")
self.oi = oi.OI()
def execute(self):
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_ROTATION_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
if Constants.TANK_PERCENT_OUTPUT:
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
else:
self.drive.setDirectionVelocity(
x_speed*Constants.TANK_VELOCITY_MAX, y_speed*Constants.TANK_VELOCITY_MAX, rotation*Constants.TANK_VELOCITY_MAX)
def execute(self):
pov = oi.OI().driver.getPOV(self.pov_port)
if pov != -1 and not isinstance(self.drive.currentCommand,
turntoangle.TurnToAngle):
pov = angles.positiveAngleToMixedAngle(pov)
pov //= 45
to_turn_to = SnapListener.POV_ARRAY[int(pov)]
to_turn_to = int(to_turn_to)
Dash.putNumber("Snap Turning Target", to_turn_to)
# logging.debug("Starting snap to angle")
turntoangle.TurnToAngle(to_turn_to).start()
def execute(self):
long_speed = oi.OI().operator.getY() * 0.5
short_speed = oi.OI().operator.getThrottle() * 0.5
self.longarm.m_motor.setPercentOutput(long_speed)
self.shortarm.m_motor.setPercentOutput(short_speed)
def execute(self):
power = math.pow(oi.OI().driver.getY(), Constants.TANK_DRIVE_EXPONENT)
rotation = oi.OI().driver.getZ()
left = power - rotation
right = power + rotation
self.drive.setPercentOutput(left, right)
def execute(self):
long_speed = oi.OI().operator.getY() * Constants.CLIMB_SPEED
short_speed = oi.OI().operator.getThrottle() * Constants.CLIMB_SPEED
self.longarm.m_motor.setPercentOutput(long_speed)
self.shortarm.m_motor.setPercentOutput(short_speed)
def execute(self):
pov = oi.OI().driver.getPOV(self.pov_port)
logging.debug("drive current command: {}".format(self.drive.currentCommand))
if pov != -1 and not self.drive.currentCommand is SnapListener:
turntoangle.TurnToAngle(pov).start()
def robotInit(self):
# NetworkTables
self.nt_robot = networktables.NetworkTables.getTable('Robot')
### Subsystems ###
# Drivetrain
self.drive = subsystems.drivetrain.Drivetrain(self)
# Driverstation
self.driverstation = wpilib.DriverStation.getInstance()
# Power Distribution Panel
self.pdp = wpilib.PowerDistributionPanel()
# Robot Components -- subsystems
self.hatch = subsystems.hatch.Hatch(self)
self.cargo = subsystems.cargo.Cargo(self)
self.climber = subsystems.climber.Climber(self)
# Limelight / camera
self.limelight = subsystems.limelight.Limelight(self)
# wpilib.CameraServer.launch()
# Arduino / line sensors
self.arduino = subsystems.arduino.Arduino(self)
# Gyro
# self.gyro = wpilib.ADXRS450_Gyro()
# Ultrasonics
# self.ultrasonic_front = wpilib.AnalogInput(const.AIN_ULTRASONIC_FRONT)
# self.ultrasonic_rear = wpilib.AnalogInput(const.AIN_ULTRASONIC_REAR)
self.match_time = 9999
# Operator Input
self.oi = oi.OI(self)
# Encoders -- change after encoders are decided and added
self.use_enc_correction = False
self.drive_encoder_left = wpilib.Encoder(
const.DIO_DRIVE_ENC_LEFT_1,
const.DIO_DRIVE_ENC_LEFT_2,
reverseDirection=const.DRIVE_ENCODER_LEFT_REVERSED)
self.drive_encoder_right = wpilib.Encoder(
const.DIO_DRIVE_ENC_RIGHT_1,
const.DIO_DRIVE_ENC_RIGHT_2,
reverseDirection=const.DRIVE_ENCODER_RIGHT_REVERSED)
self.drive_encoder_left.setDistancePerPulse(1 /
const.DRIVE_TICKS_PER_FOOT)
self.drive_encoder_right.setDistancePerPulse(
1 / const.DRIVE_TICKS_PER_FOOT)
self.climber.climber_motor_1.setSelectedSensorPosition(0, 0, 10)
self.climber.climber_motor_2.setSelectedSensorPosition(0, 0, 10)
# Built in Accelerometer
self.accel = wpilib.BuiltInAccelerometer()
self.accel.setRange(0) # Sets range from -2g to 2g
# Array for average accel.
self.accel_samples = []
self.last_accel_value = 0.0
# Pressure Sensor (200 psi)
# self.pressure_sensor = wpilib.AnalogInput(const.AIN_PRESSURE_SENSOR)
# Timer for pressure sensor's running average
# self._pressure_samples = []
# self._last_pressure_value = 0.0
# self.pressure_timer = wpilib.Timer()
# self.pressure_timer.start()
# self.pressure_timer_delay = 1.0 # times per second
# Time robot object was created
self.start_time = time.time()
## Scheduler ##
self.scheduler = wpilib.command.Scheduler.getInstance()
### LOGGING ###
# Timers for NetworkTables update so we don't use too much bandwidth
self.log_timer = wpilib.Timer()
self.log_timer.start()
self.log_timer_delay = 0.25 # 4 times/second
# Disable LW telemetry before comp to improve loop times
wpilib.LiveWindow.disableAllTelemetry()
self.limelight.set_startup_modes()
def robotInit(self):
# Gear plate limit switch
self.gear_switch = wpilib.DigitalInput(const.ID_GEAR_SWITCH)
### Subsystems ###
# Agitator
self.agitator = subsystems.agitator.Agitator(self)
# Climber
self.climber = subsystems.climber.Climber(self)
# Drive for Xbox Controller
self.drive = subsystems.drivetrain.Drivetrain(self)
# Gear
self.gear_funnel = subsystems.gear_funnel.Gear_Funnel(self)
self.gear_lexan = subsystems.gear_lexan.Gear_Lexan(self)
self.gear_claw = subsystems.gear_claw.Gear_Claw(self)
# Ground Feeder
self.feeder = subsystems.feeder.Feeder(self)
# Shooter
self.shooter = subsystems.shooter.Shooter(self)
# Shooter Camera
self.shooter_camera = subsystems.shooter_camera.Shooter_Camera(self)
# Gear Camera
self.gear_camera = subsystems.gear_camera.Gear_Camera(self)
# Encoders
self.drive_encoder_left = wpilib.Encoder(2, 3, reverseDirection=True)
self.drive_encoder_left.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_left.reset()
self.drive_encoder_right = wpilib.Encoder(0, 1, reverseDirection=False)
self.drive_encoder_right.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_right.reset()
# Pressure sensor (200 psi)
self.pressure_sensor = wpilib.AnalogInput(const.ID_PRESSURE_SENSOR)
self._pressure_samples = []
self._last_pressure_value = 0.0
# Gyro
self.gyro = wpilib.ADXRS450_Gyro()
#self.gyro = wpilib.AnalogGyro( 0 )
#self.gyro.setSensitivity( 0.08 )
### Misc ###
# Operator Input
self.oi = oi.OI(self)
# Log to file
self.log_file = open(
os.path.join(os.path.dirname(__file__), 'log.csv'), 'w')
# Time robot object was created
self.start_time = time.time()
## Autonomous ##
self.subsystems = {
'agitator': self.agitator,
'climber': self.climber,
'drive': self.drive,
'gear_funnel': self.gear_funnel,
'gear_lexan': self.gear_lexan,
'gear_claw': self.gear_claw,
'feeder': self.feeder,
'shooter_camera': self.shooter_camera,
'gear_camera': self.gear_camera,
'shooter': self.shooter,
}
## Scheduler ##
self.scheduler = wpilib.command.Scheduler.getInstance()
## MISC ##
self.gear_is_ejecting = False
self.gear_ejecting_start_time = 0
### Logging ###
# NetworkTables
self.nt_smartdash = networktables.NetworkTable.getTable(
'SmartDashboard')
self.nt_grip_peg = networktables.NetworkTable.getTable(
'vision/peg_targets')
self.nt_grip_boiler = networktables.NetworkTable.getTable(
'vision/boiler_targets')
self.nt_vision = networktables.NetworkTable.getTable('vision')
# Timers for NetworkTables update so we don't use too much bandwidth
self.log_timer = wpilib.Timer()
self.log_timer.start()
self.log_timer_delay = 0.1 # 10 times/second
# Timer for pressure sensor's running average
self.pressure_timer = wpilib.Timer()
self.pressure_timer.start()
self.pressure_timer_delay = 1.0 # once per second
self.log()
import oi as oiInterface
import tello
import vision
import time
tello = tello.Tello()
oi = oiInterface.OI()
visionController = vision.TelloVision(tello)
DEADZONE = 0.075
MANUAL = 0
TRACK_FACES = 1
def mainLoop():
print("Main: Starting Program loop")
enabled = True
mode = MANUAL
while True:
oi.update()
if enabled:
if mode == MANUAL:
manualControl()
elif mode == TRACK_FACES:
pass
def manualControl():
'''
Maps controller Axis to Tello Flight:
Left X = left/right
Left Y = Forward/backward
Right X = YAW