Refactoring

.gitignore

@@ -0,0 +1,3 @@
+pi/mc/__pycache__/*
+*.py[cod]
+pi/mc/pycache/*

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

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))

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:

pi/mc/mc.py → 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: