Skip to content

Initial motors class #162

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Initial motors class #162

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
35a4774
Initial motors class
chrisruk Sep 1, 2022
cf20234
Send port command
chrisruk Nov 23, 2022
5ed1604
Add sleep to see if that helps
chrisruk Nov 24, 2022
cdb7368
Fix motor spinning backward then forward issue
chrisruk Nov 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion buildhat/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,6 @@
from .hat import Hat
from .light import Light
from .matrix import Matrix
from .motors import Motor, MotorPair, PassiveMotor
from .motors import Motor, MotorPair, Motors, PassiveMotor
from .serinterface import BuildHAT
from .wedo import MotionSensor, TiltSensor
122 changes: 98 additions & 24 deletions buildhat/motors.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ def run_for_rotations(self, rotations, speed=None, blocking=True):
raise MotorError("Invalid Speed")
self.run_for_degrees(int(rotations * 360), speed, blocking)

def _run_for_degrees(self, degrees, speed):
def _run_for_degrees(self, degrees, speed, run=True):
self._runmode = MotorRunmode.DEGREES
mul = 1
if speed < 0:
Expand All @@ -153,10 +153,12 @@ def _run_for_degrees(self, degrees, speed):
pos = self.get_position()
newpos = ((degrees * mul) + pos) / 360.0
pos /= 360.0
self._run_positional_ramp(pos, newpos, speed)
self._runmode = MotorRunmode.NONE
cmd = self._run_positional_ramp(pos, newpos, speed, run)
if run:
self._runmode = MotorRunmode.NONE
return cmd

def _run_to_position(self, degrees, speed, direction):
def _run_to_position(self, degrees, speed, direction, run=True):
self._runmode = MotorRunmode.DEGREES
data = self.get()
pos = data[1]
Expand All @@ -179,10 +181,12 @@ def _run_to_position(self, degrees, speed, direction):
raise MotorError("Invalid direction, should be: shortest, clockwise or anticlockwise")
# Convert current motor position to decimal rotations from preset position to match newpos units
pos /= 360.0
self._run_positional_ramp(pos, newpos, speed)
self._runmode = MotorRunmode.NONE
setup, cmd = self._run_positional_ramp(pos, newpos, speed, run)
if run:
self._runmode = MotorRunmode.NONE
return (setup, cmd)

def _run_positional_ramp(self, pos, newpos, speed):
def _run_positional_ramp(self, pos, newpos, speed, run):
"""Ramp motor

:param pos: Current motor position in decimal rotations (from preset position)
Expand All @@ -192,14 +196,17 @@ def _run_positional_ramp(self, pos, newpos, speed):
# Collapse speed range to -5 to 5
speed *= 0.05
dur = abs((newpos - pos) / speed)
cmd = (f"port {self.port}; combi 0 1 0 2 0 3 0 ; select 0 ; pid {self.port} 0 1 s4 0.0027777778 0 5 0 .1 3 ; "
f"set ramp {pos} {newpos} {dur} 0\r")
self._write(cmd)
with self._hat.rampcond[self.port]:
self._hat.rampcond[self.port].wait()
if self._release:
time.sleep(0.2)
self.coast()
setup = f"port {self.port}; combi 0 1 0 2 0 3 0 ; select 0 ; pid {self.port} 0 1 s4 0.0027777778 0 5 0 .1 3 ;"
cmd = f"set ramp {pos:.4f} {newpos:.4f} {dur:.4f} 0 ; "
#self._write(setup)
if run:
self._write(f"{cmd}\r")
with self._hat.rampcond[self.port]:
self._hat.rampcond[self.port].wait()
if self._release:
time.sleep(0.2)
self.coast()
return (setup, cmd)

def run_for_degrees(self, degrees, speed=None, blocking=True):
"""Run motor for N degrees
Expand Down Expand Up @@ -246,16 +253,19 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes
else:
self._run_to_position(degrees, speed, direction)

def _run_for_seconds(self, seconds, speed):
def _run_for_seconds(self, seconds, speed, run=True):
self._runmode = MotorRunmode.SECONDS
cmd = (f"port {self.port} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {self.port} 0 0 s1 1 0 0.003 0.01 0 100; "
f"set pulse {speed} 0.0 {seconds} 0\r")
self._write(cmd)
with self._hat.pulsecond[self.port]:
self._hat.pulsecond[self.port].wait()
if self._release:
self.coast()
self._runmode = MotorRunmode.NONE
setup = f"port {self.port} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {self.port} 0 0 s1 1 0 0.003 0.01 0 100;\r"
cmd = f"set pulse {speed} 0.0 {seconds} 0 ; "
self._write(setup)
if run:
self._write(f"{cmd}\r")
with self._hat.pulsecond[self.port]:
self._hat.pulsecond[self.port].wait()
if self._release:
self.coast()
self._runmode = MotorRunmode.NONE
return cmd

def run_for_seconds(self, seconds, speed=None, blocking=True):
"""Run motor for N seconds
Expand Down Expand Up @@ -520,3 +530,67 @@ def run_to_position(self, degreesl, degreesr, speed=None, direction="shortest"):
th2.start()
th1.join()
th2.join()


class Motors:
def __init__(self, *ports):
"""Initialise motor"""
Device._setup()
self._ports = []
for port in ports:
self._ports += [Motor(port)]
self.default_speed = 20

def run_for_degrees(self, degrees):
speed = self.default_speed
cmd = ""
for motor in self._ports:
cmd += motor._run_for_degrees(degrees, speed, run=False)
self._write(f"{cmd}\r")

def run_for_seconds(self, seconds):
speed = self.default_speed
cmd = ""
for motor in self._ports:
cmd += motor._run_for_seconds(seconds, speed, run=False)
self._write(f"{cmd}\r")

def run_to_position_ok(self, degrees, direction="shortest"):
speed = self.default_speed
cmd = ""
for motor in self._ports:
cmd = motor._run_to_position(degrees, speed, direction, run=False)
self._write(f"{cmd}\r")

def run_to_position(self, degrees, direction="shortest"):
speed = self.default_speed
cmd = ""
setup = ""
for motor in self._ports:
s, c = motor._run_to_position(degrees, speed, direction, run=False)
cmd += f"port {motor.port} ; {c} "
setup += s
self._write(f"{setup}\r")
self._write(f"{cmd}\r")

def start(self):
speed = self.default_speed
cmd = ""
i = 0
for motor in self._ports:
setup = f"port {motor.port} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {motor.port} 0 0 s1 1 0 0.003 0.01 0 100;\r"
self._write(setup)
cmd += f"port {motor.port} ; set {speed} ; "
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We had a problem in PR #125 where combi initialization was causing a noticeable lag in motor pairings, so it was separated out from the set speed. Perhaps loop the motor list again and put the set command in there so there's not as much lag between the starting motor movements?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point, will definitely have to alter for that! This is very rough at the moment

i = i + 1
self._write(f"{cmd}\r")

def stop(self):
cmd = ""
for motor in self._ports:
cmd += f"port {motor.port} ; coast ;"
self._write(f"{cmd}\r")
time.sleep(1)

def _write(self, cmd):
#self.isconnected()
Device._instance.write(cmd.encode())