diff --git a/coderbot/api.py b/coderbot/api.py index be2e9c78..f2d7d8ba 100644 --- a/coderbot/api.py +++ b/coderbot/api.py @@ -114,6 +114,7 @@ def move(body): distance=body.get("distance") if (speed is None or speed == 0) or (elapse is not None and distance is not None): return 400 + speed = max(-100, min(100, speed)) bot.move(speed=speed, elapse=elapse, distance=distance) return 200 @@ -123,6 +124,7 @@ def turn(body): distance=body.get("distance") if (speed is None or speed == 0) or (elapse is not None and distance is not None): return 400 + speed = max(-100, min(100, speed)) bot.turn(speed=speed, elapse=elapse, distance=distance) return 200 @@ -201,9 +203,10 @@ def listPhotos(): def getPhoto(name): mimetype = {'jpg': 'image/jpeg', 'mp4': 'video/mp4'} + name = os.path.basename(name) try: media_file = cam.get_photo_file(name) - return send_file(media_file, mimetype=mimetype.get(name[:-3], 'image/jpeg'), max_age=0) + return send_file(media_file, mimetype=mimetype.get(name[-3:], 'image/jpeg'), max_age=0) except picamera.exc.PiCameraError as e: logging.error("Error: %s", str(e)) return 503 @@ -211,6 +214,7 @@ def getPhoto(name): return 404 def savePhoto(name, body): + name = os.path.basename(name) try: cam.update_photo({"name": name, "tag": body.get("tag")}) except FileNotFoundError: @@ -218,6 +222,7 @@ def savePhoto(name, body): def deletePhoto(name): logging.debug("photo delete") + name = os.path.basename(name) try: cam.delete_photo(name) except FileNotFoundError: @@ -235,10 +240,10 @@ def saveSettings(body): return 200 def updateFromPackage(): - os.system('sudo bash /home/pi/clean-update.sh') + subprocess.run(['sudo', 'bash', '/home/pi/clean-update.sh'], check=False) file_to_upload = connexion.request.files['file_to_upload'] file_to_upload.save(os.path.join('/home/pi/', 'update.tar')) - os.system('sudo reboot') + subprocess.run(['sudo', 'reboot'], check=False) return 200 def listMusicPackages(): diff --git a/coderbot/audio.py b/coderbot/audio.py index 42a4eeca..f9d61fd4 100644 --- a/coderbot/audio.py +++ b/coderbot/audio.py @@ -21,6 +21,7 @@ from array import array import time import logging +import subprocess import wave import audioop import pyaudio @@ -64,8 +65,16 @@ def exit(self): def say(self, what, locale='en'): if what and "$" in what: self.play(what[1:] + SOUNDEXT) - elif what and what: - os.system('espeak --stdout -v' + locale + ' -p 90 -a 200 -s 150 -g 10 "' + what + '" 2>>/dev/null | aplay -q') + elif what: + try: + espeak = subprocess.Popen( + ['espeak', '--stdout', '-v', locale, '-p', '90', '-a', '200', '-s', '150', '-g', '10', what], + stdout=subprocess.PIPE, stderr=subprocess.DEVNULL + ) + subprocess.run(['aplay', '-q'], stdin=espeak.stdout, stderr=subprocess.DEVNULL) + espeak.wait() + except FileNotFoundError: + logging.warning("espeak or aplay not found") def normalize(self, snd_data): "Average the volume out" diff --git a/coderbot/camera.py b/coderbot/camera.py index 8866b9fd..8e4bcf7f 100644 --- a/coderbot/camera.py +++ b/coderbot/camera.py @@ -127,9 +127,8 @@ def load_photo_metadata(self): self.save_photo_metadata() def save_photo_metadata(self): - f = open(PHOTO_METADATA_FILE, "wt") - json.dump(self._photos, f) - f.close() + with open(PHOTO_METADATA_FILE, "wt") as f: + json.dump(self._photos, f) def update_photo(self, photo): for p in self._photos: @@ -152,17 +151,14 @@ def photo_take(self): photo_index = self.get_next_photo_index() filename = PHOTO_PREFIX + str(photo_index) + self._camera.PHOTO_FILE_EXT filename_thumb = PHOTO_PREFIX + str(photo_index) + PHOTO_THUMB_SUFFIX + self._camera.PHOTO_FILE_EXT - of = open(PHOTO_PATH + "/" + filename, "wb+") - oft = open(PHOTO_PATH + "/" + filename_thumb, "wb+") im_str = self.get_image_jpeg() - of.write(im_str) - # thumb - im_pil = PILImage.open(BytesIO(im_str)) - im_pil.resize(PHOTO_THUMB_SIZE).save(oft) + with open(PHOTO_PATH + "/" + filename, "wb+") as of: + of.write(im_str) + with open(PHOTO_PATH + "/" + filename_thumb, "wb+") as oft: + im_pil = PILImage.open(BytesIO(im_str)) + im_pil.resize(PHOTO_THUMB_SIZE).save(oft) self._photos.append({"name":filename}) self.save_photo_metadata() - of.close() - oft.close() def is_recording(self): return self.recording @@ -186,15 +182,14 @@ def video_rec(self, video_name=None): except Exception: pass - oft = open(PHOTO_PATH + "/" + filename_thumb, "wb") - im_str = self._camera.get_image_jpeg() - im_pil = PILImage.open(BytesIO(im_str)) - im_pil.resize(PHOTO_THUMB_SIZE).save(oft) + with open(PHOTO_PATH + "/" + filename_thumb, "wb") as oft: + im_str = self._camera.get_image_jpeg() + im_pil = PILImage.open(BytesIO(im_str)) + im_pil.resize(PHOTO_THUMB_SIZE).save(oft) self._photos.append({"name":filename}) self.save_photo_metadata() self._camera.video_rec(PHOTO_PATH + "/" + filename) self.video_start_time = time.time() - oft.close() def video_stop(self): if self.recording: diff --git a/coderbot/coderbot.py b/coderbot/coderbot.py index 89c0de1c..b1ebf088 100644 --- a/coderbot/coderbot.py +++ b/coderbot/coderbot.py @@ -17,8 +17,6 @@ # 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. ############################################################################ -import os -import sys import time from math import copysign import logging @@ -102,11 +100,12 @@ class CoderBot(object): # pylint: disable=too-many-instance-attributes def __init__(self, motor_trim_factor=1.0, motor_min_power=0, motor_max_power=100, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)): + self._mpu = None try: self._mpu = mpu.AccelGyroMag() logging.info("MPU available") - except: - logging.info("MPU not available") + except Exception: + logging.warning("MPU not available") self.GPIOS = HW_VERSIONS.get(hw_version, GPIO_CODERBOT_V_5()) self._pin_out = [self.GPIOS.PIN_LEFT_FORWARD, self.GPIOS.PIN_RIGHT_FORWARD, self.GPIOS.PIN_LEFT_BACKWARD, self.GPIOS.PIN_RIGHT_BACKWARD, self.GPIOS.PIN_SERVO_1, self.GPIOS.PIN_SERVO_2] @@ -178,11 +177,20 @@ def turn(self, speed=100, elapse=None, distance=None): self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance) def turn_angle(self, speed=100, angle=0): - z = self._mpu.get_gyro()[2] + if self._mpu is None: + logging.warning("MPU not available, cannot turn by angle") + return + accumulated = 0.0 self.turn(speed, elapse=0) - while abs(z - self._mpu.get_gyro()[2]) < angle: + t = time.time() + while abs(accumulated) < angle: + now = time.time() + dt = now - t + t = now + gyro_z = self._mpu.get_gyro()[2] + accumulated += gyro_z * dt + logging.info("turn_angle: accumulated=%.2f target=%s", accumulated, angle) time.sleep(0.05) - logging.info(self._mpu.get_gyro()[2]) self.stop() def forward(self, speed=100, elapse=None, distance=None): @@ -204,6 +212,8 @@ def get_sonar_distance(self, sonar_id=0): return self.sonar[sonar_id].get_distance() def get_mpu_accel(self, axis=None): + if self._mpu is None: + return None acc = self._mpu.get_acc() if axis is None: return acc @@ -211,17 +221,23 @@ def get_mpu_accel(self, axis=None): return int(acc[axis]*100.0)/100.0 def get_mpu_gyro(self, axis=None): + if self._mpu is None: + return None gyro = self._mpu.get_gyro() if axis is None: return gyro else: - return int(gyro[axis]*100.0)/200.0 + return int(gyro[axis]*100.0)/100.0 def get_mpu_heading(self): + if self._mpu is None: + return None hdg = self._mpu.get_hdg() return int(hdg) def get_mpu_temp(self): + if self._mpu is None: + return None temp = self._mpu.get_temp() return int(temp*100.0)/100.0 @@ -242,7 +258,7 @@ def stop(self): self._twin_motors_enc.stop() def is_moving(self): - return self._twin_motors_enc._is_moving + return self._twin_motors_enc.is_moving() # Distance travelled getter def distance(self): @@ -254,7 +270,7 @@ def speed(self): # CoderBot direction getter def direction(self): - return self._twin_motors_enc.speed() + return self._twin_motors_enc.direction() def set_callback(self, gpio, callback, elapse): self._cb_elapse[gpio] = elapse * 1000 diff --git a/coderbot/config.py b/coderbot/config.py index 071dd7da..d0c3e878 100644 --- a/coderbot/config.py +++ b/coderbot/config.py @@ -37,14 +37,13 @@ def read(cls): cls.restore() with open(CONFIG_FILE, 'r') as f: cls._config = json.load(f) - f.close() return cls._config @classmethod def write(cls, config): cls._config = config - f = open(CONFIG_FILE, 'w') - json.dump(cls._config, f) + with open(CONFIG_FILE, 'w') as f: + json.dump(cls._config, f) return cls._config @classmethod diff --git a/coderbot/cv/camera.py b/coderbot/cv/camera.py index f2b1b68f..ae4dbaba 100644 --- a/coderbot/cv/camera.py +++ b/coderbot/cv/camera.py @@ -122,8 +122,8 @@ def get_image_bgr(self): def set_overlay_text(self, text): try: self.camera.annotate_text = text - except picamera.PiCameraValueError: - logging.info("PiCameraValueError") + except Exception: + logging.info("Unable to set overlay text") def close(self): self.camera.close() diff --git a/coderbot/cv/image.py b/coderbot/cv/image.py index 3c602379..6f2b587a 100644 --- a/coderbot/cv/image.py +++ b/coderbot/cv/image.py @@ -29,8 +29,8 @@ try: from pyzbar.pyzbar import decode -except: - logging.info("zbar not availabe") +except Exception: + logging.info("zbar not available") class Image(): r_from = np.float32([[0, 0], [640, 0], [640, 480], [0, 480]]) @@ -123,7 +123,7 @@ def grayscale(self): return Image(data) def blackwhite(self): - data = cv2.threshold(self._data, 128, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) + _, data = cv2.threshold(self._data, 128, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) return Image(data) def invert(self): diff --git a/coderbot/main.py b/coderbot/main.py index 36383240..80648c9a 100644 --- a/coderbot/main.py +++ b/coderbot/main.py @@ -95,15 +95,16 @@ def run_server(): CNNManager.get_instance() EventManager.get_instance("coderbot") - if app.bot_config.get('load_at_start') and app.bot_config.get('load_at_start'): + if app.bot_config.get('load_at_start'): prog = app.prog_engine.load(app.bot_config.get('load_at_start')) prog.execute() + + bot.set_callback(bot.GPIOS.PIN_PUSHBUTTON, button_pushed, 100) + except ValueError as e: app.bot_config = {} logging.error(e) - bot.set_callback(bot.GPIOS.PIN_PUSHBUTTON, button_pushed, 100) - remove_doreset_file() app.run(host="0.0.0.0", port=5000) diff --git a/coderbot/motion.py b/coderbot/motion.py index 95d66563..29a325d7 100644 --- a/coderbot/motion.py +++ b/coderbot/motion.py @@ -170,10 +170,9 @@ def track_keypoints(self, prev_image, cur_image, tracks): #print "initial tp: ", len(self.tracks), " current tp: ", len(new_tracks) #print len(new_tracks), len(tracks) tracks[:] = new_tracks[:] - if tracks is False: + if not tracks: logging.warning("lost ALL tp!") self.bot.stop() - #exit(0) #cv2.polylines(self.vis, [np.int32(tr) for tr in self.tracks], False, (0, 255, 0)) def calc_motion(self): diff --git a/coderbot/music.py b/coderbot/music.py index 0544b468..ba028e78 100644 --- a/coderbot/music.py +++ b/coderbot/music.py @@ -29,6 +29,7 @@ import os import sox import time +import logging class Music: _instance = None @@ -52,7 +53,7 @@ def __init__(self,managerPackage): #os.putenv('AUDIODRIVER', 'alsa') #os.putenv('AUDIODEV', 'hw:1,0') self.managerPackage = managerPackage - print("We have create a class: MUSICAL") + logging.info("Music class initialized") def test(self): tfm = sox.Transformer() @@ -71,7 +72,7 @@ def play_pause(self, duration): # @para alteration: if it is a diesis or a bemolle # @param time: duration of the note in seconds def play_note(self, note, instrument='piano', alteration='none', duration=1.0): - print(note) + logging.debug("play_note: %s", note) tfm = sox.Transformer() duration = float(duration) @@ -85,7 +86,7 @@ def play_note(self, note, instrument='piano', alteration='none', duration=1.0): if note in self.noteDict : shift = self.noteDict[note]+ alt else: - print('note not exist') + logging.warning('note does not exist: %s', note) return tfm.pitch(shift, quick=False) @@ -93,7 +94,7 @@ def play_note(self, note, instrument='piano', alteration='none', duration=1.0): if self.managerPackage.isPackageAvailable(instrument): tfm.preview('./sounds/notes/' + instrument + '/audio.wav') else: - print("no instrument:"+str(instrument)+" present in this coderbot!") + logging.warning("no instrument: %s present in this coderbot!", instrument) def play_animal(self, instrument, note='G2', alteration='none', duration=1.0): tfm = sox.Transformer() @@ -106,47 +107,17 @@ def play_animal(self, instrument, note='G2', alteration='none', duration=1.0): elif alteration == 'diesis': alt = 1.0 - if note == 'C2': - shift = -7.0 + alt - elif note == 'D2': - shift = -5.0 + alt - elif note == 'E2': - shift = -3.0 + alt - elif note == 'F2': - shift = -2.0 + alt - elif note == 'F#2': - shift = -1.0 + alt - elif note == 'G2': - shift = 0.0 + alt - elif note == 'A2': - shift = 2.0 + alt - elif note == 'Bb2': - shift = 3.0 + alt - elif note == 'B2': - shift = 4.0 + alt - elif note == 'C3': - shift = 5.0 + alt - elif note == 'D3': - shift = 7.0 + alt - elif note == 'E3': - shift = 9.0 + alt - elif note == 'F3': - shift = 10.0 + alt - elif note == 'G3': - shift = 12.0 + alt - - if note in self.noteDict : - shift = self.noteDict[note]+ alt + if note in self.noteDict: + shift = self.noteDict[note] + alt else: - print('note not exist') + logging.warning('note does not exist: %s', note) return if self.managerPackage.isPackageAvailable(instrument): tfm.preview('./sounds/notes/' + instrument + '/audio.wav') else: - print("no animal verse:"+str(instrument)+" present in this coderbot!") + logging.warning("no animal verse: %s present in this coderbot!", instrument) return tfm.pitch(shift, quick=False) tfm.trim(0.0, end_time=0.5*duration) - #tfm.stretch(time, window=20) tfm.preview('./sounds/notes/' + instrument + '/audio.wav') diff --git a/coderbot/musicPackages.py b/coderbot/musicPackages.py index e671b7e7..44715421 100644 --- a/coderbot/musicPackages.py +++ b/coderbot/musicPackages.py @@ -30,6 +30,8 @@ import os import logging import copy +import shutil +import subprocess class MusicPackage: @@ -150,7 +152,7 @@ def deletePackage(self, packageName): return 2 if os.path.exists('./sounds/notes/' + packageName): - os.system('rm -rf ./sounds/notes/' + packageName) + shutil.rmtree('./sounds/notes/' + packageName) return 1 @@ -189,9 +191,9 @@ def addPackage(self, filename): logging.info("errore, il pacchetto " + pkgname + " ha versione precendente a quello attualmente installato") raise ValueError() else: - os.system('unzip -o ' + '/tmp/' + filename + " -d /tmp") - os.system('mkdir ' + pkgpath) - os.system('mv /tmp/' + pkgname + "/" + 'audio.wav ' + pkgpath + '/') + subprocess.run(['unzip', '-o', '/tmp/' + filename, '-d', '/tmp'], check=True) + os.makedirs(pkgpath, exist_ok=True) + shutil.move('/tmp/' + pkgname + '/audio.wav', pkgpath + '/audio.wav') with open('/tmp/' + pkgname + '/' + pkgname + '.json') as json_file: logging.info("adding " + pkgname + " package") @@ -208,7 +210,7 @@ def addPackage(self, filename): self.updatePackages() - os.system('rm -rf /tmp/' + pkgname) + shutil.rmtree('/tmp/' + pkgname, ignore_errors=True) def isPackageAvailable(self,namePackage): diff --git a/coderbot/program.py b/coderbot/program.py index bc66fc3e..e158d0a1 100644 --- a/coderbot/program.py +++ b/coderbot/program.py @@ -82,8 +82,7 @@ def __init__(self): # initialise DB from default programs query = Query() self.lock = Lock() - for dirname, dirnames, filenames, in os.walk(PROGRAMS_PATH_DEFAULTS): - dirnames + for dirname, _dirnames, filenames in os.walk(PROGRAMS_PATH_DEFAULTS): for filename in filenames: if PROGRAM_PREFIX in filename: program_name = filename[len(PROGRAM_PREFIX):-len(PROGRAM_SUFFIX)] @@ -154,7 +153,6 @@ def get_current_program(self): return self._program class Program: - _running = False @property def dom_code(self): @@ -162,6 +160,7 @@ def dom_code(self): def __init__(self, name, code=None, dom_code=None, default=False): self._thread = None + self._running = False self.name = name self._dom_code = dom_code self._code = code @@ -213,9 +212,37 @@ def run(self, *args): self._log = "" #clear log imports = "import json\n" code = imports + self._code - env = globals() + # Restricted namespace: only expose robot control functions, + # not the full global scope (prevents access to os, sys, etc.) + safe_env = { + '__builtins__': { + 'range': range, 'len': len, 'int': int, 'float': float, + 'str': str, 'bool': bool, 'list': list, 'dict': dict, + 'tuple': tuple, 'set': set, 'abs': abs, 'min': min, + 'max': max, 'round': round, 'print': print, + 'True': True, 'False': False, 'None': None, + 'isinstance': isinstance, 'type': type, + 'enumerate': enumerate, 'zip': zip, 'map': map, + 'filter': filter, 'sorted': sorted, 'reversed': reversed, + 'sum': sum, 'any': any, 'all': all, + 'ValueError': ValueError, 'TypeError': TypeError, + 'RuntimeError': RuntimeError, 'Exception': Exception, + 'KeyError': KeyError, 'IndexError': IndexError, + '__import__': lambda name, *args, **kwargs: + __import__(name) if name in ('json', 'math', 'time', 'random') else + (_ for _ in ()).throw(ImportError(f"import of '{name}' is not allowed")), + }, + 'get_cam': get_cam, + 'get_bot': get_bot, + 'get_motion': get_motion, + 'get_audio': get_audio, + 'get_prog_eng': get_prog_eng, + 'get_event': get_event, + 'get_music': get_music, + 'get_atmega': get_atmega, + } logging.debug("** start code **\n"+str(code)+ "\n** end code **") - exec(code, env, env) + exec(code, safe_env, safe_env) except RuntimeError as re: logging.info("quit: %s", str(re)) get_prog_eng().log(str(re)) diff --git a/coderbot/rotary_encoder/motorencoder.py b/coderbot/rotary_encoder/motorencoder.py index 2b2ad82a..6517969b 100644 --- a/coderbot/rotary_encoder/motorencoder.py +++ b/coderbot/rotary_encoder/motorencoder.py @@ -1,5 +1,7 @@ import pigpio import threading +import logging +from collections import deque from time import sleep, time from rotary_encoder.rotarydecoder import RotaryDecoder @@ -14,6 +16,19 @@ class MotorEncoder: Every movement method must acquire lock in order not to have concurrency problems on GPIO READ/WRITE """ + # Encoder geometry constants + # Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolutions) + # Encoder ratio: 16 ticks per motor revolution + # 1 wheel revolution = 120 * 16 = 1920 ticks (single channel) + # Both channels, EITHER_EDGE: 1920 * 2 = 3840 callbacks per revolution + # R = 32.5mm → circumference = 2πR = 204.2mm + # 3840 ticks = 204.2mm → 1 tick = 0.053mm + DISTANCE_PER_TICK = 0.053 # mm per encoder callback + + # Speed calculation: sliding window size (ticks) + # Smaller = more responsive but noisier; larger = smoother but laggier + SPEED_WINDOW_SIZE = 20 + # default constructor def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, feedback_pin_B): # setting pin variables @@ -25,18 +40,16 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe self._feedback_pin_B = feedback_pin_B # setting movement variables - self._direction = 0 - self._distance_per_tick = 0.06 #(mm) - self._ticks = 0 + self._direction = 0 # commanded direction: 1=forward, -1=backward, 0=stopped + self._encoder_direction = 0 # direction detected by quadrature encoder + self._ticks = 0 # total ticks (unsigned, for speed/distance magnitude) + self._signed_ticks = 0 # signed ticks (for directional distance) self._power = 0 - self._encoder_speed = 0 + self._encoder_speed = 0.0 self._is_moving = False - # quadrature encoder variables - self._start_timer = 0 - self._current_timer = 0 - self._ticks_threshold = 100 - self._ticks_counter = 0 + # sliding window for speed calculation + self._tick_history = deque(maxlen=self.SPEED_WINDOW_SIZE + 1) # other self._encoder_lock = threading.RLock() @@ -47,16 +60,19 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe def ticks(self): return self._ticks - # distance + # distance (unsigned magnitude) def distance(self): - #return self._distance - return self._ticks * self._distance_per_tick + return self._ticks * self.DISTANCE_PER_TICK - # direction + # signed distance (positive=forward, negative=backward) + def signed_distance(self): + return self._signed_ticks * self.DISTANCE_PER_TICK + + # direction from encoder feedback def direction(self): - return self._direction + return self._encoder_direction - # speed + # speed (always positive, magnitude only) def speed(self): return self._encoder_speed @@ -65,29 +81,27 @@ def is_moving(self): return self._is_moving # MOVEMENT - """ The stop function acquires the lock to operate on motor - then writes a 0 on movement pins to stop the motor - and releases the lock afterwards + """ The control function sets PWM to drive the motor at the given power. Motor speed on range 0 - 100 already set on PWM_set_range(100) if a time_elapse parameter value is provided, motion is locked for a certain amount of time """ def control(self, power=100.0, time_elapse=0): - # resetting distance and ticks before new movement - self._distance = 0 # resetting distance travelled - self._ticks = 0 # resetting ticks + # resetting ticks before new movement + self._ticks = 0 + self._signed_ticks = 0 self._direction = 1 if power > 0 else -1 # setting direction according to speed - self._power = abs(power) # setting current power + self._power = abs(power) # setting current power if self._enable_pin is not None: self._pi.write(self._enable_pin, True) # enabling motors # going forward - if (self._direction == 1): + if self._direction == 1: self._pi.write(self._backward_pin, 0) self._pi.set_PWM_dutycycle(self._forward_pin, self._power) - # going bacakward + # going backward else: self._pi.write(self._forward_pin, 0) self._pi.set_PWM_dutycycle(self._backward_pin, self._power) @@ -95,13 +109,11 @@ def control(self, power=100.0, time_elapse=0): self._is_moving = True # movement time elapse - if (time_elapse > 0): + if time_elapse > 0: sleep(time_elapse) self.stop() - """ The stop function acquires the lock to operate on motor - then writes a 0 on movement pins to stop the motor - and releases the lock afterwards """ + """ The stop function writes a 0 on movement pins to stop the motor """ def stop(self): # stopping motor @@ -114,69 +126,58 @@ def stop(self): # stop auxiliary function, resets wheel state def reset_state(self): # returning state variables to consistent state - # after stopping, values of distance and ticks remains until + # after stopping, values of distance and ticks remain until # next movement - self._ticks = 0 # resetting ticks - self._power = 0 # resetting PWM power - self._encoder_speed = 0 # resetting encoder speed - self._direction = 0 # resetting direction - self._start_timer = 0 - self._current_timer = 0 - self._ticks_counter = 0 - self._is_moving = False # resetting moving flag + self._ticks = 0 + self._signed_ticks = 0 + self._power = 0 + self._encoder_speed = 0.0 + self._direction = 0 + self._encoder_direction = 0 + self._tick_history.clear() + self._is_moving = False # adjust power for velocity control loop def adjust_power(self, power): - self._power = power # setting current power + self._power = abs(power) # setting current power # adjusting power forward - if (self._direction == 1): - self._pi.set_PWM_dutycycle(self._forward_pin, abs(power)) - # adjusting power bacakward + if self._direction == 1: + self._pi.set_PWM_dutycycle(self._forward_pin, self._power) + # adjusting power backward else: - self._pi.set_PWM_dutycycle(self._backward_pin, abs(power)) + self._pi.set_PWM_dutycycle(self._backward_pin, self._power) # CALLBACK """ The callback function rotary_callback is called on EITHER_EDGE by the - rotary_decoder with a parameter value of 1 (1 new tick) - - Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolution) - - Encoder ratio: 16:1 encoder ticks for 1 motor revolution - - 1 wheel revolution = 120 * 16 = 1920 ticks - - R = 32.5mm - - 1 wheel revolution = 2πR = 2 * π * 32.5mm = 204.2mm - - 3840 ticks = 204.2mm - - 1 tick = 0.053mm - - 1 tick : 0.053mm = x : 1000mm -> x = 18867 ticks approximately - So 0.053 is the ticks->distance(mm) conversion coefficient - The callback function calculates current velocity by taking groups of - ticks_threshold ticks""" - # callback function - def rotary_callback(self, tick): - self._encoder_lock.acquire() - - # taking groups of n ticks each - if (self._ticks_counter == 0): - self._start_timer = tick # clock started - elif (abs(self._ticks_counter) == self._ticks_threshold): - self._current_timer = tick - elapse = (self._current_timer - self._start_timer) / 1000000.0 # calculating time elapse - # calculating current speed - self._encoder_speed = self._ticks_threshold * self._distance_per_tick / elapse # (mm/s) - - self._ticks += 1 # updating ticks - - if(abs(self._ticks_counter) < self._ticks_threshold): - self._ticks_counter += 1 - else: - self._start_timer = tick # clock started - self._ticks_counter = 0 - - # updating ticks counter using module - # 0, 1, 2, ... 8, 9, 10, 0, 1, 2, ... - # not ideal, module on ticks counter not precise, may miss an interrupt - #self._ticks_counter += 1 % (self._ticks_threshold + 1) - - self._encoder_lock.release() # releasing lock + rotary_decoder with direction (+1/-1) and tick (pigpio timestamp). + + Speed is calculated using a sliding window of recent ticks for + responsive, continuous updates. Uses pigpio.tickDiff() to handle + the 32-bit microsecond counter wraparound (~72 min). """ + + def rotary_callback(self, direction, tick): + with self._encoder_lock: + # update direction from quadrature decoder + self._encoder_direction = direction + + # update tick counts + self._ticks += 1 + self._signed_ticks += direction + + # sliding window speed calculation + self._tick_history.append(tick) + + if len(self._tick_history) >= self.SPEED_WINDOW_SIZE + 1: + # we have enough ticks for a speed measurement + oldest_tick = self._tick_history[0] + elapsed_us = pigpio.tickDiff(oldest_tick, tick) + if elapsed_us > 0: + self._encoder_speed = ( + self.SPEED_WINDOW_SIZE * self.DISTANCE_PER_TICK + / (elapsed_us / 1000000.0) + ) # mm/s + # else: not enough ticks yet, keep speed at last known value # callback cancelling def cancel_callback(self): diff --git a/coderbot/rotary_encoder/rotarydecoder.py b/coderbot/rotary_encoder/rotarydecoder.py index 99fd8766..d5980705 100644 --- a/coderbot/rotary_encoder/rotarydecoder.py +++ b/coderbot/rotary_encoder/rotarydecoder.py @@ -3,21 +3,25 @@ import pigpio import threading + class RotaryDecoder: - """ Class to decode mechanical rotary encoder pulses """ + """ Class to decode mechanical rotary encoder pulses. + + Uses quadrature decoding to determine both tick count and + rotation direction from two phase-shifted encoder channels. """ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback): self._pi = pi self._feedback_pin_A = feedback_pin_A # encoder feedback pin A self._feedback_pin_B = feedback_pin_B # encoder feedback pin B - self._callback = callback # callback function on event + self._callback = callback # callback function: callback(direction, tick) self._direction = 0 # direction, forward = 1, backward = -1, steady = 0 self._levelA = 0 # value of encoder feedback pin A self._levelB = 0 # value of encoder feedback pin B - + self._lock = threading.RLock() # setting up GPIO @@ -27,8 +31,9 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback): self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP) # callback function on EITHER_EDGE for each pin - self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulseA) - self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulseA) + # Uses full quadrature decoding (_pulse) for direction detection + self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse) + self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse) self._lastGpio = None @@ -40,8 +45,8 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback): Note: level = 0 falling edge 1 raising edge 2 from watchdog - - +---------+ +---------+ 0 + + +---------+ +---------+ 0 | | | | B | | | | | | | | @@ -50,9 +55,9 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback): | | | | A | | | | | | | | - ----+ +---------+ +----------+ 1 - - + ----+ +---------+ +----------+ 1 + + +---------+ +---------+ 0 | | | | A | | | | @@ -67,39 +72,34 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback): def _pulse(self, gpio, level, tick): self._lock.acquire() # interrupt comes from pin A - if (gpio == self._feedback_pin_A): + if gpio == self._feedback_pin_A: self._levelA = level # set level of squared wave (0, 1) on A # interrupt comes from pin B else: self._levelB = level # set level of squared wave (0, 1) on B - if (gpio != self._lastGpio): # debounce + if gpio != self._lastGpio: # debounce self._lastGpio = gpio # forward (A leading B) - if (gpio == self._feedback_pin_A and - ((level == 1 and self._levelB == 0) or - (level == 0 and self._levelB == 1))): - self._direction = 1 # forward + if (gpio == self._feedback_pin_A and + ((level == 1 and self._levelB == 0) or + (level == 0 and self._levelB == 1))): + self._direction = 1 # forward # backward (B leading A) - elif (gpio == self._feedback_pin_B and - ((level == 1 and self._levelA == 0) or - (level == 0 and self._levelA == 1))): - self._direction = -1 # forward - + elif (gpio == self._feedback_pin_B and + ((level == 1 and self._levelA == 0) or + (level == 0 and self._levelA == 1))): + self._direction = -1 # backward + direction = self._direction self._lock.release() - self._callback(direction) - - def _pulseA(self, gpio, level, tick): - self._callback(tick) + # Pass both direction and tick to the callback + self._callback(direction, tick) def cancel(self): - """ Cancel the rotary encoder decoder callbacks. """ self._callback_triggerA.cancel() self._callback_triggerB.cancel() - - diff --git a/coderbot/rotary_encoder/wheelsaxel.py b/coderbot/rotary_encoder/wheelsaxel.py index abfef001..22061a84 100644 --- a/coderbot/rotary_encoder/wheelsaxel.py +++ b/coderbot/rotary_encoder/wheelsaxel.py @@ -14,6 +14,17 @@ class WheelsAxel: It also tries to handle the inconsistent tension on wheels that makes one wheel go slower than the other """ + # Maximum integral error accumulation (prevents windup) + MAX_INTEGRAL = 5.0 + + # Cross-coupling gain for straight-line correction. + # Penalizes speed difference between wheels to keep the robot straight. + PID_KP_DIFF = 0.3 + + # Maximum power headroom factor. + # PID can increase power up to this factor above the requested power. + POWER_HEADROOM = 1.3 + def __init__(self, pi, enable_pin, left_forward_pin, left_backward_pin, left_encoder_feedback_pin_A, left_encoder_feedback_pin_B, right_forward_pin, right_backward_pin, right_encoder_feedback_pin_A, right_encoder_feedback_pin_B, @@ -43,16 +54,13 @@ def __init__(self, pi, enable_pin, self.pid_max_speed = pid_params[3] self.pid_sample_time = pid_params[4] - # other - #self._wheelsAxle_lock = threading.RLock() # race condition lock - # STATE GETTERS """ Distance and speed are calculated by a mean of the feedback from the two motors """ def is_moving(self): return self._left_motor.is_moving() or self._right_motor.is_moving() - + # distance def distance(self): l_dist = self._left_motor.distance() @@ -69,22 +77,22 @@ def speed(self): def direction(self): l_dir = self._left_motor.direction() r_dir = self._right_motor.direction() - if(l_dir == r_dir): + if l_dir == r_dir: return l_dir else: return 0 # MOVEMENT - """ Movement wrapper method + """ Movement wrapper method if time is specified and distance is not, control_time is called if distance is specified and time is not, control_distance is called if both distance and time are specified, control_velocity is called """ def control(self, power_left=100, power_right=100, time_elapse=None, target_distance=None): - if(time_elapse is not None and target_distance is None): # time + if time_elapse is not None and target_distance is None: # time self.control_time(power_left, power_right, time_elapse) - elif(time_elapse is None and target_distance is not None): # distance + elif time_elapse is None and target_distance is not None: # distance self.control_distance(power_left, power_right, target_distance) - else: # velocity + else: # velocity self.control_velocity(time_elapse, target_distance) """ Motor time control allows the motors @@ -93,188 +101,146 @@ def control_time(self, power_left=100, power_right=100, time_elapse=-1): if time_elapse > 0: return self.control_time_encoder(power_left, power_right, time_elapse) - # applying tension to motors + # applying tension to motors (open-loop, no PID — used for + # continuous/manual control with time_elapse=-1) self._left_motor.control(power_left, -1) self._right_motor.control(power_right, -1) self._is_moving = True - # moving for desired time - # fixed for direct control that uses time_elapse -1 and stops manually - if(time_elapse > 0): - sleep(time_elapse) - self.stop() - """ Motor time control allows the motors - to run for a certain amount of time """ + to run for a certain amount of time with PID """ def control_time_encoder(self, power_left=100, power_right=100, time_elapse=-1): - #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire - self._is_moving = True - - # get desired direction from power, then normalize on power > 0 - left_direction = power_left/abs(power_left) - right_direction = power_right/abs(power_right) - power_left = abs(power_left) - power_right = abs(power_right) - - self._left_motor.reset_state() - self._right_motor.reset_state() - - # applying tension to motors - self._left_motor.control(power_left * left_direction) - self._right_motor.control(power_right * right_direction) - - #PID parameters - # assuming that power_right is equal to power_left and that coderbot - # moves at 11.5mm/s at full PWM duty cycle - - target_speed_left = (self.pid_max_speed / 100) * power_left #velocity [mm/s] - target_speed_right = (self.pid_max_speed / 100) * power_right # velocity [mm/s] - - left_derivative_error = 0 - right_derivative_error = 0 - left_integral_error = 0 - right_integral_error = 0 - left_prev_error = 0 - right_prev_error = 0 time_init = time() - - # moving for certaing amount of distance - logging.info("moving? " + str(self._is_moving) + " distance: " + str(self.distance()) + " target: " + str(time_elapse)) - while(time() - time_init < time_elapse and self._is_moving == True): - # PI controller - logging.debug("speed.left: " + str(self._left_motor.speed()) + " speed.right: " + str(self._right_motor.speed())) - if(abs(self._left_motor.speed()) > 10 and abs(self._right_motor.speed()) > 10): - # relative error - left_error = float(target_speed_left - self._left_motor.speed()) / target_speed_left - right_error = float(target_speed_right - self._right_motor.speed()) / target_speed_right - - left_correction = (left_error * self.pid_kp) + (left_derivative_error * self.pid_kd) + (left_integral_error * self.pid_ki) - right_correction = (right_error * self.pid_kp) + (right_derivative_error * self.pid_kd) + (right_integral_error * self.pid_ki) - - corrected_power_left = power_left + (left_correction * power_left) - corrected_power_right = power_right + (right_correction * power_right) - - #print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI)) - #print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI)) - - # conrispondent new power - power_left_norm = max(min(corrected_power_left, power_left), 0) - power_right_norm = max(min(corrected_power_right, power_right), 0) - - logging.debug("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) + - " le:" + str(left_error) + " re: " + str(right_error) + - " ld:" + str(left_derivative_error) + " rd: " + str(right_derivative_error) + - " li:" + str(left_integral_error) + " ri: " + str(right_integral_error) + - " lc: " + str(int(left_correction)) + " rc: " + str(int(right_correction)) + - " lp: " + str(int(power_left_norm)) + " rp: " + str(int(power_right_norm))) - - # adjusting power on each motors - self._left_motor.adjust_power(power_left_norm * left_direction ) - self._right_motor.adjust_power(power_right_norm * right_direction) - - left_derivative_error = (left_error - left_prev_error) / self.pid_sample_time - right_derivative_error = (right_error - right_prev_error) / self.pid_sample_time - left_integral_error += (left_error * self.pid_sample_time) - right_integral_error += (right_error * self.pid_sample_time) - - left_prev_error = left_error - right_prev_error = right_error - - # checking each SAMPLETIME seconds - sleep(self.pid_sample_time) - - logging.info("control_distance.stop, target elapse: " + str(time_elapse) + - " actual distance: " + str(self.distance()) + - " l ticks: " + str(self._left_motor.ticks()) + - " r ticks: " + str(self._right_motor.ticks())) - # robot arrived - self.stop() + condition = lambda: time() - time_init < time_elapse + self._pid_loop(power_left, power_right, condition, "elapse", time_elapse) """ Motor distance control allows the motors to run for a certain amount of distance (mm) """ def control_distance(self, power_left=100, power_right=100, target_distance=0): - #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire + condition = lambda: abs(self.distance()) < abs(target_distance) + self._pid_loop(power_left, power_right, condition, "dist", target_distance) + + def _pid_loop(self, power_left, power_right, continue_condition, label, target_value): + """Shared PID control loop used by both time and distance control modes. + + Features: + - Per-wheel PID speed regulation (proportional + integral + derivative) + - Cross-coupling term that penalizes speed difference between wheels + for straight-line driving correction + - Integral windup protection (clamped accumulation) + - Power headroom: PID can increase power above requested level + to compensate for friction/load + + Args: + power_left: power for left motor (-100 to 100) + power_right: power for right motor (-100 to 100) + continue_condition: callable returning True while loop should run + label: label for logging ("elapse" or "dist") + target_value: target value for logging + """ + # Guard against zero power (would cause ZeroDivisionError) + if power_left == 0 or power_right == 0: + logging.warning("_pid_loop called with zero power, skipping") + return + self._is_moving = True - # get desired direction from power, then normalize on power > 0 - left_direction = power_left/abs(power_left) - right_direction = power_right/abs(power_right) + # get desired direction from power, then normalize on power > 0 + left_direction = power_left / abs(power_left) + right_direction = power_right / abs(power_right) power_left = abs(power_left) power_right = abs(power_right) - self._left_motor.reset_state() - self._right_motor.reset_state() - # applying tension to motors self._left_motor.control(power_left * left_direction) self._right_motor.control(power_right * right_direction) - #PID parameters - # assuming that power_right is equal to power_left and that coderbot - # moves at 11.5mm/s at full PWM duty cycle - - target_speed_left = (self.pid_max_speed / 100) * power_left #velocity [mm/s] - target_speed_right = (self.pid_max_speed / 100) * power_right # velocity [mm/s] - - # SOFT RESPONSE - # KP = 0.04 #proportional coefficient - # KD = 0.02 # derivative coefficient - # KI = 0.005 # integral coefficient - - # MEDIUM RESPONSE - # KP = 0.9 # proportional coefficient - # KD = 0.1 # derivative coefficient - # KI = 0.05 # integral coefficient - - # STRONG RESPONSE - # KP = 0.9 # proportional coefficient - # KD = 0.05 # derivative coefficient - # KI = 0.03 # integral coefficient - - left_derivative_error = 0 - right_derivative_error = 0 - left_integral_error = 0 - right_integral_error = 0 - left_prev_error = 0 - right_prev_error = 0 - # moving for certaing amount of distance - logging.info("moving? " + str(self._is_moving) + " distance: " + str(self.distance()) + " target: " + str(target_distance)) - while(abs(self.distance()) < abs(target_distance) and self._is_moving == True): - # PI controller - logging.debug("speed.left: " + str(self._left_motor.speed()) + " speed.right: " + str(self._right_motor.speed())) - if(abs(self._left_motor.speed()) > 10 and abs(self._right_motor.speed()) > 10): - # relative error - left_error = float(target_speed_left - self._left_motor.speed()) / target_speed_left - right_error = float(target_speed_right - self._right_motor.speed()) / target_speed_right - - left_correction = (left_error * self.pid_kp) + (left_derivative_error * self.pid_kd) + (left_integral_error * self.pid_ki) - right_correction = (right_error * self.pid_kp) + (right_derivative_error * self.pid_kd) + (right_integral_error * self.pid_ki) - + # PID target speeds (proportional to requested power) + target_speed_left = (self.pid_max_speed / 100) * power_left # mm/s + target_speed_right = (self.pid_max_speed / 100) * power_right # mm/s + + # Determine if this is straight-line mode (same target for both wheels) + straight_line = (target_speed_left == target_speed_right) + + # PID state variables + left_derivative_error = 0.0 + right_derivative_error = 0.0 + left_integral_error = 0.0 + right_integral_error = 0.0 + left_prev_error = 0.0 + right_prev_error = 0.0 + + # Power limits: allow PID to increase power above requested + max_power_left = min(100, power_left * self.POWER_HEADROOM) + max_power_right = min(100, power_right * self.POWER_HEADROOM) + + logging.info("moving? %s distance: %s target %s: %s", + self._is_moving, self.distance(), label, target_value) + + while continue_condition() and self._is_moving: + left_speed = self._left_motor.speed() + right_speed = self._right_motor.speed() + + logging.debug("speed.left: %s speed.right: %s", left_speed, right_speed) + + if left_speed > 10 and right_speed > 10: + # --- Per-wheel PID error (relative) --- + left_error = (target_speed_left - left_speed) / target_speed_left + right_error = (target_speed_right - right_speed) / target_speed_right + + # --- Per-wheel PID correction --- + left_correction = ( + left_error * self.pid_kp + + left_derivative_error * self.pid_kd + + left_integral_error * self.pid_ki + ) + right_correction = ( + right_error * self.pid_kp + + right_derivative_error * self.pid_kd + + right_integral_error * self.pid_ki + ) + + # --- Cross-coupling for straight-line correction --- + # If both wheels should go the same speed, penalize + # any difference between them + if straight_line and target_speed_left > 0: + speed_diff_error = (left_speed - right_speed) / target_speed_left + left_correction -= speed_diff_error * self.PID_KP_DIFF + right_correction += speed_diff_error * self.PID_KP_DIFF + + # --- Apply corrections to power --- corrected_power_left = power_left + (left_correction * power_left) - corrected_power_right = power_right + (right_correction * power_right) - - #print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI)) - #print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI)) - - # conrispondent new power - power_left_norm = max(min(corrected_power_left, power_left), 0) - power_right_norm = max(min(corrected_power_right, power_right), 0) - - logging.debug("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) + - " le:" + str(left_error) + " re: " + str(right_error) + - " ld:" + str(left_derivative_error) + " rd: " + str(right_derivative_error) + - " li:" + str(left_integral_error) + " ri: " + str(right_integral_error) + - " lc: " + str(int(left_correction)) + " rc: " + str(int(right_correction)) + - " lp: " + str(int(power_left_norm)) + " rp: " + str(int(power_right_norm))) - - # adjusting power on each motors - self._left_motor.adjust_power(power_left_norm * left_direction ) + corrected_power_right = power_right + (right_correction * power_right) + + # Clamp to [0, max_power] — allows PID to INCREASE power + power_left_norm = max(min(corrected_power_left, max_power_left), 0) + power_right_norm = max(min(corrected_power_right, max_power_right), 0) + + logging.debug( + "ls:%d rs:%d le:%.3f re:%.3f ld:%.3f rd:%.3f " + "li:%.3f ri:%.3f lc:%.3f rc:%.3f lp:%d rp:%d", + int(left_speed), int(right_speed), + left_error, right_error, + left_derivative_error, right_derivative_error, + left_integral_error, right_integral_error, + left_correction, right_correction, + int(power_left_norm), int(power_right_norm)) + + # adjusting power on each motor + self._left_motor.adjust_power(power_left_norm * left_direction) self._right_motor.adjust_power(power_right_norm * right_direction) + # --- Update derivative term --- left_derivative_error = (left_error - left_prev_error) / self.pid_sample_time right_derivative_error = (right_error - right_prev_error) / self.pid_sample_time - left_integral_error += (left_error * self.pid_sample_time) - right_integral_error += (right_error * self.pid_sample_time) + + # --- Update integral term with windup protection --- + left_integral_error += left_error * self.pid_sample_time + right_integral_error += right_error * self.pid_sample_time + left_integral_error = max(-self.MAX_INTEGRAL, + min(self.MAX_INTEGRAL, left_integral_error)) + right_integral_error = max(-self.MAX_INTEGRAL, + min(self.MAX_INTEGRAL, right_integral_error)) left_prev_error = left_error right_prev_error = right_error @@ -282,33 +248,25 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0): # checking each SAMPLETIME seconds sleep(self.pid_sample_time) - logging.info("control_distance.stop, target dist: " + str(target_distance) + - " actual distance: " + str(self.distance()) + - " l ticks: " + str(self._left_motor.ticks()) + - " r ticks: " + str(self._right_motor.ticks())) + logging.info("control.stop, target %s: %s actual distance: %s l ticks: %s r ticks: %s", + label, target_value, self.distance(), + self._left_motor.ticks(), self._right_motor.ticks()) # robot arrived self.stop() """ Motor speed control to travel given distance - in given time adjusting power on motors + in given time adjusting power on motors NOT very intuitive, idea has been postponed""" def control_velocity(self, time_elapse=0, target_distance=0): pass """ The stop function calls the two stop functions of the two - correspondent motors. - Locks are automatically obtained """ + correspondent motors. """ def stop(self): # stopping left and right motors self._left_motor.stop() self._right_motor.stop() - # trying to fix distance different than zero after - # wheels has stopped by re-resetting state after 0.5s - - #self._left_motor.reset_state() - #self._right_motor.reset_state() - # updating state logging.info("stopping") self._is_moving = False diff --git a/coderbot/runtime_test.py b/coderbot/runtime_test.py index 4a6260a5..3ebfb129 100644 --- a/coderbot/runtime_test.py +++ b/coderbot/runtime_test.py @@ -91,7 +91,7 @@ def __test_encoder(): assert (c.speed() == 0) return 1 - except: + except Exception: return -1 # sonar diff --git a/coderbot/sonar.py b/coderbot/sonar.py index e54f7a19..88969871 100644 --- a/coderbot/sonar.py +++ b/coderbot/sonar.py @@ -39,8 +39,8 @@ def __init__(self, pi, trigger, echo): pi.set_mode(self._trig, pigpio.OUTPUT) pi.set_mode(self._echo, pigpio.INPUT) - self._cb = pi.callback(self._trig, pigpio.EITHER_EDGE, self._cbf) - self._cb = pi.callback(self._echo, pigpio.EITHER_EDGE, self._cbf) + self._cb_trig = pi.callback(self._trig, pigpio.EITHER_EDGE, self._cbf) + self._cb_echo = pi.callback(self._echo, pigpio.EITHER_EDGE, self._cbf) self._inited = True @@ -79,7 +79,6 @@ def read(self): return None def get_distance(self): - time.sleep(0.05) return round(self.read() / self.MICROSECONDS * self.SOUND_SPEED / 2, 1) # seconds / microseconds * sound speed / 2 (half trip) def cancel(self): @@ -89,6 +88,7 @@ def cancel(self): """ if self._inited: self._inited = False - self._cb.cancel() + self._cb_trig.cancel() + self._cb_echo.cancel() self.pi.set_mode(self._trig, self._trig_mode) self.pi.set_mode(self._echo, self._echo_mode)