diff --git a/build.sh b/build.sh index 84ba6e3..657ddf0 100644 --- a/build.sh +++ b/build.sh @@ -4,7 +4,7 @@ mkdir output DEB="empty" -cd client_s2 +cd client source build_deb.sh echo "Ran build deb, created: $DEB" mv $DEB ../output diff --git a/client_s2/.gitignore b/client/.gitignore similarity index 100% rename from client_s2/.gitignore rename to client/.gitignore diff --git a/client_s2/.idea/.gitignore b/client/.idea/.gitignore similarity index 100% rename from client_s2/.idea/.gitignore rename to client/.idea/.gitignore diff --git a/client_s2/.idea/client_s2.iml b/client/.idea/client_s2.iml similarity index 100% rename from client_s2/.idea/client_s2.iml rename to client/.idea/client_s2.iml diff --git a/client_s2/.idea/inspectionProfiles/Project_Default.xml b/client/.idea/inspectionProfiles/Project_Default.xml similarity index 100% rename from client_s2/.idea/inspectionProfiles/Project_Default.xml rename to client/.idea/inspectionProfiles/Project_Default.xml diff --git a/client_s2/.idea/inspectionProfiles/profiles_settings.xml b/client/.idea/inspectionProfiles/profiles_settings.xml similarity index 100% rename from client_s2/.idea/inspectionProfiles/profiles_settings.xml rename to client/.idea/inspectionProfiles/profiles_settings.xml diff --git a/client_s2/.idea/misc.xml b/client/.idea/misc.xml similarity index 100% rename from client_s2/.idea/misc.xml rename to client/.idea/misc.xml diff --git a/client_s2/.idea/modules.xml b/client/.idea/modules.xml similarity index 100% rename from client_s2/.idea/modules.xml rename to client/.idea/modules.xml diff --git a/client_s2/.idea/saveactions_settings.xml b/client/.idea/saveactions_settings.xml similarity index 100% rename from client_s2/.idea/saveactions_settings.xml rename to client/.idea/saveactions_settings.xml diff --git a/client_s2/.idea/vcs.xml b/client/.idea/vcs.xml similarity index 100% rename from client_s2/.idea/vcs.xml rename to client/.idea/vcs.xml diff --git a/client_s1/MANIFEST.in b/client/MANIFEST.in similarity index 100% rename from client_s1/MANIFEST.in rename to client/MANIFEST.in diff --git a/client_s2/build_deb.sh b/client/build_deb.sh similarity index 100% rename from client_s2/build_deb.sh rename to client/build_deb.sh diff --git a/client_s2/compLib/.gitignore b/client/compLib/.gitignore similarity index 100% rename from client_s2/compLib/.gitignore rename to client/compLib/.gitignore diff --git a/client_s2/compLib/Api.py b/client/compLib/Api.py similarity index 100% rename from client_s2/compLib/Api.py rename to client/compLib/Api.py diff --git a/client_s2/compLib/CompLib.proto b/client/compLib/CompLib.proto similarity index 100% rename from client_s2/compLib/CompLib.proto rename to client/compLib/CompLib.proto diff --git a/client_s2/compLib/CompLibClient.py b/client/compLib/CompLibClient.py similarity index 100% rename from client_s2/compLib/CompLibClient.py rename to client/compLib/CompLibClient.py diff --git a/client_s2/compLib/CompLib_pb2.py b/client/compLib/CompLib_pb2.py similarity index 100% rename from client_s2/compLib/CompLib_pb2.py rename to client/compLib/CompLib_pb2.py diff --git a/client_s2/compLib/DoubleElimination.py b/client/compLib/DoubleElimination.py similarity index 100% rename from client_s2/compLib/DoubleElimination.py rename to client/compLib/DoubleElimination.py diff --git a/client_s2/compLib/Encoder.py b/client/compLib/Encoder.py similarity index 100% rename from client_s2/compLib/Encoder.py rename to client/compLib/Encoder.py diff --git a/client_s2/compLib/HealthCheck.py b/client/compLib/HealthCheck.py similarity index 100% rename from client_s2/compLib/HealthCheck.py rename to client/compLib/HealthCheck.py diff --git a/client_s2/compLib/IRSensor.py b/client/compLib/IRSensor.py similarity index 100% rename from client_s2/compLib/IRSensor.py rename to client/compLib/IRSensor.py diff --git a/client_s2/compLib/Motor.py b/client/compLib/Motor.py similarity index 100% rename from client_s2/compLib/Motor.py rename to client/compLib/Motor.py diff --git a/client_s2/compLib/Movement.py b/client/compLib/Movement.py similarity index 100% rename from client_s2/compLib/Movement.py rename to client/compLib/Movement.py diff --git a/client_s2/compLib/Seeding.py b/client/compLib/Seeding.py similarity index 100% rename from client_s2/compLib/Seeding.py rename to client/compLib/Seeding.py diff --git a/client_s2/compLib/__init__.py b/client/compLib/__init__.py similarity index 100% rename from client_s2/compLib/__init__.py rename to client/compLib/__init__.py diff --git a/client_s2/dev01.py b/client/dev01.py similarity index 100% rename from client_s2/dev01.py rename to client/dev01.py diff --git a/client_s2/dev03.py b/client/dev03.py similarity index 100% rename from client_s2/dev03.py rename to client/dev03.py diff --git a/client_s2/docs/.gitignore b/client/docs/.gitignore similarity index 100% rename from client_s2/docs/.gitignore rename to client/docs/.gitignore diff --git a/client_s1/docs/Makefile b/client/docs/Makefile similarity index 100% rename from client_s1/docs/Makefile rename to client/docs/Makefile diff --git a/client_s1/docs/make.bat b/client/docs/make.bat similarity index 100% rename from client_s1/docs/make.bat rename to client/docs/make.bat diff --git a/client_s1/docs/source/_static/.gitkeep b/client/docs/source/_static/.gitkeep similarity index 100% rename from client_s1/docs/source/_static/.gitkeep rename to client/docs/source/_static/.gitkeep diff --git a/client_s2/docs/source/conf.py b/client/docs/source/conf.py similarity index 100% rename from client_s2/docs/source/conf.py rename to client/docs/source/conf.py diff --git a/client_s1/docs/source/images/compair-logo-white.svg b/client/docs/source/images/compair-logo-white.svg similarity index 100% rename from client_s1/docs/source/images/compair-logo-white.svg rename to client/docs/source/images/compair-logo-white.svg diff --git a/client_s2/docs/source/index.rst b/client/docs/source/index.rst similarity index 100% rename from client_s2/docs/source/index.rst rename to client/docs/source/index.rst diff --git a/client_s2/docs/source/lib/classes/Motor.rst b/client/docs/source/lib/classes/Motor.rst similarity index 100% rename from client_s2/docs/source/lib/classes/Motor.rst rename to client/docs/source/lib/classes/Motor.rst diff --git a/client_s1/docs/source/lib/images/chessboard.jpg b/client/docs/source/lib/classes/images/chessboard.jpg similarity index 100% rename from client_s1/docs/source/lib/images/chessboard.jpg rename to client/docs/source/lib/classes/images/chessboard.jpg diff --git a/client_s1/docs/source/lib/images/chessboard_detected.jpg b/client/docs/source/lib/classes/images/chessboard_detected.jpg similarity index 100% rename from client_s1/docs/source/lib/images/chessboard_detected.jpg rename to client/docs/source/lib/classes/images/chessboard_detected.jpg diff --git a/client_s1/docs/source/lib/images/opencv_http_stream.png b/client/docs/source/lib/classes/images/opencv_http_stream.png similarity index 100% rename from client_s1/docs/source/lib/images/opencv_http_stream.png rename to client/docs/source/lib/classes/images/opencv_http_stream.png diff --git a/client_s1/docs/source/lib/images/opencv_processed.png b/client/docs/source/lib/classes/images/opencv_processed.png similarity index 100% rename from client_s1/docs/source/lib/images/opencv_processed.png rename to client/docs/source/lib/classes/images/opencv_processed.png diff --git a/client_s2/docs/source/lib/index.rst b/client/docs/source/lib/index.rst similarity index 100% rename from client_s2/docs/source/lib/index.rst rename to client/docs/source/lib/index.rst diff --git a/client_s2/docs/source/other/usage.rst b/client/docs/source/other/usage.rst similarity index 100% rename from client_s2/docs/source/other/usage.rst rename to client/docs/source/other/usage.rst diff --git a/client_s2/lf.py b/client/lf.py similarity index 100% rename from client_s2/lf.py rename to client/lf.py diff --git a/client_s2/main.py b/client/main.py similarity index 100% rename from client_s2/main.py rename to client/main.py diff --git a/client_s2/postinstall.sh b/client/postinstall.sh similarity index 100% rename from client_s2/postinstall.sh rename to client/postinstall.sh diff --git a/client_s2/setup.py b/client/setup.py similarity index 100% rename from client_s2/setup.py rename to client/setup.py diff --git a/client_s1/sphinx_to_github.sh b/client/sphinx_to_github.sh similarity index 100% rename from client_s1/sphinx_to_github.sh rename to client/sphinx_to_github.sh diff --git a/client_s2/test.py b/client/test.py similarity index 100% rename from client_s2/test.py rename to client/test.py diff --git a/client_s1/.gitignore b/client_s1/.gitignore deleted file mode 100644 index 8c9cdf5..0000000 --- a/client_s1/.gitignore +++ /dev/null @@ -1,252 +0,0 @@ -.idea -# Created by https://www.toptal.com/developers/gitignore/api/python,pycharm,code -# Edit at https://www.toptal.com/developers/gitignore?templates=python,pycharm,code - -### Code ### -.vscode/* -!.vscode/tasks.json -!.vscode/launch.json -*.code-workspace - -### PyCharm ### -# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider -# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 - -# User-specific stuff -.idea/**/workspace.xml -.idea/**/tasks.xml -.idea/**/usage.statistics.xml -.idea/**/dictionaries -.idea/**/shelf - -# Generated files -.idea/**/contentModel.xml - -# Sensitive or high-churn files -.idea/**/dataSources/ -.idea/**/dataSources.ids -.idea/**/dataSources.local.xml -.idea/**/sqlDataSources.xml -.idea/**/dynamic.xml -.idea/**/uiDesigner.xml -.idea/**/dbnavigator.xml - -# Gradle -.idea/**/gradle.xml -.idea/**/libraries - -# Gradle and Maven with auto-import -# When using Gradle or Maven with auto-import, you should exclude module files, -# since they will be recreated, and may cause churn. Uncomment if using -# auto-import. -# .idea/artifacts -# .idea/compiler.xml -# .idea/jarRepositories.xml -# .idea/modules.xml -# .idea/*.iml -# .idea/modules -# *.iml -# *.ipr - -# CMake -cmake-build-*/ - -# Mongo Explorer plugin -.idea/**/mongoSettings.xml - -# File-based project format -*.iws - -# IntelliJ -out/ - -# mpeltonen/sbt-idea plugin -.idea_modules/ - -# JIRA plugin -atlassian-ide-plugin.xml - -# Cursive Clojure plugin -.idea/replstate.xml - -# Crashlytics plugin (for Android Studio and IntelliJ) -com_crashlytics_export_strings.xml -crashlytics.properties -crashlytics-build.properties -fabric.properties - -# Editor-based Rest Client -.idea/httpRequests - -# Android studio 3.1+ serialized cache file -.idea/caches/build_file_checksums.ser - -### PyCharm Patch ### -# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 - -# *.iml -# modules.xml -# .idea/misc.xml -# *.ipr - -# Sonarlint plugin -# https://plugins.jetbrains.com/plugin/7973-sonarlint -.idea/**/sonarlint/ - -# SonarQube Plugin -# https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin -.idea/**/sonarIssues.xml - -# Markdown Navigator plugin -# https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced -.idea/**/markdown-navigator.xml -.idea/**/markdown-navigator-enh.xml -.idea/**/markdown-navigator/ - -# Cache file creation bug -# See https://youtrack.jetbrains.com/issue/JBR-2257 -.idea/$CACHE_FILE$ - -# CodeStream plugin -# https://plugins.jetbrains.com/plugin/12206-codestream -.idea/codestream.xml - -### Python ### -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] -*$py.class - -# C extensions -*.so - -# Distribution / packaging -.Python -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -parts/ -sdist/ -var/ -wheels/ -pip-wheel-metadata/ -share/python-wheels/ -*.egg-info/ -.installed.cfg -*.egg -MANIFEST - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.nox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -*.py,cover -.hypothesis/ -.pytest_cache/ -pytestdebug.log - -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py -db.sqlite3 -db.sqlite3-journal - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ -doc/_build/ - -# PyBuilder -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# IPython -profile_default/ -ipython_config.py - -# pyenv -.python-version - -# pipenv -# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. -# However, in case of collaboration, if having platform-specific dependencies or dependencies -# having no cross-platform support, pipenv may install dependencies that don't work, or not -# install all needed dependencies. -#Pipfile.lock - -# PEP 582; used by e.g. github.com/David-OConnor/pyflow -__pypackages__/ - -# Celery stuff -celerybeat-schedule -celerybeat.pid - -# SageMath parsed files -*.sage.py - -# Environments -.env -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ -pythonenv* - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ -.dmypy.json -dmypy.json - -# Pyre type checker -.pyre/ - -# pytype static type analyzer -.pytype/ - -# profiling data -.prof - -# End of https://www.toptal.com/developers/gitignore/api/python,pycharm,code diff --git a/client_s1/build_deb.sh b/client_s1/build_deb.sh deleted file mode 100755 index 7261b47..0000000 --- a/client_s1/build_deb.sh +++ /dev/null @@ -1,47 +0,0 @@ -##!/usr/bin/zsh -export PYTHONDONTWRITEBYTECODE=1 - -# be sure to change version if needed! -fpm -s python --python-bin python3 --python-pip pip3 --python-package-name-prefix python3 \ - -m '"Joel Klimont" ' \ - --license 'proprietary' \ - --description 'Library for robot used in the competition' \ - --after-install postinstall.sh \ - --after-upgrade postinstall.sh \ - --deb-generate-changes \ - --deb-priority "optional" \ - --deb-systemd "complib.service" \ - -d "python3-pip" \ - -d "nginx" \ - -d "libnginx-mod-rtmp" \ - -d "libsystemd-dev" \ - -d "python3-systemd" \ - -d "gstreamer1.0-tools" \ - -d "gstreamer1.0-plugins-bad" \ - -d "gstreamer1.0-plugins-base" \ - -d "gstreamer1.0-plugins-good" \ - -d "gstreamer1.0-omx-rpi" \ - -d "gstreamer1.0-omx-rpi-config" \ - -d "opencv-dev" \ - -d "opencv-libs" \ - -d "opencv-licenses" \ - -d "opencv-main" \ - -d "opencv-python" \ - -d "opencv-scripts" \ - -d "libatlas-base-dev" \ - -d "pigpio" \ - -d "python-pigpio" \ - -d "python3-pigpio" \ - -d "python3-numpy" \ - -d "ffmpeg" \ - -v 0.4.1-1 -t deb setup.py - -# --deb-changelog changelog \ -# --deb-upstream-changelog changelog \ -# --deb-field "Distribution: stable" \ -# --deb-dist "stable" \ - -#sudo apt purge python3-complib -y -#sudo apt install ./python3-* -#sudo apt search complib -#ar vx ./python3* \ No newline at end of file diff --git a/client_s1/changelog b/client_s1/changelog deleted file mode 100644 index a6c361a..0000000 --- a/client_s1/changelog +++ /dev/null @@ -1,5 +0,0 @@ -python3-complib (0.0.2-4) stable; urgency=low - - * Initial release. - - -- Joel Klimont Fri, 15 Jan 2021 23:14:01 +0100 \ No newline at end of file diff --git a/client_s1/compLib/Api.py b/client_s1/compLib/Api.py deleted file mode 100644 index 4c8cb99..0000000 --- a/client_s1/compLib/Api.py +++ /dev/null @@ -1,233 +0,0 @@ -import json -import os -import time -from typing import Dict, Tuple, List - -import requests - -from compLib.LogstashLogging import Logging - -API_URL = os.getenv("API_URL", "http://localhost:5000/") + "api/" - -api_override = os.getenv("API_FORCE", "") - -if api_override != "": - print(f"API_URL was set to {API_URL} but was overwritten with {api_override}") - API_URL = api_override - -API_URL_GET_DELIVERY = API_URL + "getDelivery" -API_URL_GET_MATERIAL = API_URL + "getMaterial" -API_URL_GET_GARBAGE = API_URL + "getGarbage" -API_URL_GET_LIST_CARGO = API_URL + "listCargo" -API_URL_GET_CARGO = API_URL + "getCargo/" -API_URL_GET_ROBOT_STATE = API_URL + "getRobotState" - -API_URL_GET_POS = API_URL + "getPos" -API_URL_GET_OP = API_URL + "getOp" -API_URL_GET_GOAL = API_URL + "getGoal" -API_URL_GET_ITEMS = API_URL + "getItems" -API_URL_GET_SCORES = API_URL + "getScores" -API_URL_GET_METEOROID = API_URL + "getMeteoroids" - - -class Seeding: - """Class used for communicating with seeding api - """ - - @staticmethod - def get_delivery() -> Tuple[Dict, int]: - """Makes the /api/getDelivery call to the api. - - :return: Json Object and status code as returned by the api. - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_DELIVERY) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.get_delivery = {result}, status code = {res.status_code}") - return result, res.status_code - - @staticmethod - def get_material() -> Tuple[Dict, int]: - """Makes the /api/getMaterial call to the api. - - :return: Json Object and status code as returned by the api. - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_MATERIAL) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.get_material = {result}, status code = {res.status_code}") - return result, res.status_code - - @staticmethod - def get_garbage() -> Tuple[Dict, int]: - """Makes the /api/getGarbage call to the api. - - :return: Json Object and status code as returned by the api. - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_GARBAGE) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.get_garbage {result}, status code = {res.status_code}") - return result, res.status_code - - @staticmethod - def list_cargo() -> Tuple[Dict, int]: - """Makes the /api/listCargo call to the api. - - :return: Json Object and status code as returned by the api. - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_LIST_CARGO) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.list_cargo {result}, status code = {res.status_code}") - return result, res.status_code - - @staticmethod - def get_cargo(color: str) -> Tuple[Dict, int]: - """Makes the /api/getCargo call to the api. - - :param color: Color parameter which specifies which cargo should be taken. (A string which is either "green", "red", "yellow" or "blue") The function only picks up one package. - :return: Json Object and status code as returned by the api. - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_CARGO + color) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.get_cargo {result}, status code = {res.status_code}") - return result, res.status_code - - @staticmethod - def get_robot_state() -> Tuple[Dict, int]: - res = requests.get(API_URL_GET_ROBOT_STATE) - result = json.loads(res.content) - Logging.get_logger().debug(f"Seeding.get_robot_state {result}, status code = {res.status_code}") - return result, res.status_code - - -class Position: - """Datastructure for holding a position - """ - - def __init__(self, x, y, degrees): - self.x = x - self.y = y - self.degrees = degrees - - def __repr__(self): - return f"Position(x={self.x}, y={self.y}, degrees={self.degrees})" - - def __str__(self): - return f"Position(x={round(self.x, 5)}, y={round(self.y, 5)}, degrees={round(self.degrees, 5)})" - - -class DoubleElim: - """Class used for communicating with double elimination api - """ - - @staticmethod - def get_position() -> Tuple[Position, int]: - """Makes the /api/getPos call to the api. - - :return: A Position object with robot position - :rtype: Tuple[Position, int] - """ - res = requests.get(API_URL_GET_POS) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_position timeout!") - time.sleep(0.01) - return DoubleElim.get_position() - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_position = {response}, status code = {res.status_code}") - return Position(response["x"], response["y"], response["degrees"]), res.status_code - - @staticmethod - def get_opponent() -> Tuple[Position, int]: - """Makes the /api/getOp call to the api. - - :return: A Position object with opponents robot position - :rtype: Tuple[Position, int] - """ - res = requests.get(API_URL_GET_OP) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_opponent timeout!") - time.sleep(0.01) - return DoubleElim.get_opponent() - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_opponent = x:{response}, status code = {res.status_code}") - return Position(response["x"], response["y"], response["degrees"]), res.status_code - - @staticmethod - def get_goal() -> Tuple[Position, int]: - """Makes the /api/getGoal call to the api. - - :return: A Position object with x and y coordinates of the goal, rotation is always -1 - :rtype: Tuple[Position, int] - """ - res = requests.get(API_URL_GET_GOAL) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_goal timeout!") - time.sleep(0.01) - return DoubleElim.get_goal() - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_goal = {response}, status code = {res.status_code}") - return Position(response["x"], response["y"], -1), res.status_code - - @staticmethod - def get_items() -> Tuple[List[Dict], int]: - """Makes the /api/getItems call to the api. - - :return: A list will all items currently on the game field. Items are dictionaries that look like: {"id": 0, "x": 0, "y": 0} - :rtype: Tuple[List[Dict], int] - """ - res = requests.get(API_URL_GET_ITEMS) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_items timeout!") - time.sleep(0.01) - return DoubleElim.get_items() - elif res.status_code == 503: - return [], 503 - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_items = {response}, status code = {res.status_code}") - return response, res.status_code - - @staticmethod - def get_scores() -> Tuple[Dict, int]: - """Makes the /api/getScores call to the api. - - :return: A dictionary with all scores included like: {"self":2,"opponent":0} - :rtype: Tuple[Dict, int] - """ - res = requests.get(API_URL_GET_SCORES) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_scores timeout!") - time.sleep(0.01) - return DoubleElim.get_scores() - elif res.status_code == 503: - return {"self": 0, "opponent": 0}, 503 - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_scores = {response}, status code = {res.status_code}") - return response, res.status_code - - @staticmethod - def get_meteoroids() -> Tuple[List[Dict], int]: - """Makes the /api/getMeteoroids call to the api. - - :return: A list will all meteoroids currently on the game field. Meteoroids are dictionaries that look like: {"x": 0, "y": 0} - :rtype: Tuple[List[Dict], int] - """ - res = requests.get(API_URL_GET_METEOROID) - if res.status_code == 408: - Logging.get_logger().error(f"DoubleElim.get_meteoroids timeout!") - time.sleep(0.01) - return DoubleElim.get_meteoroids() - elif res.status_code == 503: - return [], 503 - - response = json.loads(res.content) - Logging.get_logger().debug(f"DoubleElim.get_meteoroids = {response}, status code = {res.status_code}") - return response, res.status_code - diff --git a/client_s1/compLib/Display.py b/client_s1/compLib/Display.py deleted file mode 100644 index d85d3b8..0000000 --- a/client_s1/compLib/Display.py +++ /dev/null @@ -1,49 +0,0 @@ -import string - -from compLib.LogstashLogging import logstash_logger -from compLib.Spi import Spi, Register - -LINE_COUNT = 4 -CHARS_PER_LINE = 16 - - -class Display(object): - """Access the display on the robot. - The display is split into 4 Rows and 16 Columns. Each function call changes one line at a time. - """ - - @staticmethod - def write(line: int, text: str): - """Write a string of text to the integrated display. - - :param line: Line to write. Between 1 and 4 - :param text: Text to write. Up to 16 characters - :raises: IndexError - """ - if len(text) > CHARS_PER_LINE: - logstash_logger.error(f"Too many characters specified!") - return - - if line <= 0 or line > LINE_COUNT: - raise IndexError("Invalid line number specified") - - to_write = [0] * CHARS_PER_LINE - for i in range(len(text)): - to_write[i] = ord(text[i]) - - if line == 1: - Spi.write_array(Register.DISPLAY_LINE_1_C0, CHARS_PER_LINE, to_write) - elif line == 2: - Spi.write_array(Register.DISPLAY_LINE_2_C0, CHARS_PER_LINE, to_write) - elif line == 3: - Spi.write_array(Register.DISPLAY_LINE_3_C0, CHARS_PER_LINE, to_write) - elif line == 4: - Spi.write_array(Register.DISPLAY_LINE_4_C0, CHARS_PER_LINE, to_write) - - @staticmethod - def clear(): - """Clear the display - - """ - for i in range(1, LINE_COUNT + 1): - Display.write(i, "") diff --git a/client_s1/compLib/Encoder.py b/client_s1/compLib/Encoder.py deleted file mode 100644 index 8d9d4d3..0000000 --- a/client_s1/compLib/Encoder.py +++ /dev/null @@ -1,127 +0,0 @@ -import atexit -from enum import Enum - -from compLib.LogstashLogging import Logging -from compLib.Spi import Spi, Register - -MOTOR_COUNT = 4 - - -encoder_start_values = [0] * (MOTOR_COUNT + 1) - - -class Encoder(object): - """Class used to read the encoders - """ - - @staticmethod - def handle_wrap(raw_value, port): - """Handle overflow and underflow of int for encoders. - - :param raw_value: Raw value which was read on port - :param port: Port, which the motor is connected to. Between 1 and 4 - :raises: IndexError - :return: Current encoder position - """ - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid encoder port specified!") - - diff = raw_value - encoder_start_values[port] - if diff > 2 ** 31: - diff -= 2 ** 32 - elif diff < -2 ** 31: - diff += 2 ** 32 - - return diff - - @staticmethod - def read_raw(port: int) -> int: - """Read raw encoder from a specified port. Will not be reset to 0 at start. - - :param port: Port, which the motor is connected to. Between 1 and 4 - :raises: IndexError - :return: Current encoder position - """ - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid encoder port specified!") - - raw_value = 0 - - if port == 1: - raw_value = Spi.read(Register.MOTOR_1_POS_B3, 4) - elif port == 2: - raw_value = Spi.read(Register.MOTOR_2_POS_B3, 4) - elif port == 3: - raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4) - elif port == 4: - raw_value = Spi.read(Register.MOTOR_4_POS_B3, 4) - - return raw_value - - @staticmethod - def read_all_raw(): - """Read all encoders at once. - This is faster than read_raw as it only uses one SPI call for all encoders! - - :return: Tuple of all current raw encoder positions - """ - - encoders = Spi.read_array(Register.MOTOR_1_POS_B3, 4 * 4) - - encoder_1 = int.from_bytes( - encoders[0:4], byteorder='big', signed=False) - encoder_2 = int.from_bytes( - encoders[4:8], byteorder='big', signed=False) - encoder_3 = int.from_bytes( - encoders[8:12], byteorder='big', signed=False) - encoder_4 = int.from_bytes( - encoders[12:16], byteorder='big', signed=False) - - return (encoder_1, encoder_2, encoder_3, encoder_4) - - @staticmethod - def read_all(): - """Read all encoders at once. - This is faster than read as it only uses one SPI call for all encoders! - - :return: Tuple of all current encoder positions - """ - encoders = Encoder.read_all_raw() - - return (Encoder.handle_wrap(encoders[0], 1), - Encoder.handle_wrap(encoders[1], 2), - Encoder.handle_wrap(encoders[2], 3), - Encoder.handle_wrap(encoders[3], 4)) - - @staticmethod - def read(port: int) -> int: - """Read encoder from a specified port - - :param port: Port, which the motor is connected to. Between 1 and 4 - :raises: IndexError - :return: Current encoder position - """ - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid encoder port specified!") - - return Encoder.handle_wrap(Encoder.read_raw(port), port) - - @staticmethod - def clear(port: int): - """Reset encoder position to 0 - - :param port: Port, which the motor is connected to. Between 1 and 4 - :raises: IndexError - """ - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid encoder port specified!") - - encoder_start_values[port] = Encoder.read_raw(port) - - @staticmethod - def clear_all(): - """Reset all encoder positions to 0 - """ - - for i in range(1, MOTOR_COUNT + 1): - encoder_start_values[i] = Encoder.read_raw(i) diff --git a/client_s1/compLib/IPService.py b/client_s1/compLib/IPService.py deleted file mode 100644 index 1a97fff..0000000 --- a/client_s1/compLib/IPService.py +++ /dev/null @@ -1,116 +0,0 @@ -import datetime -import os -import socket -import threading -import time - -import systemd.daemon -from compLib.Lock import Lock - -try: - from compLib.LogstashLogging import Logging -except Exception as e: - import logging - - - class Logger(): - def __init__(self): - self.logger = logging.Logger('compApi background') - - def get_logger(self): - return self.logger - - Logging = Logger() - print(f"Could not import compLib.LogstashLogging: {str(e)}") - Logging.get_logger().error(f"Could not import compLib.LogstashLogging: {str(e)}") - -print("after basic imports") - -RUN_IP_CHECK = False -try: - from compLib.Display import Display - from compLib.Spi import Spi - from compLib import __version__ - RUN_IP_CHECK = True -except Exception as e: - print(f"Could not import display or spi for ip output {str(e)}") - Logging.get_logger().warning(f"Could not import display or spi for ip output {str(e)}") - -print(f"After display and Spi import") - -__run = """raspivid -t 0 -b 5000000 -w 1280 -h 720 -fps 30 -n -o - | gst-launch-1.0 fdsrc ! video/x-h264,width=1280,height=720,framerate=30/1,noise-reduction=1,profile=high,stream-format=byte-stream ! h264parse ! queue ! flvmux streamable=true ! rtmpsink location=\"rtmp://localhost/live/stream\"""" - -STREAM_RASPI = False if os.getenv("STREAM_RASPI", "false") == "false" else True -IP_OUTPUT = False if os.getenv("IP_OUTPUT", "true") != "true" else True - - -def get_ip(): - s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - s.connect(('10.255.255.255', 1)) - IP = s.getsockname()[0] - except Exception: - IP = 'Not connected' - print(f"Error could not query ip: {e}") - finally: - s.close() - return IP - - -def write_ip_to_screen(): - while os.getenv("IP_OUTPUT", "true") == "true": - try: - if not Lock.is_locked(): - Lock.lock() - ip = str(get_ip()) - print(f"writing {ip} to display") - Display.write(2, f"LIB: V{__version__}") - Display.write(3, f"FW: V{Spi.get_version()}") - Display.write(4, f"IP: {ip}") - Display.write(1, datetime.datetime.now().strftime("%b %d %H:%M:%S")) - Lock.unlock() - - time.sleep(10) - except Exception as e: - print(f"Exception in write ip thread: {e}") - time.sleep(5) - - -if __name__ == '__main__': - try: - systemd.daemon.notify(systemd.daemon.Notification.READY) - except: - Logging.get_logger().warning("Warning, old systemd version detected") - systemd.daemon.notify('READY=1') - - - ip_output = None - if RUN_IP_CHECK and IP_OUTPUT: - print("starting ip output") - Logging.get_logger().info("starting ip output") - Lock.unlock() - try: - Spi.disable_health_check() - ip_output = threading.Thread(target=write_ip_to_screen) - ip_output.start() - print("starting ip output - DONE") - Logging.get_logger().info("starting ip output - DONE") - except Exception as e: - print(f"could not start ip output -> {str(e)}") - Logging.get_logger().error(f"could not start ip output -> {str(e)}") - - if STREAM_RASPI: - print("starting gstreamer background process") - Logging.get_logger().info("starting gstreamer background process") - os.system(__run) - print("gstreamer stopped...") - Logging.get_logger().error("gstreamer stopped...") - else: - print("not starting gstreamer background process") - Logging.get_logger().info("not starting gstreamer background process") - if ip_output is not None: - ip_output.join() - else: - print("ip display output failed to initialize.. sleeping for a day, good night") - Logging.get_logger().info("ip display output failed to initialize.. sleeping for a day, good night") - time.sleep(60 * 60 * 24) diff --git a/client_s1/compLib/IRSensor.py b/client_s1/compLib/IRSensor.py deleted file mode 100644 index 3051124..0000000 --- a/client_s1/compLib/IRSensor.py +++ /dev/null @@ -1,81 +0,0 @@ -from compLib.LogstashLogging import Logging -from compLib.MetricsLogging import MetricsLogging -from compLib.Spi import Spi, Register - -SENSOR_COUNT = 5 - - -class IRSensor(object): - """Access the different IR Sensors of the robot - """ - - @staticmethod - def read(sensor: int) -> int: - """Read one infrared sensor - - :param sensor: Which sensor to read. Between 1 and 5 - :raises: IndexError - :return: Sensor value. 10 bit accuracy - :rtype: int - """ - if sensor <= 0 or sensor > SENSOR_COUNT: - raise IndexError("Invalid sensor specified!") - - result = 0 - if sensor == 1: - result = Spi.read(Register.IR_1_H, 2) - elif sensor == 2: - result = Spi.read(Register.IR_2_H, 2) - elif sensor == 3: - result = Spi.read(Register.IR_3_H, 2) - elif sensor == 4: - result = Spi.read(Register.IR_4_H, 2) - elif sensor == 5: - result = Spi.read(Register.IR_5_H, 2) - - MetricsLogging.put("Infrared", result, sensor) - return result - - @staticmethod - def read_all(): - """Read all IR sensors at once. - This is faster than read as it only uses one SPI call for all sensors! - - :return: Tuple of all current ir sensors - """ - - sensors = Spi.read_array(Register.IR_1_H, 5 * 2) - - sensor_1 = int.from_bytes( - sensors[0:2], byteorder='big', signed=False) - sensor_2 = int.from_bytes( - sensors[2:4], byteorder='big', signed=False) - sensor_3 = int.from_bytes( - sensors[4:6], byteorder='big', signed=False) - sensor_4 = int.from_bytes( - sensors[6:8], byteorder='big', signed=False) - sensor_5 = int.from_bytes( - sensors[8:10], byteorder='big', signed=False) - - return (sensor_1, sensor_2, sensor_3, sensor_4, sensor_5) - - @staticmethod - def set(sensor: int, on: bool): - """Turn on / off a IR emitter - - :param sensor: Which sensor to read. Between 1 and 5 - :raises: IndexError - """ - if sensor <= 0 or sensor > SENSOR_COUNT: - raise IndexError("Invalid sensor specified!") - - if sensor == 1: - Spi.write(Register.IR_1_LED, 1, on) - elif sensor == 2: - Spi.write(Register.IR_2_LED, 1, on) - elif sensor == 3: - Spi.write(Register.IR_3_LED, 1, on) - elif sensor == 4: - Spi.write(Register.IR_4_LED, 1, on) - elif sensor == 5: - Spi.write(Register.IR_5_LED, 1, on) diff --git a/client_s1/compLib/Lock.py b/client_s1/compLib/Lock.py deleted file mode 100644 index 2184b6b..0000000 --- a/client_s1/compLib/Lock.py +++ /dev/null @@ -1,25 +0,0 @@ -from filelock import FileLock, Timeout -import atexit - -FILELOCK_PATH = "/root/complib.lock" - -global_lock = FileLock(FILELOCK_PATH, timeout=1) -class Lock(object): - @staticmethod - def lock(): - global_lock.acquire() - - @staticmethod - def unlock(): - global_lock.release() - - @staticmethod - def is_locked(): - try: - global_lock.acquire() - global_lock.release() - return False - except Timeout: - return True - -atexit.register(Lock.unlock) \ No newline at end of file diff --git a/client_s1/compLib/LogstashLogging.py b/client_s1/compLib/LogstashLogging.py deleted file mode 100644 index b36196a..0000000 --- a/client_s1/compLib/LogstashLogging.py +++ /dev/null @@ -1,102 +0,0 @@ -import logging -import os -import sys -import requests - -from logstash_async.transport import HttpTransport -from logstash_async.handler import AsynchronousLogstashHandler - -EXTENSIVE_LOGGING = os.getenv("EXTENSIVE_LOGGING", "False") - -if EXTENSIVE_LOGGING == "True": - EXTENSIVE_LOGGING = True -else: - EXTENSIVE_LOGGING = False - -host = 'logstash.robo4you.at' -port = 443 - -logstash_logger = logging.getLogger('logstash') -logstash_logger.setLevel(logging.INFO) - -transport = HttpTransport( - host, - port, - username="robo", - password="competition", - timeout=60.0, -) - -asynchronousLogstashHandler = AsynchronousLogstashHandler( - host, - port, - transport=transport, - database_path='logs.db' -) - - -class StreamToLogger(object): - """ - Fake file-like stream object that redirects writes to a logger instance. - """ - - def __init__(self, textio, log_level=logging.INFO): - self.logger = logging.getLogger('logstash') - self.console = textio - self.log_level = log_level - self.linebuf = '' - - def write(self, buf): - self.console.write(buf) - temp_linebuf = self.linebuf + buf - for line in buf.splitlines(True): - self.linebuf += line - - def flush(self): - if self.linebuf != '': - self.logger.log(self.log_level, self.linebuf.rstrip()) - self.linebuf = '' - self.console.flush() - asynchronousLogstashHandler.flush() - - -if EXTENSIVE_LOGGING: - try: - r = requests.get(f"https://{host}:{port}") - if r.status_code == 401: - logstash_logger.addHandler(asynchronousLogstashHandler) - - so = StreamToLogger(sys.stdout, logging.INFO) - sys.stdout = so - - se = StreamToLogger(sys.stderr, logging.ERROR) - sys.stderr = se - else: - print(f"Could not connect to {host} -> ERROR CODE: {r.status_code}!") - - except requests.exceptions.ConnectionError as identifier: - print(f"Could not connect to {host}!") - print(f"Error loading logger was -> {identifier}") -else: - print("Extensive logging is disabled! No logs will be sent over network...") - - -class Logging(object): - """Can be used to manually use the logger or turn up the logging level used for debugging this library. - """ - - @staticmethod - def get_logger() -> logging.Logger: - """Get the logger object used to communicate with logstash - - :return: Python logger - :rtype: logging.Logger - """ - return logstash_logger - - @staticmethod - def set_debug(): - """Turns up the logging level of the library to debug. Should be used, when debugging with the - Competition organizers - """ - logstash_logger.setLevel(logging.DEBUG) diff --git a/client_s1/compLib/MetricsLogging.py b/client_s1/compLib/MetricsLogging.py deleted file mode 100644 index 4535af5..0000000 --- a/client_s1/compLib/MetricsLogging.py +++ /dev/null @@ -1,97 +0,0 @@ -from influxdb_client import InfluxDBClient, Point, WritePrecision -import socket -import uuid -import os -import multiprocessing -import datetime -import requests -import time - -CONCURRENCY = 1 - -HOSTNAME = socket.gethostname() -RUN_TRACE = str(uuid.uuid4()) - -TOKEN = "aCdxnntw-Zp3agci8Z32lQAR_ep6MhdmWOJG7ObnoikYqe7nKAhFYx1jVGBpipQuId79SC4Jl0J6IBYVqauJyw==" -ORG = "robo4you" -BUCKET = "compAIR" - -INFLUX_HOST = "https://influxdb.comp-air.at" - -EXTENSIVE_LOGGING = os.getenv("EXTENSIVE_LOGGING", "True") -if EXTENSIVE_LOGGING == "True": - EXTENSIVE_LOGGING = True -else: - EXTENSIVE_LOGGING = False - - -influx_client = InfluxDBClient(url=INFLUX_HOST, token=TOKEN) -write_api = influx_client.write_api() - -point_queue = multiprocessing.Queue() -workers = [] - -class MetricsLogging(): - """Used to send metrics / points to a influxdb - """ - - @staticmethod - def is_influx_reachable() -> bool: - """Check if we can send metrics to the database - - :return: Is reachable? - :rtype: bool - """ - try: - r = requests.get(INFLUX_HOST) - if r.status_code == 200: - return True - else: - print(f"Could not connect to {INFLUX_HOST} -> ERROR CODE: {r.status_code}!") - return False - except requests.exceptions.RequestException as identifier: - print(f"Could not connect to {INFLUX_HOST}!") - - @staticmethod - def put(sensorType, value, port): - """Put a datapoint into a time-series-database - - :param sensorType: A key used to identify a datapoint - :param value: Value measured by the sensor - :param port: Port of the sensor which was read - """ - if EXTENSIVE_LOGGING: - point = Point(sensorType) \ - .tag("host", HOSTNAME) \ - .tag("runID", RUN_TRACE) \ - .tag("port", port) \ - .field("value", value) \ - .time(datetime.datetime.utcnow(), WritePrecision.MS) - point_queue.put_nowait(point) - - @staticmethod - def worker(): - point = object() - for job in iter(point_queue.get, point): - try: - write_api.write(BUCKET, ORG, point) - time.sleep(0.1) - except Exception as e: - pass - finally: - pass - - @staticmethod - def start_workers(): - global EXTENSIVE_LOGGING - if EXTENSIVE_LOGGING: - if MetricsLogging.is_influx_reachable(): - for i in range(CONCURRENCY): - worker = multiprocessing.Process(target=MetricsLogging.worker, daemon=True) - worker.start() - workers.append(worker) - else: - EXTENSIVE_LOGGING = False - - -MetricsLogging.start_workers() diff --git a/client_s1/compLib/Motor.py b/client_s1/compLib/Motor.py deleted file mode 100644 index 51e0c02..0000000 --- a/client_s1/compLib/Motor.py +++ /dev/null @@ -1,226 +0,0 @@ -import atexit -from enum import IntEnum - -from compLib.LogstashLogging import Logging -from compLib.MetricsLogging import MetricsLogging -from compLib.Spi import Spi, Register - -MOTOR_COUNT = 4 -MAX_MOTOR_SPEED = 65535 -MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0 -NEW_MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.0] -MOTOR_CURVE = [0.0, 0.0, 426.5, 692.0, 842.5, 953.5, 1032.5, 1090.5, 1135.5, 1171.0, 1203.5, 1230.0, 1249.5, 1268.0, 1283.0, 1298.5, 1308.0, 1320.0, 1332.0, 1339.5, 1352.5] - -SPEED_LOCK = False -SPEED_MULT = 1.0 - - -class MotorMode(IntEnum): - COAST = 0, - FORWARD = 1, - BACKWARD = 2, - BREAK = 3 - - -class Motor(object): - """Class used to control the motors - """ - - @staticmethod - def pwm(port: int, pwm: int, mode: MotorMode): - """Set specified motor to a specific pwm value and motor mode - - :param port: Port, which the motor is connected to. 1-4 allowed - :param pwm: Raw PWM value which the motor should be set to. From 0 to 2^16 - 1 - :param mode: Motor mode. See enum MotorMode for more info - :raises: IndexError - """ - - if SPEED_LOCK: - return - - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid Motor port specified!") - - if port == 1: - Spi.write(Register.MOTOR_1_PWM_H, 2, pwm) - Spi.write(Register.PWM_1_CTRL, 1, int(mode)) - elif port == 2: - Spi.write(Register.MOTOR_2_PWM_H, 2, pwm) - Spi.write(Register.PWM_2_CTRL, 1, int(mode)) - elif port == 3: - Spi.write(Register.MOTOR_3_PWM_H, 2, pwm) - Spi.write(Register.PWM_3_CTRL, 1, int(mode)) - elif port == 4: - Spi.write(Register.MOTOR_4_PWM_H, 2, pwm) - Spi.write(Register.PWM_4_CTRL, 1, int(mode)) - - @staticmethod - def power_raw(port: int, percent: float, log_metric = True): - """Set specified motor to percentage power - - :param port: Port, which the motor is connected to. 1-4 - :param percent: Percentage of max speed. between -100 and 100 - :raises: IndexError - """ - - if port <= 0 or port > MOTOR_COUNT: - raise IndexError("Invalid Motor port specified!") - - if percent < -100 or percent > 100: - raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!") - - if log_metric: - MetricsLogging.put("MotorRaw", float(percent), port) - - mode = MotorMode.COAST - if percent < 0: - percent = abs(percent) - mode = MotorMode.BACKWARD - elif percent > 0: - mode = MotorMode.FORWARD - - percent *= SPEED_MULT - pwm = percent * MOTOR_PERCENTAGE_MULT - - Motor.pwm(port, int(pwm), mode) - - @staticmethod - def power(port: int, percent: float): - """Set specified motor to percentage power, percentage is linearized - so that it's roughly proportional to the speed - - :param port: Port, which the motor is connected to. 1-4 - :param percent: Percentage of max speed. between -100 and 100 - :raises: IndexError - """ - raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent)) - if percent < 0: - raw_power *= -1 - - MetricsLogging.put("Motor", float(percent), port) - Motor.power_raw(port, raw_power, False) - - @staticmethod - def all_off(): - """ - Turns of all motors - """ - Logging.get_logger().debug(f"Motor.all_off") - - for i in range(1, MOTOR_COUNT + 1): - Motor.active_break(i) - - @staticmethod - def active_break(port: int): - """ - Actively break with a specific motor - - :param port: Port, which the motor is connected to. 1-4 - """ - Motor.pwm(port, 0, MotorMode.BREAK) - - @staticmethod - def set_motor_curve(curve): - """ - Set the global motor curve, must be a float array with exactly 21 elements. - [0] = x ticks/s (0% power) - [1] = x ticks/s (5% power) - [2] = x ticks/s (10% power) - ... - [20] = x ticks/s (100% power) - - :param curve: float array with 21 elements - :raises: ValueError - """ - if (len(curve) != 21): - raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!') - - global MOTOR_CURVE - MOTOR_CURVE = curve - - @staticmethod - def get_motor_curve(): - """ - Get the currently active motor curve. Check set_motor_curve() for more info. - - :return: current motor curve - """ - return MOTOR_CURVE - - @staticmethod - def __map(x, in_min, in_max, out_min, out_max): - """ - Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/ - """ - x = float(x) - in_min = float(in_min) - in_max = float(in_max) - out_min = float(out_min) - out_max = float(out_max) - - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min - - @staticmethod - def __interpolateSpeed(curve, speed): - """ - Interpolate a speed in the specified motor curve and return - the 'power percentage that is needed to reach that speed' - """ - if speed < min(curve) or speed > max(curve): - raise ValueError(f'Speed out of range: {str(speed)} ticks/s') - - for index in range(len(curve) - 1): - if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]: - return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5) - - return 0 - - @staticmethod - def __linearizePower(curve, power): - """ - Takes raw power and returns it corrected so that the motor speed would be roughly linear - """ - if power < 0 or power > 100: - raise ValueError(f'Power out of range: {str(power)}%') - - requiredSpeed = Motor.__map(power, 0, 100, min(curve), max(curve)) - - return Motor.__interpolateSpeed(curve, requiredSpeed) - - - @staticmethod - def robot_state_loop(): - import time - from compLib.Api import Seeding - - global SPEED_LOCK - global SPEED_MULT - - while True: - try: - state, status = Seeding.get_robot_state() - SPEED_MULT = state["speed"] / 100.0 - - if state["left"] != -1 or state["right"] != -1: - Motor.power(1, state["right"]) - Motor.power(4, state["left"]) - SPEED_LOCK = True - time.sleep(state["time"]) - SPEED_LOCK = False - Motor.power(1, 0) - Motor.power(4, 0) - else: - time.sleep(0.25) - except: - time.sleep(0.25) - pass - - @staticmethod - def start_robot_state_loop(): - from threading import Thread - robot_state_thread = Thread(target=Motor.robot_state_loop) - robot_state_thread.setDaemon(True) - robot_state_thread.start() - -atexit.register(Motor.all_off) diff --git a/client_s1/compLib/Odom.py b/client_s1/compLib/Odom.py deleted file mode 100644 index c88a0ef..0000000 --- a/client_s1/compLib/Odom.py +++ /dev/null @@ -1,101 +0,0 @@ -import time -import math -from compLib.Encoder import Encoder -from compLib.Robot import Robot - -last_run = time.time() -last_enc_left = 0 -last_enc_right = 0 - -pos_x = 0 -pos_y = 0 -orientation = 0 - - -class Odometry(): - """DTO used for holding all odometry information. \n - Coordinate system: \n - X: + Forward; - Backwards \n - Y: + Right; - Left \n - Orientation: + Right; - Left - """ - - def __init__(self, x, y, orientation): - self.x = x - self.y = y - self.orientation = orientation - - def get_x(self) -> float: - """Returns distance driven on x-axis in meters""" - return self.x - - def get_y(self) -> float: - """Returns distance driven on y-axis in meters""" - return self.y - - def get_orientation(self) -> float: - """Returns degrees turned in radians""" - return self.orientation - - def __str__(self): - return f"X: {self.x} Y: {self.y} O: {self.orientation}" - - -class Odom(object): - """Class used to track the movement of the robot in X, Y, Theta (Orientation) - """ - - @staticmethod - def get_odom() -> Odometry: - """ - :return: Current orientation of the robot - """ - return Odometry(pos_x, pos_y, orientation) - - @staticmethod - def clear() -> None: - """ - Clears the current odometry information and start from X, Y, Orientation set to 0 - """ - global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation - - last_run = 0 - last_enc_left = 0 - last_enc_right = 0 - pos_x = 0 - pos_y = 0 - orientation = 0 - - @staticmethod - def update() -> None: - """ - Updates the current odometry information of the robot, Should be called in a loop with at least 100HZ. - Do not clear encoder positions between updates! Must be cleared, when clearing encoders! - """ - global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation - - now = time.time() - if last_run + 0.01 < now: - last_run = now - - encoder_positions = Encoder.read_all() - current_left = encoder_positions[Robot.LEFT_PORT - 1] - current_right = -encoder_positions[Robot.RIGHT_PORT - 1] - distance_left = (current_left - last_enc_left) / Robot.TICKS_PER_METER - distance_right = (current_right - last_enc_right) / Robot.TICKS_PER_METER - last_enc_left = current_left - last_enc_right = current_right - - distance = (distance_left + distance_right) / 2.0 - theta = (distance_left - distance_right) / Robot.ARBOR_LENGTH_M - - if distance != 0: - distance_x = math.cos(theta) * distance - distance_y = -math.sin(theta) * distance - - pos_x = pos_x + (math.cos(orientation) * distance_x - - math.sin(orientation) * distance_y) - pos_y = pos_y + (math.sin(orientation) * distance_x + - math.cos(orientation) * distance_y) - if theta != 0: - orientation += theta \ No newline at end of file diff --git a/client_s1/compLib/Reset.py b/client_s1/compLib/Reset.py deleted file mode 100644 index c297cfc..0000000 --- a/client_s1/compLib/Reset.py +++ /dev/null @@ -1,24 +0,0 @@ -import RPi.GPIO as GPIO -import time - -GPIO.setwarnings(False) -RESET_PIN = 23 -BOOT_PIN = 17 - -class Reset: - """Reset the co-processor - """ - - @staticmethod - def reset_bot(): - """Reset the co-processor - """ - GPIO.setmode(GPIO.BCM) - GPIO.setup(RESET_PIN, GPIO.OUT) - GPIO.setup(BOOT_PIN, GPIO.OUT) - - GPIO.output(RESET_PIN, GPIO.LOW) - GPIO.output(BOOT_PIN, GPIO.LOW) - time.sleep(0.1) - GPIO.output(RESET_PIN, GPIO.HIGH) - time.sleep(1.5) diff --git a/client_s1/compLib/Robot.py b/client_s1/compLib/Robot.py deleted file mode 100644 index 2609cbd..0000000 --- a/client_s1/compLib/Robot.py +++ /dev/null @@ -1,24 +0,0 @@ -import math - - -class Robot(object): - WHEEL_CIRCUMFERENCE_MM = 71.0 * math.pi - """Circumference of a wheel in millimeters""" - - TICKS_PER_TURN = 27.7 * 100.0 - """Ticks per 360 degree turn of a wheel""" - - ARBOR_LENGTH_MM = 139.0 - """Distance between the two wheels in millimeters""" - - ARBOR_LENGTH_M = ARBOR_LENGTH_MM / 1000.0 - """Distance between the two wheels in meters""" - - TICKS_PER_METER = 1000.0 / WHEEL_CIRCUMFERENCE_MM * TICKS_PER_TURN - """Ticks after driving one meter""" - - LEFT_PORT = 4 - """Motor port for the left motor""" - - RIGHT_PORT = 1 - """Motor port for the right motor""" diff --git a/client_s1/compLib/Servo.py b/client_s1/compLib/Servo.py deleted file mode 100644 index 8aefeee..0000000 --- a/client_s1/compLib/Servo.py +++ /dev/null @@ -1,63 +0,0 @@ -from compLib.Spi import Spi, Register - - -SERVO_COUNT = 8 - -MIN_ANGLE = -90.0 -MAX_ANGLE = 90.0 - - -def map(x, in_min, in_max, out_min, out_max): - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min - - -class Servo: - """Control the servo ports on the robot - """ - - @staticmethod - def set_position(port: int, angle: float, offset: float = 90.0): - """Set position of servo connected to port - - :param port: port between 1 and 8 - :param angle: Angle of servo - """ - if port <= 0 or port > SERVO_COUNT: - raise IndexError("Invalid Servo port specified!") - - angle = max(min(angle, MAX_ANGLE), MIN_ANGLE) - - mapped_angle = int(map(angle, MIN_ANGLE, MAX_ANGLE, 0, 65535)) - - if port == 1: - Spi.write(Register.SERVO_1_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_1_CTRL, 1, 4) - elif port == 2: - Spi.write(Register.SERVO_2_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_1_CTRL, 1, 4) - elif port == 3: - Spi.write(Register.SERVO_3_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_2_CTRL, 1, 4) - elif port == 4: - Spi.write(Register.SERVO_4_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_2_CTRL, 1, 4) - elif port == 5: - Spi.write(Register.SERVO_5_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_3_CTRL, 1, 4) - elif port == 6: - Spi.write(Register.SERVO_6_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_3_CTRL, 1, 4) - elif port == 7: - Spi.write(Register.SERVO_7_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_4_CTRL, 1, 4) - elif port == 8: - Spi.write(Register.SERVO_8_PWM_H, 2, mapped_angle) - Spi.write(Register.PWM_4_CTRL, 1, 4) - - @staticmethod - def setup_position(): - """Set position of servos to the position used during the setup process - """ - - for i in range(1, SERVO_COUNT + 1): - Servo.set_position(i, 0) diff --git a/client_s1/compLib/Spi.py b/client_s1/compLib/Spi.py deleted file mode 100644 index 90c08a5..0000000 --- a/client_s1/compLib/Spi.py +++ /dev/null @@ -1,222 +0,0 @@ -import importlib -from threading import Thread -import multiprocessing -from enum import IntEnum -import time -import sys -import random - -from compLib.LogstashLogging import Logging - -SPI_BUS = 1 -SPI_DEVICE = 2 -SPI_SPEED = 4000000 -SPI_BUFFER_SIZE = 32 -SPI_HEALTH = True - -# For development purposes -spi_found = importlib.util.find_spec("spidev") is not None -spi = None -spi_lock = multiprocessing.Lock() -last_spi_call = time.time() -if spi_found: - import spidev - spi = spidev.SpiDev() - spi.open(SPI_BUS, SPI_DEVICE) - spi.max_speed_hz = SPI_SPEED - spi.mode = 0 - spi.bits_per_word = 8 - -class Register(IntEnum): - IDENTIFICATION_MODEL_ID = 1, - IDENTIFICATION_MODEL_REV_MAJOR = 2, - IDENTIFICATION_MODEL_REV_MINOR = 3, - IDENTIFICATION_MODEL_REV_PATCH = 4, - - # Motor encoder positions - MOTOR_1_POS_B3 = 10, - MOTOR_1_POS_B2 = 11, - MOTOR_1_POS_B1 = 12, - MOTOR_1_POS_B0 = 13, - MOTOR_2_POS_B3 = 14, - MOTOR_2_POS_B2 = 15, - MOTOR_2_POS_B1 = 16, - MOTOR_2_POS_B0 = 17, - MOTOR_3_POS_B3 = 18, - MOTOR_3_POS_B2 = 19, - MOTOR_3_POS_B1 = 20, - MOTOR_3_POS_B0 = 21, - MOTOR_4_POS_B3 = 22, - MOTOR_4_POS_B2 = 23, - MOTOR_4_POS_B1 = 24, - MOTOR_4_POS_B0 = 25, - - # PWM Control Modes - PWM_1_CTRL = 26, - PWM_2_CTRL = 27, - PWM_3_CTRL = 28, - PWM_4_CTRL = 29, - - # Motor pwm speed - MOTOR_1_PWM_H = 30, - MOTOR_1_PWM_L = 31, - MOTOR_2_PWM_H = 32, - MOTOR_2_PWM_L = 33, - MOTOR_3_PWM_H = 34, - MOTOR_3_PWM_L = 35, - MOTOR_4_PWM_H = 36, - MOTOR_4_PWM_L = 37, - - # Servo goal position - SERVO_1_PWM_H = 38, - SERVO_1_PWM_L = 39, - SERVO_2_PWM_H = 40, - SERVO_2_PWM_L = 41, - SERVO_3_PWM_H = 42, - SERVO_3_PWM_L = 43, - SERVO_4_PWM_H = 44, - SERVO_4_PWM_L = 45, - SERVO_5_PWM_H = 46, - SERVO_5_PWM_L = 47, - SERVO_6_PWM_H = 48, - SERVO_6_PWM_L = 49, - SERVO_7_PWM_H = 50, - SERVO_7_PWM_L = 51, - SERVO_8_PWM_H = 52, - SERVO_8_PWM_L = 53, - - # IR Sensor value - IR_1_H = 54, - IR_1_L = 55, - IR_2_H = 56, - IR_2_L = 57, - IR_3_H = 58, - IR_3_L = 59, - IR_4_H = 60, - IR_4_L = 61, - IR_5_H = 62, - IR_5_L = 63, - IR_1_LED = 64, - IR_2_LED = 65, - IR_3_LED = 66, - IR_4_LED = 67, - IR_5_LED = 68, - - # Display registers - DISPLAY_LINE_1_C0 = 69, - DISPLAY_LINE_2_C0 = 85, - DISPLAY_LINE_3_C0 = 101, - DISPLAY_LINE_4_C0 = 117 - - -class Spi(object): - - @staticmethod - def transfer(tx_buffer: list): - if not spi_found: - return [] * SPI_BUFFER_SIZE - - write_reg = tx_buffer[1] - tx_buffer_copy = tx_buffer.copy() - - with spi_lock: - spi.xfer(tx_buffer) - rx_buffer = spi.xfer([0] * SPI_BUFFER_SIZE) - - if rx_buffer[1] != write_reg: - Logging.get_logger().error(f"Warning! SPI error during read/write of register {write_reg}! Retrying automagically") - time.sleep(random.uniform(0.0, 0.1)) - return Spi.transfer(tx_buffer_copy) - - global last_spi_call - last_spi_call = time.time() - - return rx_buffer - - @staticmethod - def read(reg: int, length: int): - return int.from_bytes(Spi.read_array(reg, length), byteorder='big', signed=False) - - @staticmethod - def read_array(reg: int, length: int): - if not type(reg) is int: - reg = int(reg) - - tx_buf = [0] * SPI_BUFFER_SIZE - - tx_buf[0] = 0 - tx_buf[1] = reg - tx_buf[2] = length - - rx_buf = Spi.transfer(tx_buf) - return rx_buf[2:2 + length] - - @staticmethod - def write(reg: int, length: int, value: int): - if not type(reg) is int: - reg = int(reg) - - tx_buf = [0] * SPI_BUFFER_SIZE - - tx_buf[0] = 1 - tx_buf[1] = reg - tx_buf[2] = length - - pos = 3 - for i in value.to_bytes(length, 'big'): - tx_buf[pos] = i - pos += 1 - - rx_buf = Spi.transfer(tx_buf) - return int.from_bytes(rx_buf[2:2 + length], byteorder='big', signed=False) - - @staticmethod - def write_array(reg: int, length: int, values): - if not type(reg) is int: - reg = int(reg) - - tx_buf = [0] * SPI_BUFFER_SIZE - - tx_buf[0] = 1 - tx_buf[1] = reg - tx_buf[2] = length - - pos = 3 - for i in values: - tx_buf[pos] = i - pos += 1 - - rx_buf = Spi.transfer(tx_buf) - return rx_buf - - @staticmethod - def health_check(): - if Spi.read(Register.IDENTIFICATION_MODEL_ID, 1) != 3: - Logging.get_logger().error(f"Unable to read Version! Make sure the mainboard is connected!") - print("Unable to read Version! Make sure the mainboard is connected!") - - @staticmethod - def start_health_check_loop(): - health_check_thread = Thread(target=Spi.health_check_loop) - health_check_thread.setDaemon(True) - health_check_thread.start() - - @staticmethod - def disable_health_check(): - global SPI_HEALTH - SPI_HEALTH = False - - @staticmethod - def health_check_loop(): - while SPI_HEALTH: - if last_spi_call + 0.5 < time.time(): - Spi.health_check() - time.sleep(0.1) - - @staticmethod - def get_version(): - major = Spi.read(Register.IDENTIFICATION_MODEL_REV_MAJOR, 1) - minor = Spi.read(Register.IDENTIFICATION_MODEL_REV_MINOR, 1) - patch = Spi.read(Register.IDENTIFICATION_MODEL_REV_PATCH, 1) - - return f"{major}.{minor}.{patch}" diff --git a/client_s1/compLib/Vision.py b/client_s1/compLib/Vision.py deleted file mode 100644 index c3221a2..0000000 --- a/client_s1/compLib/Vision.py +++ /dev/null @@ -1,207 +0,0 @@ -import os -import queue -import socket -import threading - -import cv2 -from flask import Flask, Response - -from compLib.LogstashLogging import Logging - -RTMP_SERVER = os.getenv("RTMP_SERVER", "rtmp://localhost/live/stream") -SERVE_VIDEO = os.getenv("SERVER_SRC", "/live") -BUILDING_DOCS = os.getenv("BUILDING_DOCS", "false") - -app = Flask(__name__) - -HTML = """ - - - Opencv Output - - -

