From f2d51376838f094a8f5286a26f93fb48e8bf728f Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 5 Nov 2019 19:41:52 -0800
Subject: [PATCH 01/20] Created script to test reading UDP commands from
 general source

---
 readUDPCommands.py | 91 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 91 insertions(+)
 create mode 100644 readUDPCommands.py

diff --git a/readUDPCommands.py b/readUDPCommands.py
new file mode 100644
index 0000000..9b1927c
--- /dev/null
+++ b/readUDPCommands.py
@@ -0,0 +1,91 @@
+import pigpio
+from src.Controller import step_controller, Controller
+from src.HardwareInterface import send_servo_commands, initialize_pwm
+from src.PupperConfig import (
+    MovementReference,
+    GaitParams,
+    StanceParams,
+    SwingParams,
+    ServoParams,
+    PWMParams,
+)
+import time
+import numpy as np
+import UDPComms
+
+
+def set_velocity(controller, velocity_x, velocity_y):
+    controller.movement_reference.v_xy_ref = np.array([velocity_x, velocity_y])
+    controller.movement_reference.wz_ref = 0
+
+
+def turn_radians(pi_board, pwm_params, servo_params, controller, speed, radians):
+    time_to_run = radians / speed
+    turn_for_time(pi_board, pwm_params, servo_params, controller, speed, time_to_run)
+
+
+def turn_degrees(pi_board, pwm_params, servo_params, controller, speed, degrees):
+    turn_radians(pi_board, pwm_params, servo_params, controller, speed, degrees * np.pi / 180)
+
+
+def turn_for_time(pi_board, pwm_params, servo_params, controller, speed, time):
+    start_time = time.time()
+    while time.time() - start_time < time:
+        controller.movement_reference.v_xy_ref = np.array([0.0, 0.00])
+        controller.movement_reference.wz_ref = speed
+        step_controller(controller)
+        send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
+
+
+def main():
+    """Main program
+    """
+    pi_board = pigpio.pi()
+    pwm_params = PWMParams()
+    servo_params = ServoParams()
+
+    controller = Controller()
+    controller.movement_reference = MovementReference()
+    controller.movement_reference.v_xy_ref = np.array([0.0, 0.00])
+    controller.movement_reference.wz_ref = 0 #given in radians per second
+    controller.swing_params = SwingParams()
+    controller.swing_params.z_clearance = 0.06
+    controller.stance_params = StanceParams()
+    controller.stance_params.delta_y = 0.08
+    controller.gait_params = GaitParams()
+    controller.gait_params.dt = 0.01
+
+    initialize_pwm(pi_board, pwm_params)
+
+    last_loop = time.time()
+    now = last_loop
+    start = time.time()
+    # values = UDPComms.Subscriber(8870)
+    msg = {"command": "move_forward", "time": "10"}  # simulated move command
+    #msg = {"command": "turn", "speed": "0.1", "radians": 1.0}  # simulated turn command
+    for i in range(6000):
+        last_loop = time.time()
+        #get some info from UDP
+        #msg = values.get()
+        command = msg["command"]
+        if command is "set_velocity":
+            set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
+        elif command is "turn_radian":
+            turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["radians"])
+        elif command is "turn_degrees":
+            turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["degrees"])
+
+        step_controller(controller)
+        send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
+        if i is 100:
+            msg = {"command": "turn_radian", "speed": "0.1", "radians": 1.0}
+        if i is 101:
+            msg = {}
+        while now - last_loop < controller.gait_params.dt:
+            now = time.time()
+        print("Time since last loop: ", now - last_loop)
+    end = time.time()
+    print("seconds per loop: ", (end - start) / 1000.0)
+
+
+main()

From 5f52bc045cf779d9f90db98a490a1840cbb2138d Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 5 Nov 2019 19:42:46 -0800
Subject: [PATCH 02/20] Updated to use UDP readings

---
 readUDPCommands.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 9b1927c..5b28bab 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -60,13 +60,13 @@ def main():
     last_loop = time.time()
     now = last_loop
     start = time.time()
-    # values = UDPComms.Subscriber(8870)
-    msg = {"command": "move_forward", "time": "10"}  # simulated move command
+    values = UDPComms.Subscriber(8870)
+    #msg = {"command": "move_forward", "time": "10"}  # simulated move command
     #msg = {"command": "turn", "speed": "0.1", "radians": 1.0}  # simulated turn command
     for i in range(6000):
         last_loop = time.time()
         #get some info from UDP
-        #msg = values.get()
+        msg = values.get()
         command = msg["command"]
         if command is "set_velocity":
             set_velocity(controller, msg["velocity_x"], msg["velocity_y"])

From e4df36a10321b2f6f4a2da6f3a65dfe5b12e6159 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Sun, 10 Nov 2019 19:03:25 -0800
Subject: [PATCH 03/20] Added program to send commands to robot from user input

