moved the joystick parsing code into UserInput.py

Nate711 · Nate711 · commit f8b798c23e76 · 2019-12-10T13:42:29.000-08:00
diff --git a/run_robot.py b/run_robot.py
@@ -1,4 +1,7 @@
 import pigpio
+import numpy as np
+import UDPComms
+import time
 from src.Controller import step_controller, Controller
 from src.HardwareInterface import send_servo_commands, initialize_pwm
 from src.PupperConfig import (
@@ -9,16 +12,16 @@
     ServoParams,
     PWMParams,
 )
-import time
-import numpy as np
-import UDPComms
+from src.UserInput import UserInputs, get_input, update_controller
 
 
 def main():
     """Main program
     """
     pi_board = pigpio.pi()
     pwm_params = PWMParams()
+    initialize_pwm(pi_board, pwm_params)
+
     servo_params = ServoParams()
 
     controller = Controller()
@@ -34,68 +37,31 @@ def main():
     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)
+    user_input = UserInputs()
 
-    values = UDPComms.Subscriber(8830, timeout=0.3)
     last_loop = time.time()
     now = last_loop
     start = time.time()
 
-    gait_mode = 0  # 0 for non-walking, 1 for walking
-    prev_gait_toggle = 0
-
-    non_walking_gait = np.array(
-        [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]
-    )
-    walking_gait = np.array([[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]])
-
     for i in range(60000):
         last_loop = time.time()
-        step_controller(controller)
-        send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
-
-        try:
-            msg = values.get()
-        except UDPComms.timeout:
-            print("timeout")
-            msg = {
-                "x": 0,
-                "y": 0,
-                "twist": 0,
-                "pitch": 0,
-                "gait_toggle": 0,
-                "stance_movement": 0,
-            }
-        x_vel = msg["y"] / 7.0
-        y_vel = -msg["x"] / 7.0
-        yaw_rate = -msg["twist"] * 0.8
-        gait_toggle = msg["gait_toggle"]
-        stance_movement = msg["stance_movement"]
-
-        # Check for gait toggle
-        if prev_gait_toggle == 0 and gait_toggle == 1:
-            gait_mode = not gait_mode
-        prev_gait_toggle = gait_toggle
+        
+        # Parse the udp joystick commands and then update the robot controller's parameters
+        get_input(user_input)
+        update_controller(controller, user_input)
 
-        pitch = msg["pitch"] * 30.0 * np.pi / 180.0
-
-        controller.movement_reference.v_xy_ref = np.array([x_vel, y_vel])
-        controller.movement_reference.wz_ref = yaw_rate
-        controller.movement_reference.pitch = pitch
-
-        if gait_mode == 0:
-            controller.gait_params.contact_phases = non_walking_gait
-        else:
-            controller.gait_params.contact_phases = walking_gait
+        # Step the controller forward by dt
+        step_controller(controller)
 
-        # Note this is negative since it is the feet relative to the body
-        controller.movement_reference.z_ref -= 0.001 * stance_movement
+        # Update the pwm widths going to the servos
+        send_servo_commands(pi_board, pwm_params, servo_params, controller.joint_angles)
 
+        # Wait until it's time to execute again
         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)
 
diff --git a/src/Controller.py b/src/Controller.py
@@ -22,6 +22,7 @@ def __init__(self):
 
         self.ticks = 0
 
+        # Set default for foot locations and joint angles
         self.foot_locations = (
             self.stance_params.default_stance
             + np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis]
@@ -111,30 +112,4 @@ def step_controller(controller):
     controller.joint_angles = four_legs_inverse_kinematics(
         rotated_foot_locations, controller.robot_config
     )
-    controller.ticks += 1
-
-
-def run():
-    """Testing function that runs the robot for one second.
-    
-    Returns
-    -------
-    (Numpy array (3, 4, timesteps), Numpy array (3, 4, timesteps))
-        (history of foot locations, history of joint angles)
-    """
-    c = Controller()
-    c.movement_reference.v_xy_ref = np.array([0.2, 0.0])
-    c.movement_reference.wz_ref = 0.5
-
-    tf = 1.0
-    time_steps = int(tf / c.gait_params.dt)
-
-    foot_loc_history = np.zeros((3, 4, time_steps))
-    joint_angle_history = np.zeros((3, 4, time_steps))
-
-    for i in range(time_steps):
-        step_controller(c)
-        foot_loc_history[:, :, i] = c.foot_locations
-        joint_angle_history[:, :, i] = c.joint_angles
-
-    return foot_loc_history, joint_angle_history
+    controller.ticks += 1
diff --git a/src/PupperConfig.py b/src/PupperConfig.py
@@ -1,5 +1,4 @@
 import numpy as np
-import numpy as np
 from scipy.linalg import solve
 
 
diff --git a/src/UserInput.py b/src/UserInput.py
@@ -0,0 +1,54 @@
+import UDPComms
+import numpy as np
+
+
+class UserInputs:
+    def __init__(self, udp_port=8830):
+        self.x_vel = 0.0
+        self.y_vel = 0.0
+        self.yaw_rate = 0.0
+        self.pitch = 0.0
+        self.stance_movement = 0
+        self.gait_toggle = 0
+        self.gait_mode = 0
+        self.previous_gait_toggle = 0
+        self.udp_handle = UDPComms.Subscriber(udp_port, timeout=0.3)
+
+
+def get_input(user_input_obj):
+    try:
+        msg = user_input_obj.udp_handle.get()
+        user_input_obj.x_vel = msg["y"] * 0.14
+        user_input_obj.y_vel = msg["x"] * -0.14
+        user_input_obj.yaw_rate = msg["twist"] * -0.8
+        user_input_obj.pitch = msg["pitch"] * 30 * np.pi / 180.0
+        user_input_obj.gait_toggle = msg["gait_toggle"]
+        user_input_obj.stance_movement = msg["stance_movement"]
+
+        # Update gait mode
+        if user_input_obj.previous_gait_toggle == 0 and user_input_obj.gait_toggle == 1:
+            user_input_obj.gait_mode = not user_input_obj.gait_mode
+        user_input_obj.previous_gait_toggle = user_input_obj.gait_toggle
+
+    except UDPComms.timeout:
+        print("UDP Timed out")
+
+
+def update_controller(controller, user_input_obj):
+    controller.movement_reference.v_xy_ref = np.array(
+        [user_input_obj.x_vel, user_input_obj.y_vel]
+    )
+    controller.movement_reference.wz_ref = user_input_obj.yaw_rate
+    controller.movement_reference.pitch = user_input_obj.pitch
+
+    if user_input_obj.gait_mode == 0:
+        controller.gait_params.contact_phases = np.array(
+            [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]
+        )
+    else:
+        controller.gait_params.contact_phases = np.array(
+            [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]
+        )
+
+    # Note this is negative since it is the feet relative to the body
+    controller.movement_reference.z_ref -= 0.001 * user_input_obj.stance_movement

Original file line number	Diff line number	Diff line change
`@@ -1,5 +1,4 @@`
`1`	`1`	`import numpy as np`
`2`		`-import numpy as np`
`3`	`2`	`from scipy.linalg import solve`
`4`	`3`
`5`	`4`