def robotInit(self):
self.prior_autospeed = 0
self.joystick = wpilib.Joystick(0)
self.left_motor_master = CANSparkMax(1, MotorType.kBrushless)
self.right_motor_master = CANSparkMax(4, MotorType.kBrushless)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.left_motor_master,
CANSparkMax(3, MotorType.kBrushless)
)
self.right_motors = wpilib.SpeedControllerGroup(
self.right_motor_master,
CANSparkMax(6, MotorType.kBrushless)
)
# Configure Gyro
# Note that the angle from the NavX and all implementors of wpilib Gyro
# must be negated because getAngle returns a clockwise positive angle
self.gyro = navx.AHRS.create_spi()
# Configure drivetrain movement
self.drive = DifferentialDrive(self.left_motors, self.right_motors)
self.drive.setDeadband(0)
# Configure encoder related functions -- getDistance and getrate should return
# units and units/s
self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.left_motor_master.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.right_encoder = self.right_motor_master.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set the update rate instead of using flush because of a ntcore bug
# -> probably don't want to do this on a robot in competition
NetworkTables.getDefault().setUpdateRate(0.010)
def __init__(self, robot):
super().__init__()
self.robot = robot
self.requires(self.robot.drivetrain)
NetworkTables.initialize(server='10.59.3.2')
self.table = NetworkTables.getDefault().getTable("limelight")
self.Distance = 0.1
self.Aim = 0.1
self.min_aim_command = 0.05
self.steering_adjust = 0.0
vision = Processing.VisionThread(debug=False)
vision.setCameraCharacteristics(Constants.camera2Matrix,Constants.camera2DistortionCoefficients)
if(ret):
vision.run(camframe1)
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize(server='roborio-3373-frc.local')
sd = NetworkTables.getTable("VisionData")
#cameraPre = NetworkTables.getTable("PreLoad")
cvSource = cs.CvSource("Vision Camera", cs.VideoMode.PixelFormat.kMJPEG, 480, 320, 10)
cvMjpegServer = cs.MjpegServer("httpCamera1", port=5801)
cvMjpegServer.setSource(cvSource)
csTable = NetworkTables.getDefault()
csTable.getEntry("/CameraPublisher/PiCam/streams").setStringArray(["mjpeg:http://10.33.73.20:5801/?action=stream"])
#cvSource2 = cs.CvSource("Camera2", cs.VideoMode.PixelFormat.kMJPEG, 320, 240, 15)
#cvMjpegServer2 = cs.MjpegServer("httpCamera2", port=5802)
#cvMjpegServer2.setSource(cvSource2)
cameras=[{"camera":visionCam,"stream":cvSource,"id":VisonCamera,"timeout":0}]
count = 0
#NetworkTables.initialize()
def switchCamera(camobject,deviceNum):
#subprocess.call(shlex.split("v4l2-ctl -d "+str(deviceNum)+" -c exposure_auto=3"))
#subprocess.call(shlex.split("v4l2-ctl -d "+str(deviceNum)+" -c exposure_absolute=150"))
#subprocess.call(shlex.split("v4l2-ctl -d "+str(deviceNum)+" -c saturation=255"))
#subprocess.call(shlex.split("v4l2-ctl -d "+str(deviceNum)+" -c white_balance_temperature_auto=0"))
#imports
import cv2
import time
import numpy as np
import math
from enum import Enum
import random as rng
from networktables import NetworkTables
import datetime
from datetime import timedelta
rng.seed(12345)
#NetworkTables.initialize(server='10.22.83.2')
NetworkTables.startClientTeam(2283)
table = NetworkTables.getDefault().getTable('SmartDashboard')
#global variables
IMAGE_WIDTH = 320
IMAGE_HEIGHT = 240
center_coordinates = (int(IMAGE_WIDTH / 2), int(IMAGE_HEIGHT / 2))
blur_ksize = 7
'''hue = [65,80]
sat = [85,255]
val = [40,255]'''
hue = [75, 100]
sat = [75, 255]
val = [20, 255]
if (connectToRobot):
print("Looking for robot...")
cond = threading.Condition()
notified = [False]
def connectionListener(connected, info):
print(info, '; Connected=%s' % connected)
with cond:
notified[0] = True
cond.notify()
NetworkTables.initialize(server=roborioAddress)
NetworkTables.addConnectionListener(connectionListener,
immediateNotify=True)
table = NetworkTables.getTable(networkTable)
inst = NetworkTables.getDefault()
# ----------Start Camera----------
print("Setting up camera")
os.system('./setupCamera.sh')
cap = cv2.VideoCapture(0)
cap.set(3, imgWidth)
cap.set(4, imgHeight)
cap.set(5, framerate)
cap.set(15, exposure)
print("Camera resolution: ", cap.get(3), "x", cap.get(4))
print("Framerate: ", cap.get(5))
print("Exposure: ", cap.get(15))
print("Running...")
# ----------Main Loop----------