---
 sendUDPCommands.py | 42 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)
 create mode 100644 sendUDPCommands.py

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
new file mode 100644
index 0000000..9fc027d
--- /dev/null
+++ b/sendUDPCommands.py
@@ -0,0 +1,42 @@
+import os
+from UDPComms import Publisher
+import signal
+
+drive_pub = Publisher(8870)
+
+
+# prevents quiting on pi when run through systemd
+def handler(signum, frame):
+    print("GOT singal", signum)
+
+
+signal.signal(signal.SIGHUP, handler)
+
+# those two lines allow for running headless (hopefully)
+os.environ["SDL_VIDEODRIVER"] = "dummy"
+os.putenv('DISPLAY', ':0.0')
+
+# Prints the values for axis0
+while True:
+    print("running")
+    command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
+    msg = {"command": command}
+    if command == "set_velocity":
+        velocity_x = input("Please enter an x velocity: ")
+        velocity_y = input("Please enter an y velocity: ")
+        msg["velocity_x"] = velocity_x
+        msg["velocity_y"] = velocity_y
+    elif command == "turn_radian":
+        speed = input("Please enter an turn speed: ")
+        radians = input("Please enter the number of radians you wish to turn: ")
+        msg["speed"] = speed
+        msg["radians"] = radians
+    elif command == "turn_degrees":
+        speed = input("Please enter an turn speed: ")
+        degrees = input("Please enter the number of degrees you wish to turn: ")
+        msg["speed"] = speed
+        msg["radians"] = degrees
+    elif command == "break":
+        break
+    print(msg)
+    drive_pub.send(msg)

From 7a971add80b5e4b2b4f0eaac4d4fd7e026b3b58d Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Sun, 10 Nov 2019 19:03:41 -0800
Subject: [PATCH 04/20] Fixed bugs

---
 readUDPCommands.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 5b28bab..1213baf 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -68,11 +68,11 @@ def main():
         #get some info from UDP
         msg = values.get()
         command = msg["command"]
-        if command is "set_velocity":
+        if command == "set_velocity":
             set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
-        elif command is "turn_radian":
+        elif command == "turn_radian":
             turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["radians"])
-        elif command is "turn_degrees":
+        elif command == "turn_degrees":
             turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["degrees"])
 
         step_controller(controller)

From 1cb34292cfcefe62aa07fad524b56f81156ae5a0 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Sun, 10 Nov 2019 19:15:58 -0800
Subject: [PATCH 05/20] Hardcoded test values

---
 sendUDPCommands.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 9fc027d..824641d 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -19,6 +19,7 @@ def handler(signum, frame):
 # Prints the values for axis0
 while True:
     print("running")
+    '''
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
     if command == "set_velocity":
@@ -38,5 +39,9 @@ def handler(signum, frame):
         msg["radians"] = degrees
     elif command == "break":
         break
+        '''
+    msg = {"command": command}
+    msg["velocity_x"] = 0.2
+    msg["velocity_y"] = 0.2
     print(msg)
     drive_pub.send(msg)

From bec5dff8cd98a958b51dfad4a4fcc8f21cb922fe Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Sun, 10 Nov 2019 19:50:59 -0800
Subject: [PATCH 06/20] Added sleep and user interactivity

---
 sendUDPCommands.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 824641d..233f6cb 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -1,4 +1,5 @@
 import os
+import time
 from UDPComms import Publisher
 import signal
 
@@ -19,7 +20,6 @@ def handler(signum, frame):
 # Prints the values for axis0
 while True:
     print("running")
-    '''
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
     if command == "set_velocity":
@@ -39,9 +39,10 @@ def handler(signum, frame):
         msg["radians"] = degrees
     elif command == "break":
         break
-        '''
+
     msg = {"command": command}
     msg["velocity_x"] = 0.2
     msg["velocity_y"] = 0.2
     print(msg)
     drive_pub.send(msg)
+    time.sleep(2)

From 4a6759b390f51a5a536c6979183f65319ceb1a3a Mon Sep 17 00:00:00 2001
From: Nathan Kau <nathankau@gmail.com>
Date: Mon, 11 Nov 2019 04:11:36 +0000
Subject: [PATCH 07/20] Changed speed

---
 sendUDPCommands.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 824641d..ff7941a 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -18,7 +18,7 @@ def handler(signum, frame):
 
 # Prints the values for axis0
 while True:
-    print("running")
+    #print("running")
     '''
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
@@ -40,8 +40,8 @@ def handler(signum, frame):
     elif command == "break":
         break
         '''
-    msg = {"command": command}
+    msg = {"command": "set_velocity"}
     msg["velocity_x"] = 0.2
-    msg["velocity_y"] = 0.2
-    print(msg)
+    msg["velocity_y"] = 0.0
+    #print(msg)
     drive_pub.send(msg)

From f2df11ad952aff91399144dd3c46f733996550e8 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Sun, 10 Nov 2019 20:33:06 -0800
Subject: [PATCH 08/20] Added try except when receiving messages