Opencv Output

- - - -""" - -# it would be better to use jinja2 here, but I don't want to blow up the package dependencies... -HTML = HTML.replace("{{ VIDEO_DST }}", SERVE_VIDEO) - - -class __Streaming: - """ - Class that handles rtmp streaming for opencv. - - DO NOT CREATE AN INSTANCE OF THIS CLASS YOURSELF! - - This is automatically done when importing this module. Use Vision.Streaming which is - an instance of this class! - - grab frames -> do your own processing -> publish frame -> view on http server - """ - - class __NoBufferVideoCapture: - def __init__(self, cam): - self.cap = cv2.VideoCapture(cam) - self.cap.set(3, 640) - self.cap.set(4, 480) - self.q = queue.Queue(maxsize=5) - t = threading.Thread(target=self._reader) - t.daemon = True - t.start() - - def _reader(self): - while True: - ret, frame = self.cap.read() - if not ret: - break - if not self.q.empty(): - try: - self.q.get_nowait() - except queue.Empty: - pass - self.q.put(frame) - - def read(self): - return self.q.get() - - def __init__(self): - """ - Create instance of __Streaming class - - This is done implicitly when importing the vision module and will only fail if you would - create an object of this class. (There can (SHOULD!) only be one VideCapture) - """ - Logging.get_logger().info("capturing rtmp stream is disabled in this version") - #self.__camera_stream = cv2.VideoCapture(RTMP_SERVER) - #self.__camera_stream = cv2.VideoCapture(0) - #self.__camera_stream.set(3, 640) - #self.__camera_stream.set(4, 480) - self.__camera_stream = self.__NoBufferVideoCapture(0) - self.__newest_frame = None - self.__lock = threading.Lock() - Logging.get_logger().info("Initialized vision") - - def get_frame(self): - """ - Grab the newest frame from the rtmp stream. - - :return: An opencv frame - """ - #ret, img16 = self.__camera_stream.read() - img16 = self.__camera_stream.read() - return img16 - - def publish_frame(self, image): - """ - Publish an opencv frame to the http webserver. - - :param image: Opencv frame that will be published - :return: None - """ - with self.__lock: - if image is not None: - self.__newest_frame = image.copy() - - def _newest_frame_generator(self): - """ - Private generator which is called directly from flask server. - - :return: Yields image/jpeg encoded frames published from publish_frame function. - """ - while True: - # use a buffer frame to copy the newest frame with lock and then freeing it immediately - buffer_frame = None - with self.__lock: - if self.__newest_frame is None: - continue - - buffer_frame = self.__newest_frame.copy() - - # encode frame for jpeg stream - (flag, encoded_image) = cv2.imencode(".jpg", buffer_frame) - - # if there was an error try again with the next frame - if not flag: - continue - - # else yield encoded frame with mimetype image/jpeg - yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + - bytearray(encoded_image) + b'\r\n') - - -Streaming = None -if BUILDING_DOCS == "false": - # instantiate private class __Streaming - Streaming = __Streaming() - Logging.get_logger().info("created instance of streaming class") - - -@app.route("/live") -def __video_feed(): - """ - Define route for serving jpeg stream. - - :return: Return the response generated along with the specific media. - """ - return Response(Streaming._newest_frame_generator(), - mimetype="multipart/x-mixed-replace; boundary=frame") - - -@app.route("/") -def __index(): - """ - Define route for serving a static http site to view the stream. - - :return: Static html page - """ - return HTML - - -def get_ip(): - s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - s.connect(('10.255.255.255', 1)) - IP = s.getsockname()[0] - except Exception as e: - IP = '127.0.0.1' - print(f"Error could not query ip: {e}") - finally: - s.close() - return IP - - -def __start_flask(): - """ - Function for running flask server in a thread. - - :return: - """ - Logging.get_logger().info("starting flask server") - app.run(host="0.0.0.0", port=9898, debug=True, threaded=True, use_reloader=False) - ip = get_ip() - Logging.get_logger().info(f"Vision stream started and can be viewed on: {ip}:9898") - print(f"\033[92mVision stream started and can be viewed on: {ip}:9898\033[0m") - - -if BUILDING_DOCS == "false": - # start flask service in the background - __webserver_thread = threading.Thread(target=__start_flask) - __webserver_thread.start() - -# for debugging and testing start processing frames and detecting a 6 by 9 calibration chessboard -if __name__ == '__main__' and BUILDING_DOCS == "false": - while True: - frame = Streaming.get_frame() - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) - - # processing - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - - # find the chessboard corners - ret, corners = cv2.findChessboardCorners(gray, (6, 9), None) - - cv2.drawChessboardCorners(frame, (6, 9), corners, ret) - Streaming.publish_frame(frame) diff --git a/client_s1/compLib/__init__.py b/client_s1/compLib/__init__.py deleted file mode 100644 index e16b119..0000000 --- a/client_s1/compLib/__init__.py +++ /dev/null @@ -1,50 +0,0 @@ -__version__ = "0.4.1-1" - -import sys -import importlib -import compLib.LogstashLogging -import compLib.MetricsLogging -from compLib.Lock import Lock - -apt_found = importlib.util.find_spec("apt") is not None -spi_found = importlib.util.find_spec("spidev") is not None - - -if apt_found: - import apt - - try: - __versions = apt.Cache()["python3-complib"].versions - if len(__versions) != 1: - print(f"Starting compLib! \033[91mVersion: {__version__} is outdated\033[0m\n" - f"\033[92m[!] run the command 'sudo apt update && sudo apt install python3-complib' to install the newest version\033[0m") - else: - print(f"Starting compLib! \033[92mVersion: {__version__} is up to date\033[0m") - except Exception as e: - compLib.LogstashLogging.Logging.get_logger().error(f"error during checking apt package version -> {str(e)}") - print(f"\033[91merror during checking apt package version -> {str(e)}\033[0m\n") -else: - print("apt is not installed! This is for local development only!") - -if Lock.is_locked(): - print("Another program using compLib is still running! Delete /root/complib.lock if this is an error. Exiting...") - sys.exit() -else: - Lock.lock() - -if spi_found: - from compLib.Spi import Spi - from compLib.Reset import Reset - from compLib.Encoder import Encoder - from compLib.Motor import Motor - import logging - - print(f"\033[34mInitializing chipmunk board...\033[0m") - Reset.reset_bot() - Spi.health_check() - Spi.start_health_check_loop() - Motor.start_robot_state_loop() - Encoder.clear_all() - print(f"\033[34mReady\033[0m\n") -else: - print("spidev is not installed! This is for local development only!") diff --git a/client_s1/complib.service b/client_s1/complib.service deleted file mode 100644 index 758aaf7..0000000 --- a/client_s1/complib.service +++ /dev/null @@ -1,11 +0,0 @@ -[Unit] -Description=Monitoring service -[Service] -ExecStart=/usr/bin/python3 /usr/local/lib/python3.7/dist-packages/compLib/IPService.py -Environment="debug=False" -Restart=always -RestartSec=5 -Type=notify -[Install] -Alias=complib -WantedBy=default.target \ No newline at end of file diff --git a/client_s1/docs/.gitignore b/client_s1/docs/.gitignore deleted file mode 100644 index 1e0c211..0000000 --- a/client_s1/docs/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -build -logs.db \ No newline at end of file diff --git a/client_s1/docs/source/conf.py b/client_s1/docs/source/conf.py deleted file mode 100644 index 7fdb3b3..0000000 --- a/client_s1/docs/source/conf.py +++ /dev/null @@ -1,60 +0,0 @@ -# Configuration file for the Sphinx documentation builder. -# -# This file only contains a selection of the most common options. For a full -# list see the documentation: -# https://www.sphinx-doc.org/en/master/usage/configuration.html - -# -- Path setup -------------------------------------------------------------- - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys - -sys.path.insert(0, os.path.abspath('../..')) -sys.setrecursionlimit(1500) -os.environ["EXTENSIVE_LOGGING"] = "False" - -# -- Project information ----------------------------------------------------- - -project = 'CompLib' -copyright = '2022, Verein zur Förderung von Wissenschaft und Technik an Schulen (F-WuTS)' -author = 'robo4you' - -# The full version, including alpha/beta/rc tags -release = '0.2.3' - -# -- General configuration --------------------------------------------------- - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = [ - 'sphinx.ext.autodoc', - 'sphinx_rtd_theme' -] - -autodoc_mock_imports = ["smbus", "compLib.PCA9685", "RPi", - "pigpio", "flask", "apt", "influxdb_client"] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This pattern also affects html_static_path and html_extra_path. -exclude_patterns = [] - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = 'sphinx_rtd_theme' - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] diff --git a/client_s1/docs/source/index.rst b/client_s1/docs/source/index.rst deleted file mode 100644 index f10ea78..0000000 --- a/client_s1/docs/source/index.rst +++ /dev/null @@ -1,19 +0,0 @@ -.. image:: images/compair-logo-white.svg - -Competition Robot Library -############################# - -.. toctree:: - :maxdepth: 2 - :caption: Contents: - -Contents -********* - -.. toctree:: - :maxdepth: 5 - :glob: - - self - other/usage - lib/* diff --git a/client_s1/docs/source/lib/Api.rst b/client_s1/docs/source/lib/Api.rst deleted file mode 100644 index 850f69b..0000000 --- a/client_s1/docs/source/lib/Api.rst +++ /dev/null @@ -1,107 +0,0 @@ -.. _lib_api: - -Api -**** - -Seeding -======== - -.. autoclass:: compLib.Api.Seeding - :members: - -Double Elimination -=================== - -.. autoclass:: compLib.Api.DoubleElim - :members: - -Position -======== - -.. autoclass:: compLib.Api.Position - :members: - - -Examples -======== - -Calling Seeding API ---------------------- - -.. code-block:: python - - from compLib.Api import Seeding - - zones, code = Seeding.get_delivery() - if code == 403: - print(f"I am not in the correct zone to make that request!") - else: - print(f"First we need to go to zone {zone[0]}") - # put code here to follow line and drive to the zone - print(f"Now we need to go to zone {zone[1]}") - # put code here to follow line and drive to the next zone - print(f"Now we need to go to zone {zone[2]}") - # put code here to follow line and drive to the last zone - print(f"We delivered all packages, hopefully we scored some points!") - -.. code-block:: python - - from compLib.Api import Seeding - - package, code = Seeding.get_cargo("yellow") - if code == 403: - print(f"I am not in the correct zone to make that request!") - elif code == 404: - print(f"I am in the correct zone, but there is no yellow package here.") - elif code == 413: - print(f"I am in the correct zone, but I already have two packages loaded.") - else code == 200: - print(f"The {package['color']} has been picked up!") - -Calling Double Elimination API ----------------------------------- - -.. code-block:: python - - from compLib.Api import DoubleElim - - position, status = DoubleElim.get_position() - print(f"Position of my robot is: x={position.x}, y={position.y} and rotation is: {position.degrees}, the server responded with status code: {status}") - - goal, status = DoubleElim.get_goal() - print(f"Goal is at: x={goal.x}, y={goal.y}, the server responded with status code: {status}") - -.. code-block:: python - - from compLib.Api import DoubleElim - import time - - # function which waits for the game to be started (you should include this in your double elimination program) - def wait_for_start(): - _, status = DoubleElim.get_position() - while status == 503: - time.sleep(0.1) - _, status = DoubleElim.get_position() - - wait_for_start() - print(f"Game has started, lets score some points!!") - - position, status = DoubleElim.get_position() - print(f"Position of my robot is: x={position.x}, y={position.y} and rotation is: {position.degrees}, the server responded with status code: {status}") - - opponent_position, status = DoubleElim.get_opponent() - print(f"Position of the opponents robot is: x={opponent_position.x}, y={opponent_position.y} and rotation is: {opponent_position.degrees}, the server responded with status code: {status}") - - goal, status = DoubleElim.get_goal() - print(f"Goal is at: x={goal.x}, y={goal.y}, the server responded with status code: {status}") - - items, status = DoubleElim.get_items() - print(f"There are currently {len(items)} on the gameboard: {items}, the server responded with status code: {status}") - - score, status = DoubleElim.get_score() - print(f"The current score of the game is {score}, the server responded with status code: {status}") - - meteoroids, status = DoubleElim.get_meteoroids() - print(f"The current meteoroids in the game are {meteoroids}, the server responded with status code: {status}") - -In this second example we wait until the game is started by the judges and then make all possible requests once. You should use the wait_for_start function in your double elimination program. If your robot starts too soon your run will not count! diff --git a/client_s1/docs/source/lib/Aruco.rst b/client_s1/docs/source/lib/Aruco.rst deleted file mode 100644 index 51059ae..0000000 --- a/client_s1/docs/source/lib/Aruco.rst +++ /dev/null @@ -1,83 +0,0 @@ -.. _lib_vision: - -Aruco -******* - -Examples -========= - -Recognizing ArUco tags -------------------------- - -.. code-block:: python - - import time - import cv2 - from cv2 import aruco - from compLib import Vision - - ARUCO_DICT = cv2.aruco.Dictionary_get(aruco.DICT_6X6_250) - ARUCO_PARAMETERS = aruco.DetectorParameters_create() - - def getTagCenterFromFrame(id, frame): - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - - corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters = ARUCO_PARAMETERS) - frame = aruco.drawDetectedMarkers(frame.copy(), corners, ids) - - if ids is None: - return frame, None, None - - for tag_id, corner in zip(ids, corners): - if (tag_id[0] == id): - - x, y = 0, 0 - for i in range(4): - x += corner[0][i][0] * 0.25 - y += corner[0][i][1] * 0.25 - - return frame, x, y - return frame, None, None - - # Get the center from the aruco tag with the specified id - # in pixel coordinates (0-640, 0-480) - def getTagPosition(id): - frame = Vision.Streaming.get_frame() - frame, x, y = getTagCenterFromFrame(id, frame) - Vision.Streaming.publish_frame(frame) - return x, y - - # Get the normalized center coordinates from the aruco tag - # with the specified id - # left is -1, right +1 - # bottom is -1, top +1 - def getNormalizedTagPosition(id): - frame = Vision.Streaming.get_frame() - frame, x, y = getTagCenterFromFrame(id, frame) - Vision.Streaming.publish_frame(frame) - - if x is None or y is None: - return None, None - - height, width = frame.shape[:2] - x = x / width * 2.0 - 1.0 - y = -(y / height * 2.0 - 1.0) - return x, y - - if __name__ == '__main__': - - desiredID = 11 - - while True: - x, y = getNormalizedTagPosition(desiredID) - - if x is not None: - print("X Coordinate: ", x) - else: - print("Tag not found") - - -This example shows how to recognize ArUco tags based on their id and position. -You can specify an ID of the tag you want to use and if it's found, the coordinates of the center are returned. -With the normalized function this is very easy: The x-coordinate is -1 on the left, 1 on the right and 0 in the center of the screen, same for y. -This way it is quite simple to act on the position of the tag. diff --git a/client_s1/docs/source/lib/Display.rst b/client_s1/docs/source/lib/Display.rst deleted file mode 100644 index 6701845..0000000 --- a/client_s1/docs/source/lib/Display.rst +++ /dev/null @@ -1,23 +0,0 @@ -.. _lib_display: - -Display -******* - -Class Documentation -==================== - -.. autoclass:: compLib.Display.Display - :members: - -Examples -========= - -Write a line to the display ---------------------------- - -.. code-block:: python - - import time - from compLib.Display import Display - - Display.write(1, "Hello World!") \ No newline at end of file diff --git a/client_s1/docs/source/lib/Encoder.rst b/client_s1/docs/source/lib/Encoder.rst deleted file mode 100644 index 1d5bf30..0000000 --- a/client_s1/docs/source/lib/Encoder.rst +++ /dev/null @@ -1,10 +0,0 @@ -.. _lib_encoder: - -Encoder -******* - -Class Documentation -==================== - -.. autoclass:: compLib.Encoder.Encoder - :members: diff --git a/client_s1/docs/source/lib/IRSensor.rst b/client_s1/docs/source/lib/IRSensor.rst deleted file mode 100644 index b1dd232..0000000 --- a/client_s1/docs/source/lib/IRSensor.rst +++ /dev/null @@ -1,20 +0,0 @@ -.. _lib_irsensor: - -Infrared Sensor -**************** - -.. autoclass:: compLib.IRSensor.IRSensor - :members: - -Examples -========= - -Testing analog sensors -------------------------- - -.. code-block:: python - - from compLib import IRSensor - - while True: - print ("left: {} middle: {} right: {}".format(IRSensor.read(1), IRSensor.read(3), IRSensor.read(5))) \ No newline at end of file diff --git a/client_s1/docs/source/lib/Linefollower.rst b/client_s1/docs/source/lib/Linefollower.rst deleted file mode 100644 index 41de752..0000000 --- a/client_s1/docs/source/lib/Linefollower.rst +++ /dev/null @@ -1,79 +0,0 @@ -.. _lib_Linefollower: - -Linefollower Examples -********************* - -Simple Linefollower -------------------------- - -.. code-block:: python - - from compLib.Motor import Motor - from compLib.Display import Display - from compLib.IRSensor import IRSensor - from compLib.Encoder import Encoder - - import time - - IRSensor.set(1, True) - IRSensor.set(2, True) - IRSensor.set(3, True) - IRSensor.set(4, True) - IRSensor.set(5, True) - - DRIVE_SPEED = 75 - COLOR_BREAK = 900 - KP = 10.0 - KD = 0.0 - - def drive(leftSpeed, rightSpeed): - rightSpeed *= -0.906 - - Motor.power(1, min(max(-100, rightSpeed), 100)) - Motor.power(4, min(max(-100, leftSpeed), 100)) - - def follow(sleepTime = 0.1): - lastError = 0 - sensorsBlack = 0 - while sensorsBlack < 3: - sensorsBlack = 0 - for i in range(1, 6): - if IRSensor.read(i) > COLOR_BREAK: - sensorsBlack += 1 - - error = lastError - if IRSensor.read(3) > COLOR_BREAK: - error = 0 - elif IRSensor.read(1) > COLOR_BREAK: - error = -1.5 - elif IRSensor.read(5) > COLOR_BREAK: - error = 1.5 - elif IRSensor.read(2) > COLOR_BREAK: - error = -1 - elif IRSensor.read(4) > COLOR_BREAK: - error = 1 - elif error == 1.5: - error = 3 - elif error == -1.5: - error = -3 - - lastError = error - - adjustment = KP * error + KD * (error - lastError) - leftSpeed = DRIVE_SPEED + adjustment - rightSpeed = DRIVE_SPEED - adjustment - - print(f"{leftSpeed} {rightSpeed} {adjustment} {error}") - drive(leftSpeed, rightSpeed) - - drive(0, 0) - time.sleep(sleepTime) - - def main(): - follow() - follow() - follow() - follow() - follow(0.2) - - main() \ No newline at end of file diff --git a/client_s1/docs/source/lib/Logging.rst b/client_s1/docs/source/lib/Logging.rst deleted file mode 100644 index 5fe6d41..0000000 --- a/client_s1/docs/source/lib/Logging.rst +++ /dev/null @@ -1,22 +0,0 @@ -.. _lib_logging: - -Logging -******* - -Class Documentation -==================== - -.. autoclass:: compLib.LogstashLogging.Logging - :members: - -Examples -========= - -Turn up the logging --------------------- - -.. code-block:: python - - from compLib.LogstashLogging import Logging - - Logging.set_debug() \ No newline at end of file diff --git a/client_s1/docs/source/lib/Motor.rst b/client_s1/docs/source/lib/Motor.rst deleted file mode 100644 index bd8b253..0000000 --- a/client_s1/docs/source/lib/Motor.rst +++ /dev/null @@ -1,23 +0,0 @@ -.. _lib_motor: - -Motor -****** - -Class Documentation -==================== - -.. autoclass:: compLib.Motor.Motor - :members: - -Examples -========= - -Driving straight (maybe) -------------------------- - -.. code-block:: python - - from compLib.Motor import Motor - - Motor.power(1, -50) - Motor.power(4, 50) \ No newline at end of file diff --git a/client_s1/docs/source/lib/Odom.rst b/client_s1/docs/source/lib/Odom.rst deleted file mode 100644 index 672e11c..0000000 --- a/client_s1/docs/source/lib/Odom.rst +++ /dev/null @@ -1,50 +0,0 @@ -.. _lib_odom: - -Odometry -******** - -Class Documentation -==================== - -.. autoclass:: compLib.Odom.Odometry - :members: - -.. autoclass:: compLib.Odom.Odom - :members: - - -Examples -========= - -Getting actual distance driven ------------------------------- - -.. code-block:: python - - import time - import math - from compLib.Motor import Motor - from compLib.Encoder import Encoder - from compLib.Odom import Odom, Odometry - - # distance in meters - # speed in % of max speed - def drive_example(distance, speed): - Odom.update() - odom = Odom.get_odom() - while abs(odom.get_x()) < distance: - Odom.update() - odom = Odom.get_odom() - - Motor.power(4, speed) - Motor.power(1, -speed) - - print(f" Forward: {odom.get_x()} m") - print(f" Right: {odom.get_y()} m") - print(f" Turned: {math.degrees(odom.get_orientation())} degrees") - - Motor.active_break(1) - Motor.active_break(4) - time.sleep(0.1) - Encoder.clear_all() - Odom.clear() \ No newline at end of file diff --git a/client_s1/docs/source/lib/QC.rst b/client_s1/docs/source/lib/QC.rst deleted file mode 100644 index 924fa71..0000000 --- a/client_s1/docs/source/lib/QC.rst +++ /dev/null @@ -1,71 +0,0 @@ -.. _lib_qc: - -Quality Control -############### - -Infrared Test -------------------------- - -.. code-block:: python - - from compLib.IRSensor import IRSensor - import time - - IRSensor.set(1, True) - IRSensor.set(2, True) - IRSensor.set(3, True) - IRSensor.set(4, True) - IRSensor.set(5, True) - - while True: - t = time.time() - for i in range(1, 6): - print(f"{i}: {IRSensor.read(i)}") - print("") - time.sleep(0.2) - -Motor Test -------------------------- - -.. code-block:: python - - from compLib.Motor import Motor - from compLib.Encoder import Encoder - import time - - Motor.power(1, -50) - Motor.power(4, 50) - - while True: - print(f"L:{Encoder.read(4)} R:{Encoder.read(1)}") - time.sleep(0.1) - -Servo Test -------------------------- - -.. code-block:: python - - from compLib.Servo import Servo - import time - - for i in range(1, 8 + 1): - Servo.set_position(i, 45) - print(f"{i}") - time.sleep(1) - - Servo.setup_position() - time.sleep(10) - -Vision Test -------------------------- - -.. code-block:: python - - import cv2 - from compLib import Vision - from compLib.Servo import Servo - - while True: - frame = Vision.Streaming.get_frame() - Vision.Streaming.publish_frame(frame) - Servo.set_position(3, -45) diff --git a/client_s1/docs/source/lib/Robot.rst b/client_s1/docs/source/lib/Robot.rst deleted file mode 100644 index c7aa712..0000000 --- a/client_s1/docs/source/lib/Robot.rst +++ /dev/null @@ -1,11 +0,0 @@ -.. _lib_robot: - -Robot -****** - -Class Documentation -==================== - -.. autoclass:: compLib.Robot.Robot - :members: - :private-members: \ No newline at end of file diff --git a/client_s1/docs/source/lib/Servo.rst b/client_s1/docs/source/lib/Servo.rst deleted file mode 100644 index d991b73..0000000 --- a/client_s1/docs/source/lib/Servo.rst +++ /dev/null @@ -1,7 +0,0 @@ -.. _lib_servo: - -Servo -****** - -.. autoclass:: compLib.Servo.Servo - :members: \ No newline at end of file diff --git a/client_s1/docs/source/lib/Vision.rst b/client_s1/docs/source/lib/Vision.rst deleted file mode 100644 index 5c030d9..0000000 --- a/client_s1/docs/source/lib/Vision.rst +++ /dev/null @@ -1,104 +0,0 @@ -.. _lib_vision: - -Vision -******* - -This module provides an interface for grabbing an rtmp stream and using the images to do some processing in opencv. - -How do I use this module? - -1. Get frames from the raspberry pi camera -2. -- here comes your own processing -- -3. Publish the processed frames on an http server -4. You can view the http stream of your processed images in a web browser - -Opencv Stream -============== - -Because of the rtmp stream needing to buffer some frames and waiting for P-Frames, importing this module might take up -to 5 Seconds. - -.. autoclass:: compLib.Vision.__Streaming - :members: - -Examples -========= - -Using the Vision Module -------------------------- - -.. code-block:: python - - import cv2 - from compLib import Vision - - while True: - # get newest opencv frame from camera - frame = Vision.Streaming.get_frame() - - # do some processing with the frame..... - - # publish frame to streaming server - Vision.Streaming.publish_frame(frame) - -Connect the raspberry pi to your internet and view the stream at: "http://your_raspi_ip:9898/". This should display -your raspberry pi camera. Note: the stream will lag a little bit BUT the processing of the image will be done in -realtime. - -The output on the website should show whatever your raspberry pi cam records: - -.. image:: images/opencv_http_stream.png - :width: 680 - :alt: Processed frames from opencv - - -Chessboard Detection ------------------------------------------- - -In this example we process the captured stream of images and want to detect chessboards. Run this example and -point your raspberry pi camera to a chessboard and it should be detected. - -For testing you can point it at this image: - -.. image:: images/chessboard.jpg - :width: 680 - :alt: Chessboard for opencv processing - -.. code-block:: python - - import cv2 - from compLib import Vision - - while True: - # get newest opencv frame from camera - frame = Vision.Streaming.get_frame() - - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) - - # convert image to grayscale image - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - - # find the chessboard corners - ret, corners = cv2.findChessboardCorners(gray, (6, 9), None) - - # draw detected chessboard position onto the image - cv2.drawChessboardCorners(frame, (6, 9), corners, ret) - - # publish frame to streaming server - Vision.Streaming.publish_frame(frame) - -Connect the raspberry pi to your internet and view the stream at: "http://your_raspi_ip:9898/". - -The output image should look like this: - -.. image:: images/chessboard_detected.jpg - :width: 680 - :alt: Processed frames from opencv - -Here is a screenshot of the stream website while viewing the chessboard in this documentation. - -.. image:: images/opencv_processed.png - :width: 680 - :alt: Processed frames from opencv - - diff --git a/client_s1/docs/source/other/usage.rst b/client_s1/docs/source/other/usage.rst deleted file mode 100644 index 50fd86d..0000000 --- a/client_s1/docs/source/other/usage.rst +++ /dev/null @@ -1,29 +0,0 @@ -.. _other_usage: - -Usage -###### - -.. code-block:: python - - import time - from compLib.Motor import * - - def forward(): - Motor.power(1, -30); - Motor.power(2, 30); - - - def backward(): - Motor.power(1, 30); - Motor.power(2, -30); - - def main(): - print("hallo ich bin ein roboter beep buup") - - forward() - time.sleep(1) - backward() - time.sleep(1) - - if __name__ == '__main__': - main() \ No newline at end of file diff --git a/client_s1/postinstall.sh b/client_s1/postinstall.sh deleted file mode 100644 index fabfa94..0000000 --- a/client_s1/postinstall.sh +++ /dev/null @@ -1,63 +0,0 @@ -grep -qxF "apt update" /etc/rc.local -if [ $? -ne 0 ]; then - echo "adding apt update to rc.local" - sed -i "2s/^/apt update\n/" /etc/rc.local -fi - -install_package() { - echo "Installing package '$1' via pip3" - pip3 install "$1" - if [ $? -eq 0 ]; then - echo "Successfully installed pip3 package '$1'" - else - echo "Could not install pip3 package '$1'" - exit 1 - fi -} - -#install_package "smbus" -#install_package "requests" -#install_package "flask" -#install_package "python-logstash-async" -#install_package "RPi.GPIO" -#install_package "spidev" -#install_package "influxdb_client" - -pip3 install smbus requests flask python-logstash-async RPi.GPIO spidev influxdb_client filelock - -echo "Setting up opencv4" -pkg-config --modversion opencv4 - -echo "Setting up nginx rtmp server" -sudo /etc/init.d/nginx start - -{ - echo 'load_module "modules/ngx_rtmp_module.so";' - echo 'worker_processes auto;' - echo 'rtmp_auto_push on;' - echo 'events {}' - echo 'rtmp {' - echo ' server {' - echo ' listen 1935;' - echo ' listen [::]:1935 ipv6only=on;' - echo ' application live {' - echo ' live on;' - echo ' record off;' - echo ' }' - echo ' }' - echo '}' -} >| /etc/nginx/nginx.conf - -echo "Starting pigpiod daemon" - -sudo systemctl start pigpiod -sudo systemctl enable pigpiod - -base64 -d << UPD -CiBfX19fX18gICAgIF9fX19fXyAgICAgX18gICAgX18gICAgIF9fX19fXyAgIF9fICAgICAgICAgX18gICAgIF9fX19fXyAgICAgICAgICAgICAgICAgIAovXCAgX19fXCAgIC9cICBfXyBcICAgL1wgIi0uLyAgXCAgIC9cICA9PSBcIC9cIFwgICAgICAgL1wgXCAgIC9cICA9PSBcICAgICAgICAgICAgICAgICAKXCBcIFxfX19fICBcIFwgXC9cIFwgIFwgXCBcLS4vXCBcICBcIFwgIF8tLyBcIFwgXF9fX18gIFwgXCBcICBcIFwgIF9fPCAgICAgICAgICAgICAgICAgCiBcIFxfX19fX1wgIFwgXF9fX19fXCAgXCBcX1wgXCBcX1wgIFwgXF9cICAgIFwgXF9fX19fXCAgXCBcX1wgIFwgXF9fX19fXCAgICAgICAgICAgICAgIAogIFwvX19fX18vICAgXC9fX19fXy8gICBcL18vICBcL18vICAgXC9fLyAgICAgXC9fX19fXy8gICBcL18vICAgXC9fX19fXy8gICAgICAgICAgICAgICAKICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgCiBfXyAgX18gICAgIF9fX19fXyAgICAgIF9fX19fXyAgIF9fX19fXyAgICAgICAgX19fX18gICAgIF9fX19fXyAgICAgX19fX19fICAgX19fX19fICAgIAovXCBcL1wgXCAgIC9cICA9PSBcICAgIC9cX18gIF9cIC9cICBfXyBcICAgICAgL1wgIF9fLS4gIC9cICBfXyBcICAgL1xfXyAgX1wgL1wgIF9fX1wgICAKXCBcIFxfXCBcICBcIFwgIF8tLyAgICBcL18vXCBcLyBcIFwgXC9cIFwgICAgIFwgXCBcL1wgXCBcIFwgIF9fIFwgIFwvXy9cIFwvIFwgXCAgX19cICAgCiBcIFxfX19fX1wgIFwgXF9cICAgICAgICAgXCBcX1wgIFwgXF9fX19fXCAgICAgXCBcX19fXy0gIFwgXF9cIFxfXCAgICBcIFxfXCAgXCBcX19fX19cIAogIFwvX19fX18vICAgXC9fLyAgICAgICAgICBcL18vICAgXC9fX19fXy8gICAgICBcL19fX18vICAgXC9fL1wvXy8gICAgIFwvXy8gICBcL19fX19fLyA= -UPD -echo "" -echo "" - -service complib restart -systemctl enable complib diff --git a/client_s1/setup.py b/client_s1/setup.py deleted file mode 100644 index 19e14da..0000000 --- a/client_s1/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -import setuptools -import os - -base_dir = os.path.dirname(__file__) -readme_path = os.path.join(base_dir, "README.md") -if os.path.exists(readme_path): - with open(readme_path) as stream: - long_description = stream.read() -else: - long_description = "" - -setuptools.setup( - name="complib", - version="0.4.1-1", - author="F-WuTs", - author_email="--", - description="", - summary="Library for the competition", - platforms=["any"], - long_description=long_description, - long_description_content_type="text/markdown", - url="https://github.com/F-WuTS/compLIB", - packages=setuptools.find_packages(), - include_package_data=True, - classifiers=[ - "Programming Language :: Python :: 3.7", - "License :: Other/Proprietary License", - "Operating System :: Unix" - ], - license="proprietary"#, - #install_requires=[ - # "smbus", - # "requests", - # "python-logstash-async" - #] -) diff --git a/client_s1/test.py b/client_s1/test.py deleted file mode 100644 index 52d6fa3..0000000 --- a/client_s1/test.py +++ /dev/null @@ -1,61 +0,0 @@ - -import unittest - -import os -os.environ["API_URL"] = "http://127.0.0.1:5000/" -from compLib.Api import Seeding -from compLib.Api import DoubleElim - - -class TestApiServer(unittest.TestCase): - def test_all(self): - ret, code = Seeding.get_garbage() - print(ret) - print(code) - ret, code = Seeding.get_delivery() - print(ret) - print(code) - ret, code = Seeding.get_material() - print(ret) - print(code) - ret, code = Seeding.list_cargo() - print(ret) - print(code) - ret, code = Seeding.get_cargo("green") - print(ret) - print(code) - ret, code = Seeding.get_cargo("yellow") - print(ret) - print(code) - ret, code = Seeding.get_cargo("blue") - print(ret) - print(code) - ret, code = Seeding.get_cargo("red") - print(ret) - print(code) - - -class TestApiServerDoubleElim(unittest.TestCase): - def test_all(self): - ret, code = DoubleElim.get_goal() - print(ret) - print(code) - ret, code = DoubleElim.get_items() - print(ret) - print(code) - ret, code = DoubleElim.get_scores() - print(ret) - print(code) - ret, code = DoubleElim.get_opponent() - print(ret) - print(code) - ret, code = DoubleElim.get_position() - print(ret) - print(code) - ret, code = DoubleElim.get_meteoroids() - print(ret) - print(code) - - -if __name__ == '__main__': - unittest.main() diff --git a/client_s2/MANIFEST.in b/client_s2/MANIFEST.in deleted file mode 100644 index 1a69f4c..0000000 --- a/client_s2/MANIFEST.in +++ /dev/null @@ -1 +0,0 @@ -include readme.md diff --git a/client_s2/docs/Makefile b/client_s2/docs/Makefile deleted file mode 100644 index d0c3cbf..0000000 --- a/client_s2/docs/Makefile +++ /dev/null @@ -1,20 +0,0 @@ -# Minimal makefile for Sphinx documentation -# - -# You can set these variables from the command line, and also -# from the environment for the first two. -SPHINXOPTS ?= -SPHINXBUILD ?= sphinx-build -SOURCEDIR = source -BUILDDIR = build - -# Put it first so that "make" without argument is like "make help". -help: - @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) - -.PHONY: help Makefile - -# Catch-all target: route all unknown targets to Sphinx using the new -# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). -%: Makefile - @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/client_s2/docs/make.bat b/client_s2/docs/make.bat deleted file mode 100644 index 6247f7e..0000000 --- a/client_s2/docs/make.bat +++ /dev/null @@ -1,35 +0,0 @@ -@ECHO OFF - -pushd %~dp0 - -REM Command file for Sphinx documentation - -if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-build -) -set SOURCEDIR=source -set BUILDDIR=build - -if "%1" == "" goto help - -%SPHINXBUILD% >NUL 2>NUL -if errorlevel 9009 ( - echo. - echo.The 'sphinx-build' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-build' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 -) - -%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% -goto end - -:help -%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% - -:end -popd diff --git a/client_s2/docs/source/_static/.gitkeep b/client_s2/docs/source/_static/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/client_s2/docs/source/images/compair-logo-white.svg b/client_s2/docs/source/images/compair-logo-white.svg deleted file mode 100644 index 8fb8da1..0000000 --- a/client_s2/docs/source/images/compair-logo-white.svg +++ /dev/null @@ -1,189 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - - - diff --git a/client_s2/docs/source/lib/classes/images/chessboard.jpg b/client_s2/docs/source/lib/classes/images/chessboard.jpg deleted file mode 100644 index 53ffa83..0000000 Binary files a/client_s2/docs/source/lib/classes/images/chessboard.jpg and /dev/null differ diff --git a/client_s2/docs/source/lib/classes/images/chessboard_detected.jpg b/client_s2/docs/source/lib/classes/images/chessboard_detected.jpg deleted file mode 100644 index 4f64fb6..0000000 Binary files a/client_s2/docs/source/lib/classes/images/chessboard_detected.jpg and /dev/null differ diff --git a/client_s2/docs/source/lib/classes/images/opencv_http_stream.png b/client_s2/docs/source/lib/classes/images/opencv_http_stream.png deleted file mode 100644 index 8fc6853..0000000 Binary files a/client_s2/docs/source/lib/classes/images/opencv_http_stream.png and /dev/null differ diff --git a/client_s2/docs/source/lib/classes/images/opencv_processed.png b/client_s2/docs/source/lib/classes/images/opencv_processed.png deleted file mode 100644 index 27b59c7..0000000 Binary files a/client_s2/docs/source/lib/classes/images/opencv_processed.png and /dev/null differ diff --git a/client_s2/sphinx_to_github.sh b/client_s2/sphinx_to_github.sh deleted file mode 100755 index b4f51cb..0000000 --- a/client_s2/sphinx_to_github.sh +++ /dev/null @@ -1,23 +0,0 @@ -export BUILDING_DOCS=true -cd docs || exit -rm -rf build -rm -rf gh-pages - -make html - -git clone git@github.com:F-WuTS/compLIB.git gh-pages - -cd gh-pages || exit -git checkout gh-pages - -cp -r ../build/html/* . - -git add . -git commit -a -m "Update documentation" -git push origin gh-pages - -cd .. -rm -rf gh-pages - -cd .. -export BUILDING_DOCS=false \ No newline at end of file diff --git a/server/.idea/workspace.xml b/server/.idea/workspace.xml deleted file mode 100644 index e30254c..0000000 --- a/server/.idea/workspace.xml +++ /dev/null @@ -1,187 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o deleted file mode 100644 index 6551d10..0000000 Binary files a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o and /dev/null differ diff --git a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o deleted file mode 100644 index ded33f1..0000000 Binary files a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o and /dev/null differ diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o deleted file mode 100644 index e87eceb..0000000 Binary files a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o and /dev/null differ diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o deleted file mode 100644 index 7a98719..0000000 Binary files a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o and /dev/null differ diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o deleted file mode 100644 index 0b913ab..0000000 Binary files a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o and /dev/null differ diff --git a/server_prototype/.gitignore b/server_prototype/.gitignore deleted file mode 100644 index 5b4bcb6..0000000 --- a/server_prototype/.gitignore +++ /dev/null @@ -1,106 +0,0 @@ - -# Created by https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode -# Edit at https://www.toptal.com/developers/gitignore?templates=c++,macos,linux,visualstudiocode - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json -!.vscode/*.code-snippets - -# Local History for Visual Studio Code -.history/ - -# Built Visual Studio Code Extensions -*.vsix - -### VisualStudioCode Patch ### -# Ignore all local history of files -.history -.ionide - -# Support for Project snippet scope - -# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode - -build diff --git a/server_prototype/.idea/.gitignore b/server_prototype/.idea/.gitignore deleted file mode 100644 index 8bf4d45..0000000 --- a/server_prototype/.idea/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml -# Datasource local storage ignored files -/dataSources/ -/dataSources.local.xml diff --git a/server_prototype/.idea/.name b/server_prototype/.idea/.name deleted file mode 100644 index 4e811b7..0000000 --- a/server_prototype/.idea/.name +++ /dev/null @@ -1 +0,0 @@ -compLib_server \ No newline at end of file diff --git a/server_prototype/.idea/misc.xml b/server_prototype/.idea/misc.xml deleted file mode 100644 index 79b3c94..0000000 --- a/server_prototype/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/server_prototype/.idea/modules.xml b/server_prototype/.idea/modules.xml deleted file mode 100644 index fbd90b4..0000000 --- a/server_prototype/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/server_prototype/.idea/saveactions_settings.xml b/server_prototype/.idea/saveactions_settings.xml deleted file mode 100644 index b7b3f34..0000000 --- a/server_prototype/.idea/saveactions_settings.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/server_prototype/.idea/serialmonitor_settings.xml b/server_prototype/.idea/serialmonitor_settings.xml deleted file mode 100644 index eff936b..0000000 --- a/server_prototype/.idea/serialmonitor_settings.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/server_prototype/.idea/server.iml b/server_prototype/.idea/server.iml deleted file mode 100644 index f08604b..0000000 --- a/server_prototype/.idea/server.iml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/server_prototype/.idea/vcs.xml b/server_prototype/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/server_prototype/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/server_prototype/CMakeLists.txt b/server_prototype/CMakeLists.txt deleted file mode 100644 index 00c5bb0..0000000 --- a/server_prototype/CMakeLists.txt +++ /dev/null @@ -1,54 +0,0 @@ -cmake_minimum_required(VERSION 3.16) - -set(PROJECT_NAME compLib_server) -project(${PROJECT_NAME}) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) - -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif () - -set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") -set(CMAKE_CXX_FLAGS_DEBUG "-g") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") - -find_package(Protobuf REQUIRED) -find_package(spdlog REQUIRED) -find_package(pigpio REQUIRED) - -set(PROTO_FILES - protos/CompLib.proto - ) - -set(SRC_FILES - src/main.cpp - src/spi.cpp - src/encoder.cpp - src/motor.cpp - src/pid.cpp - ) - -set(HDR_FILES - include/messageBuilder.hpp - include/errorMessages.hpp - include/spi.hpp - include/reset.hpp - include/mathUtils.hpp - include/encoder.hpp - include/robot.hpp - include/motor.hpp - include/pid.hpp - ) - -include_directories(third_party/asio) - -PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HDR ${PROTO_FILES}) -add_executable(${PROJECT_NAME} ${SRC_FILES} ${HDR_FILES} ${PROTO_SRC} ${PROTO_HDR} ${PROTO_FILES}) -target_include_directories(${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - PUBLIC ${PROTOBUF_INCLUDE_DIRS} - ) -target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} pigpio spdlog::spdlog) diff --git a/server_prototype/cmake/modules/Findpigpio.cmake b/server_prototype/cmake/modules/Findpigpio.cmake deleted file mode 100644 index 1308822..0000000 --- a/server_prototype/cmake/modules/Findpigpio.cmake +++ /dev/null @@ -1,31 +0,0 @@ -################################################################################ -### Find the pigpio shared libraries. -################################################################################ - -# Find the path to the pigpio includes. -find_path(pigpio_INCLUDE_DIR - NAMES pigpio.h pigpiod_if.h pigpiod_if2.h - HINTS /usr/local/include) - -# Find the pigpio libraries. -find_library(pigpio_LIBRARY - NAMES libpigpio.so - HINTS /usr/local/lib) -find_library(pigpiod_if_LIBRARY - NAMES libpigpiod_if.so - HINTS /usr/local/lib) -find_library(pigpiod_if2_LIBRARY - NAMES libpigpiod_if2.so - HINTS /usr/local/lib) - -# Set the pigpio variables to plural form to make them accessible for -# the paramount cmake modules. -set(pigpio_INCLUDE_DIRS ${pigpio_INCLUDE_DIR}) -set(pigpio_INCLUDES ${pigpio_INCLUDE_DIR}) - -# Handle REQUIRED, QUIET, and version arguments -# and set the _FOUND variable. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(pigpio - DEFAULT_MSG - pigpio_INCLUDE_DIR pigpio_LIBRARY pigpiod_if_LIBRARY pigpiod_if2_LIBRARY) \ No newline at end of file diff --git a/server_prototype/include/encoder.hpp b/server_prototype/include/encoder.hpp deleted file mode 100644 index eadf62b..0000000 --- a/server_prototype/include/encoder.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef COMPLIB_SERVER_ENCODER_HPP -#define COMPLIB_SERVER_ENCODER_HPP - -#include -#include -#include - -#define ENCODER_COUNT 4 -#define ENCODER_CACHE_DURATION_MS 4 // 200 HZ - -class Encoder { -public: - std::vector read_all(); - std::vector read_all_cached(); - - static Encoder& get_instance() - { - static Encoder instance; - return instance; - } - - Encoder(Encoder const&) = delete; - void operator=(Encoder const&) = delete; - -private: - Encoder(); - int32_t encoder_cache[ENCODER_COUNT] = {0}; - std::chrono::time_point last_read = std::chrono::system_clock::now(); -}; - -#endif // COMPLIB_SERVER_ENCODER_HPP \ No newline at end of file diff --git a/server_prototype/include/errorMessages.hpp b/server_prototype/include/errorMessages.hpp deleted file mode 100644 index 421fd9b..0000000 --- a/server_prototype/include/errorMessages.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// -// Created by KonstantinViesure on 06.03.22. -// - -#ifndef COMPLIB_SERVER_ERRORMESSAGES_HPP -#define COMPLIB_SERVER_ERRORMESSAGES_HPP - -#define ERROR_MESSAGE_UNKNOWN "Header.message_type unknown" - -#endif //COMPLIB_SERVER_ERRORMESSAGES_HPP diff --git a/server_prototype/include/mathUtils.hpp b/server_prototype/include/mathUtils.hpp deleted file mode 100644 index b4eee56..0000000 --- a/server_prototype/include/mathUtils.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef COMPLIB_SERVER_MATHUTILS_HPP -#define COMPLIB_SERVER_MATHUTILS_HPP - -#include - -namespace mathUtils { - inline int int_from_bytes(uint8_t *data, int length) { - int ret = 0; - - int i = 0; - for (int j = length -1; j >= 0; j--) { - ret = ret | (data[i] << (j * 8)); - i++; - } - return ret; - } - - inline void bytes_from_int(int data, int length, uint8_t *result) { - int i = 0; - for (int j = length -1; j >= 0; j--) { - result[i] = ((data >> (j * 8)) & 0xffu); - i++; - } - } -} - -#endif // COMPLIB_SERVER_MATHUTILS_HPP \ No newline at end of file diff --git a/server_prototype/include/messageBuilder.hpp b/server_prototype/include/messageBuilder.hpp deleted file mode 100644 index 599a937..0000000 --- a/server_prototype/include/messageBuilder.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef COMPLIB_SERVER_MESSAGECREATION_HPP -#define COMPLIB_SERVER_MESSAGECREATION_HPP - -#include - -namespace MessageBuilder { - CompLib::Header *header(const std::string &message_type) { - auto header = new CompLib::Header(); - header->set_message_type(message_type); - return header; - } - - CompLib::Status *status(bool successful, const std::string &error_message) { - auto status = new CompLib::Status(); - status->set_successful(successful); - status->set_error_message(error_message); - return status; - } - - CompLib::GenericResponse *genericResponse(bool successful, const std::string &error_message) { - auto genericResponse = new CompLib::GenericResponse(); - genericResponse->set_allocated_header(header(CompLib::GenericResponse::descriptor()->full_name())); - genericResponse->set_allocated_status(status(successful, error_message)); - return genericResponse; - } -} - -#endif //COMPLIB_SERVER_MESSAGECREATION_HPP diff --git a/server_prototype/include/motor.hpp b/server_prototype/include/motor.hpp deleted file mode 100644 index 7ee67b9..0000000 --- a/server_prototype/include/motor.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef COMPLIB_SERVER_MOTOR_HPP -#define COMPLIB_SERVER_MOTOR_HPP - -#include -#include -#include -#include - -#include "include/robot.hpp" - -class Motor { -public: - enum Mode : uint8_t { - COAST = 0, - FORWARD = 1, - BACKWARD = 2, - BREAK = 3, - SERVO = 4, - NONE = 5 - }; - - enum Control : uint8_t { - POWER = 0, - SPEED = 1 - }; - - static Motor& get_instance() - { - static Motor instance; - return instance; - } - - Motor(Motor const&) = delete; - void operator=(Motor const&) = delete; - - void set_speed(uint8_t port, double rpm); - void set_power(uint8_t port, double percent); - static void set_pwm(uint8_t port, uint16_t pwm, Mode mode); - - std::vector get_speed(); - -private: - Motor(); - - // Speed calculation and speed filter - int32_t last_encoder_values[MOTOR_COUNT] = {0}; - double filtered_speeds[MOTOR_COUNT] = {0}; - std::chrono::system_clock::time_point last_time_encoders_read; - - // Speed control - double speed_targets[MOTOR_COUNT] = {0}; - double motor_control_modes[MOTOR_COUNT] = {0}; - std::thread speed_control_thread; - - void reset_speed(); - void speed_control_loop(); - - void _set_power(uint8_t port, double percent); -}; - -#endif // COMPLIB_SERVER_MOTOR_HPP \ No newline at end of file diff --git a/server_prototype/include/pid.hpp b/server_prototype/include/pid.hpp deleted file mode 100644 index 862376b..0000000 --- a/server_prototype/include/pid.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef COMPLIB_SERVER_PID_HPP -#define COMPLIB_SERVER_PID_HPP - -#include - -#define PID_RESET_DELAY_S 0.1L - -class PID -{ -public: - PID(float P, float I, float D, float ramp, float limit); - ~PID() = default; - - float operator() (float setpoint, float process_variable); - void reset(); - - float P = 1; - float I = 0; - float D = 0; - float setpoint_ramp = 0; - float limit = 0; - -protected: - float error_prev = 0; - float setpoint_prev = 0; - float integral_prev = 0; - std::chrono::system_clock::time_point timestamp_prev = std::chrono::high_resolution_clock::now(); -}; - -#endif // COMPLIB_SERVER_PID_HPP \ No newline at end of file diff --git a/server_prototype/include/reset.hpp b/server_prototype/include/reset.hpp deleted file mode 100644 index 3761606..0000000 --- a/server_prototype/include/reset.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef COMPLIB_SERVER_RESET_HPP -#define COMPLIB_SERVER_RESET_HPP - -#include - -#define RESET_PIN 23 -#define BOOT_PIN 17 - -#define RESET_SLEEP_TIME_US 1000 * 100 -#define RESET_STARTUP_SLEEP_TIME_US 1000 * 500 - -namespace Reset { - void reset_robot() { - gpioInitialise(); - - gpioSetMode(BOOT_PIN, PI_OUTPUT); - gpioSetMode(RESET_PIN, PI_OUTPUT); - - gpioWrite(BOOT_PIN, 0); - gpioWrite(RESET_PIN, 0); - - usleep(RESET_SLEEP_TIME_US); - - gpioWrite(RESET_PIN, 1); - - usleep(RESET_STARTUP_SLEEP_TIME_US); - } -} - - -#endif //COMPLIB_SERVER_RESET_HPP diff --git a/server_prototype/include/robot.hpp b/server_prototype/include/robot.hpp deleted file mode 100644 index 9fea841..0000000 --- a/server_prototype/include/robot.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef COMPLIB_SERVER_ROBOT_HPP -#define COMPLIB_SERVER_ROBOT_HPP - -#define PI 3.14159265359L - -#define WHEEL_CIRCUMFERENCE_MM (71.0L * PI) -#define TICKS_PER_TURN (27.7L * 100.0L) -#define ARBOR_LENGTH_MM 139.0L -#define ARBOR_LENGTH_M (ARBOR_LENGTH_MM / 1000.0L) -#define TICKS_PER_METER (1000.0L / WHEEL_CIRCUMFERENCE_MM * TICKS_PER_TURN) - -#define MOTOR_COUNT 4 -#define MAX_MOTOR_SPEED 65535 - -#define MOTOR_MAX_DELTA_TIME_S 0.1L -#define MOTOR_FILTER_ALPHA 0.05L -#define MOTOR_FILTER_RESET_DELAY_US (1000 * 10) -#define MOTOR_SPEED_CONTROL_RESET_DELAY_S 0.01L -#define MOTOR_SPEED_CONTROL_LOOP_HZ 150 -#define MOTOR_SPEED_CONTROL_KP 0.5L -#define MOTOR_SPEED_CONTROL_KI 5.0L -#define MOTOR_SPEED_CONTROL_KD 0.025L - -#define LEFT_PORT 3 -#define RIGHT_PORT 0 - -#endif // COMPLIB_SERVER_ROBOT_HPP \ No newline at end of file diff --git a/server_prototype/include/spi.hpp b/server_prototype/include/spi.hpp deleted file mode 100644 index c48e5a4..0000000 --- a/server_prototype/include/spi.hpp +++ /dev/null @@ -1,140 +0,0 @@ -#ifndef COMPLIB_SERVER_SPI_HPP -#define COMPLIB_SERVER_SPI_HPP - -#include - -//SPI_MODE_0 (0,0) CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge -//SPI_MODE_1 (0,1) CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge -//SPI_MODE_2 (1,0) CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge -//SPI_MODE_3 (1,1) CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge - -#define SPI_BUFFER_SIZE 20 -#define SPI_SPEED 2000000 // 2 MHz -#define SPI_BITS_PER_WORD 8 - -class Spi { -public: - static Spi& getInstance() - { - static Spi instance; - return instance; - } - - Spi(Spi const&) = delete; - void operator=(Spi const&) = delete; - - int read(uint8_t reg, uint8_t length); - void read_array(uint8_t reg, uint8_t length, uint8_t* data); - - void write(uint8_t reg, uint8_t length, int value); - void write_array(uint8_t reg, uint8_t length, const uint8_t* data); - - enum Register : uint8_t { - IDENTIFICATION_MODEL_ID = 1, - IDENTIFICATION_MODEL_REV_MAJOR = 2, - IDENTIFICATION_MODEL_REV_MINOR = 3, - IDENTIFICATION_MODEL_REV_PATCH = 4, - - // Motor encoder positions - MOTOR_1_POS_B3 = 10, - MOTOR_1_POS_B2 = 11, - MOTOR_1_POS_B1 = 12, - MOTOR_1_POS_B0 = 13, - MOTOR_2_POS_B3 = 14, - MOTOR_2_POS_B2 = 15, - MOTOR_2_POS_B1 = 16, - MOTOR_2_POS_B0 = 17, - MOTOR_3_POS_B3 = 18, - MOTOR_3_POS_B2 = 19, - MOTOR_3_POS_B1 = 20, - MOTOR_3_POS_B0 = 21, - MOTOR_4_POS_B3 = 22, - MOTOR_4_POS_B2 = 23, - MOTOR_4_POS_B1 = 24, - MOTOR_4_POS_B0 = 25, - - // PWM Control Modes - PWM_1_CTRL = 26, - PWM_2_CTRL = 27, - PWM_3_CTRL = 28, - PWM_4_CTRL = 29, - - // Motor pwm speed - MOTOR_1_PWM_H = 30, - MOTOR_1_PWM_L = 31, - MOTOR_2_PWM_H = 32, - MOTOR_2_PWM_L = 33, - MOTOR_3_PWM_H = 34, - MOTOR_3_PWM_L = 35, - MOTOR_4_PWM_H = 36, - MOTOR_4_PWM_L = 37, - - // Servo goal position - SERVO_1_PWM_H = 38, - SERVO_1_PWM_L = 39, - SERVO_2_PWM_H = 40, - SERVO_2_PWM_L = 41, - SERVO_3_PWM_H = 42, - SERVO_3_PWM_L = 43, - SERVO_4_PWM_H = 44, - SERVO_4_PWM_L = 45, - SERVO_5_PWM_H = 46, - SERVO_5_PWM_L = 47, - SERVO_6_PWM_H = 48, - SERVO_6_PWM_L = 49, - SERVO_7_PWM_H = 50, - SERVO_7_PWM_L = 51, - SERVO_8_PWM_H = 52, - SERVO_8_PWM_L = 53, - - // IR Sensor value - IR_1_H = 54, - IR_1_L = 55, - IR_2_H = 56, - IR_2_L = 57, - IR_3_H = 58, - IR_3_L = 59, - IR_4_H = 60, - IR_4_L = 61, - IR_5_H = 62, - IR_5_L = 63, - IR_1_LED = 64, - IR_2_LED = 65, - IR_3_LED = 66, - IR_4_LED = 67, - IR_5_LED = 68, - - // Display registers - DISPLAY_LINE_1_C0 = 69, - DISPLAY_LINE_2_C0 = 85, - DISPLAY_LINE_3_C0 = 101, - DISPLAY_LINE_4_C0 = 117, - - // Motor encoder velocities - MOTOR_1_VEL_H = 118, - MOTOR_1_VEL_L = 119, - MOTOR_2_VEL_H = 120, - MOTOR_2_VEL_L = 121, - MOTOR_3_VEL_H = 122, - MOTOR_3_VEL_L = 123, - MOTOR_4_VEL_H = 124, - MOTOR_4_VEL_L = 125 - }; - -private: - Spi(); - - int spi_file_descriptor{}; - uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0}; - uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0}; - - std::recursive_mutex spi_mutex; - - void transfer(); - void clear_buffers(); - - uint8_t calculate_checksum(uint8_t* data, uint8_t length); -}; - - -#endif //COMPLIB_SERVER_SPI_HPP diff --git a/server_prototype/protos/CompLib.proto b/server_prototype/protos/CompLib.proto deleted file mode 100644 index 77a202d..0000000 --- a/server_prototype/protos/CompLib.proto +++ /dev/null @@ -1,36 +0,0 @@ -syntax = "proto3"; - -package CompLib; - -message Header { - string message_type = 1; -} - -message Status { - bool successful = 1; - string error_message = 2; -} - -message GenericRequest { - Header header = 1; -} - -message GenericResponse { - Header header = 1; - Status status = 2; -} - -message ReadSensorsRequest { - Header header = 1; -} - -message ReadSensorsResponse { - Header header = 1; - Status status = 2; - uint32 ir_1 = 3; - uint32 ir_2 = 4; - uint32 ir_3 = 5; - uint32 ir_4 = 6; - uint32 ir_5 = 7; -} - diff --git a/server_prototype/src/encoder.cpp b/server_prototype/src/encoder.cpp deleted file mode 100644 index 3879b87..0000000 --- a/server_prototype/src/encoder.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "include/encoder.hpp" -#include "include/mathUtils.hpp" -#include "include/spi.hpp" - -std::vector Encoder::read_all() { - std::vector result; - uint8_t result_bytes[ENCODER_COUNT * 4] = {0}; - - Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes); - - for (int i = 0; i < ENCODER_COUNT; i++) { - int32_t encoder_value = mathUtils::int_from_bytes(result_bytes + i * 4, 4); - result.push_back(encoder_value); - encoder_cache[i] = encoder_value; - } - - last_read = std::chrono::system_clock::now(); - return result; -} - -std::vector Encoder::read_all_cached() { - auto last_read_delay_ms = std::chrono::duration_cast( - std::chrono::system_clock::now() - last_read - ).count(); - - if (last_read_delay_ms > ENCODER_CACHE_DURATION_MS) { - return read_all(); - } - return std::vector(std::begin(encoder_cache), std::end(encoder_cache)); -} - -Encoder::Encoder() { - -} \ No newline at end of file diff --git a/server_prototype/src/main.cpp b/server_prototype/src/main.cpp deleted file mode 100644 index f1c4d58..0000000 --- a/server_prototype/src/main.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include -#include -#include -#include -#include -#include "include/mathUtils.hpp" -#include "include/messageBuilder.hpp" -#include "include/errorMessages.hpp" -#include -#include -#include - -#include "include/spi.hpp" -#include "include/reset.hpp" -#include "include/encoder.hpp" -#include "include/motor.hpp" -#include "include/robot.hpp" - -#define SOCKET_PATH "/tmp/compLib" -#define BUFFER_SIZE 64 - -google::protobuf::Message *processMessage(const std::string &serializedMessage) { - CompLib::GenericRequest message; - message.ParseFromString(serializedMessage); - auto messageTypeName = message.header().message_type(); - - if (messageTypeName == CompLib::ReadSensorsRequest::GetDescriptor()->full_name()) { - CompLib::ReadSensorsRequest readSensorsRequest; - readSensorsRequest.ParseFromString(serializedMessage); - } else if (messageTypeName == CompLib::ReadSensorsResponse::GetDescriptor()->full_name()) { - CompLib::ReadSensorsResponse readSensorsResponse; - readSensorsResponse.ParseFromString(serializedMessage); - std::cout << readSensorsResponse.ir_1() << std::endl; - std::cout << readSensorsResponse.ir_2() << std::endl; - std::cout << readSensorsResponse.ir_3() << std::endl; - std::cout << readSensorsResponse.ir_4() << std::endl; - std::cout << readSensorsResponse.ir_5() << std::endl; - } else { - std::cout << messageTypeName << " not found!" << std::endl; - } - - google::protobuf::Message *returnMessage = MessageBuilder::genericResponse(false, ERROR_MESSAGE_UNKNOWN); - return returnMessage; -} - -[[noreturn]] void socketServer() { - struct sockaddr_un socketAddress; - int socketFileDescriptor = socket(AF_UNIX, SOCK_STREAM, 0); - remove(SOCKET_PATH); - - memset(&socketAddress, 0, sizeof(struct sockaddr_un)); - socketAddress.sun_family = AF_UNIX; - strncpy(socketAddress.sun_path, SOCKET_PATH, sizeof(socketAddress.sun_path) - 1); - - if (bind(socketFileDescriptor, (struct sockaddr *) &socketAddress, sizeof(struct sockaddr_un)) == -1) { - exit(-1); - } - - if (listen(socketFileDescriptor, 1) == -1) { - exit(-2); - } - - char readBuffer[BUFFER_SIZE]; - char writeBuffer[BUFFER_SIZE]; - for (;;) { - int clientFileDescriptor = accept(socketFileDescriptor, NULL, NULL); - auto numRead = read(clientFileDescriptor, readBuffer, 1); - std::cout << numRead << std::endl; - - uint8_t messageSize = readBuffer[0]; - std::cout << std::to_string(messageSize) << std::endl; - - numRead = read(clientFileDescriptor, readBuffer, readBuffer[0]); - std::cout << numRead << std::endl; - - std::string stringMessage; - for (int i{}; i < messageSize; i++) { - stringMessage += readBuffer[i]; - } - - auto response = processMessage(stringMessage); - uint8_t responseSize = response->ByteSizeLong(); - writeBuffer[0] = responseSize; - - std::cout << std::to_string(responseSize) << std::endl; - write(clientFileDescriptor, writeBuffer, 1); - - response->SerializeToArray(writeBuffer, BUFFER_SIZE); - write(clientFileDescriptor, writeBuffer, responseSize); - - close(clientFileDescriptor); - } -} - -int main() { - // socketServer(); - Reset::reset_robot(); - spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); - // Spi &spi = Spi::getInstance(); - // int version = spi.read(1, 1); - - // double count = 20000; - // double start_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - // for (int i{0}; i < count; i++){ - // spi.read(1, 1); - // } - // double end_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - - // double kp = 0.5; - // double ki = 5.0; - // double kd = 0.025; - double setpoint = 75; - // double errror_sum = 0; - // double last_error = 0; - // int port = 0; - // long double accumulator = 0; - // long double alpha = 0.2; - // auto last_time = std::chrono::high_resolution_clock::now(); - // auto start_time = last_time; - // int i = 0; - while (1 == 1) { - // setpoint += 0.5; - // auto current_time = std::chrono::high_resolution_clock::now(); - // long double delta_seconds = std::chrono::duration_cast(current_time - last_time).count() / 1000000.0; - // long double delta_seconds_total = std::chrono::duration_cast(current_time - start_time).count() / 1000000.0; - - // last_time = current_time; - - // auto speeds = Motor::get_instance().get_speed(); - // double e = setpoint - speeds.at(0); - // errror_sum += e * delta_seconds; - // double error_diff = (e - last_error) / delta_seconds; - // last_error = e; - // double u = kp * e + errror_sum * ki + error_diff * kd; - - // Motor::power(0, std::min(std::max(-100.0, u), 100.0)); - - // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0)); - - usleep(1000.0 * (1000.0 / 10.0)); - // Motor::get_instance().set_speed(0, -setpoint); - // Motor::get_instance().set_speed(3, setpoint); - // i += 1; - // if (i % 1000 == 0) { - // setpoint *= -1; - // } - auto speeds = Motor::get_instance().get_speed(); - spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint); - - // auto current_time = std::chrono::high_resolution_clock::now(); - // long double delta_seconds = std::chrono::duration_cast(current_time - last_time).count() / 1000000.0; - // last_time = current_time; - - // uint8_t result_bytes[2] = {0}; - // Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, 2, result_bytes); - // int16_t tps = mathUtils::int_from_bytes(result_bytes, 2); - // std::vector current_ticks = Encoder::read_all(); - - // long double delta_ticks = current_ticks.at(0) - last_ticks; - - // spdlog::info("{} {}", tps, delta_ticks / delta_seconds); - - // last_ticks = current_ticks.at(0); - } - return 0; -} - diff --git a/server_prototype/src/motor.cpp b/server_prototype/src/motor.cpp deleted file mode 100644 index 0176b5b..0000000 --- a/server_prototype/src/motor.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include -#include -#include -#include - -#include "include/motor.hpp" -#include "include/spi.hpp" -#include "include/robot.hpp" -#include "include/encoder.hpp" -#include "include/mathUtils.hpp" -#include "include/pid.hpp" - -void check_port(uint8_t port) { - if (port >= MOTOR_COUNT) { - throw std::invalid_argument("Invalid motor port specified"); - } -} - -Motor::Motor() { - reset_speed(); - - speed_control_thread = std::thread(&Motor::speed_control_loop, this); - speed_control_thread.detach(); -} - -void Motor::set_speed(uint8_t port, double rpm) { - check_port(port); - - speed_targets[port] = rpm; - motor_control_modes[port] = Motor::Control::SPEED; -} - -void Motor::set_power(uint8_t port, double percent) { - motor_control_modes[port] = Motor::Control::POWER; - _set_power(port, percent); -} - -void Motor::_set_power(uint8_t port, double percent) { - check_port(port); - if (abs(percent) > 100) { - throw std::invalid_argument("Invalid motor percent specified. Should be -100 <= percent <= 100"); - } - - Mode mode = Motor::Mode::COAST; - if (percent < 0) { - mode = Motor::Mode::BACKWARD; - } else if (percent > 0) { - mode = Motor::Mode::FORWARD; - } - - uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0); - Motor::set_pwm(port, pwm, mode); -} - -void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) { - check_port(port); - if (mode > Motor::Mode::BREAK) { - throw std::invalid_argument("Invalid motor mode specified"); - } - - if (port == 0) { - Spi::getInstance().write(Spi::Register::MOTOR_1_PWM_H, 2, pwm); - Spi::getInstance().write(Spi::Register::PWM_1_CTRL, 1, mode); - } else if (port == 1) { - Spi::getInstance().write(Spi::Register::MOTOR_2_PWM_H, 2, pwm); - Spi::getInstance().write(Spi::Register::PWM_2_CTRL, 1, mode); - } else if (port == 2) { - Spi::getInstance().write(Spi::Register::MOTOR_3_PWM_H, 2, pwm); - Spi::getInstance().write(Spi::Register::PWM_3_CTRL, 1, mode); - } else if (port == 3) { - Spi::getInstance().write(Spi::Register::MOTOR_4_PWM_H, 2, pwm); - Spi::getInstance().write(Spi::Register::PWM_4_CTRL, 1, mode); - } -} - -std::vector Motor::get_speed() { - // auto current_time = std::chrono::high_resolution_clock::now(); - // long double delta_seconds = std::chrono::duration_cast(current_time - last_time_encoders_read).count() / 1000000.0; - - // last_time_encoders_read = current_time; - - // if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) { - // reset_speed(); - // usleep(MOTOR_FILTER_RESET_DELAY_US); - // return Motor::get_speed(); - // } - - // std::vector current_ticks = Encoder::read_all(); - // std::vector velocities_rpm; - // for (int i = 0; i < MOTOR_COUNT; i++) { - // long double delta_ticks = current_ticks.at(i) - last_encoder_values[i]; - - // long double velocity_rpm = (delta_ticks / delta_seconds) * 60.0L / TICKS_PER_TURN; - // filtered_speeds[i] = (long double) ((MOTOR_FILTER_ALPHA * velocity_rpm) + (1.0 - MOTOR_FILTER_ALPHA) * filtered_speeds[i]); - // velocities_rpm.push_back(filtered_speeds[i]); - - // last_encoder_values[i] = current_ticks.at(i); - // } - - std::vector velocities_rpm; - uint8_t result_bytes[MOTOR_COUNT * 2] = {0}; - - Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, MOTOR_COUNT * 2, result_bytes); - for (int i = 0; i < MOTOR_COUNT; i++) { - int16_t ticks_per_second = mathUtils::int_from_bytes(result_bytes + i * 2, 2); - velocities_rpm.push_back(ticks_per_second * 60.0L / TICKS_PER_TURN); - } - - return velocities_rpm; -} - -void Motor::reset_speed() { - last_time_encoders_read = std::chrono::high_resolution_clock::now(); - - auto current_ticks = Encoder::get_instance().read_all_cached(); - for (int i = 0; i < MOTOR_COUNT; i++) { - filtered_speeds[i] = 0; - last_encoder_values[i] = current_ticks.at(i); - } -} - -void Motor::speed_control_loop() { - std::vector pids; - - for (int i = 0; i < MOTOR_COUNT; i++) { - pids.push_back( - PID{MOTOR_SPEED_CONTROL_KP, MOTOR_SPEED_CONTROL_KI, MOTOR_SPEED_CONTROL_KD, 100.0, 100.0} - ); - } - - while (1 == 1) { - float sleep_time = std::round(1000.0 / MOTOR_SPEED_CONTROL_LOOP_HZ); - std::this_thread::sleep_for(std::chrono::microseconds((int) sleep_time * 1000)); - - bool should_control_speed = false; - for (int i = 0; i < MOTOR_COUNT; i++) { - if (motor_control_modes[i] == Motor::Control::SPEED) { - should_control_speed = true; - } - } - - if (!should_control_speed) { - // spdlog::info("continue"); - continue; - } - - auto speeds = Motor::get_instance().get_speed(); - for (int i = 0; i < MOTOR_COUNT; i++) { - if (motor_control_modes[i] == Motor::Control::SPEED) { - float power = pids.at(i)(speed_targets[i], speeds.at(i)); - _set_power(i, power); - } - } - } -} \ No newline at end of file diff --git a/server_prototype/src/pid.cpp b/server_prototype/src/pid.cpp deleted file mode 100644 index f795922..0000000 --- a/server_prototype/src/pid.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "include/pid.hpp" -#include -#include -#include - -PID::PID(float P, float I, float D, float ramp, float limit) - : P(P) - , I(I) - , D(D) - , setpoint_ramp(ramp) - , limit(limit) - , error_prev(0.0f) - , setpoint_prev(0.0f) - , integral_prev(0.0f) -{ - timestamp_prev = std::chrono::high_resolution_clock::now(); -} - -float PID::operator() (float setpoint, float process_variable){ - // calculate the time from the last call - auto timestamp_now = std::chrono::high_resolution_clock::now(); - long double delta_seconds = std::chrono::duration_cast(timestamp_now - timestamp_prev).count() / 1000000.0; - - if (delta_seconds > PID_RESET_DELAY_S) { - reset(); - delta_seconds = std::chrono::duration_cast(timestamp_now - timestamp_prev).count() / 1000000.0; - } - - if(setpoint_ramp > 0){ - float setpoint_rate = (setpoint - setpoint_prev) / delta_seconds; - if (setpoint_rate > setpoint_ramp) { - setpoint = setpoint_prev + setpoint_ramp * delta_seconds; - } else if (setpoint_rate < -setpoint_ramp) { - setpoint = setpoint_prev - setpoint_ramp * delta_seconds; - } - } - - float error = setpoint - process_variable; - - float proportional = P * error; - // Tustin transform of the integral part - float integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev); - float derivative = D * ((error - error_prev) / delta_seconds); - - float output = proportional + integral + derivative; - - // antiwindup - limit the output variable - output = std::min(std::max(-limit, output), limit); - // spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds); - - integral_prev = integral; - setpoint_prev = setpoint; - error_prev = error; - timestamp_prev = timestamp_now; - return output; -} - -void PID::reset() { - spdlog::info("RESET"); - integral_prev = 0; - setpoint_prev = 0; - error_prev = 0; - timestamp_prev = std::chrono::high_resolution_clock::now(); -} \ No newline at end of file diff --git a/server_prototype/src/spi.cpp b/server_prototype/src/spi.cpp deleted file mode 100644 index 9314905..0000000 --- a/server_prototype/src/spi.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include -#include -#include - -#include "include/spi.hpp" -#include "include/mathUtils.hpp" - -void check_for_error(int error_code, std::string error_message) { - if (error_code < 0) { - spdlog::error(error_message); - exit(1); - } -} - -Spi::Spi() { - int spi_mode = SPI_MODE_0; - int spi_bits_per_workd = SPI_BITS_PER_WORD; - int spi_speed = SPI_SPEED; - - check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); -} - -void Spi::clear_buffers() { - memset(tx_buffer, 0, SPI_BUFFER_SIZE); - memset(rx_buffer, 0, SPI_BUFFER_SIZE); -} - -uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) { - int sum = 0; - for (int i = 0; i < length; i++) { - sum += data[i]; - } - return sum % 256; -} - -int Spi::read(uint8_t reg, uint8_t length) { - std::lock_guard lock(spi_mutex); - - uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; - read_array(reg, length, read_buffer); - - return mathUtils::int_from_bytes(read_buffer, length); -} - -void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) { - std::lock_guard lock(spi_mutex); - - clear_buffers(); - tx_buffer[0] = 0; - tx_buffer[1] = reg; - tx_buffer[2] = length; - - transfer(); - - uint8_t checksum = calculate_checksum(rx_buffer, length + 3); - if (checksum != rx_buffer[length + 3]) { - spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum); - } - - memcpy(data, rx_buffer + 2, length); -} - -void Spi::write(uint8_t reg, uint8_t length, int value) { - std::lock_guard lock(spi_mutex); - - uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; - mathUtils::bytes_from_int(value, length, write_buffer); - write_array(reg, length, write_buffer); -} - -void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) { - std::lock_guard lock(spi_mutex); - - clear_buffers(); - tx_buffer[0] = 1; - tx_buffer[1] = reg; - tx_buffer[2] = length; - memcpy(tx_buffer + 3, data, length); - - transfer(); -} - -void Spi::transfer() { - std::lock_guard lock(spi_mutex); - - struct spi_ioc_transfer spi; - memset(&spi, 0, sizeof(spi)); - - spi.tx_buf = (unsigned long) tx_buffer; - spi.rx_buf = (unsigned long) rx_buffer; - spi.len = SPI_BUFFER_SIZE; - spi.delay_usecs = 0; - spi.speed_hz = SPI_SPEED; - spi.bits_per_word = SPI_BITS_PER_WORD; - spi.cs_change = 0; - - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - - if (tx_buffer[1] != rx_buffer[1]) { - spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); - } -} diff --git a/server_v2/.gitignore b/server_v2/.gitignore deleted file mode 100644 index 8e2c66b..0000000 --- a/server_v2/.gitignore +++ /dev/null @@ -1,108 +0,0 @@ - -# Created by https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode -# Edit at https://www.toptal.com/developers/gitignore?templates=c++,macos,linux,visualstudiocode - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json -!.vscode/*.code-snippets - -# Local History for Visual Studio Code -.history/ - -# Built Visual Studio Code Extensions -*.vsix - -### VisualStudioCode Patch ### -# Ignore all local history of files -.history -.ionide - -# Support for Project snippet scope - -# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode - - -cmake-build* -build diff --git a/server_v2/.idea/.gitignore b/server_v2/.idea/.gitignore deleted file mode 100644 index 8bf4d45..0000000 --- a/server_v2/.idea/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml -# Datasource local storage ignored files -/dataSources/ -/dataSources.local.xml diff --git a/server_v2/.idea/.name b/server_v2/.idea/.name deleted file mode 100644 index 4e811b7..0000000 --- a/server_v2/.idea/.name +++ /dev/null @@ -1 +0,0 @@ -compLib_server \ No newline at end of file diff --git a/server_v2/.idea/deployment.xml b/server_v2/.idea/deployment.xml deleted file mode 100644 index c9520d5..0000000 --- a/server_v2/.idea/deployment.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/server_v2/.idea/misc.xml b/server_v2/.idea/misc.xml deleted file mode 100644 index 79b3c94..0000000 --- a/server_v2/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/server_v2/.idea/modules.xml b/server_v2/.idea/modules.xml deleted file mode 100644 index fbd90b4..0000000 --- a/server_v2/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/server_v2/.idea/saveactions_settings.xml b/server_v2/.idea/saveactions_settings.xml deleted file mode 100644 index b7b3f34..0000000 --- a/server_v2/.idea/saveactions_settings.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/server_v2/.idea/serialmonitor_settings.xml b/server_v2/.idea/serialmonitor_settings.xml deleted file mode 100644 index eff936b..0000000 --- a/server_v2/.idea/serialmonitor_settings.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/server_v2/.idea/server.iml b/server_v2/.idea/server.iml deleted file mode 100644 index f08604b..0000000 --- a/server_v2/.idea/server.iml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/server_v2/.idea/vcs.xml b/server_v2/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/server_v2/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt deleted file mode 100755 index 3f9b80a..0000000 --- a/server_v2/CMakeLists.txt +++ /dev/null @@ -1,95 +0,0 @@ -cmake_minimum_required(VERSION 3.16) - -set(PROJECT_NAME compLib_server) -project(${PROJECT_NAME}) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) - -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif () - -find_package(Protobuf REQUIRED) -find_package(spdlog REQUIRED) - -if (UNIX AND ${CMAKE_SYSTEM_PROCESSOR} STREQUAL armv7l) - message("Running on Raspberry Pi") - add_definitions(-DIS_RASPI) - set(CMAKE_CXX_FLAGS_RELEASE "-O3") - - find_package(pigpio REQUIRED) - set(LIBRARIES "pigpio" "spdlog::spdlog" "systemd") - set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") - - set(IS_RASPI true) -endif () - -if (NOT IS_RASPI) - message("NOT Running on RaspberryPi") - set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3") - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3") - - set(LIBRARIES "spdlog::spdlog") -endif () - -message("CMAKE_SYSTEM_PROCESSOR = ${CMAKE_SYSTEM_PROCESSOR}") - -set(PROTO_FILES - protos/CompLib.proto - ) - -set(SRC_FILES - src/main.cpp - src/spi.cpp - src/IRSensors.cpp - src/Cache.cpp - src/Motors.cpp - src/Encoders.cpp - src/ClosedLoopMotorController.cpp - src/PID.cpp - src/OdometryController.cpp - src/Odometry.cpp - src/communication/MessageProcessor.cpp - src/communication/MessageBuilder.cpp - src/communication/UnixSocketServer.cpp - src/communication/TCPSocketServer.cpp - src/GoToController.cpp - src/HealthChecker.cpp - src/Display.cpp - src/IPWritingService.cpp) - -set(HDR_FILES - include/spi.hpp - include/reset.hpp - include/mathUtils.hpp - include/networkUtils.hpp - include/IRSensors.hpp - include/Robot.hpp - include/Cache.hpp - include/Motors.hpp - include/Encoders.hpp - include/ClosedLoopMotorController.hpp - include/PID.hpp - include/OdometryController.hpp - include/Odometry.hpp - include/communication/MessageProcessor.hpp - include/communication/MessageBuilder.hpp - include/communication/UnixSocketServer.hpp - include/communication/TCPSocketServer.hpp - include/GoToController.hpp - include/HealthChecker.hpp - include/Display.hpp - include/IPWritingService.hpp) - -include_directories(third_party/asio) - -PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HDR ${PROTO_FILES}) -add_executable(${PROJECT_NAME} ${SRC_FILES} ${HDR_FILES} ${PROTO_SRC} ${PROTO_HDR} ${PROTO_FILES}) -target_include_directories(${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - PUBLIC ${PROTOBUF_INCLUDE_DIRS} - ) -target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} ${LIBRARIES}) diff --git a/server_v2/build_deb.sh b/server_v2/build_deb.sh deleted file mode 100755 index fcbcc08..0000000 --- a/server_v2/build_deb.sh +++ /dev/null @@ -1,24 +0,0 @@ -export VERSION="1.0.0-3" - -fpm \ - -s dir -t deb \ - -m '"Joel Klimont" ' \ - --license 'proprietary' \ - --description 'Library for robot used in the competition' \ - --url "https://github.com/F-WuTS/compLIB" \ - --deb-priority "optional" \ - --deb-systemd "compsrv.service" \ - --after-install postinstall.sh \ - --after-upgrade postinstall.sh \ - --name compsrv \ - --version $VERSION \ - --architecture all \ - -d "libprotobuf23" \ - -d "protobuf-compiler" \ - -d "python3-protobuf" \ - -d "pigpio" \ - -d "libsystemd-dev" \ - ./test_bin/compsrv=/usr/bin/compsrv - -# MUST BE RUN IN BASH, zsh has 'ar' as an alias to 'apt' -# ar -xv compsrv-1.0.0-0-all.deb diff --git a/server_v2/cmake/modules/Findpigpio.cmake b/server_v2/cmake/modules/Findpigpio.cmake deleted file mode 100644 index 1308822..0000000 --- a/server_v2/cmake/modules/Findpigpio.cmake +++ /dev/null @@ -1,31 +0,0 @@ -################################################################################ -### Find the pigpio shared libraries. -################################################################################ - -# Find the path to the pigpio includes. -find_path(pigpio_INCLUDE_DIR - NAMES pigpio.h pigpiod_if.h pigpiod_if2.h - HINTS /usr/local/include) - -# Find the pigpio libraries. -find_library(pigpio_LIBRARY - NAMES libpigpio.so - HINTS /usr/local/lib) -find_library(pigpiod_if_LIBRARY - NAMES libpigpiod_if.so - HINTS /usr/local/lib) -find_library(pigpiod_if2_LIBRARY - NAMES libpigpiod_if2.so - HINTS /usr/local/lib) - -# Set the pigpio variables to plural form to make them accessible for -# the paramount cmake modules. -set(pigpio_INCLUDE_DIRS ${pigpio_INCLUDE_DIR}) -set(pigpio_INCLUDES ${pigpio_INCLUDE_DIR}) - -# Handle REQUIRED, QUIET, and version arguments -# and set the _FOUND variable. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(pigpio - DEFAULT_MSG - pigpio_INCLUDE_DIR pigpio_LIBRARY pigpiod_if_LIBRARY pigpiod_if2_LIBRARY) \ No newline at end of file diff --git a/server_v2/compsrv.service b/server_v2/compsrv.service deleted file mode 100755 index 94548d7..0000000 --- a/server_v2/compsrv.service +++ /dev/null @@ -1,11 +0,0 @@ -[Unit] -Description=Complib backend control server -[Service] -ExecStart=/usr/bin/compsrv -Environment="debug=False" -Restart=always -RestartSec=5 -Type=notify -[Install] -Alias=compsrv -WantedBy=default.target \ No newline at end of file diff --git a/server_v2/compsrv_1.0.0-3_all.deb b/server_v2/compsrv_1.0.0-3_all.deb deleted file mode 100644 index d8f9d01..0000000 Binary files a/server_v2/compsrv_1.0.0-3_all.deb and /dev/null differ diff --git a/server_v2/include/Cache.hpp b/server_v2/include/Cache.hpp deleted file mode 100644 index c4d4dc9..0000000 --- a/server_v2/include/Cache.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef COMPLIB_SERVER_CACHE_HPP -#define COMPLIB_SERVER_CACHE_HPP - -#include - -class Cache { -public: - explicit Cache(int rate_hz); - - bool is_expired(); - - void update(); - -private: - - typedef std::chrono::steady_clock clock; - - std::chrono::microseconds cache_duration{}; - std::chrono::time_point last_update{}; -}; - - -#endif //COMPLIB_SERVER_CACHE_HPP diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp deleted file mode 100644 index 23865dd..0000000 --- a/server_v2/include/ClosedLoopMotorController.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP -#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP - - -#include -#include -#include -#include - -#include "include/PID.hpp" -#include "include/Robot.hpp" - -class ClosedLoopMotorController { -public: - static ClosedLoopMotorController &getInstance() { - static ClosedLoopMotorController instance; - return instance; - } - - ClosedLoopMotorController(ClosedLoopMotorController const &) = delete; - - void operator=(ClosedLoopMotorController const &) = delete; - - void set_power(uint8_t port, double power); - - void set_speed(uint8_t port, double speed_rad_s); - - void stop_all(); - - void generate_step_response_data(); - - void generate_tuned_step_response_data(); - - void calibrate_wheel_ticks(double turns, double ticks_per_turn); - -private: - enum ControlMode : uint8_t { - NONE = 0, - POWER = 1, - SPEED = 2 - }; - - ClosedLoopMotorController(); - - [[noreturn]] void speed_control_loop(); - - std::array pids; - std::array control_modes{ControlMode::NONE}; - std::array speed_targets{0}; - - std::thread speed_control_thread; - std::recursive_mutex speed_control_mutex; -}; - - -#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP diff --git a/server_v2/include/Display.hpp b/server_v2/include/Display.hpp deleted file mode 100644 index bb40622..0000000 --- a/server_v2/include/Display.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef COMPLIB_SERVER_DISPLAY_HPP -#define COMPLIB_SERVER_DISPLAY_HPP - -#include -#include - -class Display { -public: - static void write(uint8_t line, std::string text); - -private: - - Display() = default; -}; - -#endif //COMPLIB_SERVER_DISPLAY_HPP diff --git a/server_v2/include/Encoders.hpp b/server_v2/include/Encoders.hpp deleted file mode 100644 index 5c8eb58..0000000 --- a/server_v2/include/Encoders.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef COMPLIB_SERVER_ENCODERS_HPP -#define COMPLIB_SERVER_ENCODERS_HPP - -#include -#include -#include "include/Cache.hpp" -#include "include/Robot.hpp" - -class Encoders { -public: - static Encoders &getInstance() { - static Encoders instance; - return instance; - } - - Encoders(Encoders const &) = delete; - - void operator=(Encoders const &) = delete; - - std::array get_positions(); - - std::array get_velocities_rad_s(); - - std::array get_velocities_ticks_s(); - -private: - Encoders() = default; - - Cache position_cache{ROBOT_ENCODER_RATE_HZ}; - Cache velocity_cache{ROBOT_ENCODER_RATE_HZ}; - Cache ticks_cache{ROBOT_ENCODER_RATE_HZ}; - - std::array cached_positions = {0}; - std::array cached_velocities_rad_s = {0}; - std::array cached_velocities_ticks_s = {0}; -}; - - -#endif //COMPLIB_SERVER_ENCODERS_HPP diff --git a/server_v2/include/GoToController.hpp b/server_v2/include/GoToController.hpp deleted file mode 100644 index bec969b..0000000 --- a/server_v2/include/GoToController.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef COMPLIB_SERVER_GOTOCONTROLLER_HPP -#define COMPLIB_SERVER_GOTOCONTROLLER_HPP - -class GoToController { -public: - - static void drive_distance(double distance_m, double v_ms); - - static void turn_degrees(double angle_deg, double v_rad_s); - - static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s); - -private: - GoToController() = default; -}; - - -#endif //COMPLIB_SERVER_GOTOCONTROLLER_HPP diff --git a/server_v2/include/HealthChecker.hpp b/server_v2/include/HealthChecker.hpp deleted file mode 100644 index 15e7784..0000000 --- a/server_v2/include/HealthChecker.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef COMPLIB_SERVER_HEALTHCHECKER_HPP -#define COMPLIB_SERVER_HEALTHCHECKER_HPP - -#include -#include - -#define HEALTH_CHECK_TIMEOUT_MS 500 - -class HealthChecker { -public: - static HealthChecker &getInstance() { - static HealthChecker instance; - return instance; - } - - HealthChecker(HealthChecker const &) = delete; - - void operator=(HealthChecker const &) = delete; - - void update(); - -private: - HealthChecker(); - - [[noreturn]] void check_loop(); - - typedef std::chrono::steady_clock clock; - - std::thread check_thread; - std::chrono::time_point last_update; - bool stop_sent = false; -}; - - -#endif //COMPLIB_SERVER_HEALTHCHECKER_HPP diff --git a/server_v2/include/IPWritingService.hpp b/server_v2/include/IPWritingService.hpp deleted file mode 100644 index 0978710..0000000 --- a/server_v2/include/IPWritingService.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef COMPLIB_SERVER_IPWRITINGSERVICE_HPP -#define COMPLIB_SERVER_IPWRITINGSERVICE_HPP - -#include - -class IPWritingService { -public: - IPWritingService(); - -private: - [[noreturn]] void writing_loop(); - - std::thread writing_thread; -}; - -#endif //COMPLIB_SERVER_IPWRITINGSERVICE_HPP diff --git a/server_v2/include/IRSensors.hpp b/server_v2/include/IRSensors.hpp deleted file mode 100644 index b41374d..0000000 --- a/server_v2/include/IRSensors.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef COMPLIB_SERVER_IRSENSORS_HPP -#define COMPLIB_SERVER_IRSENSORS_HPP - -#include -#include - -#include "include/Cache.hpp" -#include "include/Robot.hpp" - -class IRSensors { -public: - static IRSensors &getInstance() { - static IRSensors instance; - return instance; - } - - IRSensors(IRSensors const &) = delete; - - void operator=(IRSensors const &) = delete; - - static void enable(); - - static void disable(); - - std::array read(); - -private: - - IRSensors() = default; - - Cache cache{ROBOT_IR_RATE_HZ}; - - std::array cached_values = {0}; -}; - - -#endif //COMPLIB_SERVER_IRSENSORS_HPP diff --git a/server_v2/include/Motors.hpp b/server_v2/include/Motors.hpp deleted file mode 100644 index 7688804..0000000 --- a/server_v2/include/Motors.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef COMPLIB_SERVER_MOTORS_HPP -#define COMPLIB_SERVER_MOTORS_HPP - -#include - -class Motors { -public: - enum Mode : uint8_t { - COAST = 0, - FORWARD = 1, - BACKWARD = 2, - BREAK = 3, - SERVO = 4, - NONE = 5 - }; - - static void set_power(uint8_t port, double percent); - - static void set_pwm(uint8_t port, uint16_t pwm, Mode mode); - -private: - - Motors() = default; -}; - -#endif //COMPLIB_SERVER_MOTORS_HPP diff --git a/server_v2/include/Odometry.hpp b/server_v2/include/Odometry.hpp deleted file mode 100644 index cc51a01..0000000 --- a/server_v2/include/Odometry.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef COMPLIB_SERVER_ODOMETRY_HPP -#define COMPLIB_SERVER_ODOMETRY_HPP - -class Odometry { - -public: - Odometry(); - - Odometry(double x_position, double y_position, double angular_orientation); - - double get_x_position() const; - - double get_y_position() const; - - double get_angular_orientation() const; - - -private: - double x_position; - double y_position; - double angular_orientation; -}; - -#endif //COMPLIB_SERVER_ODOMETRY_HPP diff --git a/server_v2/include/OdometryController.hpp b/server_v2/include/OdometryController.hpp deleted file mode 100644 index f588cae..0000000 --- a/server_v2/include/OdometryController.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP -#define COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP - - -#include -#include -#include - -#include "include/Odometry.hpp" - -class OdometryController { -public: - static OdometryController &getInstance() { - static OdometryController instance; - return instance; - } - - OdometryController(OdometryController const &) = delete; - - void operator=(OdometryController const &) = delete; - - void enable(); - - void disable(); - - void reset(); - - Odometry get(); - - [[nodiscard]] bool is_enabled() const; - -private: - typedef std::chrono::steady_clock clock; - - OdometryController(); - - std::thread odometry_thread; - std::recursive_mutex odometry_mutex; - - [[noreturn]] void odometry_loop(); - - Odometry current_odometry{}; - bool enabled = false; - double last_position_left{0}; - double last_position_right{0}; - std::chrono::time_point last_run; - -}; - - -#endif //COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP diff --git a/server_v2/include/PID.hpp b/server_v2/include/PID.hpp deleted file mode 100644 index 2422a20..0000000 --- a/server_v2/include/PID.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef COMPLIB_SERVER_PID_HPP -#define COMPLIB_SERVER_PID_HPP - -#include - -class PID { -public: - PID(); - - PID(double P, double I, double D, double ramp, double limit); - - ~PID() = default; - - double operator()(double setpoint, double process_variable); - - double P = 1; - double I = 0; - double D = 0; - double setpoint_ramp = 0; - double limit = 0; - -private: - typedef std::chrono::steady_clock clock; -# - - double error_prev = 0; - double setpoint_prev = 0; - double integral_prev = 0; - clock::time_point timestamp_prev = clock::now(); -}; - -#endif // COMPLIB_SERVER_PID_HPP \ No newline at end of file diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp deleted file mode 100644 index a9593f9..0000000 --- a/server_v2/include/Robot.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef COMPLIB_SERVER_ROBOT_HPP -#define COMPLIB_SERVER_ROBOT_HPP - -#include - -#define ROBOT_IR_SENSOR_COUNT 5 -#define ROBOT_IR_RATE_HZ 250 - -#define ROBOT_MOTOR_COUNT 4 -#define ROBOT_MOTOR_SPEED_CONTROL_KP 15.0 -#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0 -#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.0 -#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI -#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250 - -#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250 -#define ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT 3 -#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT 0 -#define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0 -#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0 - -#define ROBOT_ENCODER_RATE_HZ 500 -#define ROBOT_ENCODER_MAX_CHANGE_RAD_S M_PI_4 - -#define ROBOT_WHEEL_RADIUS_MM 35.5 -#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0) -#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2.0 * ROBOT_WHEEL_RADIUS_MM * M_PI) -#define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI) -#define ROBOT_TICKS_PER_TURN 2770.0 -#define ROBOT_ARBOR_LENGTH_MM 140.0 -#define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0) -#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN) - -#define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1 - -#define ROBOT_GO_TO_DISTANCE_RATE_HZ 200 -#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 -#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0 -#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target -#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.075 - -#define ROBOT_GO_TO_ANGLE_RATE_HZ 200 -#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target -#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.9 -#define ROBOT_GO_TO_ANGLE_MIN_VEL M_PI_2 - -#endif //COMPLIB_SERVER_ROBOT_HPP diff --git a/server_v2/include/communication/MessageBuilder.hpp b/server_v2/include/communication/MessageBuilder.hpp deleted file mode 100644 index 37106bc..0000000 --- a/server_v2/include/communication/MessageBuilder.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef COMPLIB_SERVER_MESSAGEBUILDER_HPP -#define COMPLIB_SERVER_MESSAGEBUILDER_HPP - -#include -#include - -#include "CompLib.pb.h" -#include "include/Robot.hpp" - -class MessageBuilder { -public: - static CompLib::Header *header(const std::string &message_type); - - static CompLib::Status *status(bool successful, const std::string &error_message); - - static CompLib::GenericResponse *generic_response(bool successful, const std::string &error_message); - - static CompLib::Status *default_successful_status(); - - static CompLib::GenericResponse *default_successful_generic_response(); - - static CompLib::EncoderReadPositionsResponse *encoder_read_positions_response(std::array positions); - - static CompLib::EncoderReadVelocitiesResponse *encoder_read_velocities_response(std::array velocities); - - static CompLib::IRSensorsReadAllResponse *ir_sensors_read_all_response(std::array data); - -}; - - -#endif //COMPLIB_SERVER_MESSAGEBUILDER_HPP diff --git a/server_v2/include/communication/MessageProcessor.hpp b/server_v2/include/communication/MessageProcessor.hpp deleted file mode 100644 index 6b02888..0000000 --- a/server_v2/include/communication/MessageProcessor.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef COMPLIB_SERVER_MESSAGEPROCESSOR_HPP -#define COMPLIB_SERVER_MESSAGEPROCESSOR_HPP - -#include - -#define ERROR_MESSAGE_HEADER_UNKNOWN "Header.message_type unknown" - -class MessageProcessor { -public: - static google::protobuf::Message *process_message(const std::string &serializedMessage); - -}; - - -#endif //COMPLIB_SERVER_MESSAGEPROCESSOR_HPP \ No newline at end of file diff --git a/server_v2/include/communication/TCPSocketServer.hpp b/server_v2/include/communication/TCPSocketServer.hpp deleted file mode 100644 index 83f854e..0000000 --- a/server_v2/include/communication/TCPSocketServer.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef COMPLIB_SERVER_TCPSOCKETSERVER_HPP -#define COMPLIB_SERVER_TCPSOCKETSERVER_HPP - -#include - -#define SOCKET_BUFFER_SIZE 128 - -class TCPSocketServer { -public: - TCPSocketServer(); - -private: - [[noreturn]] void server_loop(); - - std::thread server_thread; -}; - - -#endif //COMPLIB_SERVER_TCPSOCKETSERVER_HPP diff --git a/server_v2/include/communication/UnixSocketServer.hpp b/server_v2/include/communication/UnixSocketServer.hpp deleted file mode 100644 index bae3642..0000000 --- a/server_v2/include/communication/UnixSocketServer.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef COMPLIB_SERVER_UNIXSOCKETSERVER_HPP -#define COMPLIB_SERVER_UNIXSOCKETSERVER_HPP - -#include - -#define SOCKET_PATH "/tmp/compLib" -#define SOCKET_BUFFER_SIZE 128 - -class UnixSocketServer { -public: - UnixSocketServer(); - -private: - [[noreturn]] void server_loop(); - - std::thread server_thread; -}; - - -#endif //COMPLIB_SERVER_UNIXSOCKETSERVER_HPP diff --git a/server_v2/include/mathUtils.hpp b/server_v2/include/mathUtils.hpp deleted file mode 100644 index 897fb20..0000000 --- a/server_v2/include/mathUtils.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef COMPLIB_SERVER_MATHUTILS_HPP -#define COMPLIB_SERVER_MATHUTILS_HPP - -#include -#include - -namespace mathUtils { - inline int int_from_bytes(uint8_t *data, int length) { - int ret = 0; - - int i = 0; - for (int j = length - 1; j >= 0; j--) { - ret = ret | (data[i] << (j * 8)); - i++; - } - return ret; - } - - template - inline T from_bytes(const uint8_t *data, int length) { - T ret = 0; - - int i = 0; - for (int j = length - 1; j >= 0; j--) { - ret = ret | (data[i] << (j * 8)); - i++; - } - return ret; - } - - inline void bytes_from_int(int data, int length, uint8_t *result) { - int i = 0; - for (int j = length - 1; j >= 0; j--) { - result[i] = ((data >> (j * 8)) & 0xffu); - i++; - } - } - - template - inline T wrap_angle_to_pi(T angle) { - return atan2(sin(angle), cos(angle)); - } - - template - inline T limit_min(T val, T min) { - if (val < 0.0 && val > -min) { - return -min; - } else if (val > 0.0 && val < min) { - return min; - } else if (val == 0) { - return min; - } - return val; - } - - template - inline T limit_max(T val, T min, T max) { - return fmax(min, fmin(max, val)); - } -} - -#endif // COMPLIB_SERVER_MATHUTILS_HPP \ No newline at end of file diff --git a/server_v2/include/networkUtils.hpp b/server_v2/include/networkUtils.hpp deleted file mode 100644 index 561127f..0000000 --- a/server_v2/include/networkUtils.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef COMPLIB_SERVER_NETWORKUTILS_H -#define COMPLIB_SERVER_NETWORKUTILS_H - -#include -#include -#include -#include "spdlog/spdlog.h" - -#include -#include - -namespace networkUtils { - inline std::tuple get_current_host_and_ip() { - struct ifaddrs *ifaddr, *ifa; - int family, s; - char host[NI_MAXHOST]; - - if (getifaddrs(&ifaddr) == -1) { - spdlog::error("getifaddrs failed"); - freeifaddrs(ifaddr); - return std::tuple("", ""); - } - - for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { - if (ifa->ifa_addr == NULL) - continue; - - family = ifa->ifa_addr->sa_family; - - if (family == AF_INET) { - s = getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); - if (s != 0) { - spdlog::error("getnameinfo() failed: {}", gai_strerror(s)); - freeifaddrs(ifaddr); - return std::tuple("", ""); - } - - if (std::string(ifa->ifa_name) != "lo") { - freeifaddrs(ifaddr); - return std::tuple(ifa->ifa_name, std::string(host)); - } - } - } - - return std::tuple("", ""); - } -} - -#endif //COMPLIB_SERVER_NETWORKUTILS_H diff --git a/server_v2/include/reset.hpp b/server_v2/include/reset.hpp deleted file mode 100644 index b8e9cc3..0000000 --- a/server_v2/include/reset.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef COMPLIB_SERVER_RESET_HPP -#define COMPLIB_SERVER_RESET_HPP - -#define RESET_PIN 23 -#define BOOT_PIN 17 - -#define RESET_SLEEP_TIME_US 1000 * 100 -#define RESET_STARTUP_SLEEP_TIME_US 1000 * 500 - -namespace Reset { -#ifdef IS_RASPI - -#include -#include - - void reset_robot() { - gpioInitialise(); - - gpioSetMode(BOOT_PIN, PI_OUTPUT); - gpioSetMode(RESET_PIN, PI_OUTPUT); - - gpioWrite(BOOT_PIN, 0); - gpioWrite(RESET_PIN, 0); - - usleep(RESET_SLEEP_TIME_US); - - gpioWrite(RESET_PIN, 1); - - usleep(RESET_STARTUP_SLEEP_TIME_US); - } - -#else - - void reset_robot() { - } - -#endif -} - - -#endif //COMPLIB_SERVER_RESET_HPP diff --git a/server_v2/include/spi.hpp b/server_v2/include/spi.hpp deleted file mode 100644 index 1aa08ed..0000000 --- a/server_v2/include/spi.hpp +++ /dev/null @@ -1,145 +0,0 @@ -#ifndef COMPLIB_SERVER_SPI_HPP -#define COMPLIB_SERVER_SPI_HPP - -#include - -//SPI_MODE_0 (0,0) CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge -//SPI_MODE_1 (0,1) CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge -//SPI_MODE_2 (1,0) CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge -//SPI_MODE_3 (1,1) CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge - -#define SPI_BUFFER_SIZE 20 -#define SPI_SPEED 2000000 // 2 MHz -#define SPI_BITS_PER_WORD 8 - -class Spi { -public: - static Spi &getInstance() { - static Spi instance; - return instance; - } - - Spi(Spi const &) = delete; - - void operator=(Spi const &) = delete; - - int read(uint8_t reg, uint8_t length); - - void read_array(uint8_t reg, uint8_t length, uint8_t *data); - - void write(uint8_t reg, uint8_t length, int value); - - void write_array(uint8_t reg, uint8_t length, const uint8_t *data); - - std::array read_version(); - - enum Register : uint8_t { - IDENTIFICATION_MODEL_ID = 1, - IDENTIFICATION_MODEL_REV_MAJOR = 2, - IDENTIFICATION_MODEL_REV_MINOR = 3, - IDENTIFICATION_MODEL_REV_PATCH = 4, - - // Motor encoder positions - MOTOR_1_POS_B3 = 10, - MOTOR_1_POS_B2 = 11, - MOTOR_1_POS_B1 = 12, - MOTOR_1_POS_B0 = 13, - MOTOR_2_POS_B3 = 14, - MOTOR_2_POS_B2 = 15, - MOTOR_2_POS_B1 = 16, - MOTOR_2_POS_B0 = 17, - MOTOR_3_POS_B3 = 18, - MOTOR_3_POS_B2 = 19, - MOTOR_3_POS_B1 = 20, - MOTOR_3_POS_B0 = 21, - MOTOR_4_POS_B3 = 22, - MOTOR_4_POS_B2 = 23, - MOTOR_4_POS_B1 = 24, - MOTOR_4_POS_B0 = 25, - - // PWM Control Modes - PWM_1_CTRL = 26, - PWM_2_CTRL = 27, - PWM_3_CTRL = 28, - PWM_4_CTRL = 29, - - // Motor pwm speed - MOTOR_1_PWM_H = 30, - MOTOR_1_PWM_L = 31, - MOTOR_2_PWM_H = 32, - MOTOR_2_PWM_L = 33, - MOTOR_3_PWM_H = 34, - MOTOR_3_PWM_L = 35, - MOTOR_4_PWM_H = 36, - MOTOR_4_PWM_L = 37, - - // Servo goal position - SERVO_1_PWM_H = 38, - SERVO_1_PWM_L = 39, - SERVO_2_PWM_H = 40, - SERVO_2_PWM_L = 41, - SERVO_3_PWM_H = 42, - SERVO_3_PWM_L = 43, - SERVO_4_PWM_H = 44, - SERVO_4_PWM_L = 45, - SERVO_5_PWM_H = 46, - SERVO_5_PWM_L = 47, - SERVO_6_PWM_H = 48, - SERVO_6_PWM_L = 49, - SERVO_7_PWM_H = 50, - SERVO_7_PWM_L = 51, - SERVO_8_PWM_H = 52, - SERVO_8_PWM_L = 53, - - // IR Sensor value - IR_1_H = 54, - IR_1_L = 55, - IR_2_H = 56, - IR_2_L = 57, - IR_3_H = 58, - IR_3_L = 59, - IR_4_H = 60, - IR_4_L = 61, - IR_5_H = 62, - IR_5_L = 63, - IR_1_LED = 64, - IR_2_LED = 65, - IR_3_LED = 66, - IR_4_LED = 67, - IR_5_LED = 68, - - // Display registers - DISPLAY_LINE_1_C0 = 69, - DISPLAY_LINE_2_C0 = 85, - DISPLAY_LINE_3_C0 = 101, - DISPLAY_LINE_4_C0 = 117, - - // Motor encoder velocities - MOTOR_1_VEL_H = 133, - MOTOR_1_VEL_L = 134, - MOTOR_2_VEL_H = 135, - MOTOR_2_VEL_L = 136, - MOTOR_3_VEL_H = 137, - MOTOR_3_VEL_L = 138, - MOTOR_4_VEL_H = 139, - MOTOR_4_VEL_L = 140 - }; - -private: - Spi(); - - int spi_file_descriptor{}; - uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0}; - uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0}; - - std::recursive_mutex spi_mutex; - - void transfer(); - - void clear_buffers(); - - static uint8_t calculate_checksum(uint8_t *data, uint8_t length); -}; - - -#endif //COMPLIB_SERVER_SPI_HPP diff --git a/server_v2/postinstall.sh b/server_v2/postinstall.sh deleted file mode 100755 index ba21c4f..0000000 --- a/server_v2/postinstall.sh +++ /dev/null @@ -1,2 +0,0 @@ -systemctl enable compsrv -systemctl restart compsrv diff --git a/server_v2/protos/CompLib.proto b/server_v2/protos/CompLib.proto deleted file mode 100644 index 1af66ad..0000000 --- a/server_v2/protos/CompLib.proto +++ /dev/null @@ -1,113 +0,0 @@ -syntax = "proto3"; - -package CompLib; - -message Header { - string message_type = 1; -} - -message Status { - bool successful = 1; - string error_message = 2; -} - -message GenericRequest { - Header header = 1; -} - -message GenericResponse { - Header header = 1; - Status status = 2; -} - -message EncoderReadPositionsRequest { - Header header = 1; -} - -message EncoderReadPositionsResponse { - Header header = 1; - Status status = 2; - repeated int32 positions = 3 [packed = true]; -} - -message EncoderReadVelocitiesRequest { - Header header = 1; -} - -message EncoderReadVelocitiesResponse { - Header header = 1; - Status status = 2; - repeated double velocities = 3 [packed = true]; -} - -message IRSensorsEnableRequest { - Header header = 1; -} - -message IRSensorsDisableRequest { - Header header = 1; -} - -message IRSensorsReadAllRequest { - Header header = 1; -} - -message IRSensorsReadAllResponse { - Header header = 1; - Status status = 2; - repeated uint32 data = 3 [packed = true]; -} - -message MotorSetPowerRequest { - uint32 port = 1; - double power = 2; -} - -message MotorsSetPowerRequest { - Header header = 1; - repeated MotorSetPowerRequest requests = 2; -} - -message MotorSetSpeedRequest { - uint32 port = 1; - double speed = 2; -} - -message MotorsSetSpeedRequest { - Header header = 1; - repeated MotorSetSpeedRequest requests = 2; -} - -message OdometryReadRequest { - Header header = 1; -} - -message OdometryReadResponse { - Header header = 1; - Status status = 2; - double x_position = 3; - double y_position = 4; - double orientation = 5; -} - -message DriveDistanceRequest { - Header header = 1; - double distance_m = 2; - double velocity_m_s = 3; -} - -message TurnDegreesRequest { - Header header = 1; - double angle_degrees = 2; - double velocity_rad_s = 3; -} - -message DriveRequest { - Header header = 1; - double linear_velocity_m_s = 2; - double angular_velocity_rad_s = 3; -} - -message HealthUpdateRequest { - Header header = 1; -} \ No newline at end of file diff --git a/server_v2/src/Cache.cpp b/server_v2/src/Cache.cpp deleted file mode 100644 index 0cd9a0b..0000000 --- a/server_v2/src/Cache.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "include/Cache.hpp" - -#define MS_IN_S 1000 -#define US_IN_S (1000 * MS_IN_S) - -Cache::Cache(int rate_hz) { - cache_duration = std::chrono::microseconds(US_IN_S / rate_hz); -} - -bool Cache::is_expired() { - return ((clock::now() - last_update) > cache_duration); -} - -void Cache::update() { - last_update = clock::now(); -} diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp deleted file mode 100644 index ad893cf..0000000 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include - -#include -#include - -#include "include/ClosedLoopMotorController.hpp" -#include "include/Motors.hpp" -#include "include/Encoders.hpp" - -#define MS_IN_S 1000 -#define US_IN_S (1000 * MS_IN_S) - -void ClosedLoopMotorController::set_power(uint8_t port, double power) { - std::lock_guard lock(speed_control_mutex); - control_modes.at(port) = ControlMode::POWER; - Motors::set_power(port, power); -} - -void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) { - std::lock_guard lock(speed_control_mutex); - speed_targets.at(port) = speed_ms; - if (control_modes.at(port) != ControlMode::SPEED) { - pids.at(port) = PID( - ROBOT_MOTOR_SPEED_CONTROL_KP, ROBOT_MOTOR_SPEED_CONTROL_KI, ROBOT_MOTOR_SPEED_CONTROL_KD, - ROBOT_MOTOR_SPEED_CONTROL_RAMP, 100.0); - control_modes.at(port) = ControlMode::SPEED; - } -} - -ClosedLoopMotorController::ClosedLoopMotorController() { - speed_control_thread = std::thread(&ClosedLoopMotorController::speed_control_loop, this); - speed_control_thread.detach(); -} - -[[noreturn]] void ClosedLoopMotorController::speed_control_loop() { - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ); - while (true) { - std::this_thread::sleep_for(sleep_duration); - - std::lock_guard lock(speed_control_mutex); - bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end(); - if (!should_control_speed) { - continue; - } - - auto speeds_rad_s = Encoders::getInstance().get_velocities_rad_s(); - for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) { - if (control_modes.at(i) == ControlMode::SPEED) { - spdlog::debug("CLMC: {}", i); - double power = pids.at(i)(speed_targets.at(i), speeds_rad_s.at(i)); - Motors::set_power(i, power); - } - } - } -} - -void ClosedLoopMotorController::generate_step_response_data() { - auto console = spdlog::stdout_color_mt("generate_step_response_data"); - console->set_pattern("%v"); - - auto input = 30.0; - - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID( - 1.0, 0.0, 0.0, - 1000.0, 100.0); - pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID( - 1.0, 0.0, 0.0, - 1000.0, 100.0); - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED; - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - auto start_time = std::chrono::steady_clock::now(); - for (int i = 0; i < 1000; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); - } - - input = 40.0; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 1000; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); - } - - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE; -} - -void ClosedLoopMotorController::generate_tuned_step_response_data() { - auto console = spdlog::stdout_color_mt("generate_tuned_step_response_data"); - console->set_pattern("%v"); - - auto input = 2 * M_PI; - - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID( - 5.5, 50.0, 0.0, - 4 * M_PI, 100.0); - pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID( - 5.5, 50.0, 0.0, - 4 * M_PI, 100.0); - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED; - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto start_time = std::chrono::steady_clock::now(); - for (int i = 0; i < 1500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); - } - - input = 4 * M_PI; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 1500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); - } - - input = M_PI; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 1500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); - - input += 0.01; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - } - - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0; - speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE; - control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE; -} - -void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks_per_turn) { - auto k_p = 0.0001; - auto power = 25.0; - auto min_power = 7.5; - auto port = ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT; - - auto ticks = 0.0; - auto target_ticks = ticks_per_turn * turns; - while (ticks != target_ticks) { - auto power_mult = fmax(-1.0, fmin(1.0, (target_ticks - ticks) * k_p)); - auto adjusted_power = power * power_mult; - if (adjusted_power < 0 && adjusted_power > -min_power) { - adjusted_power = -min_power; - } else if (adjusted_power > 0 && adjusted_power < min_power) { - adjusted_power = min_power; - } - - set_power(port, adjusted_power); - - spdlog::info("{:03.4f}; {:03.4f}; {:03.4f};", - ticks, target_ticks, adjusted_power); - - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - ticks = Encoders::getInstance().get_positions().at(port); - } - - set_power(port, 0); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - -} - -void ClosedLoopMotorController::stop_all() { - for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) { - set_power(i, 0); - } -} diff --git a/server_v2/src/Display.cpp b/server_v2/src/Display.cpp deleted file mode 100644 index 1aea17d..0000000 --- a/server_v2/src/Display.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "include/Display.hpp" -#include "include/spi.hpp" - -void Display::write(uint8_t line, std::string text) { - text.resize(16); - Spi::getInstance().write_array(Spi::DISPLAY_LINE_1_C0 + line * 16, 16, reinterpret_cast(text.c_str())); -} diff --git a/server_v2/src/Encoders.cpp b/server_v2/src/Encoders.cpp deleted file mode 100644 index 110d303..0000000 --- a/server_v2/src/Encoders.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "include/Encoders.hpp" -#include "include/spi.hpp" -#include "include/mathUtils.hpp" -#include - -std::array Encoders::get_positions() { - if (position_cache.is_expired()) { - uint8_t data[ROBOT_MOTOR_COUNT * 4] = {0}; - Spi::getInstance().read_array(Spi::MOTOR_1_POS_B3, 4 * ROBOT_MOTOR_COUNT, data); - for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - cached_positions.at(i) = mathUtils::from_bytes(data + i * 4, 4); - } - position_cache.update(); - } - return cached_positions; -} - -std::array Encoders::get_velocities_rad_s() { - if (velocity_cache.is_expired()) { - uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0}; - Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data); - for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - auto velocity_ticks_s = mathUtils::from_bytes(data + i * 2, 2); - auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN; - auto velocity_rad_s = velocity_rot_s * 2 * M_PI; - - auto change = velocity_rad_s - cached_velocities_rad_s.at(i); - change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S); - cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change; - } - velocity_cache.update(); - } - return cached_velocities_rad_s; -} - -std::array Encoders::get_velocities_ticks_s() { - if (ticks_cache.is_expired()) { - uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0}; - Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data); - for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - auto velocity_ticks_s = mathUtils::from_bytes(data + i * 2, 2); - cached_velocities_ticks_s.at(i) = velocity_ticks_s; - } - ticks_cache.update(); - } - return cached_velocities_ticks_s; -} diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp deleted file mode 100644 index e189d16..0000000 --- a/server_v2/src/GoToController.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include - -#include "include/GoToController.hpp" -#include "include/Robot.hpp" -#include "include/ClosedLoopMotorController.hpp" -#include "include/OdometryController.hpp" -#include "include/mathUtils.hpp" - -#define MS_IN_S 1000 -#define US_IN_S (1000 * MS_IN_S) - -// Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities -// Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code -void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) { - auto vr_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - auto vl_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s); - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s); -} - -// +dist, +vms -> forward -// -dist, +vms -> backward -// +dist, -vms -> backward -// -dist, -vms -> backward -void GoToController::drive_distance(double distance_m, double v_m_s) { - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ); - - if (distance_m < 0 || v_m_s < 0) { - distance_m = -fabs(distance_m); - v_m_s = -fabs(v_m_s); - } - - auto was_enabled = OdometryController::getInstance().is_enabled(); - OdometryController::getInstance().enable(); - auto start_x = OdometryController::getInstance().get().get_x_position(); - auto end_x = start_x + distance_m; - - auto distance_to_target = distance_m; - while (fabs(distance_to_target) > ROBOT_GO_TO_DISTANCE_END_M) { - std::this_thread::sleep_for(sleep_duration); - auto current_pos = OdometryController::getInstance().get(); - distance_to_target = end_x - current_pos.get_x_position(); - auto alpha = -mathUtils::wrap_angle_to_pi(current_pos.get_angular_orientation()); - - auto vel_mult = mathUtils::limit_max(distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P, -1.0, 1.0); - auto adjusted_speed = mathUtils::limit_min(fabs(v_m_s) * vel_mult, ROBOT_GO_TO_DISTANCE_MIN_VEL); - diff_drive_inverse_kinematics(adjusted_speed, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P); - - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};", - current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(), - distance_to_target, - alpha, vel_mult, adjusted_speed - ); - } - - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); - - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); - ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); - - if (!was_enabled) { - OdometryController::getInstance().disable(); - } -} - -// +ang, +rad -> cw -// -ang, +rad -> ccw -// +ang, -rad -> ccw -// -ang, -rad -> ccw -void GoToController::turn_degrees(double angle_deg, double v_rad_s) { - auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0)); - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ); - - if (angle_rad < 0 || v_rad_s < 0) { - angle_rad = -fabs(angle_rad); - v_rad_s = -fabs(v_rad_s); - } - - auto was_enabled = OdometryController::getInstance().is_enabled(); - OdometryController::getInstance().enable(); - - auto start_angle = OdometryController::getInstance().get().get_angular_orientation(); - auto end_angle = mathUtils::wrap_angle_to_pi(start_angle + angle_rad); - auto angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - start_angle); - while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) { - std::this_thread::sleep_for(sleep_duration); - auto current_angle = OdometryController::getInstance().get().get_angular_orientation(); - angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - current_angle); - - if (fabs(angle_to_target) > M_PI_2) { - diff_drive_inverse_kinematics(0.0, v_rad_s); - spdlog::debug("TURN: {:03.4f}; {:03.4f};", current_angle, angle_to_target); - } else { - auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0); - auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL); - diff_drive_inverse_kinematics(0.0, adjusted_speed); - spdlog::debug("TURN: {:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed); - } - } - - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); - - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); - ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); - - if (!was_enabled) { - OdometryController::getInstance().disable(); - } -} diff --git a/server_v2/src/HealthChecker.cpp b/server_v2/src/HealthChecker.cpp deleted file mode 100644 index c260d5e..0000000 --- a/server_v2/src/HealthChecker.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "include/HealthChecker.hpp" -#include "include/ClosedLoopMotorController.hpp" -#include "include/spi.hpp" -#include "include/IRSensors.hpp" - -HealthChecker::HealthChecker() { - last_update = clock::now(); - check_thread = std::thread(&HealthChecker::check_loop, this); - check_thread.detach(); -} - -void HealthChecker::update() { - last_update = clock::now(); -} - -void HealthChecker::check_loop() { - while (true) { - if (clock::now() - last_update < std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS)) { - Spi::getInstance().read_version(); - stop_sent = false; - } else if (!stop_sent) { - ClosedLoopMotorController::getInstance().stop_all(); - IRSensors::disable(); - stop_sent = true; - } - std::this_thread::sleep_for(std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS / 2)); - } -} diff --git a/server_v2/src/IPWritingService.cpp b/server_v2/src/IPWritingService.cpp deleted file mode 100644 index 045d868..0000000 --- a/server_v2/src/IPWritingService.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "include/IPWritingService.hpp" -#include "include/networkUtils.hpp" -#include "include/Display.hpp" - -IPWritingService::IPWritingService() { - writing_thread = std::thread(&IPWritingService::writing_loop, this); - writing_thread.detach(); -} - -void IPWritingService::writing_loop() { - while (true) { - auto host_and_ip = networkUtils::get_current_host_and_ip(); - Display::write(2, std::get<0>(host_and_ip)); - Display::write(3, std::get<1>(host_and_ip)); - std::this_thread::sleep_for(std::chrono::seconds(5)); - } -} diff --git a/server_v2/src/IRSensors.cpp b/server_v2/src/IRSensors.cpp deleted file mode 100644 index bc80e26..0000000 --- a/server_v2/src/IRSensors.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "include/IRSensors.hpp" -#include "include/spi.hpp" -#include "include/Robot.hpp" -#include "include/mathUtils.hpp" -#include "spdlog/spdlog.h" - -void IRSensors::enable() { - uint8_t data[ROBOT_IR_SENSOR_COUNT] = {}; - std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 1); - Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data); -} - -void IRSensors::disable() { - uint8_t data[ROBOT_IR_SENSOR_COUNT] = {}; - std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 0); - Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data); -} - - -std::array IRSensors::read() { - if (cache.is_expired()) { - uint8_t data[ROBOT_IR_SENSOR_COUNT * 2] = {0}; - Spi::getInstance().read_array(Spi::IR_1_H, 2 * ROBOT_IR_SENSOR_COUNT, data); - for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) { - cached_values.at(i) = mathUtils::from_bytes(data + i * 2, 2); - } - cache.update(); - } - return cached_values; -} \ No newline at end of file diff --git a/server_v2/src/Motors.cpp b/server_v2/src/Motors.cpp deleted file mode 100644 index da44582..0000000 --- a/server_v2/src/Motors.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "include/Motors.hpp" -#include "include/spi.hpp" -#include "include/Robot.hpp" -#include -#include - -void Motors::set_power(uint8_t port, double percent) { - if (fabs(percent) > 100.0) { - spdlog::error("Invalid motor percent specified. Should be -100 <= percent <= 100", percent); - } - - Mode mode = Mode::COAST; - if (percent < 0.0) { - mode = Mode::BACKWARD; - } else if (percent > 0.0) { - mode = Mode::FORWARD; - } - - auto pwm = (uint16_t) (fabs(percent) * (std::numeric_limits::max() / 100.0)); - set_pwm(port, pwm, mode); -} - -void Motors::set_pwm(uint8_t port, uint16_t pwm, Mode mode) { - if (port >= ROBOT_MOTOR_COUNT) { - spdlog::error("Invalid motor port {} specified", port); - } - - Spi::getInstance().write(Spi::MOTOR_1_PWM_H + port * 2, 2, pwm); - Spi::getInstance().write(Spi::PWM_1_CTRL + port, 1, mode); -} - diff --git a/server_v2/src/Odometry.cpp b/server_v2/src/Odometry.cpp deleted file mode 100644 index 7e2343b..0000000 --- a/server_v2/src/Odometry.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "include/Odometry.hpp" - -Odometry::Odometry() : x_position(0), y_position(0), angular_orientation(0) { - -} - -Odometry::Odometry(double x_position, double y_position, double angular_orientation) : x_position(x_position), y_position(y_position), angular_orientation(angular_orientation) { - -} - -double Odometry::get_x_position() const { - return x_position; -} - -double Odometry::get_y_position() const { - return y_position; -} - -double Odometry::get_angular_orientation() const { - return angular_orientation; -} diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp deleted file mode 100644 index 9b9bfdb..0000000 --- a/server_v2/src/OdometryController.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include - -#include "include/OdometryController.hpp" -#include "include/Robot.hpp" -#include "include/Encoders.hpp" -#include -#include "include/mathUtils.hpp" - -#define MS_IN_S 1000 -#define US_IN_S (1000 * MS_IN_S) - -bool OdometryController::is_enabled() const { - return enabled; -} - -void OdometryController::enable() { - last_run = clock::now(); - enabled = true; -} - -void OdometryController::disable() { - spdlog::debug("OdometryController::disable()"); - enabled = false; - OdometryController::reset(); -} - -void OdometryController::reset() { - std::lock_guard lock(odometry_mutex); - current_odometry = Odometry(); - last_run = clock::now(); -} - -Odometry OdometryController::get() { - return current_odometry; -} - - -OdometryController::OdometryController() { - odometry_thread = std::thread(&OdometryController::odometry_loop, this); - odometry_thread.detach(); -} - -[[noreturn]] void OdometryController::odometry_loop() { - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ); - while (true) { - std::this_thread::sleep_for(sleep_duration); - std::lock_guard lock(odometry_mutex); - if (enabled) { - last_run = clock::now(); - auto encoder_positions = Encoders::getInstance().get_positions(); - - auto current_position_left = - encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - auto current_position_right = - encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER; - auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER; - last_position_left = current_position_left; - last_position_right = current_position_right; - - // The section below implements differential drive kinematics. - // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin - // or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart - // and Illah R. Nourbakhsh - auto v = (distance_right + distance_left) / 2.0; - auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; - - auto curr_x = current_odometry.get_x_position(); - auto curr_y = current_odometry.get_y_position(); - auto curr_theta = current_odometry.get_angular_orientation(); - - auto x = v * cos(w); - auto y = v * sin(w); - - auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y); - auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y); - auto new_theta = curr_theta + w; - - new_theta = mathUtils::wrap_angle_to_pi(new_theta); - current_odometry = Odometry(new_x, new_y, new_theta); - - - spdlog::debug("ODOM: {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", - current_position_left, current_position_right, - distance_left, distance_right, - v, w); - } - } -} diff --git a/server_v2/src/PID.cpp b/server_v2/src/PID.cpp deleted file mode 100644 index a27360c..0000000 --- a/server_v2/src/PID.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "include/PID.hpp" -#include -#include -#include - -#define MS_IN_S 1000.0 -#define US_IN_S (1000.0 * MS_IN_S) - -using namespace std; - - -PID::PID() : P(0), I(0), D(0), setpoint_ramp(0), limit(0), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {}; - -PID::PID(double P, double I, double D, double ramp, double limit) - : P(P), I(I), D(D), setpoint_ramp(ramp), limit(limit), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) { - timestamp_prev = clock::now(); -} - -double PID::operator()(double setpoint, double process_variable) { - // calculate the time from the last call - auto timestamp_now = clock::now(); - double delta_seconds = chrono::duration_cast>(timestamp_now - timestamp_prev).count() / US_IN_S; - - if (setpoint_ramp > 0) { - double setpoint_rate = (setpoint - setpoint_prev) / delta_seconds; - if (setpoint_rate > setpoint_ramp) { - setpoint = setpoint_prev + setpoint_ramp * delta_seconds; - } else if (setpoint_rate < -setpoint_ramp) { - setpoint = setpoint_prev - setpoint_ramp * delta_seconds; - } - } - - double error = setpoint - process_variable; - - double proportional = P * error; - // Tustin transform of the integral part - double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev); - double derivative = D * ((error - error_prev) / delta_seconds); - integral = std::clamp(integral, -limit, limit); - - double output = proportional + integral + derivative; - - // anti-windup - limit the output variable - output = std::clamp(output, -limit, limit); - spdlog::debug("PID: E{:03.4f} P{:03.4f} I{:03.4f} D{:03.4f} O{:03.4f} EP{:03.4f} DS{:03.4f}, SP{:03.4f} PV{:03.4f}", - error, proportional, integral, derivative, output, error_prev, delta_seconds, setpoint, process_variable - ); - - integral_prev = integral; - setpoint_prev = setpoint; - error_prev = error; - timestamp_prev = timestamp_now; - return output; -} diff --git a/server_v2/src/communication/MessageBuilder.cpp b/server_v2/src/communication/MessageBuilder.cpp deleted file mode 100644 index 1fd1df5..0000000 --- a/server_v2/src/communication/MessageBuilder.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include - -#include "include/communication/MessageBuilder.hpp" - -using namespace CompLib; - -CompLib::Header *MessageBuilder::header(const std::string &message_type) { - auto header = new CompLib::Header(); - header->set_message_type(message_type); - return header; -} - -CompLib::Status *MessageBuilder::status(bool successful, const std::string &error_message) { - auto status = new CompLib::Status(); - status->set_successful(successful); - status->set_error_message(error_message); - return status; -} - -CompLib::GenericResponse *MessageBuilder::generic_response(bool successful, const std::string &error_message) { - auto genericResponse = new CompLib::GenericResponse(); - genericResponse->set_allocated_header(header(CompLib::GenericResponse::descriptor()->full_name())); - genericResponse->set_allocated_status(status(successful, error_message)); - return genericResponse; -} - -CompLib::Status *MessageBuilder::default_successful_status() { - return status(true, ""); -} - - -CompLib::GenericResponse *MessageBuilder::default_successful_generic_response() { - return generic_response(true, ""); -} - -CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_response(std::array positions) { - auto response = new EncoderReadPositionsResponse(); - response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name())); - response->set_allocated_status(default_successful_status()); - for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->add_positions(positions.at(i)); - } - return response; -} - -CompLib::EncoderReadVelocitiesResponse *MessageBuilder::encoder_read_velocities_response(std::array velocities) { - auto response = new EncoderReadVelocitiesResponse(); - response->set_allocated_header(header(EncoderReadVelocitiesResponse::descriptor()->full_name())); - response->set_allocated_status(default_successful_status()); - for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->add_velocities(velocities.at(i)); - } - return response; -} - -CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response(std::array data) { - auto response = new IRSensorsReadAllResponse(); - response->set_allocated_header(header(IRSensorsReadAllResponse::descriptor()->full_name())); - response->set_allocated_status(default_successful_status()); - for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) { - response->add_data(data.at(i)); - } - return response; -} diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp deleted file mode 100644 index bfd8543..0000000 --- a/server_v2/src/communication/MessageProcessor.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include "include/communication/MessageProcessor.hpp" -#include "include/communication/MessageBuilder.hpp" - -#include "include/Encoders.hpp" -#include "include/IRSensors.hpp" -#include "include/ClosedLoopMotorController.hpp" -#include "include/GoToController.hpp" -#include "include/HealthChecker.hpp" - -using namespace CompLib; - -google::protobuf::Message *MessageProcessor::process_message(const std::string &serializedMessage) { - CompLib::GenericRequest message; - message.ParseFromString(serializedMessage); - auto messageTypeName = message.header().message_type(); -// spdlog::debug("Got request with type {}", messageTypeName); - - try { - // Encoder - if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) { - return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions()); - } else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) { - return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rad_s()); - } - - // IR Sensors - if (messageTypeName == IRSensorsEnableRequest::GetDescriptor()->full_name()) { - IRSensors::enable(); - return MessageBuilder::default_successful_generic_response(); - } else if (messageTypeName == IRSensorsDisableRequest::GetDescriptor()->full_name()) { - IRSensors::disable(); - return MessageBuilder::default_successful_generic_response(); - } else if (messageTypeName == IRSensorsReadAllRequest::GetDescriptor()->full_name()) { - return MessageBuilder::ir_sensors_read_all_response(IRSensors::getInstance().read()); - } - - // Motors - if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) { - MotorsSetPowerRequest request; - request.ParseFromString(serializedMessage); - for (const auto &innerRequest: request.requests()) { - ClosedLoopMotorController::getInstance().set_power(innerRequest.port(), innerRequest.power()); - } - return MessageBuilder::default_successful_generic_response(); - } else if (messageTypeName == MotorsSetSpeedRequest::GetDescriptor()->full_name()) { - MotorsSetSpeedRequest request; - request.ParseFromString(serializedMessage); - for (const auto &innerRequest: request.requests()) { - ClosedLoopMotorController::getInstance().set_speed(innerRequest.port(), innerRequest.speed()); - } - return MessageBuilder::default_successful_generic_response(); - } - - // Drive - if (messageTypeName == DriveDistanceRequest::GetDescriptor()->full_name()) { - DriveDistanceRequest request; - request.ParseFromString(serializedMessage); - GoToController::drive_distance(request.distance_m(), request.velocity_m_s()); - return MessageBuilder::default_successful_generic_response(); - } else if (messageTypeName == TurnDegreesRequest::GetDescriptor()->full_name()) { - TurnDegreesRequest request; - request.ParseFromString(serializedMessage); - GoToController::turn_degrees(request.angle_degrees(), request.velocity_rad_s()); - return MessageBuilder::default_successful_generic_response(); - } else if (messageTypeName == DriveRequest::GetDescriptor()->full_name()) { - DriveRequest request; - request.ParseFromString(serializedMessage); - GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s()); - return MessageBuilder::default_successful_generic_response(); - } - - // Health - if (messageTypeName == HealthUpdateRequest::GetDescriptor()->full_name()) { - HealthUpdateRequest request; - request.ParseFromString(serializedMessage); - HealthChecker::getInstance().update(); - return MessageBuilder::default_successful_generic_response(); - } - - } catch (const std::exception &ex) { - spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what()); - google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what()); - return returnMessage; - } - - spdlog::error("Message {} unknown!", messageTypeName); - google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ERROR_MESSAGE_HEADER_UNKNOWN); - return returnMessage; -} diff --git a/server_v2/src/communication/TCPSocketServer.cpp b/server_v2/src/communication/TCPSocketServer.cpp deleted file mode 100644 index 405123d..0000000 --- a/server_v2/src/communication/TCPSocketServer.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include -#include -#include - -#include - -#include "include/communication/TCPSocketServer.hpp" -#include "include/communication/MessageProcessor.hpp" - -#define SERVER_PORT 9090 -#define TCP_LISTEN_BACKLOG 5 - -TCPSocketServer::TCPSocketServer() { - server_thread = std::thread(&TCPSocketServer::server_loop, this); - server_thread.detach(); -} - -[[noreturn]] void TCPSocketServer::server_loop() { - struct sockaddr_in address; - int socket_file_descriptor = socket(AF_INET, SOCK_STREAM, 0); - if (socket_file_descriptor == 0) { - spdlog::error("TCP socket initialization failed!"); - exit(EXIT_FAILURE); - } - - int opt = 1; - if (int err = setsockopt(socket_file_descriptor, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) { - spdlog::error("TCP setsockopt failed! Err: {}", err); - exit(EXIT_FAILURE); - } - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(SERVER_PORT); - - if (bind(socket_file_descriptor, (struct sockaddr *) &address, sizeof(address)) < 0) { - spdlog::error("TCP bind failed!"); - exit(EXIT_FAILURE); - } - - if (listen(socket_file_descriptor, TCP_LISTEN_BACKLOG) < 0) { - spdlog::error("TCP listen failed!"); - exit(EXIT_FAILURE); - } - - char read_buffer[SOCKET_BUFFER_SIZE]; - char write_buffer[SOCKET_BUFFER_SIZE]; - for (;;) { - int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL); - if (client_file_descriptor < 0) { - spdlog::error("TCP accept failed!"); - continue; - } - - read(client_file_descriptor, read_buffer, 1); - uint8_t message_size = read_buffer[0]; - - read(client_file_descriptor, read_buffer, read_buffer[0]); - std::string string_message; - for (int i{}; i < message_size; i++) { - string_message += read_buffer[i]; - } - - auto response = MessageProcessor::process_message(string_message); - uint8_t response_size = response->ByteSizeLong(); - write_buffer[0] = response_size; - write(client_file_descriptor, write_buffer, 1); - - response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE); - write(client_file_descriptor, write_buffer, response_size); - - close(client_file_descriptor); - } -} \ No newline at end of file diff --git a/server_v2/src/communication/UnixSocketServer.cpp b/server_v2/src/communication/UnixSocketServer.cpp deleted file mode 100644 index b9500ef..0000000 --- a/server_v2/src/communication/UnixSocketServer.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include -#include -#include -#include - -#include "include/communication/UnixSocketServer.hpp" -#include "include/communication/MessageProcessor.hpp" - -UnixSocketServer::UnixSocketServer() { - server_thread = std::thread(&UnixSocketServer::server_loop, this); - server_thread.detach(); -} - -[[noreturn]] void UnixSocketServer::server_loop() { - struct sockaddr_un socket_address; - int socket_file_descriptor = socket(AF_UNIX, SOCK_STREAM, 0); - remove(SOCKET_PATH); - - memset(&socket_address, 0, sizeof(struct sockaddr_un)); - socket_address.sun_family = AF_UNIX; - strncpy(socket_address.sun_path, SOCKET_PATH, sizeof(socket_address.sun_path) - 1); - - if (bind(socket_file_descriptor, (struct sockaddr *) &socket_address, sizeof(struct sockaddr_un)) == -1) { - exit(-1); - } - - chmod(SOCKET_PATH, 0777); - - if (listen(socket_file_descriptor, 1) == -1) { - exit(-2); - } - - char read_buffer[SOCKET_BUFFER_SIZE]; - char write_buffer[SOCKET_BUFFER_SIZE]; - for (;;) { - int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL); - - while (true) { - int bytes_received = read(client_file_descriptor, read_buffer, 1); - if (bytes_received <= 0) { - break; - } - uint8_t message_size = read_buffer[0]; - - bytes_received = read(client_file_descriptor, read_buffer, read_buffer[0]); - if (bytes_received <= 0) { - break; - } - std::string string_message; - for (int i{}; i < message_size; i++) { - string_message += read_buffer[i]; - } - - auto response = MessageProcessor::process_message(string_message); - uint8_t response_size = response->ByteSizeLong(); - write_buffer[0] = response_size; - write(client_file_descriptor, write_buffer, 1); - - response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE); - write(client_file_descriptor, write_buffer, response_size); - } - - close(client_file_descriptor); - } -} \ No newline at end of file diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp deleted file mode 100644 index 07294d1..0000000 --- a/server_v2/src/main.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include -#include - -#include "include/reset.hpp" -#include "include/OdometryController.hpp" -#include "include/ClosedLoopMotorController.hpp" -#include "include/communication/UnixSocketServer.hpp" -#include "include/communication/TCPSocketServer.hpp" -#include "include/HealthChecker.hpp" -#include "include/GoToController.hpp" -#include "include/Encoders.hpp" -#include "include/networkUtils.hpp" -#include "include/IPWritingService.hpp" - -#ifdef IS_RASPI - -#include - -#endif - -using namespace std; - -int main(int argc, char *argv[]) { - Reset::reset_robot(); - spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); - spdlog::set_level(spdlog::level::debug); - spdlog::info("Starting complib_server..."); - - UnixSocketServer unixSocketServer; - TCPSocketServer tcpSocketServer; - IPWritingService ipWritingService; - HealthChecker::getInstance(); - -#ifdef IS_RASPI - sd_notify(0, "READY=1"); -#endif - -// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT); -// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT); -// OdometryController::getInstance().enable(); - -// for (int i = 0; i < 10000; ++i) { -// auto odom = OdometryController::getInstance().get(); -// spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); -// usleep(1000); -// } - -// OdometryController::getInstance().enable(); -// GoToGoalController::diff_drive_inverse_kinematics(0.2, -M_PI_4); -// for (int i = 0; i < 800; ++i) { -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// auto odom = OdometryController::getInstance().get(); -// spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); -// } - -// GoToGoalController::go_to_point(2, 0, 0.2); -// GoToController::drive_distance(-4, 0.2); -// GoToController::turn_degrees(90, M_PI); -// this_thread::sleep_for(std::chrono::milliseconds(100)); -// GoToController::turn_degrees(-90, M_PI); - - -// while (true) { -// this_thread::sleep_for(std::chrono::milliseconds(10)); -// ClosedLoopMotorController::getInstance().set_power(0, 100); -// spdlog::debug("{}", Encoders::getInstance().get_velocities_rad_s().at(0)); -// } - -// GoToController::drive_distance(2, 0.3); -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// GoToController::drive_distance(-2, 0.3); - -// while (true) { -// GoToController::drive_distance(0.2, 0.4); -// GoToController::turn_degrees(90, M_PI_2); -// -// GoToController::drive_distance(0.2, 0.4); -// GoToController::turn_degrees(90, M_PI_2); -// -// GoToController::drive_distance(0.2, 0.4); -// GoToController::turn_degrees(90, M_PI_2); -// -// GoToController::drive_distance(0.2, 0.4); -// GoToController::turn_degrees(90, M_PI_2); -// } - - -// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1])); - -// ClosedLoopMotorController::getInstance().generate_step_response_data(); -// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data(); -// this_thread::sleep_for(std::chrono::milliseconds(1000)); - std::this_thread::sleep_for(std::chrono::hours(12)); - return 0; -} diff --git a/server_v2/src/spi.cpp b/server_v2/src/spi.cpp deleted file mode 100644 index f53a343..0000000 --- a/server_v2/src/spi.cpp +++ /dev/null @@ -1,165 +0,0 @@ -#include -#include -#include - -#include "include/spi.hpp" -#include "include/mathUtils.hpp" - -void check_for_error(int error_code, std::string error_message) { - if (error_code < 0) { - spdlog::error(error_message); - exit(1); - } -} - -void Spi::clear_buffers() { - memset(tx_buffer, 0, SPI_BUFFER_SIZE); - memset(rx_buffer, 0, SPI_BUFFER_SIZE); -} - -uint8_t Spi::calculate_checksum(uint8_t *data, uint8_t length) { - int sum = 0; - for (int i = 0; i < length; i++) { - sum += data[i]; - } - return sum % 256; -} - -#ifdef IS_RASPI - -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port - -Spi::Spi() { - int spi_mode = SPI_MODE_0; - int spi_bits_per_workd = SPI_BITS_PER_WORD; - int spi_speed = SPI_SPEED; - - check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); -} - -int Spi::read(uint8_t reg, uint8_t length) { - std::lock_guard lock(spi_mutex); - - uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; - read_array(reg, length, read_buffer); - - return mathUtils::int_from_bytes(read_buffer, length); -} - -void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { - std::lock_guard lock(spi_mutex); - - clear_buffers(); - tx_buffer[0] = 0; - tx_buffer[1] = reg; - tx_buffer[2] = length; - - transfer(); - - uint8_t checksum = calculate_checksum(rx_buffer, length + 3); - if (checksum != rx_buffer[length + 3]) { - spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length + 3], checksum); - } - - memcpy(data, rx_buffer + 2, length); -} - -void Spi::write(uint8_t reg, uint8_t length, int value) { - std::lock_guard lock(spi_mutex); - - uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; - mathUtils::bytes_from_int(value, length, write_buffer); - write_array(reg, length, write_buffer); -} - -void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { - std::lock_guard lock(spi_mutex); - - clear_buffers(); - tx_buffer[0] = 1; - tx_buffer[1] = reg; - tx_buffer[2] = length; - memcpy(tx_buffer + 3, data, length); - - transfer(); -} - -void Spi::transfer() { - std::lock_guard lock(spi_mutex); - - struct spi_ioc_transfer spi; - memset(&spi, 0, sizeof(spi)); - - spi.tx_buf = (unsigned long) tx_buffer; - spi.rx_buf = (unsigned long) rx_buffer; - spi.len = SPI_BUFFER_SIZE; - spi.delay_usecs = 0; - spi.speed_hz = SPI_SPEED; - spi.bits_per_word = SPI_BITS_PER_WORD; - spi.cs_change = 0; - - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - - if (tx_buffer[1] != rx_buffer[1]) { - spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); - } -} - -std::array Spi::read_version() { - uint8_t data[4] = {0}; - read_array(IDENTIFICATION_MODEL_ID, 4, data); - auto result = std::array(); - for (int i = 0; i < 4; i++) { - result.at(i) = data[i]; - } - return result; -} - -#else - -Spi::Spi() {} - -int Spi::read(uint8_t reg, uint8_t length) { - std::lock_guard lock(spi_mutex); - spdlog::warn("Calling SPI without actual interface."); - - return 0; -} - -void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { - std::lock_guard lock(spi_mutex); - spdlog::warn("Calling SPI without actual interface."); - clear_buffers(); - memcpy(data, rx_buffer, length); -} - -void Spi::write(uint8_t reg, uint8_t length, int value) { - std::lock_guard lock(spi_mutex); - spdlog::warn("Calling SPI without actual interface."); -} - -void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { - std::lock_guard lock(spi_mutex); - spdlog::warn("Calling SPI without actual interface."); -} - -void Spi::transfer() { - std::lock_guard lock(spi_mutex); - spdlog::warn("Calling SPI without actual interface."); -} - -std::array Spi::read_version() { - spdlog::warn("Calling SPI without actual interface."); -} - -#endif \ No newline at end of file diff --git a/server_v2/test_bin/compsrv b/server_v2/test_bin/compsrv deleted file mode 100755 index fdd79c6..0000000 Binary files a/server_v2/test_bin/compsrv and /dev/null differ