From 35a477488cbe73f5f51045ebaca8a8b4e7b95af9 Mon Sep 17 00:00:00 2001 From: chrisruk Date: Thu, 1 Sep 2022 10:34:52 +0100 Subject: [PATCH 1/4] Initial motors class --- buildhat/__init__.py | 2 +- buildhat/motors.py | 115 ++++++++++++++++++++++++++++++++++--------- 2 files changed, 92 insertions(+), 25 deletions(-) diff --git a/buildhat/__init__.py b/buildhat/__init__.py index da3a147..f3f98fe 100644 --- a/buildhat/__init__.py +++ b/buildhat/__init__.py @@ -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 diff --git a/buildhat/motors.py b/buildhat/motors.py index 62f80ea..2fbdc87 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -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: @@ -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] @@ -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 + cmd = self._run_positional_ramp(pos, newpos, speed, run) + if run: + self._runmode = MotorRunmode.NONE + return 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) @@ -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\r" + 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 cmd def run_for_degrees(self, degrees, speed=None, blocking=True): """Run motor for N degrees @@ -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 @@ -520,3 +530,60 @@ 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 = "" + for motor in self._ports: + cmd += motor._run_to_position(degrees, speed, direction, run=False) + self._write(f"{cmd}\r") + + def start(self): + speed = self.default_speed + cmd = "" + 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} ; " + 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") + + def _write(self, cmd): + #self.isconnected() + Device._instance.write(cmd.encode()) From cf2023459652c16441bfc84f7a0edee272e41cdf Mon Sep 17 00:00:00 2001 From: chrisruk Date: Wed, 23 Nov 2022 15:40:57 +0000 Subject: [PATCH 2/4] Send port command --- buildhat/motors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index 2fbdc87..1653f22 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -566,7 +566,7 @@ def run_to_position(self, degrees, direction="shortest"): speed = self.default_speed cmd = "" for motor in self._ports: - cmd += motor._run_to_position(degrees, speed, direction, run=False) + cmd += f"port {motor.port} ; {motor._run_to_position(degrees, speed, direction, run=False)} " self._write(f"{cmd}\r") def start(self): From 5ed16049046277076668ac8ea8bf500b105f8968 Mon Sep 17 00:00:00 2001 From: chrisruk Date: Thu, 24 Nov 2022 11:04:34 +0000 Subject: [PATCH 3/4] Add sleep to see if that helps --- buildhat/motors.py | 1 + 1 file changed, 1 insertion(+) diff --git a/buildhat/motors.py b/buildhat/motors.py index 1653f22..3b42b01 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -583,6 +583,7 @@ def stop(self): 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() From cdb73688396383990d162a689b72848fef71d272 Mon Sep 17 00:00:00 2001 From: chrisruk Date: Mon, 28 Nov 2022 10:11:46 +0000 Subject: [PATCH 4/4] Fix motor spinning backward then forward issue --- buildhat/motors.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index 3b42b01..8eee8bf 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -181,10 +181,10 @@ def _run_to_position(self, degrees, speed, direction, run=True): 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 - cmd = self._run_positional_ramp(pos, newpos, speed, run) + setup, cmd = self._run_positional_ramp(pos, newpos, speed, run) if run: self._runmode = MotorRunmode.NONE - return cmd + return (setup, cmd) def _run_positional_ramp(self, pos, newpos, speed, run): """Ramp motor @@ -196,9 +196,9 @@ def _run_positional_ramp(self, pos, newpos, speed, run): # Collapse speed range to -5 to 5 speed *= 0.05 dur = abs((newpos - pos) / speed) - 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\r" + 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) + #self._write(setup) if run: self._write(f"{cmd}\r") with self._hat.rampcond[self.port]: @@ -206,7 +206,7 @@ def _run_positional_ramp(self, pos, newpos, speed, run): if self._release: time.sleep(0.2) self.coast() - return cmd + return (setup, cmd) def run_for_degrees(self, degrees, speed=None, blocking=True): """Run motor for N degrees @@ -565,23 +565,29 @@ def run_to_position_ok(self, degrees, direction="shortest"): def run_to_position(self, degrees, direction="shortest"): speed = self.default_speed cmd = "" + setup = "" for motor in self._ports: - cmd += f"port {motor.port} ; {motor._run_to_position(degrees, speed, direction, run=False)} " + 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} ; " + i = i + 1 self._write(f"{cmd}\r") def stop(self): cmd = "" for motor in self._ports: - cmd += f"port {motor.port} ; coast ; " + cmd += f"port {motor.port} ; coast ;" self._write(f"{cmd}\r") time.sleep(1)