isopod/droidDepotPythonControl
isopod/droidDepotPythonControl
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Began to add moter control

Browse Source 
Co-authored-by: David Wright <david--wright@users.noreply.github.com>
This commit is contained in:
BackwardsMonday 2022-10-12 15:14:04 -06:00
parent 15315e06c2
commit e778edae86
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
droid.py
View File

@ -7,6 +7,19 @@ class Droid():
print("Initializing")
self.disabledLeds = 0x00
self.profile = profile
# assumes theta in degrees and r = 0 to 100 %
# returns a tuple of percentages: (left_thrust, right_thrust)
def __throttle_angle_to_thrust__(r, theta):
theta = ((theta + 180) % 360) - 180 # normalize value to [-180, 180)
r = min(max(0, r), 100) # normalize value to [0, 100]
v_a = r * (45 - theta % 90) / 45 # falloff of main motor
v_b = min(100, 2 * r + v_a, 2 * r - v_a) # compensation of other motor
if theta < -90: return -v_b, -v_a
if theta < 0: return -v_a, v_b
if theta < 90: return v_b, v_a
return v_a, -v_b
async def connect(self):
timeout=0.0
print("Connecting")
@ -66,8 +79,9 @@ class Droid():
ledOnCommand = bytearray.fromhex(f"27420f44440048{leds}")
await self.droid.write_gatt_char(0x000d, ledOnCommand)
def move (self ):
pass
def move (self, degrees, duration):
thrust = self.__throttle_angle_to_thrust__(degrees)
async def play_sound(self, sound_id=None, bank_id=None, cycle=False, volume=None):
if volume:
self.set_volume(volume)