---
 readUDPCommands.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 1213baf..3c19d9e 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -66,7 +66,12 @@ def main():
     for i in range(6000):
         last_loop = time.time()
         #get some info from UDP
-        msg = values.get()
+        try:
+            msg = values.get()
+        except UDPComms.timeout:
+            #no commands received yet
+            pass
+
         command = msg["command"]
         if command == "set_velocity":
             set_velocity(controller, msg["velocity_x"], msg["velocity_y"])

From 6b02e7694170da0f4598cc6076b4ccb7d80474fa Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 12 Nov 2019 19:03:36 -0800
Subject: [PATCH 09/20] Fixed error catching when there is no signal

---
 readUDPCommands.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 3c19d9e..eabfa8e 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -68,11 +68,12 @@ def main():
         #get some info from UDP
         try:
             msg = values.get()
+            command = msg["command"]
         except UDPComms.timeout:
             #no commands received yet
-            pass
+            command = "none"
+
 
-        command = msg["command"]
         if command == "set_velocity":
             set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
         elif command == "turn_radian":

From 484c2912d3406043394132e3ec7069111df1d75e Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 12 Nov 2019 19:04:55 -0800
Subject: [PATCH 10/20] Removed unneeded lines

---
 readUDPCommands.py | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index eabfa8e..01967ee 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -61,19 +61,15 @@ def main():
     now = last_loop
     start = time.time()
     values = UDPComms.Subscriber(8870)
-    #msg = {"command": "move_forward", "time": "10"}  # simulated move command
-    #msg = {"command": "turn", "speed": "0.1", "radians": 1.0}  # simulated turn command
     for i in range(6000):
         last_loop = time.time()
-        #get some info from UDP
         try:
             msg = values.get()
             command = msg["command"]
         except UDPComms.timeout:
-            #no commands received yet
+            # no commands received yet
             command = "none"
 
-
         if command == "set_velocity":
             set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
         elif command == "turn_radian":
@@ -83,10 +79,6 @@ def main():
 
         step_controller(controller)
         send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
-        if i is 100:
-            msg = {"command": "turn_radian", "speed": "0.1", "radians": 1.0}
-        if i is 101:
-            msg = {}
         while now - last_loop < controller.gait_params.dt:
             now = time.time()
         print("Time since last loop: ", now - last_loop)

From d3e9dbe53ba112aa3fc23f6351ab28d9b4eda6a0 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 12 Nov 2019 19:33:51 -0800
Subject: [PATCH 11/20] Allowed user input for UDP send

---
 sendUDPCommands.py | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 233f6cb..629b7bf 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -16,10 +16,9 @@ def handler(signum, frame):
 # those two lines allow for running headless (hopefully)
 os.environ["SDL_VIDEODRIVER"] = "dummy"
 os.putenv('DISPLAY', ':0.0')
-
+time.sleep(5)
 # Prints the values for axis0
 while True:
-    print("running")
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
     if command == "set_velocity":
@@ -40,9 +39,8 @@ def handler(signum, frame):
     elif command == "break":
         break
 
-    msg = {"command": command}
-    msg["velocity_x"] = 0.2
-    msg["velocity_y"] = 0.2
-    print(msg)
+    # msg = {"command": command}
+    # msg["velocity_x"] = 0.2
+    # msg["velocity_y"] = 0.2
     drive_pub.send(msg)
     time.sleep(2)

From 7eb4bf8b17a17e4af427864eae3bac2978a9166a Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 12 Nov 2019 19:49:11 -0800
Subject: [PATCH 12/20] Made sending commands easier

---
 sendUDPCommands.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 629b7bf..20779a1 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -21,17 +21,17 @@ def handler(signum, frame):
 while True:
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
-    if command == "set_velocity":
+    if command == "set_velocity" or command[:3] == "set":
         velocity_x = input("Please enter an x velocity: ")
         velocity_y = input("Please enter an y velocity: ")
         msg["velocity_x"] = velocity_x
         msg["velocity_y"] = velocity_y
-    elif command == "turn_radian":
+    elif command == "turn_radian" or command[:4] == "turn" and "radian" in command:
         speed = input("Please enter an turn speed: ")
         radians = input("Please enter the number of radians you wish to turn: ")
         msg["speed"] = speed
         msg["radians"] = radians
-    elif command == "turn_degrees":
+    elif command == "turn_degrees" or command[:4] == "turn" and "degrees" in command:
         speed = input("Please enter an turn speed: ")
         degrees = input("Please enter the number of degrees you wish to turn: ")
         msg["speed"] = speed

From abf5971d2eb0ff052c378bff8a01a76a49b5331e Mon Sep 17 00:00:00 2001
From: Nathan Kau <nathankau@gmail.com>
Date: Wed, 13 Nov 2019 03:59:38 +0000
Subject: [PATCH 13/20] Recalibrated the servos

