diff --git a/buildhat/motors.py b/buildhat/motors.py index f3a996c..54de6e8 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): @@ -57,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 @@ -67,6 +81,7 @@ class Motor(Device): 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 +90,7 @@ def __init__(self, port): self._cvqueue = Condition() self.when_rotated = None self._oldpos = None + self.runmode = MotorRunmode.NONE def set_default_speed(self, default_speed): """Sets the default speed of the motor @@ -91,6 +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 = MotorRunmode.DEGREES if speed is None: self.run_for_degrees(int(rotations * 360), self.default_speed, blocking) else: @@ -99,6 +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 = MotorRunmode.DEGREES mul = 1 if speed < 0: speed = abs(speed) @@ -116,8 +134,10 @@ def _run_for_degrees(self, degrees, speed): 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 data = self.get() pos = data[1] apos = data[2] @@ -148,6 +168,7 @@ def _run_to_position(self, degrees, speed, direction): if self._release: time.sleep(0.2) self.coast() + self.runmode = MotorRunmode.NONE def run_for_degrees(self, degrees, speed=None, blocking=True): """Runs motor for N degrees @@ -157,6 +178,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 = MotorRunmode.DEGREES if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -174,6 +196,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 = MotorRunmode.DEGREES if speed is None: speed = self.default_speed if not (speed >= 0 and speed <= 100): @@ -188,12 +211,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 = 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 = MotorRunmode.NONE def run_for_seconds(self, seconds, speed=None, blocking=True): """Runs motor for N seconds @@ -201,6 +226,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 = MotorRunmode.SECONDS if speed is None: speed = self.default_speed if not (speed >= -100 and speed <= 100): @@ -217,16 +243,31 @@ def start(self, speed=None): :param speed: Speed ranging from -100 to 100 """ + if self.runmode == MotorRunmode.FREE: + if self.current_freerun_speed == speed: + # Already running at this speed, do nothing + return + elif self.runmode != MotorRunmode.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 == 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 = MotorRunmode.FREE + self.current_freerun_speed = speed self._write(cmd) + def stop(self): """Stops motor""" + self.runmode = MotorRunmode.NONE + self.current_freerun_speed = 0 self.coast() def get_position(self):