Update sensor code
This commit is contained in:
Cole Deck
parent
ea5daf8689
commit
48e7c7b85f
@ -37,6 +37,7 @@ class LEDSystem():
|
||||
showtoggle = False
|
||||
clearshow = -1
|
||||
showring = -1
|
||||
solidanimcount = 0
|
||||
|
||||
def __init__(self):
|
||||
self.start = uptime()
|
||||
@ -267,7 +268,41 @@ class LEDSystem():
|
||||
offset = len(self.controllers)
|
||||
for x in range(len(self.solid_controllers)):
|
||||
ctrl = self.solid_controllers[x]
|
||||
self.sender[ctrl[0]+1].dmx_data = list(ctrl[1]) * 170
|
||||
data = list((0, 0, 0)) * 170
|
||||
#print(len(data))
|
||||
#print(len(list(ctrl[1]) * 170))
|
||||
offset = 1 - (self.solidanimcount - int(self.solidanimcount))
|
||||
#print("a", offset)
|
||||
data[int(self.solidanimcount) * 3] = int(ctrl[1][0]*offset)
|
||||
data[int(self.solidanimcount) * 3 + 1] = int(ctrl[1][1]*offset)
|
||||
data[int(self.solidanimcount) * 3 + 2] = int(ctrl[1][2]*offset)
|
||||
offset = 1 - offset
|
||||
#print("b", offset)
|
||||
if int(self.solidanimcount) <= 168:
|
||||
data[int(self.solidanimcount+1) * 3] = int(ctrl[1][0]*offset)
|
||||
data[int(self.solidanimcount+1) * 3 + 1] = int(ctrl[1][1]*offset)
|
||||
data[int(self.solidanimcount+1) * 3 + 2] = int(ctrl[1][2]*offset)
|
||||
|
||||
# if offset > 0.5: # onto next light more
|
||||
# # center +1
|
||||
# num = int(self.solidanimcount+1)
|
||||
# else:
|
||||
# num = int(self.solidanimcount)
|
||||
# offset = 1 - offset
|
||||
# if int(num) <= 168:
|
||||
# data[int(num+1) * 3] = int(ctrl[1][0]*offset/2)
|
||||
# data[int(num+1) * 3 + 1] = int(ctrl[1][1]*offset/2)
|
||||
# data[int(num+1) * 3 + 2] = int(ctrl[1][2]*offset/2)
|
||||
|
||||
# if int(num) >= 1:
|
||||
# data[int(num-1) * 3] = int(ctrl[1][0]*offset/2)
|
||||
# data[int(num-1) * 3 + 1] = int(ctrl[1][1]*offset/2)
|
||||
# data[int(num-1) * 3 + 2] = int(ctrl[1][2]*offset/2)
|
||||
|
||||
self.sender[ctrl[0]+1].dmx_data = data #list(ctrl[1]) * 170
|
||||
self.solidanimcount += 0.15
|
||||
if int(self.solidanimcount) >= 170:
|
||||
self.solidanimcount = 0
|
||||
|
||||
|
||||
self.sender.flush()
|
||||
|
||||
103
run.py
103
run.py
@ -89,6 +89,8 @@ arm_distance_old = 0
|
||||
arm_distance_total = 0
|
||||
kill_ssh = False
|
||||
mqttc.user_data_set(unacked_publish)
|
||||
spot = -1
|
||||
placed = 0
|
||||
|
||||
def arm_start_callback(res):
|
||||
fprint("Arm action complete.")
|
||||
@ -558,39 +560,61 @@ def get_sensors():
|
||||
for idx in range(len(sens)):
|
||||
reg = sens[idx]
|
||||
val = mbconn.read_holding_registers(reg)
|
||||
#fprint("Sensor " + str(idx) + " = " + str(val))
|
||||
if val is not None:
|
||||
val = val[0]
|
||||
if val == 1 and sensors[idx] >= 0: # skip negative values
|
||||
if val == 1: # skip negative values
|
||||
sensors[idx] += 1
|
||||
elif val == 0:
|
||||
if sensors[idx] >= 4:
|
||||
sensors[idx] -= 4
|
||||
else:
|
||||
sensors[idx] += 4
|
||||
sensors[idx] -= 1
|
||||
|
||||
|
||||
|
||||
|
||||
else:
|
||||
sensors = [0, 0, 0, 0]
|
||||
sensors = [-10, -10, -10, -10]
|
||||
|
||||
#fprint("Values: " + str(sensors))
|
||||
#mbconn.close()
|
||||
for x in range(len(sensors)):
|
||||
if sensors[x] >= 180: # 3 sec
|
||||
# cable newly detected on tray
|
||||
sensors[x] = -180
|
||||
fprint("Precense detected: slot " + str(x))
|
||||
return x
|
||||
if sensors[x] > 10:
|
||||
sensors[x] = 10
|
||||
|
||||
if sensors[x] < -10:
|
||||
sensors[x] = -10
|
||||
|
||||
return -1
|
||||
|
||||
def get_open_spot(sensordata):
|
||||
for x in range(len(sensordata)):
|
||||
sens = sensordata[x]
|
||||
if not sens:
|
||||
for x in range(len(sensors)):
|
||||
sens = sensors[x]
|
||||
if sens <= -5:
|
||||
print("Open spot: " + str(x))
|
||||
return x
|
||||
|
||||
# if we get here, every spot is full
|
||||
|
||||
fprint("No spots empty")
|
||||
return False
|
||||
|
||||
def get_full_spot(sensordata):
|
||||
for x in range(len(sensors)):
|
||||
sens = sensors[x]
|
||||
if sens >= 3:
|
||||
print("Full spot: " + str(x))
|
||||
return x
|
||||
|
||||
# if we get here, every spot is empty
|
||||
fprint("No spots full")
|
||||
return False
|
||||
|
||||
def get_spot(sensordata, idx):
|
||||
if sensordata[idx] >= 3:
|
||||
return True
|
||||
elif sensordata[idx] <= -5:
|
||||
return False
|
||||
else:
|
||||
return False
|
||||
|
||||
def on_publish(client, userdata, mid, reason_code, properties):
|
||||
# reason_code and properties will only be present in MQTTv5. It's always unset in MQTTv3
|
||||
try:
|
||||
@ -644,6 +668,7 @@ def mainloop_server(pool, manager):
|
||||
global arm_distance
|
||||
global arm_distance_old
|
||||
global arm_distance_total
|
||||
global placed
|
||||
|
||||
if mode != oldmode:
|
||||
print(" ***** Running mode:", mode, "***** ")
|
||||
@ -715,19 +740,19 @@ def mainloop_server(pool, manager):
|
||||
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
else:
|
||||
pass
|
||||
ledsys.mainloop(None, -1, arm_position=arm_position)
|
||||
|
||||
# every 1 second
|
||||
if secondsclock >= config["core"]["loopspeed"]:
|
||||
if secondsclock >= config["core"]["loopspeed"] / 2:
|
||||
secondsclock = 1
|
||||
arm_distance_total += arm_distance
|
||||
arm_distance_total += abs(arm_distance)
|
||||
if abs(arm_distance) < 0.001:
|
||||
arm_distance = 0.0
|
||||
|
||||
mqtt_send("{\"value\": " + str(abs(arm_distance)) + " }", "arm_speed")
|
||||
mqtt_send("{\"value\": " + str(abs(arm_distance_total)) + " }", "arm_distance")
|
||||
arm_distance_old = 0 # reset counter
|
||||
|
||||
get_sensors()
|
||||
|
||||
|
||||
|
||||
@ -788,7 +813,7 @@ def mainloop_server(pool, manager):
|
||||
if scan_value is False:
|
||||
cable_list.append(scan_value)
|
||||
elif scan_value.upper().find("BLDN.APP/") > -1:
|
||||
scan_value = scan_value[scan_value.find("bldn.app/")+9:]
|
||||
scan_value = scan_value[scan_value.upper().find("BLDN.APP/")+9:]
|
||||
else:
|
||||
cable_list.append(scan_value)
|
||||
fprint(scan_value)
|
||||
@ -903,6 +928,7 @@ def mainloop_server(pool, manager):
|
||||
|
||||
|
||||
if not mainloop_get.empty():
|
||||
global sensors
|
||||
action, get_cable = mainloop_get.get()
|
||||
if action == "show":
|
||||
animation_wait = False
|
||||
@ -916,26 +942,30 @@ def mainloop_server(pool, manager):
|
||||
sleep(30)
|
||||
killme.set(1)
|
||||
elif action == "returnCheck":
|
||||
print("Checking sensors..")
|
||||
if real:
|
||||
newtube = get_sensors()
|
||||
else:
|
||||
newtube = -1
|
||||
if newtube >= 0:
|
||||
# need to return a cable
|
||||
mainloop_get.put(("return", newtube))
|
||||
mainloop_get.put(("returnCheck", 0))
|
||||
print("Checking sensors..")
|
||||
if real:
|
||||
newtube = get_full_spot(sensors)
|
||||
else:
|
||||
newtube = -1
|
||||
if newtube >= 0:
|
||||
# need to return a cable
|
||||
mainloop_get.put(("return", newtube))
|
||||
mainloop_get.put(("returnCheck", 0))
|
||||
else:
|
||||
fprint("Movement requested. Keep clear of the machine!")
|
||||
|
||||
placed = 0
|
||||
arm_distance_total = 0
|
||||
#mqtt_send("{\"value\": " + str(time.time() * 1000) + " }", "cycle_start")
|
||||
cycle_start_time = int(time.time() * 1000)
|
||||
increment_counter()
|
||||
mqtt_send("{\"value\": " + str(counter) + " }", "pick_count_total")
|
||||
mqtt_send("{\"value\": " + str(1) + " }", "pick_count_total")
|
||||
if get_cable > -1:
|
||||
global sensors
|
||||
|
||||
global spot
|
||||
if action == "pickup":
|
||||
spot = get_open_spot(sensors)
|
||||
|
||||
if spot is not False:
|
||||
arm_ready = False
|
||||
if real:
|
||||
@ -968,11 +998,18 @@ def mainloop_server(pool, manager):
|
||||
# complete
|
||||
if arm_ready == True:
|
||||
mode = "Idle"
|
||||
sleep(9)
|
||||
if not real:
|
||||
sleep(9)
|
||||
#global sensors
|
||||
if placed > 5:
|
||||
# success
|
||||
mqtt_send("{\"value\": " + str(1) + " }", "pick_count_success")
|
||||
mqtt_send("{\"value\": " + str(int(time.time() * 1000) - cycle_start_time) + " }", "cycle_time")
|
||||
else:
|
||||
# getting cable and bringing to tray
|
||||
# led animation
|
||||
if get_spot(sensors, spot):
|
||||
placed += 1
|
||||
if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
|
||||
animation_wait = True
|
||||
start_animation = True
|
||||
@ -1025,6 +1062,8 @@ def mainloop_server(pool, manager):
|
||||
fprint("Got cable: " + str(scan_value))
|
||||
if scan_value[0:2] == "BL" or scan_value[0:2] == "AW":
|
||||
scan_value = scan_value[2:]
|
||||
print(cable_list)
|
||||
print(scan_value)
|
||||
for idx in range(len(cable_list)):
|
||||
cable = cable_list[idx]
|
||||
if cable == scan_value and cable_list_state[idx] == False:
|
||||
|
||||
@ -582,7 +582,7 @@ def tray_routine(robot, slot=0, pick_up=True):
|
||||
# Positions for each slot
|
||||
slot_distance = .052
|
||||
slot_height = -.015-.0095+0.007 # add 7mm for shim
|
||||
first_slot = -0.3084+0.01
|
||||
first_slot = -0.3084+0.01+0.003 # add 3mm for tray adjust
|
||||
slot_position = [
|
||||
[first_slot, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
|
||||
[first_slot+slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user