---
 run_robot.py        | 2 +-
 sendUDPCommands.py  | 2 +-
 src/PupperConfig.py | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/run_robot.py b/run_robot.py
index a2f8675..c290722 100644
--- a/run_robot.py
+++ b/run_robot.py
@@ -25,7 +25,7 @@ def main():
     controller.movement_reference.v_xy_ref = np.array([0.0, 0.00])
     controller.movement_reference.wz_ref = 0
     controller.swing_params = SwingParams()
-    controller.swing_params.z_clearance = 0.06
+    controller.swing_params.z_clearance = 0
     controller.stance_params = StanceParams()
     controller.stance_params.delta_y = 0.08
     controller.gait_params = GaitParams()
diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index ff7941a..7322b3f 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -41,7 +41,7 @@ def handler(signum, frame):
         break
         '''
     msg = {"command": "set_velocity"}
-    msg["velocity_x"] = 0.2
+    msg["velocity_x"] = 0.1
     msg["velocity_y"] = 0.0
     #print(msg)
     drive_pub.send(msg)
diff --git a/src/PupperConfig.py b/src/PupperConfig.py
index 1dfc1a1..5309902 100644
--- a/src/PupperConfig.py
+++ b/src/PupperConfig.py
@@ -17,7 +17,7 @@ def __init__(self):
 
         # The neutral angle of the joint relative to the modeled zero-angle in degrees, for each joint
         self.neutral_angle_degrees = np.array(
-            [[-12, -17, 6, -2], [49, 46, 47, 51], [-42, -31, -39, -33]]
+                [[-12, -19, 6, -2], [48, 45, 35, 52], [-40, -26, -40, -31]]
         )
 
         self.servo_multipliers = np.array(

From a9df80b06bfe03eb55821ccd8a9e97bada75dc9c Mon Sep 17 00:00:00 2001
From: Nathan Kau <nathankau@gmail.com>
Date: Wed, 13 Nov 2019 04:30:31 +0000
Subject: [PATCH 14/20] Removed broken user interface

---
 readUDPCommands.py | 4 ++--
 sendUDPCommands.py | 3 +++
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 01967ee..8067a0a 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -81,9 +81,9 @@ def main():
         send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
         while now - last_loop < controller.gait_params.dt:
             now = time.time()
-        print("Time since last loop: ", now - last_loop)
+       # print("Time since last loop: ", now - last_loop)
     end = time.time()
-    print("seconds per loop: ", (end - start) / 1000.0)
+    #print("seconds per loop: ", (end - start) / 1000.0)
 
 
 main()
diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 4157b38..5674cd0 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -19,6 +19,7 @@ def handler(signum, frame):
 time.sleep(5)
 # Prints the values for axis0
 while True:
+    '''
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     msg = {"command": command}
     if command == "set_velocity" or command[:3] == "set":
@@ -38,5 +39,7 @@ def handler(signum, frame):
         msg["radians"] = degrees
     elif command == "break":
         break
+        '''
+    msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}
     drive_pub.send(msg)
     time.sleep(2)

From 0460cadc861b09d8dd90e39f23b5a8d22694933e Mon Sep 17 00:00:00 2001
From: Nathan Kau <nathankau@gmail.com>
Date: Wed, 20 Nov 2019 03:06:42 +0000
Subject: [PATCH 15/20] Fixed bugs with user input

---
 readUDPCommands.py | 31 ++++++++++++++++---------------
 sendUDPCommands.py | 25 +++++++++++++------------
 2 files changed, 29 insertions(+), 27 deletions(-)

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 8067a0a..9143908 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -61,26 +61,27 @@ def main():
     now = last_loop
     start = time.time()
     values = UDPComms.Subscriber(8870)
-    for i in range(6000):
+    while 1:
         last_loop = time.time()
         try:
             msg = values.get()
             command = msg["command"]
+            print(msg)
+        #print(msg)
+            if command == "set_velocity":
+                set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
+            elif command == "turn_radians":
+                turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["radians"])
+            elif command == "turn_degrees":
+                turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["degrees"])
+
+            step_controller(controller)
+            send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
+            while now - last_loop < controller.gait_params.dt:
+                now = time.time()
         except UDPComms.timeout:
-            # no commands received yet
-            command = "none"
-
-        if command == "set_velocity":
-            set_velocity(controller, msg["velocity_x"], msg["velocity_y"])
-        elif command == "turn_radian":
-            turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["radians"])
-        elif command == "turn_degrees":
-            turn_radians(pi_board, pwm_params, servo_params, controller, msg["speed"], msg["degrees"])
-
-        step_controller(controller)
-        send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
-        while now - last_loop < controller.gait_params.dt:
-            now = time.time()
+            continue
+            #print("no command received")
        # print("Time since last loop: ", now - last_loop)
     end = time.time()
     #print("seconds per loop: ", (end - start) / 1000.0)
diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 5674cd0..e740d54 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -18,28 +18,29 @@ def handler(signum, frame):
 os.putenv('DISPLAY', ':0.0')
 time.sleep(5)
 # Prints the values for axis0
+command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
 while True:
-    '''
-    command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
-    msg = {"command": command}
     if command == "set_velocity" or command[:3] == "set":
