├── em-os ├── project-spec │ ├── configs │ │ ├── flash_parts.txt │ │ ├── rootfsconfigs │ │ │ ├── Kconfig.user │ │ │ └── user-rootfsconfig │ │ ├── systemd-conf │ │ │ └── wired.network │ │ ├── .statistics │ │ ├── rootfs.wks │ │ ├── init-ifupdown │ │ │ └── interfaces │ │ ├── busybox │ │ │ └── inetd.conf │ │ ├── zynq-generic-7z020.conf │ │ └── plnx_syshw_data │ ├── hw-description │ │ ├── metadata │ │ └── system.xsa │ ├── meta-user │ │ ├── recipes-kernel │ │ │ └── linux │ │ │ │ ├── linux-xlnx │ │ │ │ ├── bsp.cfg │ │ │ │ ├── user_2024-11-12-22-53-00.cfg │ │ │ │ ├── user_2025-02-26-00-50-00.cfg │ │ │ │ ├── user_2025-02-26-01-29-00.cfg │ │ │ │ └── user_2024-11-05-04-03-00.cfg │ │ │ │ └── linux-xlnx_%.bbappend │ │ ├── recipes-bsp │ │ │ ├── u-boot │ │ │ │ ├── files │ │ │ │ │ ├── user_2024-10-27-00-48-00.cfg │ │ │ │ │ ├── user_2024-11-11-00-54-00.cfg │ │ │ │ │ ├── bsp.cfg │ │ │ │ │ └── platform-top.h │ │ │ │ └── u-boot-xlnx_%.bbappend │ │ │ └── device-tree │ │ │ │ ├── device-tree.bbappend │ │ │ │ ├── files │ │ │ │ └── system-user.dtsi │ │ │ │ └── device-tree-sdt.inc │ │ ├── meta-xilinx-tools │ │ │ └── recipes-bsp │ │ │ │ └── uboot-device-tree │ │ │ │ ├── files │ │ │ │ └── system-user.dtsi │ │ │ │ └── uboot-device-tree.bbappend │ │ ├── conf │ │ │ ├── petalinuxbsp.conf │ │ │ ├── user-rootfsconfig │ │ │ └── layer.conf │ │ ├── COPYING.MIT │ │ └── README │ └── attributes ├── patch-6.1.12-rt6.patch.gz ├── patch-6.1.54-rt15.patch.gz ├── .petalinux │ └── metadata ├── .gitignore ├── setup.sh ├── build_pack_image.sh ├── build_run_qemu.sh └── fixes.sh ├── controller-software ├── core │ ├── controller │ │ ├── api_drivers │ │ │ ├── calls │ │ │ │ ├── src │ │ │ │ │ ├── print_float.cpp │ │ │ │ │ ├── configure_node.cpp │ │ │ │ │ ├── machine_state.cpp │ │ │ │ │ └── print_uint32_t.cpp │ │ │ │ └── inc │ │ │ │ │ ├── print_float.h │ │ │ │ │ ├── print_uint32.h │ │ │ │ │ └── base_call.h │ │ │ ├── test_controller.cpp │ │ │ ├── inc │ │ │ │ ├── api_objects.h │ │ │ │ ├── controller_api.h │ │ │ │ └── shared_mem.h │ │ │ ├── test_api.cpp │ │ │ └── src │ │ │ │ └── api_objects.cpp │ │ ├── nodes │ │ │ ├── generic_nodes │ │ │ │ ├── src │ │ │ │ │ ├── print.cpp │ │ │ │ │ ├── constant.cpp │ │ │ │ │ ├── api_input.cpp │ │ │ │ │ ├── converter.cpp │ │ │ │ │ ├── api_output.cpp │ │ │ │ │ ├── comparator.cpp │ │ │ │ │ ├── edge_delay.cpp │ │ │ │ │ ├── logic_gate.cpp │ │ │ │ │ ├── cycle_delay.cpp │ │ │ │ │ ├── edge_detect.cpp │ │ │ │ │ ├── multiplexer.cpp │ │ │ │ │ ├── bitwise_join.cpp │ │ │ │ │ ├── bitwise_split.cpp │ │ │ │ │ ├── pi_controller.cpp │ │ │ │ │ ├── vel_estimator.cpp │ │ │ │ │ ├── math_operation.cpp │ │ │ │ │ ├── em_serial_device.cpp │ │ │ │ │ ├── get_global_variable.cpp │ │ │ │ │ └── set_global_variable.cpp │ │ │ │ └── inc │ │ │ │ │ ├── get_global_variable.h │ │ │ │ │ ├── set_global_variable.h │ │ │ │ │ ├── bitwise_split.h │ │ │ │ │ ├── bitwise_join.h │ │ │ │ │ ├── edge_detect.h │ │ │ │ │ ├── edge_delay.h │ │ │ │ │ ├── constant.h │ │ │ │ │ ├── print.h │ │ │ │ │ ├── vel_estimator.h │ │ │ │ │ ├── cycle_delay.h │ │ │ │ │ └── api_output.h │ │ │ ├── inc │ │ │ │ ├── base_node.h │ │ │ │ ├── node_factory.h │ │ │ │ ├── node_core.h │ │ │ │ └── node_io.h │ │ │ └── src │ │ │ │ ├── node_io.cpp │ │ │ │ └── base_node.cpp │ │ ├── fpga_module_drivers │ │ │ ├── inc │ │ │ │ ├── global_timers.h.old │ │ │ │ ├── fanuc_encoders.h │ │ │ │ ├── yaskawa_encoders.h │ │ │ │ └── serial_interface_card.h │ │ │ └── src │ │ │ │ ├── global_timers.cpp.old │ │ │ │ └── fanuc_encoders.cpp │ │ ├── inc │ │ │ ├── controller.h │ │ │ ├── fpga_module_manager.h │ │ │ └── fpga_interface.h │ │ └── src │ │ │ ├── main.cpp │ │ │ └── fpga_module_driver_factory.cpp │ ├── .gitignore │ ├── zynq_files │ │ └── controller │ │ │ └── config │ │ │ ├── em_devices │ │ │ └── application.bin │ │ │ ├── fpga_configs │ │ │ └── test │ │ │ │ └── bitfile.bit.bin │ │ │ └── user │ │ │ ├── config.json │ │ │ └── fpga_driver_config.json │ ├── cmake │ │ └── zynq_debug.cmake │ ├── CMakePresets.json │ ├── devices │ │ └── em_hvsd │ │ │ └── config.yaml │ ├── .vscode │ │ ├── launch.json │ │ └── controller-software-core.code-workspace │ ├── readme.md │ └── CMakeLists.txt ├── config │ └── electrical │ │ ├── power │ │ ├── line_reactor.py │ │ └── power_sources.py │ │ ├── user_params.py │ │ ├── test.py │ │ ├── motors │ │ └── motor_linear.py │ │ └── parameters.py └── web │ └── web-interface │ ├── package.json │ ├── client │ └── login │ │ └── login.html │ ├── .vscode │ └── launch.json │ └── server │ ├── wsServer.js │ └── app.js ├── controller-firmware ├── Vivado │ ├── convert.bif │ ├── create_project.tcl │ ├── controller_firmware_top.xsa │ ├── controller_firmware │ │ └── controller_firmware_top.xsa │ ├── export bit.bin.tcl │ ├── save_project.tcl │ ├── .gitignore │ └── build.tcl ├── python │ ├── .gitignore │ ├── src │ │ ├── main.py │ │ ├── testing_block.py │ │ ├── sandbox │ │ │ ├── yaskawa_decoding.py │ │ │ ├── instructions.py │ │ │ ├── current_control.py │ │ │ ├── yaskawa_encoder_testing.py │ │ │ ├── yaskawa_encoder_vis.py │ │ │ ├── test_config_out.json │ │ │ ├── motion_planner_2.py │ │ │ ├── spline_paths_1.py │ │ │ ├── motion_planner_3.py │ │ │ └── biquad test.py │ │ ├── gpio_node.py │ │ ├── convergent_round.py │ │ └── global_timer.py │ ├── generators │ │ └── dma_instruction_compiler.py │ ├── requirements.txt │ └── config │ │ └── hardware configs │ │ └── hardware_config.yaml └── README.md ├── setup_subst.bat ├── petalinux └── controller_firmware_top.xsa ├── Documentation └── API Backend │ ├── Getting started │ └── Core mechanics.md │ ├── Motion │ ├── Configuration APIs.md │ └── Motion APIs.md │ └── Project goals and discussion.md ├── .vscode ├── launch.json └── tasks.json ├── README.md └── .gitignore /em-os/project-spec/configs/flash_parts.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /em-os/project-spec/hw-description/metadata: -------------------------------------------------------------------------------- 1 | HARDWARE_SOURCE= 2 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx/bsp.cfg: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/src/print_float.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/src/configure_node.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/src/machine_state.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/src/print_uint32_t.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/rootfsconfigs/Kconfig.user: -------------------------------------------------------------------------------- 1 | menu "user packages " 2 | endmenu 3 | -------------------------------------------------------------------------------- /controller-firmware/Vivado/convert.bif: -------------------------------------------------------------------------------- 1 | the_ROM_image: 2 | { 3 | S:/Vivado/bitfile.bit 4 | } -------------------------------------------------------------------------------- /setup_subst.bat: -------------------------------------------------------------------------------- 1 | subst S: C:\Users\voids\Documents\GitHub\controller-software\controller-firmware -------------------------------------------------------------------------------- /controller-firmware/python/.gitignore: -------------------------------------------------------------------------------- 1 | /venv 2 | /sim outputs 3 | /src/sandbox/yaskawa_decoded_bytes.csv -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/u-boot/files/user_2024-10-27-00-48-00.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_SD_BOOT=y 2 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/u-boot/files/user_2024-11-11-00-54-00.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_SD_BOOT_QSPI=y 2 | -------------------------------------------------------------------------------- /em-os/patch-6.1.12-rt6.patch.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/em-os/patch-6.1.12-rt6.patch.gz -------------------------------------------------------------------------------- /em-os/patch-6.1.54-rt15.patch.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/em-os/patch-6.1.54-rt15.patch.gz -------------------------------------------------------------------------------- /controller-firmware/Vivado/create_project.tcl: -------------------------------------------------------------------------------- 1 | 2 | set project_dir [lindex $argv 0] 3 | cd $project_dir 4 | 5 | source controller_firmware.tcl -------------------------------------------------------------------------------- /controller-software/config/electrical/power/line_reactor.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, field 2 | 3 | class LINE_REACTOR: 4 | pass -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/u-boot/files/bsp.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_SYS_CONFIG_NAME="platform-top" 2 | CONFIG_BOOT_SCRIPT_OFFSET=0x9C0000 3 | -------------------------------------------------------------------------------- /em-os/project-spec/attributes: -------------------------------------------------------------------------------- 1 | #Virtual Providers 2 | 3 | 4 | 5 | #defconfigs 6 | 7 | UBOOT_DEFAULT_DEFCONFIG="xilinx_zynq_virt_defconfig" 8 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/meta-xilinx-tools/recipes-bsp/uboot-device-tree/files/system-user.dtsi: -------------------------------------------------------------------------------- 1 | /include/ "system-conf.dtsi" 2 | / { 3 | }; 4 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx/user_2024-11-12-22-53-00.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_UIO_PDRV_GENIRQ=y 2 | CONFIG_UIO_DMEM_GENIRQ=y 3 | -------------------------------------------------------------------------------- /petalinux/controller_firmware_top.xsa: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/petalinux/controller_firmware_top.xsa -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/print.cpp: -------------------------------------------------------------------------------- 1 | #include "print.h" 2 | 3 | static Node_Registrar node_registrar_print("print"); -------------------------------------------------------------------------------- /em-os/project-spec/hw-description/system.xsa: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/em-os/project-spec/hw-description/system.xsa -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/conf/petalinuxbsp.conf: -------------------------------------------------------------------------------- 1 | #User Configuration 2 | 3 | #OE_TERMINAL = "tmux" 4 | IMAGE_BOOT_FILES:zynq = "BOOT.BIN,image.ub,boot.scr" 5 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/conf/user-rootfsconfig: -------------------------------------------------------------------------------- 1 | #Note: Mention Each package in individual line 2 | #These packages will get added into rootfs menu entry 3 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/rootfsconfigs/user-rootfsconfig: -------------------------------------------------------------------------------- 1 | #Note: Mention Each package in individual line 2 | #These packages will get added into rootfs menu entry 3 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/constant.cpp: -------------------------------------------------------------------------------- 1 | #include "constant.h" 2 | 3 | static Node_Registrar node_registrar_constant("constant"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/api_input.cpp: -------------------------------------------------------------------------------- 1 | #include "api_input.h" 2 | 3 | static Node_Registrar node_registrar_api_input("api_input"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/converter.cpp: -------------------------------------------------------------------------------- 1 | #include "converter.h" 2 | 3 | static Node_Registrar node_registrar_converter("converter"); -------------------------------------------------------------------------------- /controller-firmware/Vivado/controller_firmware_top.xsa: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/controller-firmware/Vivado/controller_firmware_top.xsa -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/api_output.cpp: -------------------------------------------------------------------------------- 1 | #include "api_output.h" 2 | 3 | static Node_Registrar node_registrar_api_output("api_output"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/comparator.cpp: -------------------------------------------------------------------------------- 1 | #include "comparator.h" 2 | 3 | static Node_Registrar node_registrar_comparator("comparator"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/edge_delay.cpp: -------------------------------------------------------------------------------- 1 | #include "edge_delay.h" 2 | 3 | static Node_Registrar node_registrar_edge_delay("edge_delay"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/logic_gate.cpp: -------------------------------------------------------------------------------- 1 | #include "logic_gate.h" 2 | 3 | static Node_Registrar node_registrar_logic_gate("logic_gate"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/cycle_delay.cpp: -------------------------------------------------------------------------------- 1 | #include "cycle_delay.h" 2 | 3 | static Node_Registrar node_registrar_cycle_delay("cycle_delay"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/edge_detect.cpp: -------------------------------------------------------------------------------- 1 | #include "edge_detect.h" 2 | 3 | static Node_Registrar node_registrar_edge_detect("edge_detect"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/multiplexer.cpp: -------------------------------------------------------------------------------- 1 | #include "multiplexer.h" 2 | 3 | static Node_Registrar node_registrar_multiplexer("multiplexer"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/bitwise_join.cpp: -------------------------------------------------------------------------------- 1 | #include "bitwise_join.h" 2 | 3 | static Node_Registrar node_registrar_bitwise_join("bitwise_join"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/bitwise_split.cpp: -------------------------------------------------------------------------------- 1 | #include "bitwise_split.h" 2 | 3 | static Node_Registrar node_registrar_bitwise_split("bitwise_split"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/pi_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "pi_controller.h" 2 | 3 | static Node_Registrar node_registrar_pi_controller("pi_controller"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/vel_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include "vel_estimator.h" 2 | 3 | static Node_Registrar node_registrar_vel_estimator("vel_estimator"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/math_operation.cpp: -------------------------------------------------------------------------------- 1 | #include "math_operation.h" 2 | 3 | static Node_Registrar node_registrar_math_operation("math_operation"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/em_serial_device.cpp: -------------------------------------------------------------------------------- 1 | #include "em_serial_device.h" 2 | 3 | static Node_Registrar node_registrar_em_serial_device("em_serial_device"); -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/meta-xilinx-tools/recipes-bsp/uboot-device-tree/uboot-device-tree.bbappend: -------------------------------------------------------------------------------- 1 | FILESEXTRAPATHS:prepend := "${THISDIR}/files:" 2 | 3 | SRC_URI:append = " file://system-user.dtsi" 4 | 5 | -------------------------------------------------------------------------------- /controller-firmware/Vivado/controller_firmware/controller_firmware_top.xsa: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/controller-firmware/Vivado/controller_firmware/controller_firmware_top.xsa -------------------------------------------------------------------------------- /controller-software/core/.gitignore: -------------------------------------------------------------------------------- 1 | /.ssh 2 | /build 3 | zynq_files/controller/bin 4 | zynq_files/controller/web 5 | 6 | # not using this yet, no need to track 7 | controller/inc/simdjson.h 8 | controller/src/simdjson.cpp -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/get_global_variable.cpp: -------------------------------------------------------------------------------- 1 | #include "get_global_variable.h" 2 | 3 | static Node_Registrar node_registrar_get_global_variable("get_global_variable"); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/src/set_global_variable.cpp: -------------------------------------------------------------------------------- 1 | #include "set_global_variable.h" 2 | 3 | static Node_Registrar node_registrar_set_global_variable("set_global_variable"); -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx/user_2025-02-26-00-50-00.cfg: -------------------------------------------------------------------------------- 1 | # CONFIG_HZ_100 is not set 2 | CONFIG_HZ_1000=y 3 | CONFIG_HZ=1000 4 | # CONFIG_CPU_FREQ is not set 5 | # CONFIG_CPU_IDLE is not set 6 | -------------------------------------------------------------------------------- /controller-software/core/zynq_files/controller/config/em_devices/application.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/controller-software/core/zynq_files/controller/config/em_devices/application.bin -------------------------------------------------------------------------------- /em-os/.petalinux/metadata: -------------------------------------------------------------------------------- 1 | PETALINUX_VER=2023.1 2 | VALIDATE_HW_CHKSUM=1 3 | YOCTO_SDK=839cb8db4728cc0ea4ad3c5083907dd2 4 | HARDWARE_PATH=/home/excessive/github/controller-software/petalinux/controller_firmware_top.xsa 5 | HDF_EXT=xsa 6 | -------------------------------------------------------------------------------- /controller-software/core/zynq_files/controller/config/fpga_configs/test/bitfile.bit.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExcessiveMotion/controller-software/HEAD/controller-software/core/zynq_files/controller/config/fpga_configs/test/bitfile.bit.bin -------------------------------------------------------------------------------- /controller-firmware/Vivado/export bit.bin.tcl: -------------------------------------------------------------------------------- 1 | file copy -force S:/Vivado/controller_firmware/controller_firmware.runs/impl_1/controller_firmware_top.bit S:/Vivado/bitfile.bit 2 | 3 | exec bootgen -w -arch zynq -image S:/Vivado/convert.bif -process_bitstream bin -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/device-tree/device-tree.bbappend: -------------------------------------------------------------------------------- 1 | FILESEXTRAPATHS:prepend := "${THISDIR}/files:" 2 | 3 | SRC_URI:append = " file://system-user.dtsi" 4 | 5 | require ${@'device-tree-sdt.inc' if d.getVar('SYSTEM_DTFILE') != '' else ''} 6 | -------------------------------------------------------------------------------- /em-os/.gitignore: -------------------------------------------------------------------------------- 1 | */*/config.old 2 | */*/rootfs_config.old 3 | build/ 4 | images/linux/ 5 | pre-built/linux/ 6 | .petalinux/* 7 | !.petalinux/metadata 8 | *.o 9 | *.jou 10 | *.log 11 | *.xil 12 | /components/plnx_workspace 13 | /components/yocto 14 | *.old 15 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/systemd-conf/wired.network: -------------------------------------------------------------------------------- 1 | [Match] 2 | Type=ether 3 | Name=!veth* 4 | KernelCommandLine=!nfsroot 5 | KernelCommandLine=!ip 6 | 7 | [Network] 8 | DHCP=yes 9 | 10 | [DHCP] 11 | UseMTU=yes 12 | RouteMetric=10 13 | ClientIdentifier=mac 14 | -------------------------------------------------------------------------------- /em-os/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Set the project root environment variable 3 | export PROJECT_ROOT=$(pwd) 4 | 5 | # Source the Petalinux settings script (adjust the path if needed) 6 | source ~/petalinux/settings.sh 7 | 8 | echo "Environment initialized. PROJECT_ROOT is set to $PROJECT_ROOT" 9 | -------------------------------------------------------------------------------- /Documentation/API Backend/Getting started/Core mechanics.md: -------------------------------------------------------------------------------- 1 | 2 | ## Websocket 3 | 4 | Connection to the API is established through a websocket connection to the device. Once established users can make JSON RPC 2.0 requests for motion and configuration requests 5 | 6 | ## Basic request format 7 | 8 | TBD -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/u-boot/u-boot-xlnx_%.bbappend: -------------------------------------------------------------------------------- 1 | FILESEXTRAPATHS:prepend := "${THISDIR}/files:" 2 | 3 | SRC_URI:append = " file://platform-top.h file://bsp.cfg" 4 | SRC_URI += "file://user_2024-10-27-00-48-00.cfg \ 5 | file://user_2024-11-11-00-54-00.cfg \ 6 | " 7 | 8 | -------------------------------------------------------------------------------- /Documentation/API Backend/Motion/Configuration APIs.md: -------------------------------------------------------------------------------- 1 | 2 | ### Machine setup 3 | Some format for specifying number of axis, robot shape, weight distribution, and kinematics 4 | 5 | ### IO Config 6 | specify names and properties of GPIO elements of machine 7 | 8 | ### Safety 9 | Specify zone limits and velocity limits -------------------------------------------------------------------------------- /em-os/build_pack_image.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # build 4 | petalinux-build 5 | 6 | # package binary files 7 | petalinux-package --boot --fsbl --u-boot --force 8 | 9 | # create image 10 | petalinux-package --wic --outdir ./images/linux --wic-extra-args "-c xz" -b "BOOT.BIN,image.ub,boot.scr" --wks project-spec/configs/rootfs.wks 11 | -------------------------------------------------------------------------------- /controller-firmware/python/src/main.py: -------------------------------------------------------------------------------- 1 | import os, json 2 | 3 | 4 | 5 | 6 | # create required files for a given build config 7 | 8 | def generate_config(build_config_path:str): 9 | # make sure config file exists 10 | if not os.path.exists(build_config_path): 11 | print("Config file not found") 12 | return 13 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/.statistics: -------------------------------------------------------------------------------- 1 | HW_FILE=74619627164c2cb1539a62748a99d3098f65d88aa8c61597f0f0a72f1281736f 2 | SYSTEM_CONF=1c20b0b9962cd5c621a5a6f399b649b8ea8e4a566953f9eadde08e54832be83c 3 | USER_RFS_CFG=5b41377e203fe5b2a9e7534d877796ff0a8ebb41998041aa745636d1deff9405 4 | RFS_CONF=05345a69d516e9654fd12663d5973ddbb23832e895911994cf9ced74545dc297 5 | -------------------------------------------------------------------------------- /em-os/build_run_qemu.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # build 4 | petalinux-build 5 | 6 | # package binary files 7 | petalinux-package --boot --fsbl --u-boot --force 8 | 9 | # create image 10 | petalinux-package --wic --outdir ./images/linux -b "BOOT.BIN,image.ub,boot.scr" --wks project-spec/configs/rootfs.wks 11 | 12 | # run emulator 13 | petalinux-boot --qemu --kernel 14 | 15 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "type": "node", 6 | "request": "launch", 7 | "name": "Launch Program", 8 | "skipFiles": ["/**"], 9 | "program": "${workspaceFolder}/controller-software/web/main.js" // Replace with your entry point file 10 | } 11 | ] 12 | } 13 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/rootfs.wks: -------------------------------------------------------------------------------- 1 | # Description: Creates a partitioned SD card image. Boot files 2 | # are located in the first vfat partition. Rootfs will be in second ext4 partition. 3 | 4 | part /boot --source bootimg-partition --ondisk mmcblk0 --fstype=vfat --label boot --active --align 4 --fixed-size 256M 5 | part / --source rootfs --ondisk mmcblk0 --fstype=ext4 --label root --align 4 --size 2G 6 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/test_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_api.h" 2 | #include 3 | #include 4 | 5 | 6 | int main() { 7 | 8 | controller_api api; 9 | 10 | while(1){ 11 | 12 | api.get_new_call(); 13 | api.run_calls(); 14 | 15 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 16 | } 17 | return 0; 18 | } -------------------------------------------------------------------------------- /controller-firmware/python/generators/dma_instruction_compiler.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | class compiler: 6 | def __init__(self): 7 | self.high_level_instructions = [] 8 | self.output = [] 9 | 10 | def copy(self, src_node, src_addr, dest_node, dest_addr): 11 | self.high_level_instructions.append([src_node, dest_node, "copy"]) 12 | def compile(): 13 | output = [] 14 | return output -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx/user_2025-02-26-01-29-00.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_PREEMPT_LAZY=y 2 | # CONFIG_PREEMPT is not set 3 | CONFIG_PREEMPT_RT=y 4 | CONFIG_RCU_BOOST=y 5 | CONFIG_RCU_BOOST_DELAY=500 6 | CONFIG_SLUB=y 7 | # CONFIG_SLUB_STATS is not set 8 | CONFIG_SLUB_CPU_PARTIAL=y 9 | CONFIG_STACKDEPOT=y 10 | CONFIG_SLUB_DEBUG=y 11 | # CONFIG_SLUB_DEBUG_ON is not set 12 | CONFIG_STACKTRACE=y 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Excessive Motion 2 | 3 | ## An open-source universal machine controller 4 | 5 | This repository contains multiple sectons: 6 | - controller-firmware: FPGA HDL generation 7 | - controller-software: The actual core programs that run on the controller 8 | - em-os: Petalinux project for creating the base linux OS the controller runs 9 | 10 | Be sure to checkout the README files in each of those directories for more details -------------------------------------------------------------------------------- /controller-firmware/python/requirements.txt: -------------------------------------------------------------------------------- 1 | amaranth==0.5.1 2 | amaranth-yosys==0.40.0.0.post99 3 | contourpy==1.3.0 4 | cycler==0.12.1 5 | fonttools==4.53.1 6 | importlib_resources==6.4.4 7 | Jinja2==3.1.4 8 | jschon==0.11.1 9 | kiwisolver==1.4.7 10 | MarkupSafe==2.1.5 11 | matplotlib==3.9.2 12 | packaging==24.1 13 | pillow==10.4.0 14 | pyparsing==3.1.4 15 | python-dateutil==2.9.0.post0 16 | pyvcd==0.4.0 17 | rfc3986==2.0.0 18 | six==1.16.0 19 | wasmtime==24.0.0 20 | -------------------------------------------------------------------------------- /controller-software/core/zynq_files/controller/config/user/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "fpga_config_path": "controller/config/fpga_configs/test", 3 | "fpga_instructions_file": "controller/config/user/test_instructions.json", 4 | "fpga_driver_config_file": "controller/config/user/fpga_driver_config.json", 5 | "node_config_file": "controller/config/user/new_node_config.json", 6 | "temp_path": "controller/config/temp", 7 | "software_update_frequency_hz": 1000 8 | } -------------------------------------------------------------------------------- /controller-firmware/Vivado/save_project.tcl: -------------------------------------------------------------------------------- 1 | # Set the project name and file paths 2 | set project_name "controller_firmware" 3 | set project_path "S:/Vivado/controller_firmware" 4 | set export_script_path "S:/Vivado/controller_firmware.tcl" 5 | 6 | # Open the Vivado project 7 | open_project $project_path/$project_name.xpr 8 | 9 | # Export the project to a Tcl script 10 | write_project_tcl -force $export_script_path 11 | 12 | # Close the Vivado project 13 | close_project -------------------------------------------------------------------------------- /controller-software/web/web-interface/package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "control-interface-spa", 3 | "version": "1.0.0", 4 | "main": "server/app.js", 5 | "scripts": { 6 | "start": "node server/app.js" 7 | }, 8 | "dependencies": { 9 | "body-parser": "^1.20.1", 10 | "express": "^4.18.2", 11 | "express-session": "^1.17.3", 12 | "passport": "^0.6.0", 13 | "passport-local": "^1.0.0", 14 | "ws": "^8.13.0" 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/inc/global_timers.h.old: -------------------------------------------------------------------------------- 1 | #include "fpga_module_driver_factory.h" 2 | //#include "register_helper.h" 3 | 4 | 5 | #pragma once 6 | 7 | class global_timers : public base_driver { 8 | public: 9 | uint32_t load_config(json config, std::string module_name, Node_Core* node_core, fpga_instructions* fpga_instr) override; 10 | 11 | uint32_t run() override; 12 | 13 | private: 14 | 15 | std::vector timer_values; 16 | 17 | }; -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx_%.bbappend: -------------------------------------------------------------------------------- 1 | FILESEXTRAPATHS:prepend := "${THISDIR}/${PN}:" 2 | 3 | SRC_URI:append = " file://bsp.cfg" 4 | KERNEL_FEATURES:append = " bsp.cfg" 5 | SRC_URI += "file://user_2024-11-05-04-03-00.cfg \ 6 | file://user_2024-11-12-22-53-00.cfg \ 7 | file://user_2025-02-26-00-50-00.cfg \ 8 | file://user_2025-02-26-01-29-00.cfg \ 9 | " 10 | SRC_URI:append = " file://patch-6.1.12-rt6.patch" 11 | 12 | -------------------------------------------------------------------------------- /em-os/fixes.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # fix repo issued caused by renaming branches, run after build if it fails due to missing repos 4 | sed -i 's/branch=master/branch=main/g' ${PROJECT_ROOT}/components/yocto/layers/poky/meta/recipes-extended/cracklib/cracklib_2.9.8.bb 5 | 6 | sed -i 's/branch=master/branch=main/g' ${PROJECT_ROOT}/components/yocto/layers/poky/meta/recipes-extended/cracklib/cracklib_2.9.8.bb 7 | 8 | sed -i 's/branch=master/branch=main/g' ${PROJECT_ROOT}/components/yocto/layers/poky/meta/recipes-support/bmap-tools/bmap-tools_git.bb 9 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/u-boot/files/platform-top.h: -------------------------------------------------------------------------------- 1 | #if defined(CONFIG_MICROBLAZE) 2 | #include 3 | #define CONFIG_SYS_BOOTM_LEN 0xF000000 4 | #endif 5 | #if defined(CONFIG_ARCH_ZYNQ) 6 | #include 7 | #endif 8 | #if defined(CONFIG_ARCH_ZYNQMP) 9 | #include 10 | #endif 11 | #if defined(CONFIG_ARCH_VERSAL) 12 | #include 13 | #endif 14 | #if defined(CONFIG_ARCH_VERSAL_NET) 15 | #include 16 | #endif 17 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/conf/layer.conf: -------------------------------------------------------------------------------- 1 | # We have a conf and classes directory, add to BBPATH 2 | BBPATH .= ":${LAYERDIR}" 3 | 4 | # We have recipes-* directories, add to BBFILES 5 | BBFILES += "${LAYERDIR}/recipes-*/*/*.bb \ 6 | ${LAYERDIR}/recipes-*/*/*.bbappend" 7 | 8 | # Define dynamic layers 9 | BBFILES_DYNAMIC += " \ 10 | xilinx-tools:${LAYERDIR}/meta-xilinx-tools/recipes-*/*/*.bbappend \ 11 | " 12 | 13 | BBFILE_COLLECTIONS += "meta-user" 14 | BBFILE_PATTERN_meta-user = "^${LAYERDIR}/" 15 | BBFILE_PRIORITY_meta-user = "7" 16 | LAYERSERIES_COMPAT_meta-user = "langdale" 17 | 18 | IMAGE_INSTALL:append = " nodejs" 19 | -------------------------------------------------------------------------------- /controller-software/core/cmake/zynq_debug.cmake: -------------------------------------------------------------------------------- 1 | 2 | 3 | set(CMAKE_SYSTEM_NAME Linux) 4 | set(CMAKE_SYSTEM_PROCESSOR arm) 5 | 6 | # Specify the cross compiler 7 | set(CMAKE_C_COMPILER arm-linux-gnueabihf-gcc) 8 | set(CMAKE_CXX_COMPILER arm-linux-gnueabihf-g++) 9 | 10 | # Specify the target architecture and NEON support 11 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mcpu=cortex-a9 -mfpu=neon -mfloat-abi=hard -g -O3") 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-psabi -mcpu=cortex-a9 -mfpu=neon -mfloat-abi=hard -g -O3") 13 | 14 | set(CMAKE_BUILD_TYPE Debug) 15 | 16 | # Optionally specify the sysroot if necessary 17 | # set(CMAKE_SYSROOT /path/to/sysroot) 18 | -------------------------------------------------------------------------------- /controller-firmware/python/config/hardware configs/hardware_config.yaml: -------------------------------------------------------------------------------- 1 | hardware_slots: 2 | slot_a: 3 | #TODO: support more than one card type per slot? 4 | card_type: "serial interface card" 5 | card_options: 6 | # set which internal interfaces should be connected to each serial port. 7 | # TODO: support more than one internal interface per serial port? 8 | port_0: [fanuc_rs422_encoder] 9 | port_1: [serial_device] 10 | 11 | slot_b: 12 | 13 | slot_c: 14 | 15 | slot_d: 16 | 17 | fpga_blocks: # extra internal functions 18 | cascaded_PI_controller: 19 | count: 1 20 | parameters: # build parameters for the block when instantiating it 21 | instances: 1 22 | -------------------------------------------------------------------------------- /controller-software/config/electrical/user_params.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, field 2 | 3 | 4 | # default param options to add to every device class 5 | 6 | @dataclass 7 | class USER_PARAMS: 8 | 9 | # User defined parameters 10 | name: str = field(default="PMAC Motor", metadata={'desc': 'Name of the motor', 'required':False}) 11 | model_number: str = field(default=None, metadata={'desc': 'Model number of the motor', 'required':False}) 12 | manufacturer: str = field(default=None, metadata={'desc': 'Manufacturer of the motor', 'required':False}) 13 | location: str = field(default=None, metadata={'desc': 'Location of the motor', 'required':False}) 14 | description: str = field(default=None, metadata={'desc': 'Description of the motor', 'required':False}) -------------------------------------------------------------------------------- /controller-firmware/Vivado/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything 2 | * 3 | 4 | # But not these files... 5 | !.gitignore 6 | !component.xml 7 | !.project 8 | !.cproject 9 | !constraints.xdc 10 | !*.xci 11 | !*.v 12 | !*.sv 13 | !*.svh 14 | !*.vh 15 | !*.vhd 16 | !*.tcl 17 | !*.wcfg 18 | !*.h 19 | !*.cc 20 | !*.cpp 21 | !*.c 22 | !*.inc 23 | !*.py 24 | !*.xdc 25 | !*.md 26 | !*.xsa 27 | !*.bif 28 | 29 | # ...even if they are in subdirectories 30 | !*/ 31 | 32 | # Except for xilinx auto-generated files 33 | /**/*.srcs/ 34 | /**/*.cache/ 35 | /**/*.hw/ 36 | /**/*.ip_user_files/ 37 | /**/*.runs/ 38 | /**/*.sim/ 39 | /**/*_BSP*/ 40 | /**/*_HW*/ 41 | /**/.Xil/ 42 | /**/.metadata/ 43 | /**/RemoteSystemsTempFiles/ 44 | /**/doc/ 45 | /**/webtalk/ 46 | /**/hdl/ 47 | /**/demo_tb/ 48 | /**/misc/ 49 | /**/bd/ 50 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/init-ifupdown/interfaces: -------------------------------------------------------------------------------- 1 | # /etc/network/interfaces -- configuration file for ifup(8), ifdown(8) 2 | 3 | # The loopback interface 4 | auto lo 5 | iface lo inet loopback 6 | 7 | # Wireless interfaces 8 | iface wlan0 inet dhcp 9 | wireless_mode managed 10 | wireless_essid any 11 | wpa-driver wext 12 | wpa-conf /etc/wpa_supplicant.conf 13 | 14 | iface atml0 inet dhcp 15 | 16 | # Wired or wireless interfaces 17 | auto eth0 18 | iface eth0 inet dhcp 19 | iface eth1 inet dhcp 20 | 21 | # Ethernet/RNDIS gadget (g_ether) 22 | # ... or on host side, usbnet and random hwaddr 23 | iface usb0 inet static 24 | address 192.168.7.2 25 | netmask 255.255.255.0 26 | network 192.168.7.0 27 | gateway 192.168.7.1 28 | 29 | # Bluetooth networking 30 | iface bnep0 inet dhcp 31 | 32 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/inc/api_objects.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "json.hpp" 5 | 6 | 7 | #include "machine_state.h" 8 | #include "print_uint32.h" 9 | #include "print_float.h" 10 | #include "configure_node.h" 11 | 12 | using json = nlohmann::json; 13 | 14 | #pragma once 15 | 16 | uint32_t get_new_call_object_from_call_id(std::vector>* calls, const api_call_id_t* api_call_id); 17 | 18 | uint32_t create_new_call_obj_from_string(std::vector>* calls, std::string api_call_name, uint32_t* api_call_number); 19 | 20 | uint32_t remove_last_call_obj_from_string(std::vector>* calls, uint32_t* api_call_number); // remove the last call object from the vector, used if the call object is not valid -------------------------------------------------------------------------------- /controller-software/web/web-interface/client/login/login.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Login 7 | 13 | 14 | 15 |

