From 734d325cdd9353e3e1a7fb10e95eb34f940e1ab2 Mon Sep 17 00:00:00 2001 From: mutesplash <49622611+mutesplash@users.noreply.github.com> Date: Sat, 12 Mar 2022 21:29:24 -0500 Subject: [PATCH 1/4] Only reset port settings for motor on first start() Don't allow the motor to start() if some other type of movement is currently running. Additionally, remember what the current speed of a start()ed motor is in order to not allow a serial command to be sent that effectively does nothing. --- buildhat/motors.py | 44 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index f3a996c..7c87934 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -19,6 +19,7 @@ def __init__(self, port): self._default_speed = 20 self.plimit(0.7) self.bias(0.3) + self.current_freerun_speed = 0 def set_default_speed(self, default_speed): """Sets the default speed of the motor @@ -34,11 +35,16 @@ def start(self, speed=None): :param speed: Speed ranging from -100 to 100 """ + if self.current_freerun_speed == speed: + # Already running at this speed, do nothing + return + if speed is None: speed = self._default_speed else: if not (speed >= -100 and speed <= 100): raise MotorException("Invalid Speed") + self.current_freerun_speed = speed cmd = "port {} ; pwm ; set {}\r".format(self.port, speed/100) self._write(cmd) @@ -46,6 +52,7 @@ def stop(self): """Stops motor""" cmd = "port {} ; off\r".format(self.port) self._write(cmd) + self.current_freerun_speed = 0 def plimit(self, plimit): if not (plimit >= 0 and plimit <= 1): @@ -64,9 +71,18 @@ class Motor(Device): :raises DeviceInvalid: Occurs if there is no motor attached to port """ + runmodes = { + 'None': 0, + 'Free': 1, + 'Degrees': 2, + 'Seconds': 3, + 'Position': 4 + } + def __init__(self, port): super().__init__(port) self.default_speed = 20 + self.current_freerun_speed = 0 self.mode([(1,0),(2,0),(3,0)]) self.plimit(0.7) self.bias(0.3) @@ -75,6 +91,7 @@ def __init__(self, port): self._cvqueue = Condition() self.when_rotated = None self._oldpos = None + self.runmode = Motor.runmodes['None'] def set_default_speed(self, default_speed): """Sets the default speed of the motor @@ -91,6 +108,7 @@ def run_for_rotations(self, rotations, speed=None, blocking=True): :param rotations: Number of rotations :param speed: Speed ranging from -100 to 100 """ + self.runmode = Motor.runmodes['Degrees'] if speed is None: self.run_for_degrees(int(rotations * 360), self.default_speed, blocking) else: @@ -99,6 +117,7 @@ def run_for_rotations(self, rotations, speed=None, blocking=True): self.run_for_degrees(int(rotations * 360), speed, blocking) def _run_for_degrees(self, degrees, speed): + self.runmode = Motor.runmodes['Degrees'] mul = 1 if speed < 0: speed = abs(speed) @@ -116,8 +135,10 @@ def _run_for_degrees(self, degrees, speed): if self._release: time.sleep(0.2) self.coast() + self.runmode = Motor.runmodes['None'] def _run_to_position(self, degrees, speed, direction): + self.runmode = Motor.runmodes['Position'] data = self.get() pos = data[1] apos = data[2] @@ -148,6 +169,7 @@ def _run_to_position(self, degrees, speed, direction): if self._release: time.sleep(0.2) self.coast() + self.runmode = Motor.runmodes['None'] def run_for_degrees(self, degrees, speed=None, blocking=True): """Runs motor for N degrees @@ -157,6 +179,7 @@ def run_for_degrees(self, degrees, speed=None, blocking=True): :param degrees: Number of degrees to rotate :param speed: Speed ranging from -100 to 100 """ + self.runmode = Motor.runmodes['Degrees'] if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -174,6 +197,7 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes :param degrees: Position in degrees :param speed: Speed ranging from 0 to 100 """ + self.runmode = Motor.runmodes['Position'] if speed is None: speed = self.default_speed if not (speed >= 0 and speed <= 100): @@ -188,12 +212,14 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes self._run_to_position(degrees, speed, direction) def _run_for_seconds(self, seconds, speed): + self.runmode = Motor.runmodes['Seconds'] cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set pulse {} 0.0 {} 0\r".format(self.port, self.port, speed, seconds); self._write(cmd); with self._hat.pulsecond[self.port]: self._hat.pulsecond[self.port].wait() if self._release: self.coast() + self.runmode = Motor.runmodes['None'] def run_for_seconds(self, seconds, speed=None, blocking=True): """Runs motor for N seconds @@ -201,6 +227,7 @@ def run_for_seconds(self, seconds, speed=None, blocking=True): :param seconds: Time in seconds :param speed: Speed ranging from -100 to 100 """ + self.runmode = Motor.runmodes['Seconds'] if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -217,16 +244,31 @@ def start(self, speed=None): :param speed: Speed ranging from -100 to 100 """ + if self.runmode == Motor.runmodes['Free']: + if self.current_freerun_speed == speed: + # Already running at this speed, do nothing + return + elif self.runmode != Motor.runmodes['None']: + # Motor is running some other mode, wait for it to stop or stop() it yourself + return + if speed is None: speed = self.default_speed else: if not (speed >= -100 and speed <= 100): raise MotorException("Invalid Speed") - cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set {}\r".format(self.port, self.port, speed) + cmd = "port {} ; set {}\r".format(self.port, speed) + if self.runmode == Motor.runmodes['None']: + cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set {}\r".format(self.port, self.port, speed) + self.runmode = Motor.runmodes['Free'] + self.current_freerun_speed = speed self._write(cmd) + def stop(self): """Stops motor""" + self.runmode = Motor.runmodes['None'] + self.current_freerun_speed = 0 self.coast() def get_position(self): From d7c96ca6475ef655c18bcfe4e4e4c44a693d86a3 Mon Sep 17 00:00:00 2001 From: mutesplash <49622611+mutesplash@users.noreply.github.com> Date: Sun, 13 Mar 2022 10:12:22 -0400 Subject: [PATCH 2/4] De-duplicate ramp command What is position but a specification of degrees --- buildhat/motors.py | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index 7c87934..e947907 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -75,8 +75,7 @@ class Motor(Device): 'None': 0, 'Free': 1, 'Degrees': 2, - 'Seconds': 3, - 'Position': 4 + 'Seconds': 3 } def __init__(self, port): @@ -127,18 +126,10 @@ def _run_for_degrees(self, degrees, speed): pos /= 360.0 speed *= 0.05 dur = abs((newpos - pos) / speed) - cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port, - self.port, pos, newpos, dur) - self._write(cmd) - with self._hat.rampcond[self.port]: - self._hat.rampcond[self.port].wait() - if self._release: - time.sleep(0.2) - self.coast() - self.runmode = Motor.runmodes['None'] + self._run_positional_ramp(pos, newpos, dur) def _run_to_position(self, degrees, speed, direction): - self.runmode = Motor.runmodes['Position'] + self.runmode = Motor.runmodes['Degrees'] data = self.get() pos = data[1] apos = data[2] @@ -161,6 +152,9 @@ def _run_to_position(self, degrees, speed, direction): pos /= 360.0 speed *= 0.05 dur = abs((newpos - pos) / speed) + self._run_positional_ramp(pos, newpos, dur) + + def _run_positional_ramp(self, pos, newpos, dur): cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port, self.port, pos, newpos, dur) self._write(cmd) @@ -197,7 +191,7 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes :param degrees: Position in degrees :param speed: Speed ranging from 0 to 100 """ - self.runmode = Motor.runmodes['Position'] + self.runmode = Motor.runmodes['Degrees'] if speed is None: speed = self.default_speed if not (speed >= 0 and speed <= 100): From d1d634fe8fb67af1c7036c0281f3e280da2f0cb2 Mon Sep 17 00:00:00 2001 From: mutesplash <49622611+mutesplash@users.noreply.github.com> Date: Thu, 17 Mar 2022 12:33:48 -0400 Subject: [PATCH 3/4] Switch to enums because minimum python version is 3.7 --- buildhat/motors.py | 44 ++++++++++++++++++++++---------------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index e947907..0bbc467 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -64,6 +64,13 @@ def bias(self, bias): raise MotorException("bias should be 0 to 1") self._write("port {} ; bias {}\r".format(self.port, bias)) + +class MotorRunmode(Enum): + NONE = 0 + FREE = 1 + DEGREES = 2 + SECONDS = 3 + class Motor(Device): """Motor device @@ -71,13 +78,6 @@ class Motor(Device): :raises DeviceInvalid: Occurs if there is no motor attached to port """ - runmodes = { - 'None': 0, - 'Free': 1, - 'Degrees': 2, - 'Seconds': 3 - } - def __init__(self, port): super().__init__(port) self.default_speed = 20 @@ -90,7 +90,7 @@ def __init__(self, port): self._cvqueue = Condition() self.when_rotated = None self._oldpos = None - self.runmode = Motor.runmodes['None'] + self.runmode = MotorRunmode.NONE def set_default_speed(self, default_speed): """Sets the default speed of the motor @@ -107,7 +107,7 @@ def run_for_rotations(self, rotations, speed=None, blocking=True): :param rotations: Number of rotations :param speed: Speed ranging from -100 to 100 """ - self.runmode = Motor.runmodes['Degrees'] + self.runmode = MotorRunmode.DEGREES if speed is None: self.run_for_degrees(int(rotations * 360), self.default_speed, blocking) else: @@ -116,7 +116,7 @@ def run_for_rotations(self, rotations, speed=None, blocking=True): self.run_for_degrees(int(rotations * 360), speed, blocking) def _run_for_degrees(self, degrees, speed): - self.runmode = Motor.runmodes['Degrees'] + self.runmode = MotorRunmode.DEGREES mul = 1 if speed < 0: speed = abs(speed) @@ -129,7 +129,7 @@ def _run_for_degrees(self, degrees, speed): self._run_positional_ramp(pos, newpos, dur) def _run_to_position(self, degrees, speed, direction): - self.runmode = Motor.runmodes['Degrees'] + self.runmode = MotorRunmode.DEGREES data = self.get() pos = data[1] apos = data[2] @@ -163,7 +163,7 @@ def _run_positional_ramp(self, pos, newpos, dur): if self._release: time.sleep(0.2) self.coast() - self.runmode = Motor.runmodes['None'] + self.runmode = MotorRunmode.NONE def run_for_degrees(self, degrees, speed=None, blocking=True): """Runs motor for N degrees @@ -173,7 +173,7 @@ def run_for_degrees(self, degrees, speed=None, blocking=True): :param degrees: Number of degrees to rotate :param speed: Speed ranging from -100 to 100 """ - self.runmode = Motor.runmodes['Degrees'] + self.runmode = MotorRunmode.DEGREES if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -191,7 +191,7 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes :param degrees: Position in degrees :param speed: Speed ranging from 0 to 100 """ - self.runmode = Motor.runmodes['Degrees'] + self.runmode = MotorRunmode.DEGREES if speed is None: speed = self.default_speed if not (speed >= 0 and speed <= 100): @@ -206,14 +206,14 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes self._run_to_position(degrees, speed, direction) def _run_for_seconds(self, seconds, speed): - self.runmode = Motor.runmodes['Seconds'] + self.runmode = MotorRunmode.SECONDS cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set pulse {} 0.0 {} 0\r".format(self.port, self.port, speed, seconds); self._write(cmd); with self._hat.pulsecond[self.port]: self._hat.pulsecond[self.port].wait() if self._release: self.coast() - self.runmode = Motor.runmodes['None'] + self.runmode = MotorRunmode.NONE def run_for_seconds(self, seconds, speed=None, blocking=True): """Runs motor for N seconds @@ -221,7 +221,7 @@ def run_for_seconds(self, seconds, speed=None, blocking=True): :param seconds: Time in seconds :param speed: Speed ranging from -100 to 100 """ - self.runmode = Motor.runmodes['Seconds'] + self.runmode = MotorRunmode.SECONDS if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -238,11 +238,11 @@ def start(self, speed=None): :param speed: Speed ranging from -100 to 100 """ - if self.runmode == Motor.runmodes['Free']: + if self.runmode == MotorRunmode.FREE: if self.current_freerun_speed == speed: # Already running at this speed, do nothing return - elif self.runmode != Motor.runmodes['None']: + elif self.runmode != MotorRunmode.NONE: # Motor is running some other mode, wait for it to stop or stop() it yourself return @@ -252,16 +252,16 @@ def start(self, speed=None): if not (speed >= -100 and speed <= 100): raise MotorException("Invalid Speed") cmd = "port {} ; set {}\r".format(self.port, speed) - if self.runmode == Motor.runmodes['None']: + if self.runmode == MotorRunmode.NONE: cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set {}\r".format(self.port, self.port, speed) - self.runmode = Motor.runmodes['Free'] + self.runmode = MotorRunmode.FREE self.current_freerun_speed = speed self._write(cmd) def stop(self): """Stops motor""" - self.runmode = Motor.runmodes['None'] + self.runmode = MotorRunmode.NONE self.current_freerun_speed = 0 self.coast() From 85d708ed32b23acfb17d6559d067a7c1cc7909a0 Mon Sep 17 00:00:00 2001 From: mutesplash <49622611+mutesplash@users.noreply.github.com> Date: Thu, 17 Mar 2022 12:37:09 -0400 Subject: [PATCH 4/4] Back out _run_positional_ramp --- buildhat/motors.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/buildhat/motors.py b/buildhat/motors.py index 0bbc467..54de6e8 100644 --- a/buildhat/motors.py +++ b/buildhat/motors.py @@ -126,7 +126,15 @@ def _run_for_degrees(self, degrees, speed): pos /= 360.0 speed *= 0.05 dur = abs((newpos - pos) / speed) - self._run_positional_ramp(pos, newpos, dur) + cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port, + self.port, pos, newpos, dur) + self._write(cmd) + with self._hat.rampcond[self.port]: + self._hat.rampcond[self.port].wait() + if self._release: + time.sleep(0.2) + self.coast() + self.runmode = MotorRunmode.NONE def _run_to_position(self, degrees, speed, direction): self.runmode = MotorRunmode.DEGREES @@ -152,9 +160,6 @@ def _run_to_position(self, degrees, speed, direction): pos /= 360.0 speed *= 0.05 dur = abs((newpos - pos) / speed) - self._run_positional_ramp(pos, newpos, dur) - - def _run_positional_ramp(self, pos, newpos, dur): cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port, self.port, pos, newpos, dur) self._write(cmd)