+        msg = {"command": "set_velocity"}
         velocity_x = input("Please enter an x velocity: ")
         velocity_y = input("Please enter an y velocity: ")
-        msg["velocity_x"] = velocity_x
-        msg["velocity_y"] = velocity_y
+        msg["velocity_x"] = float(velocity_x)
+        msg["velocity_y"] = float(velocity_y)
     elif command == "turn_radian" or command[:4] == "turn" and "radian" in command:
+        msg = {"command": "turn_radians"}
         speed = input("Please enter an turn speed: ")
         radians = input("Please enter the number of radians you wish to turn: ")
-        msg["speed"] = speed
-        msg["radians"] = radians
+        msg["speed"] = float(speed)
+        msg["radians"] = float(radians)
     elif command == "turn_degrees" or command[:4] == "turn" and "degrees" in command:
+        msg = {"command": "turn_degrees"}
         speed = input("Please enter an turn speed: ")
         degrees = input("Please enter the number of degrees you wish to turn: ")
-        msg["speed"] = speed
-        msg["radians"] = degrees
+        msg["speed"] = float(speed)
+        msg["radians"] = float(degrees)
     elif command == "break":
         break
-        '''
-    msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}
+    #msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}
+    print(msg)
     drive_pub.send(msg)
-    time.sleep(2)
+    #time.sleep(2)

From a19929138bd13ebc6c8b50d7af3cae400e87a3bf Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 19 Nov 2019 19:13:50 -0800
Subject: [PATCH 16/20] Put command sending in separate thread

---
 sendUDPCommands.py | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index e740d54..3a853df 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -1,5 +1,6 @@
 import os
 import time
+import threading
 from UDPComms import Publisher
 import signal
 
@@ -10,18 +11,27 @@
 def handler(signum, frame):
     print("GOT singal", signum)
 
+def send_message(message):
+    drive_pub.send(message)
 
 signal.signal(signal.SIGHUP, handler)
 
 # those two lines allow for running headless (hopefully)
 os.environ["SDL_VIDEODRIVER"] = "dummy"
 os.putenv('DISPLAY', ':0.0')
-time.sleep(5)
+time.sleep(2)
 # Prints the values for axis0
-command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
+
+msg = {"command": ""}
+
+message_thread = threading.Thread(target=send_message, args=[msg])
+message_thread.daemon = True
+message_thread.start()
+
 while True:
+    command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     if command == "set_velocity" or command[:3] == "set":
-        msg = {"command": "set_velocity"}
+        msg["command"] = "set_velocity"
         velocity_x = input("Please enter an x velocity: ")
         velocity_y = input("Please enter an y velocity: ")
         msg["velocity_x"] = float(velocity_x)
@@ -42,5 +52,5 @@ def handler(signum, frame):
         break
     #msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}
     print(msg)
-    drive_pub.send(msg)
+
     #time.sleep(2)

From 9e1c518b6ba4550057cd34f8499d3d3d3b579551 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 19 Nov 2019 19:16:13 -0800
Subject: [PATCH 17/20] Fixed thread

---
 sendUDPCommands.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index 3a853df..e9b2611 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -12,7 +12,8 @@ def handler(signum, frame):
     print("GOT singal", signum)
 
 def send_message(message):
-    drive_pub.send(message)
+    while True:
+        drive_pub.send(message)
 
 signal.signal(signal.SIGHUP, handler)
 

From 9b9c58e076b1300dd8e394f502559e01a29e8c34 Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 19 Nov 2019 19:20:57 -0800
Subject: [PATCH 18/20] Put message updates into one line

---
 sendUDPCommands.py | 12 +++---------
 1 file changed, 3 insertions(+), 9 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index e9b2611..e580727 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -32,23 +32,17 @@ def send_message(message):
 while True:
     command = input("Please enter an command (set_velocity, turn_radian, or turn_degrees or break): ")
     if command == "set_velocity" or command[:3] == "set":
-        msg["command"] = "set_velocity"
         velocity_x = input("Please enter an x velocity: ")
         velocity_y = input("Please enter an y velocity: ")
-        msg["velocity_x"] = float(velocity_x)
-        msg["velocity_y"] = float(velocity_y)
+        msg = {"command": "set_velocity", "velocity_x": float(velocity_x), "velocity_y": float(velocity_y)}
     elif command == "turn_radian" or command[:4] == "turn" and "radian" in command:
-        msg = {"command": "turn_radians"}
         speed = input("Please enter an turn speed: ")
         radians = input("Please enter the number of radians you wish to turn: ")
-        msg["speed"] = float(speed)
-        msg["radians"] = float(radians)
+        msg = {"command": "turn_radians", "speed": float(speed), "radians": float(radians)}
     elif command == "turn_degrees" or command[:4] == "turn" and "degrees" in command:
-        msg = {"command": "turn_degrees"}
         speed = input("Please enter an turn speed: ")
         degrees = input("Please enter the number of degrees you wish to turn: ")
-        msg["speed"] = float(speed)
-        msg["radians"] = float(degrees)
+        msg = {"command": "turn_radians", "speed": float(speed), "degrees": float(degrees)}
     elif command == "break":
         break
     #msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}

From de5a2d611251cb5cbb70998f1a64d0854104921e Mon Sep 17 00:00:00 2001
From: Isaiah-Turner <isaiahturner42@gmail.com>
Date: Tue, 19 Nov 2019 19:29:02 -0800
Subject: [PATCH 19/20] Fixed dictionary update

---
 sendUDPCommands.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/sendUDPCommands.py b/sendUDPCommands.py
index e580727..2e782e8 100644
--- a/sendUDPCommands.py
+++ b/sendUDPCommands.py
@@ -34,15 +34,15 @@ def send_message(message):
     if command == "set_velocity" or command[:3] == "set":
         velocity_x = input("Please enter an x velocity: ")
         velocity_y = input("Please enter an y velocity: ")
-        msg = {"command": "set_velocity", "velocity_x": float(velocity_x), "velocity_y": float(velocity_y)}
+        msg.update({"command": "set_velocity", "velocity_x": float(velocity_x), "velocity_y": float(velocity_y)})
     elif command == "turn_radian" or command[:4] == "turn" and "radian" in command:
         speed = input("Please enter an turn speed: ")
         radians = input("Please enter the number of radians you wish to turn: ")
-        msg = {"command": "turn_radians", "speed": float(speed), "radians": float(radians)}
+        msg.update({"command": "turn_radians", "speed": float(speed), "radians": float(radians)})
     elif command == "turn_degrees" or command[:4] == "turn" and "degrees" in command:
         speed = input("Please enter an turn speed: ")
         degrees = input("Please enter the number of degrees you wish to turn: ")
-        msg = {"command": "turn_radians", "speed": float(speed), "degrees": float(degrees)}
+        msg.update({"command": "turn_radians", "speed": float(speed), "degrees": float(degrees)})
     elif command == "break":
         break
     #msg = {"command": "set_velocity", "velocity_x": 0.1, "velocity_y": 0.0}

From eaa7f24975ae2bd858d1b4d31f790f1fdedad3e8 Mon Sep 17 00:00:00 2001
From: Nathan Kau <nathankau@gmail.com>
Date: Sat, 22 Feb 2020 22:33:49 +0000
Subject: [PATCH 20/20] fixed turning

---
 readUDPCommands.py       |   4 ++--
 src/.PupperConfig.py.swp | Bin 0 -> 16384 bytes
 2 files changed, 2 insertions(+), 2 deletions(-)
 create mode 100644 src/.PupperConfig.py.swp

diff --git a/readUDPCommands.py b/readUDPCommands.py
index 9143908..a742f72 100644
--- a/readUDPCommands.py
+++ b/readUDPCommands.py
@@ -28,9 +28,9 @@ def turn_degrees(pi_board, pwm_params, servo_params, controller, speed, degrees)
     turn_radians(pi_board, pwm_params, servo_params, controller, speed, degrees * np.pi / 180)
 
 
-def turn_for_time(pi_board, pwm_params, servo_params, controller, speed, time):
+def turn_for_time(pi_board, pwm_params, servo_params, controller, speed, time_len):
     start_time = time.time()
-    while time.time() - start_time < time:
+    while time.time() - start_time < time_len:
         controller.movement_reference.v_xy_ref = np.array([0.0, 0.00])
         controller.movement_reference.wz_ref = speed
         step_controller(controller)
diff --git a/src/.PupperConfig.py.swp b/src/.PupperConfig.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..1ae85e9b7297d2f8fa07b45ff25314d97c196ceb
GIT binary patch
literal 16384
zcmeI3ZHy#E8OQs;6F3DCBm%xr?x49lc4p^oZ+B7Dy}j9GaA#+k-R016X?tgCW}EBj
z9=dz>O+bYZ;~7!oOB6XH$crD0AB^Dx93KqPXoAL1iVA!)@rzLr6iJN!pQ?VF-r1K+
zjFF@}`<t1rs($LJr>dTMs%Ca2drP^(#xpqu*QJW`%){@v?ZQ_NDEC~TD7Iy}l)Tr~
z?RC4{77}(XI<jhY_()eDnHLYtZ@R0NxuADO9J}GAO<873Z)dujy9!D&6AL63I5!LQ
z+*W%0Vx^GJWm7}%VV7UFbZ*Zlr4tJz7Dz0RSRk=LVu8d0i3Ji1Bo=txTfps}r~DhO
zI1ii#2RzQlyA3|?-Qz8}5(^|2NGy<8AhAGVfy4rd1riG+7Dz0RSRk=LVu9DG1+=E3
zybrps&;bCx|EDv6KfOg!ehGd7o&`^V?}D4b7`O(!8@vk~0xw;nC{F+jnBW5N+{KFW
z1#mC82b=`&1ebxgg7d*2-mEBJ178JS0e6CBuml#t2f*dvpKnr>7r_hQdGHK)7(4*J
z2|fdE1s?<{a0t9~NKyU@{sNu{&w!`Eli*=+H)wzvFb&4R)!+d5+eM1<IH-dgzzBFR
zxE7?qrQp>IAshS|{02M;9su_O2QY95yoTRVJPsZM-vD=kI`|kk4z2|U!Lx5vlple|
zz<r<zroa$*@t~so3j7p&8=L_Tg44hOr@-x?2Hp>@0snY|qWlni51a;DAP24h2f@=g
z;5iF^0KN<~@Ci@@W8fliA$Ser;zjT%cm#YMd=k)jQNdmp3oe_xJ=<hvH>26Mws}~g
zoF;Fv?yBZ+)zv%PIqYzwb>w=H_2I5<;b$c-ehNY*>GDM6*-=IXGu9fn4Xul38>bw0
zolJDrb+av#6ncoNa9qu7P#q|@zdCf&6$LZCxDIC{vVx|2BorK0c8+pG-3Ln!^Y_;I
z2xBSM?Ae;DTPCww%w6TutZaRiBgHf}nY(VW7UwS0Y>pIBrfpk2vng6brlKp0p8|;w
zco9a97~lpbdyVpUuM<^hhQV4^&)#391%-st73*18r>|N?6!Ko`_1rM?{+5T?e(#n8
zA)>N|Wx86!RmDJbP+k8RrMg5pv)2jB6#Hc~T|Ae~WTo+F%&Dqw>aME#<3f}l8XEH7
zXEfbq^O~)7xXW!vX&9R0FiI7P&h?mD%)3POTWZ7NtrjWhSjHMZJgIICvlVqq4o4~)
zexkdunRG{2J)0S2M~|}NFmx543c;s*@CgR>a<j|Big+;cQpdx13dO11paNN>i@%);
zcs(Y&ZdVDAlZzFo2O6RM13L>QiKf>Beegk8QdMC~gTwal3d^C<wr4`lmZ*<<H1d$>
z)M2*e`>IG*#^EkHH0(UjYi$?4qhMd^b`L0KU5(~BTjB0H$K-acUSqXst(&Z=OFzdn
zvx#I^GuWzS>svIu4bKO%4jfB|n{Hr5h6~*t+zUM+TtnMq)BS%?v1VWv_SG!(iP#sJ
z!8{|~{d|9J%vmNEjoHs6Tp<6weHM!3?b9$EDOB;G>E)07f%76QRBdv@)znRIC<b@C
zWX2`{)m@K>+UOI7yQrx6ZA?};nRZ;BVMeWIxT+jmzBeN!2CUE#wKio(jVV-=u1Ma7
z$crsBTOtO4lQXF*&!6TMdURi1Db{$_jeFDuk-oh%g*vu{RZ0vfQ4iH6(!09bSRItH
zrP}ZWH?TC;$z%P`towNwp8Yv%>c*z`T(GnVujKij{WZ_Bxf4uZ#6m4@bNDZB;i!1?
zy5^@3SU>(xRBu?F4qQ}B@hO=aRXgfAP2O{D%}_P7ZE&^8+ct+ej?!A))e)NI#$k4f
zgYXk#goQ2~AhWfmu9=SL46<>bjJ*QE{>dJXG}3Yn6K`JR5hN{#o<R&IhudqG+QB5!
zyM~Un9^)q%2jMyquW%IWYz8lBthTv4RdQ^U?u%o{z#=?~TuM#nCr~Jl#VuW+;>AK-
zfxdPJwu>r(aWAeETSVySmB%^)Tl?|k?G|EU3_Am>&l)H5Qk2=T;K6v4ZE@R5%knx_
zBH2B|2mo!yHB2S(bl6s5myX`BEk{L=ucD`5lw1}aDm!w->sWMXrRTU<?N~!I^cBRo
zJPcTc7{7I0%h4$SR=evR5;mF@<DsHAn`nift57=!O*k2UEl^+W;4E4;zXO8vqp|6Y
zFrrOSYB8IQ<#ctkue0lQ3Y*Y(xp72L1?oNOT=ZHVotiIF0_D;TO6EtGckvKX;rWx5
zP~Dbob(qu8yPFw9H#MWp^iJ2pWDo(P;@u#Q?QSy7fr%A=|9>9s<*@&!@BAOfp8gDY
z5Zn)JFbayG0BHaJFYNJO0WX8+!Oy^>;4W|{Xn?nYv)Jc91nvW?;0o{x_Vh1<=fGL;
z9q=h|H)w--FagE^?f(ygr?IE+fs>#NJ_KmLe>pe^UckQn32;AXgO7qLm;)J*2A6<8
zV(<QAa0bkSDmWj|{{3@+gCTGw_&xUJzXLx3-v=9D1>`^$Tm@di{`}|QAy5Wag13WT
zV-Nl%@I_#QNpK0c82lak@3Y`3@Cf)cSOy;lb#NREfvdq)-~jk7_Tp#2gWy}>v!D%B
za2uEgFQE?~0~J91c>wHviFuC~F_&ga)rz`Moh?`9F`$RCnF$126QdMyWphL1UfUf1
z#NpbT%-c0Z7u<oOYNcr@Dw|_c1a0-{ak<P>u-eo*m?~{JY>I5jX;3wO71Du|aG~Q(
z)$-ip?n+8uC91GBa$Aw8pijv_R;cAUsVOQ77nr^Fe3jl!)|4e@qa9Vsb3U}Kv$25T
ze_*2PplJ4qMVL|a19gML(YmG+>;fDe>x<m~J1+0WOjcRKvpMYe@TAy8vX-qk$XP06
zjzR8FWJI>BC38);Et9qm*i1z1DOHbT@1TFJbW3%1X|bf%Cg)~KQlD6GSed+4l|8ap
zt=+stg$j{aD#fNPyTO^EcXZc@EhY=4+AUQTm8c(~p<9s7f8JXYSR2ED)#o}RSZTuL
zImLv(m(7Vt6x*y5-k@|X`;K04WMcC*%jmgWu4tj1$&PcAi%Yf1S@pzZWxhN&gA=LJ
zY-wt-R-OtIg<Ywz=v30zFHPI6Ab0zNXcT(CI6-~rtA>b>q6*9$;rUmz#;J6vG95^X
zD@21pEQ&h_2u4L9A;r2jorMiaLuqzL!!aHo^7b5)wF<Sl6vi&1S$;Bfo}u#wI<f1h
zwgpq-6PA1Vi89cdvWa+2*C^0Hh8V;cVv(r@YXuL<IUq)IO-P|2uNU@%UyUf2J4W)Q
z>axxb4q1Z6%|O3=UKm7{Y2!>lI-#IUirpjvA2H%1LqxQ5wlt%b=SsE3@+3qZHh5db
zhKt^?zoWOv*^Ed<DN!g8FYb_<qYySY*d)2LzTv1ypW1!`9>a*pg)s+Z(9uVZr;mk0
zNoZQBo>-bKMU^dNj!lg7G-8jjOfEl$M-W<OvSXu23%fJ?gg#&$t$WzeIx9_$ZCp=t
z6TOKBAI@dQi$$KM&?S=}#V3ktGKB)97LJ4>geKFq$x2iax7Rgzc+ikyy4f&#O+=Uo
zFIV+0CJI6_M;vM()NHu$LV6?Y#FQVPHZ*$$=b9~jh1;TZ7bj{s@}=4BI)14Z9k_|?
zrru;G{=<<PMi*@zI$dr$(v7xoB#7aPL0N#8ZD>75s-%0PwZk<BGl3i;TSqW;JHphs
z(I*!c&>fLxS19E=CU(&ouTQ+RU_jCR5g4|p2(|FimnD0>AV%$}c7K_uL@_wV2NsiE
zAnK)3qsQW<QmkgJST4Fk&dh;>e_?U5wy54%D$m>~m!?A4{IJid4WYxOJGw-RCXb(3
znp!MZ=hW)-^g?NI`&pwr$=JKxe^Fd`A~H}Y_2-^@x9#nXoI%D;V8|ME*-kO*Yo2HI
zZ+6TYBqtas)5jxX=q3~szl+I<wV%|e*2**GxgA<As`NsW_{Qj@7^}JXu4q;y=f~nc
z>c;ZCHxlF+&lTk9PcUGpfLB#M7n>fGC+o|O6$j+WO0$KDVmL~L?$g!kB4Uyg<t1t%
zIaGxzq-)>^kcA2?g`)W}avQ<9t5}4!m1a+bR!rN_9oi?uEh|EvFH-bvP6*273$deD
zk(u{02j%UORg=mUg>uE5h!qUpa?`fnUUg%NHsHD7DX~!$yLo8I;4!_!Q_?pwnq-~A
zO`wsco2F1>Z+T<^g+$TiI~9Ki5$lIrE3;~KiMn@4{zJKH_1w%x$B^HckOZL+l`GGM
z#rlh+81k_O7+hAopU_tRJ33p{?B+UsIkqjhl?ArPwR?RZ8La32cWC%3Poaj8R`P34
ggPU!4b&pfkes3zze*3;3^d@_x8ZTv!Qp?JJ0Gz}UT>t<8

literal 0
HcmV?d00001