diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a617832 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +pi/mc/__pycache__/* +*.py[cod] +pi/mc/pycache/* \ No newline at end of file diff --git a/pi/mc/__pycache__/MoteusException.cpython-39.pyc b/pi/mc/__pycache__/MoteusException.cpython-39.pyc deleted file mode 100644 index a87cde3..0000000 Binary files a/pi/mc/__pycache__/MoteusException.cpython-39.pyc and /dev/null differ diff --git a/pi/mc/get_cpu_command.py b/pi/mc/get_cpu_command.py index f3789e1..9618b4b 100644 --- a/pi/mc/get_cpu_command.py +++ b/pi/mc/get_cpu_command.py @@ -7,7 +7,13 @@ MSG_SIZE = 1024 +""" +Receives a command from the moteus controller cpu. +Args: +sock : socket we are reading from +_m : The moteus controller to send messages to +""" async def get_cpu_command(sock: socket.socket, _m: MoteusController): """ Receive command from bridge nodes using tcp socket diff --git a/pi/mc/moteus_controller.py b/pi/mc/moteus_controller.py index 21b7024..b68f8c0 100644 --- a/pi/mc/moteus_controller.py +++ b/pi/mc/moteus_controller.py @@ -1,6 +1,9 @@ import abc import asyncio import math +import json +import time +import socket from copy import deepcopy from MoteusException import MoteusCanError @@ -278,7 +281,14 @@ def is_ready(self): return self.isReady.is_set() # Wait until it is ready or there is an error + """ + Commands the dog to jump + Args: + factor (int): Essentially the multiplier of force to use when jumping + pos_offset_1 (int): + pos_offset_2 (int): + """ async def jump(self, factor=3, pos_offset_1=0, pos_offset_2=0): self.mprint('crouching') crouchtask1 = asyncio.create_task( @@ -307,3 +317,56 @@ async def jump(self, factor=3, pos_offset_1=0, pos_offset_2=0): await jumpreturn2 self.set_attributes(1, pos=math.nan, velocity=0, torque=2) self.set_attributes(2, pos=math.nan, velocity=0, torque=2) + + """ + Sends position velocity torque and other data to all the other motors + + Args: + + """ + async def send_mc_states(self, sock: socket.socket): + mc_id = 1 + loop = asyncio.get_event_loop() + while True: + start_time = time.time() + # 0. get data from the 12 motors + parsed_res = self.get_parsed_results() + if not parsed_res: + await asyncio.sleep(0.02) + continue + + # 1. if there is only 1 motor connected + if len(parsed_res) == 1: + # set all to the same vel and torque of the only motor connected and add to parsedRes + pos, vel, tor = parsed_res[0]["POSITION"], parsed_res[0]["VELOCITY"], parsed_res[0]["TORQUE"] + parsed_res += [{ + "MODE": 3, + "POSITION": pos, + "VELOCITY": vel, + "TORQUE": tor, + "VOLTAGE": 1.0, + "TEMPERATURE": 1.0, + "FAULT": 1.0 + }] * 11 + # 2. set all the 12, use motor id from 1-12 and send to cpu + mcs12_current = [ + [motor_id + 1, + float("{:.4f}".format(parsed_res[0]["POSITION"])), + float("{:.4f}".format(parsed["VELOCITY"])), + float("{:.4f}".format(parsed["TORQUE"])) + ] for motor_id, parsed in enumerate(parsed_res) + ] + + # 3. create a dict + data = {"mc12": mcs12_current, "id": mc_id} + self.mprint(data) + jsonify_data = json.dumps(data) + self.mprint(f"MC: sending curr2={mcs12_current[1]}") + + # 4. send as bytes encoded json + await loop.sock_sendall(sock, jsonify_data.encode()) + mc_id += 1 + + # 5. sleep for 20ms so its sending at 50Hz + sleep_time = time.time()-start_time + await asyncio.sleep(max(0.02-sleep_time, 0)) diff --git a/pi/mc/motor_controller.py b/pi/mc/motor_controller.py index 29286d8..6daab3f 100644 --- a/pi/mc/motor_controller.py +++ b/pi/mc/motor_controller.py @@ -8,7 +8,6 @@ class MotorController(MoteusController): - async def on_open(self, transport=None, servos=None): """ This class defines the motor controller. Args: diff --git a/pi/mc/mc.py b/pi/mc/sim_controller_test.py old mode 100755 new mode 100644 similarity index 98% rename from pi/mc/mc.py rename to pi/mc/sim_controller_test.py index 396bc67..5dfb6ab --- a/pi/mc/mc.py +++ b/pi/mc/sim_controller_test.py @@ -25,7 +25,9 @@ def get_parsed_results(): "FAULT": 1.0 }] - +""" +Retrieve commands from the controller cpu +""" async def get_cpu_command(sock): loop = asyncio.get_event_loop() while True: