diff --git a/readUDPCommands.py b/readUDPCommands.py new file mode 100644 index 0000000..a742f72 --- /dev/null +++ b/readUDPCommands.py @@ -0,0 +1,90 @@ +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_len): + start_time = 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) + 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) + 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: + continue + #print("no command received") + # print("Time since last loop: ", now - last_loop) + end = time.time() + #print("seconds per loop: ", (end - start) / 1000.0) + + +main() 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 new file mode 100644 index 0000000..2e782e8 --- /dev/null +++ b/sendUDPCommands.py @@ -0,0 +1,51 @@ +import os +import time +import threading +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) + +def send_message(message): + while True: + 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(2) +# Prints the values for axis0 + +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": + velocity_x = input("Please enter an x velocity: ") + velocity_y = input("Please enter an y velocity: ") + 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.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.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} + print(msg) + + #time.sleep(2) diff --git a/src/.PupperConfig.py.swp b/src/.PupperConfig.py.swp new file mode 100644 index 0000000..1ae85e9 Binary files /dev/null and b/src/.PupperConfig.py.swp differ 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(