Login

16 |
17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 |
25 | 26 | 27 | -------------------------------------------------------------------------------- /controller-firmware/Vivado/build.tcl: -------------------------------------------------------------------------------- 1 | open_project S:/Vivado/controller_firmware/controller_firmware.xpr 2 | 3 | update_files -from_files S:/Vivado/autogen_sources/controller.v -to_files S:/Vivado/controller_firmware/controller_firmware.srcs/sources_1/imports/autogen_sources/controller.v -filesets [get_filesets *] 4 | 5 | update_module_reference controller_firmware_Controller_0_0 6 | 7 | set_param general.maxThreads 8 8 | 9 | # update compile order (important if you’ve added/removed modules) 10 | update_compile_order -fileset sources_1 11 | 12 | # reset any prior runs (so you always start fresh) 13 | reset_run synth_1 14 | # launch synthesis 15 | launch_runs synth_1 -jobs 8 16 | wait_on_run synth_1 17 | 18 | # now implementation 19 | reset_run impl_1 20 | launch_runs impl_1 -to_step write_bitstream -jobs 8 21 | wait_on_run impl_1 22 | 23 | # cleanly exit 24 | exit 25 | -------------------------------------------------------------------------------- /controller-software/core/CMakePresets.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": 3, 3 | "configurePresets": [ 4 | { 5 | "name": "default", 6 | "hidden": true, 7 | "generator": "Ninja", 8 | "binaryDir": "${sourceDir}/build", 9 | "cacheVariables": {} 10 | }, 11 | { 12 | "name": "ZYNQ-Debug", 13 | "description": "Cross compile for ARM Cortex-A9 with NEON support", 14 | "inherits": "default", 15 | "toolchainFile": "${sourceDir}/cmake/zynq_debug.cmake", 16 | "cacheVariables": { 17 | "CMAKE_BUILD_TYPE": "Debug" 18 | } 19 | } 20 | ], 21 | "buildPresets": [ 22 | { 23 | "name": "ZYNQ-Debug", 24 | "configurePreset": "ZYNQ-Debug", 25 | "description": "Build debug version for ZYNQ", 26 | "jobs": 4 27 | } 28 | ], 29 | "testPresets": [] 30 | } 31 | -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/src/global_timers.cpp.old: -------------------------------------------------------------------------------- 1 | #include "global_timers.h" 2 | 3 | static Driver_Registrar registrar("global_timers"); 4 | 5 | uint32_t global_timers::load_config(json config, std::string module_name, Node_Core* node_core, fpga_instructions* fpga_instr){ 6 | 7 | if(!load_json_value(config, "node_address", &node_address)){ 8 | std::cerr << "Failed to load node address" << std::endl; 9 | return 1; 10 | } 11 | 12 | // configure registers 13 | Address_Map_Loader loader; 14 | loader.setup(&config, &base_mem, node_address, new std::vector); 15 | 16 | // timer_values.push_back(loader.get_register("counter", 0)); 17 | // loader.sync_with_PS(timer_values[0]); 18 | 19 | // timer_values[0]->set_value(100); 20 | 21 | 22 | return 0; 23 | } 24 | 25 | uint32_t global_timers::run(){ 26 | // do nothing for now 27 | return 0; 28 | } -------------------------------------------------------------------------------- /controller-software/config/electrical/test.py: -------------------------------------------------------------------------------- 1 | from motor_drives.em_hvsd import EM_HVSD 2 | from motors.motor_rotary import ROTARY_MOTOR 3 | from connectors import CONNECTOR_FUNCTIONS 4 | 5 | if __name__ == '__main__': 6 | motor = ROTARY_MOTOR() 7 | drive = EM_HVSD() 8 | 9 | motor.set_mode(motor.MODES.PMSM) 10 | motor.common_params.rated_speed.value = 3000 11 | motor.common_params.rated_voltage.value = 220 12 | motor.common_params.hard_max_current.value = 10 13 | #motor.specific_params.pole_pairs.value = 4 14 | 15 | drive.set_mode(drive.MODES.PMSM_FOC) 16 | drive.common_params.serial_address.value = 1 17 | 18 | print(CONNECTOR_FUNCTIONS.validate_connect_to(drive.connectors["motor"], [motor.connectors["motor"]])) 19 | 20 | print(CONNECTOR_FUNCTIONS.connect_to(drive.connectors["motor"], [motor.connectors["motor"]])) 21 | 22 | print(CONNECTOR_FUNCTIONS.update_from_connector(drive.connectors["motor"], motor.connectors["motor"])) -------------------------------------------------------------------------------- /controller-software/web/web-interface/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Launch via NPM", 9 | "request": "launch", 10 | "runtimeArgs": [ 11 | "run-script", 12 | "debug" 13 | ], 14 | "runtimeExecutable": "npm", 15 | "skipFiles": [ 16 | "/**" 17 | ], 18 | "type": "node" 19 | }, 20 | { 21 | "type": "node", 22 | "request": "launch", 23 | "name": "Launch Program", 24 | "skipFiles": [ 25 | "/**" 26 | ], 27 | "program": "${workspaceFolder}\\server\\app.js" 28 | } 29 | ] 30 | } -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/test_api.cpp: -------------------------------------------------------------------------------- 1 | #include "web_api.h" 2 | #include 3 | #include 4 | 5 | int main() { 6 | web_api api; 7 | 8 | uint32_t api_call_number; 9 | 10 | for(int c=0; c<1; c++){ 11 | 12 | for(int i=0; i<8; i++){ 13 | json call = { 14 | {"call_name", "print_uint32"}, 15 | {"value", i + 8*c} 16 | }; 17 | //std::cout << "value: " << i + 8*c << std::endl; 18 | api.add_call(&call, &api_call_number); 19 | } 20 | json call = { 21 | {"call_name", "print_uint32"}, 22 | {"bad_key", 0} 23 | }; 24 | //api.add_call(&call, &api_call_number); 25 | 26 | api.write_web_calls_to_controller(); 27 | 28 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 29 | json response; 30 | api.get_completed_calls(&response); 31 | 32 | std::cout << response.dump(4) << std::endl; // Pretty print with indentation 33 | 34 | } 35 | return 0; 36 | } -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/COPYING.MIT: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. 18 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/busybox/inetd.conf: -------------------------------------------------------------------------------- 1 | #/etc/inetd.conf: see inetd(8) for further informations. 2 | # 3 | # Internet server configuration database 4 | # 5 | # If you want to disable an entry so it isn't touched during 6 | # package updates just comment it out with a single '#' character. 7 | # 8 | # 9 | # 10 | #:INTERNAL: Internal services 11 | #echo stream tcp nowait root internal 12 | #echo dgram udp wait root internal 13 | #chargen stream tcp nowait root internal 14 | #chargen dgram udp wait root internal 15 | #discard stream tcp nowait root internal 16 | #discard dgram udp wait root internal 17 | #daytime stream tcp nowait root internal 18 | #daytime dgram udp wait root internal 19 | #time stream tcp nowait root internal 20 | #time dgram udp wait root internal 21 | telnet stream tcp nowait root telnetd telnetd -i 22 | ftp stream tcp nowait root ftpd ftpd -w 23 | -------------------------------------------------------------------------------- /Documentation/API Backend/Project goals and discussion.md: -------------------------------------------------------------------------------- 1 | This document should be used as a guide for developers to understand the overall direction of this module. Later as things kick off this document can probably be removed 2 | 3 | ## Dependencies 4 | - **[MinnowServer](https://github.com/RealTimeLogic/MinnowServer)** 5 | - Handle TCP/IP transmission 6 | - Maintains lower layer web socket needs 7 | - TBD 8 | 9 | ## Proposed transmission protocols 10 | 11 | ### JSON RPC 12 | Pros: 13 | - Human readable 14 | - Easy request response structure 15 | - Platform agnostic 16 | Cons: 17 | - Considerable amount of extra data being transferred 18 | - Slower serialization and deserialization steps 19 | 20 | ### Raw bytecode 21 | Pros: 22 | - Very small footprint 23 | - Faster serialization on the server side 24 | Cons: 25 | - Not human readable 26 | 27 | ## Some things to think about 28 | 29 | - Jogging: 30 | - Jog button pressed vs held. Is multiple messages being sent while button held or will we have a jog start and jog stop command? 31 | - General requirement should be when I let go of the button for jogging then the arm stops as well. No delay because the robot is trying to execute a series of move commands. -------------------------------------------------------------------------------- /controller-software/core/devices/em_hvsd/config.yaml: -------------------------------------------------------------------------------- 1 | gui_node_config: 2 | common: 3 | params: 4 | serial_address: 5 | name: "Serial address" 6 | type: "int" 7 | unit: "" 8 | required: true 9 | min: 0 10 | max: 255 11 | description: "Address of the device on the serial bus" 12 | 13 | hard_current_limit: 14 | name: "Hard current limit" 15 | type: "float" 16 | unit: "Amps" 17 | required: true 18 | min: 0 19 | max: 60 20 | description: "Maximum current that the device can output in percent of the maximum current" 21 | auto_fill_enable: true 22 | auto_fill_by: ["hard_max_current"] 23 | 24 | connectors: 25 | #TODO: Add connectors 26 | 27 | mode_specific: 28 | pmsm_foc: 29 | params: 30 | commutation_offset: 31 | name: "Commutation offset" 32 | type: "float" 33 | unit: "Radians" 34 | required: false 35 | min: -6.28 36 | max: 6.28 37 | description: "Offset of the commutation angle in radians" 38 | auto_fill_enable: true 39 | auto_fill_by: [] 40 | 41 | connectors: 42 | #TODO: Add connectors -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/inc/fanuc_encoders.h: -------------------------------------------------------------------------------- 1 | #include "fpga_module_driver_factory.h" 2 | //#include "register_helper.h" 3 | 4 | 5 | #pragma once 6 | 7 | class fanuc_encoders : public base_driver { 8 | public: 9 | //uint32_t load_config(json* config, std::string module_name, Node_Core* node_core, fpga_instructions* fpga_instr) override; 10 | 11 | uint32_t custom_load_config(json* user_driver_config) override; 12 | 13 | uint32_t run() override; 14 | 15 | private: 16 | 17 | Register* trigger = nullptr; 18 | 19 | Register* multiturn_count = nullptr; 20 | Register* singleturn_count = nullptr; 21 | Register* commutation_count = nullptr; 22 | Register* crc_error = nullptr; 23 | Register* no_response = nullptr; 24 | Register* unindexed = nullptr; 25 | Register* battery_fail = nullptr; 26 | Register* done = nullptr; 27 | 28 | struct encoder_data{ 29 | uint32_t multiturn_count = 0; 30 | uint32_t singleturn_count = 0; 31 | uint16_t commutation_angle = 0; 32 | bool battery_fail = false; 33 | bool unindexed = false; 34 | bool no_resonse = false; 35 | bool crc_error = false; 36 | bool done = false; 37 | }; 38 | 39 | uint32_t old_microseconds = 0; 40 | 41 | std::vector encoders; 42 | 43 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/get_global_variable.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | /* 4 | 5 | Make a global variable accessible by nodes 6 | 7 | */ 8 | 9 | #pragma once 10 | 11 | class get_global_variable: public base_node { 12 | private: 13 | 14 | public: 15 | 16 | get_global_variable(){ 17 | execution_number = -1; 18 | type = "get_global_variable"; 19 | } 20 | 21 | unsigned int run() override { 22 | // nothing to do 23 | return 0; 24 | } 25 | 26 | uint32_t set_global_var_output(std::string output_name, global_variable* variable) override { 27 | // add if output doesn't already exist 28 | if(outputs.find(output_name) != outputs.end()){ 29 | std::cerr << "Error: output name already exists" << std::endl; 30 | return 2; // output name already exists 31 | } 32 | 33 | // TODO: might want to store these pointers somewhere so they can be freed later 34 | void** variable_ptr = new void*; 35 | 36 | variable->get_data_pointer(variable_ptr); 37 | 38 | outputs.emplace(output_name, output(variable->get_type(), *variable_ptr, &execution_number)); 39 | 40 | return 0; 41 | } 42 | 43 | }; -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /controller-firmware/src/autogen_sources 2 | *.jou 3 | *.log 4 | /.Xil 5 | /controller-firmware/Vivado/.Xil 6 | *.pyc 7 | /controller-software/config/venv 8 | /controller-software/web/node_modules/.bin 9 | /controller-software/web/node_modules 10 | 11 | controller-software/* 12 | !controller-software/config/ 13 | !controller-software/core/ 14 | !controller-software/vitis/ 15 | !controller-software/web/ 16 | /controller-software/web/build 17 | controller-software/core/api_drivers/test_controller 18 | 19 | .vscode/settings.json 20 | controller-firmware/python/pyvenv.cfg 21 | *.vcd 22 | controller-software/core/zynq_files/controller/config/fpga_configs/bit_files/bitfile.bit.bin 23 | controller-software/core/zynq_files/controller/config/fpga_configs/controller_config.json 24 | controller-software/core/zynq_files/controller/bin/controller_core 25 | fpga_config.json 26 | 27 | controller-software/web/web-interface/node_modules 28 | 29 | 30 | # ignore large data capture files 31 | controller-firmware/python/src/sandbox/analog.csv 32 | controller-firmware/python/src/sandbox/fanuc_encoder_rs485_raw.csv 33 | controller-firmware/python/src/sandbox/fanuc_rs485_detected_edges.csv 34 | controller-firmware/python/src/sandbox/yaskawa_decoded_manchester_raw.csv 35 | controller-firmware/python/src/sandbox/t.txt 36 | 37 | 38 | controller-firmware/Vivado/autogen_sources/* 39 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/inc/controller_api.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "json.hpp" 7 | 8 | #pragma once 9 | #include "api_objects.h" 10 | 11 | using json = nlohmann::json; 12 | 13 | class controller_api { 14 | private: 15 | std::vector> calls; 16 | shared_mem shared_mem_obj; 17 | Base_API default_call_obj; 18 | api_call_id_t api_call_id = api_call_id_t::DEFAULT; 19 | 20 | uint32_t get_next_api_call_id_from_shared_mem(); 21 | 22 | uint32_t get_last_web_to_controller_call(); 23 | 24 | 25 | static std::mutex data_mutex; 26 | static std::condition_variable data_ready; 27 | static std::string* json_to_parse; 28 | static json* parsed_json; 29 | static bool* json_parsed; 30 | static std::atomic parsing_complete; 31 | static void json_parser_thread(); 32 | 33 | void setup_json_parser(); 34 | 35 | Node_Core* node_core = nullptr; 36 | 37 | public: 38 | controller_api(); 39 | 40 | void setup(Node_Core* node_core_); 41 | 42 | uint32_t get_new_call(); 43 | 44 | uint32_t run_calls(); 45 | 46 | ~controller_api(); 47 | }; 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /controller-firmware/python/src/testing_block.py: -------------------------------------------------------------------------------- 1 | from amaranth import * 2 | from amaranth.sim import Simulator 3 | from amaranth.lib.memory import Memory 4 | from amaranth.back import verilog 5 | from amaranth.lib.wiring import In, Out 6 | from amaranth.lib import wiring 7 | 8 | 9 | # minimal rtl block for testing, just some bram 10 | 11 | 12 | class test_block(wiring.Component): 13 | 14 | def __init__(self, size) -> None: 15 | super().__init__({ 16 | "write_data": In(32), 17 | "write_enable": In(1), 18 | "address": In(16), 19 | "read_data": Out(32) 20 | }) 21 | self.size = size 22 | 23 | def elaborate(self, platform): 24 | m = Module() 25 | 26 | m.submodules.memory = self.memory = Memory(shape=unsigned(32), depth=self.size, init=[]) # small memory for testing 27 | 28 | self.externalReadPort = self.memory.read_port(domain="sync_100") 29 | self.externalWritePort = self.memory.write_port(domain="sync_100") 30 | 31 | # connect external memory interfaces 32 | m.d.comb += self.externalReadPort.addr.eq(self.address) 33 | m.d.comb += self.externalWritePort.addr.eq(self.address) 34 | m.d.comb += self.externalWritePort.data.eq(self.write_data) 35 | m.d.comb += self.externalWritePort.en.eq(self.write_enable) 36 | m.d.comb += self.read_data.eq(self.externalReadPort.data) 37 | 38 | return m 39 | -------------------------------------------------------------------------------- /controller-software/core/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "name": "Remote Debug on Zynq", 6 | "type": "cppdbg", 7 | "request": "launch", 8 | "program": "${workspaceFolder}/zynq_files/controller/bin/controller_core", 9 | "args": [], 10 | "stopAtEntry": false, // stop at main? 11 | "targetArchitecture": "arm", 12 | "cwd": "${workspaceFolder}/build", 13 | "environment": [], 14 | "externalConsole": false, 15 | "MIMode": "gdb", 16 | "miDebuggerPath": "/usr/bin/gdb-multiarch", // Ensure gdb-multiarch is installed in WSL 17 | "setupCommands": [ 18 | { 19 | "description": "Enable pretty-printing for gdb", 20 | "text": "-enable-pretty-printing", 21 | "ignoreFailures": true 22 | } 23 | ], 24 | "preLaunchTask": "Launch gdbserver on Zynq", 25 | "miDebuggerServerAddress": "192.168.2.100:1234", // Replace with your SSH alias and port 26 | "sourceFileMap": { 27 | "/home/em-os/controller/": "${workspaceFolder}/zynq_files/controller/" // Adjust if your source files are in a different location 28 | }, 29 | "logging": { 30 | "trace": true, 31 | "traceResponse": true, 32 | "engineLogging": true 33 | } 34 | } 35 | ] 36 | } 37 | -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/yaskawa_decoding.py: -------------------------------------------------------------------------------- 1 | import libscrc 2 | 3 | # load in csv with decoded bytes from manchester signal 4 | 5 | # sort packets by finding the start cmd from the controller 6 | 7 | file = "controller-firmware/python/src/sandbox/t.txt" 8 | 9 | def bin_to_bytes(s): 10 | return bytes(int(s[i:i+8][::-1], 2) for i in range(0, len(s), 8)) 11 | 12 | def get_packets(filename): 13 | with open(filename) as f: 14 | next(f) # skip header 15 | packet = [] 16 | prev_ts = None 17 | for line in f: 18 | ts, value = line.strip().split(',') 19 | ts = float(ts) 20 | if prev_ts is not None and ts - prev_ts > 500e-9: 21 | yield packet 22 | packet = [] 23 | prev_ts = ts 24 | packet.append(int(value)) 25 | yield packet 26 | 27 | results = [] 28 | 29 | for packet in get_packets(file): 30 | s = ''.join(str(x) for x in packet) 31 | if not s.startswith('0101010101010101001111110') or not s.count('111111') == 2 or not s.endswith('01111110'): 32 | print("bad packet") 33 | continue 34 | payload = s[25:-8].replace('111110', '11111') 35 | if payload == '1111111111111111': 36 | continue 37 | 38 | print(payload) 39 | b = bin_to_bytes(payload) 40 | 41 | #results.append(b) 42 | #print("\t".join(f"{byte:d}" for byte in b)) 43 | 44 | #print(results) 45 | 46 | # 10100000 00100100 01011000 10010001 11011001 10110100 00000000 00100010 01011111 10010000 00000000 00000000 00100110 00010111 -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi: -------------------------------------------------------------------------------- 1 | /include/ "system-conf.dtsi" 2 | / { 3 | 4 | 5 | 6 | 7 | fpga_shared_mem@000F0000 { 8 | #address-cells = <1>; 9 | #size-cells = <1>; 10 | ranges; 11 | 12 | compatible = "generic-uio"; 13 | reg = <0x000F0000 0x00008000>; // 32KB 14 | 15 | // IRQ_F2P[0] high level (memory update start) 16 | // IRQ_F2P[1] high level (memory update done) 17 | // IRQ_F2P[2] high level (DMA cycle start) 18 | // IRQ_F2P[3] high level (DMA cycle done) 19 | }; 20 | 21 | fpga_mem_update_running_irq{ 22 | compatible = "generic-uio"; 23 | interrupts = <0 29 4>; 24 | interrupt-names = "mem_update_run"; 25 | interrupt-parent = <&intc>; 26 | }; 27 | 28 | fpga_mem_update_done_irq{ 29 | compatible = "generic-uio"; 30 | interrupts = <0 30 4>; 31 | interrupt-names = "mem_update_done"; 32 | interrupt-parent = <&intc>; 33 | }; 34 | 35 | fpga_cycle_running_irq{ 36 | compatible = "generic-uio"; 37 | interrupts = <0 31 4>; 38 | interrupt-names = "cycle_run"; 39 | interrupt-parent = <&intc>; 40 | }; 41 | 42 | fpga_cycle_done_irq{ 43 | compatible = "generic-uio"; 44 | interrupts = <0 32 4>; 45 | interrupt-names = "cycle_done"; 46 | interrupt-parent = <&intc>; 47 | }; 48 | 49 | 50 | chosen { 51 | bootargs = "console=ttyPS0,115200 earlycon root=/dev/mmcblk0p2 rw rootwait clk_ignore_unused uio_pdrv_genirq.of_id=generic-uio"; 52 | stdout-path = "serial0:115200n8"; 53 | }; 54 | 55 | 56 | }; 57 | 58 | 59 | &sdhci0 { 60 | disable-wp; 61 | }; 62 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/inc/base_node.h: -------------------------------------------------------------------------------- 1 | #include "json.hpp" 2 | #include 3 | #include "node_io.h" 4 | #include "base_global_variable.h" 5 | 6 | #pragma once 7 | 8 | using json = nlohmann::json; 9 | 10 | // TODO: add settings support to configure nodes 11 | 12 | class base_node{ 13 | protected: 14 | std::map inputs; 15 | std::map outputs; 16 | 17 | std::string type = ""; 18 | 19 | bool marked_for_deletion = false; 20 | 21 | public: 22 | 23 | int execution_number = -256; 24 | 25 | base_node(); 26 | uint32_t get_output_object(std::string output_name, output*& ptr); 27 | uint32_t connect_input(std::string input_name, std::shared_ptr source_node, std::string source_output_name); 28 | uint32_t connect_input(std::string input_name, output* source_output); 29 | 30 | // only for set/get global var nodes 31 | virtual uint32_t set_global_var_input(std::string input_name, global_variable* variable); 32 | virtual uint32_t set_global_var_output(std::string output_name, global_variable* variable); 33 | 34 | std::string get_type(){return type;} 35 | 36 | 37 | uint32_t disconnect_input(std::string input_name); 38 | uint32_t reconnect_inputs(); 39 | uint32_t configure_execution_number(bool* execution_number_modified); 40 | void mark_for_deletion(); 41 | virtual uint32_t run() = 0; 42 | virtual uint32_t configure_settings(json* data); 43 | ~base_node(); 44 | }; -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-bsp/device-tree/device-tree-sdt.inc: -------------------------------------------------------------------------------- 1 | SRC_URI:append = " ${@" ".join(["file://%s" % f for f in (d.getVar('EXTRA_DT_FILES') or "").split()])}" 2 | 3 | # We need the deployed output 4 | PROC_TUNE:versal = "cortexa72" 5 | PROC_TUNE:zynqmp = "cortexa53" 6 | APU_DT_FILE_NAME ?= "${PROC_TUNE}-${SOC_FAMILY}-linux.dts" 7 | 8 | EXTRA_DT_FILES ??= "" 9 | EXTRA_DTFILE_PREFIX ??= "system" 10 | EXTRA_DTFILES_BUNDLE ??= "" 11 | 12 | do_configure:append () { 13 | if ! grep -e "^/include/ \"system-user.dtsi\"$" "${DT_FILES_PATH}/${APU_DT_FILE_NAME}"; then 14 | echo "/include/ \"system-user.dtsi\"" >> ${DT_FILES_PATH}/${APU_DT_FILE_NAME} 15 | fi 16 | for f in ${EXTRA_DT_FILES}; do 17 | cp ${WORKDIR}/${f} ${DT_FILES_PATH}/ 18 | done 19 | } 20 | 21 | devicetree_do_compile:append() { 22 | import subprocess 23 | 24 | apu_dt_file = d.getVar('APU_DT_FILE_NAME').replace('.dts', '.dtb') or '' 25 | if not apu_dt_file or not os.path.isfile(apu_dt_file): 26 | return 27 | 28 | if d.getVar('EXTRA_DTFILES_BUNDLE'): 29 | ccdtb_prefix = d.getVar('EXTRA_DTFILE_PREFIX') 30 | extra_dt_files = d.getVar('EXTRA_DT_FILES').split() or [] 31 | for dtsfile in extra_dt_files: 32 | dtname = os.path.splitext(os.path.basename(dtsfile))[0] 33 | if os.path.isfile(f"{dtname}.dtbo"): 34 | fdtargs = ["fdtoverlay", "-o", f"{ccdtb_prefix}-{dtname}.dtb", "-i", apu_dt_file, f"{dtname}.dtbo"] 35 | bb.note("Running {0}".format(" ".join(fdtargs))) 36 | subprocess.run(fdtargs, check = True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) 37 | } 38 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/README: -------------------------------------------------------------------------------- 1 | This README file contains information on the contents of the 2 | meta-user layer. 3 | 4 | Please see the corresponding sections below for details. 5 | 6 | 7 | Dependencies 8 | ============ 9 | 10 | This layer depends on: 11 | 12 | URI: git://git.openembedded.org/bitbake 13 | branch: master 14 | 15 | URI: git://git.openembedded.org/openembedded-core 16 | layers: meta 17 | branch: master 18 | 19 | URI: git://git.yoctoproject.org/xxxx 20 | layers: xxxx 21 | branch: master 22 | 23 | 24 | Patches 25 | ======= 26 | 27 | Please submit any patches against the meta-user layer to the 28 | xxxx mailing list (xxxx@zzzz.org) and cc: the maintainer: 29 | 30 | Maintainer: XXX YYYYYY 31 | 32 | 33 | Table of Contents 34 | ================= 35 | 36 | I. Adding the meta-user layer to your build 37 | II. Misc 38 | 39 | 40 | I. Adding the meta-user layer to your build 41 | ================================================= 42 | 43 | --- replace with specific instructions for the meta-user layer --- 44 | 45 | In order to use this layer, you need to make the build system aware of 46 | it. 47 | 48 | Assuming the meta-user layer exists at the top-level of your 49 | yocto build tree, you can add it to the build system by adding the 50 | location of the meta-user layer to bblayers.conf, along with any 51 | other layers needed. e.g.: 52 | 53 | BBLAYERS ?= " \ 54 | /path/to/yocto/meta \ 55 | /path/to/yocto/meta-poky \ 56 | /path/to/yocto/meta-yocto-bsp \ 57 | /path/to/yocto/meta-meta-user \ 58 | " 59 | 60 | 61 | II. Misc 62 | ======== 63 | 64 | --- replace with specific information about the meta-user layer --- 65 | -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/inc/yaskawa_encoders.h: -------------------------------------------------------------------------------- 1 | #include "fpga_module_driver_factory.h" 2 | 3 | 4 | #pragma once 5 | 6 | class yaskawa_encoders : public base_driver { 7 | public: 8 | uint32_t custom_load_config(json* user_driver_config) override; 9 | 10 | uint32_t run() override; 11 | 12 | private: 13 | 14 | uint16_t t = 0; 15 | 16 | Register* trigger = nullptr; 17 | Register* req_packet = nullptr; 18 | 19 | const uint64_t singleturn_res = 1ULL << 32; 20 | const uint64_t multiturn_res = 1ULL << 16; // only 16 bits for multiturn count 21 | const uint64_t half_multiturn_count = multiturn_res >> 1; 22 | 23 | 24 | 25 | struct encoder_data{ 26 | uint32_t multiturn_count = 0; 27 | uint32_t singleturn_count = 0; 28 | uint16_t commutation_angle = 0; 29 | bool battery_fail = false; 30 | bool unindexed = false; 31 | bool no_resonse = false; 32 | bool crc_error = false; 33 | bool done = false; 34 | 35 | double total_turns = 0.0; // calculated from multiturn and singleturn counts 36 | bool turn_wrap = false; // flag if multiturn count is ambiguous 37 | 38 | Register* multiturn_count_reg = nullptr; 39 | Register* singleturn_count_reg = nullptr; 40 | Register* commutation_count_reg = nullptr; 41 | Register* crc_error_reg = nullptr; 42 | Register* no_response_reg = nullptr; 43 | Register* unindexed_reg = nullptr; 44 | Register* battery_fail_reg = nullptr; 45 | Register* done_reg = nullptr; 46 | 47 | }; 48 | 49 | void convert_turns(encoder_data* data); 50 | std::vector encoders; 51 | 52 | }; -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/instructions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | END = 0 4 | NOP = 1 5 | COPY = 2 6 | 7 | 8 | def create_instruction(source_node, destination_node, source_address, destination_address, instruction): 9 | data = source_node | (destination_node << 8) | (source_address << 16) | (destination_address << 32) | (instruction << 48) 10 | return data 11 | 12 | 13 | def extract_instruction(data): 14 | source_node = data & 0xff 15 | destination_node = (data >> 8) & 0xff 16 | source_address = (data >> 16) & 0xffff 17 | destination_address = (data >> 32) & 0xffff 18 | instruction = (data >> 48) & 0xf 19 | 20 | print('source_node:', source_node) 21 | print('destination_node:', destination_node) 22 | print('source_address:', source_address) 23 | print('destination_address:', destination_address) 24 | print('instruction:', instruction) 25 | 26 | return source_node, destination_node, source_address, destination_address, instruction 27 | 28 | # extract_instruction(562950020530432) 29 | # print("\n\n") 30 | # extract_instruction(562954315563264) 31 | # print("\n\n") 32 | # extract_instruction(562962905563392) 33 | # print("\n\n") 34 | # extract_instruction(562949953552385) 35 | # print("\n\n") 36 | # extract_instruction(562954248650753) 37 | # print("\n\n") 38 | 39 | extract_instruction(562958543355906) 40 | print("\n\n") 41 | 42 | extract_instruction(562962838388738) 43 | print("\n\n") 44 | 45 | extract_instruction(562967133421570) 46 | print("\n\n") 47 | 48 | extract_instruction(562971428454402) 49 | print("\n\n") 50 | 51 | extract_instruction(562950020530432) 52 | print("\n\n") 53 | 54 | print(np.log2(256)) 55 | 56 | 57 | #0xb6fc8008 58 | #0xb6fc800c 59 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/inc/node_factory.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | #include "base_node.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class Node_Factory { 11 | public: 12 | using Creator = std::function()>; 13 | 14 | // Registers a creator function with a string key 15 | static void registerType(const std::string& typeName, Creator creator) { 16 | auto& map = getMap(); 17 | if (map.find(typeName) != map.end()) { 18 | throw std::runtime_error("Type already registered: " + typeName); 19 | } 20 | map[typeName] = creator; 21 | } 22 | 23 | // Creates an object based on the string key 24 | static std::shared_ptr create_shared(const std::string& typeName) { 25 | auto& map = getMap(); 26 | auto it = map.find(typeName); 27 | if (it != map.end()) { 28 | return (it->second)(); 29 | } 30 | throw std::runtime_error("Type not registered: " + typeName); 31 | } 32 | 33 | private: 34 | // Returns the static map 35 | static std::unordered_map& getMap() { 36 | static std::unordered_map map; 37 | return map; 38 | } 39 | }; 40 | 41 | // Template class to register a type with the factory 42 | template 43 | class Node_Registrar { 44 | public: 45 | Node_Registrar(const std::string& typeName) { 46 | Node_Factory::registerType(typeName, []() -> std::shared_ptr { 47 | return std::make_shared(); 48 | }); 49 | 50 | std::cout << "Registered node: " << typeName << std::endl; 51 | } 52 | }; 53 | -------------------------------------------------------------------------------- /em-os/project-spec/meta-user/recipes-kernel/linux/linux-xlnx/user_2024-11-05-04-03-00.cfg: -------------------------------------------------------------------------------- 1 | CONFIG_STAGING=y 2 | # CONFIG_RTL8192U is not set 3 | # CONFIG_RTLLIB is not set 4 | # CONFIG_RTS5208 is not set 5 | 6 | # 7 | # IIO staging drivers 8 | # 9 | 10 | # 11 | # Accelerometers 12 | # 13 | # CONFIG_ADIS16203 is not set 14 | # CONFIG_ADIS16240 is not set 15 | # end of Accelerometers 16 | 17 | # 18 | # Analog to digital converters 19 | # 20 | # CONFIG_AD7816 is not set 21 | # end of Analog to digital converters 22 | 23 | # 24 | # Analog digital bi-direction converters 25 | # 26 | # CONFIG_ADT7316 is not set 27 | # end of Analog digital bi-direction converters 28 | 29 | # 30 | # Direct Digital Synthesis 31 | # 32 | # CONFIG_AD9832 is not set 33 | # CONFIG_AD9834 is not set 34 | # end of Direct Digital Synthesis 35 | 36 | # 37 | # Network Analyzer, Impedance Converters 38 | # 39 | # CONFIG_AD5933 is not set 40 | # end of Network Analyzer, Impedance Converters 41 | 42 | # 43 | # Active energy metering IC 44 | # 45 | # CONFIG_ADE7854 is not set 46 | # end of Active energy metering IC 47 | 48 | # 49 | # Resolver to digital converters 50 | # 51 | # CONFIG_AD2S1210 is not set 52 | # end of Resolver to digital converters 53 | # end of IIO staging drivers 54 | 55 | # CONFIG_STAGING_MEDIA is not set 56 | # CONFIG_STAGING_BOARD is not set 57 | # CONFIG_LTE_GDM724X is not set 58 | # CONFIG_KS7010 is not set 59 | # CONFIG_PI433 is not set 60 | # CONFIG_XIL_AXIS_FIFO is not set 61 | # CONFIG_FIELDBUS_DEV is not set 62 | # CONFIG_QLGE is not set 63 | # CONFIG_VME_BUS is not set 64 | CONFIG_XILINX_FCLK=y 65 | # CONFIG_XLNX_TSMUX is not set 66 | # CONFIG_XROE_FRAMER is not set 67 | # CONFIG_XROE_TRAFFIC_GEN is not set 68 | # CONFIG_SERIAL_UARTLITE_RS485 is not set 69 | # CONFIG_XILINX_TSN is not set 70 | -------------------------------------------------------------------------------- /em-os/project-spec/configs/zynq-generic-7z020.conf: -------------------------------------------------------------------------------- 1 | #@TYPE: Machine 2 | #@NAME: zynq-generic-7z020 3 | #@DESCRIPTION: Machine configuration for the zynq-generic-7z020 boards. 4 | 5 | #### Preamble 6 | MACHINEOVERRIDES =. "${@['', 'zynq-generic-7z020:']['zynq-generic-7z020' !='${MACHINE}']}" 7 | #### Regular settings follow 8 | 9 | # Add system XSA 10 | HDF_EXT = "xsa" 11 | HDF_BASE = "file://" 12 | HDF_PATH = "${TOPDIR}/../project-spec/hw-description/system.xsa" 13 | 14 | # Yocto device-tree variables 15 | YAML_CONSOLE_DEVICE_CONFIG:pn-device-tree ?= "ps7_uart_1" 16 | YAML_MAIN_MEMORY_CONFIG:pn-device-tree = "PS7_DDR_0" 17 | DT_PADDING_SIZE:pn-device-tree ?= "0x1000" 18 | DTC_FLAGS:pn-device-tree ?= "-@" 19 | YAML_DT_BOARD_FLAGS:zynq-generic-7z020 = "{BOARD template}" 20 | 21 | # Yocto linux-xlnx variables 22 | 23 | # Yocto u-boot-xlnx variables 24 | UBOOT_MACHINE ?= "xilinx_zynq_virt_defconfig" 25 | HAS_PLATFORM_INIT:append = " xilinx_zynq_virt_defconfig" 26 | 27 | # Yocto FSBL variables 28 | YAML_SERIAL_CONSOLE_STDIN:pn-fsbl-firmware ?= "ps7_uart_1" 29 | YAML_SERIAL_CONSOLE_STDOUT:pn-fsbl-firmware ?= "ps7_uart_1" 30 | 31 | # Yocto KERNEL Variables 32 | UBOOT_ENTRYPOINT = "0x200000" 33 | UBOOT_LOADADDRESS = "0x200000" 34 | KERNEL_EXTRA_ARGS:zynq += "UIMAGE_LOADADDR=${UBOOT_ENTRYPOINT}" 35 | 36 | #Set DDR Base address for u-boot-xlnx-scr variables 37 | DDR_BASEADDR = "0x0" 38 | SKIP_APPEND_BASEADDR = "0" 39 | 40 | # zynq-generic-7z020 Serial Console 41 | SERIAL_CONSOLES = "115200;ttyPS0" 42 | SERIAL_CONSOLES_CHECK = "${SERIAL_CONSOLES}" 43 | YAML_SERIAL_CONSOLE_BAUDRATE = "115200" 44 | 45 | # Required generic machine inclusion 46 | require conf/machine/zynq-generic.conf 47 | 48 | # Yocto MACHINE_FEATURES Variable 49 | MACHINE_FEATURES += "fpga-overlay" 50 | 51 | #### No additional settings should be after the Postamble 52 | #### Postamble 53 | PACKAGE_EXTRA_ARCHS:append = "${@['', 'zynq_generic_7z020']['zynq-generic-7z020' != '${MACHINE}']}" 54 | -------------------------------------------------------------------------------- /controller-firmware/README.md: -------------------------------------------------------------------------------- 1 | # controller-firmware (FPGA HDL) 2 | 3 | ## Dev environment setup 4 | 5 | ### Requirements 6 | - [Visual Studio Code](https://code.visualstudio.com/) 7 | - [Python 3.12](https://www.python.org/downloads/release/python-3123/) 8 | - [Vivado 2023.1](https://www.xilinx.com/support/download/index.html/content/xilinx/en/downloadNav/vivado-design-tools/2023-1.html) 9 | - [Surfer](https://surfer-project.org/) (optional, used to view HDL sim trace files) 10 | 11 | 12 | ### Setup (windows/linux) 13 | #### 1. Run the "setup project" task in VS Code (Terminal -> Run Task... -> setup project) 14 | This will: 15 | - Run subst to create a new drive leading to vivado files, this is needed to keep path lengths short (windows only) 16 | - Create the python virtual environment 17 | - Install required python packages 18 | - Create the vivado project 19 | 20 | ## Creating new FPGA modules 21 | These modules add user-configurable functionality to the controller 22 | 23 | ### Register mapping 24 | All modules must include a register map object, this will: 25 | - Define the module and provide any global settings the it's software driver may need 26 | - Define all memory addresses available to the driver 27 | - Data types, sizes, and packing if applicable for all memory 28 | - Group offsets and alignement in the case of modules that are designed to handle multiple instances of something (ex: 1-32 encoders) 29 | 30 | ### Ports 31 | All modules must follow the same port format and timing sequence for memory access 32 | 33 | 34 | #### View [registers2.py](python/src/registers2.py) and [example_module.py](python/src/example_module.py) for more details 35 | 36 | 37 | ## Commiting changes 38 | ### Vivado (only needed for vivado project changes, *not amaranth changes*) 39 | #### Run the "save vivado project" task in VS Code (Terminal -> Run Task... -> save vivado project) 40 | This will save the vivado project to a .tcl file which is in the repo and can be committed 41 | -------------------------------------------------------------------------------- /controller-software/core/readme.md: -------------------------------------------------------------------------------- 1 | install cross-platform compilers: 2 | sudo apt install gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf 3 | 4 | install build tools: 5 | sudo apt update 6 | sudo apt install build-essential ninja-build 7 | 8 | install cmake 9 | sudo apt install cmake 10 | 11 | install debugger: 12 | sudo apt install gdb-multiarch 13 | 14 | create ssh keys: 15 | ssh-keygen -t rsa -b 4096 -C "your_email@example.com" 16 | save them in the core/.ssh folder 17 | 18 | copy ssh key to zynq: 19 | ssh-copy-id -i .ssh/zynq em-os@192.168.1.243 20 | 21 | (remove past configs if zynq has been re-imaged, don't do this if you have other keys saved, just remove the zynq one) 22 | rm -r /home/excessive/.ssh 23 | 24 | create ssh config: 25 | mkdir -p ~/.ssh && chmod 700 ~/.ssh 26 | 27 | create config: 28 | touch ~/.ssh/config 29 | 30 | set permissions: 31 | chmod 600 ~/.ssh/config 32 | 33 | configure ssh settings: 34 | nano ~/.ssh/config 35 | 36 | add config: 37 | Host zynq 38 | HostName 192.168.1.243 39 | User em-os 40 | IdentityFile /home/excessive/controller-software/controller-software/core/.ssh/zynq (replace with actual path to the keys) 41 | Port 22 42 | 43 | replace "miDebuggerServerAddress" with the correct IP address in .vscode/launch.json 44 | 45 | build project to copy files to zynq 46 | 47 | TODO: figure out a way to better handle the permissions, currently you must enter the password each time (maybe not that bad?) 48 | 49 | allow write access so we can update it later from vs code (note this is unsafe since a malicious bin could be added) 50 | sudo chmod u+w controller/bin/controller_core 51 | 52 | 53 | update zynq node modules 54 | in controller/web, run npm install 55 | 56 | update zynq time (required for certs to be valid) 57 | sudo date -s "2025-3-21 18:37:00" 58 | 59 | 60 | petalinux setup: 61 | package images: 62 | petalinux-package --boot --fsbl --u-boot --force 63 | 64 | create disk image: 65 | petalinux-package --wic --outdir /home/excessive --wic-extra-args "-c xz" -b "BOOT.BIN,image.ub,boot.scr" --wks project-spec/configs/rootfs.wks 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/current_control.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | # Simulation parameters 5 | R = .1 # Resistance in ohms 6 | L = .001 # Inductance in henries 7 | Vdc = 350.0 # DC supply voltage 8 | f_pwm = 25e3 # PWM frequency in Hz 9 | 10 | current_setpioint = 50.0 # Setpoint for current controller 11 | bemf = 40.0 # Back EMF voltage 12 | 13 | t_stop = 2e-3 # Simulate for 2 ms 14 | dt = .1e-6 # Time step of .1 microsecond 15 | 16 | 17 | 18 | MAX_DUTY_CYCLE = 0.95 19 | MIN_DUTY_CYCLE = 0.00 20 | 21 | 22 | 23 | # Time array 24 | time = np.arange(0, t_stop, dt) 25 | 26 | # Storage for current 27 | current = np.zeros_like(time) 28 | 29 | I = 0.0 # initial current 30 | # Derived parameters 31 | T_pwm = 1.0 / f_pwm 32 | 33 | old_t_mod = 0 34 | 35 | duty_cycle = 0 36 | for i in range(1, len(time)): 37 | 38 | t = time[i] 39 | # Determine if we are in the "on" or "off" portion of the PWM cycle 40 | # Use modulo operation to find where we are within a PWM period 41 | t_mod = t % T_pwm 42 | 43 | if t_mod < old_t_mod: 44 | 45 | applied_voltage = Vdc - bemf 46 | 47 | a = (applied_voltage/R) - current_setpioint 48 | b = (applied_voltage/R) - I 49 | 50 | total_time = -(L/R) * np.log(a/b) 51 | 52 | duty_cycle = min(MAX_DUTY_CYCLE, max(MIN_DUTY_CYCLE, total_time/T_pwm)) 53 | 54 | pass 55 | old_t_mod = t_mod 56 | 57 | on_time = duty_cycle * T_pwm 58 | off_time = T_pwm - on_time 59 | 60 | 61 | if t_mod < on_time: 62 | V = Vdc - bemf 63 | else: 64 | V = -bemf 65 | 66 | # Compute dI/dt 67 | dIdt = (V - R*I) / L 68 | dIdt *= 1 69 | # Integrate using Euler method 70 | I = I + dIdt * dt 71 | 72 | # Store current 73 | current[i] = I 74 | 75 | # Plotting 76 | plt.figure(figsize=(10, 5)) 77 | plt.plot(time*1e4, current, label='Current through inductor') 78 | plt.title('PWM driven RL load') 79 | plt.xlabel('Time (ms)') 80 | plt.ylabel('Current (A)') 81 | plt.grid(True) 82 | plt.legend() 83 | plt.show() 84 | -------------------------------------------------------------------------------- /controller-software/core/controller/inc/controller.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "controller_api.h" 12 | #include 13 | #include "fpga_interface.h" 14 | #include "fpga_module_manager.h" 15 | 16 | #pragma once 17 | 18 | 19 | class Controller { 20 | private: 21 | uint64_t cycle_count = 0; // count of FPGA cycles since startup 22 | 23 | bool quit = false; // flag to exit the controller 24 | bool quit_on_fpga_update_fail = false; // flag to exit the controller if the FPGA update fails 25 | 26 | uint32_t software_update_period_us = 1000; // 1khz default 27 | uint32_t software_update_frequency = 1000; // 1khz default 28 | 29 | // FPGA drivers 30 | Fpga_Interface fpga; 31 | fpga_module_manager fpga_manager; 32 | 33 | // API driver 34 | controller_api api; 35 | std::thread noncritical_thread; 36 | 37 | // Node core 38 | Node_Core node_core; 39 | 40 | uint32_t critical_calls(); // hard-realtime calls that must be completed each cycle 41 | 42 | void setup_main_thread(); 43 | 44 | uint64_t microseconds = 0; 45 | void update_microseconds(); 46 | 47 | void run(); 48 | 49 | uint32_t load_fpga_config(); 50 | std::string fpga_config_path = ""; 51 | 52 | std::string fpga_driver_config_file = ""; 53 | 54 | uint32_t save_default_configs(); 55 | std::string temp_path = ""; 56 | 57 | uint32_t load_user_configs(); 58 | std::string user_node_config_path = ""; 59 | std::string user_fpga_intructions_config_path = ""; 60 | 61 | uint32_t start_nodejs(); 62 | void quit_nodejs(); 63 | 64 | 65 | public: 66 | Controller(); 67 | 68 | uint32_t load_config(std::string file_path); // load the main config file 69 | 70 | void start(); 71 | 72 | ~Controller(); 73 | }; -------------------------------------------------------------------------------- /controller-firmware/python/src/gpio_node.py: -------------------------------------------------------------------------------- 1 | from amaranth import * 2 | from amaranth.sim import Simulator 3 | from amaranth.back import verilog 4 | from amaranth.lib import wiring 5 | from amaranth.lib.wiring import In, Out 6 | from shift_dma import shift_dma_node 7 | from registers import * 8 | 9 | 10 | class gpio_node(wiring.Component): 11 | 12 | def __init__(self) -> None: 13 | super().__init__({ 14 | "gpio_out": Out(32), 15 | "gpio_out_enable": Out(32), 16 | "gpio_in": In(32), 17 | }) 18 | 19 | self.desc = RTL_Block("gpio_node") 20 | self.desc.addCompatibleDriver("gpio_node") 21 | self.address = 255 22 | 23 | self.desc.addRegister(Register("gpio_out", RegisterDataType.UNSIGNED, ReadWritePermissions.WRITE, "GPIO out register", width=32)) 24 | self.desc.addRegister(Register("gpio_out_enable", RegisterDataType.UNSIGNED, ReadWritePermissions.WRITE, "GPIO out enable register", width=32)) 25 | self.desc.addRegister(Register("gpio_in", RegisterDataType.UNSIGNED, ReadWritePermissions.READ, "GPIO in register", width=32)) 26 | 27 | self.desc.generateAddressMap() 28 | 29 | def elaborate(self, platform): 30 | m = Module() 31 | 32 | m.submodules.shift_dma = self.shift_dma = shift_dma_node(self.address) 33 | 34 | with m.If(self.shift_dma.bram_address == self.desc.getRegistor("gpio_out").address): 35 | m.d.sync += self.shift_dma.bram_read_data.eq(self.gpio_out) 36 | with m.If(self.shift_dma.bram_write_enable == 1): 37 | m.d.sync += self.gpio_out.eq(self.shift_dma.bram_write_data) 38 | 39 | with m.If(self.shift_dma.bram_address == self.desc.getRegistor("gpio_out_enable").address): 40 | m.d.sync += self.shift_dma.bram_read_data.eq(self.gpio_out_enable) 41 | with m.If(self.shift_dma.bram_write_enable == 1): 42 | m.d.sync += self.gpio_out_enable.eq(self.shift_dma.bram_write_data) 43 | 44 | with m.If(self.shift_dma.bram_address == self.desc.getRegistor("gpio_in").address): 45 | m.d.sync += self.shift_dma.bram_read_data.eq(self.gpio_in) 46 | 47 | return m 48 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/inc/shared_mem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | class shared_mem{ 13 | private: 14 | const unsigned int data_buffer_size = 256 * 4; // size of shared memory buffer for data (64 32-bit values) 15 | const unsigned int control_buffer_size = 32 * 4 * 4; // size of shared memory buffer for control info (able to hold 32 API calls) 16 | 17 | void* web_to_controller_data_mem = nullptr; 18 | void* web_to_controller_control_mem = nullptr; 19 | void* controller_to_web_data_mem = nullptr; 20 | void* controller_to_web_control_mem = nullptr; 21 | 22 | void* persistent_web_mem = nullptr; 23 | 24 | int web_to_controller_data_map; 25 | int web_to_controller_control_map; 26 | int controller_to_web_data_map; 27 | int controller_to_web_control_map; 28 | 29 | int persistent_web_map; 30 | 31 | unsigned int create_shared_mem(unsigned int size, void *&mem, int &shm_fd, std::string shm_name, int prot_flags = PROT_READ | PROT_WRITE); 32 | unsigned int open_shared_mem(unsigned int size, void *&mem, int &shm_fd, std::string shm_name, int prot_flags = PROT_READ | PROT_WRITE); 33 | 34 | public: 35 | 36 | void* get_web_to_controller_data_mem() const { return web_to_controller_data_mem; } 37 | void* get_web_to_controller_control_mem() const { return web_to_controller_control_mem; } 38 | void* get_controller_to_web_data_mem() const { return controller_to_web_data_mem; } 39 | void* get_controller_to_web_control_mem() const { return controller_to_web_control_mem; } 40 | 41 | void* get_persistent_web_mem() const { return persistent_web_mem; } 42 | 43 | ~shared_mem(); 44 | 45 | unsigned int controller_create_shared_mem(); 46 | unsigned int web_create_shared_mem(); 47 | 48 | unsigned int get_data_buffer_size() { return data_buffer_size; } 49 | unsigned int get_control_buffer_size() { return control_buffer_size; } 50 | 51 | void unmap_shared_mem(); 52 | void close_shared_mem(); 53 | }; 54 | 55 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/set_global_variable.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | /* 4 | 5 | Make a global variable accessible by nodes 6 | 7 | */ 8 | 9 | #pragma once 10 | 11 | class set_global_variable: public base_node { 12 | private: 13 | bool default_enable = true; 14 | struct input_pair{ 15 | input* input_ptr = nullptr; 16 | input* enable_ptr = nullptr; // optional enable input, if not set, the node will always run 17 | global_variable* variable = nullptr; 18 | }; 19 | 20 | std::vector input_pairs; 21 | 22 | public: 23 | 24 | set_global_variable(){ 25 | //execution_number = -1; 26 | type = "set_global_variable"; 27 | } 28 | 29 | unsigned int run() override { 30 | 31 | // copy input data to global variable 32 | for(auto& pair : input_pairs){ 33 | if(pair.variable != nullptr && pair.input_ptr->data_pointer != nullptr){ 34 | if(!*(bool*)pair.enable_ptr->data_pointer){ 35 | continue; // skip if enable input is false 36 | } 37 | pair.variable->set_value(pair.input_ptr->data_pointer); 38 | } 39 | } 40 | return 0; 41 | } 42 | 43 | uint32_t set_global_var_input(std::string input_name, global_variable* variable) override { 44 | // add if input doesn't already exist 45 | if(inputs.find(input_name) != inputs.end()){ 46 | std::cerr << "Error: input name already exists" << std::endl; 47 | return 2; // input name already exists 48 | } 49 | 50 | inputs.emplace(input_name, input(variable->get_type(), nullptr)); 51 | inputs.emplace(input_name + "_enable", input(io_type::BOOL, &default_enable)); // optional enable input 52 | 53 | input_pair new_pair; 54 | new_pair.input_ptr = &inputs[input_name]; 55 | new_pair.enable_ptr = &inputs[input_name + "_enable"]; 56 | new_pair.variable = variable; 57 | input_pairs.push_back(new_pair); 58 | 59 | return 0; 60 | } 61 | 62 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/bitwise_split.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class bitwise_split: public base_node { 7 | private: 8 | bool output_bits[32] = {false}; 9 | uint8_t bit_count = 0; 10 | input* in = nullptr; 11 | 12 | io_type type = io_type::UNDEFINED; 13 | 14 | bool configured = false; 15 | 16 | public: 17 | 18 | bitwise_split(){ 19 | } 20 | 21 | unsigned int run() override { 22 | 23 | for(uint8_t i = 0; i < bit_count; i++){ 24 | output_bits[i] = (*(uint32_t*)(in->data_pointer) >> i) & 1; 25 | } 26 | 27 | return 0; 28 | } 29 | 30 | unsigned int configure_settings(json* json) override{ 31 | 32 | /* 33 | parse settings from json 34 | { 35 | "config":{ 36 | "type": "uint8" // only unsigned types supported 37 | } 38 | } 39 | */ 40 | 41 | if(configured){ 42 | std::cerr << "Error: bitwise_split node already configured" << std::endl; 43 | return 1; 44 | } 45 | 46 | type = get_io_type(json->at("type").get()); 47 | 48 | switch(type){ 49 | case io_type::UINT8: 50 | bit_count = 8; 51 | break; 52 | case io_type::UINT16: 53 | bit_count = 16; 54 | break; 55 | case io_type::UINT32: 56 | bit_count = 32; 57 | break; 58 | default: 59 | std::cerr << "Error: invalid type for bitwise_split node" << std::endl; 60 | return 1; 61 | } 62 | 63 | inputs.emplace("input", input(type, nullptr)); 64 | in = &inputs["input"]; 65 | 66 | for(uint8_t i = 0; i < bit_count; i++){ 67 | outputs.emplace("output_bit_" + std::to_string(i), output(io_type::BOOL, &output_bits[i], &execution_number)); 68 | } 69 | 70 | configured = true; 71 | 72 | return 0; 73 | } 74 | 75 | }; -------------------------------------------------------------------------------- /controller-software/web/web-interface/server/wsServer.js: -------------------------------------------------------------------------------- 1 | // server/wsServer.js 2 | const { WebSocketServer } = require('ws'); 3 | 4 | function initWebSocketServer(server) { 5 | // Attach a WebSocketServer to an existing HTTP/S server 6 | const wss = new WebSocketServer({ server }); 7 | 8 | wss.on('connection', (ws) => { 9 | console.log('[WS] Client connected.'); 10 | 11 | ws.on('message', (data) => { 12 | try { 13 | const message = JSON.parse(data.toString()); 14 | 15 | // Expecting JSON-RPC 2.0 format: { "jsonrpc": "2.0", "method": "...", "params": {...}, "id": 123 } 16 | if (message.jsonrpc !== '2.0' || !message.method) { 17 | return ws.send(JSON.stringify({ 18 | jsonrpc: '2.0', 19 | id: message.id || null, 20 | error: { code: -32600, message: 'Invalid Request' } 21 | })); 22 | } 23 | 24 | console.log('[WS] Received message:', message); 25 | 26 | // Example: If method is "updateGamepadValue" or something 27 | if (message.method === 'updateGamepadValue') { 28 | // message.params might look like { "value": 0.5 } 29 | const { value } = message.params; 30 | 31 | // Log or process the numeric value 32 | console.log(`Gamepad value: ${value}`); 33 | 34 | // Respond with a JSON-RPC success 35 | ws.send(JSON.stringify({ 36 | jsonrpc: '2.0', 37 | id: message.id, 38 | result: { status: 'OK', receivedValue: value } 39 | })); 40 | } else { 41 | // Method not recognized 42 | ws.send(JSON.stringify({ 43 | jsonrpc: '2.0', 44 | id: message.id, 45 | error: { code: -32601, message: 'Method not found' } 46 | })); 47 | } 48 | } catch (err) { 49 | console.error('Error parsing WS message:', err); 50 | ws.send(JSON.stringify({ 51 | jsonrpc: '2.0', 52 | id: null, 53 | error: { code: -32700, message: 'Parse error' } 54 | })); 55 | } 56 | }); 57 | 58 | ws.on('close', () => { 59 | console.log('[WS] Client disconnected.'); 60 | }); 61 | }); 62 | 63 | console.log('[WS] WebSocket server initialized.'); 64 | } 65 | 66 | module.exports = { initWebSocketServer }; 67 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/bitwise_join.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class bitwise_join: public base_node { 7 | private: 8 | uint32_t out = 0; 9 | input* in_bits[32] = {nullptr}; // pointers to input bits 10 | uint8_t bit_count = 0; 11 | 12 | io_type type = io_type::UNDEFINED; 13 | 14 | bool configured = false; 15 | 16 | public: 17 | 18 | bitwise_join(){ 19 | } 20 | 21 | unsigned int run() override { 22 | 23 | out = 0; 24 | 25 | for(uint8_t i = 0; i < bit_count; i++){ 26 | out |= (*(bool*)(in_bits[i]->data_pointer) << i); 27 | } 28 | 29 | return 0; 30 | } 31 | 32 | unsigned int configure_settings(json* json) override{ 33 | 34 | /* 35 | parse settings from json 36 | { 37 | "config":{ 38 | "type": "uint8" // only unsigned types supported 39 | } 40 | } 41 | */ 42 | 43 | if(configured){ 44 | std::cerr << "Error: bitwise_join node already configured" << std::endl; 45 | return 1; 46 | } 47 | 48 | type = get_io_type(json->at("type").get()); 49 | 50 | switch(type){ 51 | case io_type::UINT8: 52 | bit_count = 8; 53 | break; 54 | case io_type::UINT16: 55 | bit_count = 16; 56 | break; 57 | case io_type::UINT32: 58 | bit_count = 32; 59 | break; 60 | default: 61 | std::cerr << "Error: invalid type for bitwise_join node" << std::endl; 62 | return 1; 63 | } 64 | 65 | outputs.emplace("output", output(type, &out, &execution_number)); 66 | 67 | for(uint8_t i = 0; i < bit_count; i++){ 68 | inputs.emplace("input_bit_" + std::to_string(i), input(io_type::BOOL, nullptr)); 69 | in_bits[i] = &inputs["input_bit_" + std::to_string(i)]; 70 | } 71 | 72 | configured = true; 73 | 74 | return 0; 75 | } 76 | 77 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/src/fanuc_encoders.cpp: -------------------------------------------------------------------------------- 1 | #include "fanuc_encoders.h" 2 | 3 | static Driver_Registrar registrar("fanuc_encoders"); 4 | 5 | uint32_t fanuc_encoders::custom_load_config(json* user_driver_config){ 6 | 7 | cpy_time_reference(0); 8 | cpy_write_priority(); 9 | cpy_execution_window(0, 1000); 10 | 11 | trigger = loader.get_register("trigger", 0); 12 | cpy_destination(trigger); 13 | cpy_source("trigger"); 14 | cpy_add_instruction(); 15 | 16 | // auto encoder = loader.get_group("encoder", 0); 17 | // multiturn_count = encoder->get_register("multiturn_count", 0); 18 | // singleturn_count = encoder->get_register("singleturn_count", 0); 19 | // #loader.sync_with_PS(singleturn_count); 20 | // commutation_count = encoder->get_register("commutation_count", 0); 21 | // loader.sync_with_PS(commutation_count); 22 | // auto status = encoder->get_register("status", 0); 23 | // loader.sync_with_PS(status); 24 | // battery_fail = status->get_register("battery_fail"); 25 | // unindexed = status->get_register("unindexed"); 26 | // no_response = status->get_register("no_response"); 27 | // crc_error = status->get_register("crc_fail"); 28 | // done = status->get_register("done"); 29 | 30 | 31 | // encoder_pos = singleturn_count->get_raw_data_ptr(); 32 | // encoder_multiturn_count = multiturn_count->get_raw_data_ptr(); 33 | 34 | return 0; 35 | } 36 | 37 | uint32_t fanuc_encoders::run(){ 38 | 39 | // if(*microseconds > old_microseconds + 1000000){ 40 | // // std::cout << "Encoder multiturn count: " << multiturn_count->get_value() << std::endl; 41 | // std::cout << "Encoder single turn count: " << (singleturn_count->get_value()) << std::endl; 42 | // // std::cout << "Encoder commutation count: " << commutation_count->get_value() << std::endl; 43 | // //std::cout << "Encoder CRC error: " << crc_error->get_value() << std::endl; 44 | // //std::cout << "Encoder no response: " << no_response->get_value() << std::endl; 45 | // //std::cout << "Encoder unindexed: " << unindexed->get_value() << std::endl; 46 | // //std::cout << "Encoder battery fail: " << battery_fail->get_value() << std::endl; 47 | // //std::cout << "Encoder done: " << done->get_value() << std::endl << std::endl; 48 | // old_microseconds = *microseconds; 49 | // } 50 | 51 | return 0; 52 | } -------------------------------------------------------------------------------- /controller-software/config/electrical/motors/motor_linear.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, field 2 | 3 | class LINEAR_MOTOR: 4 | def __init__(self): 5 | self.types = [self.PMAC] 6 | self.motor_type = None 7 | self.user_params = None 8 | 9 | 10 | @dataclass 11 | class COMMON: 12 | # Common motor parameters 13 | rated_speed: float = field(default=None, metadata={'unit': 'm/s', 'desc': 'Rated speed of the motor', 'required':True}) 14 | max_speed: float = field(default=None, metadata={'unit': 'm/s', 'desc': 'Maximum speed of the motor', 'required':False}) 15 | rated_voltage: float = field(default=None, metadata={'unit': 'V', 'desc': 'Rated voltage of the motor', 'required':True}) 16 | rated_current: float = field(default=None, metadata={'unit': 'A', 'desc': 'Rated current of the motor', 'required':True}) 17 | soft_max_current: float = field(default=None, metadata={'unit': 'A', 'desc': 'Soft maximum current of the motor', 'required':False}) 18 | hard_max_current: float = field(default=None, metadata={'unit': 'A', 'desc': 'Hard maximum current of the motor, beyond which the motor will be damaged', 'required':True}) 19 | force_constant: float = field(default=None, metadata={'unit': 'N/A', 'desc': 'Force constant of the motor', 'required':False}) 20 | bemf_constant: float = field(default=None, metadata={'unit': 'V/(m/s)', 'desc': 'Back EMF constant of the motor', 'required':False}) 21 | phase_resistance: float = field(default=None, metadata={'unit': 'Ohm', 'desc': 'Phase resistance of the motor', 'required':False}) 22 | phase_inductance: float = field(default=None, metadata={'unit': 'H', 'desc': 'Phase inductance of the motor', 'required':False}) 23 | mass: float = field(default=None, metadata={'unit': 'kg', 'desc': 'Mass of the moving part of the motor', 'required':False}) 24 | thermal_time_constant: float = field(default=None, metadata={'unit': 's', 'desc': 'Thermal time constant of the motor', 'required':False}) 25 | thermal_resistance: float = field(default=None, metadata={'unit': 'K/W', 'desc': 'Thermal resistance of the motor to ambient', 'required':False}) 26 | max_temperature: float = field(default=None, metadata={'unit': 'C', 'desc': 'Maximum temperature of the motor', 'required':False}) 27 | 28 | @dataclass 29 | class PMAC: 30 | # PMAC specific parameters 31 | pole_distance: int = field(default=None, metadata={'desc': 'Distance between poles on the motor', 'required':False}) 32 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/src/api_objects.cpp: -------------------------------------------------------------------------------- 1 | #include "api_objects.h" 2 | 3 | 4 | uint32_t get_new_call_object_from_call_id(std::vector>* calls, const api_call_id_t* api_call_id) { 5 | if(*api_call_id == api_call_id_t::DEFAULT){ 6 | calls->push_back(std::make_shared()); 7 | } 8 | else if(*api_call_id == api_call_id_t::MACHINE_STATE){ 9 | calls->push_back(std::make_shared()); 10 | } 11 | else if(*api_call_id == api_call_id_t::PRINT_UINT32){ 12 | calls->push_back(std::make_shared()); 13 | } 14 | else if(*api_call_id == api_call_id_t::PRINT_FLOAT){ 15 | calls->push_back(std::make_shared()); 16 | } 17 | else if(*api_call_id == api_call_id_t::CONFIGURE_NODE){ 18 | calls->push_back(std::make_shared()); 19 | } 20 | else { 21 | return 1; // error: unknown call ID 22 | } 23 | 24 | return 0; 25 | } 26 | 27 | uint32_t create_new_call_obj_from_string(std::vector>* calls, std::string api_call_name, uint32_t* api_call_number) { 28 | if(api_call_name.compare("machine_state") == 0){ 29 | calls->push_back(std::make_shared()); 30 | } 31 | else if(api_call_name.compare("print_uint32") == 0){ 32 | calls->push_back(std::make_shared()); 33 | } 34 | else if(api_call_name.compare("print_float") == 0){ 35 | calls->push_back(std::make_shared()); 36 | } 37 | else if(api_call_name.compare("configure_node") == 0){ 38 | calls->push_back(std::make_shared()); 39 | } 40 | else { 41 | return 1; 42 | } 43 | 44 | calls->back()->api_call_number = Base_API::web_to_controller_call_count + 1; 45 | *api_call_number = calls->back()->api_call_number; 46 | Base_API::web_to_controller_call_count++; 47 | calls->back()->persistent_web_mem[0] = Base_API::web_to_controller_call_count; 48 | 49 | return 0; 50 | } 51 | 52 | uint32_t remove_last_call_obj_from_string(std::vector>* calls, uint32_t* api_call_number) { // remove the last call object from the vector, used if the call object is not valid 53 | 54 | // decrement call numbers 55 | api_call_number--; 56 | Base_API::web_to_controller_call_count--; 57 | calls->back()->persistent_web_mem[0] = Base_API::web_to_controller_call_count; 58 | 59 | calls->pop_back(); // remove failed call object 60 | return 0; 61 | } -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/edge_detect.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class edge_detect: public base_node { 7 | private: 8 | bool out = false; // output value 9 | bool falling_edge = false; // true if the edge is falling, false if rising 10 | input* in = nullptr; // pointer to the input 11 | 12 | bool last_in = false; 13 | 14 | bool configured = false; 15 | 16 | public: 17 | 18 | edge_detect(){ 19 | } 20 | 21 | unsigned int run() override { 22 | 23 | bool in_value = *(bool*)(in->data_pointer); 24 | 25 | if(in_value != last_in){ 26 | // edge detected 27 | if(falling_edge){ 28 | out = !in_value; // output true if falling edge 29 | } 30 | else{ 31 | out = in_value; // output true if rising edge 32 | } 33 | } 34 | else{ 35 | out = false; // no edge detected 36 | } 37 | last_in = in_value; 38 | 39 | return 0; 40 | } 41 | 42 | unsigned int configure_settings(json* json) override{ 43 | 44 | /* 45 | parse settings from json 46 | { 47 | "config":{ 48 | "rising_edge": "true", 49 | "falling_edge": "false", 50 | } 51 | } 52 | */ 53 | 54 | if(configured){ 55 | std::cerr << "Error: edge_delay node already configured" << std::endl; 56 | return 1; 57 | } 58 | 59 | if(json->find("rising_edge") != json->end()){ 60 | falling_edge = !json->at("rising_edge").get(); 61 | } 62 | else if(json->find("falling_edge") != json->end()){ 63 | falling_edge = json->at("falling_edge").get(); 64 | } 65 | else{ 66 | std::cerr << "Error: edge_delay node configuration missing 'rising_edge' or 'falling_edge'" << std::endl; 67 | return 1; 68 | } 69 | 70 | 71 | outputs.emplace("output", output(io_type::BOOL, &out, &execution_number)); 72 | 73 | inputs.emplace("input", input(io_type::BOOL, nullptr)); 74 | in = &inputs["input"]; 75 | 76 | configured = true; 77 | 78 | return 0; 79 | } 80 | 81 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | //#include "api_drivers/controller_api.h" 2 | //#include "nodes/node_core.h" 3 | #include 4 | #include "controller.h" 5 | 6 | #include "kins_6_axis.h" 7 | 8 | int main() { 9 | 10 | // InverseKinematics ik; 11 | // ik.benchmark(); 12 | // return 0; 13 | 14 | Controller controller; 15 | 16 | controller.load_config("controller/config/user/config.json"); 17 | 18 | controller.start(); 19 | 20 | //controller.run(); 21 | 22 | // startup internal API 23 | // controller_api api; 24 | 25 | // node_network* net = new node_network(); 26 | 27 | // net->set_type(node_network::update_type::ASYNC); 28 | // net->set_timeout_usec(100); 29 | // //net->set_timeout_sec(2); 30 | 31 | // unsigned int cycle = 0; 32 | 33 | // net->add_node(new bool_constant(), "constant"); 34 | // net->add_node(new logic_not(), "not_0"); 35 | // net->add_node(new logic_not(), "not_1"); 36 | // net->add_node(new logic_not(), "not_2"); 37 | // net->add_node(new logic_not(), "not_3"); 38 | // net->add_node(new logic_and(), "and_0"); 39 | // net->add_node(new bool_print_cout(), "bool_print"); 40 | // net->add_node(new nothing_delay(), "delay_0"); 41 | 42 | // output* out = nullptr; 43 | 44 | // net->connect_nodes("constant", "output", "delay_0", "input"); 45 | // net->connect_nodes("delay_0", "output", "not_0", "input"); 46 | // net->connect_nodes("not_0", "output", "not_1", "input"); 47 | // net->connect_nodes("not_1", "output", "not_2", "input"); 48 | // net->connect_nodes("not_2", "output", "bool_print", "input"); 49 | 50 | // //net->nodes["not_2"]->get_output_object("output", out); 51 | 52 | // //net->nodes[1]->connect_input("input", net->nodes[4], "output"); // connect first not to fourth not (creates circular dependency) 53 | 54 | // //std::cout << "output: " << *reinterpret_cast(out->data_pointer) << std::endl; 55 | 56 | // net->rebuild_execution_order(); 57 | // net->run(&cycle); 58 | // cycle++; 59 | 60 | // std::this_thread::sleep_for(std::chrono::seconds(2)); 61 | 62 | // net->run(&cycle); 63 | // cycle++; 64 | 65 | // std::this_thread::sleep_for(std::chrono::seconds(2)); 66 | 67 | // // net->connect_nodes("not_2", "output", "and_0", "input_A"); 68 | // // net->connect_nodes("not_0", "output", "and_0", "input_B"); 69 | // // net->connect_nodes("and_0", "output", "bool_print", "input"); 70 | 71 | // // net->rebuild_execution_order(); 72 | // net->run(&cycle); 73 | 74 | return 0; 75 | } -------------------------------------------------------------------------------- /controller-software/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | 4 | # Setup compiler settings 5 | set(CMAKE_C_STANDARD 11) 6 | set(CMAKE_C_STANDARD_REQUIRED ON) 7 | set(CMAKE_C_EXTENSIONS ON) 8 | set(CMAKE_CXX_STANDARD 20) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | set(CMAKE_CXX_EXTENSIONS ON) 11 | set(PROJ_PATH ${CMAKE_CURRENT_SOURCE_DIR}) 12 | message("Build type: " ${CMAKE_BUILD_TYPE}) 13 | 14 | 15 | project(controller_core) 16 | enable_language(C CXX ASM) 17 | 18 | set(EXECUTABLE controller_core) 19 | 20 | # 21 | # List of source files to compile 22 | # 23 | file(GLOB_RECURSE sources_SRCS 24 | ${PROJ_PATH}/controller/src/*.cpp 25 | ${PROJ_PATH}/controller/src/*.c 26 | ${PROJ_PATH}/controller/fpga_module_drivers/src/*.cpp 27 | ${PROJ_PATH}/controller/fpga_module_drivers/src/*.c 28 | ${PROJ_PATH}/controller/nodes/src/*.cpp 29 | ${PROJ_PATH}/controller/nodes/src/*.c 30 | ${PROJ_PATH}/controller/nodes/generic_nodes/src/*.cpp 31 | ${PROJ_PATH}/controller/nodes/generic_nodes/src/*.c 32 | ${PROJ_PATH}/controller/nodes/global_variables/src/*.cpp 33 | ${PROJ_PATH}/controller/nodes/global_variables/src/*.c 34 | ${PROJ_PATH}/controller/nodes/solvers/src/*.cpp 35 | ${PROJ_PATH}/controller/nodes/solvers/src/*.c 36 | ${PROJ_PATH}/controller/api_drivers/src/*.cpp 37 | ${PROJ_PATH}/controller/api_drivers/src/*.c 38 | ${PROJ_PATH}/controller/api_drivers/calls/src/*.cpp 39 | ${PROJ_PATH}/controller/api_drivers/calls/src/*.c 40 | 41 | ) 42 | 43 | # Executable files 44 | add_executable(${EXECUTABLE} ${sources_SRCS}) 45 | 46 | # 47 | # Include directories 48 | # 49 | set(include_path_DIRS 50 | ${PROJ_PATH}/controller/inc 51 | ${PROJ_PATH}/controller/fpga_module_drivers/inc 52 | ${PROJ_PATH}/controller/nodes/inc 53 | ${PROJ_PATH}/controller/nodes/generic_nodes/inc 54 | ${PROJ_PATH}/controller/nodes/global_variables/inc 55 | ${PROJ_PATH}/controller/nodes/solvers/inc 56 | ${PROJ_PATH}/controller/api_drivers/inc 57 | ${PROJ_PATH}/controller/api_drivers/calls/inc 58 | 59 | ) 60 | 61 | # Include paths 62 | target_include_directories(${EXECUTABLE} PRIVATE ${include_path_DIRS}) 63 | target_link_libraries(${EXECUTABLE} PRIVATE librt.so) 64 | target_link_libraries(${EXECUTABLE} PRIVATE libpthread.so) 65 | 66 | # copy all required files to the zynq directory 67 | add_custom_command(TARGET controller_core POST_BUILD 68 | COMMAND ${CMAKE_COMMAND} -E copy_if_different 69 | $ 70 | ${CMAKE_CURRENT_SOURCE_DIR}/zynq_files/controller/bin/controller_core 71 | ) 72 | 73 | -------------------------------------------------------------------------------- /controller-software/core/controller/fpga_module_drivers/inc/serial_interface_card.h: -------------------------------------------------------------------------------- 1 | #include "fpga_module_driver_factory.h" 2 | 3 | 4 | #pragma once 5 | 6 | class serial_interface_card : public base_driver { 7 | public: 8 | uint32_t custom_load_config(json* user_driver_config) override; 9 | 10 | uint32_t run() override; 11 | 12 | private: 13 | 14 | // TODO: probably should make a way to free this memory apon destruction (if we allow reconfig without restarting) 15 | struct registers2{ 16 | Register* rs485_mode_enable; 17 | Register* rs422_mode_enable; 18 | Register* quadrature_mode_enable; 19 | Register* i2c_config_read_mode; 20 | Register* i2c_config_device_address; 21 | Register* i2c_config_reg_address; 22 | Register* i2c_config_data_length; 23 | Register* i2c_config_start; 24 | Register* i2c_status_busy; 25 | Register* i2c_status_error; 26 | Register* i2c_data_rx; 27 | Register* i2c_data_tx; 28 | 29 | } regs2; 30 | 31 | struct i2c_transfer{ 32 | uint8_t device_address = 0; 33 | uint8_t reg_address = 0; 34 | const uint16_t* out_data = nullptr; 35 | uint16_t* in_data = nullptr; 36 | uint8_t data_length = 0; 37 | bool read = false; 38 | }; 39 | 40 | struct io_expander_data{ 41 | uint16_t configuration = 0; 42 | uint16_t inputs = 0; 43 | uint16_t outputs = 0; 44 | }; 45 | 46 | io_expander_data io_expander_data[3]; // data for the 3 io expanders 47 | 48 | enum port_mode : uint8_t { 49 | RS485 = 0, 50 | RS422 = 1, 51 | //QUADRATURE = 2 // not implemented yet 52 | NONE = 255 53 | }; 54 | 55 | port_mode port_modes[10]; // current port modes for the 10 ports 56 | 57 | uint32_t hdl_config = 0; 58 | 59 | // TODO: make a separate class for i2c 60 | 61 | std::vector i2c_transfers; // list of i2c transfers to be done 62 | uint8_t last_i2c_transfer = 0; // index of the last i2c transfer sent 63 | uint32_t i2c_consecutive_error_count = 0; 64 | 65 | void run_i2c_transfers(); 66 | 67 | uint32_t configure_port_mode(uint8_t port, port_mode mode); 68 | 69 | bool* manual_led_0_mode = nullptr; 70 | bool* manual_led_1_mode = nullptr; 71 | bool* manual_led_2_mode = nullptr; 72 | 73 | enum led_mode : uint8_t { 74 | OFF = 0, 75 | ON = 1, 76 | BLINK_SLOW = 2, 77 | BLINK_MED = 3, 78 | BLINK_FAST = 4 79 | }; 80 | 81 | led_mode led_modes[3]; // current led modes for the 3 leds 82 | 83 | bool power_out_5v5_1_ok = false; 84 | bool power_out_5v5_2_ok = false; 85 | bool power_out_24v_ok = false; 86 | bool power_out_enable = false; 87 | 88 | 89 | }; -------------------------------------------------------------------------------- /controller-software/core/.vscode/controller-software-core.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "../" 5 | } 6 | ], 7 | "settings": { 8 | "files.associations": { 9 | "algorithm": "cpp", 10 | "array": "cpp", 11 | "atomic": "cpp", 12 | "bit": "cpp", 13 | "bitset": "cpp", 14 | "cctype": "cpp", 15 | "charconv": "cpp", 16 | "chrono": "cpp", 17 | "cinttypes": "cpp", 18 | "clocale": "cpp", 19 | "cmath": "cpp", 20 | "compare": "cpp", 21 | "complex": "cpp", 22 | "concepts": "cpp", 23 | "condition_variable": "cpp", 24 | "cstddef": "cpp", 25 | "cstdint": "cpp", 26 | "cstdio": "cpp", 27 | "cstdlib": "cpp", 28 | "cstring": "cpp", 29 | "ctime": "cpp", 30 | "cwchar": "cpp", 31 | "cwctype": "cpp", 32 | "exception": "cpp", 33 | "format": "cpp", 34 | "forward_list": "cpp", 35 | "fstream": "cpp", 36 | "functional": "cpp", 37 | "future": "cpp", 38 | "initializer_list": "cpp", 39 | "iomanip": "cpp", 40 | "ios": "cpp", 41 | "iosfwd": "cpp", 42 | "iostream": "cpp", 43 | "istream": "cpp", 44 | "iterator": "cpp", 45 | "limits": "cpp", 46 | "list": "cpp", 47 | "locale": "cpp", 48 | "map": "cpp", 49 | "memory": "cpp", 50 | "mutex": "cpp", 51 | "new": "cpp", 52 | "numeric": "cpp", 53 | "optional": "cpp", 54 | "ostream": "cpp", 55 | "random": "cpp", 56 | "ratio": "cpp", 57 | "set": "cpp", 58 | "sstream": "cpp", 59 | "stdexcept": "cpp", 60 | "stop_token": "cpp", 61 | "streambuf": "cpp", 62 | "string": "cpp", 63 | "system_error": "cpp", 64 | "thread": "cpp", 65 | "tuple": "cpp", 66 | "type_traits": "cpp", 67 | "typeinfo": "cpp", 68 | "unordered_map": "cpp", 69 | "unordered_set": "cpp", 70 | "utility": "cpp", 71 | "variant": "cpp", 72 | "vector": "cpp", 73 | "xfacet": "cpp", 74 | "xhash": "cpp", 75 | "xiosbase": "cpp", 76 | "xlocale": "cpp", 77 | "xlocbuf": "cpp", 78 | "xlocinfo": "cpp", 79 | "xlocmes": "cpp", 80 | "xlocmon": "cpp", 81 | "xlocnum": "cpp", 82 | "xloctime": "cpp", 83 | "xmemory": "cpp", 84 | "xstring": "cpp", 85 | "xtr1common": "cpp", 86 | "xtree": "cpp", 87 | "xutility": "cpp", 88 | "cstdarg": "cpp", 89 | "*.tcc": "cpp", 90 | "deque": "cpp", 91 | "memory_resource": "cpp", 92 | "string_view": "cpp", 93 | "any": "cpp", 94 | "codecvt": "cpp", 95 | "filesystem": "cpp", 96 | "valarray": "cpp", 97 | "hash_map": "cpp", 98 | "hash_set": "cpp", 99 | "numbers": "cpp", 100 | "ranges": "cpp", 101 | "semaphore": "cpp" 102 | } 103 | }, 104 | "extensions": { 105 | "recommendations": [ 106 | "ms-vscode.cmake-tools", 107 | "ms-vscode.cpptools-extension-pack" 108 | ] 109 | } 110 | } -------------------------------------------------------------------------------- /controller-software/config/electrical/power/power_sources.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, field 2 | 3 | class POWER_SOURCE: 4 | def __init__(self): 5 | self.types = [self.AC1, self.AC3, self.DC, self.BATTERY] 6 | self.supply_type = None 7 | self.user_params = None 8 | 9 | @dataclass 10 | class COMMON: 11 | # common power source parameters 12 | nominal_voltage: float = field(default=None, metadata={'unit': 'V', 'desc': 'Nominal voltage of the power source', 'required':True}) 13 | max_positive_current: float = field(default=None, metadata={'unit': 'A', 'desc': 'Maximum current allowed to be pulled from the supply', 'required':True}) 14 | max_negative_current: float = field(default=0.0, metadata={'unit': 'A', 'desc': 'Maximum current allowed to be pushed to the supply', 'required':True}) 15 | 16 | @dataclass 17 | class AC1: 18 | # AC1 specific parameters 19 | line_frequency: float = field(default=None, metadata={'unit': 'Hz', 'desc': 'Line frequency in Hz', 'required':True}) 20 | allowed_voltage_fluxuation: float = field(default=10, metadata={'unit': '%', 'desc': 'Max voltage fluxuation in percent', 'required':True}) 21 | 22 | @dataclass 23 | class AC3: 24 | # AC3 specific parameters 25 | line_frequency: float = field(default=None, metadata={'unit': 'Hz', 'desc': 'Line frequency in Hz', 'required':True}) 26 | allowed_voltage_fluxuation: float = field(default=10, metadata={'unit': '%', 'desc': 'Max voltage fluxuation in percent', 'required':True}) 27 | 28 | @dataclass 29 | class DC: 30 | # DC specific parameters 31 | allowed_voltage_fluxuation: float = field(default=10, metadata={'unit': '%', 'desc': 'Max voltage fluxuation in percent', 'required':True}) 32 | 33 | @dataclass 34 | class BATTERY: 35 | # Battery specific parameters 36 | max_voltage: float = field(default=None, metadata={'unit': 'V', 'desc': 'Maximum voltage of the battery', 'required':True}) 37 | min_voltage: float = field(default=None, metadata={'unit': 'V', 'desc': 'Minimum voltage of the battery', 'required':True}) 38 | over_under_voltage_trip_time: float = field(default=5, metadata={'unit': 's', 'desc': 'Time before the battery trips on over/under voltage', 'required':True}) 39 | max_temperature: float = field(default=None, metadata={'unit': 'C', 'desc': 'Maximum temperature allowed for the battery', 'required':False}) 40 | min_temperature: float = field(default=None, metadata={'unit': 'C', 'desc': 'Minimum temperature allowed for the battery', 'required':False}) 41 | cell_count: int = field(default=None, metadata={'desc': 'Number of cells in the battery', 'required':False}) 42 | chemistry: str = field(default=None, metadata={'desc': 'Chemistry of the battery', 'required':False}) -------------------------------------------------------------------------------- /controller-software/core/controller/inc/fpga_module_manager.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "json.hpp" 5 | #include "fpga_interface.h" 6 | #include "fpga_instructions.h" 7 | 8 | 9 | // include all drivers here 10 | 11 | #include "serial_interface_card.h" 12 | //#include "global_timers.h" 13 | #include "fanuc_encoders.h" 14 | #include "yaskawa_encoders.h" 15 | #include "em_serial_controller.h" 16 | 17 | 18 | #pragma once 19 | using json = nlohmann::json; 20 | 21 | 22 | class fpga_module_manager { 23 | public: 24 | fpga_module_manager(); 25 | ~fpga_module_manager(); 26 | 27 | uint32_t load_config(std::string config_file); 28 | 29 | uint32_t load_instructions(std::string file_path); 30 | 31 | uint32_t save_instructions(std::string file_path, bool write_protected = false); 32 | 33 | uint32_t set_fpga_interface(Fpga_Interface* fpga_interface); 34 | 35 | uint32_t initialize_fpga(); 36 | 37 | uint32_t load_drivers(std::string driver_config_file); 38 | 39 | uint32_t compile_instructions(); 40 | 41 | void set_update_frequency(uint32_t frequency); 42 | 43 | uint32_t create_global_variables(); 44 | 45 | void set_microseconds(const uint64_t* microseconds); 46 | 47 | uint32_t run_update(); 48 | 49 | std::shared_ptr get_driver(uint32_t index); 50 | 51 | Node_Core* node_core = nullptr; 52 | 53 | private: 54 | 55 | const uint64_t* microseconds = nullptr; 56 | 57 | json config; 58 | Fpga_Interface* fpga_interface; 59 | fpga_mem_layout mem_layout; 60 | 61 | std::string fpga_config_path = ""; 62 | 63 | void* PS_PL_control_ptr = nullptr; 64 | void* PL_PS_control_ptr = nullptr; 65 | void* PS_PL_data_ptr = nullptr; 66 | void* PL_PS_data_ptr = nullptr; 67 | void* PS_PL_dma_instructions_ptr = nullptr; 68 | 69 | // how much memory has been allocated to the drivers 70 | uint32_t allocated_PS_PL_address = 0; 71 | uint32_t allocated_PL_PS_address = 0; 72 | 73 | std::vector fpga_instructions_old; 74 | bool instructions_modified = false; 75 | 76 | fpga_instructions fpga_instr = fpga_instructions(); 77 | 78 | std::vector> drivers; // this will point to all drivers that are loaded 79 | 80 | uint32_t load_driver(json config, std::string module_name, json* user_driver_config); 81 | 82 | template 83 | bool load_json_value(const json& config, const std::string& value_name, T* dest); 84 | 85 | uint32_t load_mem_layout(); 86 | 87 | uint32_t set_memory_pointers(fpga_mem* mem); // set the memory pointers for the driver 88 | 89 | uint32_t allocate_driver_memory(const fpga_mem* mem); // allocate memory that the driver will use 90 | 91 | uint32_t write_instructions_to_fpga(); // write the instructions to the FPGA 92 | 93 | }; -------------------------------------------------------------------------------- /Documentation/API Backend/Motion/Motion APIs.md: -------------------------------------------------------------------------------- 1 | 2 | # Jogging 3 | 4 | API commands for Joggin the robot at {{JogSpeed}} rotations per minute (TODO: define jog speed) 5 | 6 | Because of the stream like nature of jogging, jogging requests are sent as notifications without id. On a success the client will not receive any response. On a failure the API will send out a {{MotionError}} notification. Jogging requests should be sent at a regular frequency in order to maintain the current motion. When no jogging message has not been received for {{JogFrequency}} (default 100ms) milliseconds, the motion controller will immediatly stop. 7 | 8 | Be advised that high latency connection can result in motor stutter. Increasing the {{JogFrequency}} can fix this but will result in a more delayed stop time. 9 | 10 | ### Raw joint movements 11 | 12 | This is a notification command. There should not be a ID in this request. 13 | ``` 14 | {"method": "jogJoint", "params": [jointIndex, direction]} 15 | ``` 16 | Params: 17 | jointIndex: 0 based index for the joint. 18 | direction: 1 for positive direction, 0 for negative direction 19 | 20 | Result: None 21 | Error: 22 | {{MotionError}} notification will be sent out if a jog command could not be followed. 23 | Example: 24 | ``` 25 | {"method": "jogJoint", "params": [0, 1]} 26 | ``` 27 | Robot joint 0 should go in the positive direction at {{JogSpeed}} 28 | 29 | ### Tool frame jogging 30 | 31 | This is a notification command. There should not be a ID in this request. 32 | ``` 33 | {"method": "jogTool", "params": [toolIndex, vector]} 34 | ``` 35 | Params: 36 | toolIndex: 0 based index for the tool frame. 37 | vector: Array designating the direction vecor in XYZ format (TODO: this is an assumption on the capabilites from the motion planning team. Change this if needed) 38 | 39 | Result: None 40 | Error: 41 | {{MotionError}} notification will be sent out if a jog command could not be followed. 42 | Example: 43 | ``` 44 | {"method": "jogTool", "params": [0, [0,1,0]]} 45 | ``` 46 | Robot should go forward in the Y direction with repect to the tool frame 47 | 48 | ### Workspace frame jogging 49 | This is a notification command. There should not be a ID in this request. 50 | ``` 51 | {"method": "jogWorkspace", "params": [workspaceIndex, vector]} 52 | ``` 53 | Params: 54 | workspaceIndex: 0 based index for the workspace frame. 55 | vector: Array designating the direction vecor in XYZ format (TODO: this is an assumption on the capabilites from the motion planning team. Change this if needed) 56 | 57 | Result: None 58 | Error: 59 | {{MotionError}} notification will be sent out if a jog command could not be followed. 60 | 61 | Example: 62 | ``` 63 | {"method": "jogWorkspace", "params": [0, [0,1,0]]} 64 | ``` 65 | Robot should go forward in the Y direction with repect to the workspace frame -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/inc/print_float.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "base_call.h" 4 | 5 | // print value to the console on the controller side 6 | class print_float: public Base_API { 7 | private: 8 | float value = 0; 9 | 10 | public: 11 | print_float(){ 12 | api_call_id = api_call_id_t::PRINT_FLOAT; 13 | } 14 | 15 | unsigned int web_input_data(json* data) override { 16 | if(data->contains("value")){ 17 | if(data->at("value").is_number()){ 18 | value = data->at("value").get(); 19 | } 20 | else { 21 | return 1; 22 | } 23 | } 24 | else { 25 | return 1; 26 | } 27 | return 0; 28 | } 29 | 30 | unsigned int web_output_data(json* data) override { 31 | data->emplace("call_name", "print_float"); 32 | data->emplace("value", value); 33 | return 0; 34 | } 35 | 36 | unsigned int web_write_to_shared_mem() override { 37 | // write the call info to shared memory 38 | 39 | uint32_t start_index = web_to_controller_data_mem_index; 40 | uint32_t size = sizeof(value); 41 | 42 | copy_to_web_to_controller_data_buffer(&value, sizeof(value)); 43 | 44 | update_web_to_controller_control_buffer(api_call_id_t::PRINT_FLOAT, start_index, size); 45 | 46 | return 0; 47 | } 48 | 49 | unsigned int controller_read_from_shared_mem() override { 50 | // read the data from shared memory, layout must match the format set in web_write_to_shared_mem 51 | 52 | copy_from_web_to_controller_data_buffer(&value, sizeof(value)); 53 | 54 | return 0; 55 | } 56 | 57 | unsigned int controller_write_to_shared_mem() override { 58 | // write the call info to shared memory, layout must match web_read_from_shared_mem 59 | 60 | uint32_t start_index = controller_to_web_data_mem_index; 61 | uint32_t size = sizeof(value); 62 | 63 | copy_to_controller_to_web_data_buffer(&value, sizeof(value)); 64 | 65 | update_controller_to_web_control_buffer(api_call_id_t::PRINT_FLOAT, start_index, size); 66 | 67 | return 0; 68 | } 69 | 70 | unsigned int web_read_from_shared_mem() override { 71 | // read the data from shared memory, layout must match the format set in controller_write_to_shared_mem 72 | 73 | copy_from_controller_to_web_data_buffer(&value, sizeof(value)); 74 | 75 | return 0; 76 | } 77 | 78 | unsigned int run() override { 79 | // run the API call 80 | 81 | std::cout << "value: " << value << std::endl; 82 | 83 | return 0; 84 | } 85 | }; 86 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/inc/print_uint32.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "base_call.h" 4 | 5 | // print value to the console on the controller side 6 | class print_uint32: public Base_API { 7 | private: 8 | uint32_t value = 0; 9 | 10 | public: 11 | print_uint32(){ 12 | api_call_id = api_call_id_t::PRINT_UINT32; 13 | } 14 | 15 | unsigned int web_input_data(json* data) override { 16 | if(data->contains("value")){ 17 | if(data->at("value").is_number_integer()){ 18 | value = data->at("value").get(); 19 | } 20 | else { 21 | return 1; 22 | } 23 | } 24 | else { 25 | return 1; 26 | } 27 | return 0; 28 | } 29 | 30 | unsigned int web_output_data(json* data) override { 31 | data->emplace("call_name", "print_uint32"); 32 | data->emplace("value", value); 33 | return 0; 34 | } 35 | 36 | unsigned int web_write_to_shared_mem() override { 37 | // write the call info to shared memory 38 | 39 | uint32_t start_index = web_to_controller_data_mem_index; 40 | uint32_t size = sizeof(value); 41 | 42 | copy_to_web_to_controller_data_buffer(&value, sizeof(value)); 43 | 44 | update_web_to_controller_control_buffer(api_call_id_t::PRINT_UINT32, start_index, size); 45 | 46 | return 0; 47 | } 48 | 49 | unsigned int controller_read_from_shared_mem() override { 50 | // read the data from shared memory, layout must match the format set in web_write_to_shared_mem 51 | 52 | copy_from_web_to_controller_data_buffer(&value, sizeof(value)); 53 | 54 | return 0; 55 | } 56 | 57 | unsigned int controller_write_to_shared_mem() override { 58 | // write the call info to shared memory, layout must match web_read_from_shared_mem 59 | 60 | uint32_t start_index = controller_to_web_data_mem_index; 61 | uint32_t size = sizeof(value); 62 | 63 | copy_to_controller_to_web_data_buffer(&value, sizeof(value)); 64 | 65 | update_controller_to_web_control_buffer(api_call_id_t::PRINT_UINT32, start_index, size); 66 | 67 | return 0; 68 | } 69 | 70 | unsigned int web_read_from_shared_mem() override { 71 | // read the data from shared memory, layout must match the format set in controller_write_to_shared_mem 72 | 73 | copy_from_controller_to_web_data_buffer(&value, sizeof(value)); 74 | 75 | return 0; 76 | } 77 | 78 | unsigned int run() override { 79 | // run the API call 80 | 81 | std::cout << "value: " << value << std::endl; 82 | 83 | return 0; 84 | } 85 | }; 86 | -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/yaskawa_encoder_testing.py: -------------------------------------------------------------------------------- 1 | import pyvisa 2 | import numpy as np 3 | 4 | # Connect to the instrument (update the resource string with your DG1022's IP) 5 | rm = pyvisa.ResourceManager() 6 | rm.list_resources() 7 | awg = rm.open_resource('TCPIP0::192.168.1.236::INSTR') 8 | 9 | 10 | sample_rate = 40e6 # 40 MSa/s 11 | 12 | data = "0011001100110011001100110011001101001010101010110010101010110010101010110010101010110011001010101010110" 13 | 14 | invert = False 15 | 16 | bit_time = 100e-9 17 | 18 | tx_points = "" 19 | enable_points = "" 20 | 21 | if invert: 22 | data = data.replace("0", "2") 23 | data = data.replace("1", "0") 24 | data = data.replace("2", "1") 25 | 26 | for i in range(0, len(data)): 27 | if data[i] == '1': 28 | for j in range(0, int(sample_rate * bit_time)): 29 | tx_points = f"{tx_points}1.0," 30 | else: 31 | for j in range(0, int(sample_rate * bit_time)): 32 | tx_points = f"{tx_points}-1.0," 33 | 34 | for j in range(0, int(sample_rate * bit_time)): 35 | enable_points = f"{enable_points}1.0," 36 | 37 | for j in range(0, int(sample_rate * bit_time) * 4000): 38 | tx_points = f"{tx_points}-1.0," 39 | enable_points = f"{enable_points}-1.0," 40 | 41 | #tx_waveform = np.array(points) 42 | 43 | # Convert numpy arrays to comma-separated strings (the format expected by the DG1022) 44 | #tx_str = ','.join(map(str, tx_waveform)) 45 | #tx_en_str = ','.join(map(str, tx_en_waveform)) 46 | 47 | awg.write("OUTP1 OFF") 48 | awg.write("OUTP2 OFF") 49 | 50 | awg.write("SOUR1:FUNC USER") 51 | awg.write("SOUR1:VOLT:UNIT VPP") 52 | awg.write("SOUR1:VOLT:HIGH 3.3") 53 | awg.write("SOUR1:VOLT:LOW 0") 54 | # awg.write("SOUR1:FREQ 10000") 55 | awg.write("SOUR1:APPL:ARB") 56 | 57 | awg.write("SOUR2:FUNC USER") 58 | awg.write("SOUR2:VOLT:UNIT VPP") 59 | awg.write("SOUR2:VOLT:HIGH 3.3") 60 | awg.write("SOUR2:VOLT:LOW 0") 61 | # awg.write("SOUR2:FREQ 10000") 62 | awg.write("SOUR2:APPL:ARB") 63 | 64 | 65 | # --- Configure Channel 1 for TX --- 66 | awg.write(f"SOUR1:DATA VOLATILE,{tx_points}") 67 | awg.write(f"SOUR2:DATA VOLATILE,{enable_points}") 68 | 69 | # awg.write("DATA:DAC VOLATILE,8192,16383,8192,0") 70 | awg.write("SOUR1:FUNC:USER VOLATILE") 71 | awg.write("SOUR2:FUNC:USER VOLATILE") 72 | 73 | # Set both channels to burst mode triggered by the same source 74 | # awg.write("SOUR1:BURSt:STAT ON") 75 | # awg.write("SOUR1:BURSt:MODE TRIG") 76 | # awg.write("SOUR1:BURSt:SOUR INT") 77 | # awg.write("SOUR2:BURSt:STAT ON") 78 | # awg.write("SOUR2:BURSt:MODE TRIG") 79 | # awg.write("SOUR2:BURSt:SOUR INT") 80 | 81 | # --- Enable outputs --- 82 | awg.write("OUTP1 ON") 83 | awg.write("OUTP2 ON") 84 | 85 | awg.write("TRIG:IMM") 86 | 87 | 88 | # Optionally, configure triggering/synchronization if needed: 89 | # awg.write("TRIG:SOUR IMM") # Immediate trigger (or configure external trigger as required) 90 | 91 | #awg.write("SOUR2:PHAS:ALIGN") # Align the phases of both channels 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/yaskawa_encoder_vis.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | packetDecoding = { 6 | "single_turn": (60, 75), 7 | "multi_turn": (76, 91), 8 | "unknown_1": (56, 59), 9 | "unknown_2": (40, 47), 10 | } 11 | 12 | 13 | 14 | def get_packets(filename): 15 | with open(filename) as f: 16 | next(f) # skip header 17 | packet = {} 18 | byteData = "" 19 | for line in f: 20 | t, status, timestamp, duration, mosi, miso = line.strip().split(',') 21 | status = status[1:-1] 22 | if status == "enable": 23 | packet = {} 24 | timestamp = float(timestamp) 25 | packet["timestamp"] = timestamp 26 | byteData = "" 27 | continue 28 | 29 | if status == "disable": 30 | if len(byteData) != 13*8: # 13 bytes, less than this means we have a bad packet 31 | continue 32 | 33 | # interpret the byte data 34 | for key, (start, end) in packetDecoding.items(): 35 | bits = byteData[start:end+1] 36 | bits = bits[::-1] 37 | number = int("0b"+bits, base=0) 38 | packet[key] = number 39 | yield packet 40 | continue 41 | 42 | miso = miso[2:] 43 | byteData += miso[::-1] 44 | 45 | 46 | 47 | if __name__ == "__main__": 48 | file = "controller-firmware/python/src/sandbox/yaskawa_decoded_bytes.csv" 49 | packets = [] 50 | for packet in get_packets(file): 51 | packets.append(packet) 52 | 53 | packets = packets[0:100] # limit the number of packets for testing 54 | 55 | # print the packets 56 | # for packet in packets: 57 | # print(f"timestamp: {packet['timestamp']}, single_turn: {packet['single_turn']}, multi_turn: {packet['multi_turn']}, unknown_1: {packet['unknown_1']}, unknown_2: {packet['unknown_2']}") 58 | 59 | # graph the packet values 60 | import matplotlib.pyplot as plt 61 | 62 | timestamps = [pkt["timestamp"] for pkt in packets] 63 | single_turn = [pkt["single_turn"] for pkt in packets] 64 | multi_turn = [pkt["multi_turn"] for pkt in packets] 65 | unknown_1 = [pkt["unknown_1"] for pkt in packets] 66 | unknown_2 = [pkt["unknown_2"]*256 for pkt in packets] 67 | 68 | # bits0 = [] 69 | # bits1 = [] 70 | # bits2 = [] 71 | # bits3 = [] 72 | 73 | # for i in unknown_1: 74 | # bits0.append((i >> 0) & 1) 75 | # bits1.append((i >> 1) & 1) 76 | # bits2.append((i >> 2) & 1) 77 | # bits3.append((i >> 3) & 1) 78 | 79 | plt.figure(figsize=(10, 6)) 80 | plt.plot(timestamps, unknown_2, label='unknown_2', color='blue') 81 | plt.plot(timestamps, single_turn, label='single_turn', color='red') 82 | plt.xlabel('seconds') 83 | plt.ylabel('unknown_2') 84 | plt.title('Unknown 2 vs Seconds') 85 | plt.grid(True) 86 | plt.show() -------------------------------------------------------------------------------- /controller-firmware/python/src/convergent_round.py: -------------------------------------------------------------------------------- 1 | from amaranth import * 2 | from amaranth.sim import Simulator 3 | import numpy as np 4 | from amaranth.back import verilog 5 | import math 6 | 7 | 8 | class convergentRound(Elaboratable): 9 | """ 10 | signed rounding with the lowest bias 11 | 12 | requires 1 clock cycle 13 | """ 14 | 15 | def __init__(self, input_width, output_width): 16 | assert input_width > output_width, "Input width must be greater than output width" 17 | self.input = Signal(signed(input_width)) 18 | self.LastInput = Signal(signed(input_width)) 19 | self.output = Signal(signed(output_width)) 20 | self.done = Signal(reset=1) 21 | 22 | def elaborate(self, platform): 23 | m = Module() 24 | 25 | # Calculate the bit-width difference 26 | shift = len(self.input) - len(self.output) 27 | 28 | # Extract the rounding bits 29 | round_bit = self.input[shift - 1] 30 | 31 | sticky_bit = self.input[:shift - 1].bool() 32 | 33 | # Determine if the input is negative 34 | is_negative = self.input[-1] 35 | 36 | self.rounding = Signal(signed(len(self.output))) 37 | 38 | with m.If(self.input[shift:-1].all() & ~is_negative): # prevent overflow due to round-up when near max positive value 39 | m.d.comb += self.rounding.eq(self.input[shift:]) 40 | 41 | with m.Elif(round_bit & sticky_bit): # round up 42 | m.d.comb += self.rounding.eq(self.input[shift:] + 1) 43 | 44 | with m.Elif(~round_bit): # round down 45 | m.d.comb += self.rounding.eq(self.input[shift:]) 46 | 47 | with m.Elif(round_bit & ~sticky_bit): # round to even 48 | m.d.comb += self.rounding.eq(self.input[shift:] + self.input[shift]) 49 | 50 | m.d.sync += self.output.eq(self.rounding) 51 | m.d.sync += self.LastInput.eq(self.input) 52 | 53 | with m.If(self.input != self.LastInput): 54 | m.d.comb += self.done.eq(0) 55 | with m.Else(): 56 | m.d.comb += self.done.eq(1) 57 | 58 | return m 59 | 60 | 61 | 62 | dut = convergentRound(8, 4) # round a 8bit number to 4 bit 63 | 64 | async def convergentRoundBench(ctx): 65 | 66 | bias = 0 67 | 68 | for i in range(-2**7+1, 2**7-1): 69 | ctx.set(dut.input, i) 70 | await ctx.tick().repeat(3) 71 | 72 | out = ctx.get(dut.output) 73 | 74 | print(i, out) 75 | await ctx.tick() 76 | 77 | 78 | 79 | if __name__ == "__main__": 80 | 81 | sim = Simulator(dut) 82 | sim.add_clock(1/int(100e6)) 83 | sim.add_testbench(convergentRoundBench) 84 | with sim.write_vcd("convergentRound.vcd"): 85 | sim.run() 86 | 87 | # if (True): # export 88 | # top = convergentRound(32, 16) 89 | # with open("controller-firmware/src/amaranth sources/convergentRound.v", "w") as f: 90 | # f.write(verilog.convert(top, name="convergentRound", ports=top.ports)) -------------------------------------------------------------------------------- /controller-software/core/zynq_files/controller/config/user/fpga_driver_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "node_1_serial_card_B": { 3 | "port_modes": { 4 | "0": "RS422", 5 | "1": "RS422", 6 | "8": "RS422", 7 | "9": "RS422" 8 | }, 9 | 10 | "led_modes": { 11 | "0": "BLINK_FAST", 12 | "1": "NODE_VAR", 13 | "2": "NODE_VAR" 14 | } 15 | }, 16 | 17 | "node_2_serial_card_C": { 18 | "port_modes": { 19 | "0": "RS485", 20 | "1": "RS485", 21 | "2": "RS485", 22 | "3": "RS485", 23 | "4": "RS485", 24 | "5": "RS485", 25 | "8": "RS422", 26 | "9": "RS422" 27 | }, 28 | 29 | "led_modes": { 30 | "0": "BLINK_SLOW", 31 | "1": "NODE_VAR", 32 | "2": "NODE_VAR" 33 | } 34 | }, 35 | 36 | "node_5_em_serial_controller_A": { 37 | "baud_rate": 12.5e6, 38 | "devices": { 39 | "0": { 40 | "cyclic_read_node_vars":0, 41 | "cyclic_write_node_vars":1 42 | }, 43 | "1": { 44 | "cyclic_read_node_vars":0, 45 | "cyclic_write_node_vars":1 46 | }, 47 | "2": { 48 | "cyclic_read_node_vars":0, 49 | "cyclic_write_node_vars":1 50 | }, 51 | "3": { 52 | "cyclic_read_node_vars":0, 53 | "cyclic_write_node_vars":1 54 | }, 55 | "4": { 56 | "cyclic_read_node_vars":0, 57 | "cyclic_write_node_vars":1 58 | } 59 | } 60 | }, 61 | 62 | "node_6_em_serial_controller_B": { 63 | "baud_rate": 12.5e6, 64 | "devices": { 65 | "0": { 66 | "cyclic_read_node_vars":0, 67 | "cyclic_write_node_vars":1 68 | }, 69 | "1": { 70 | "cyclic_read_node_vars":0, 71 | "cyclic_write_node_vars":1 72 | }, 73 | "2": { 74 | "cyclic_read_node_vars":0, 75 | "cyclic_write_node_vars":1 76 | }, 77 | "3": { 78 | "cyclic_read_node_vars":0, 79 | "cyclic_write_node_vars":1 80 | } 81 | } 82 | }, 83 | 84 | "node_7_em_serial_controller_C": { 85 | "baud_rate": 12.5e6, 86 | "devices": { 87 | "0": { 88 | "cyclic_read_node_vars":0, 89 | "cyclic_write_node_vars":0 90 | } 91 | } 92 | }, 93 | 94 | "node_8_em_serial_controller_D": { 95 | "baud_rate": 12.5e6, 96 | "devices": { 97 | "0": { 98 | "cyclic_read_node_vars":0, 99 | "cyclic_write_node_vars":0 100 | } 101 | } 102 | } 103 | } -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/edge_delay.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class edge_delay: public base_node { 7 | private: 8 | bool out = false; // output value 9 | bool falling_edge = false; // true if the edge is falling, false if rising 10 | input* in = nullptr; // pointer to the input 11 | 12 | // times are in execution cycles 13 | uint32_t cycles = 0; 14 | uint32_t elapsed_cycles = 0; // cycles since the last edge 15 | 16 | bool configured = false; 17 | 18 | public: 19 | 20 | edge_delay(){ 21 | } 22 | 23 | unsigned int run() override { 24 | 25 | bool in_value = *(bool*)(in->data_pointer); 26 | 27 | if(falling_edge){ 28 | if(in_value){ 29 | elapsed_cycles = 0; 30 | out = true; 31 | } 32 | else if (elapsed_cycles < cycles){ 33 | elapsed_cycles++; 34 | } 35 | else{ 36 | out = false; 37 | } 38 | } 39 | else{ // rising edge 40 | if(!in_value){ 41 | elapsed_cycles = 0; 42 | out = false; 43 | } 44 | else if (elapsed_cycles < cycles){ 45 | elapsed_cycles++; 46 | } 47 | else{ 48 | out = true; 49 | } 50 | } 51 | 52 | return 0; 53 | } 54 | 55 | unsigned int configure_settings(json* json) override{ 56 | 57 | /* 58 | parse settings from json 59 | { 60 | "config":{ 61 | "rising_edge": "true", 62 | "falling_edge": "false", 63 | "cycles": 10 64 | } 65 | } 66 | */ 67 | 68 | if(configured){ 69 | std::cerr << "Error: edge_delay node already configured" << std::endl; 70 | return 1; 71 | } 72 | 73 | if(json->find("rising_edge") != json->end()){ 74 | falling_edge = !json->at("rising_edge").get(); 75 | } 76 | else if(json->find("falling_edge") != json->end()){ 77 | falling_edge = json->at("falling_edge").get(); 78 | } 79 | else{ 80 | std::cerr << "Error: edge_delay node configuration missing 'rising_edge' or 'falling_edge'" << std::endl; 81 | return 1; 82 | } 83 | 84 | cycles = json->at("cycles").get(); 85 | 86 | 87 | outputs.emplace("output", output(io_type::BOOL, &out, &execution_number)); 88 | 89 | inputs.emplace("input", input(io_type::BOOL, nullptr)); 90 | in = &inputs["input"]; 91 | 92 | configured = true; 93 | 94 | return 0; 95 | } 96 | 97 | }; -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2.0.0", 3 | "options": { 4 | "env": { 5 | "xilinx_path": "/home/excessive/Xilinx", 6 | "xilinx_version": "2023.1", 7 | "petalinux_path": "/home/excessive/PetaLinux" 8 | } 9 | }, 10 | 11 | "tasks": [ 12 | { 13 | "label": "create subst path vivado", 14 | "type": "shell", 15 | "windows": { "command": "if not exist S: subst S: ${workspaceFolder}/controller-firmware"}, 16 | "linux": { "command": "echo ''"}, 17 | "problemMatcher": [] 18 | }, 19 | { 20 | "label": "create subst path vitis", 21 | "type": "shell", 22 | "windows": { "command": "if not exist T: subst T: ${workspaceFolder}/controller-software"}, 23 | "linux": { "command": "echo ''"}, 24 | "problemMatcher": [] 25 | }, 26 | { 27 | "label": "setup python venv", 28 | "type": "shell", 29 | "command": "if not exist venv python -m venv venv", 30 | "options": { 31 | "cwd": "${workspaceFolder}/controller-firmware/python/" 32 | }, 33 | "problemMatcher": [] 34 | }, 35 | { 36 | "label": "install python packages", 37 | "type": "shell", 38 | "command": "venv\\Scripts\\activate.bat && pip install -r requirements.txt", 39 | "options": { 40 | "cwd": "${workspaceFolder}/controller-firmware/python/" 41 | }, 42 | "dependsOn": ["setup python venv"], 43 | "problemMatcher": [] 44 | }, 45 | { 46 | "label": "create vivado project", 47 | "type": "shell", 48 | "windows": { "command": "${env:xilinx_path}/vivado/${env:xilinx_version}/settings64.bat && if not exist S:vivado/controller_firmware vivado -mode batch -source ${workspaceFolder}/controller-firmware/vivado/create_project.tcl -tclargs S:Vivado"}, 49 | "linux": { "command": "source ${xilinx_path}/Vivado/${xilinx_version}/settings64.sh && if [ ! -d ${workspaceFolder}/controller-firmware/Vivado/controller-firmware ]; then vivado -mode batch -source ${workspaceFolder}/controller-firmware/Vivado/create_project.tcl -tclargs ${workspaceFolder}/controller-firmware/Vivado; fi"}, 50 | "problemMatcher": [], 51 | "dependsOn": ["create subst path vivado"], 52 | }, 53 | { 54 | "label": "save vivado project", 55 | "type": "shell", 56 | "command": "c:/xilinx/vivado/2023.1/settings64.bat && vivado -mode batch -source ${workspaceFolder}/controller-firmware/vivado/save_project.tcl", // TODO: make this vivado path easier to change 57 | "problemMatcher": [], 58 | "dependsOn": ["create subst path"] 59 | }, 60 | { 61 | "label": "setup project", 62 | "dependsOn": ["create vivado project", "install python packages"], 63 | "problemMatcher": [] 64 | } 65 | //TODO: add tasks for copying and installing petalinux files (.xsa file, etc.) 66 | ] 67 | } -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/test_config_out.json: -------------------------------------------------------------------------------- 1 | { 2 | "info": { 3 | "date": "2025-06-07", 4 | "time": "19:40:42.875177470", 5 | "fpga_config": "controller/config/fpga_configs/test", 6 | "write_protected": false 7 | }, 8 | "networks": { 9 | "serial devices": { 10 | "enable": true, 11 | "type": "sync", 12 | "timeout_usec": 10000, 13 | "update_cycle_trigger_count": 1, 14 | "execution_order": 0, 15 | "connections": { 16 | "0": { 17 | "src_node": "get_global_variable_node_vars.drivers.node_7_em_serial_controller_C.device_0", 18 | "src_port": "output", 19 | "dst_node": "estop_board", 20 | "dst_port": "em_device" 21 | }, 22 | "1": { 23 | "src_node": "case_fan_speed", 24 | "src_port": "output", 25 | "dst_node": "estop_board", 26 | "dst_port": "" 27 | } 28 | }, 29 | "nodes": { 30 | "get_global_variable_node_vars.drivers.node_7_em_serial_controller_C.device_0": { 31 | "type": "get_global_variable", 32 | "config": { 33 | "variables": { 34 | "output": "node_vars.drivers.node_7_em_serial_controller_C.device_0" 35 | } 36 | } 37 | }, 38 | "estop_board": { 39 | "type": "em_serial_device", 40 | "config": { 41 | "enabled": false, 42 | "comm": { 43 | "device_address": 0, 44 | "timeout_tries": 0 45 | }, 46 | "device": { 47 | "device_descriptor": "controller/config/em_devices/device_H1_0_F0.json", 48 | "cyclic_write_regs": [], 49 | "cyclic_read_regs": [], 50 | "async_write_regs": [ 51 | { 52 | "name": "fan_cmd_speed", 53 | "update_rate_cycles": 100 54 | } 55 | ], 56 | "async_read_regs": [ 57 | { 58 | "name": "dms_ok", 59 | "update_rate_cycles": 100 60 | } 61 | ], 62 | "set": [] 63 | } 64 | } 65 | }, 66 | "case_fan_speed": { 67 | "type": "constant", 68 | "config": { 69 | "value": "10000", 70 | "type": "uint16" 71 | } 72 | } 73 | } 74 | } 75 | } 76 | } -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/constant.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | // constants cannot be changed once configured 6 | 7 | class constant: public base_node { 8 | private: 9 | void* data_ptr = nullptr; // pointer to the data that will be used as output 10 | output* output_ptr = nullptr; // pointer to the output object 11 | 12 | public: 13 | 14 | constant(){ 15 | execution_number = -1; 16 | } 17 | 18 | unsigned int run() override { 19 | // nothing to do 20 | return 0; 21 | } 22 | 23 | unsigned int configure_settings(json* json) override{ 24 | 25 | /* 26 | parse settings from json 27 | { 28 | "config":{ 29 | "type": "uint8", 30 | "value": -1.0, 31 | 32 | } 33 | } 34 | */ 35 | 36 | if(data_ptr != nullptr){ 37 | std::cerr << "Error: constant node already configured" << std::endl; 38 | return 1; 39 | } 40 | 41 | io_type type = get_io_type(json->at("type").get()); 42 | if(type == io_type::UNDEFINED){ 43 | std::cerr << "Error: invalid type for constant node" << std::endl; 44 | return 1; 45 | } 46 | 47 | switch(type){ 48 | case io_type::UINT8: 49 | data_ptr = new uint8_t(json->at("value").get()); 50 | break; 51 | case io_type::INT8: 52 | data_ptr = new int8_t(json->at("value").get()); 53 | break; 54 | case io_type::UINT16: 55 | data_ptr = new uint16_t(json->at("value").get()); 56 | break; 57 | case io_type::INT16: 58 | data_ptr = new int16_t(json->at("value").get()); 59 | break; 60 | case io_type::UINT32: 61 | data_ptr = new uint32_t(json->at("value").get()); 62 | break; 63 | case io_type::INT32: 64 | data_ptr = new int32_t(json->at("value").get()); 65 | break; 66 | case io_type::FLOAT: 67 | data_ptr = new float(json->at("value").get()); 68 | break; 69 | case io_type::DOUBLE: 70 | data_ptr = new double(json->at("value").get()); 71 | break; 72 | case io_type::BOOL: 73 | data_ptr = new bool(json->at("value").get()); 74 | break; 75 | default: 76 | std::cerr << "Error: invalid type for constant node" << std::endl; 77 | return 1; 78 | } 79 | 80 | outputs.emplace("output", output(type, data_ptr, &execution_number)); 81 | 82 | return 0; 83 | } 84 | 85 | }; -------------------------------------------------------------------------------- /em-os/project-spec/configs/plnx_syshw_data: -------------------------------------------------------------------------------- 1 | device_id: 7z020 2 | hw_design_name: controller_firmware_top 3 | processor: 4 | ps7_cortexa9_0: 5 | arch: arm 6 | ip_name: ps7_cortexa9 7 | slaves_strings: ps7_afi_0 ps7_afi_1 ps7_afi_2 ps7_afi_3 ps7_coresight_comp_0 ps7_ddr_0 ps7_ddrc_0 ps7_dev_cfg_0 ps7_dma_ns ps7_dma_s ps7_ethernet_0 ps7_globaltimer_0 ps7_gpio_0 ps7_gpv_0 ps7_intc_dist_0 ps7_iop_bus_config_0 ps7_l2cachec_0 ps7_ocmc_0 ps7_pl310_0 ps7_pmu_0 ps7_qspi_0 ps7_qspi_linear_0 ps7_ram_0 ps7_ram_1 ps7_scuc_0 ps7_scugic_0 ps7_scutimer_0 ps7_scuwdt_0 ps7_sd_0 ps7_slcr_0 ps7_uart_1 ps7_xadc_0 8 | slaves: 9 | ps7_ddr_0_bankless: 10 | device_type: memory 11 | ip_name: ps7_ddr 12 | baseaddr: 0x0 13 | highaddr: 0x3FFFFFFF 14 | ps7_uart_1: 15 | device_type: serial 16 | ip_name: ps7_uart 17 | ps7_ethernet_0: 18 | device_type: ethernet 19 | ip_name: ps7_ethernet 20 | ps7_qspi_0_bankless: 21 | device_type: flash 22 | ip_name: ps7_qspi 23 | ps7_sd_0: 24 | device_type: sd 25 | ip_name: ps7_sdio 26 | ps7_afi_0: 27 | ip_name: ps7_afi 28 | ps7_afi_1: 29 | ip_name: ps7_afi 30 | ps7_afi_2: 31 | ip_name: ps7_afi 32 | ps7_afi_3: 33 | ip_name: ps7_afi 34 | ps7_coresight_comp_0: 35 | ip_name: ps7_coresight_comp 36 | ps7_ddr_0: 37 | ip_name: ps7_ddr 38 | ps7_ddrc_0: 39 | ip_name: ps7_ddrc 40 | ps7_dev_cfg_0: 41 | ip_name: ps7_dev_cfg 42 | ps7_dma_ns: 43 | ip_name: ps7_dma 44 | ps7_dma_s: 45 | ip_name: ps7_dma 46 | ps7_globaltimer_0: 47 | ip_name: ps7_globaltimer 48 | ps7_gpio_0: 49 | ip_name: ps7_gpio 50 | ps7_gpv_0: 51 | ip_name: ps7_gpv 52 | ps7_intc_dist_0: 53 | ip_name: ps7_intc_dist 54 | ps7_iop_bus_config_0: 55 | ip_name: ps7_iop_bus_config 56 | ps7_l2cachec_0: 57 | ip_name: ps7_l2cachec 58 | ps7_ocmc_0: 59 | ip_name: ps7_ocmc 60 | ps7_pl310_0: 61 | ip_name: ps7_pl310 62 | ps7_pmu_0: 63 | ip_name: ps7_pmu 64 | ps7_qspi_0: 65 | ip_name: ps7_qspi 66 | ps7_qspi_linear_0: 67 | ip_name: ps7_qspi_linear 68 | ps7_ram_0: 69 | ip_name: ps7_ram 70 | ps7_ram_1: 71 | ip_name: ps7_ram 72 | ps7_scuc_0: 73 | ip_name: ps7_scuc 74 | ps7_scugic_0: 75 | ip_name: ps7_scugic 76 | ps7_scutimer_0: 77 | ip_name: ps7_scutimer 78 | ps7_scuwdt_0: 79 | ip_name: ps7_scuwdt 80 | ps7_slcr_0: 81 | ip_name: ps7_slcr 82 | ps7_xadc_0: 83 | ip_name: ps7_xadc 84 | -------------------------------------------------------------------------------- /controller-software/core/controller/inc/fpga_interface.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #pragma once 6 | 7 | struct fpga_mem_layout { 8 | 9 | uint32_t OCM_BASE_ADDR=0; 10 | uint32_t OCM_SIZE=0; 11 | 12 | uint32_t PS_to_PL_control_base_addr_offset=0; 13 | uint32_t PS_to_PL_control_size=0; // size of the memory in 8 bit words 14 | uint32_t PL_to_PS_control_base_addr_offset=0; 15 | uint32_t PL_to_PS_control_size=0; // size of the memory in 8 bit words 16 | 17 | uint32_t PS_to_PL_data_base_addr_offset=0; 18 | uint32_t PS_to_PL_data_size=0; // size of the memory in 8 bit words 19 | uint32_t PL_to_PS_data_base_addr_offset=0; 20 | uint32_t PL_to_PS_data_size=0; // size of the memory in 8 bit words 21 | 22 | uint32_t PS_to_PL_dma_instructions_base_addr_offset=0; 23 | uint32_t PS_to_PL_dma_instructions_size=0; // size of the memory in 8 bit words 24 | 25 | uint32_t data_memory_size=0; // size of the data memory in 32 bit words 26 | }; 27 | 28 | 29 | class Fpga_Interface { 30 | public: 31 | Fpga_Interface(); 32 | ~Fpga_Interface(); 33 | 34 | uint32_t initialize(fpga_mem_layout mem_layout, std::string bitstreamPath); // resets shared memory and loads bitstream 35 | 36 | 37 | // get pointers to memory accessible by the FPGA 38 | // these should ONLY be used after the FPGA has finished reading/writing to the memory and the fpga_busy flag is cleared 39 | // note that if the FPGA memory becomes unallocated, these functions will throw errors, but the previously returned pointers will likely lead to segfaults 40 | void* get_PS_to_PL_control_pointer(uint32_t offset); // should only be written to 41 | void* get_PL_to_PS_control_pointer(uint32_t offset); // should only be read from 42 | 43 | void* get_PS_to_PL_data_pointer(uint32_t offset); // should only be written to 44 | void* get_PL_to_PS_data_pointer(uint32_t offset); // should only be read from 45 | 46 | void* get_PS_to_PL_dma_instructions_pointer(uint32_t offset); // should only be written to 47 | 48 | uint32_t wait_for_update(); // wait for new data from the FPGA to be ready 49 | 50 | uint32_t set_update_frequency(uint32_t frequency); // frequency at which the FPGA will update in Hz 51 | 52 | void cache_flush_all(); // writes any changed data from CPU to memory 53 | void cache_invalidate_all(); // invalidates cached memory from FPGA 54 | 55 | 56 | 57 | private: 58 | fpga_mem_layout mem_layout; 59 | 60 | uint16_t* fpga_main_trigger_counter = nullptr; // 16 bit counter at 25 Mhz 61 | uint16_t* fpga_watchdog = nullptr; // this value must be written at LEAST every 16 update cycles to prevent the FPGA DMA from shutting down 62 | bool first_cycle = true; 63 | 64 | void* ocm_base_pointer; 65 | 66 | struct pollfd mem_update_running_fds[1]; 67 | struct pollfd mem_update_done_fds[1]; 68 | 69 | void cache_flush(void* addr, uint32_t size); 70 | void cache_invalidate(void* addr, uint32_t size); 71 | 72 | void feed_watchdog(); // feed the watchdog to prevent the FPGA DMA from shutting down 73 | 74 | void error_if_nullptr(); // check to ensure memory is allocated before returning any pointers 75 | }; 76 | -------------------------------------------------------------------------------- /controller-firmware/python/src/global_timer.py: -------------------------------------------------------------------------------- 1 | from amaranth import * 2 | from amaranth.lib.wiring import Component, In, Out 3 | from amaranth.sim import Simulator 4 | from registers2 import * 5 | 6 | 7 | 8 | class Global_Timers(Component): 9 | # Global timers for the controller 10 | 11 | 12 | def __init__(self): 13 | 14 | self.number_of_timers = 8 15 | 16 | super().__init__({ 17 | "timer_pulse" : Out(8), 18 | 19 | "trigger": In(1), 20 | 21 | "bram_address": Out(16), 22 | "bram_write_data": Out(32), 23 | "bram_read_data": In(32), 24 | "bram_write_enable": Out(1) 25 | }) 26 | 27 | # setup registers for DMA access 28 | driver_settings = { 29 | "clock_frequency": 25e6 30 | } 31 | self.rm = RegisterMapGenerator("global_timers", ["global_timers"], driver_settings, "Global timers for triggering events in the fpga") 32 | 33 | self.rm.add(Register("counter", rw="w", type="unsigned", width=32, desc="Value that the timer counts down from before triggering", bank_size=self.number_of_timers)) 34 | self.rm.generate() 35 | 36 | def elaborate(self, platform): 37 | m = Module() 38 | 39 | self.current_counts = Array(Signal(name=f"current_count_{_}", shape=16) for _ in range(self.number_of_timers)) 40 | self.reset_counts = Array(Signal(name=f"reset_count_{_}", shape=16, reset=10) for _ in range(self.number_of_timers)) 41 | 42 | # with m.Switch(self.bram_address): 43 | # for i in range(self.number_of_timers): 44 | # with m.Case(i): 45 | # with m.If(self.bram_write_enable): 46 | # m.d.sync_100 += self.reset_counts[i].eq(self.bram_write_data) 47 | 48 | with m.If(self.trigger): # trigger input resets first timer 49 | m.d.sync_25 += self.current_counts[0].eq(self.reset_counts[0]) 50 | 51 | for i in range(self.number_of_timers): 52 | 53 | with m.Elif(self.current_counts[i] != 0): 54 | m.d.sync_25 += self.current_counts[i].eq(self.current_counts[i] - 1) 55 | 56 | with m.If(self.current_counts[i] == 1): # trigger output when timer reaches 1, will create a pulse 1 clock cycle long 57 | m.d.comb += self.timer_pulse[i].eq(1) 58 | if i < self.number_of_timers - 1: 59 | m.d.sync_25 += self.current_counts[i+1].eq(self.reset_counts[i+1]) # start the next timer 60 | 61 | return m 62 | 63 | 64 | 65 | dut = Global_Timers() 66 | async def bench(ctx): 67 | # ctx.set(dut.bram_address, 0) 68 | # ctx.set(dut.bram_write_data, 20) 69 | # ctx.set(dut.bram_write_enable, 1) 70 | # await ctx.tick("sync_100") 71 | # ctx.set(dut.bram_write_enable, 0) 72 | 73 | 74 | await ctx.tick("sync_25").repeat(10) 75 | ctx.set(dut.trigger, 1) 76 | await ctx.tick("sync_25") 77 | ctx.set(dut.trigger, 0) 78 | 79 | await ctx.tick("sync_25").repeat(100) 80 | 81 | 82 | 83 | if __name__ == "__main__": 84 | 85 | sim = Simulator(dut) 86 | sim.add_clock(1/25e6, domain="sync_25") 87 | #sim.add_clock(1/100e6, domain="sync_100") 88 | sim.add_testbench(bench) 89 | with sim.write_vcd("global_timers_test.vcd"): 90 | sim.run() 91 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/inc/node_core.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // #ifndef NODE_CORE_H 8 | // #define NODE_CORE_H 9 | #include "node_network.h" 10 | #include "base_global_variable.h" 11 | 12 | class Node_Core { 13 | private: 14 | // set some limits to keep things from getting out of hand 15 | const uint32_t max_networks = 20; // maximum number of networks allowed 16 | const uint32_t max_nodes_per_network = 20; // maximum number of nodes allowed per network 17 | // TODO: add limits for global variable counts 18 | 19 | std::map> networks; 20 | std::vector> network_execution_order; 21 | 22 | std::map> global_variables; 23 | 24 | uint32_t cycle = 0; 25 | 26 | bool enable = false; // enable running the networks 27 | bool single_cycle_step = false; // run one cycle at a time 28 | //bool single_network_step = false; // run one network at a time (not yet implemented) 29 | 30 | std::string fpga_config_file = ""; // file used to configure the fpga 31 | 32 | public: 33 | Node_Core(); 34 | 35 | uint32_t run_update(); 36 | 37 | uint32_t save(std::string file, bool write_protected = false); 38 | 39 | uint32_t load(std::string file); 40 | 41 | uint32_t set_fpga_config(std::string name); 42 | 43 | uint32_t set_enable(bool enable_); 44 | 45 | uint32_t set_single_cycle_step(bool single_cycle_step_); 46 | 47 | uint32_t add_node(std::string network_name, std::string node_name, std::string node_type); 48 | 49 | uint32_t remove_node(std::string network_name, std::string node_name); 50 | 51 | uint32_t configure_node(std::string network_name, std::string node_name, json* data); 52 | 53 | uint32_t connect_nodes(std::string network_name, std::string source_node_name, std::string source_output_name, std::string target_node_name, std::string target_input_name); 54 | 55 | //uint32_t connect_varaiable_get(std::string target_node_name); // only for get_global_variable nodes 56 | 57 | //uint32_t connect_varaiable_set(std::string target_node_name); // only for set_global_variable nodes 58 | 59 | uint32_t create_global_variable(std::string name, io_type type); 60 | 61 | uint32_t get_global_variable_data_ptr(std::string name, void** data); 62 | 63 | uint32_t set_global_variable_data_ptr(std::string name, void* data); 64 | 65 | uint32_t set_global_variable_value(std::string name, void* value); 66 | 67 | io_type get_global_variable_type(std::string name); 68 | 69 | uint32_t delete_global_variable(std::string name); 70 | 71 | uint32_t rename_global_variable(std::string old_name, std::string new_name); 72 | 73 | uint32_t create_network(std::string name); 74 | 75 | uint32_t rename_network(std::string old_name, std::string new_name); 76 | 77 | uint32_t delete_network(std::string name); 78 | 79 | uint32_t configure_network(std::string name, json* data); 80 | 81 | uint32_t rebuild_execution_order(std::string name); 82 | 83 | uint32_t rebuild_overall_execution_order(); 84 | 85 | }; 86 | 87 | //#endif -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/motion_planner_2.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import math 4 | 5 | # ------------------------------- 6 | # Configuration / Input Variables 7 | # ------------------------------- 8 | 9 | # Define the control points (x, y) on a 500x500 canvas. 10 | points = np.array([ 11 | [100, 300], 12 | [100, 100], 13 | [300, 300], 14 | [400, 200], 15 | [500, 300], 16 | [600, 200], 17 | [700, 300], 18 | [800, 200], 19 | [900, 300], 20 | [900, 100] 21 | 22 | ], dtype=np.float32) 23 | 24 | tolerance = 20 25 | velocity = 50 26 | acceleration = 100 27 | 28 | 29 | # Canvas dimensions 30 | width, height = 1000, 1000 31 | 32 | # ------------------------------- 33 | # Compute the Interpolated Curve 34 | # ------------------------------- 35 | 36 | curve = [] # Will hold the (x,y) points of the curve 37 | 38 | curve.append(points[0]) 39 | 40 | circles = [] 41 | 42 | min_radius = (velocity ** 2) / acceleration 43 | 44 | for i in range(1, len(points) - 1): 45 | 46 | v1 = points[i - 1] - points[i] 47 | v2 = points[i + 1] - points[i] 48 | v1 = v1 / np.linalg.norm(v1) 49 | v2 = v2 / np.linalg.norm(v2) 50 | angle = math.acos(np.dot(v1, v2)) 51 | 52 | # find the radius required to meet the tolerance 53 | radius = (tolerance * math.sin(angle / 2)) / (1 - math.sin(angle / 2)) 54 | 55 | #radius = min(radius, min_radius) # Ensure the radius is at least the minimum required to meet the velocity and acceleration constraints 56 | #radius = min_radius 57 | 58 | # find the center of the circle 59 | 60 | # resulting acceleration vector from v1 to v2 61 | a = v1 + v2 62 | 63 | # normalize the acceleration vector 64 | a = a / np.linalg.norm(a) 65 | 66 | # extend the acceleration vector to the center of the circle 67 | center = points[i] + a * (radius/math.sin(angle/2)) 68 | 69 | circles.append((center, radius)) 70 | 71 | 72 | 73 | 74 | curve = np.array(curve, dtype=np.float32) 75 | 76 | # ------------------------------- 77 | # Animation Loop with OpenCV 78 | # ------------------------------- 79 | 80 | for i in range(len(curve)): 81 | # Create a blank black image. 82 | img = np.ones((height, width, 3), dtype=np.uint8) 83 | img *= 100 # Set all pixels to gray 84 | 85 | # Draw the points as small green circles. 86 | for pt in points: 87 | cv2.circle(img, (int(pt[0]), int(pt[1])), 5, (0, 255, 0), -1) 88 | cv2.circle(img, (int(pt[0]), int(pt[1])), tolerance, (0, 255, 0), 1) # Draw a green circle around the point to indicate the max tolerance. 89 | 90 | # Draw the circles 91 | for center, radius in circles: 92 | cv2.circle(img, (int(center[0]), int(center[1])), int(radius), (0, 0, 255), 1) 93 | 94 | # Draw lines connecting the control points (the control polygon) in yellow. 95 | for j in range(1, len(points)): 96 | pt1 = tuple(points[j - 1].astype(int)) 97 | pt2 = tuple(points[j].astype(int)) 98 | cv2.line(img, pt1, pt2, (0, 255, 255), 1) 99 | 100 | # Draw the portion of the computed curve up to the current sample in blue. 101 | for j in range(1, i): 102 | pt1 = tuple(curve[j - 1].astype(int)) 103 | pt2 = tuple(curve[j].astype(int)) 104 | cv2.line(img, pt1, pt2, (255, 0, 0), 2) 105 | 106 | # Display the image. 107 | cv2.imshow("Curve Animation", img) 108 | 109 | # Wait for 10 ms; exit early if the ESC key (27) is pressed. 110 | if cv2.waitKey() & 0xFF == 27: 111 | break 112 | 113 | cv2.destroyAllWindows() 114 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/print.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class print: public base_node { 7 | private: 8 | input* in = nullptr; 9 | input* enable_in = nullptr; // rising edge bool to trigger the print 10 | std::string message = ""; 11 | 12 | io_type type = io_type::UNDEFINED; // type of the data 13 | 14 | bool last_enable_in = false; 15 | 16 | bool configured = false; 17 | 18 | public: 19 | 20 | print(){ 21 | } 22 | 23 | unsigned int run() override { 24 | 25 | bool enable_input_value = *(bool*)enable_in->data_pointer; 26 | 27 | if(enable_input_value && !last_enable_in){ 28 | // rising edge detected 29 | if(type == io_type::BOOL){ 30 | std::cout << message << ": " << *(bool*)in->data_pointer << std::endl; 31 | } 32 | else if(type == io_type::UINT8){ 33 | std::cout << message << ": " << *(uint8_t*)in->data_pointer << std::endl; 34 | } 35 | else if(type == io_type::INT8){ 36 | std::cout << message << ": " << *(int8_t*)in->data_pointer << std::endl; 37 | } 38 | else if(type == io_type::UINT16){ 39 | std::cout << message << ": " << *(uint16_t*)in->data_pointer << std::endl; 40 | } 41 | else if(type == io_type::INT16){ 42 | std::cout << message << ": " << *(int16_t*)in->data_pointer << std::endl; 43 | } 44 | else if(type == io_type::UINT32){ 45 | std::cout << message << ": " << *(uint32_t*)in->data_pointer << std::endl; 46 | } 47 | else if(type == io_type::INT32){ 48 | std::cout << message << ": " << *(int32_t*)in->data_pointer << std::endl; 49 | } 50 | else if(type == io_type::DOUBLE){ 51 | std::cout << message << ": " << *(double*)in->data_pointer << std::endl; 52 | } 53 | else if(type == io_type::FLOAT){ 54 | std::cout << message << ": " << *(float*)in->data_pointer << std::endl; 55 | } 56 | } 57 | last_enable_in = enable_input_value; 58 | 59 | 60 | return 0; 61 | } 62 | 63 | unsigned int configure_settings(json* json) override{ 64 | 65 | /* 66 | parse settings from json 67 | { 68 | "config":{ 69 | "type": "uint8", 70 | "name": "My Print Node" 71 | } 72 | } 73 | */ 74 | 75 | if(configured){ 76 | std::cerr << "Error: cycle_delay node already configured" << std::endl; 77 | return 1; 78 | } 79 | 80 | 81 | type = get_io_type(json->at("type").get()); 82 | if(type == io_type::UNDEFINED){ 83 | std::cerr << "Error: cycle_delay node type is undefined" << std::endl; 84 | return 1; 85 | } 86 | message = json->at("name").get(); 87 | 88 | inputs.emplace("input", input(type, nullptr)); 89 | inputs.emplace("enable", input(io_type::BOOL, nullptr)); 90 | in = &inputs["input"]; 91 | enable_in = &inputs["enable"]; 92 | 93 | configured = true; 94 | 95 | return 0; 96 | } 97 | 98 | }; -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/spline_paths_1.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import math 4 | 5 | tolerance = 20.0 6 | 7 | def average_position(points: np.array, weights: np.array) -> np.array: 8 | return np.sum(points * weights[:, None], axis=0) / np.sum(weights) 9 | 10 | def spline(t, control_points: np.array): 11 | 12 | degree = 3 # number of control points active per time 1 13 | 14 | segment = math.floor(t) 15 | 16 | # first find the center point between the outer control points 17 | center = (control_points[segment] + control_points[segment + 2]) / 2 18 | 19 | # find distance from center to middle control point 20 | distance = np.linalg.norm(control_points[segment + 1] - center) 21 | 22 | # find center weights to ensure middle tolerance is met 23 | center_weight = 2.0 * (distance/tolerance - 1.0) 24 | # outer control points have weight 1.0 25 | 26 | 27 | 28 | 29 | # ------------------------------- 30 | # Configuration / Input Variables 31 | # ------------------------------- 32 | 33 | # Define the control points (x, y) on a 500x500 canvas. 34 | control_points = np.array([ 35 | [100, 300], 36 | [200, 100], 37 | [300, 400], 38 | [400, 200] 39 | ], dtype=np.float32) 40 | 41 | # Define a weight for each point. A higher weight pulls the curve closer to that point. 42 | weights = np.array([1, 5, 1, 1], dtype=np.float32) 43 | 44 | # Number of samples along the curve for drawing 45 | num_samples = 1000 46 | 47 | # Canvas dimensions 48 | width, height = 500, 500 49 | 50 | # ------------------------------- 51 | # Compute the Interpolated Curve 52 | # ------------------------------- 53 | 54 | curve = [] # Will hold the (x,y) points of the curve 55 | 56 | if len(control_points) < 3: 57 | # If only two points, do simple linear interpolation. 58 | for i in range(num_samples): 59 | t = i / (num_samples - 1) 60 | pt = (1 - t) * control_points[0] + t * control_points[1] 61 | curve.append(pt) 62 | else: 63 | # For three or more control points, use the weighted Bézier curve. 64 | for i in range(num_samples): 65 | t = i / (num_samples - 1) 66 | pt = rational_bezier(t, control_points, weights) 67 | curve.append(pt) 68 | 69 | curve = np.array(curve, dtype=np.float32) 70 | 71 | # ------------------------------- 72 | # Animation Loop with OpenCV 73 | # ------------------------------- 74 | 75 | for i in range(1, len(curve)): 76 | # Create a blank black image. 77 | img = np.ones((height, width, 3), dtype=np.uint8) 78 | img *= 100 # Set all pixels to gray 79 | 80 | # Draw the control points as small green circles. 81 | for pt in control_points: 82 | cv2.circle(img, (int(pt[0]), int(pt[1])), 5, (0, 255, 0), -1) 83 | 84 | # Draw lines connecting the control points (the control polygon) in yellow. 85 | for j in range(1, len(control_points)): 86 | pt1 = tuple(control_points[j - 1].astype(int)) 87 | pt2 = tuple(control_points[j].astype(int)) 88 | cv2.line(img, pt1, pt2, (0, 255, 255), 1) 89 | 90 | # Draw the portion of the computed curve up to the current sample in blue. 91 | for j in range(1, i): 92 | pt1 = tuple(curve[j - 1].astype(int)) 93 | pt2 = tuple(curve[j].astype(int)) 94 | cv2.line(img, pt1, pt2, (255, 0, 0), 2) 95 | 96 | # Display the image. 97 | cv2.imshow("Weighted Bézier Curve Animation", img) 98 | 99 | # Wait for 10 ms; exit early if the ESC key (27) is pressed. 100 | if cv2.waitKey(10) & 0xFF == 27: 101 | break 102 | 103 | cv2.destroyAllWindows() 104 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/src/node_io.cpp: -------------------------------------------------------------------------------- 1 | #include "node_io.h" 2 | 3 | output::output(io_type type_, void* data, int* source_execution_number_){ 4 | type = type_; 5 | data_pointer = data; 6 | source_execution_number = source_execution_number_; 7 | } 8 | 9 | void output::mark_for_deletion(){ 10 | delete_flag = true; 11 | } 12 | 13 | input::input(io_type type_, void* default_value_){ 14 | type = type_; 15 | default_value = default_value_; 16 | 17 | if(default_value != nullptr){ 18 | data_pointer = default_value; 19 | return; 20 | } 21 | 22 | // create default value based on type if none provided 23 | switch (type) 24 | { 25 | case io_type::BOOL: 26 | default_value = new bool(false); 27 | break; 28 | case io_type::UINT8: 29 | default_value = new uint8_t(0); 30 | break; 31 | case io_type::UINT16: 32 | default_value = new uint16_t(0); 33 | break; 34 | case io_type::UINT32: 35 | default_value = new uint32_t(0); 36 | break; 37 | case io_type::INT8: 38 | default_value = new int8_t(0); 39 | break; 40 | case io_type::INT16: 41 | default_value = new int16_t(0); 42 | break; 43 | case io_type::INT32: 44 | default_value = new int32_t(0); 45 | break; 46 | case io_type::FLOAT: 47 | default_value = new float(0.0); 48 | break; 49 | case io_type::DOUBLE: 50 | default_value = new double(0.0); 51 | break; 52 | case io_type::EM_SERIAL_DEVICE: 53 | default_value = nullptr; // no default 54 | break; 55 | } 56 | 57 | data_pointer = default_value; 58 | 59 | } 60 | 61 | uint32_t input::set_default(){ 62 | // if(default_value == nullptr){ 63 | // std::cerr << "Error: default value is null" << std::endl; 64 | // return 1; // default value is null 65 | // } 66 | source_execution_number = &default_execution_number; 67 | data_pointer = default_value; 68 | source_output = nullptr; 69 | return 0; 70 | } 71 | 72 | uint32_t input::set_input_data(output* source){ 73 | uint32_t ret = 0; 74 | if(source == nullptr){ 75 | std::cerr << "Error: source is null" << std::endl; 76 | ret = 1; // source is null 77 | if(set_default()){ 78 | ret = 6; // error setting input source and setting default value 79 | } 80 | } 81 | 82 | else if(source->type != type){ 83 | std::cerr << "Error: type mismatch: " << get_io_type_string(source->type) << " -> " << get_io_type_string(type) << std::endl; 84 | ret = 2; // type mismatch 85 | } 86 | 87 | else if(source->data_pointer == nullptr){ 88 | std::cerr << "Error: data pointer is null" << std::endl; 89 | ret = 3; // data pointer is null 90 | } 91 | 92 | else if(source->delete_flag){ 93 | std::cerr << "Error: source output is marked for deletion" << std::endl; 94 | ret = 4; // source output is marked for deletion 95 | } 96 | 97 | else if(source->source_execution_number == nullptr){ 98 | std::cerr << "Error: source execution number is null" << std::endl; 99 | ret = 5; // source execution number is null 100 | } 101 | 102 | if(ret){ 103 | return ret; 104 | } 105 | 106 | else{ 107 | source_output = source; 108 | data_pointer = source->data_pointer; 109 | source_execution_number = source->source_execution_number; 110 | } 111 | return 0; 112 | } 113 | 114 | uint32_t input::reconnect(){ 115 | return set_input_data(source_output); 116 | } -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/motion_planner_3.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import math 4 | 5 | time_step = 0.1 6 | max_velocity = 800 7 | max_acceleration = 40 8 | max_jerk = 10 9 | 10 | 11 | points = np.array([ 12 | [100, 500], 13 | [900, 500] 14 | ], dtype=np.float32) 15 | 16 | 17 | def line_2d_distance_to_time(start_point: np.array, end_point: np.array, distance: float) -> np.array: 18 | speed = 1.0 19 | 20 | line_length = np.linalg.norm(end_point - start_point) 21 | time = distance / speed 22 | time = time * (distance / line_length) 23 | return (distance / speed), line_length 24 | 25 | def interpolate_line(start_point: np.array, end_point: np.array, time: float) -> np.array: 26 | return (1 - time) * start_point + time * end_point 27 | 28 | 29 | line_length = line_2d_distance_to_time(points[0], points[1], 0.0)[1] 30 | 31 | acc_time = 0 32 | vel_time = 0 33 | 34 | 35 | # check length to reach max velocity while accelerating 36 | time_to_max_velocity = max_velocity / max_acceleration 37 | distance_to_max_velocity = 0.5 * max_acceleration * (time_to_max_velocity ** 2) 38 | 39 | # handle case where max velocity is reached before half the line length 40 | if distance_to_max_velocity <= line_length/2: 41 | acc_time = time_to_max_velocity 42 | vel_time = (line_length - 2 * distance_to_max_velocity) / max_velocity 43 | else: 44 | # handle case where max velocity is reached after half the line length 45 | 46 | # find the time to reach half the line length while accelerating 47 | time_to_half_length = math.sqrt((line_length/2) / (0.5 * max_acceleration)) 48 | acc_time = time_to_half_length 49 | 50 | 51 | distance = 0.0 52 | n = 0 53 | 54 | linear_points = [] 55 | time = 0.0 56 | u = 0.0 57 | while 1: 58 | if time < acc_time: 59 | distance = 0.5 * max_acceleration * (time ** 2) 60 | u = distance / line_length 61 | elif time < acc_time + vel_time: 62 | distance = 0.5 * max_acceleration * (acc_time ** 2) 63 | distance = max_velocity * (time - acc_time) 64 | u = distance / line_length 65 | else: 66 | distance = line_length - 0.5 * max_acceleration * ((acc_time - (time - acc_time - vel_time)) ** 2) 67 | u = distance / line_length 68 | 69 | pt = interpolate_line(points[0], points[1], u) 70 | linear_points.append(pt) 71 | 72 | time += time_step 73 | 74 | if time > acc_time + vel_time + acc_time: 75 | break 76 | 77 | 78 | animation_positions = np.array(linear_points, dtype=np.int32) 79 | # Prepare an OpenCV animation window to visualize the motion using the positions array. 80 | # Normalize positions to fit within the window. 81 | 82 | width = 1000 83 | height = 1000 84 | 85 | # Create a blank white canvas. 86 | canvas = np.ones((height, width, 3), dtype=np.uint8) * 255 87 | 88 | # Animate the movement by drawing the traveled path. 89 | if 1: 90 | #for i, pt in enumerate(animation_positions): 91 | for index in range(0, len(animation_positions), 1): 92 | pt = animation_positions[index] 93 | i = index 94 | frame = canvas.copy() 95 | if i > 0: 96 | # Draw the path up to the current point. 97 | for j in range(1, i + 1): 98 | cv2.line(frame, animation_positions[j - 1], animation_positions[j], (0, 0, 255), 2) 99 | # Mark the current position. 100 | cv2.circle(frame, pt, 5, (255, 0, 0), -1) 101 | cv2.imshow("Motion Animation", frame) 102 | if cv2.waitKey(20) == 27: # Exit on pressing ESC key 103 | break 104 | 105 | cv2.waitKey(0) 106 | cv2.destroyAllWindows() 107 | -------------------------------------------------------------------------------- /controller-software/core/controller/api_drivers/calls/inc/base_call.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include // For smart pointers 3 | #include 4 | #include "json.hpp" 5 | #include "node_core.h" 6 | 7 | 8 | using json = nlohmann::json; 9 | 10 | #include "shared_mem.h" 11 | 12 | 13 | #ifndef BASE_API_H 14 | #define BASE_API_H 15 | 16 | 17 | enum api_call_id_t { // list of all possible API calls 18 | DEFAULT, 19 | MACHINE_STATE, 20 | PRINT_UINT32, 21 | PRINT_FLOAT, 22 | CONFIGURE_NODE 23 | }; 24 | 25 | // Base_API class 26 | class Base_API { 27 | public: 28 | 29 | static uint32_t data_buffer_size; 30 | static uint32_t control_buffer_size; 31 | 32 | static void * web_to_controller_data_base_address; 33 | static void * controller_to_web_data_base_address; 34 | 35 | static void * web_to_controller_control_base_address; 36 | static void * controller_to_web_control_base_address; 37 | 38 | static volatile uint32_t * persistent_web_mem; // created by the contoller, but updated and read by the web side to keep track of the last data index (incase web side restarts) 39 | 40 | static uint32_t web_to_controller_data_mem_index; 41 | static uint32_t controller_to_web_data_mem_index; 42 | 43 | static uint32_t web_to_controller_control_mem_index; // index of 32 bit values 44 | static uint32_t controller_to_web_control_mem_index; // index of 32 bit values 45 | 46 | static uint32_t web_to_controller_call_count; 47 | static uint32_t controller_to_web_call_count; 48 | 49 | static uint32_t last_api_call_number; 50 | 51 | uint32_t api_call_number = 0; 52 | uint32_t api_call_id = 0; 53 | uint32_t data_start_index = 0; 54 | uint32_t data_size = 0; 55 | 56 | Node_Core* node_core = nullptr; 57 | 58 | std::string json_data_string = ""; 59 | json json_data; 60 | bool json_parsed = true; 61 | 62 | Base_API(); 63 | 64 | virtual ~Base_API() = default; 65 | 66 | // controller side 67 | 68 | // writes the control info and data to shared memory 69 | virtual uint32_t controller_write_to_shared_mem(); 70 | 71 | // reads the data from shared memory 72 | virtual uint32_t controller_read_from_shared_mem(); 73 | 74 | // run the API call 75 | virtual uint32_t run(); 76 | 77 | 78 | // web side 79 | 80 | // interprets a string and pointer to the associated data, then casts the pointer to the correct type and saves it to the object 81 | virtual uint32_t web_input_data(json* data); 82 | 83 | // interprets the data in the object and saves it to a json object 84 | virtual uint32_t web_output_data(json* data); 85 | 86 | // writes the control info and data to shared memory 87 | virtual uint32_t web_write_to_shared_mem(); 88 | 89 | // reads the data from shared memory 90 | virtual uint32_t web_read_from_shared_mem(); 91 | 92 | 93 | uint32_t update_web_to_controller_control_buffer(api_call_id_t api_call_id, uint32_t start_index, uint32_t size); // web side 94 | 95 | 96 | uint32_t update_controller_to_web_control_buffer(api_call_id_t api_call_id, uint32_t start_index, uint32_t size); // controller side 97 | 98 | 99 | uint32_t copy_to_web_to_controller_data_buffer(void* data_in, uint32_t size); // web side 100 | 101 | 102 | uint32_t copy_to_controller_to_web_data_buffer(void* data_in, uint32_t size); // controller side 103 | 104 | 105 | uint32_t copy_from_web_to_controller_data_buffer(void* data_out, uint32_t size); // controller side 106 | 107 | 108 | uint32_t copy_from_controller_to_web_data_buffer(void* data_out, uint32_t size); // web side 109 | 110 | 111 | uint32_t set_shared_memory(shared_mem shared_mem_obj); 112 | 113 | }; 114 | 115 | #endif -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/vel_estimator.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | class vel_estimator: public base_node { 6 | private: 7 | 8 | float output_val = 0.0f; // Current velocity estimate 9 | 10 | input* position = nullptr; // Input for position (angle) measurements 11 | 12 | 13 | /** 14 | * Simple exponentially-weighted velocity estimator. 15 | * v_est[k] = α * raw_diff + (1-α) * v_est[k-1] 16 | */ 17 | class VelocityEstimatorIIR { 18 | public: 19 | /** 20 | * Call on each new encoder reading. 21 | * @param theta New angle measurement. 22 | * @return Smoothed velocity estimate. 23 | */ 24 | float update(double turns) { 25 | if (!initialized & turns != 0.0) { 26 | prev_turns = turns; 27 | initialized = true; 28 | return v_est; 29 | } 30 | // Compute raw difference (wrapped) 31 | double d = turns - prev_turns; 32 | float raw_v = float(d) / dt; 33 | // Exponential smoothing 34 | v_est = alpha * raw_v + (1.0f - alpha) * v_est; 35 | prev_turns = turns; 36 | return v_est; 37 | } 38 | 39 | uint32_t setAlpha(float new_alpha) { 40 | if (new_alpha > 0.0f && new_alpha <= 1.0f) { 41 | alpha = new_alpha; 42 | } else { 43 | return 1; // Invalid alpha 44 | } 45 | return 0; // Success 46 | } 47 | 48 | void setDt(float new_dt) { 49 | if (new_dt > 0.0f) { 50 | dt = new_dt; 51 | } else { 52 | std::cerr << "Error: dt must be positive" << std::endl; 53 | } 54 | } 55 | 56 | 57 | private: 58 | float alpha = .2f; 59 | float dt = 0.001f; // Time step in seconds TODO: get this from the containing network 60 | double prev_turns = 0.0f; 61 | float v_est; 62 | bool initialized = false; 63 | }; 64 | 65 | VelocityEstimatorIIR estimator; 66 | 67 | public: 68 | vel_estimator() { 69 | inputs.emplace("input", input(io_type::DOUBLE, nullptr)); 70 | outputs.emplace("output", output(io_type::FLOAT, &output_val, &execution_number)); 71 | position = &inputs["input"]; 72 | } 73 | 74 | uint32_t configure_settings(json* data) override { 75 | 76 | /* 77 | parse settings from json 78 | 79 | { 80 | "config": 81 | { 82 | "alpha": 0.2, // smoothing factor (0.0 = no smoothing, 1.0 = no change) 83 | } 84 | } 85 | 86 | */ 87 | 88 | if(data->find("alpha") != data->end()){ 89 | estimator.setAlpha(data->at("alpha")); 90 | } 91 | else { 92 | std::cerr << "Error: 'alpha' not found in configuration" << std::endl; 93 | return 1; // Error code for missing alpha 94 | } 95 | return 0; 96 | } 97 | 98 | uint32_t run() override { 99 | float vel = estimator.update(*reinterpret_cast(position->data_pointer)); 100 | output_val = vel; 101 | return 0; 102 | } 103 | }; 104 | 105 | -------------------------------------------------------------------------------- /controller-software/web/web-interface/server/app.js: -------------------------------------------------------------------------------- 1 | // server/app.js 2 | 3 | const express = require('express'); 4 | const path = require('path'); 5 | const bodyParser = require('body-parser'); 6 | const session = require('express-session'); 7 | const passport = require('passport'); 8 | const LocalStrategy = require('passport-local').Strategy; 9 | const { initWebSocketServer } = require('./wsServer'); 10 | const http = require('http'); 11 | 12 | const app = express(); 13 | 14 | // ===== Middleware Setup ===== 15 | 16 | // Parse URL-encoded bodies (as sent by HTML forms) 17 | app.use(bodyParser.urlencoded({ extended: false })); 18 | 19 | // Setup session management (for demo, a hard-coded secret is used) 20 | app.use(session({ 21 | secret: 'your_secret_key', // In production, use a secure, unpredictable secret 22 | resave: false, 23 | saveUninitialized: false, 24 | })); 25 | 26 | // Initialize Passport and use sessions 27 | app.use(passport.initialize()); 28 | app.use(passport.session()); 29 | 30 | // ===== Fake User Database ===== 31 | 32 | // For this example, we’ll use an in-memory user “database”. 33 | // In a real application, replace this with your user data store. 34 | const users = [ 35 | { id: 1, username: 'admin', password: 'admin' }, 36 | { id: 2, username: 'user', password: 'user' }, 37 | // You can add more users here 38 | ]; 39 | 40 | // ===== Passport Local Strategy ===== 41 | 42 | passport.use(new LocalStrategy((username, password, done) => { 43 | const user = users.find(u => u.username === username); 44 | if (!user) { 45 | return done(null, false, { message: 'Incorrect username.' }); 46 | } 47 | if (user.password !== password) { 48 | return done(null, false, { message: 'Incorrect password.' }); 49 | } 50 | return done(null, user); 51 | })); 52 | 53 | passport.serializeUser((user, done) => { 54 | done(null, user.id); 55 | }); 56 | 57 | passport.deserializeUser((id, done) => { 58 | const user = users.find(u => u.id === id); 59 | done(null, user); 60 | }); 61 | 62 | // ===== Routes ===== 63 | 64 | // Home page: if logged in, redirect to SPA; otherwise, show the login page. 65 | app.get('/', (req, res) => { 66 | if (req.isAuthenticated() || 1) { // For demo purposes, we’ll always redirect to the SPA 67 | return res.redirect('/spa'); 68 | } 69 | res.sendFile(path.join(__dirname, '..', 'client', 'login/login.html')); 70 | }); 71 | 72 | // Login handler: authenticate using Passport. 73 | app.post('/login', passport.authenticate('local', { 74 | successRedirect: '/spa', 75 | failureRedirect: '/', // In a real app, you might want to show a message on failure. 76 | })); 77 | 78 | // Middleware to ensure a route is only accessible when authenticated. 79 | function ensureAuthenticated(req, res, next) { 80 | if (req.isAuthenticated()) { 81 | return next(); 82 | } 83 | res.redirect('/'); 84 | } 85 | 86 | // The test SPA: only accessible if the user is logged in. 87 | app.get('/spa', (req, res) => { // skip ensureAuthenticated for demo purposes 88 | res.sendFile(path.join(__dirname, '..', 'client', 'spa_1/spa.html')); 89 | }); 90 | 91 | // Logout route: end the session and redirect to login. 92 | app.get('/logout', (req, res, next) => { 93 | req.logout(err => { 94 | if (err) { return next(err); } 95 | res.redirect('/'); 96 | }); 97 | }); 98 | 99 | // Serve any static assets from the /client folder. 100 | app.use(express.static(path.join(__dirname, '..', 'client'))); 101 | 102 | 103 | // Create HTTP server from Express 104 | const server = http.createServer(app); 105 | 106 | // Initialize the WebSocket server 107 | initWebSocketServer(server); 108 | 109 | // Finally, start listening 110 | const PORT = process.env.PORT || 3000; 111 | server.listen(PORT, '0.0.0.0', () => { 112 | console.log(`Server listening on port ${PORT}`); 113 | }); -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/src/base_node.cpp: -------------------------------------------------------------------------------- 1 | #include "base_node.h" 2 | 3 | 4 | base_node::base_node(){ 5 | // do nothing 6 | } 7 | 8 | uint32_t base_node::get_output_object(std::string output_name, output*& ptr){ 9 | if(outputs.find(output_name) == outputs.end()){ 10 | return 1; // output name not found 11 | } 12 | ptr = &outputs[output_name]; 13 | return 0; 14 | } 15 | 16 | uint32_t base_node::connect_input(std::string input_name, std::shared_ptr source_node, std::string source_output_name){ 17 | if(inputs.find(input_name) == inputs.end()){ 18 | std::cerr << "Error: internal input name not found: " << input_name << std::endl; 19 | return 1; // internal input name not found 20 | } 21 | 22 | output* output_pointer = nullptr; 23 | if(source_node->get_output_object(source_output_name, output_pointer) != 0){ 24 | std::cerr << "Error: source output name not found: " << source_output_name << std::endl; 25 | return 2; // source output name not found 26 | } 27 | 28 | if (inputs[input_name].set_input_data(output_pointer) != 0){ 29 | std::cerr << "Error: setting input data on " << input_name << std::endl; 30 | return 3; // error setting input data 31 | } 32 | 33 | return 0; 34 | } 35 | 36 | uint32_t base_node::connect_input(std::string input_name, output* source_output){ 37 | if(inputs.find(input_name) == inputs.end()){ 38 | return 1; // internal input name not found 39 | } 40 | 41 | if (inputs[input_name].set_input_data(source_output) != 0){ 42 | return 2; // error setting input data 43 | } 44 | 45 | return 0; 46 | } 47 | 48 | uint32_t base_node::set_global_var_input(std::string input_name, global_variable* variable){ 49 | return 1; // not implemented by default 50 | } 51 | 52 | uint32_t base_node::set_global_var_output(std::string output_name, global_variable* variable){ 53 | return 1; // not implemented by default 54 | } 55 | 56 | uint32_t base_node::disconnect_input(std::string input_name){ 57 | if(inputs.find(input_name) == inputs.end()){ 58 | return 1; // internal input name not found 59 | } 60 | 61 | if (inputs[input_name].set_default() != 0){ 62 | return 2; // error setting default value 63 | } 64 | 65 | return 0; 66 | } 67 | 68 | // fix any inputs pointing to invalid ouitputs by switching to the default value 69 | uint32_t base_node::reconnect_inputs(){ 70 | for (auto& pair : inputs) { 71 | pair.second.reconnect(); 72 | } 73 | return 0; 74 | } 75 | 76 | uint32_t base_node::configure_execution_number(bool* execution_number_modified){ 77 | if(reconnect_inputs() != 0){ 78 | return 1; // error reconnecting inputs 79 | } 80 | 81 | // find the highest execution number of all inputs 82 | int highest_execution_input = -256; 83 | for (auto& pair : inputs) { 84 | if(*pair.second.source_execution_number > highest_execution_input){ 85 | highest_execution_input = *pair.second.source_execution_number; 86 | } 87 | } 88 | 89 | // update the execution number if needed and signal it was modified 90 | if(execution_number != highest_execution_input + 1){ 91 | *execution_number_modified = true; 92 | execution_number = highest_execution_input + 1; 93 | } 94 | 95 | return 0; 96 | } 97 | 98 | // mark all outputs for deletion 99 | void base_node::mark_for_deletion(){ 100 | for (auto& pair : outputs) { 101 | pair.second.mark_for_deletion(); 102 | } 103 | marked_for_deletion = true; 104 | } 105 | 106 | // configure internal settings 107 | uint32_t base_node::configure_settings(json* data){ 108 | return 0; // no settings to configure 109 | } 110 | 111 | base_node::~base_node(){ 112 | // do nothing 113 | } -------------------------------------------------------------------------------- /controller-software/core/controller/src/fpga_module_driver_factory.cpp: -------------------------------------------------------------------------------- 1 | #include "fpga_module_driver_factory.h" 2 | 3 | uint32_t base_driver::load_config(json* config, std::string module_name, Node_Core* node_core, fpga_instructions* fpga_instr, json* user_driver_config){ 4 | this->node_core = node_core; 5 | this->fpga_instr = fpga_instr; 6 | this->config = config; 7 | 8 | loader.setup(module_name, config, &base_mem); 9 | 10 | node_address = loader.get_node_index(); 11 | 12 | node_var_prefix = "node_vars.drivers." + module_name; 13 | 14 | if(custom_load_config(user_driver_config) != 0){ 15 | std::cerr << "Failed to load custom config" << std::endl; 16 | return 1; 17 | } 18 | 19 | return 0; 20 | } 21 | 22 | void base_driver::cpy_source(Register* reg){ 23 | cpy.set_source(reg->pl_data.full_name); 24 | copy_reg = reg; 25 | } 26 | 27 | void base_driver::cpy_destination(Register* reg){ 28 | cpy.set_destination(reg->pl_data.full_name); 29 | copy_reg = reg; 30 | } 31 | 32 | void base_driver::cpy_source(std::string var_name){ 33 | std::string full_name = node_var_prefix + "." + var_name; 34 | 35 | if(copy_reg == nullptr){ 36 | throw std::runtime_error("Register src/dst must be set before node variable"); 37 | } 38 | 39 | node_core->create_global_variable(full_name, io_type::UINT32); 40 | 41 | node_core->get_global_variable_data_ptr(full_name, (void**)&(copy_reg->ps_data.software_data_ptr)); 42 | 43 | // add subregister pointers to node variable 44 | for(auto& var_ptr : copy_reg->sub_register_var_ptrs){ 45 | node_core->get_global_variable_data_ptr(full_name, var_ptr); 46 | } 47 | 48 | cpy.set_source(full_name); 49 | } 50 | 51 | void base_driver::add_node_var(std::string var_name, io_type type, void** data_ptr){ 52 | node_core->create_global_variable(node_var_prefix + "." + var_name, type); 53 | node_core->get_global_variable_data_ptr(node_var_prefix + "." + var_name, data_ptr); 54 | } 55 | 56 | void base_driver::cpy_destination(std::string var_name){ 57 | std::string full_name = node_var_prefix + "." + var_name; 58 | 59 | if(copy_reg == nullptr){ 60 | throw std::runtime_error("Register src/dst must be set before node variable"); 61 | } 62 | 63 | node_core->create_global_variable(full_name, io_type::UINT32); 64 | 65 | node_core->get_global_variable_data_ptr(full_name, (void**)&(copy_reg->ps_data.software_data_ptr)); 66 | 67 | // add subregister pointers to node variable 68 | for(auto& var_ptr : copy_reg->sub_register_var_ptrs){ 69 | node_core->get_global_variable_data_ptr(full_name, var_ptr); 70 | } 71 | 72 | cpy.set_destination(full_name); 73 | } 74 | 75 | void base_driver::cpy_dynamic_source(std::string var_name, uint32_t** data_ptr){ 76 | std::string full_name = node_var_prefix + "." + var_name; 77 | 78 | node_core->create_global_variable(full_name, io_type::UINT32); 79 | node_core->get_global_variable_data_ptr(full_name, (void**)data_ptr); 80 | 81 | cpy.set_register_index_source(full_name); 82 | } 83 | 84 | void base_driver::cpy_add_instruction(){ 85 | fpga_instr->add(cpy); 86 | cpy.clear_register_index_source(); 87 | copy_reg = nullptr; 88 | } 89 | 90 | void base_driver::cpy_time_reference(uint32_t time_reference){ 91 | cpy.set_time_reference(time_reference); 92 | } 93 | 94 | void base_driver::cpy_time_reference(fpga_instructions::copy* ref_instruction, bool write_priority){ 95 | cpy.set_time_reference(ref_instruction, write_priority); 96 | } 97 | 98 | void base_driver::cpy_execution_window(int32_t earliest, int32_t latest){ 99 | cpy.set_execution_window(earliest, latest); 100 | } 101 | 102 | void base_driver::cpy_write_priority(){ 103 | cpy.set_write_priority(); 104 | } 105 | 106 | void base_driver::cpy_read_priority(){ 107 | cpy.set_read_priority(); 108 | } 109 | 110 | -------------------------------------------------------------------------------- /controller-firmware/python/src/sandbox/biquad test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | encoderCountReal = 0.0 5 | encoderCountInteger = 0 6 | 7 | ENCODER_COUNT = 2**32 8 | 9 | filterData = [0, 0] 10 | 11 | PRESCALER = 1 * 2**32 12 | INPUT_ROUNDING = 32 13 | OUTPUT_ROUNDING = 32 14 | A1 = int(-0.04515176221313341 * 2**32) 15 | A2 = int(-0.9404614587008322 * 2**32) 16 | 17 | B0 = int(0.003596694771508549 * 2**32) 18 | B1 = int(0.007193389543017098 * 2**32) 19 | B2 = int(0.003596694771508549 * 2**32) 20 | 21 | times = [] 22 | filteredPos = [] 23 | realEncoder = [] 24 | integerEncoder = [] 25 | 26 | for i in range(4000): 27 | 28 | # update encoder 29 | encoderCountReal += .001 30 | encoderCountInteger = round(encoderCountReal, 0) 31 | if(encoderCountInteger > ENCODER_COUNT-1): 32 | encoderCountInteger -= ENCODER_COUNT 33 | encoderCountReal -= ENCODER_COUNT 34 | elif(encoderCountInteger < 0): 35 | encoderCountInteger += ENCODER_COUNT 36 | encoderCountReal += ENCODER_COUNT 37 | 38 | if(encoderCountInteger > 0): 39 | print("true") 40 | 41 | if(i == 200): 42 | encoderCountReal += ENCODER_COUNT -50 43 | 44 | temp = int(encoderCountInteger * PRESCALER) 45 | temp -= int(filterData[0] * A1) 46 | temp -= int(filterData[1] * A2) 47 | 48 | dn = round(temp / pow(2, INPUT_ROUNDING), 0) 49 | 50 | 51 | temp2 = int(dn * B0) 52 | temp2 += int(filterData[0] * B1) 53 | temp2 += int(filterData[1] * B2) 54 | 55 | 56 | yn = round(temp2 / pow(2, OUTPUT_ROUNDING), 0) 57 | 58 | filterData.insert(0, dn) 59 | filterData.pop(-1) 60 | 61 | print(filterData) 62 | 63 | times.append(i) 64 | filteredPos.append(yn) 65 | realEncoder.append(encoderCountReal) 66 | integerEncoder.append(encoderCountInteger) 67 | 68 | 69 | plt.plot(times, filteredPos) 70 | plt.plot(times, realEncoder) 71 | plt.plot(times, integerEncoder) 72 | plt.show() 73 | 74 | 75 | # import numpy as np 76 | 77 | # class BiquadFilter: 78 | # def __init__(self, b0, b1, b2, a1, a2): 79 | # # Initialize coefficients 80 | # self.b0, self.b1, self.b2 = b0, b1, b2 81 | # self.a1, self.a2 = a1, a2 82 | 83 | # # Initialize state variables 84 | # self.x1, self.x2 = 0.0, 0.0 85 | # self.y1, self.y2 = 0.0, 0.0 86 | 87 | # def process(self, x): 88 | # # Direct Form II filter implementation 89 | # y = self.b0 * x + self.b1 * self.x1 + self.b2 * self.x2 - self.a1 * self.y1 - self.a2 * self.y2 90 | 91 | # # Update state 92 | # self.x2 = self.x1 93 | # self.x1 = x 94 | # self.y2 = self.y1 95 | # self.y1 = y 96 | 97 | # return y 98 | 99 | # # Example usage 100 | # if __name__ == "__main__": 101 | # # Define filter coefficients (example values) 102 | # b0, b1, b2 = 5.074331248486237e-9, 1.0148662496972475e-8, 5.074331248486237e-9 103 | # a1, a2 = -1.999798506779029, 0.9997985270763541 104 | 105 | # # Create a BiquadFilter object 106 | # filter = BiquadFilter(b0, b1, b2, a1, a2) 107 | 108 | # # Example input signal (simple sinusoidal signal) 109 | # fs = 44100 # Sampling frequency 110 | # t = np.linspace(0, 1, fs, endpoint=False) # Time vector 111 | # input_signal = np.sin(2 * np.pi * 50 * t) # 5 Hz sine wave 112 | 113 | # # Process the signal through the filter 114 | # output_signal = np.array([filter.process(x) for x in input_signal]) 115 | 116 | # # You can use matplotlib to visualize the input and output signals 117 | # import matplotlib.pyplot as plt 118 | # plt.figure() 119 | # plt.subplot(2, 1, 1) 120 | # plt.title('Input Signal') 121 | # plt.plot(t, input_signal) 122 | # plt.subplot(2, 1, 2) 123 | # plt.title('Output Signal') 124 | # plt.plot(t, output_signal) 125 | # plt.show() 126 | 127 | -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/inc/node_io.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | // All data types that can be used in the node editor, this includes even custom objects 7 | enum io_type{ 8 | UNDEFINED, 9 | UINT8, 10 | UINT16, 11 | UINT32, 12 | INT8, 13 | INT16, 14 | INT32, 15 | DOUBLE, 16 | BOOL, 17 | FLOAT, 18 | EM_SERIAL_DEVICE 19 | }; 20 | 21 | static const std::map io_type_size = { 22 | {io_type::UINT8, sizeof(uint8_t)}, 23 | {io_type::INT8, sizeof(int8_t)}, 24 | {io_type::UINT16, sizeof(uint16_t)}, 25 | {io_type::INT16, sizeof(int16_t)}, 26 | {io_type::UINT32, sizeof(uint32_t)}, 27 | {io_type::INT32, sizeof(int32_t)}, 28 | {io_type::DOUBLE, sizeof(double)}, 29 | {io_type::BOOL, sizeof(bool)}, 30 | {io_type::FLOAT, sizeof(float)} 31 | }; 32 | 33 | static io_type get_io_type(std::string type){ 34 | 35 | std::transform(type.begin(), type.end(), type.begin(), ::tolower); 36 | 37 | if(type.find("uint8") != std::string::npos){ 38 | return io_type::UINT8; 39 | } 40 | else if(type.find("int8") != std::string::npos){ 41 | return io_type::INT8; 42 | } 43 | else if(type.find("uint16") != std::string::npos){ 44 | return io_type::UINT16; 45 | } 46 | else if(type.find("int16") != std::string::npos){ 47 | return io_type::INT16; 48 | } 49 | else if(type.find("uint32") != std::string::npos){ 50 | return io_type::UINT32; 51 | } 52 | else if(type.find("int32") != std::string::npos){ 53 | return io_type::INT32; 54 | } 55 | else if(type == "double"){ 56 | return io_type::DOUBLE; 57 | } 58 | else if(type == "bool"){ 59 | return io_type::BOOL; 60 | } 61 | else if(type == "float"){ 62 | return io_type::FLOAT; 63 | } 64 | else if(type == "em_serial_device"){ 65 | return io_type::EM_SERIAL_DEVICE; 66 | } 67 | else{ 68 | return io_type::UNDEFINED; 69 | } 70 | } 71 | 72 | static std::string get_io_type_string(io_type type){ 73 | switch(type){ 74 | case io_type::UINT8: return "uint8"; 75 | case io_type::INT8: return "int8"; 76 | case io_type::UINT16: return "uint16"; 77 | case io_type::INT16: return "int16"; 78 | case io_type::UINT32: return "uint32"; 79 | case io_type::INT32: return "int32"; 80 | case io_type::DOUBLE: return "double"; 81 | case io_type::BOOL: return "bool"; 82 | case io_type::FLOAT: return "float"; 83 | case io_type::EM_SERIAL_DEVICE: return "em_serial_device"; 84 | default: return "undefined"; 85 | } 86 | } 87 | 88 | class output{ 89 | public: 90 | io_type type = UNDEFINED; 91 | 92 | int* source_execution_number = nullptr; 93 | 94 | bool delete_flag = false; 95 | 96 | void* data_pointer = nullptr; 97 | 98 | output() = default; 99 | 100 | output(io_type type_, void* data, int* source_execution_number_); 101 | 102 | void mark_for_deletion(); 103 | 104 | io_type get_type(){return type;}; 105 | }; 106 | 107 | class input{ 108 | private: 109 | io_type type = UNDEFINED; 110 | 111 | int default_execution_number = -256; 112 | 113 | void* default_value = nullptr; 114 | 115 | output* source_output = nullptr; 116 | 117 | 118 | 119 | public: 120 | 121 | int* source_execution_number = nullptr; 122 | 123 | void* data_pointer = nullptr; 124 | 125 | input() = default; 126 | 127 | input(io_type type_, void* default_value_); 128 | 129 | uint32_t set_default(); 130 | 131 | // set where the input should retreive its data from 132 | uint32_t set_input_data(output* source); 133 | 134 | uint32_t reconnect(); 135 | 136 | io_type get_type(){return type;}; 137 | 138 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/cycle_delay.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | 3 | #pragma once 4 | 5 | 6 | class cycle_delay: public base_node { 7 | private: 8 | void* out = nullptr; // output value 9 | input* in = nullptr; // pointer to the input 10 | 11 | static const uint16_t mem_size_bytes = 256; // memory for storing the delay data, not that max cycles will depend on the data type also 12 | uint8_t data[mem_size_bytes]; 13 | 14 | io_type type = io_type::UNDEFINED; // type of the data 15 | size_t data_size = 0; // size of the data in bytes 16 | uint16_t max_cycles = 0; // maximum cycles that can be delayed, depends on the data type and memory size 17 | 18 | // times are in execution cycles 19 | uint16_t cycles = 0; 20 | uint16_t current_in_cycle = 0; 21 | uint16_t current_out_cycle = 0; 22 | 23 | bool configured = false; 24 | 25 | public: 26 | 27 | cycle_delay(){ 28 | } 29 | 30 | unsigned int run() override { 31 | 32 | memcpy(data + current_in_cycle * data_size, in->data_pointer, data_size); // input to buffer 33 | memcpy(out, data + current_out_cycle * data_size, data_size); // buffer to output 34 | 35 | current_in_cycle++; 36 | current_out_cycle++; 37 | if(current_in_cycle > cycles){ 38 | current_in_cycle = 0; // reset the input cycle counter 39 | } 40 | if(current_out_cycle > cycles){ 41 | current_out_cycle = 0; // reset the output cycle counter 42 | } 43 | 44 | 45 | return 0; 46 | } 47 | 48 | unsigned int configure_settings(json* json) override{ 49 | 50 | /* 51 | parse settings from json 52 | { 53 | "config":{ 54 | "type": "uint8", 55 | "cycles": 10 56 | } 57 | } 58 | */ 59 | 60 | if(configured){ 61 | std::cerr << "Error: cycle_delay node already configured" << std::endl; 62 | return 1; 63 | } 64 | 65 | 66 | type = get_io_type(json->at("type").get()); 67 | cycles = json->at("cycles").get(); 68 | if(type == io_type::UNDEFINED){ 69 | std::cerr << "Error: cycle_delay node type is undefined" << std::endl; 70 | return 1; 71 | } 72 | if(cycles == 0){ 73 | std::cerr << "Error: cycle_delay node cycles is 0" << std::endl; 74 | return 1; 75 | } 76 | data_size = io_type_size.at(type); 77 | max_cycles = mem_size_bytes / data_size; 78 | if(cycles > max_cycles){ 79 | std::cerr << "Error: cycle_delay node cycles is greater than maximum cycles for type " << get_io_type_string(type) << " (memory too small)" << std::endl; 80 | return 1; 81 | } 82 | 83 | 84 | outputs.emplace("output", output(type, &out, &execution_number)); 85 | 86 | inputs.emplace("input", input(type, nullptr)); 87 | in = &inputs["input"]; 88 | 89 | if(in->data_pointer == nullptr){ 90 | std::cerr << "Error: cycle_delay node input has no default" << std::endl; 91 | return 1; 92 | } 93 | 94 | // initialize the data memory 95 | for(uint16_t i = 0; i < max_cycles; i++){ 96 | memcpy(data + i*data_size, in->data_pointer, data_size); 97 | } 98 | 99 | memcpy(out, in->data_pointer, data_size); // set the output to the current input value 100 | 101 | current_in_cycle = cycles; 102 | 103 | configured = true; 104 | 105 | return 0; 106 | } 107 | 108 | }; -------------------------------------------------------------------------------- /controller-software/core/controller/nodes/generic_nodes/inc/api_output.h: -------------------------------------------------------------------------------- 1 | #include "node_factory.h" 2 | #include 3 | 4 | /* 5 | 6 | Allows API commands to be sent to the controller to set signal values 7 | values are clamped to the min/max settings 8 | if a timeout value is set, the value will be reset to the configured default value (0 by default) after the timeout period 9 | 10 | */ 11 | 12 | #pragma once 13 | 14 | class api_output: public base_node { 15 | private: 16 | 17 | input* input_ptr = nullptr; // pointer to the input object 18 | 19 | io_type type = io_type::UNDEFINED; // input type 20 | 21 | bool configured = false; // true if the node has been configured 22 | 23 | public: 24 | 25 | api_output(){ 26 | } 27 | 28 | unsigned int run() override { 29 | // nothing to do 30 | return 0; 31 | } 32 | 33 | unsigned int configure_settings(json* json) override{ 34 | 35 | /* 36 | parse settings from json 37 | { 38 | 39 | "type": "uint8" 40 | "get": 0 // adding the get key will trigger the retrieval of the value from the input 41 | } 42 | */ 43 | 44 | 45 | 46 | if(json->find("type") != json->end()){ // config mode 47 | if(configured){ 48 | std::cerr << "Error: api_input node already configured" << std::endl; 49 | return 1; 50 | } 51 | type = get_io_type((*json)["type"].get()); 52 | if(type == io_type::UNDEFINED || type == io_type::EM_SERIAL_DEVICE){ 53 | std::cerr << "Error: invalid type for api_output node" << std::endl; 54 | return 1; 55 | } 56 | 57 | inputs.emplace("input", input(type, nullptr)); 58 | input_ptr = &inputs["input"]; // save the input pointer for later use 59 | 60 | configured = true; 61 | } 62 | 63 | if(!configured){ 64 | std::cerr << "Error: api_input node not configured" << std::endl; 65 | return 1; 66 | } 67 | 68 | if(json->find("get") != json->end()){ // get the value from the input 69 | switch(type){ 70 | case io_type::UINT8: 71 | (*json)["get"] = *(uint8_t*)input_ptr->data_pointer; 72 | break; 73 | case io_type::INT8: 74 | (*json)["get"] = *(int8_t*)input_ptr->data_pointer; 75 | break; 76 | case io_type::UINT16: 77 | (*json)["get"] = *(uint16_t*)input_ptr->data_pointer; 78 | break; 79 | case io_type::INT16: 80 | (*json)["get"] = *(int16_t*)input_ptr->data_pointer; 81 | break; 82 | case io_type::UINT32: 83 | (*json)["get"] = *(uint32_t*)input_ptr->data_pointer; 84 | break; 85 | case io_type::INT32: 86 | (*json)["get"] = *(int32_t*)input_ptr->data_pointer; 87 | break; 88 | case io_type::FLOAT: 89 | (*json)["get"] = *(float*)input_ptr->data_pointer; 90 | break; 91 | case io_type::DOUBLE: 92 | (*json)["get"] = *(double*)input_ptr->data_pointer; 93 | break; 94 | case io_type::BOOL: 95 | (*json)["get"] = *(bool*)input_ptr->data_pointer; 96 | break; 97 | default: 98 | std::cerr << "Error: invalid type for api_output node" << std::endl; 99 | } 100 | } 101 | 102 | return 0; 103 | } 104 | 105 | }; -------------------------------------------------------------------------------- /controller-software/config/electrical/parameters.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass, field, asdict 2 | import math 3 | from typing import Any 4 | 5 | def set_parents(parent): 6 | # Automatically set the parent reference in child instances 7 | for field_name in parent.__dataclass_fields__: 8 | value = getattr(parent, field_name) 9 | if hasattr(value, 'parent'): 10 | value.parent = parent 11 | 12 | @dataclass 13 | class TEXT_PARAMETER: 14 | description: str = field(default="", metadata={'desc': 'Description'}) 15 | required: bool = field(default=False, metadata={'desc': 'Required for system to startup (not able to be auto calibrated or calculated)'}) 16 | value: str = field(default=None, metadata={'desc': 'Input text'}) 17 | auto_fill_enable: bool = field(default=False, metadata={'desc': 'Locked from being auto filled'}) 18 | manual_fill_enable: bool = field(default=True, metadata={'desc': 'Locked from being manually filled'}) 19 | max_length: int = field(default=100, metadata={'desc': 'Maximum length of the text'}) 20 | auto_fill_by: list = field(default_factory=list, metadata={'desc': 'External parameters that can auto fill this parameter'}) 21 | 22 | parent: Any = field(default=None, repr=False, compare=False, metadata={'exclude': True}) 23 | 24 | 25 | @dataclass 26 | class FLOAT_PARAMETER: 27 | description: str = field(default="", metadata={'desc': 'Description'}) 28 | unit: str = field(default="", metadata={'desc': 'Unit of the parameter'}) 29 | required: bool = field(default=False, metadata={'desc': 'Required for system to startup (not able to be auto calibrated or calculated)'}) 30 | value: float = field(default=None, metadata={'desc': 'Input value'}) 31 | auto_fill_enable: bool = field(default=False, metadata={'desc': 'Locked from being auto filled'}) 32 | manual_fill_enable: bool = field(default=True, metadata={'desc': 'Locked from being manually filled'}) 33 | max_value: float = field(default=math.inf, metadata={'desc': 'Maximum value'}) 34 | min_value: float = field(default=-math.inf, metadata={'desc': 'Minimum value'}) 35 | auto_fill_by: list = field(default_factory=list, metadata={'desc': 'External parameters that can auto fill this parameter'}) 36 | 37 | parent: Any = field(default=None, repr=False, compare=False, metadata={'exclude': True}) 38 | 39 | @dataclass 40 | class INT_PARAMETER: 41 | description: str = field(default="", metadata={'desc': 'Description'}) 42 | unit: str = field(default="", metadata={'desc': 'Unit of the parameter'}) 43 | required: bool = field(default=False, metadata={'desc': 'Required for system to startup (not able to be auto calibrated or calculated)'}) 44 | value: int = field(default=None, metadata={'desc': 'Input value'}) 45 | auto_fill_enable: bool = field(default=False, metadata={'desc': 'Locked from being auto filled'}) 46 | manual_fill_enable: bool = field(default=True, metadata={'desc': 'Locked from being manually filled'}) 47 | max_value: int = field(default=math.inf, metadata={'desc': 'Maximum value'}) 48 | min_value: int = field(default=-math.inf, metadata={'desc': 'Minimum value'}) 49 | auto_fill_by: list = field(default_factory=list, metadata={'desc': 'External parameters that can auto fill this parameter'}) 50 | 51 | parent: Any = field(default=None, repr=False, compare=False, metadata={'exclude': True}) 52 | 53 | 54 | # User defined parameters that aren't device specific 55 | @dataclass 56 | class USER_PARAMS: 57 | 58 | name: TEXT_PARAMETER = field(default_factory=lambda: TEXT_PARAMETER(description="Name of the device", required=True, max_length=25)) 59 | model_number: TEXT_PARAMETER = field(default_factory=lambda: TEXT_PARAMETER(description="Model number of the device", max_length=100)) 60 | manufacturer: TEXT_PARAMETER = field(default_factory=lambda: TEXT_PARAMETER(description="Manufacturer of the device", max_length=100)) 61 | location: TEXT_PARAMETER = field(default_factory=lambda: TEXT_PARAMETER(description="Location of the device", max_length=100)) 62 | description: TEXT_PARAMETER = field(default_factory=lambda: TEXT_PARAMETER(description="Description of the device", max_length=1000)) 63 | 64 | def __post_init__(self): 65 | set_parents(self) --------------------------------------------------------------------------------