def __init__(self, shared_stuff, cmd_queue, sensor_thread):
self.shared = shared_stuff
self.cmd_queue = cmd_queue
self.arm = arm.robotArm(self.shared)
self.drive = driveUnit.driveUnit()
self.current_speed = [None, None]
self.linedet = se.LineDetector()
self.state = None
self.stored_state = None
self.has_package = self.shared["hasPackage"] = False
self.station_queue = []
self.flush_timer = time.time()
self.arm_return_pos = None
self.carry_pos = (0, 100, 130, 0, 150, 140)
"""a simple testprogram for the pcThread class that is used to connects to a user and receives commands and sends data back"""
from pcThread import pcThread
import Queue
import time
import copy
import driveUnit
import arm
robot_arm=arm.robotArm()
driver=driveUnit.driveUnit()
commandQueue=Queue.Queue()
sensorList= {"lineSensor" : [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
"distance" : [0, 0],
"armPosition" : [0, 0, 255, 4, 5, 5],
"errorCodes" : ["YngveProgrammedMeWrong"],
"motorSpeed" : [70, 70],
"latestCalibration" : "0000-00-00-15:00",
"autoMotor" : True,
"autoArm" : False,
"regulator" : [0, 0],
"error" : 0}
pcThreadObject=pcThread(commandQueue,sensorList)
pcThreadObject.daemon=True
pcThreadObject.start()
while True:
if not commandQueue.empty():
command=(commandQueue.get())
if command[0]=="motorSpeed":
left=int(command[1][0])
right=int(command[1][1])
driver.setMotorLeft(left)
#Setup switched
GPIO.setup(pinEndSwitch, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(pinSwitch, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
#Setup servos
futabaR = motors.Servo(19, 178, 160, 0)
spektrum = motors.Servo(40, 159, 115, 1)
hitec = motors.Servo(0, 180, 180, 2)
reely = motors.Servo(0, 180, 180, 3)
futabaS = motors.Servo(18, 178, 160, 4)
#Setup stepper
stepper = motors.Stepper(32, 36, 38, 40)
#Setup robot arm
robotArm = arm.robotArm([futabaR, spektrum, hitec, reely])
#Setup switch arm
switchArm = arm.switchArm(futabaS, stepper)
lStepTime = time.time()
while (GPIO.input(pinEndSwitch) != GPIO.HIGH):
if (time.time() > lStepTime + 0.002):
stepper.step(False)
lStepTime = time.time()
try:
while (True):
#Joystick input handling
if GPIO.input(pinRight) == GPIO.HIGH:
if switchArm.getAngle() < 180:
