From 534cc23c8de906b4a38a82b8a3c227860736ab44 Mon Sep 17 00:00:00 2001
From: Cole Deck <cdeck@hawk.iit.edu>
Date: Wed, 31 Jul 2024 18:25:10 -0500
Subject: [PATCH] Update MQTT commands, arm height

---
 pick_count.pkl |  1 -
 pick_count.txt |  1 +
 run.py         | 37 +++++++++++++++++++++++++++----------
 ur5_control.py |  2 +-
 4 files changed, 29 insertions(+), 12 deletions(-)
 delete mode 100644 pick_count.pkl
 create mode 100644 pick_count.txt

diff --git a/pick_count.pkl b/pick_count.pkl
deleted file mode 100644
index 24de7c8..0000000
--- a/pick_count.pkl
+++ /dev/null
@@ -1 +0,0 @@
-€K7.
\ No newline at end of file
diff --git a/pick_count.txt b/pick_count.txt
new file mode 100644
index 0000000..7c6ba0f
--- /dev/null
+++ b/pick_count.txt
@@ -0,0 +1 @@
+55
\ No newline at end of file
diff --git a/run.py b/run.py
index 3f1d581..2311388 100755
--- a/run.py
+++ b/run.py
@@ -32,6 +32,7 @@ from uptime import uptime
 import fileserver
 import paho.mqtt.client as mqtt
 import pickle
+import time
 
 # set to false to run without real hardware for development
 real = False
@@ -79,7 +80,8 @@ timecount = 0
 secondsclock = 0
 unacked_publish = set()
 mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
-counter_file = 'pick_count.pkl'
+counter_file = 'pick_count.txt'
+cycle_start_time = time.time()
 
 
 mqttc.user_data_set(unacked_publish)
@@ -134,22 +136,32 @@ def initialize_counter():
     global counter
     try:
         with open(counter_file, 'rb') as file:
-            counter = pickle.load(file)
+            counter = int(file.readline().decode())
+            fprint("Read count at " + str(counter))
             if not isinstance(counter, int):
                 raise ValueError("Counter is not an integer")
-    except (FileNotFoundError, ValueError, pickle.PickleError):
+                
+    except (FileNotFoundError, ValueError) as e:
         counter = 101
+        fprint("Error. Resetting count." + str(e))
+        fprint(counter)
         save_counter()
 
+    fprint(counter)
+
 # Save the counter to the file
 def save_counter():
+    global counter
+    fprint(counter)
     with open(counter_file, 'wb') as file:
-        pickle.dump(counter, file)
+        file.write(str(counter).encode())
 
 # Increment the counter
 def increment_counter():
     global counter
-    counter += 1
+    fprint(counter)
+    fprint("Setting count to " + str(counter + 1))
+    counter = counter + 1
     save_counter()
 
 def check_server():
@@ -499,7 +511,7 @@ def setup_server(pool, manager):
         mqttc.connect(config["mqtt"]["server"])
         mqttc.loop_start()
     
-    initialize_counter()
+    
 
     return True
 
@@ -623,6 +635,8 @@ def mainloop_server(pool, manager):
     global failcount
     global timecount
     global secondsclock
+    global cycle_start_time
+
 
     if mode != oldmode:
         print(" ***** Running mode:", mode, "***** ")
@@ -630,6 +644,7 @@ def mainloop_server(pool, manager):
         mqtt_send(mode, "mode")
         if mode == "Startup": # very first loop
             pass
+            initialize_counter()
 
     if killme.value > 0:
         killall()
@@ -752,7 +767,7 @@ def mainloop_server(pool, manager):
                 global scan_value
                 if scan_value is False:
                     cable_list.append(scan_value)
-                elif scan_value.find("bldn.app/") > -1:
+                elif scan_value.upper.find("BLDN.APP/") > -1:
                     scan_value = scan_value[scan_value.find("bldn.app/")+9:]
                 else:
                     cable_list.append(scan_value)
@@ -889,9 +904,10 @@ def mainloop_server(pool, manager):
                     killme.set(1)
                 else:
                     fprint("Movement requested. Keep clear of the machine!")
-                    mqtt_send("start", "cycle_start")
+                    #mqtt_send("{\"value\": " + str(time.time() * 1000) + " }", "cycle_start")
+                    cycle_start_time = int(time.time() * 1000)
                     increment_counter()
-                    mqtt_send(counter, "pick_count_total")
+                    mqtt_send("{\"value\": " + str(counter) + " }", "pick_count_total")
                     if get_cable > -1:
                         global sensors
                         if action == "pickup":
@@ -928,7 +944,8 @@ def mainloop_server(pool, manager):
         # complete
         if arm_ready == True:
             mode = "Idle"
-            mqtt_send("start", "cycle_end")
+            sleep(9)
+            mqtt_send("{\"value\": " + str(int(time.time() * 1000) - cycle_start_time) + " }", "cycle_time")
         else:
             # getting cable and bringing to tray
             # led animation
diff --git a/ur5_control.py b/ur5_control.py
index 9380ef2..21c3c97 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -261,7 +261,7 @@ def move_to_home(robot, keep_flip=False, speed=2):
         flip(robot)
 
     # Move robot to home position
-    rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.35), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
+    rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.25), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
 
     return True