diff --git a/led_control.py b/led_control.py index 497eecf..c5942da 100755 --- a/led_control.py +++ b/led_control.py @@ -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() diff --git a/run.py b/run.py index 73259aa..569a8b2 100755 --- a/run.py +++ b/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: diff --git a/ur5_control.py b/ur5_control.py index 0269453..65b6cab 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -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],