├── .gitignore ├── cring ├── .gitignore └── mag-inverse │ ├── hand_models.cpp │ ├── mag_models.cpp │ ├── utils.cpp │ ├── mag-runtime │ ├── comms.h │ ├── CMakeLists.txt │ ├── mag-runtime.vcxproj.filters │ ├── comms.cpp │ ├── mag-runtime.vcxproj │ └── runtime.cpp │ ├── CMakeLists.txt │ ├── settings.h │ ├── cmake │ └── FindCeres.cmake │ ├── file_utils.h │ ├── mag-inverse │ ├── mag-inverse.vcxproj.filters │ └── mag-inverse.vcxproj │ ├── global_model │ ├── global_model.vcxproj.filters │ └── global_model.vcxproj │ ├── utils.h │ ├── mag-inverse.sln │ ├── hand_models.h │ ├── .gitignore │ ├── mag_models.h │ └── file_utils.cpp ├── pcb ├── wristBand_rx │ ├── ring_rx_v3_sensor │ │ ├── mcu.Harness │ │ ├── coils.Harness │ │ ├── coil_processing.Harness │ │ ├── coils.SchDoc │ │ ├── ring_rx_v3_sensor.pdf │ │ ├── ring_rx_v3_sensor.xls │ │ ├── coil_processing.SchDoc │ │ ├── ring_rx_v3_sensor.PrjPcbStructure │ │ └── __Previews │ │ │ ├── coil_processing.HarnessPreview │ │ │ └── coils.HarnessPreview │ ├── ring_rx_v3_controller │ │ ├── mcu.Harness │ │ ├── coil_processing.Harness │ │ ├── coils.Harness │ │ ├── mcu.SchDoc │ │ ├── rx.SchDoc │ │ ├── coils.SchDoc │ │ ├── power.SchDoc │ │ ├── psoc.SchDoc │ │ ├── coil_processing.SchDoc │ │ ├── ring_rx_v3_controller.pdf │ │ ├── ring_rx_v3_controller.xls │ │ ├── ring_rx_v3_controller.BomDoc │ │ ├── ring_rx_v3_controller.PcbDoc │ │ └── ring_rx_v3_controller.PrjPcbStructure │ └── ring_rx_v3 │ │ ├── assmblyDrawing.pdf │ │ ├── ring_rx_v3_multi.MbaDoc │ │ ├── ring_rx_v3_multi.MbsDoc │ │ └── ring_rx_v3.PrjMbdStructure └── ring_tx │ ├── ring_v3.PrjPcbStructure │ ├── ring_v3.pdf │ ├── ring_v3.PcbDoc │ ├── ring_v3.SchDoc │ └── ring_v3.BomDoc ├── cad ├── ring │ ├── ring_20.STL │ ├── ring_22.STL │ ├── ring_inset_20.STL │ ├── ring_inset_22.STL │ ├── tracking_adapter_20.STL │ └── tracking_adapter_22.STL └── wristband │ ├── wristband.STL │ └── tracking_adapter.STL ├── firmware ├── AuraRing-Rx-top │ ├── .gitignore │ ├── SPI.h │ ├── uart.h │ ├── clock.h │ ├── timer.h │ ├── .ccsproject │ ├── timer.c │ ├── targetConfigs │ │ ├── readme.txt │ │ └── MSP430FR2422.ccxml │ ├── SPI.c │ ├── uart.c │ ├── clock.c │ ├── main.c │ └── lnk_msp430fr2422.cmd ├── AuraRing-Rx-bottom │ ├── SPI.h │ ├── uart.h │ ├── clock.h │ ├── timer.h │ ├── timer.c │ ├── .ccsproject │ ├── targetConfigs │ │ ├── readme.txt │ │ └── MSP430FR2422.ccxml │ ├── SPI.c │ ├── uart.c │ ├── clock.c │ ├── main.c │ └── lnk_msp430fr2422.cmd └── .gitignore ├── pyring ├── settings.py ├── view_raw.py ├── .gitignore └── controller_lib.py ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | .dropbox 3 | desktop.ini -------------------------------------------------------------------------------- /cring/.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | build/ 3 | position-nn-inverse/ -------------------------------------------------------------------------------- /cring/mag-inverse/hand_models.cpp: -------------------------------------------------------------------------------- 1 | #include "hand_models.h" 2 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag_models.cpp: -------------------------------------------------------------------------------- 1 | #include "mag_models.h" 2 | 3 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/mcu.Harness: -------------------------------------------------------------------------------- 1 | diff_signal=sig-,sig+ 2 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/mcu.Harness: -------------------------------------------------------------------------------- 1 | diff_signal=sig-,sig+ 2 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/coil_processing.Harness: -------------------------------------------------------------------------------- 1 | raw_coil=c-,c+ 2 | -------------------------------------------------------------------------------- /pcb/ring_tx/ring_v3.PrjPcbStructure: -------------------------------------------------------------------------------- 1 | Record=TopLevelDocument|FileName=ring_v3.SchDoc 2 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/coils.Harness: -------------------------------------------------------------------------------- 1 | diff_raw=c+,c- 2 | diff_rect=SIGP,SIGN 3 | -------------------------------------------------------------------------------- /cad/ring/ring_20.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/ring_20.STL -------------------------------------------------------------------------------- /cad/ring/ring_22.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/ring_22.STL -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/coils.Harness: -------------------------------------------------------------------------------- 1 | diff_signal=sig+,sig- 2 | raw_coil=c+,c- 3 | -------------------------------------------------------------------------------- /pcb/ring_tx/ring_v3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/ring_tx/ring_v3.pdf -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/coil_processing.Harness: -------------------------------------------------------------------------------- 1 | diff_raw=c-,c+ 2 | diff_rect=SIGP,SIGN 3 | -------------------------------------------------------------------------------- /cad/ring/ring_inset_20.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/ring_inset_20.STL -------------------------------------------------------------------------------- /cad/ring/ring_inset_22.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/ring_inset_22.STL -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/.gitignore: -------------------------------------------------------------------------------- 1 | .jxbrowser-data/ 2 | RemoteSystemsTempFiles/ 3 | /.metadata/ 4 | **/Debug -------------------------------------------------------------------------------- /pcb/ring_tx/ring_v3.PcbDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/ring_tx/ring_v3.PcbDoc -------------------------------------------------------------------------------- /pcb/ring_tx/ring_v3.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/ring_tx/ring_v3.SchDoc -------------------------------------------------------------------------------- /cad/wristband/wristband.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/wristband/wristband.STL -------------------------------------------------------------------------------- /cad/ring/tracking_adapter_20.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/tracking_adapter_20.STL -------------------------------------------------------------------------------- /cad/ring/tracking_adapter_22.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/ring/tracking_adapter_22.STL -------------------------------------------------------------------------------- /cad/wristband/tracking_adapter.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/cad/wristband/tracking_adapter.STL -------------------------------------------------------------------------------- /pyring/settings.py: -------------------------------------------------------------------------------- 1 | from local_settings import * 2 | 3 | # Port should be defined in local_settings 4 | # PORT = "COM25" 5 | 6 | BAUD = 460800 -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3/assmblyDrawing.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3/assmblyDrawing.pdf -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/mcu.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/mcu.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/rx.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/rx.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/coils.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_sensor/coils.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3/ring_rx_v3_multi.MbaDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3/ring_rx_v3_multi.MbaDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3/ring_rx_v3_multi.MbsDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3/ring_rx_v3_multi.MbsDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/coils.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/coils.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/power.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/power.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/psoc.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/psoc.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/ring_rx_v3_sensor.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_sensor/ring_rx_v3_sensor.pdf -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/ring_rx_v3_sensor.xls: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_sensor/ring_rx_v3_sensor.xls -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/coil_processing.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_sensor/coil_processing.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/coil_processing.SchDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/coil_processing.SchDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.pdf -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.xls: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.xls -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.BomDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.BomDoc -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.PcbDoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ubicomplab/AuraRing/HEAD/pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.PcbDoc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AuraRing 2 | Precise Electromagnetic Finger Tracking 3 | 4 | AuraRing is 5-DoF electromagnetic tracker that enables precise, accurate, and fine-grained finger tracking for AR, VR and wearable applications. -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/SPI.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPI.h 3 | * 4 | * Created on: Jan 15, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef SPI_H_ 9 | #define SPI_H_ 10 | 11 | void spi_init(); 12 | 13 | #endif /* SPI_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/SPI.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SPI.h 3 | * 4 | * Created on: Jan 15, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef SPI_H_ 9 | #define SPI_H_ 10 | 11 | void spi_init(); 12 | 13 | #endif /* SPI_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/uart.h: -------------------------------------------------------------------------------- 1 | /* 2 | * uart.h 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef UART_H_ 9 | #define UART_H_ 10 | 11 | void uart_init(); 12 | 13 | #endif /* UART_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/uart.h: -------------------------------------------------------------------------------- 1 | /* 2 | * uart.h 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef UART_H_ 9 | #define UART_H_ 10 | 11 | void uart_init(); 12 | 13 | #endif /* UART_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/clock.h: -------------------------------------------------------------------------------- 1 | /* 2 | * clock.h 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef CLOCK_H_ 9 | #define CLOCK_H_ 10 | 11 | void clock_init(); 12 | 13 | #endif /* CLOCK_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * timer.h 3 | * 4 | * Created on: Jan 16, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef TIMER_H_ 9 | #define TIMER_H_ 10 | 11 | void timer_init(); 12 | 13 | #endif /* TIMER_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/clock.h: -------------------------------------------------------------------------------- 1 | /* 2 | * clock.h 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef CLOCK_H_ 9 | #define CLOCK_H_ 10 | 11 | void clock_init(); 12 | 13 | #endif /* CLOCK_H_ */ 14 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * timer.h 3 | * 4 | * Created on: Jan 16, 2019 5 | * Author: farshid 6 | */ 7 | 8 | #ifndef TIMER_H_ 9 | #define TIMER_H_ 10 | 11 | void timer_init(); 12 | 13 | #endif /* TIMER_H_ */ 14 | -------------------------------------------------------------------------------- /pyring/view_raw.py: -------------------------------------------------------------------------------- 1 | import pyrealtime as prt 2 | from controller_lib import get_device_data 3 | 4 | 5 | def main(): 6 | get_device_data(show_plot=True) 7 | prt.LayerManager.session().run() 8 | 9 | 10 | if __name__ == "__main__": 11 | main() 12 | -------------------------------------------------------------------------------- /cring/mag-inverse/utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "utils.h" 6 | #include "hand_models.h" 7 | #include "settings.h" 8 | 9 | 10 | 11 | template 12 | ostream& operator<<(ostream& o, const array& arr) 13 | { 14 | copy(arr.cbegin(), arr.cend(), ostream_iterator(o, " ")); 15 | return o; 16 | } 17 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/comms.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | typedef std::array sensor_data; 6 | 7 | int init_winsock(); 8 | int init_socket(SOCKET &); 9 | int init_out_socket(SOCKET& s); 10 | int get_sensor_data_from_socket(SOCKET s, sensor_data& data); 11 | int send_solution_to_socket(SOCKET s, const double* buffer, int len_bytes); 12 | void close_socket(SOCKET); -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | include_directories(../../jointAngle/jointAngle/jointAngle) 3 | add_executable(runtime runtime.cpp comms.cpp ../file_utils.cpp ../hand_models.cpp ../mag_models.cpp ../utils.cpp ../../jointAngle/jointAngle/jointAngle/joint_ik.cpp) 4 | target_link_libraries(runtime ${CERES_LIBRARIES}) 5 | if(WIN32) 6 | target_link_libraries(runtime wsock32 ws2_32) 7 | endif() 8 | target_link_libraries(runtime gflags) -------------------------------------------------------------------------------- /cring/mag-inverse/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeList.txt : Top-level CMake project file, do global configuration 2 | # and include sub-projects here. 3 | # 4 | cmake_minimum_required (VERSION 3.8) 5 | 6 | project ("mag-inverse") 7 | set(CMAKE_MODULE_PATH ${mag-inverse_SOURCE_DIR}/cmake) 8 | 9 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 10 | 11 | find_package(Ceres CONFIG REQUIRED PATHS C:/lib/Ceres) 12 | include_directories(${CERES_INCLUDE_DIRS}) 13 | 14 | find_package(gflags REQUIRED) 15 | include_directories(.) 16 | 17 | # Include sub-projects. 18 | #add_subdirectory ("calibration") 19 | add_subdirectory ("mag-runtime") 20 | add_subdirectory ("mag-inverse") 21 | add_subdirectory ("global_model") 22 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/.ccsproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/timer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * timer.c 3 | * 4 | * Created on: Jan 16, 2019 5 | * Author: farshid 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | 12 | #define PERIOD 8 13 | 14 | void timer_init() { 15 | //Start TIMER_A 16 | Timer_A_initUpModeParam initUpParam = {0}; 17 | initUpParam.clockSource = TIMER_A_CLOCKSOURCE_SMCLK; 18 | initUpParam.clockSourceDivider = TIMER_A_CLOCKSOURCE_DIVIDER_1; 19 | initUpParam.timerPeriod = PERIOD; 20 | initUpParam.timerInterruptEnable_TAIE = TIMER_A_TAIE_INTERRUPT_DISABLE; 21 | initUpParam.timerClear = TIMER_A_DO_CLEAR; 22 | initUpParam.startTimer = false; 23 | Timer_A_initUpMode(TIMER_A0_BASE, &initUpParam); 24 | } 25 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/timer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * timer.c 3 | * 4 | * Created on: Jan 16, 2019 5 | * Author: farshid 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | 12 | #define PERIOD 8 13 | 14 | void timer_init() { 15 | //Start TIMER_A 16 | Timer_A_initUpModeParam initUpParam = {0}; 17 | initUpParam.clockSource = TIMER_A_CLOCKSOURCE_SMCLK; 18 | initUpParam.clockSourceDivider = TIMER_A_CLOCKSOURCE_DIVIDER_1; 19 | initUpParam.timerPeriod = PERIOD; 20 | initUpParam.timerInterruptEnable_TAIE = TIMER_A_TAIE_INTERRUPT_DISABLE; 21 | initUpParam.timerClear = TIMER_A_DO_CLEAR; 22 | initUpParam.startTimer = false; 23 | Timer_A_initUpMode(TIMER_A0_BASE, &initUpParam); 24 | } 25 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/.ccsproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/targetConfigs/readme.txt: -------------------------------------------------------------------------------- 1 | The 'targetConfigs' folder contains target-configuration (.ccxml) files, automatically generated based 2 | on the device and connection settings specified in your project on the Properties > General page. 3 | 4 | Please note that in automatic target-configuration management, changes to the project's device and/or 5 | connection settings will either modify an existing or generate a new target-configuration file. Thus, 6 | if you manually edit these auto-generated files, you may need to re-apply your changes. Alternatively, 7 | you may create your own target-configuration file for this project and manage it manually. You can 8 | always switch back to automatic target-configuration management by checking the "Manage the project's 9 | target-configuration automatically" checkbox on the project's Properties > General page. -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/targetConfigs/readme.txt: -------------------------------------------------------------------------------- 1 | The 'targetConfigs' folder contains target-configuration (.ccxml) files, automatically generated based 2 | on the device and connection settings specified in your project on the Properties > General page. 3 | 4 | Please note that in automatic target-configuration management, changes to the project's device and/or 5 | connection settings will either modify an existing or generate a new target-configuration file. Thus, 6 | if you manually edit these auto-generated files, you may need to re-apply your changes. Alternatively, 7 | you may create your own target-configuration file for this project and manage it manually. You can 8 | always switch back to automatic target-configuration management by checking the "Manage the project's 9 | target-configuration automatically" checkbox on the project's Properties > General page. -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/ring_rx_v3_sensor.PrjPcbStructure: -------------------------------------------------------------------------------- 1 | Record=TopLevelDocument|FileName=coil_processing.SchDoc 2 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c1|SchDesignator=c1|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 3 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c2|SchDesignator=c2|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 4 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c3|SchDesignator=c3|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 5 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/targetConfigs/MSP430FR2422.ccxml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/targetConfigs/MSP430FR2422.ccxml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /cring/mag-inverse/settings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | #define USE_JOINT 0 8 | #define APPLY_SIGN_CORRECTION 0 9 | #define HALLUCINATE_SIGN 0 10 | //#define USE_MARKERS 0 11 | #define NUM_RX 3 12 | #define USE_POS 1 13 | //#define USE_PALM 0 14 | 15 | 16 | static const std::string get_home_dir() { 17 | char* auraring_dir; 18 | size_t auraring_dir_len; 19 | errno_t auraring_dir_err = _dupenv_s(&auraring_dir, &auraring_dir_len, "AURARING_DATA"); 20 | if (auraring_dir_err) { 21 | throw std::runtime_error("Could not find AURARING_DATA environment variable"); 22 | } 23 | return std::string(auraring_dir); 24 | } 25 | const string HOME_DIR = get_home_dir(); 26 | 27 | const string CERES_DIRECTORY = HOME_DIR + R"(/ceres/)"; 28 | const string CERES_PRED_DIRECTORY = HOME_DIR + R"(/ceres/)"; 29 | const string PROCESSED_DRECTORY = HOME_DIR + R"(/processed/)"; 30 | const string MATLAB_DRECTORY = HOME_DIR + R"(/MATLAB/)"; -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 ubicomplab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/SPI.c: -------------------------------------------------------------------------------- 1 | /* 2 | * SPI.c 3 | * 4 | * Created on: Jan 15, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | 11 | void spi_init() { 12 | //Initialize slave to MSB first, inactive high clock polarity and 3 wire SPI 13 | EUSCI_B_SPI_initMasterParam param = {0}; 14 | param.selectClockSource = EUSCI_B_SPI_CLOCKSOURCE_SMCLK; 15 | param.clockSourceFrequency = CS_getSMCLK(); 16 | param.desiredSpiClock = 1000000; //4MHz 17 | param.msbFirst = EUSCI_B_SPI_MSB_FIRST; 18 | param.clockPhase = EUSCI_B_SPI_PHASE_DATA_CAPTURED_ONFIRST_CHANGED_ON_NEXT; 19 | param.clockPolarity = EUSCI_B_SPI_CLOCKPOLARITY_INACTIVITY_LOW; 20 | param.spiMode = EUSCI_B_SPI_3PIN; 21 | EUSCI_B_SPI_initMaster(EUSCI_B0_BASE, ¶m); 22 | 23 | 24 | // //Enable SPI Module 25 | // UCB0BR0 = 0x01; // /2,fBitClock = fBRCLK/(UCBRx+1). 26 | EUSCI_B_SPI_enable(EUSCI_B0_BASE); 27 | // //Enable Receive interrupt 28 | EUSCI_B_SPI_enableInterrupt(EUSCI_B0_BASE, EUSCI_B_SPI_RECEIVE_INTERRUPT | EUSCI_B_SPI_TRANSMIT_INTERRUPT); 29 | 30 | 31 | } 32 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/SPI.c: -------------------------------------------------------------------------------- 1 | /* 2 | * SPI.c 3 | * 4 | * Created on: Jan 15, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | 11 | void spi_init() { 12 | //Initialize slave to MSB first, inactive high clock polarity and 3 wire SPI 13 | EUSCI_B_SPI_initMasterParam param = {0}; 14 | param.selectClockSource = EUSCI_B_SPI_CLOCKSOURCE_SMCLK; 15 | param.clockSourceFrequency = CS_getSMCLK(); 16 | param.desiredSpiClock = 1000000; //4MHz 17 | param.msbFirst = EUSCI_B_SPI_MSB_FIRST; 18 | param.clockPhase = EUSCI_B_SPI_PHASE_DATA_CAPTURED_ONFIRST_CHANGED_ON_NEXT; 19 | param.clockPolarity = EUSCI_B_SPI_CLOCKPOLARITY_INACTIVITY_LOW; 20 | param.spiMode = EUSCI_B_SPI_3PIN; 21 | EUSCI_B_SPI_initMaster(EUSCI_B0_BASE, ¶m); 22 | 23 | 24 | // //Enable SPI Module 25 | // UCB0BR0 = 0x01; // /2,fBitClock = fBRCLK/(UCBRx+1). 26 | EUSCI_B_SPI_enable(EUSCI_B0_BASE); 27 | // //Enable Receive interrupt 28 | EUSCI_B_SPI_enableInterrupt(EUSCI_B0_BASE, EUSCI_B_SPI_RECEIVE_INTERRUPT | EUSCI_B_SPI_TRANSMIT_INTERRUPT); 29 | 30 | 31 | } 32 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3/ring_rx_v3.PrjMbdStructure: -------------------------------------------------------------------------------- 1 | Record=DiagramHierarchy|SourceDocument=ring_rx_vs_multi.MbsDoc|Kind=Multi-board Schematic Module|Designator=1|Source=..\ring_rx_v3_controller\ring_rx_v3_controller.PrjPcb 2 | Record=SubProject|SourceDocument=ring_rx_vs_multi.MbsDoc|ProjectPath=..\ring_rx_v3_controller\ring_rx_v3_controller.PrjPcb 3 | Record=DiagramHierarchy|SourceDocument=ring_rx_vs_multi.MbsDoc|Kind=Multi-board Schematic Module|Designator=2|Source=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 4 | Record=SubProject|SourceDocument=ring_rx_vs_multi.MbsDoc|ProjectPath=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 5 | Record=DiagramHierarchy|SourceDocument=ring_rx_v3_multi.MbsDoc|Kind=Multi-board Schematic Module|Designator=1|Source=..\ring_rx_v3_controller\ring_rx_v3_controller.PrjPcb 6 | Record=SubProject|SourceDocument=ring_rx_v3_multi.MbsDoc|ProjectPath=..\ring_rx_v3_controller\ring_rx_v3_controller.PrjPcb 7 | Record=DiagramHierarchy|SourceDocument=ring_rx_v3_multi.MbsDoc|Kind=Multi-board Schematic Module|Designator=3|Source=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 8 | Record=SubProject|SourceDocument=ring_rx_v3_multi.MbsDoc|ProjectPath=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 9 | Record=DiagramHierarchy|SourceDocument=ring_rx_v3_multi.MbsDoc|Kind=Multi-board Schematic Module|Designator=2|Source=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 10 | Record=SubProject|SourceDocument=ring_rx_v3_multi.MbsDoc|ProjectPath=..\ring_rx_v3_sensor\ring_rx_v3_sensor.PrjPcb 11 | -------------------------------------------------------------------------------- /cring/mag-inverse/cmake/FindCeres.cmake: -------------------------------------------------------------------------------- 1 | # - Find Ceres library 2 | # Find the native Ceres includes and library 3 | # This module defines 4 | # CERES_INCLUDE_DIRS, where to find ceres.h, Set when 5 | # CERES_INCLUDE_DIR is found. 6 | # CERES_LIBRARIES, libraries to link against to use Ceres. 7 | # CERES_ROOT_DIR, The base directory to search for Ceres. 8 | # This can also be an environment variable. 9 | # CERES_FOUND, If false, do not try to use Ceres. 10 | # 11 | # also defined, but not for general use are 12 | # CERES_LIBRARY, where to find the Ceres library. 13 | 14 | # If CERES_ROOT_DIR was defined in the environment, use it. 15 | IF(NOT CERES_ROOT_DIR AND NOT $ENV{CERES_ROOT_DIR} STREQUAL "") 16 | SET(CERES_ROOT_DIR $ENV{CERES_ROOT_DIR}) 17 | ENDIF() 18 | 19 | SET(_ceres_SEARCH_DIRS 20 | ${CERES_ROOT_DIR} 21 | C:/lib/Ceres 22 | ) 23 | 24 | FIND_PATH(CERES_INCLUDE_DIR 25 | NAMES 26 | ceres/ceres.h 27 | HINTS 28 | ${_ceres_SEARCH_DIRS} 29 | PATH_SUFFIXES 30 | include 31 | ) 32 | 33 | FIND_LIBRARY(CERES_LIBRARY 34 | NAMES 35 | ceres 36 | HINTS 37 | ${_ceres_SEARCH_DIRS} 38 | PATH_SUFFIXES 39 | lib64 lib 40 | ) 41 | 42 | # handle the QUIETLY and REQUIRED arguments and set CERES_FOUND to TRUE if 43 | # all listed variables are TRUE 44 | INCLUDE(FindPackageHandleStandardArgs) 45 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(ceres DEFAULT_MSG 46 | CERES_LIBRARY CERES_INCLUDE_DIR) 47 | 48 | IF(CERES_FOUND) 49 | SET(CERES_LIBRARIES ${CERES_LIBRARY}) 50 | SET(CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR}) 51 | ENDIF(CERES_FOUND) 52 | 53 | MARK_AS_ADVANCED( 54 | CERES_INCLUDE_DIR 55 | CERES_LIBRARY 56 | ) -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/mag-runtime.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | Source Files 23 | 24 | 25 | Source Files 26 | 27 | 28 | Source Files 29 | 30 | 31 | 32 | 33 | Header Files 34 | 35 | 36 | Header Files 37 | 38 | 39 | Header Files 40 | 41 | 42 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_controller/ring_rx_v3_controller.PrjPcbStructure: -------------------------------------------------------------------------------- 1 | Record=TopLevelDocument|FileName=rx.SchDoc 2 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c1|SchDesignator=c1|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 3 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c2|SchDesignator=c2|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 4 | Record=SheetSymbol|SourceDocument=coil_processing.SchDoc|Designator=c3|SchDesignator=c3|FileName=coils.SchDoc|SymbolType=Normal|RawFileName=coils.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 5 | Record=SheetSymbol|SourceDocument=rx.SchDoc|Designator=U_coil_processing|SchDesignator=U_coil_processing|FileName=coil_processing.SchDoc|SymbolType=Normal|RawFileName=coil_processing.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 6 | Record=SheetSymbol|SourceDocument=rx.SchDoc|Designator=U_coil_processing|SchDesignator=U_coil_processing|FileName=coil_processing.SchDoc|SymbolType=Normal|RawFileName=coil_processing.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 7 | Record=SheetSymbol|SourceDocument=rx.SchDoc|Designator=U_mcu|SchDesignator=U_mcu|FileName=mcu.SchDoc|SymbolType=Normal|RawFileName=mcu.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 8 | Record=SheetSymbol|SourceDocument=rx.SchDoc|Designator=U_power|SchDesignator=U_power|FileName=power.SchDoc|SymbolType=Normal|RawFileName=power.SchDoc|DesignItemId= |SourceLibraryName= |ObjectKind=Sheet Symbol|RevisionGUID= |ItemGUID= |VaultGUID= 9 | -------------------------------------------------------------------------------- /cring/mag-inverse/file_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "settings.h" 3 | #include 4 | #include "hand_models.h" 5 | #include "mag_models.h" 6 | 7 | struct DataFrame { 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | array sensors; 10 | Eigen::Vector3d ring_pos; 11 | Eigen::Quaterniond ring_q; 12 | array joint_angles; 13 | #ifdef USE_PALM 14 | Eigen::Vector3 palm_pos; 15 | Eigen::Quaterniond palm_q; 16 | #endif 17 | //#ifdef USE_MARKERS 18 | // vector markers; 19 | //#endif 20 | }; 21 | 22 | struct per_frame_opt_parameters { 23 | #if USE_JOINT 24 | array joint_angles; 25 | Eigen::Vector3d slack_pos; 26 | Eigen::Quaterniond slack_rot; 27 | HandModel hand_model; 28 | #else 29 | Eigen::Vector3d ring_pos; 30 | Eigen::Quaterniond ring_rot; 31 | #endif 32 | }; 33 | 34 | 35 | void writePerFrameResults(string fileName, string trial, const Eigen::Vector3d& ring_pos, const Eigen::Quaterniond& ring_rot, const Eigen::Vector3d& ring_pos_actual, const Eigen::Quaterniond& ring_rot_actual, const per_frame_opt_parameters& hand_model, 36 | bool newFile); 37 | 38 | void writeCalibration(string fileName, string trial, const CoilCalibrationModel& coil, bool newFile); 39 | 40 | vector parseCSVFile(string fileName, string trial, char delimiter, unsigned int limit=0); 41 | vector parseProjectedCSVFile(string fileName, string trial, char delimiter, unsigned int limit = 0); 42 | 43 | CalibrationModel parseCalibratedFile(string fileName, string trial, char delimiter); 44 | 45 | void writeHandModel(string fileName, string trial, const HandModel& hand_model); 46 | void writeCSVFile(string fileName, string trial, vector> angles, vector> joints, vector raw_data); 47 | 48 | HandModel parseHandModel(string fileName, string trial); 49 | vector> parseMatlab(string fileName, string trial); -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/uart.c: -------------------------------------------------------------------------------- 1 | /* 2 | * uart.c 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | void uart_init() { 11 | 12 | 13 | 14 | // Configuration for 115200 UART with SMCLK at 16384000 15 | // These values were generated using the online tool available at: 16 | // http://software-dl.ti.com/msp430/msp430 public sw/mcu/msp430/MSP430BaudRateConverter/index.html 17 | // EUSCI_A_UART_initParam uartConfig = { 18 | // EUSCI_A_UART_CLOCKSOURCE_SMCLK, // SMCLK Clock Source 19 | // 1, // BRDIV = 8 clockPrescalar 20 | // 0, // UCxBRF = 14 firstModReg 21 | // 0xF7, // UCxBRS = 34=0x22 secondModReg 22 | // EUSCI_A_UART_NO_PARITY, // No Parity 23 | // EUSCI_A_UART_LSB_FIRST, // MSB First 24 | // EUSCI_A_UART_ONE_STOP_BIT, // One stop bit 25 | // EUSCI_A_UART_MODE, // UART mode 26 | // EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION // Oversampling Baudrate 27 | // }; 28 | 29 | // Configure UART 30 | UCA0CTLW0 |= UCSWRST; 31 | UCA0CTLW0 |= UCSSEL__SMCLK; 32 | 33 | // Baud Rate calculation : 460800 34 | // 16000000/(16*460800) = 2.17013 35 | // Fractional portion = 0.083 36 | // User's Guide Table 22-5: UCBRSx = 0x49 37 | // UCBRFx = int ( (2.17013-2)*16) = 2 38 | UCA0BR0 = 2; // 16000000/16/460800 39 | UCA0BR1 = 0x00; 40 | UCA0MCTLW = 0xBB00 | UCOS16 | UCBRF_2; 41 | 42 | // Settings P2.0 and P2.1 as UART pins. 43 | GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P2, 44 | GPIO_PIN0 | GPIO_PIN1, 45 | GPIO_PRIMARY_MODULE_FUNCTION); 46 | 47 | UCA0CTLW0 &= ~UCSWRST; // Initialize eUSCI 48 | // UCA0IE |= UCRXIE; // Enable USCI_A0 RX interrupt 49 | 50 | 51 | // // Configure and enable the UART peripheral 52 | // EUSCI_A_UART_init(EUSCI_A0_BASE, &uartConfig); 53 | // EUSCI_A_UART_enable(EUSCI_A0_BASE); 54 | } 55 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/uart.c: -------------------------------------------------------------------------------- 1 | /* 2 | * uart.c 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | void uart_init() { 11 | 12 | 13 | 14 | // Configuration for 115200 UART with SMCLK at 16384000 15 | // These values were generated using the online tool available at: 16 | // http://software-dl.ti.com/msp430/msp430 public sw/mcu/msp430/MSP430BaudRateConverter/index.html 17 | // EUSCI_A_UART_initParam uartConfig = { 18 | // EUSCI_A_UART_CLOCKSOURCE_SMCLK, // SMCLK Clock Source 19 | // 1, // BRDIV = 8 clockPrescalar 20 | // 0, // UCxBRF = 14 firstModReg 21 | // 0xF7, // UCxBRS = 34=0x22 secondModReg 22 | // EUSCI_A_UART_NO_PARITY, // No Parity 23 | // EUSCI_A_UART_LSB_FIRST, // MSB First 24 | // EUSCI_A_UART_ONE_STOP_BIT, // One stop bit 25 | // EUSCI_A_UART_MODE, // UART mode 26 | // EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION // Oversampling Baudrate 27 | // }; 28 | 29 | // Configure UART 30 | UCA0CTLW0 |= UCSWRST; 31 | UCA0CTLW0 |= UCSSEL__SMCLK; 32 | 33 | // Baud Rate calculation : 460800 34 | // 16000000/(16*460800) = 2.17013 35 | // Fractional portion = 0.083 36 | // User's Guide Table 22-5: UCBRSx = 0x49 37 | // UCBRFx = int ( (2.17013-2)*16) = 2 38 | UCA0BR0 = 2; // 16000000/16/460800 39 | UCA0BR1 = 0x00; 40 | UCA0MCTLW = 0xBB00 | UCOS16 | UCBRF_2; 41 | 42 | // Settings P2.0 and P2.1 as UART pins. 43 | GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P2, 44 | GPIO_PIN0 | GPIO_PIN1, 45 | GPIO_PRIMARY_MODULE_FUNCTION); 46 | 47 | UCA0CTLW0 &= ~UCSWRST; // Initialize eUSCI 48 | // UCA0IE |= UCRXIE; // Enable USCI_A0 RX interrupt 49 | 50 | 51 | // // Configure and enable the UART peripheral 52 | // EUSCI_A_UART_init(EUSCI_A0_BASE, &uartConfig); 53 | // EUSCI_A_UART_enable(EUSCI_A0_BASE); 54 | } 55 | -------------------------------------------------------------------------------- /pyring/.gitignore: -------------------------------------------------------------------------------- 1 | *.txt 2 | 3 | .idea/ 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | pip-wheel-metadata/ 27 | share/python-wheels/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | MANIFEST 32 | 33 | # PyInstaller 34 | # Usually these files are written by a python script from a template 35 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 36 | *.manifest 37 | *.spec 38 | 39 | # Installer logs 40 | pip-log.txt 41 | pip-delete-this-directory.txt 42 | 43 | # Unit test / coverage reports 44 | htmlcov/ 45 | .tox/ 46 | .nox/ 47 | .coverage 48 | .coverage.* 49 | .cache 50 | nosetests.xml 51 | coverage.xml 52 | *.cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | 65 | # Flask stuff: 66 | instance/ 67 | .webassets-cache 68 | 69 | # Scrapy stuff: 70 | .scrapy 71 | 72 | # Sphinx documentation 73 | docs/_build/ 74 | 75 | # PyBuilder 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | .python-version 87 | 88 | # celery beat schedule file 89 | celerybeat-schedule 90 | 91 | # SageMath parsed files 92 | *.sage.py 93 | 94 | # Environments 95 | .env 96 | .venv 97 | env/ 98 | venv/ 99 | ENV/ 100 | env.bak/ 101 | venv.bak/ 102 | 103 | # Spyder project settings 104 | .spyderproject 105 | .spyproject 106 | 107 | # Rope project settings 108 | .ropeproject 109 | 110 | # mkdocs documentation 111 | /site 112 | 113 | # mypy 114 | .mypy_cache/ 115 | .dmypy.json 116 | dmypy.json 117 | 118 | # Pyre type checker 119 | .pyre/ -------------------------------------------------------------------------------- /cring/mag-inverse/mag-inverse/mag-inverse.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Header Files 20 | 21 | 22 | Header Files 23 | 24 | 25 | Header Files 26 | 27 | 28 | Header Files 29 | 30 | 31 | Header Files 32 | 33 | 34 | 35 | 36 | Source Files 37 | 38 | 39 | Source Files 40 | 41 | 42 | Source Files 43 | 44 | 45 | Source Files 46 | 47 | 48 | Source Files 49 | 50 | 51 | -------------------------------------------------------------------------------- /cring/mag-inverse/global_model/global_model.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | Source Files 23 | 24 | 25 | Source Files 26 | 27 | 28 | Source Files 29 | 30 | 31 | Source Files 32 | 33 | 34 | 35 | 36 | Header Files 37 | 38 | 39 | Header Files 40 | 41 | 42 | Header Files 43 | 44 | 45 | Header Files 46 | 47 | 48 | Header Files 49 | 50 | 51 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/comms.cpp: -------------------------------------------------------------------------------- 1 | #include "comms.h" 2 | #include 3 | #include 4 | #pragma comment(lib,"ws2_32.lib") //Winsock Library 5 | 6 | #define BUFLEN 512 //Max length of buffer 7 | #define PORT 8888 //The port on which to listen for incoming data 8 | #define PORT_OUT 9884 //The port on which to listen for incoming data 9 | 10 | 11 | int init_winsock() { 12 | WSADATA wsa; 13 | 14 | 15 | //Initialise winsock 16 | printf("\nInitialising Winsock..."); 17 | if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) 18 | { 19 | printf("Failed. Error Code: %d", WSAGetLastError()); 20 | return -1; 21 | } 22 | printf("Initialised.\n"); 23 | return 0; 24 | } 25 | 26 | int init_socket(SOCKET& s) 27 | { 28 | struct sockaddr_in server; 29 | 30 | 31 | //Create a socket 32 | if ((s = socket(AF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) 33 | { 34 | printf("Could not create socket: %d", WSAGetLastError()); 35 | return -1; 36 | } 37 | printf("Socket created.\n"); 38 | 39 | //Prepare the sockaddr_in structure 40 | server.sin_family = AF_INET; 41 | server.sin_addr.s_addr = INADDR_ANY; 42 | server.sin_port = htons(PORT); 43 | 44 | //Bind 45 | if (::bind(s, (struct sockaddr *)& server, sizeof(server)) == SOCKET_ERROR) 46 | { 47 | printf("Bind failed with error code : %d\n", WSAGetLastError()); 48 | return -1; 49 | } 50 | puts("Bind done"); 51 | 52 | return 0; 53 | } 54 | 55 | int init_out_socket(SOCKET& s) 56 | { 57 | 58 | 59 | //Create a socket 60 | if ((s = socket(AF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) 61 | { 62 | printf("Could not create socket: %d", WSAGetLastError()); 63 | return -1; 64 | } 65 | printf("Socket created.\n"); 66 | 67 | return 0; 68 | } 69 | 70 | int receive_from_socket(SOCKET s, char* buf) 71 | { 72 | int slen, recv_len; 73 | struct sockaddr_in si_other; 74 | slen = sizeof(si_other); 75 | 76 | //printf("Waiting for data...\n"); 77 | fflush(stdout); 78 | 79 | //clear the buffer by filling null, it might have previously received data 80 | memset(buf, 0, BUFLEN); 81 | 82 | //try to receive some data, this is a blocking call 83 | if ((recv_len = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *) & si_other, &slen)) == SOCKET_ERROR) 84 | { 85 | printf(" recvfrom() failed with error code : %d\n", WSAGetLastError()); 86 | return -1; 87 | } 88 | 89 | //print details of the client/peer and the data received 90 | //char ipbuf[INET_ADDRSTRLEN]; 91 | //inet_ntop(AF_INET, &si_other.sin_addr, ipbuf, sizeof(ipbuf)); 92 | //printf(" Received packet from %s:%d\n", ipbuf, ntohs(si_other.sin_port)); 93 | //printf(" Data: %s\n", buf); 94 | 95 | 96 | return recv_len; 97 | } 98 | 99 | int get_sensor_data_from_socket(SOCKET s, sensor_data& data) 100 | { 101 | char buf[BUFLEN]; 102 | int size = receive_from_socket(s, buf); 103 | if (size < 0) { 104 | return -1; 105 | } 106 | 107 | for (int i = 0; i < 9; i++) { 108 | data[i] = *((float*)(buf) + i); 109 | } 110 | 111 | return 0; 112 | } 113 | 114 | int send_solution_to_socket(SOCKET s, const double* buffer, int len_bytes) { 115 | 116 | 117 | struct sockaddr_in si_other; 118 | si_other.sin_family = AF_INET; 119 | si_other.sin_port = htons(PORT_OUT); 120 | inet_pton(AF_INET, "127.0.0.1", &si_other.sin_addr.S_un.S_addr); 121 | 122 | if (sendto(s, (char*)buffer, len_bytes, 0, (struct sockaddr*)& si_other, sizeof(si_other)) == SOCKET_ERROR) 123 | { 124 | printf("sendto() failed with error code : %d\n", WSAGetLastError()); 125 | return -1; 126 | } 127 | return 0; 128 | } 129 | 130 | void close_socket(SOCKET s) 131 | { 132 | closesocket(s); 133 | WSACleanup(); 134 | 135 | return; 136 | } -------------------------------------------------------------------------------- /cring/mag-inverse/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "settings.h" 3 | #include 4 | #include 5 | #include "ceres/ceres.h" 6 | #include "Eigen/Core" 7 | #include "Eigen/Geometry" 8 | #include "hand_models.h" 9 | 10 | using namespace std; 11 | 12 | #define PI 3.1415926535897932384626433832795028841971693993751058209749 13 | 14 | template T sgn(const T& val) { 15 | return T((T(0) < val) - (val < T(0))); 16 | } 17 | 18 | template inline double D(const T& x); 19 | template <> inline double D(const double& x) { 20 | return x; 21 | } 22 | template inline double D(const ceres::Jet& x) { 23 | return x.a; 24 | } 25 | 26 | template 27 | inline void print_vec(Eigen::Vector3 vec) { 28 | printf("%f, %f, %f", D(vec[0]), D(vec[1]), D(vec[2])); 29 | } 30 | template 31 | inline void print_quat(Eigen::Quaternion q) { 32 | printf("%f, %f, %f, %f", D(q.w()), D(q.x()), D(q.y()), D(q.z())); 33 | } 34 | 35 | template inline void print_float(T x); 36 | template <> inline void print_float(double x) { 37 | printf("%f\n", x); 38 | } 39 | template inline void print_float(ceres::Jet x) { 40 | printf("%f\n", x.a); 41 | } 42 | 43 | 44 | template inline void print_jacobian(T x); 45 | 46 | template inline void print_jacobian(T x); 47 | template <> inline void print_jacobian(double x) { 48 | printf("%f", x); 49 | } 50 | 51 | template inline void print_jacobian(ceres::Jet x) { 52 | printf("%f - %f %f %f %f\n", x.a, x.v[0], x.v[1], x.v[2], x.v[3]); 53 | } 54 | 55 | 56 | template inline void print_hand_model(T const* const wrist_offset, T const* const wrist_offset_rot, T const* const); 57 | 58 | template <> inline void print_hand_model(double const* const wrist_offset, double const* const wrist_offset_rot, double const* const hand_model) 59 | { 60 | printf("wrist_offset: (%f, %f, %f)\t rx_rot: (%f, %f, %f, %f)\t, wrist_len: %f, finger_len: %f\n", wrist_offset[0], wrist_offset[1], wrist_offset[2], wrist_offset_rot[0], wrist_offset_rot[1], wrist_offset_rot[2], wrist_offset_rot[3], hand_model[0], hand_model[1]); 61 | } 62 | 63 | template inline void print_hand_model(ceres::Jet const* const wrist_offset, ceres::Jet const* const wrist_offset_rot, ceres::Jet const* const hand_model) 64 | { 65 | printf("wrist_offset: (%f, %f, %f)\t rx_rot: (%f, %f, %f, %f)\t, wrist_len: %f, finger_len: %f\n", wrist_offset[0].a, wrist_offset[1].a, wrist_offset[2].a, wrist_offset_rot[0].a, wrist_offset_rot[1].a, wrist_offset_rot[2].a, wrist_offset_rot[3].a, hand_model[0].a, hand_model[1].a); 66 | } 67 | 68 | inline void print_hand_model(const HandModel &hand_model) 69 | { 70 | printf("wrist_offset: (%f, %f, %f)\nrx_rot: (%f, %f, %f, %f)\npalm_offset: (%f, %f, %f)\npalm_rot: (%f, %f, %f, %f)\nwrist_len: %f, finger_len: %f, cmc_alpha: %f\n", 71 | hand_model.wrist_offset[0], hand_model.wrist_offset[1], hand_model.wrist_offset[2], 72 | hand_model.wrist_offset_rot.w(), hand_model.wrist_offset_rot.x(), hand_model.wrist_offset_rot.y(), hand_model.wrist_offset_rot.z(), 73 | hand_model.palm_offset[0], hand_model.palm_offset[1], hand_model.palm_offset[2], 74 | hand_model.palm_offset_rot.w(), hand_model.palm_offset_rot.x(), hand_model.palm_offset_rot.y(), hand_model.palm_offset_rot.z(), 75 | hand_model.bone_lengths[0], hand_model.bone_lengths[1], hand_model.cmc_alpha); 76 | 77 | printf("base_slack_pos: (%f, %f, %f)\nbase_slack_rot: (%f, %f, %f, %f)\n", 78 | hand_model.base_slack_pos[0], hand_model.base_slack_pos[1], hand_model.base_slack_pos[2], 79 | hand_model.base_slack_rot.w(), hand_model.base_slack_rot.x(), hand_model.base_slack_rot.y(), hand_model.base_slack_rot.z()); 80 | } -------------------------------------------------------------------------------- /cring/mag-inverse/mag-inverse.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 15 4 | VisualStudioVersion = 15.0.28307.438 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "mag-inverse", "mag-inverse\mag-inverse.vcxproj", "{99E09A45-DEF2-4F8D-8763-D0574AF016CF}" 7 | EndProject 8 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "global_model", "global_model\global_model.vcxproj", "{0346D26E-3AA3-4176-B1B5-9395EDDC31BE}" 9 | EndProject 10 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "recalibrate", "recalibrate\recalibrate.vcxproj", "{5A585487-5AAF-4C66-B29B-0FF60A1BC408}" 11 | EndProject 12 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "mag-runtime", "mag-runtime\mag-runtime.vcxproj", "{486DF039-C998-4055-9CDA-C9FD2E478166}" 13 | EndProject 14 | Global 15 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 16 | Debug|x64 = Debug|x64 17 | Debug|x86 = Debug|x86 18 | Release|x64 = Release|x64 19 | Release|x86 = Release|x86 20 | EndGlobalSection 21 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 22 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Debug|x64.ActiveCfg = Debug|x64 23 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Debug|x64.Build.0 = Debug|x64 24 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Debug|x86.ActiveCfg = Debug|Win32 25 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Debug|x86.Build.0 = Debug|Win32 26 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Release|x64.ActiveCfg = Release|x64 27 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Release|x64.Build.0 = Release|x64 28 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Release|x86.ActiveCfg = Release|Win32 29 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF}.Release|x86.Build.0 = Release|Win32 30 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Debug|x64.ActiveCfg = Debug|x64 31 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Debug|x64.Build.0 = Debug|x64 32 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Debug|x86.ActiveCfg = Debug|Win32 33 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Debug|x86.Build.0 = Debug|Win32 34 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Release|x64.ActiveCfg = Release|x64 35 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Release|x64.Build.0 = Release|x64 36 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Release|x86.ActiveCfg = Release|Win32 37 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE}.Release|x86.Build.0 = Release|Win32 38 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Debug|x64.ActiveCfg = Debug|x64 39 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Debug|x64.Build.0 = Debug|x64 40 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Debug|x86.ActiveCfg = Debug|Win32 41 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Debug|x86.Build.0 = Debug|Win32 42 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Release|x64.ActiveCfg = Release|x64 43 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Release|x64.Build.0 = Release|x64 44 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Release|x86.ActiveCfg = Release|Win32 45 | {5A585487-5AAF-4C66-B29B-0FF60A1BC408}.Release|x86.Build.0 = Release|Win32 46 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Debug|x64.ActiveCfg = Debug|x64 47 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Debug|x64.Build.0 = Debug|x64 48 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Debug|x86.ActiveCfg = Debug|Win32 49 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Debug|x86.Build.0 = Debug|Win32 50 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Release|x64.ActiveCfg = Release|x64 51 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Release|x64.Build.0 = Release|x64 52 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Release|x86.ActiveCfg = Release|Win32 53 | {486DF039-C998-4055-9CDA-C9FD2E478166}.Release|x86.Build.0 = Release|Win32 54 | EndGlobalSection 55 | GlobalSection(SolutionProperties) = preSolution 56 | HideSolutionNode = FALSE 57 | EndGlobalSection 58 | GlobalSection(ExtensibilityGlobals) = postSolution 59 | SolutionGuid = {7DCBD184-8F66-497B-B334-13CB3D8B6684} 60 | EndGlobalSection 61 | EndGlobal 62 | -------------------------------------------------------------------------------- /firmware/.gitignore: -------------------------------------------------------------------------------- 1 | .jxbrowser-data/ 2 | RemoteSystemsTempFiles/ 3 | /.metadata/ 4 | **/Debug 5 | 6 | ######### 7 | ## OE App 8 | ######## 9 | *.r 10 | 11 | 12 | ################# 13 | ## Eclipse 14 | ################# 15 | 16 | *.pydevproject 17 | .project 18 | .metadata 19 | bin/ 20 | tmp/ 21 | *.tmp 22 | *.bak 23 | *.swp 24 | *~.nib 25 | local.properties 26 | .classpath 27 | .settings/ 28 | .loadpath 29 | 30 | # External tool builders 31 | .externalToolBuilders/ 32 | 33 | # Locally stored "Eclipse launch configurations" 34 | *.launch 35 | 36 | # CDT-specific 37 | .cproject 38 | 39 | # PDT-specific 40 | .buildpath 41 | 42 | 43 | ################# 44 | ## Visual Studio 45 | ################# 46 | 47 | ## Ignore Visual Studio temporary files, build results, and 48 | ## files generated by popular Visual Studio add-ons. 49 | 50 | # User-specific files 51 | *.suo 52 | *.user 53 | *.sln.docstates 54 | 55 | # Build results 56 | 57 | [Dd]ebug/ 58 | [Rr]elease/ 59 | x64/ 60 | build/ 61 | [Bb]in/ 62 | [Oo]bj/ 63 | 64 | # MSTest test Results 65 | [Tt]est[Rr]esult*/ 66 | [Bb]uild[Ll]og.* 67 | 68 | *_i.c 69 | *_p.c 70 | *.ilk 71 | *.meta 72 | *.obj 73 | *.pch 74 | *.pdb 75 | *.pgc 76 | *.pgd 77 | *.rsp 78 | *.sbr 79 | *.tlb 80 | *.tli 81 | *.tlh 82 | *.tmp 83 | *.tmp_proj 84 | *.log 85 | *.vspscc 86 | *.vssscc 87 | .builds 88 | *.pidb 89 | *.log 90 | *.scc 91 | 92 | # Visual C++ cache files 93 | ipch/ 94 | *.aps 95 | *.ncb 96 | *.opensdf 97 | *.sdf 98 | *.cachefile 99 | 100 | # Visual Studio profiler 101 | *.psess 102 | *.vsp 103 | *.vspx 104 | 105 | # Guidance Automation Toolkit 106 | *.gpState 107 | 108 | # ReSharper is a .NET coding add-in 109 | _ReSharper*/ 110 | *.[Rr]e[Ss]harper 111 | 112 | # TeamCity is a build add-in 113 | _TeamCity* 114 | 115 | # DotCover is a Code Coverage Tool 116 | *.dotCover 117 | 118 | # NCrunch 119 | *.ncrunch* 120 | .*crunch*.local.xml 121 | 122 | # Installshield output folder 123 | [Ee]xpress/ 124 | 125 | # DocProject is a documentation generator add-in 126 | DocProject/buildhelp/ 127 | DocProject/Help/*.HxT 128 | DocProject/Help/*.HxC 129 | DocProject/Help/*.hhc 130 | DocProject/Help/*.hhk 131 | DocProject/Help/*.hhp 132 | DocProject/Help/Html2 133 | DocProject/Help/html 134 | 135 | # Click-Once directory 136 | publish/ 137 | 138 | # Publish Web Output 139 | *.Publish.xml 140 | *.pubxml 141 | *.publishproj 142 | 143 | # NuGet Packages Directory 144 | ## TODO: If you have NuGet Package Restore enabled, uncomment the next line 145 | #packages/ 146 | 147 | # Windows Azure Build Output 148 | csx 149 | *.build.csdef 150 | 151 | # Windows Store app package directory 152 | AppPackages/ 153 | 154 | # Others 155 | sql/ 156 | *.Cache 157 | ClientBin/ 158 | [Ss]tyle[Cc]op.* 159 | ~$* 160 | *~ 161 | *.dbmdl 162 | *.[Pp]ublish.xml 163 | *.pfx 164 | *.publishsettings 165 | 166 | # RIA/Silverlight projects 167 | Generated_Code/ 168 | 169 | # Backup & report files from converting an old project file to a newer 170 | # Visual Studio version. Backup files are not needed, because we have git ;-) 171 | _UpgradeReport_Files/ 172 | Backup*/ 173 | UpgradeLog*.XML 174 | UpgradeLog*.htm 175 | 176 | # SQL Server files 177 | App_Data/*.mdf 178 | App_Data/*.ldf 179 | 180 | ############# 181 | ## Windows detritus 182 | ############# 183 | 184 | # Windows image file caches 185 | Thumbs.db 186 | ehthumbs.db 187 | 188 | # Folder config file 189 | Desktop.ini 190 | 191 | # Recycle Bin used on file shares 192 | $RECYCLE.BIN/ 193 | 194 | # Mac crap 195 | .DS_Store 196 | 197 | 198 | ############# 199 | ## Python 200 | ############# 201 | 202 | *.py[cod] 203 | 204 | # Packages 205 | *.egg 206 | *.egg-info 207 | dist/ 208 | build/ 209 | eggs/ 210 | parts/ 211 | var/ 212 | sdist/ 213 | develop-eggs/ 214 | .installed.cfg 215 | 216 | # Installer logs 217 | pip-log.txt 218 | 219 | # Unit test / coverage reports 220 | .coverage 221 | .tox 222 | 223 | #Translations 224 | *.mo 225 | 226 | #Mr Developer 227 | .mr.developer.cfg -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/__Previews/coil_processing.HarnessPreview: -------------------------------------------------------------------------------- 1 | [Preview] 2 | LargeImageOriginalSize=708000 3 | LargeImageWidth=354 4 | LargeImageHeight=500 5 | LargeImage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| MediumImageOriginalSize=177000 7 | MediumImageWidth=177 8 | MediumImageHeight=250 9 | MediumImage=789CEDD6EB8ADB301006D079FF47DD6D37BECA966DC5959C424BA185854268737E1C7C91252C1846DF799E7102000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000F057EC4789945294727C7A6ECEF9E9FFCFEB484B8EB1EF621A531C658B9CC618C729F6ED518743BD5FD635BEBCBDD79A9E639EE7B8DDEAF7D314DDED6BBDCE3174E3F53C0C736C395DF3DAFB755DE2D6F591B7BDDEAF4FDF2BFFAFD6378FDA77CFF31ECB9222EF7BADC5DFF7D252CA2FF3F7EB7AECAD67FF183BCABDBEDB9EBE3F5ED367B244B93FFF7F795D2D4BF4DD478C433BFB6B0F4ED3234B7CEF9F5D3F5CB9E2EDFDE3911BFAFEAAED9629D2FCC8196DBC1CC735BE2C4B5DAF7BFABE781DA59EF7F334D6BA5CEBD97FD4FABBC538A5AB3EDB78CE5BEC3557B43CBB6D5BACF5B9D570ABF15CF3EECF6BB5DCDBEAF94F390400000000FE45DF0064E19DCB 10 | SmallImageOriginalSize=63600 11 | SmallImageWidth=106 12 | SmallImageHeight=150 13 | SmallImage=789CEDD3C16AC5201005D0F9FFDF4DA28989A64A6920BC075D356FD1B338E0CCB8102F739E679C000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000003CAEB5334A291F7F07AF725EBB14297D1B592D4B8A799A629EE7AB3FEAD4FBA56C91728EA5CF64FA9CD65A57633F8ED8F7E3F6F763F673AEB546ABA36E57FDE9B7FF27EFF669ECCD34CDD73EADEB1A65DB62EB19E6946FF9F18CDB3E75BFDEAF3202E06F7C01B531A14A 14 | 15 | -------------------------------------------------------------------------------- /pcb/wristBand_rx/ring_rx_v3_sensor/__Previews/coils.HarnessPreview: -------------------------------------------------------------------------------- 1 | [Preview] 2 | LargeImageOriginalSize=708000 3 | LargeImageWidth=354 4 | LargeImageHeight=500 5 | LargeImage=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 6 | MediumImageOriginalSize=177000 7 | MediumImageWidth=177 8 | MediumImageHeight=250 9 | MediumImage=789CEDD65B8BDB301006D0F9FF3F75B7DDC437D9F225EE28817D2C74290492F370B08C2463C1E7F19CE7192700000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000FFC5B61F514A89E3D87FB4BFD6FAF433F01ECA5C63E8AE310E25F6638D5A86188631B6F591C13EC7F3B2C4AF8FCFCCF414D334C5E592EBC731AE97DF795FA22E73D4798EA9CC31CF25BA7E88DBB1C5713B63ECC77CCE11C334C792FB87BE8FB1EB9E7E6E5E4BAB997BD6DDF3BCDD3358B72DD6BFD4D1E338BEC7CB5263CEEC2EB96FCEF192791EC729CEDB634DC9B935BF87F6DC92399FA7FC26F2FAEC33F3FAFEA59768F5F6D9EFCBFB6ABD4477FDCA7FFC9435346B70191FBDC4B6DEE7AF5D7FEF2B3E3EBFEEF5B5CF3EA065BBF514256B6A9FFBD69A59DFF7FB7CABB16DFED9E7E27D1CC72DA671C8DC2DD90FEF99E74B0C63B9E7B3CDD7BAC6565B8FB0644FB0C692F72DA32DE3AD0F5EEB96EB1EB5BAAEDB771F0D00000000AFE40F853C9CA3 10 | SmallImageOriginalSize=63600 11 | SmallImageWidth=106 12 | SmallImageHeight=150 13 | SmallImage=789CEDD3C16E8520100550FEFF77555050EC60D397BCB4DD3E13731627300C0BC8E49EE7994E000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000003EAEF733D55A6F7F07BF95B2869C72FE3666B52C39CDD394E6797E9D9738DBB6F5EA8D59965CAEBA949296B877F73F9EAEF71E8ED4F63DB5B6BFE569F4C67A1C47CC648B7EBBFA3DEA5A5B6AB11F758DDEDDFF78BABFF2942333D334BFF2B4AE25AD919B2D663232F4333F3EE72D4FE1DF7B91A1BBDF0AC0B37D0131AFA0BE 14 | 15 | -------------------------------------------------------------------------------- /cring/mag-inverse/hand_models.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Eigen/Core" 4 | #include "Eigen/Geometry" 5 | #include 6 | 7 | using namespace std; 8 | 9 | struct HandModel 10 | { 11 | Eigen::Vector3d wrist_offset; 12 | Eigen::Quaterniond wrist_offset_rot; 13 | array bone_lengths; // wristLen,fingerLen 14 | Eigen::Vector3d palm_offset; 15 | Eigen::Quaterniond palm_offset_rot; 16 | Eigen::Vector3d base_slack_pos; 17 | Eigen::Quaterniond base_slack_rot; 18 | double cmc_alpha; 19 | }; 20 | 21 | 22 | template struct hand_model_data { 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | Eigen::Vector3 ring_pos; 25 | Eigen::Vector3 knuckle_pos; 26 | Eigen::Vector3 wrist_pos; 27 | Eigen::Vector3 palm_pos; 28 | Eigen::Quaternion ring_rot; 29 | Eigen::Quaternion palm_rot; 30 | Eigen::Quaternion wrist_rot; 31 | Eigen::Quaternion knuckle_rot; 32 | }; 33 | 34 | 35 | template 36 | hand_model_data compute_hand_model(const Eigen::Vector3& wrist_offset, const Eigen::Quaternion& wrist_offset_rot, 37 | #ifdef USE_PALM 38 | const Eigen::Vector3& palm_offset, const Eigen::Quaternion& palm_offset_rot, 39 | #endif 40 | const array& bone_lengths, const T& cmc_alpha, 41 | const T& wrist_theta, const T& wrist_phi, const T& finger_theta, const T& finger_phi, 42 | const Eigen::Vector3& slack_pos, const Eigen::Quaternion& slack_rot) 43 | { 44 | hand_model_data data; 45 | 46 | 47 | /* 48 | Device frame 49 | Wrist frame 50 | CMC frame 51 | Knuckle frame 52 | Finger frame = ring frame 53 | */ 54 | 55 | data.wrist_pos = wrist_offset; 56 | Eigen::Quaternion wrist_frame = wrist_offset_rot; 57 | 58 | Eigen::AngleAxis wrist_angle_axis_x(wrist_theta, Eigen::Vector3(T(1), T(0), T(0))); 59 | Eigen::AngleAxis wrist_angle_axis_y(wrist_phi, Eigen::Vector3(T(0), T(1), T(0))); 60 | 61 | Eigen::Quaternion cmc_frame = wrist_frame * wrist_angle_axis_x * wrist_angle_axis_y; 62 | 63 | Eigen::AngleAxis cmc_rot(cmc_alpha, Eigen::Vector3(T(0), T(0), T(-1))); 64 | Eigen::Quaternion knuckle_frame = cmc_frame * cmc_rot; 65 | 66 | Eigen::AngleAxis finger_angle_axis_x(finger_theta, Eigen::Vector3(T(1), T(0), T(0))); 67 | Eigen::AngleAxis finger_angle_axis_y(finger_phi, Eigen::Vector3(T(0), T(1), T(0))); 68 | // This used to be cmc_frame * finger_angle_axis_x * finger_angle_axis_y; 69 | Eigen::Quaternion ring_frame = knuckle_frame * finger_angle_axis_x * finger_angle_axis_y; 70 | 71 | data.knuckle_pos = data.wrist_pos + knuckle_frame * Eigen::Vector3(T(0), T(0), -bone_lengths[0]); 72 | data.ring_pos = data.knuckle_pos + ring_frame * Eigen::Vector3(T(0), T(0), -bone_lengths[1]); 73 | 74 | data.ring_pos += slack_pos; 75 | ring_frame *= slack_rot; 76 | 77 | data.ring_rot = ring_frame.conjugate(); 78 | data.knuckle_rot = knuckle_frame.conjugate(); 79 | data.wrist_rot = wrist_frame.conjugate(); 80 | 81 | #ifdef USE_PALM 82 | data.palm_rot = (knuckle_frame * palm_offset_rot).conjugate(); 83 | data.palm_pos = data.wrist_pos + knuckle_frame * palm_offset; 84 | #endif 85 | 86 | return data; 87 | } 88 | 89 | template 90 | hand_model_data compute_hand_model(const HandModel& model, const T& wrist_theta, const T& wrist_phi, const T& finger_theta, const T& finger_phi, const Eigen::Vector3& slack_pos, const Eigen::Quaternion& slack_rot) 91 | { 92 | Eigen::Vector3 rx_offset_T = model.wrist_offset.cast(); 93 | Eigen::Quaternion rx_offset_rot_T = model.wrist_offset_rot.cast(); 94 | Eigen::Vector3 base_slack_pos_T = model.base_slack_pos.cast(); 95 | Eigen::Quaternion base_slack_rot_T = model.base_slack_rot.cast(); 96 | std::array bone_lengths_T = { T(model.bone_lengths[0]), T(model.bone_lengths[1]) }; 97 | T cmc_alpha_T = T(model.cmc_alpha); 98 | 99 | #ifdef USE_PALM 100 | Eigen::Vector3 palm_offset_T = model.palm_offset.cast(); 101 | Eigen::Quaternion palm_offset_rot_T = model.palm_offset_rot.cast(); 102 | return compute_hand_model(rx_offset_T, rx_offset_rot_T, palm_offset_T, palm_offset_rot_T, bone_lengths_T, cmc_alpha_T, wrist_theta, wrist_phi, finger_theta, finger_phi, slack_pos + base_slack_pos_T, slack_rot * base_slack_rot_T); 103 | #else 104 | return compute_hand_model(rx_offset_T, rx_offset_rot_T, bone_lengths_T, cmc_alpha_T, wrist_theta, wrist_phi, finger_theta, finger_phi, slack_pos + base_slack_pos_T, slack_rot * base_slack_rot_T); 105 | #endif 106 | } 107 | 108 | template 109 | hand_model_data compute_hand_model(const HandModel& model, const array& joint_angles, const Eigen::Vector3& slack_pos, const Eigen::Quaternion& slack_rot) 110 | { 111 | return compute_hand_model(model, joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], slack_pos, slack_rot); 112 | } 113 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/clock.c: -------------------------------------------------------------------------------- 1 | /* 2 | * clock.c 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | void Software_Trim(); // Software Trim to get the best DCOFTRIM value 11 | #define MCLK_FREQ_MHZ 8 // MCLK = 8MHz 12 | 13 | void clock_init() { 14 | // Configure one FRAM waitstate as required by the device datasheet for MCLK 15 | // operation beyond 8MHz _before_ configuring the clock system. 16 | FRCTL0 = FRCTLPW | NWAITS_1; 17 | 18 | __bis_SR_register(SCG0); // disable FLL 19 | CSCTL3 |= SELREF__REFOCLK; // Set REFO as FLL reference source 20 | CSCTL0 = 0; // clear DCO and MOD registers 21 | CSCTL1 &= ~(DCORSEL_7); // Clear DCO frequency select bits first 22 | CSCTL1 |= DCORSEL_5; // Set DCO = 16MHz 23 | CSCTL2 = FLLD_0 + 487; // DCOCLKDIV = 16MHz 24 | __delay_cycles(3); 25 | __bic_SR_register(SCG0); // enable FLL 26 | Software_Trim(); // Software Trim to get the best DCOFTRIM value 27 | while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // FLL locked 28 | 29 | CSCTL4 = SELMS__DCOCLKDIV | SELA__REFOCLK; // set default REFO(~32768Hz) as ACLK source, ACLK = 32768Hz 30 | // default DCOCLKDIV as MCLK and SMCLK source, SMCLK = 16MHz 31 | // CSCTL5 = DIVM__2; // Divide MCLK by 2, MCLK = 8MHz 32 | 33 | // __bis_SR_register(SCG0); // disable FLL 34 | // CSCTL3 |= SELREF__REFOCLK; // Set REFO as FLL reference source 35 | // CSCTL0 = 0; // clear DCO and MOD registers 36 | // CSCTL1 &= ~(DCORSEL_7); // Clear DCO frequency select bits first 37 | // CSCTL1 |= DCORSEL_3; // Set DCO = 8MHz 38 | // CSCTL2 = FLLD_0 + 243; // DCODIV = 8MHz 39 | // __delay_cycles(3); 40 | // __bic_SR_register(SCG0); // enable FLL 41 | // Software_Trim(); // Software Trim to get the best DCOFTRIM value 42 | // while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // Poll until FLL is locked 43 | // 44 | // CSCTL4 = SELMS__DCOCLKDIV | SELA__REFOCLK; // set default REFO(~32768Hz) as ACLK source, ACLK = 32768Hz 45 | // // default DCODIV as MCLK and SMCLK source 46 | // Configure MCLK and SMCLK = 1MHz 47 | // ACLK is at 32,768kHz; Max 40kHz;CS_REFOCLK_SELECT=32.768kHz;CS_VLOCLK_SELECT=10kHz 48 | // DCORSEL = DCORSEL_5; /* 16 MHz */ 49 | // CS_initClockSignal(CS_MCLK, CS_DCOCLKDIV_SELECT, CS_CLOCK_DIVIDER_1); 50 | // CS_initClockSignal(CS_SMCLK, CS_DCOCLKDIV_SELECT, CS_CLOCK_DIVIDER_1); 51 | // CS_initClockSignal(CS_ACLK, CS_REFOCLK_SELECT, CS_CLOCK_DIVIDER_1); 52 | } 53 | 54 | 55 | void Software_Trim() 56 | { 57 | unsigned int oldDcoTap = 0xffff; 58 | unsigned int newDcoTap = 0xffff; 59 | unsigned int newDcoDelta = 0xffff; 60 | unsigned int bestDcoDelta = 0xffff; 61 | unsigned int csCtl0Copy = 0; 62 | unsigned int csCtl1Copy = 0; 63 | unsigned int csCtl0Read = 0; 64 | unsigned int csCtl1Read = 0; 65 | unsigned int dcoFreqTrim = 3; 66 | unsigned char endLoop = 0; 67 | 68 | do 69 | { 70 | CSCTL0 = 0x100; // DCO Tap = 256 71 | do 72 | { 73 | CSCTL7 &= ~DCOFFG; // Clear DCO fault flag 74 | }while (CSCTL7 & DCOFFG); // Test DCO fault flag 75 | 76 | __delay_cycles((unsigned int)3000 * MCLK_FREQ_MHZ);// Wait FLL lock status (FLLUNLOCK) to be stable 77 | // Suggest to wait 24 cycles of divided FLL reference clock 78 | while((CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)) && ((CSCTL7 & DCOFFG) == 0)); 79 | 80 | csCtl0Read = CSCTL0; // Read CSCTL0 81 | csCtl1Read = CSCTL1; // Read CSCTL1 82 | 83 | oldDcoTap = newDcoTap; // Record DCOTAP value of last time 84 | newDcoTap = csCtl0Read & 0x01ff; // Get DCOTAP value of this time 85 | dcoFreqTrim = (csCtl1Read & 0x0070)>>4;// Get DCOFTRIM value 86 | 87 | if(newDcoTap < 256) // DCOTAP < 256 88 | { 89 | newDcoDelta = 256 - newDcoTap; // Delta value between DCPTAP and 256 90 | if((oldDcoTap != 0xffff) && (oldDcoTap >= 256)) // DCOTAP cross 256 91 | endLoop = 1; // Stop while loop 92 | else 93 | { 94 | dcoFreqTrim--; 95 | CSCTL1 = (csCtl1Read & (~DCOFTRIM)) | (dcoFreqTrim<<4); 96 | } 97 | } 98 | else // DCOTAP >= 256 99 | { 100 | newDcoDelta = newDcoTap - 256; // Delta value between DCPTAP and 256 101 | if(oldDcoTap < 256) // DCOTAP cross 256 102 | endLoop = 1; // Stop while loop 103 | else 104 | { 105 | dcoFreqTrim++; 106 | CSCTL1 = (csCtl1Read & (~DCOFTRIM)) | (dcoFreqTrim<<4); 107 | } 108 | } 109 | 110 | if(newDcoDelta < bestDcoDelta) // Record DCOTAP closest to 256 111 | { 112 | csCtl0Copy = csCtl0Read; 113 | csCtl1Copy = csCtl1Read; 114 | bestDcoDelta = newDcoDelta; 115 | } 116 | 117 | }while(endLoop == 0); // Poll until endLoop == 1 118 | 119 | CSCTL0 = csCtl0Copy; // Reload locked DCOTAP 120 | CSCTL1 = csCtl1Copy; // Reload locked DCOFTRIM 121 | while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // Poll until FLL is locked 122 | } 123 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/clock.c: -------------------------------------------------------------------------------- 1 | /* 2 | * clock.c 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: farshid 6 | */ 7 | #include 8 | #include 9 | 10 | void Software_Trim(); // Software Trim to get the best DCOFTRIM value 11 | #define MCLK_FREQ_MHZ 8 // MCLK = 8MHz 12 | 13 | void clock_init() { 14 | // Configure one FRAM waitstate as required by the device datasheet for MCLK 15 | // operation beyond 8MHz _before_ configuring the clock system. 16 | FRCTL0 = FRCTLPW | NWAITS_1; 17 | 18 | __bis_SR_register(SCG0); // disable FLL 19 | CSCTL3 |= SELREF__REFOCLK; // Set REFO as FLL reference source 20 | CSCTL0 = 0; // clear DCO and MOD registers 21 | CSCTL1 &= ~(DCORSEL_7); // Clear DCO frequency select bits first 22 | CSCTL1 |= DCORSEL_5; // Set DCO = 16MHz 23 | CSCTL2 = FLLD_0 + 487; // DCOCLKDIV = 16MHz 24 | __delay_cycles(3); 25 | __bic_SR_register(SCG0); // enable FLL 26 | Software_Trim(); // Software Trim to get the best DCOFTRIM value 27 | while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // FLL locked 28 | 29 | CSCTL4 = SELMS__DCOCLKDIV | SELA__REFOCLK; // set default REFO(~32768Hz) as ACLK source, ACLK = 32768Hz 30 | // default DCOCLKDIV as MCLK and SMCLK source, SMCLK = 16MHz 31 | // CSCTL5 = DIVM__2; // Divide MCLK by 2, MCLK = 8MHz 32 | 33 | // __bis_SR_register(SCG0); // disable FLL 34 | // CSCTL3 |= SELREF__REFOCLK; // Set REFO as FLL reference source 35 | // CSCTL0 = 0; // clear DCO and MOD registers 36 | // CSCTL1 &= ~(DCORSEL_7); // Clear DCO frequency select bits first 37 | // CSCTL1 |= DCORSEL_3; // Set DCO = 8MHz 38 | // CSCTL2 = FLLD_0 + 243; // DCODIV = 8MHz 39 | // __delay_cycles(3); 40 | // __bic_SR_register(SCG0); // enable FLL 41 | // Software_Trim(); // Software Trim to get the best DCOFTRIM value 42 | // while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // Poll until FLL is locked 43 | // 44 | // CSCTL4 = SELMS__DCOCLKDIV | SELA__REFOCLK; // set default REFO(~32768Hz) as ACLK source, ACLK = 32768Hz 45 | // // default DCODIV as MCLK and SMCLK source 46 | // Configure MCLK and SMCLK = 1MHz 47 | // ACLK is at 32,768kHz; Max 40kHz;CS_REFOCLK_SELECT=32.768kHz;CS_VLOCLK_SELECT=10kHz 48 | // DCORSEL = DCORSEL_5; /* 16 MHz */ 49 | // CS_initClockSignal(CS_MCLK, CS_DCOCLKDIV_SELECT, CS_CLOCK_DIVIDER_1); 50 | // CS_initClockSignal(CS_SMCLK, CS_DCOCLKDIV_SELECT, CS_CLOCK_DIVIDER_1); 51 | // CS_initClockSignal(CS_ACLK, CS_REFOCLK_SELECT, CS_CLOCK_DIVIDER_1); 52 | } 53 | 54 | 55 | void Software_Trim() 56 | { 57 | unsigned int oldDcoTap = 0xffff; 58 | unsigned int newDcoTap = 0xffff; 59 | unsigned int newDcoDelta = 0xffff; 60 | unsigned int bestDcoDelta = 0xffff; 61 | unsigned int csCtl0Copy = 0; 62 | unsigned int csCtl1Copy = 0; 63 | unsigned int csCtl0Read = 0; 64 | unsigned int csCtl1Read = 0; 65 | unsigned int dcoFreqTrim = 3; 66 | unsigned char endLoop = 0; 67 | 68 | do 69 | { 70 | CSCTL0 = 0x100; // DCO Tap = 256 71 | do 72 | { 73 | CSCTL7 &= ~DCOFFG; // Clear DCO fault flag 74 | }while (CSCTL7 & DCOFFG); // Test DCO fault flag 75 | 76 | __delay_cycles((unsigned int)3000 * MCLK_FREQ_MHZ);// Wait FLL lock status (FLLUNLOCK) to be stable 77 | // Suggest to wait 24 cycles of divided FLL reference clock 78 | while((CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)) && ((CSCTL7 & DCOFFG) == 0)); 79 | 80 | csCtl0Read = CSCTL0; // Read CSCTL0 81 | csCtl1Read = CSCTL1; // Read CSCTL1 82 | 83 | oldDcoTap = newDcoTap; // Record DCOTAP value of last time 84 | newDcoTap = csCtl0Read & 0x01ff; // Get DCOTAP value of this time 85 | dcoFreqTrim = (csCtl1Read & 0x0070)>>4;// Get DCOFTRIM value 86 | 87 | if(newDcoTap < 256) // DCOTAP < 256 88 | { 89 | newDcoDelta = 256 - newDcoTap; // Delta value between DCPTAP and 256 90 | if((oldDcoTap != 0xffff) && (oldDcoTap >= 256)) // DCOTAP cross 256 91 | endLoop = 1; // Stop while loop 92 | else 93 | { 94 | dcoFreqTrim--; 95 | CSCTL1 = (csCtl1Read & (~DCOFTRIM)) | (dcoFreqTrim<<4); 96 | } 97 | } 98 | else // DCOTAP >= 256 99 | { 100 | newDcoDelta = newDcoTap - 256; // Delta value between DCPTAP and 256 101 | if(oldDcoTap < 256) // DCOTAP cross 256 102 | endLoop = 1; // Stop while loop 103 | else 104 | { 105 | dcoFreqTrim++; 106 | CSCTL1 = (csCtl1Read & (~DCOFTRIM)) | (dcoFreqTrim<<4); 107 | } 108 | } 109 | 110 | if(newDcoDelta < bestDcoDelta) // Record DCOTAP closest to 256 111 | { 112 | csCtl0Copy = csCtl0Read; 113 | csCtl1Copy = csCtl1Read; 114 | bestDcoDelta = newDcoDelta; 115 | } 116 | 117 | }while(endLoop == 0); // Poll until endLoop == 1 118 | 119 | CSCTL0 = csCtl0Copy; // Reload locked DCOTAP 120 | CSCTL1 = csCtl1Copy; // Reload locked DCOFTRIM 121 | while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1)); // Poll until FLL is locked 122 | } 123 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define BUFFER_SIZE 8 //Buffer size for LPF, only powers of two 9 | const int SPITXData = 0x07; // dummy data to trigger SPI rx interrupt 10 | uint16_t SPIRXData = 0x00; //SPI 8 bit reading 11 | uint32_t sampled = 0; //6 channel adc readings 12 | uint8_t counter_timer = 0; //timer need to trigger 4 times for the whole adc read 13 | uint8_t adc_channel = 0; 14 | uint8_t buffer_count = 0; 15 | int16_t adc_a[BUFFER_SIZE][3]; 16 | int16_t adc_b[BUFFER_SIZE][3]; 17 | int16_t filter[3]; 18 | uint16_t frame_num = 0; 19 | 20 | 21 | long aclk =0; 22 | long mclk =0; 23 | long smclk =0; 24 | 25 | void low_pass(int16_t channel_b[][3]) { 26 | int32_t sum_b[3] = {0,0,0}; 27 | uint8_t i=0; uint8_t j=0; 28 | for (j=3; j > 0; j--) { 29 | for (i=BUFFER_SIZE; i > 0; i--) { 30 | sum_b[j-1] += channel_b[i-1][j-1]; 31 | } 32 | filter[j-1] = sum_b[j-1] >> 3; 33 | 34 | } 35 | } 36 | /** 37 | * main.c 38 | */ 39 | int main(void) 40 | { 41 | WDTCTL = WDTPW | WDTHOLD; // stop watchdog timer 42 | 43 | SYSCFG3 = 1; // To enble UART to be Port 2 instead of Port 1 44 | 45 | // // Setting P1.1(SCLK), P1.2(SIMO) and P1.3(SOMI) as SPI pins. 46 | GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, 47 | GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3, 48 | GPIO_PRIMARY_MODULE_FUNCTION); 49 | 50 | // Setting P1.7 as ~CS for SPI 51 | P1DIR |= BIT7; 52 | P1OUT |= BIT7; 53 | 54 | // Setting P1.4, P1.5 ,P1.6 as A0A1A2: 000=1-2dif, 010=3-4dif, 001=5-6dif 55 | P1DIR |= BIT4 | BIT5 | BIT6; 56 | P1OUT &= ~BIT4 & ~BIT5 & ~BIT6; 57 | 58 | clock_init(); 59 | 60 | aclk = CS_getACLK(); 61 | smclk = CS_getSMCLK(); 62 | mclk = CS_getMCLK(); 63 | 64 | //Setting up TIMER_A 65 | // timer_init(); 66 | 67 | spi_init(); 68 | uart_init(); 69 | PM5CTL0 &= ~LOCKLPM5; 70 | 71 | while (1) { 72 | for (buffer_count=BUFFER_SIZE; buffer_count > 0; buffer_count--) { 73 | // Single SPI takes 72us,(1/5e06)*32*9 74 | for (adc_channel = 3; adc_channel > 0; adc_channel--) 75 | { 76 | switch (adc_channel) { 77 | case 3: 78 | P1OUT &= ~BIT4 & ~BIT5 & ~BIT6; 79 | break; 80 | case 2: 81 | P1OUT = (P1OUT & (~BIT4 & ~BIT6)) | (BIT5); 82 | break; 83 | case 1: 84 | P1OUT = (P1OUT & (~BIT4 & ~BIT5)) | (BIT6); 85 | break; 86 | } 87 | //reset everything 88 | sampled = 0; 89 | //initiates the data transfer and conversion process for the SLAVE(AD7265). 90 | P1OUT &= ~BIT7; 91 | //// Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); 92 | for (counter_timer = 4; counter_timer > 0; counter_timer--) 93 | { 94 | UCB0IE |= UCTXIE; // Enable TX interrupt 95 | __bis_SR_register(LPM0_bits | GIE); // enable global interrupts, enter LPM0 96 | __no_operation(); // For debug,Remain in LPM0 97 | sampled = (sampled << 8) + (SPIRXData & 0xFF); 98 | } 99 | adc_b[buffer_count-1][adc_channel-1] = ((sampled >> 2) & 0xFFF) << 4; // 12bit ADC 100 | adc_b[buffer_count-1][adc_channel-1] = adc_b[buffer_count-1][adc_channel-1] >> 4; 101 | 102 | P1OUT |= BIT7; 103 | } 104 | } 105 | low_pass(adc_b); 106 | // UART take 238us, (1/460800)*10*11 107 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, 'U'); 108 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, 'W'); 109 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (frame_num & 0xFF)); 110 | for (adc_channel = 3; adc_channel > 0; adc_channel--) 111 | { 112 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (filter[(adc_channel-1)] & 0xF00)>>8); 113 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (filter[(adc_channel-1)] & 0xFF)>>0); 114 | } 115 | frame_num++; 116 | __no_operation(); // For debug,Remain in LPM0 117 | 118 | } 119 | 120 | return 0; 121 | } 122 | 123 | // TIMER_A ISR 124 | #if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 125 | #pragma vector=TIMER0_A0_VECTOR 126 | __interrupt void TA0_ISR(void) 127 | #elif defined(__GNUC__) 128 | void __attribute__ ((interrupt(TIMER0_A0_VECTOR))) TA0_ISR (void) 129 | #else 130 | #error Compiler not supported! 131 | #endif 132 | { 133 | counter_timer += counter_timer; 134 | while (!(UCB0IFG&UCTXIFG)); 135 | EUSCI_B_SPI_transmitData(EUSCI_B0_BASE, SPITXData); 136 | 137 | } 138 | // SPI_B ISR 139 | #if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 140 | #pragma vector=USCI_B0_VECTOR 141 | __interrupt void USCI_B0_ISR(void) 142 | #elif defined(__GNUC__) 143 | void __attribute__ ((interrupt(USCI_B0_VECTOR))) USCI_B0_ISR (void) 144 | #else 145 | #error Compiler not supported! 146 | #endif 147 | { 148 | switch(__even_in_range(UCB0IV,USCI_SPI_UCTXIFG)) 149 | { 150 | case USCI_NONE: break; // Vector 0 - no interrupt 151 | case USCI_SPI_UCRXIFG: 152 | SPIRXData = UCB0RXBUF; 153 | // UCB0IFG &= ~UCRXIFG; 154 | __bic_SR_register_on_exit(LPM0_bits);// Wake up to setup next TX 155 | break; 156 | case USCI_SPI_UCTXIFG: 157 | UCB0TXBUF = SPITXData; // Transmit characters 158 | UCB0IE &= ~UCTXIE; 159 | break; 160 | default: break; 161 | } 162 | } 163 | 164 | //UART_A ISR 165 | //#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 166 | //#pragma vector=USCI_A0_VECTOR 167 | //__interrupt void USCI_A0_ISR(void) 168 | //#elif defined(__GNUC__) 169 | //void __attribute__ ((interrupt(USCI_A0_VECTOR))) USCI_A0_ISR (void) 170 | //#else 171 | //#error Compiler not supported! 172 | //#endif 173 | //{ 174 | // switch(__even_in_range(UCA0IV,USCI_UART_UCTXCPTIFG)) 175 | // { 176 | // case USCI_NONE: break; 177 | // case USCI_UART_UCRXIFG: 178 | // while(!(UCA0IFG&UCTXIFG)); 179 | // UCA0TXBUF = UCA0RXBUF; 180 | // __no_operation(); 181 | // break; 182 | // case USCI_UART_UCTXIFG: break; 183 | // case USCI_UART_UCSTTIFG: break; 184 | // case USCI_UART_UCTXCPTIFG: break; 185 | // default: break; 186 | // } 187 | //} 188 | 189 | -------------------------------------------------------------------------------- /cring/mag-inverse/.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug/ 2 | cmake-build-release/ 3 | *.csv 4 | ## Ignore Visual Studio temporary files, build results, and 5 | ## files generated by popular Visual Studio add-ons. 6 | ## 7 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 8 | 9 | # User-specific files 10 | *.rsuser 11 | *.suo 12 | *.user 13 | *.userosscache 14 | *.sln.docstates 15 | 16 | # User-specific files (MonoDevelop/Xamarin Studio) 17 | *.userprefs 18 | 19 | # Build results 20 | [Dd]ebug/ 21 | [Dd]ebugPublic/ 22 | [Rr]elease/ 23 | [Rr]eleases/ 24 | x64/ 25 | x86/ 26 | [Aa][Rr][Mm]/ 27 | [Aa][Rr][Mm]64/ 28 | bld/ 29 | [Bb]in/ 30 | [Oo]bj/ 31 | [Ll]og/ 32 | 33 | # Visual Studio 2015/2017 cache/options directory 34 | .vs/ 35 | # Uncomment if you have tasks that create the project's static files in wwwroot 36 | #wwwroot/ 37 | 38 | # Visual Studio 2017 auto generated files 39 | Generated\ Files/ 40 | 41 | # MSTest test Results 42 | [Tt]est[Rr]esult*/ 43 | [Bb]uild[Ll]og.* 44 | 45 | # NUNIT 46 | *.VisualState.xml 47 | TestResult.xml 48 | 49 | # Build Results of an ATL Project 50 | [Dd]ebugPS/ 51 | [Rr]eleasePS/ 52 | dlldata.c 53 | 54 | # Benchmark Results 55 | BenchmarkDotNet.Artifacts/ 56 | 57 | # .NET Core 58 | project.lock.json 59 | project.fragment.lock.json 60 | artifacts/ 61 | 62 | # StyleCop 63 | StyleCopReport.xml 64 | 65 | # Files built by Visual Studio 66 | *_i.c 67 | *_p.c 68 | *_h.h 69 | *.ilk 70 | *.meta 71 | *.obj 72 | *.iobj 73 | *.pch 74 | *.pdb 75 | *.ipdb 76 | *.pgc 77 | *.pgd 78 | *.rsp 79 | *.sbr 80 | *.tlb 81 | *.tli 82 | *.tlh 83 | *.tmp 84 | *.tmp_proj 85 | *_wpftmp.csproj 86 | *.log 87 | *.vspscc 88 | *.vssscc 89 | .builds 90 | *.pidb 91 | *.svclog 92 | *.scc 93 | 94 | # Chutzpah Test files 95 | _Chutzpah* 96 | 97 | # Visual C++ cache files 98 | ipch/ 99 | *.aps 100 | *.ncb 101 | *.opendb 102 | *.opensdf 103 | *.sdf 104 | *.cachefile 105 | *.VC.db 106 | *.VC.VC.opendb 107 | 108 | # Visual Studio profiler 109 | *.psess 110 | *.vsp 111 | *.vspx 112 | *.sap 113 | 114 | # Visual Studio Trace Files 115 | *.e2e 116 | 117 | # TFS 2012 Local Workspace 118 | $tf/ 119 | 120 | # Guidance Automation Toolkit 121 | *.gpState 122 | 123 | # ReSharper is a .NET coding add-in 124 | _ReSharper*/ 125 | *.[Rr]e[Ss]harper 126 | *.DotSettings.user 127 | 128 | # JustCode is a .NET coding add-in 129 | .JustCode 130 | 131 | # TeamCity is a build add-in 132 | _TeamCity* 133 | 134 | # DotCover is a Code Coverage Tool 135 | *.dotCover 136 | 137 | # AxoCover is a Code Coverage Tool 138 | .axoCover/* 139 | !.axoCover/settings.json 140 | 141 | # Visual Studio code coverage results 142 | *.coverage 143 | *.coveragexml 144 | 145 | # NCrunch 146 | _NCrunch_* 147 | .*crunch*.local.xml 148 | nCrunchTemp_* 149 | 150 | # MightyMoose 151 | *.mm.* 152 | AutoTest.Net/ 153 | 154 | # Web workbench (sass) 155 | .sass-cache/ 156 | 157 | # Installshield output folder 158 | [Ee]xpress/ 159 | 160 | # DocProject is a documentation generator add-in 161 | DocProject/buildhelp/ 162 | DocProject/Help/*.HxT 163 | DocProject/Help/*.HxC 164 | DocProject/Help/*.hhc 165 | DocProject/Help/*.hhk 166 | DocProject/Help/*.hhp 167 | DocProject/Help/Html2 168 | DocProject/Help/html 169 | 170 | # Click-Once directory 171 | publish/ 172 | 173 | # Publish Web Output 174 | *.[Pp]ublish.xml 175 | *.azurePubxml 176 | # Note: Comment the next line if you want to checkin your web deploy settings, 177 | # but database connection strings (with potential passwords) will be unencrypted 178 | *.pubxml 179 | *.publishproj 180 | 181 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 182 | # checkin your Azure Web App publish settings, but sensitive information contained 183 | # in these scripts will be unencrypted 184 | PublishScripts/ 185 | 186 | # NuGet Packages 187 | *.nupkg 188 | # The packages folder can be ignored because of Package Restore 189 | **/[Pp]ackages/* 190 | # except build/, which is used as an MSBuild target. 191 | !**/[Pp]ackages/build/ 192 | # Uncomment if necessary however generally it will be regenerated when needed 193 | #!**/[Pp]ackages/repositories.config 194 | # NuGet v3's project.json files produces more ignorable files 195 | *.nuget.props 196 | *.nuget.targets 197 | 198 | # Microsoft Azure Build Output 199 | csx/ 200 | *.build.csdef 201 | 202 | # Microsoft Azure Emulator 203 | ecf/ 204 | rcf/ 205 | 206 | # Windows Store app package directories and files 207 | AppPackages/ 208 | BundleArtifacts/ 209 | Package.StoreAssociation.xml 210 | _pkginfo.txt 211 | *.appx 212 | 213 | # Visual Studio cache files 214 | # files ending in .cache can be ignored 215 | *.[Cc]ache 216 | # but keep track of directories ending in .cache 217 | !?*.[Cc]ache/ 218 | 219 | # Others 220 | ClientBin/ 221 | ~$* 222 | *~ 223 | *.dbmdl 224 | *.dbproj.schemaview 225 | *.jfm 226 | *.pfx 227 | *.publishsettings 228 | orleans.codegen.cs 229 | 230 | # Including strong name files can present a security risk 231 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 232 | #*.snk 233 | 234 | # Since there are multiple workflows, uncomment next line to ignore bower_components 235 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 236 | #bower_components/ 237 | # ASP.NET Core default setup: bower directory is configured as wwwroot/lib/ and bower restore is true 238 | **/wwwroot/lib/ 239 | 240 | # RIA/Silverlight projects 241 | Generated_Code/ 242 | 243 | # Backup & report files from converting an old project file 244 | # to a newer Visual Studio version. Backup files are not needed, 245 | # because we have git ;-) 246 | _UpgradeReport_Files/ 247 | Backup*/ 248 | UpgradeLog*.XML 249 | UpgradeLog*.htm 250 | ServiceFabricBackup/ 251 | *.rptproj.bak 252 | 253 | # SQL Server files 254 | *.mdf 255 | *.ldf 256 | *.ndf 257 | 258 | # Business Intelligence projects 259 | *.rdl.data 260 | *.bim.layout 261 | *.bim_*.settings 262 | *.rptproj.rsuser 263 | *- Backup*.rdl 264 | 265 | # Microsoft Fakes 266 | FakesAssemblies/ 267 | 268 | # GhostDoc plugin setting file 269 | *.GhostDoc.xml 270 | 271 | # Node.js Tools for Visual Studio 272 | .ntvs_analysis.dat 273 | node_modules/ 274 | 275 | # Visual Studio 6 build log 276 | *.plg 277 | 278 | # Visual Studio 6 workspace options file 279 | *.opt 280 | 281 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 282 | *.vbw 283 | 284 | # Visual Studio LightSwitch build output 285 | **/*.HTMLClient/GeneratedArtifacts 286 | **/*.DesktopClient/GeneratedArtifacts 287 | **/*.DesktopClient/ModelManifest.xml 288 | **/*.Server/GeneratedArtifacts 289 | **/*.Server/ModelManifest.xml 290 | _Pvt_Extensions 291 | 292 | # Paket dependency manager 293 | .paket/paket.exe 294 | paket-files/ 295 | 296 | # FAKE - F# Make 297 | .fake/ 298 | 299 | # JetBrains Rider 300 | .idea/ 301 | *.sln.iml 302 | 303 | # CodeRush personal settings 304 | .cr/personal 305 | 306 | # Python Tools for Visual Studio (PTVS) 307 | __pycache__/ 308 | *.pyc 309 | 310 | # Cake - Uncomment if you are using it 311 | # tools/** 312 | # !tools/packages.config 313 | 314 | # Tabs Studio 315 | *.tss 316 | 317 | # Telerik's JustMock configuration file 318 | *.jmconfig 319 | 320 | # BizTalk build output 321 | *.btp.cs 322 | *.btm.cs 323 | *.odx.cs 324 | *.xsd.cs 325 | 326 | # OpenCover UI analysis results 327 | OpenCover/ 328 | 329 | # Azure Stream Analytics local run output 330 | ASALocalRun/ 331 | 332 | # MSBuild Binary and Structured Log 333 | *.binlog 334 | 335 | # NVidia Nsight GPU debugger configuration file 336 | *.nvuser 337 | 338 | # MFractors (Xamarin productivity tool) working folder 339 | .mfractor/ 340 | 341 | # Local History for Visual Studio 342 | .localhistory/ 343 | 344 | # BeatPulse healthcheck temp database 345 | healthchecksdb 346 | 347 | # Backup folder for Package Reference Convert tool in Visual Studio 2017 348 | MigrationBackup/ -------------------------------------------------------------------------------- /cring/mag-inverse/mag_models.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Eigen/Core" 4 | #include "Eigen/Geometry" 5 | #include "utils.h" 6 | 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | template 12 | struct CoilCalibrationModel 13 | { 14 | T global_gain; 15 | Eigen::Vector3 noise; 16 | Eigen::Vector3 bias; 17 | Eigen::Vector3 gain; 18 | Eigen::Vector3 per_channel_gain; 19 | Eigen::Vector3 sensor_offset; 20 | Eigen::Quaternion sensor_rot_offset; 21 | Eigen::Vector3 base_sensor_offset; 22 | Eigen::Quaternion base_sensor_rot_offset; 23 | Eigen::Vector3 ring_offset; 24 | Eigen::Quaternion ring_rot_offset; 25 | array crosstalk; 26 | 27 | template CoilCalibrationModel cast() const 28 | { 29 | CoilCalibrationModel model; 30 | model.global_gain = T(global_gain); 31 | model.noise = noise.cast(); 32 | model.bias = bias.cast(); 33 | model.gain = gain.cast(); 34 | model.per_channel_gain = per_channel_gain.cast(); 35 | model.sensor_offset = sensor_offset.cast(); 36 | model.sensor_rot_offset = sensor_rot_offset.cast(); 37 | model.ring_offset = ring_offset.cast(); 38 | model.ring_rot_offset = ring_rot_offset.cast(); 39 | model.base_sensor_offset = base_sensor_offset; 40 | model.base_sensor_rot_offset = base_sensor_rot_offset; 41 | model.crosstalk = { T(crosstalk[0]), T(crosstalk[1]), T(crosstalk[2]) }; 42 | return model; 43 | } 44 | }; 45 | 46 | template 47 | struct CalibrationModel 48 | { 49 | CoilCalibrationModel coil1; 50 | CoilCalibrationModel coil2; 51 | CoilCalibrationModel coil3; 52 | 53 | template CalibrationModel cast() const 54 | { 55 | CalibrationModel model; 56 | model.coil1 = coil1.cast(); 57 | model.coil2 = coil2.cast(); 58 | model.coil3 = coil3.cast(); 59 | return model; 60 | } 61 | }; 62 | 63 | 64 | 65 | template 66 | Eigen::Vector3 dipole_model(const Eigen::Vector3& pos) { 67 | T r = pos.norm(); 68 | T r5 = pow(r, 5); 69 | T r2 = pow(r, 2); 70 | T Bx = pos[0] * pos[2] * T(3) / (r5); 71 | T By = pos[1] * pos[2] * T(3) / (r5); 72 | T Bz = (pos[2] * pos[2] * T(3) - r2) / (r5); 73 | return Eigen::Vector3(Bx, By, Bz) * T(1e5); 74 | } 75 | 76 | 77 | 78 | template 79 | Eigen::Vector3 forward_model(const Eigen::Vector3& ring_pos, const Eigen::Quaternion& ring_rot, const CoilCalibrationModel& model, bool preserve_sign, Vector3& out_field) 80 | { 81 | Eigen::Vector3 sensor_pos = model.base_sensor_offset + model.sensor_offset; 82 | Eigen::Quaternion sensor_rot = model.sensor_rot_offset * model.base_sensor_rot_offset.cast(); 83 | Eigen::Vector3 ring_pos_adj = ring_pos + (ring_rot.conjugate() * model.ring_offset); 84 | //Eigen::Vector3 ring_pos_adj = ring_pos; 85 | Eigen::Quaternion ring_rot_adj = ring_rot * model.ring_rot_offset; 86 | 87 | Eigen::Vector3 sensor_ring = ring_rot_adj * (sensor_pos - ring_pos_adj); 88 | Eigen::Vector3 field = ring_rot_adj.conjugate() * dipole_model(sensor_ring); 89 | 90 | 91 | Eigen::Vector3 field_sensor_frame = sensor_rot * field; 92 | out_field = field_sensor_frame; 93 | 94 | Eigen::Vector3 sensor; 95 | 96 | 97 | for (unsigned int i = 0; i < 3; i++) 98 | { 99 | sensor[i] = pow(field_sensor_frame[i] * model.global_gain / model.per_channel_gain[i], 2) + pow(model.noise[i], 2); 100 | } 101 | 102 | /*sensor[0] += pow(field_sensor_frame[1] * model.crosstalk[0] + field_sensor_frame[2] * model.crosstalk[1], 2); 103 | sensor[1] += pow(field_sensor_frame[0] * model.crosstalk[0] + field_sensor_frame[2] * model.crosstalk[2], 2); 104 | sensor[2] += pow(field_sensor_frame[0] * model.crosstalk[1] + field_sensor_frame[1] * model.crosstalk[2], 2);*/ 105 | 106 | sensor[0] += pow(field_sensor_frame[1] * model.crosstalk[0], 2) + pow(field_sensor_frame[2] * model.crosstalk[1], 2); 107 | sensor[1] += pow(field_sensor_frame[0] * model.crosstalk[0], 2) + pow(field_sensor_frame[2] * model.crosstalk[2], 2); 108 | sensor[2] += pow(field_sensor_frame[0] * model.crosstalk[1], 2) + pow(field_sensor_frame[1] * model.crosstalk[2], 2); 109 | 110 | for (unsigned int i = 0; i < 3; i++) 111 | { 112 | sensor[i] = model.gain[i] * ceres::sqrt(sensor[i]) - model.bias[i]; 113 | if (preserve_sign) 114 | { 115 | sensor[i] = sgn(field_sensor_frame[i]) * sensor[i]; 116 | } 117 | if (!isnormal(D(sensor[i]))) { 118 | printf("complex result\n"); 119 | } 120 | } 121 | 122 | return sensor; 123 | } 124 | 125 | template 126 | array forward_model(const Eigen::Vector3& ring_pos, const Eigen::Quaternion& ring_rot, const CalibrationModel& model, bool preserve_sign) 127 | { 128 | Vector3 field; 129 | Eigen::Vector3 sensor_pred1 = forward_model(ring_pos, ring_rot, model.coil1, preserve_sign, field); 130 | Eigen::Vector3 sensor_pred2 = forward_model(ring_pos, ring_rot, model.coil2, preserve_sign, field); 131 | Eigen::Vector3 sensor_pred3 = forward_model(ring_pos, ring_rot, model.coil3, preserve_sign, field); 132 | array all_sensors; 133 | for (unsigned int i = 0; i < 3; i++) { 134 | all_sensors[i] = sensor_pred1[i]; 135 | all_sensors[i + 3] = sensor_pred2[i]; 136 | all_sensors[i + 6] = sensor_pred3[i]; 137 | } 138 | return all_sensors; 139 | } 140 | 141 | 142 | 143 | //template inline void print_calibration(CalibrationModel model); 144 | template 145 | inline void print_calibration(CalibrationModel model) { 146 | /*printf("Gain 1: "); 147 | print_vec(model.gain_1); 148 | printf("\n"); 149 | 150 | printf("Gain 2: "); 151 | print_vec(model.gain_2); 152 | printf("\n"); 153 | 154 | 155 | printf("Bias: %f\n", D(model.bias)); 156 | printf("Noise: %f\n", D(model.noise));*/ 157 | 158 | printf("global gain: %f, %f\n", D(model.coil1.global_gain), D(model.coil2.global_gain)); 159 | 160 | printf("sensor_1_offset: "); 161 | print_vec(model.coil1.sensor_offset); 162 | printf("\n"); 163 | printf("sensor_2_offset: "); 164 | print_vec(model.coil2.sensor_offset); 165 | printf("\n"); 166 | 167 | printf("sensor_1_rot_offset: "); 168 | print_quat(model.coil1.sensor_rot_offset); 169 | printf("\n"); 170 | printf("sensor_2_rot_offset: "); 171 | print_quat(model.coil2.sensor_rot_offset); 172 | printf("\n"); 173 | 174 | /*printf("crosstalk_1: (%f, %f, %f)\n", D(model.crosstalk_1[0]), D(model.crosstalk_1[1]), D(model.crosstalk_1[2])); 175 | printf("crosstalk_2: (%f, %f, %f)\n", D(model.crosstalk_2[0]), D(model.crosstalk_2[1]), D(model.crosstalk_2[2]));*/ 176 | } 177 | 178 | template 179 | inline void print_calibration(CoilCalibrationModel model) { 180 | printf("Noise: "); 181 | print_vec(model.noise); 182 | printf("\n"); 183 | printf("Gain: "); 184 | print_vec(model.gain); 185 | printf("\n"); 186 | printf("Bias: "); 187 | print_vec(model.bias); 188 | printf("\n"); 189 | 190 | 191 | printf("global gain: %f\n", D(model.global_gain)); 192 | 193 | printf("sensor_offset: "); 194 | print_vec(model.sensor_offset); 195 | printf("\n"); 196 | 197 | printf("sensor_rot_offset: "); 198 | print_quat(model.sensor_rot_offset); 199 | printf("\n"); 200 | 201 | printf("ring_offset: "); 202 | print_vec(model.ring_offset); 203 | printf("\n"); 204 | 205 | printf("ring_rot_offset: "); 206 | print_quat(model.ring_rot_offset); 207 | printf("\n"); 208 | 209 | 210 | printf("per_channel_gain: "); 211 | print_vec(model.per_channel_gain); 212 | printf("\n"); 213 | 214 | printf("crosstalk: %f, %f, %f\n", D(model.crosstalk[0]), D(model.crosstalk[1]), D(model.crosstalk[2])); 215 | /*printf("crosstalk_1: (%f, %f, %f)\n", D(model.crosstalk_1[0]), D(model.crosstalk_1[1]), D(model.crosstalk_1[2])); 216 | printf("crosstalk_2: (%f, %f, %f)\n", D(model.crosstalk_2[0]), D(model.crosstalk_2[1]), D(model.crosstalk_2[2]));*/ 217 | } 218 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define USE_BOTTOM 9 | 10 | #define BUFFER_SIZE 4 //Buffer size for LPF, only powers of two 11 | const int SPITXData = 0x07; // dummy data to trigger SPI rx interrupt 12 | uint16_t SPIRXData = 0x00; //SPI 8 bit reading 13 | uint32_t sampled = 0; //6 channel adc readings 14 | uint8_t counter_timer = 0; //timer need to trigger 4 times for the whole adc read 15 | uint8_t adc_channel = 0; 16 | uint8_t buffer_count = 0; 17 | int16_t adc_a[BUFFER_SIZE][3]; 18 | int16_t adc_b[BUFFER_SIZE][3]; 19 | int16_t filter[6]; 20 | 21 | uint16_t frame_num = 0; 22 | unsigned char bottomBoard_buffer[9]; 23 | uint8_t bottomBoard_index = 0; 24 | 25 | long aclk =0; 26 | long mclk =0; 27 | long smclk =0; 28 | 29 | void low_pass(int16_t channel_a[][3], int16_t channel_b[][3]) { 30 | int32_t sum_a[3]={0,0,0}; 31 | int32_t sum_b[3]={0,0,0}; 32 | uint8_t i=0; uint8_t j=0; 33 | for (j=3; j > 0; j--) { 34 | for (i=BUFFER_SIZE; i > 0; i--) { 35 | sum_a[j-1] += channel_a[i-1][j-1]; 36 | sum_b[j-1] +=channel_b[i-1][j-1]; 37 | } 38 | filter[(j-1)*2] = sum_a[j-1] >> 2; 39 | filter[(j-1)*2+1] = sum_b[j-1] >> 2; 40 | 41 | } 42 | } 43 | /** 44 | * main.c 45 | */ 46 | int main(void) 47 | { 48 | WDTCTL = WDTPW | WDTHOLD; // stop watchdog timer 49 | 50 | SYSCFG3 = 1; // To enble UART to be Port 2 instead of Port 1 51 | 52 | // // Setting P1.1(SCLK), P1.2(SIMO) and P1.3(SOMI) as SPI pins. 53 | GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, 54 | GPIO_PIN1 | GPIO_PIN2 | GPIO_PIN3, 55 | GPIO_PRIMARY_MODULE_FUNCTION); 56 | 57 | // Setting P1.7 as ~CS for SPI 58 | P1DIR |= BIT7; 59 | P1OUT |= BIT7; 60 | 61 | // Setting P1.4, P1.5 ,P1.6 as A0A1A2: 000=1-2dif, 010=3-4dif, 001=5-6dif 62 | P1DIR |= BIT4 | BIT5 | BIT6; 63 | P1OUT &= ~BIT4 & ~BIT5 & ~BIT6; 64 | 65 | clock_init(); 66 | 67 | aclk = CS_getACLK(); 68 | smclk = CS_getSMCLK(); 69 | mclk = CS_getMCLK(); 70 | 71 | //Setting up TIMER_A 72 | // timer_init(); 73 | 74 | spi_init(); 75 | uart_init(); 76 | PM5CTL0 &= ~LOCKLPM5; 77 | 78 | while (1) { 79 | for (buffer_count=BUFFER_SIZE; buffer_count > 0; buffer_count--) { 80 | // Single SPI takes 72us,(1/5e06)*32*9 81 | for (adc_channel = 3; adc_channel > 0; adc_channel--) 82 | { 83 | switch (adc_channel) { 84 | case 3: 85 | P1OUT &= ~BIT4 & ~BIT5 & ~BIT6; 86 | break; 87 | case 2: 88 | P1OUT = (P1OUT & (~BIT4 & ~BIT6)) | (BIT5); 89 | break; 90 | case 1: 91 | P1OUT = (P1OUT & (~BIT4 & ~BIT5)) | (BIT6); 92 | break; 93 | } 94 | //reset everything 95 | sampled = 0; 96 | //initiates the data transfer and conversion process for the SLAVE(AD7265). 97 | P1OUT &= ~BIT7; 98 | //// Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); 99 | for (counter_timer = 4; counter_timer > 0; counter_timer--) 100 | { 101 | UCB0IE |= UCTXIE; // Enable TX interrupt 102 | __bis_SR_register(LPM0_bits | GIE); // enable global interrupts, enter LPM0 103 | __no_operation(); // For debug,Remain in LPM0 104 | sampled = (sampled << 8) + (SPIRXData & 0xFF); 105 | } 106 | 107 | adc_b[buffer_count-1][adc_channel-1] = ((sampled >> 2) & 0xFFF) << 4; // 12bit ADC 108 | adc_b[buffer_count-1][adc_channel-1] = adc_b[buffer_count-1][adc_channel-1] >> 4; 109 | adc_a[buffer_count-1][adc_channel-1] = ((sampled >> 18) & 0xFFF) << 4; // 12bit ADC 110 | adc_a[buffer_count-1][adc_channel-1] = adc_a[buffer_count-1][adc_channel-1] >> 4; 111 | 112 | P1OUT |= BIT7; 113 | } 114 | } 115 | low_pass(adc_a, adc_b); 116 | #ifdef USE_BOTTOM 117 | // recieve from bottom board 118 | EUSCI_A_UART_enableInterrupt(EUSCI_A0_BASE,EUSCI_A_UART_RECEIVE_INTERRUPT); 119 | while (bottomBoard_index <= 8) { 120 | __bis_SR_register(LPM0_bits | GIE); // enable global interrupts, enter LPM0 121 | if (bottomBoard_index == 0) { 122 | if (bottomBoard_buffer[0] != 0x55 ) { 123 | continue; 124 | } 125 | } 126 | if (bottomBoard_index == 1) { 127 | if (bottomBoard_buffer[1] != 0x57) { 128 | bottomBoard_index = 0; 129 | continue; 130 | } 131 | } 132 | bottomBoard_index ++; 133 | } 134 | EUSCI_A_UART_disableInterrupt(EUSCI_A0_BASE,EUSCI_A_UART_RECEIVE_INTERRUPT); 135 | bottomBoard_index = 0; 136 | #endif 137 | // UART take 238us, (1/460800)*10*11 138 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, 'U'); 139 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, 'W'); 140 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (frame_num & 0xFF)); 141 | for (adc_channel = 3; adc_channel > 0; adc_channel--) 142 | { 143 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (filter[(adc_channel-1)*2] & 0xFF0)>>4); 144 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, ((filter[(adc_channel-1)*2] & 0x00F)<<4)+((filter[(adc_channel-1)*2 + 1] & 0xF00)>>8)); 145 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, (filter[(adc_channel-1)*2 + 1] & 0xFF)>>0); 146 | } 147 | uint8_t counter_bottomBoard; 148 | for (counter_bottomBoard = 2; counter_bottomBoard < 9; counter_bottomBoard++) 149 | { 150 | EUSCI_A_UART_transmitData(EUSCI_A0_BASE, bottomBoard_buffer[counter_bottomBoard]); 151 | } 152 | __no_operation(); // For debug,Remain in LPM0 153 | frame_num ++; 154 | 155 | } 156 | 157 | return 0; 158 | } 159 | 160 | // TIMER_A ISR 161 | #if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 162 | #pragma vector=TIMER0_A0_VECTOR 163 | __interrupt void TA0_ISR(void) 164 | #elif defined(__GNUC__) 165 | void __attribute__ ((interrupt(TIMER0_A0_VECTOR))) TA0_ISR (void) 166 | #else 167 | #error Compiler not supported! 168 | #endif 169 | { 170 | counter_timer += counter_timer; 171 | while (!(UCB0IFG&UCTXIFG)); 172 | EUSCI_B_SPI_transmitData(EUSCI_B0_BASE, SPITXData); 173 | 174 | } 175 | // SPI_B ISR 176 | #if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 177 | #pragma vector=USCI_B0_VECTOR 178 | __interrupt void USCI_B0_ISR(void) 179 | #elif defined(__GNUC__) 180 | void __attribute__ ((interrupt(USCI_B0_VECTOR))) USCI_B0_ISR (void) 181 | #else 182 | #error Compiler not supported! 183 | #endif 184 | { 185 | switch(__even_in_range(UCB0IV,USCI_SPI_UCTXIFG)) 186 | { 187 | case USCI_NONE: break; // Vector 0 - no interrupt 188 | case USCI_SPI_UCRXIFG: 189 | SPIRXData = UCB0RXBUF; 190 | // UCB0IFG &= ~UCRXIFG; 191 | __bic_SR_register_on_exit(LPM0_bits);// Wake up to setup next TX 192 | break; 193 | case USCI_SPI_UCTXIFG: 194 | UCB0TXBUF = SPITXData; // Transmit characters 195 | UCB0IE &= ~UCTXIE; 196 | break; 197 | default: break; 198 | } 199 | } 200 | 201 | //UART_A ISR 202 | #if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__) 203 | #pragma vector=USCI_A0_VECTOR 204 | __interrupt void USCI_A0_ISR(void) 205 | #elif defined(__GNUC__) 206 | void __attribute__ ((interrupt(USCI_A0_VECTOR))) USCI_A0_ISR (void) 207 | #else 208 | #error Compiler not supported! 209 | #endif 210 | { 211 | switch(__even_in_range(UCA0IV,USCI_UART_UCTXCPTIFG)) 212 | { 213 | case USCI_NONE: break; 214 | case USCI_UART_UCRXIFG: 215 | bottomBoard_buffer[bottomBoard_index] = UCA0RXBUF; 216 | __no_operation(); 217 | __bic_SR_register_on_exit(LPM0_bits);// Wake up to setup next RX 218 | break; 219 | case USCI_UART_UCTXIFG: break; 220 | case USCI_UART_UCSTTIFG: break; 221 | case USCI_UART_UCTXCPTIFG: break; 222 | default: break; 223 | } 224 | } 225 | 226 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/mag-runtime.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 15.0 23 | {486DF039-C998-4055-9CDA-C9FD2E478166} 24 | magruntime 25 | 10.0.17763.0 26 | 27 | 28 | 29 | Application 30 | true 31 | v141 32 | MultiByte 33 | 34 | 35 | Application 36 | false 37 | v141 38 | true 39 | MultiByte 40 | 41 | 42 | Application 43 | true 44 | v141 45 | MultiByte 46 | 47 | 48 | Application 49 | false 50 | v141 51 | true 52 | MultiByte 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | Level3 76 | MaxSpeed 77 | true 78 | true 79 | WIN32;NDEBUG;_WINDOWS;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 80 | true 81 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include;$(SolutionDir)..\jointAngle\jointAngle\jointAngle 82 | stdcpp14 83 | Speed 84 | 85 | 86 | true 87 | true 88 | ceres.lib;glog.lib;gflags_static.lib;shlwapi.lib;%(AdditionalDependencies) 89 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 90 | 91 | 92 | 93 | 94 | Level3 95 | Disabled 96 | true 97 | WIN32;_DEBUG;_CONSOLE;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 98 | true 99 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include;$(SolutionDir)..\jointAngle\jointAngle\jointAngle 100 | stdcpp14 101 | 102 | 103 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 104 | ceres-debug.lib;glog_debug.lib;gflags_static_debug.lib;shlwapi.lib;%(AdditionalDependencies) 105 | Console 106 | true 107 | 108 | 109 | 110 | 111 | Level3 112 | Disabled 113 | true 114 | true 115 | 116 | 117 | 118 | 119 | Level3 120 | MaxSpeed 121 | true 122 | true 123 | true 124 | true 125 | 126 | 127 | true 128 | true 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | -------------------------------------------------------------------------------- /cring/mag-inverse/global_model/global_model.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 15.0 23 | {0346D26E-3AA3-4176-B1B5-9395EDDC31BE} 24 | globalmodel 25 | 10.0.17763.0 26 | 27 | 28 | 29 | Application 30 | true 31 | v141 32 | MultiByte 33 | 34 | 35 | Application 36 | false 37 | v141 38 | true 39 | MultiByte 40 | 41 | 42 | Application 43 | true 44 | v141 45 | MultiByte 46 | 47 | 48 | Application 49 | false 50 | v141 51 | true 52 | MultiByte 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | Level3 76 | Custom 77 | true 78 | true 79 | true 80 | WIN32;NDEBUG;_WINDOWS;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 81 | true 82 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include 83 | stdcpp14 84 | AnySuitable 85 | Speed 86 | true 87 | true 88 | 89 | 90 | true 91 | true 92 | ceres.lib;glog.lib;gflags_static.lib;shlwapi.lib;%(AdditionalDependencies) 93 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 94 | 95 | 96 | 97 | 98 | Level3 99 | Disabled 100 | true 101 | WIN32;_DEBUG;_CONSOLE;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 102 | true 103 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include 104 | stdcpp14 105 | 106 | 107 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 108 | ceres-debug.lib;glog_debug.lib;gflags_static_debug.lib;shlwapi.lib;%(AdditionalDependencies) 109 | true 110 | 111 | 112 | 113 | 114 | Level3 115 | Disabled 116 | true 117 | true 118 | 119 | 120 | 121 | 122 | Level3 123 | MaxSpeed 124 | true 125 | true 126 | true 127 | true 128 | 129 | 130 | true 131 | true 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag-inverse/mag-inverse.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 15.0 23 | {99E09A45-DEF2-4F8D-8763-D0574AF016CF} 24 | Win32Proj 25 | maginverse 26 | 10.0.17763.0 27 | 28 | 29 | 30 | Application 31 | true 32 | v141 33 | Unicode 34 | 35 | 36 | Application 37 | false 38 | v141 39 | true 40 | Unicode 41 | 42 | 43 | Application 44 | true 45 | v141 46 | Unicode 47 | 48 | 49 | Application 50 | false 51 | v141 52 | true 53 | Unicode 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | Level3 78 | Disabled 79 | true 80 | WIN32;_DEBUG;_CONSOLE;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 81 | true 82 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include 83 | stdcpp14 84 | 85 | 86 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 87 | ceres-debug.lib;glog_debug.lib;gflags_static_debug.lib;shlwapi.lib;%(AdditionalDependencies) 88 | Console 89 | true 90 | 91 | 92 | 93 | 94 | Use 95 | Level3 96 | Disabled 97 | true 98 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 99 | true 100 | pch.h 101 | 102 | 103 | Console 104 | true 105 | 106 | 107 | 108 | 109 | Level3 110 | MaxSpeed 111 | true 112 | true 113 | WIN32;NDEBUG;_WINDOWS;GFLAGS_DLL_DECLARE_FLAG=;GFLAGS_DLL_DEFINE_FLAG=;GLOG_NO_ABBREVIATED_SEVERITIES;GOOGLE_GLOG_DLL_DECL=;GFLAGS_IS_A_DLL=0;%(PreprocessorDefinitions) 114 | true 115 | $(SolutionDir);$(AURARING_RUNTIME_PATH)\deps\google-glog\include;$(AURARING_RUNTIME_PATH)\deps\eigen\include\eigen3;$(AURARING_RUNTIME_PATH)\deps\gflags\include;$(AURARING_RUNTIME_PATH)\deps\ceres\include 116 | stdcpp14 117 | Speed 118 | 119 | 120 | true 121 | true 122 | ceres.lib;glog.lib;gflags_static.lib;shlwapi.lib;%(AdditionalDependencies) 123 | $(SolutionDir)..\bin\lib;$(AURARING_RUNTIME_PATH)\deps\google-glog\lib;$(AURARING_RUNTIME_PATH)\deps\gflags\lib;$(AURARING_RUNTIME_PATH)\deps\ceres\lib;%(AdditionalLibraryDirectories) 124 | 125 | 126 | 127 | 128 | Use 129 | Level3 130 | MaxSpeed 131 | true 132 | true 133 | true 134 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 135 | true 136 | pch.h 137 | 138 | 139 | Console 140 | true 141 | true 142 | true 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | -------------------------------------------------------------------------------- /pyring/controller_lib.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from settings import PORT, BAUD, DATA_ROOT 4 | import serial 5 | import pyrealtime as prt 6 | import struct 7 | import numpy as np 8 | 9 | OSR = 256 10 | BYTES_PER_SAMPLE = 2 11 | RX_CHANNELS_ADC = 3 12 | BUFFER_SIZE = 8 13 | AGGREGATE = 1 14 | RX_CHANNELS = 9 # was 6 15 | 16 | NUM_SIGN_BYTES = 1 17 | 18 | NUM_BYTES = BYTES_PER_SAMPLE * RX_CHANNELS_ADC * AGGREGATE + NUM_SIGN_BYTES 19 | PACKET_FORMAT = '<' + 'B' * RX_CHANNELS_ADC * AGGREGATE * BYTES_PER_SAMPLE + ("B" * NUM_SIGN_BYTES) 20 | 21 | USE_NATIVE = True 22 | RECORD = False 23 | 24 | VICON_PORT = 9987 25 | 26 | FRAME_FORMAT = "IHhhhB" 27 | FRAME_SIZE = 13 28 | FRAMES_PER_PACKET = 40 29 | 30 | 31 | def convert_to_volts(data): 32 | center = 0x800000 >> int(((256 / OSR) - 1) * 3) 33 | if BYTES_PER_SAMPLE == 2: 34 | center = center >> 8 35 | return np.sign(data) * (np.abs(data) - center) / (center - 1) * -1.2 36 | 37 | 38 | def fix_signs(data, counts, buffer_size): 39 | signs = [(1 if x > buffer_size / 2 else -1) for x in counts] 40 | return data * signs 41 | 42 | 43 | def fix_signs_single_byte(data, sign_byte): 44 | values = [(sign_byte >> 0) & 0x03, (sign_byte >> 2) & 0x03, (sign_byte >> 4) & 0x03] 45 | # print(values) 46 | signs = [(1 if x >= 2 else -1) for x in values] 47 | return data * signs 48 | 49 | 50 | BYTES_PER_GROUP = 3 51 | AGGREGATE = 1 52 | DEMO = False 53 | 54 | 55 | def process(data): 56 | try: 57 | data = struct.unpack('>' + 'B' * 17, data) 58 | data = np.array([int(x) for x in data]) 59 | 60 | except (ValueError, struct.error): 61 | print("Parse error") 62 | return None 63 | 64 | def convert_adc_bottom(data): 65 | all_data = np.zeros((3, 1)) 66 | for i in range(3): 67 | adc_b = data[1 + i * 2] + ((data[0 + i * 2] & 0x0f) << 8) 68 | adc_b = adc_b - 2 ** 12 if adc_b > 2 ** 11 - 1 else adc_b 69 | all_data[i] = -adc_b 70 | result = all_data.T / 2048 * 1.2 71 | return result 72 | 73 | def convert_adc_top(data): 74 | 75 | all_data = np.zeros((6, 1)) 76 | for i in range(3): 77 | adc_a = (data[0 + i * BYTES_PER_GROUP] << 4) + ((data[1 + i * BYTES_PER_GROUP] & 0xf0) >> 4) 78 | adc_b = data[2 + i * BYTES_PER_GROUP] + ((data[1 + i * BYTES_PER_GROUP] & 0x0f) << 8) 79 | adc_a = adc_a - 2 ** 12 if adc_a > 2 ** 11 - 1 else adc_a 80 | adc_b = adc_b - 2 ** 12 if adc_b > 2 ** 11 - 1 else adc_b 81 | all_data[i * 2 + 0] = -adc_a 82 | all_data[i * 2 + 1] = -adc_b 83 | 84 | result = all_data.T / 2048 * 1.2 85 | return result 86 | 87 | # top = convert_adc(data[1:10], 'top') 88 | # bottom = convert_adc(data[11:17], 'bottom') 89 | top = convert_adc_top(data[1:10]) 90 | bottom = convert_adc_bottom(data[11:17]) 91 | # bottom = bottom[0,[1,3,5]] 92 | all_data = np.hstack((np.atleast_2d(data[[0, 10]]), top, bottom)) 93 | if np.any(all_data < -.02): 94 | print("bad packet") 95 | return None 96 | return all_data 97 | 98 | 99 | @prt.transformer 100 | def decimate(layer): 101 | return layer[0, :] 102 | 103 | 104 | @prt.transformer(multi_output=True) 105 | def split(data): 106 | return {"frame_num": data[:, 0:2], "adc": data[:, 2:]} 107 | 108 | last_data = [0,0] 109 | @prt.transformer 110 | def diff(data): 111 | global last_data 112 | d1 = data[0,:] - last_data 113 | d = np.diff(data, axis=0) 114 | last_data = data[-1,:] 115 | # print(data) 116 | return np.vstack((d1, d)) 117 | 118 | 119 | @prt.transformer 120 | def get_channel(data): 121 | return data[:,0] 122 | 123 | 124 | @prt.transformer 125 | def get_energy(x): 126 | return np.atleast_2d(np.sum(x ** 2, axis=1)).T 127 | 128 | 129 | @prt.transformer 130 | def get_taps(energy_thresholds): 131 | energy = energy_thresholds['energy'] 132 | if "thresholds" in energy_thresholds: 133 | thresholds = energy_thresholds['thresholds'] 134 | else: 135 | thresholds = (0.000008, 0.000015) 136 | if np.max(energy) > thresholds[1]: # 0.000015: 137 | # print(2) 138 | return np.array([2]) 139 | # else: 140 | # return False 141 | # return np.array([1]) 142 | elif np.max(energy) > thresholds[0]: # 0.000008: 143 | # print(1) 144 | return np.array([1]) 145 | else: 146 | return np.array([0]) 147 | 148 | 149 | def decode_mag_file(line): 150 | data = np.array(eval(line.decode('utf-8'))) 151 | return data 152 | 153 | 154 | def playback_device_data(key, show_plot=True, strip_frame_num=True): 155 | filename = os.path.join(DATA_ROOT, "recordings", f"mag_{key}.txt") 156 | if not os.path.exists(filename): 157 | filename = f"mag_{key}.txt" 158 | data = prt.PlaybackLayer(filename, rate=59, decoder=decode_mag_file, print_fps=True, loop=True) 159 | if strip_frame_num: 160 | split_data = split(data) 161 | adc = split_data.get_port("adc") 162 | else: 163 | adc = data 164 | 165 | if show_plot: 166 | fm = prt.FigureManager(fps=10000) 167 | prt.TimePlotLayer(split_data.get_port("adc"), window_size=5000, n_channels=RX_CHANNELS, ylim=(-0.1, 1), lw=1, 168 | fig_manager=fm) 169 | return data, adc 170 | 171 | 172 | def get_device_data(show_plot=True, buffer_size=BUFFER_SIZE): 173 | serial_port = serial.Serial(prt.find_serial_port(PORT), BAUD, timeout=5) 174 | serial_buffer = prt.FixedBuffer(buffer_size, use_np=True, shape=(RX_CHANNELS + 2,), axis=0) 175 | raw_data = prt.ByteSerialReadLayer.from_port(serial=serial_port, decoder=process, print_fps=True, preamble=b'UW', 176 | num_bytes=17, buffer=serial_buffer, multi_output=False) 177 | split_data = split(raw_data) 178 | # prt.PrintLayer(raw_data) 179 | if show_plot: 180 | fm = prt.FigureManager(fps=10000) 181 | if DEMO: 182 | filtered = prt.ExponentialFilter(split_data.get_port("adc"), alpha=.1, batch=True) 183 | prt.TimePlotLayer(filtered, window_size=5000, n_channels=RX_CHANNELS, ylim=(0, 0.2), lw=3, fig_manager=fm) 184 | else: 185 | # import scipy.signal 186 | # # filtered = prt.ExponentialFilter(split_data.get_port("adc"), alpha=1, batch=True) 187 | # prt.TimePlotLayer(split_data.get_port("adc"), window_size=5000, n_channels=RX_CHANNELS, ylim=(0, 1), lw=1, fig_manager=fm) 188 | # sos = scipy.signal.butter(5, [25,55], fs=472, btype="bandpass", output='sos') 189 | # filtered = prt.SOSFilter(split_data.get_port("adc"), sos, axis=0, shape=(9,)) 190 | # energy = get_energy(filtered) 191 | # 192 | # prt.TimePlotLayer(energy, window_size=1000, n_channels=1, ylim=(0, .00005), lw=1)#, fig_manager=fm) 193 | # filtered2 = prt.ExponentialFilter(energy, alpha=.3, batch=True) 194 | # prt.TimePlotLayer(filtered2, window_size=1000, n_channels=1, ylim=(0, .00005), lw=1)#, fig_manager=fm) 195 | # # prt.Spectrogram(get_channel(filtered)) 196 | # taps = get_taps(filtered2) 197 | # prt.TextPlotLayer(taps) 198 | 199 | prt.TimePlotLayer(split_data.get_port("adc"), window_size=5000, n_channels=RX_CHANNELS, ylim=(-0.1, 1), lw=1, fig_manager=fm) 200 | # prt.TimePlotLayer(diff(split_data.get_port("frame_num")), window_size=5000, n_channels=2, ylim=(0, 5), lw=1) 201 | 202 | return raw_data, split_data.get_port("adc") 203 | 204 | 205 | def encode_touch(x): 206 | return "Touch" if x else "" 207 | 208 | 209 | class ThresholdPlot(prt.TimePlotLayer): 210 | def __init__(self, port_in, thresholds, *args, **kwargs): 211 | super().__init__(port_in, *args, **kwargs) 212 | self.thresholds = thresholds 213 | self.threshold_series = [] 214 | 215 | def transform(self, data): 216 | super().transform(data) 217 | return self.thresholds 218 | 219 | def post_init(self, data): 220 | 221 | for i, threshold in enumerate(self.thresholds): 222 | handle, = self.ax.plot([], [], '-', lw=self.lw, label=f"threshold_{i}") 223 | self.threshold_series.append(handle) 224 | 225 | self.fig_manager.fig.canvas.mpl_connect('button_press_event', self.on_click) 226 | super().post_init(data) 227 | 228 | def on_click(self, event): 229 | if event.xdata > self.window_size / 2: 230 | self.thresholds[1] = event.ydata 231 | else: 232 | self.thresholds[0] = event.ydata 233 | 234 | def update_fig(self, data): 235 | for (i, series) in enumerate(self.threshold_series): 236 | series.set_data([self.x_data[0], self.x_data[-1]], [self.thresholds[i], self.thresholds[i]]) 237 | 238 | return super().update_fig(data) + self.threshold_series 239 | 240 | 241 | def detect_touch(data, show_plot=True): 242 | 243 | import scipy.signal 244 | # filtered = prt.ExponentialFilter(split_data.get_port("adc"), alpha=1, batch=True) 245 | sos = scipy.signal.butter(5, [25,80], fs=472, btype="bandpass", output='sos') 246 | filtered = prt.SOSFilter(data, sos, axis=0, shape=(9,)) 247 | energy = get_energy(filtered) 248 | 249 | smoothed_energy = prt.ExponentialFilter(energy, alpha=.2, batch=True) 250 | # prt.TimePlotLayer(filtered2, window_size=1000, n_channels=1, ylim=(0, .000005), lw=1)#, fig_manager=fm) 251 | # prt.Spectrogram(get_channel(filtered), fs=472) 252 | energy_thresholds = prt.MergeLayer(None) 253 | 254 | energy_thresholds.set_input(smoothed_energy, "energy") 255 | if show_plot: 256 | thresholds = ThresholdPlot(smoothed_energy, thresholds=[.000002, .00001], window_size=1000, n_channels=1, ylim=(0, .00008), lw=1) 257 | thresholds.fig_manager.fps = 10 258 | energy_thresholds.set_input(thresholds, key="thresholds") 259 | taps = get_taps(energy_thresholds) 260 | # prt.TextPlotLayer(taps, encoder=encode_touch) 261 | return taps, filtered, smoothed_energy 262 | -------------------------------------------------------------------------------- /pcb/ring_tx/ring_v3.BomDoc: -------------------------------------------------------------------------------- 1 | |RECORD=BOM|VERSION=2|FILENAME=C:\Users\emwhit.CSENETID\Dropbox\projects\mag-ring\pcb\ring_v2\ring_v2.$$$|KIND=ALTIUM_DESIGNER_LIVEBOM|DATE=25.01.2019|TIME=19:09:13|CURRENCY=USD|PRODUCTIONQUANTITY=1|PERUNITTARGETPRICE=0|PERUNITACTUALPRICE=0 2 | |RECORD=GeneralOptions|OPENEXPORTED=False|ADDTOPROJECT=False|FORCEFIT=False|NOTFITTED=False|TEMPLATEFILENAME=|BATCHMODE=0|SUPPLIERPRODQTY=0|SUPPLIERAUTOQTY=False|SUPPLIERUSECACHEDPRICING=False|SUPPLIERCURRENCY=|SOLUTIONSPERITEM=1|SUPPLIERSPERSOLUTION=1|DISPLAYTOTALROW=False|DISPLAYSUBTOTALSFORGROUPS=False|INCLUDEPCBPARAMETERS=False|INCLUDEDBPARAMETERS=False|INCLUDEVAULTPARAMETERS=False|INCLUDECLOUDPARAMETERS=False|INCLUDEDOCUMENTPARAMETERS=False|FILTEROUTRISKYSOLUTIONS=True|DESIGNATORGROUPING=No 3 | |RECORD=SortOrderOptions|SORTORDER= 4 | |RECORD=GroupOrderOptions|GROUPORDER= 5 | |RECORD=VisibleOrderOptions|VISIBLEORDER=Line #=71!Comment=118!Description=99!Designator=97!Quantity=84!Manufacturer 1=119!Manufacturer Part Number 1=186!Manufacturer Lifecycle 1=164!Supplier 1=92!Supplier Part Number 1=159!Supplier Unit Price 1=144!Supplier Subtotal 1=139!Value=68 6 | |RECORD=VisibleOrderOptions2|BASEVIEW=Line #=71!Comment=118!Description=99!Designator=97!Quantity=84!Manufacturer 1=119!Manufacturer Part Number 1=186!Manufacturer Lifecycle 1=164!Supplier 1=92!Supplier Part Number 1=159!Supplier Unit Price 1=144!Supplier Subtotal 1=139!Value=68|FLATVIEW=Line #=-1!Comment=-1!Description=-1!Designator=-1!Manufacturer 1=119!Manufacturer Part Number 1=186!Manufacturer Lifecycle 1=164!Supplier 1=92!Supplier Part Number 1=159!Supplier Unit Price 1=144!Supplier Subtotal 1=139|CONSOLIDATEDVIEW=Line #=-1!Comment=-1!Description=-1!Designator=-1!Quantity=-1!Manufacturer 1=119!Manufacturer Part Number 1=186!Manufacturer Lifecycle 1=164!Supplier 1=92!Supplier Part Number 1=159!Supplier Unit Price 1=144!Supplier Subtotal 1=139 7 | |RECORD=CustomColumns|CUSTOMCOLUMNS= 8 | |RECORD=ColumnsAliases|COLUMNSALIASES=Comment=Name 9 | |RECORD=FilterOptions|FILTER= 10 | |RECORD=CheckGroupCompParamsOptions|CHECKGROUPCOMPPARAMS=Comment=0,Description=0,Value=0,Footprint=1 11 | |RECORD=ManufacturerLinkOptions|MANUFACTURERLINKPARAMETERS= 12 | |RECORD=FavoriteSuppliers|SUPPLIERS=|USESUPPLIERORDER=False 13 | |RECORD=CheckOptions|CHECKLEVELS= 14 | |RECORD=LineNumberingOptions|STARTVALUE=|INCREMENT= 15 | |RECORD=Catalog|DBLINKFILEPATH= 16 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=Miscellaneous Devices.IntLib\Cap Semi|DESIGNITEMID=Cap Semi|ITEMSOURCE=Miscellaneous Devices.IntLib|STATUS=Unknown|DESCRIPTION=Capacitor (Semiconductor SIM Model)|USERCOMMENTS=|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=,"Capacitor (Semiconductor SIM Model)"|COMPONENTPARAMETERS=Comment=,"Component Kind=Standard","Description=Capacitor (Semiconductor SIM Model)",Footprint=1608[0603],"Ibis Model=",LatestRevisionDate=29-May-2009,"LatestRevisionNote=IPC-7351 Footprint Added.","Library Name=Miscellaneous Devices.IntLib","Library Reference=Cap Semi","PackageDescription=Chip Capacitor",PackageReference=C1206,PCB3D=,"Pin Info=",Published=8-Jun-2000,"Publisher=Altium Limited","Signal Integrity=Cap Semi",Simulation=CAP|CUSTOMPARAMETERS= 17 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 18 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\Cap|DESIGNITEMID=Cap|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=Ceramic Capacitor (Surface Mount)|USERCOMMENTS=Cap|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=100pF,Cap,"Ceramic Capacitor (Surface Mount)"|COMPONENTPARAMETERS=Comment=Cap,"Component Kind=Standard","Description=Ceramic Capacitor (Surface Mount)","Digi-Key Part Number=",Footprint=0402,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=Cap","Manufacturer=Generic Ceramic Chip Cap (TBD)","Manufacturer Part Number=",Note=,PCB3D=,"Pin Info=",Rating=,"Signal Integrity=",Simulation=,Value=100pF|CUSTOMPARAMETERS= 19 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 20 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\JST-PH-small|DESIGNITEMID=JST-PH-small|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=|USERCOMMENTS=JST-PH-small|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=,JST-PH-small|COMPONENTPARAMETERS=Comment=JST-PH-small,"Component Kind=Standard",Description=,Footprint=JST-PH-small,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=JST-PH-small",PCB3D=,"Pin Info=","Signal Integrity=",Simulation=|CUSTOMPARAMETERS= 21 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 22 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=Miscellaneous Connectors.IntLib\MHDR1X2|DESIGNITEMID=MHDR1X2|ITEMSOURCE=Miscellaneous Connectors.IntLib|STATUS=Unknown|DESCRIPTION=Header, 2-Pin|USERCOMMENTS=MHDR1X2|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH="Header, 2-Pin",MHDR1X2|COMPONENTPARAMETERS=Comment=MHDR1X2,"Component Kind=Standard","Description=Header, 2-Pin",Footprint=MHDR1X2,"Ibis Model=",LatestRevisionDate=17-Jul-2002,"LatestRevisionNote=Re-released for DXP Platform.","Library Name=Miscellaneous Connectors.IntLib","Library Reference=MHDR1X2",PCB3D=,"Pin Info=","Publisher=Altium Limited","Signal Integrity=Connector",Simulation=|CUSTOMPARAMETERS= 23 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 24 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\Res|DESIGNITEMID=Res|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=Chip Resistor (Surface Mount)|USERCOMMENTS=Res|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=100,"Chip Resistor (Surface Mount)",Res|COMPONENTPARAMETERS=Comment=Res,"Component Kind=Standard","Description=Chip Resistor (Surface Mount)","Digi-Key Part Number=",Footprint=0402,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=Res","Manufacturer=Generic Chip Res (TBD)","Manufacturer Part Number=",PCB3D=,"Pin Info=",Rating=,"Signal Integrity=",Simulation=,Value=100|CUSTOMPARAMETERS= 25 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 26 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\ACZ-32.768KBE-T|DESIGNITEMID=ACZ-32.768KBE-T|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=32.768kHz XO (Standard) CMOS Oscillator 3.3V Enable/Disable 4-SMD, No Lead|USERCOMMENTS=ACZ-32.768KBE-T|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH="32.768kHz XO (Standard) CMOS Oscillator 3.3V Enable/Disable 4-SMD, No Lead",ACZ-32.768KBE-T|COMPONENTPARAMETERS=Comment=ACZ-32.768KBE-T,"Component Kind=Standard",ComponentLink1Description=Digi-Key,ComponentLink1URL=https://www.digikey.com/product-detail/en/ACZ-32.768KBE-T/887-1670-2-ND/3454688?utm_campaign=buynow&WT.z_cid=ref_octopart_dkc_buynow&utm_medium=aggregator&curr=usd&site=us&utm_source=octopart,ComponentLink2Description=Datasheet,ComponentLink2URL=http://www.txccrystal.com/images/pdf/acz.pdf,"Description=32.768kHz XO (Standard) CMOS Oscillator 3.3V Enable/Disable 4-SMD, No Lead",Footprint=ACZ-32.768KBE-T,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=ACZ-32.768KBE-T",PCB3D=,"Pin Info=","Signal Integrity=",Simulation=,"Supplier 1=Arrow","Supplier Part Number 1=ACZ-32.768KBE-T"|CUSTOMPARAMETERS= 27 | |RECORD=PartChoice|PARTCHOICETYPE=AUTO_SCHLINKS|SUPPLIERSOURCE=Ciiva|MANUFACTURER=TXC|MANUFACTURERPARTNO=ACZ-32.768KBE-T|SUPPLIER=Arrow|SUPPLIERPARTNO=ACZ-32.768KBE-T|PROVIDERNAME=Ciiva|PROVIDERGUID=7A819525-F41E-4ADC-9CD0-9D9FA8B5FCE8|DESCRIPTION=TXC ACZ-32.768KBE-T OSCILLATOR, 32.768KHZ, 5 X 3.2MM, CMOS|RANK=3|AVAILABLEQUANTITY=1880|BESTUNITPRICE=0|CURRENCY=USD|PRODUCTPHOTOURL=https://ciiva.com/cloudsearch/fileimage/2111576|MANUFACTURERPACKAGING=|SUPPLIERPACKAGING=Cut Strips|LIFECYCLESTATUSID=Production|LIFECYCLESTATUS=Volume Production|PINNED=True|PRIORITY=1|LASTUPDATEDAT=43489.2643402778|LASTUPDATED=Updated 2 days ago|PRICECOUNT=7|P1=1.1632|Q1=1|P2=1.1516|Q2=10|P3=1.1399|Q3=25|P4=1.1284|Q4=100|P5=1.1168|Q5=250|P6=1.1075|Q6=500|P7=1.0959|Q7=1000 28 | |RECORD=Solution|MANUFACTURER=TXC|MANUFACTURERPARTNO=ACZ-32.768KBE-T|RANK=5 29 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 30 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\TPS70933|DESIGNITEMID=TPS70933|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=3.3V Ultra low power Voltage Regulator|USERCOMMENTS=TPS709|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH="3.3V Ultra low power Voltage Regulator",TPS709|COMPONENTPARAMETERS=Comment=TPS709,"Component Kind=Standard","Description=3.3V Ultra low power Voltage Regulator",Footprint=SOT95P280X145-5N,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=TPS70933",PCB3D=,"Pin Info=","Signal Integrity=",Simulation=,"Supplier 1=Digi-Key","Supplier Part Number 1=296-39474-1-ND"|CUSTOMPARAMETERS= 31 | |RECORD=PartChoice|PARTCHOICETYPE=AUTO_SCHLINKS|SUPPLIERSOURCE=Ciiva|MANUFACTURER=Texas Instruments|MANUFACTURERPARTNO=TPS70933DBVT|SUPPLIER=Digi-Key|SUPPLIERPARTNO=296-39474-1-ND|PROVIDERNAME=Ciiva|PROVIDERGUID=7A819525-F41E-4ADC-9CD0-9D9FA8B5FCE8|DESCRIPTION=LDO Regulator Pos 3.3V 0.15A 5-Pin SOT-23 T/R|RANK=3|AVAILABLEQUANTITY=13985|BESTUNITPRICE=0|CURRENCY=USD|PRODUCTPHOTOURL=https://ciiva.com/cloudsearch/fileimage/1956003|MANUFACTURERPACKAGING=|SUPPLIERPACKAGING=Cut Tape|LIFECYCLESTATUSID=Production|LIFECYCLESTATUS=Volume Production|PINNED=True|PRIORITY=1|LASTUPDATEDAT=43489.2408449074|LASTUPDATED=Updated 2 days ago|PRICECOUNT=3|P1=1.34|Q1=1|P2=1.2|Q2=10|P3=0.9358|Q3=100 32 | |RECORD=Solution|MANUFACTURER=Texas Instruments|MANUFACTURERPARTNO=TPS70933DBVT|RANK=5 33 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 34 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\Cap|DESIGNITEMID=Cap|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=Ceramic Capacitor (Surface Mount)|USERCOMMENTS=Cap|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=10uF,Cap,"Ceramic Capacitor (Surface Mount)"|COMPONENTPARAMETERS=Comment=Cap,"Component Kind=Standard","Description=Ceramic Capacitor (Surface Mount)","Digi-Key Part Number=",Footprint=0402,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=Cap","Manufacturer=Generic Ceramic Chip Cap (TBD)","Manufacturer Part Number=",Note=,PCB3D=,"Pin Info=",Rating=,"Signal Integrity=",Simulation=,Value=10uF|CUSTOMPARAMETERS= 35 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 36 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\Cap|DESIGNITEMID=Cap|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=Ceramic Capacitor (Surface Mount)|USERCOMMENTS=Cap|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=1uF,Cap,"Ceramic Capacitor (Surface Mount)"|COMPONENTPARAMETERS=Comment=Cap,"Component Kind=Standard","Description=Ceramic Capacitor (Surface Mount)","Digi-Key Part Number=",Footprint=0402,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=Cap","Manufacturer=Generic Ceramic Chip Cap (TBD)","Manufacturer Part Number=",Note=,PCB3D=,"Pin Info=",Rating=,"Signal Integrity=",Simulation=,Value=1uF|CUSTOMPARAMETERS= 37 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 38 | |RECORD=CatalogItem|ITEMTYPE=Automatic-NonVault|UNIQUEID=maglib.SchLib\Socket|DESIGNITEMID=Socket|ITEMSOURCE=maglib.SchLib|STATUS=Unknown|DESCRIPTION=|USERCOMMENTS=Socket|NOTE=|LINENUMBER=|SELECTEDPARAMETERSHASH=,Socket|COMPONENTPARAMETERS=Comment=Socket,"Component Kind=Standard",Description=,Footprint=socket,"Ibis Model=","Library Name=maglib.SchLib","Library Reference=Socket",PCB3D=,"Pin Info=","Signal Integrity=",Simulation=|CUSTOMPARAMETERS= 39 | |RECORD=PartChoiceGroups|GROUPBYFIELDCOUNT=0 40 | -------------------------------------------------------------------------------- /cring/mag-inverse/mag-runtime/runtime.cpp: -------------------------------------------------------------------------------- 1 | // mag-inverse.cpp : This file contains the 'main' function. Program execution begins and ends there. 2 | // 3 | 4 | #include 5 | #include "utils.h" 6 | #include "settings.h" 7 | #include "joint_ik.h" 8 | #include 9 | #include 10 | #include // std::copy 11 | #include 12 | #include "ceres/ceres.h" 13 | #include "ceres/rotation.h" 14 | #include "ceres/internal/port.h" 15 | #include "ceres/jet.h" 16 | #include "ceres/rotation.h" 17 | #include "glog/logging.h" 18 | #include "hand_models.h" 19 | #include "mag_models.h" 20 | #include "file_utils.h" 21 | #include "comms.h" 22 | #include // for high_resolution_clock 23 | #include 24 | 25 | 26 | #define SOLVE_JOINT 1 27 | 28 | constexpr unsigned int N = 0; 29 | DEFINE_string(hand_model, "", "hand model csv file to load"); 30 | DEFINE_string(calibration, "", "calibration csv file to load"); 31 | 32 | 33 | bool newFile = true; 34 | 35 | using namespace ceres; 36 | using namespace Eigen; 37 | using namespace std; 38 | 39 | 40 | 41 | 42 | template 43 | void ComputeCost(const array& sensor, const array& sensor_pred, T* residual, const Vector3& field1, const Vector3& field2, const Vector3& field3) 44 | { 45 | Eigen::Map> sensor_v(sensor.data()); 46 | Eigen::Map> sensor_pred_v(sensor_pred.data()); 47 | 48 | //cout << sensor_v << sensor_pred_v << endl; 49 | 50 | Eigen::Vector diff = (sensor_v - sensor_pred_v); 51 | for (unsigned int i = 0; i < NUM_RX * 3; i++) 52 | { 53 | Vector3 field; 54 | switch (i / 3) { 55 | case 0: 56 | field = field1; 57 | break; 58 | case 1: 59 | field = field2; 60 | break; 61 | case 2: 62 | field = field3; 63 | break; 64 | } 65 | if (field[i % 3] > 0.0025 || field[i % 3] < -0.0025) 66 | { 67 | //residual[i] = ceres::abs(diff[i]) * T(500000) * ceres::abs(field[i % 3]); 68 | residual[i] = diff[i] * T(5000000); 69 | //residual[i] = diff[i] * T(500000) * field[i % 3]; 70 | } 71 | else 72 | { 73 | residual[i] = T(0); 74 | } 75 | //print_jacobian(residual[i]); 76 | //printf("%f\n", D(residual[i])); 77 | } 78 | 79 | 80 | } 81 | 82 | double get_cost(const Eigen::Vector3d& pos, const Eigen::Quaterniond& rot, const array& sensors, const CalibrationModel& model) { 83 | Vector3d field; 84 | Eigen::Vector3d field_pred1 = forward_model(pos, rot, model.coil1, false, field); 85 | Eigen::Vector3d field_pred2 = forward_model(pos, rot, model.coil2, false, field); 86 | Eigen::Vector3d field_pred3 = forward_model(pos, rot, model.coil3, false, field); 87 | 88 | double error = 0; 89 | for (int j = 0; j < 3; j++) { 90 | error += pow(field_pred1[j] - sensors[j], 2) * 1000; 91 | error += pow(field_pred2[j] - sensors[j + 3], 2) * 1000; 92 | error += pow(field_pred3[j] - sensors[j + 6], 2) * 1000; 93 | } 94 | return error; 95 | } 96 | 97 | class PerFrameCostFunctor 98 | { 99 | public: 100 | void* operator new(size_t i) 101 | { 102 | return _mm_malloc(i, 16); 103 | } 104 | 105 | void operator delete(void* p) 106 | { 107 | _mm_free(p); 108 | } 109 | PerFrameCostFunctor(const CalibrationModel& model) : model_(model) {} 110 | 111 | template bool operator()(const T* const p_pos, const T* const p_rot, T* residual) const { 112 | Eigen::Map> pos(p_pos); 113 | Eigen::Map> rot(p_rot); 114 | 115 | 116 | CalibrationModel model_T = model_.cast(); 117 | Vector3 field1, field2, field3; 118 | 119 | Eigen::Vector3 sensor_1_pred = forward_model(pos, rot, model_T.coil1, false, field1); 120 | Eigen::Vector3 sensor_2_pred = forward_model(pos, rot, model_T.coil2, false, field2); 121 | Eigen::Vector3 sensor_3_pred = forward_model(pos, rot, model_T.coil3, false, field3); 122 | 123 | array sensor; 124 | array sensor_pred; 125 | for (unsigned int i = 0; i < NUM_RX * 3; i++) { 126 | sensor[i] = T(signed_sensors[i]); 127 | } 128 | for (unsigned int i = 0; i < 3; i++) { 129 | sensor_pred[i] = sensor_1_pred[i]; 130 | sensor_pred[i + 3] = sensor_2_pred[i]; 131 | sensor_pred[i + 6] = sensor_3_pred[i]; 132 | } 133 | 134 | ComputeCost(sensor, sensor_pred, residual, field1, field2, field3); 135 | 136 | 137 | return true; 138 | } 139 | void UpdateData(const sensor_data& sensors) { 140 | signed_sensors = sensors; 141 | } 142 | private: 143 | CalibrationModel model_; 144 | sensor_data signed_sensors; 145 | }; 146 | 147 | 148 | class SmoothPositionCostFunctor 149 | { 150 | public: 151 | void* operator new(size_t i) 152 | { 153 | return _mm_malloc(i, 16); 154 | } 155 | 156 | void operator delete(void* p) 157 | { 158 | _mm_free(p); 159 | } 160 | SmoothPositionCostFunctor(const Eigen::Vector3d& ring_pos, const Eigen::Quaterniond& ring_rot) : 161 | ring_pos_(ring_pos), ring_rot_(ring_rot) {} 162 | 163 | template bool operator()(const T* const ring_pos, const T* const ring_rot, T* residual) const { 164 | Eigen::Map> pos(ring_pos); 165 | Eigen::Map> rot(ring_rot); 166 | 167 | Eigen::Map> last_position(ring_pos_.data()); 168 | 169 | T position_error = (pos - last_position.cast()).squaredNorm(); 170 | Eigen::Quaternion rot_diff = rot.conjugate() * ring_rot_.cast(); 171 | Eigen::AngleAxis rot_diff_AA(rot_diff); 172 | T rot_error = rot_diff_AA.angle(); 173 | //residual[0] = (position_error + rot_error)*T(1); 174 | residual[0] = (position_error)* T(1); 175 | return true; 176 | } 177 | void UpdateData(const Eigen::Vector3d& ring_pos, const Eigen::Quaterniond& ring_rot) { 178 | ring_pos_ = ring_pos; 179 | ring_rot_ = ring_rot; 180 | } 181 | private: 182 | Eigen::Vector3d ring_pos_; 183 | Eigen::Quaterniond ring_rot_; 184 | }; 185 | 186 | 187 | class SmoothRotationCostFunctor 188 | { 189 | public: 190 | void* operator new(size_t i) 191 | { 192 | return _mm_malloc(i, 16); 193 | } 194 | 195 | void operator delete(void* p) 196 | { 197 | _mm_free(p); 198 | } 199 | SmoothRotationCostFunctor(const Eigen::Quaterniond& ring_rot) : ring_rot_(ring_rot) {} 200 | 201 | template bool operator()(const T* const ring_rot, T* residual) const { 202 | Eigen::Map> rot(ring_rot); 203 | 204 | Eigen::Quaternion rot_diff = rot.conjugate() * ring_rot_.cast(); 205 | Eigen::AngleAxis rot_diff_AA(rot_diff); 206 | T rot_error = rot_diff_AA.angle(); 207 | 208 | Vector3 x_proj = rot * Vector3(T(1), T(0), T(0)); 209 | 210 | residual[0] = x_proj[0];// rot_error; 211 | return true; 212 | } 213 | void UpdateData(const Eigen::Quaterniond& ring_rot) { 214 | ring_rot_ = ring_rot; 215 | } 216 | private: 217 | Eigen::Quaterniond ring_rot_; 218 | }; 219 | 220 | struct ProblemInfo 221 | { 222 | PerFrameCostFunctor* cost; 223 | ceres::Solver::Options options; 224 | ceres::Problem frame_problem; 225 | ceres::Solver::Summary summary; 226 | per_frame_opt_parameters param; 227 | }; 228 | 229 | ProblemInfo& init_problem(const CalibrationModel& calibration_model) 230 | { 231 | std::cout << "Start Per Frame Opt" << endl; 232 | 233 | static ProblemInfo problem; 234 | 235 | 236 | per_frame_opt_parameters param_lower; 237 | per_frame_opt_parameters param_upper; 238 | 239 | problem.param.ring_pos = Vector3d(-15, 0, -120); 240 | problem.param.ring_rot = Quaterniond(1, 0, 0, 0); 241 | 242 | 243 | param_lower.ring_pos = { -60, -130, -150 }; 244 | param_lower.ring_rot = Eigen::Quaterniond(-1, -1, -1, -1); 245 | 246 | param_upper.ring_pos = { 85, 90, -50 }; 247 | param_upper.ring_rot = Eigen::Quaterniond(1, 1, 1, 1); 248 | 249 | 250 | problem.frame_problem.AddParameterBlock(problem.param.ring_pos.data(), 3); 251 | problem.frame_problem.AddParameterBlock(problem.param.ring_rot.coeffs().data(), 4, new ceres::EigenQuaternionParameterization()); 252 | 253 | // set bounds on parameters 254 | for (unsigned int i = 0; i < 3; ++i) { 255 | problem.frame_problem.SetParameterLowerBound(problem.param.ring_pos.data(), i, param_lower.ring_pos[i]); 256 | problem.frame_problem.SetParameterUpperBound(problem.param.ring_pos.data(), i, param_upper.ring_pos[i]); 257 | } 258 | for (unsigned int i = 0; i < 4; ++i) { 259 | problem.frame_problem.SetParameterLowerBound(problem.param.ring_rot.coeffs().data(), i, param_lower.ring_rot.coeffs()[i]); 260 | problem.frame_problem.SetParameterUpperBound(problem.param.ring_rot.coeffs().data(), i, param_upper.ring_rot.coeffs()[i]); 261 | } 262 | 263 | 264 | HandModel null_model; 265 | problem.cost = new PerFrameCostFunctor(calibration_model); 266 | ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(problem.cost); 267 | problem.frame_problem.AddResidualBlock(cost_function, NULL, problem.param.ring_pos.data(), problem.param.ring_rot.coeffs().data()); 268 | 269 | 270 | problem.summary = Solver::Summary(); 271 | problem.options = Solver::Options(); 272 | problem.options.minimizer_progress_to_stdout = false; 273 | problem.options.num_threads = 1; 274 | problem.options.max_num_iterations = 100; 275 | 276 | return problem; 277 | } 278 | 279 | void solve_frame(ProblemInfo& problem, const sensor_data& sensors) 280 | { 281 | auto start_time = std::chrono::high_resolution_clock::now(); 282 | // Run the solver! 283 | problem.cost->UpdateData(sensors); 284 | 285 | ceres::Solve(problem.options, &problem.frame_problem, &problem.summary); 286 | 287 | auto now = std::chrono::high_resolution_clock::now(); 288 | std::chrono::duration elapsed = now - start_time; 289 | start_time = now; 290 | //printf("%f sec\n", elapsed.count()); 291 | 292 | return; 293 | } 294 | 295 | 296 | int main(int argc, char* argv[]) 297 | { 298 | gflags::ParseCommandLineFlags(&argc, &argv, true); 299 | string MODE = ""; 300 | std::cout << "RUNNING PER FRAME OPT" << endl; 301 | CalibrationModel calibration_model = {}; 302 | std::cout << "STARTED PARSING DATA" << endl; 303 | 304 | #if SOLVE_JOINT 305 | HandModel hand_model = parseHandModel(CERES_DIRECTORY + "handModel", FLAGS_hand_model); 306 | hand_model.bone_lengths[0] = 95; 307 | hand_model.bone_lengths[1] = 33; 308 | hand_model.wrist_offset = Vector3d(0, -30, 10); 309 | 310 | JointProblemInfo& joint_problem = init_per_frame_problem(hand_model, false); 311 | #endif 312 | 313 | string calib = FLAGS_calibration; 314 | calibration_model = parseCalibratedFile(CERES_DIRECTORY + "calibrate", calib, ' '); 315 | sensor_data data; 316 | int ret_val = 0; 317 | 318 | ProblemInfo& problem = init_problem(calibration_model); 319 | 320 | init_winsock(); 321 | SOCKET s; 322 | init_socket(s); 323 | SOCKET s2; 324 | init_out_socket(s2); 325 | double buffer[25]; 326 | hand_model_data solved_hand; 327 | int i = 0; 328 | while (1) 329 | { 330 | ret_val = get_sensor_data_from_socket(s, data); 331 | sensor_data temp = data; 332 | data[0] = temp[0]; 333 | data[1] = temp[4]; 334 | data[2] = temp[2]; 335 | data[3] = temp[1]; 336 | data[4] = temp[5]; 337 | data[5] = temp[3]; 338 | data[6] = temp[6]; 339 | data[7] = temp[8]; 340 | data[8] = temp[7]; 341 | 342 | if (ret_val == 0) { 343 | solve_frame(problem, data); 344 | //problem.param.ring_rot = Quaterniond(1, 0, 0, 0); 345 | #if SOLVE_JOINT 346 | DataFrame frame; 347 | frame.ring_pos = problem.param.ring_pos; 348 | frame.ring_q = problem.param.ring_rot; 349 | solve_joint_frame(joint_problem, frame); 350 | solved_hand = compute_hand_model(hand_model, joint_problem.joint_angles[0], joint_problem.joint_angles[1], joint_problem.joint_angles[2], joint_problem.joint_angles[3], Vector3d(0, 0, 0), Quaterniond(1, 0, 0, 0)); 351 | #endif 352 | i = 0; 353 | memcpy_s(buffer + i, sizeof(buffer), problem.param.ring_pos.data(), sizeof(double) * 3); 354 | i += 3; 355 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), problem.param.ring_rot.coeffs().data(), sizeof(double) * 4); 356 | i += 4; 357 | #if SOLVE_JOINT 358 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), joint_problem.joint_angles.data(), sizeof(double) * 4); 359 | i += 4; 360 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), solved_hand.ring_pos.data(), sizeof(double) * 3); 361 | i += 3; 362 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), solved_hand.ring_rot.coeffs().data(), sizeof(double) * 4); 363 | i += 4; 364 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), solved_hand.knuckle_pos.data(), sizeof(double) * 3); 365 | i += 3; 366 | memcpy_s(buffer + i, sizeof(buffer) - i * sizeof(double), solved_hand.knuckle_rot.coeffs().data(), sizeof(double) * 4); 367 | i += 4; 368 | #endif 369 | 370 | send_solution_to_socket(s2, buffer, sizeof(buffer)); 371 | } 372 | } 373 | std::cout << "Press any key to exit..."; 374 | cin.get(); 375 | } 376 | 377 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-bottom/lnk_msp430fr2422.cmd: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * Copyright (C) 2012 - 2018 Texas Instruments Incorporated - http://www.ti.com/ 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the 15 | * distribution. 16 | * 17 | * Neither the name of Texas Instruments Incorporated nor the names of 18 | * its contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * Default linker command file for Texas Instruments MSP430FR2422 34 | * 35 | *****************************************************************************/ 36 | 37 | /******************************************************************************/ 38 | /* */ 39 | /* Usage: lnk430 -o -m lnk.cmd */ 40 | /* cl430 -z -o -m lnk.cmd */ 41 | /* */ 42 | /*----------------------------------------------------------------------------*/ 43 | /* These linker options are for command line linking only. For IDE linking, */ 44 | /* you should set your linker options in Project Properties */ 45 | /* -c LINK USING C CONVENTIONS */ 46 | /* -stack 0x0100 SOFTWARE STACK SIZE */ 47 | /* -heap 0x0100 HEAP AREA SIZE */ 48 | /* */ 49 | /*----------------------------------------------------------------------------*/ 50 | /* 1.206 */ 51 | /*----------------------------------------------------------------------------*/ 52 | 53 | /****************************************************************************/ 54 | /* SPECIFY THE SYSTEM MEMORY MAP */ 55 | /****************************************************************************/ 56 | 57 | MEMORY 58 | { 59 | BSL0 : origin = 0x1000, length = 0x800 60 | INFO : origin = 0x1800, length = 0x100 61 | RAM : origin = 0x2000, length = 0x800 62 | FRAM : origin = 0xE300, length = 0x1C80 63 | BSL1 : origin = 0xFFC00, length = 0x400 64 | JTAGSIGNATURE : origin = 0xFF80, length = 0x0004, fill = 0xFFFF 65 | BSLSIGNATURE : origin = 0xFF84, length = 0x0004, fill = 0xFFFF 66 | INT00 : origin = 0xFF88, length = 0x0002 67 | INT01 : origin = 0xFF8A, length = 0x0002 68 | INT02 : origin = 0xFF8C, length = 0x0002 69 | INT03 : origin = 0xFF8E, length = 0x0002 70 | INT04 : origin = 0xFF90, length = 0x0002 71 | INT05 : origin = 0xFF92, length = 0x0002 72 | INT06 : origin = 0xFF94, length = 0x0002 73 | INT07 : origin = 0xFF96, length = 0x0002 74 | INT08 : origin = 0xFF98, length = 0x0002 75 | INT09 : origin = 0xFF9A, length = 0x0002 76 | INT10 : origin = 0xFF9C, length = 0x0002 77 | INT11 : origin = 0xFF9E, length = 0x0002 78 | INT12 : origin = 0xFFA0, length = 0x0002 79 | INT13 : origin = 0xFFA2, length = 0x0002 80 | INT14 : origin = 0xFFA4, length = 0x0002 81 | INT15 : origin = 0xFFA6, length = 0x0002 82 | INT16 : origin = 0xFFA8, length = 0x0002 83 | INT17 : origin = 0xFFAA, length = 0x0002 84 | INT18 : origin = 0xFFAC, length = 0x0002 85 | INT19 : origin = 0xFFAE, length = 0x0002 86 | INT20 : origin = 0xFFB0, length = 0x0002 87 | INT21 : origin = 0xFFB2, length = 0x0002 88 | INT22 : origin = 0xFFB4, length = 0x0002 89 | INT23 : origin = 0xFFB6, length = 0x0002 90 | INT24 : origin = 0xFFB8, length = 0x0002 91 | INT25 : origin = 0xFFBA, length = 0x0002 92 | INT26 : origin = 0xFFBC, length = 0x0002 93 | INT27 : origin = 0xFFBE, length = 0x0002 94 | INT28 : origin = 0xFFC0, length = 0x0002 95 | INT29 : origin = 0xFFC2, length = 0x0002 96 | INT30 : origin = 0xFFC4, length = 0x0002 97 | INT31 : origin = 0xFFC6, length = 0x0002 98 | INT32 : origin = 0xFFC8, length = 0x0002 99 | INT33 : origin = 0xFFCA, length = 0x0002 100 | INT34 : origin = 0xFFCC, length = 0x0002 101 | INT35 : origin = 0xFFCE, length = 0x0002 102 | INT36 : origin = 0xFFD0, length = 0x0002 103 | INT37 : origin = 0xFFD2, length = 0x0002 104 | INT38 : origin = 0xFFD4, length = 0x0002 105 | INT39 : origin = 0xFFD6, length = 0x0002 106 | INT40 : origin = 0xFFD8, length = 0x0002 107 | INT41 : origin = 0xFFDA, length = 0x0002 108 | INT42 : origin = 0xFFDC, length = 0x0002 109 | INT43 : origin = 0xFFDE, length = 0x0002 110 | INT44 : origin = 0xFFE0, length = 0x0002 111 | INT45 : origin = 0xFFE2, length = 0x0002 112 | INT46 : origin = 0xFFE4, length = 0x0002 113 | INT47 : origin = 0xFFE6, length = 0x0002 114 | INT48 : origin = 0xFFE8, length = 0x0002 115 | INT49 : origin = 0xFFEA, length = 0x0002 116 | INT50 : origin = 0xFFEC, length = 0x0002 117 | INT51 : origin = 0xFFEE, length = 0x0002 118 | INT52 : origin = 0xFFF0, length = 0x0002 119 | INT53 : origin = 0xFFF2, length = 0x0002 120 | INT54 : origin = 0xFFF4, length = 0x0002 121 | INT55 : origin = 0xFFF6, length = 0x0002 122 | INT56 : origin = 0xFFF8, length = 0x0002 123 | INT57 : origin = 0xFFFA, length = 0x0002 124 | INT58 : origin = 0xFFFC, length = 0x0002 125 | RESET : origin = 0xFFFE, length = 0x0002 126 | } 127 | 128 | /****************************************************************************/ 129 | /* SPECIFY THE SECTIONS ALLOCATION INTO MEMORY */ 130 | /****************************************************************************/ 131 | 132 | SECTIONS 133 | { 134 | GROUP(ALL_FRAM) 135 | { 136 | GROUP(READ_WRITE_MEMORY) 137 | { 138 | .TI.persistent : {} /* For #pragma persistent */ 139 | } 140 | 141 | GROUP(READ_ONLY_MEMORY) 142 | { 143 | .cinit : {} /* Initialization tables */ 144 | .pinit : {} /* C++ constructor tables */ 145 | .binit : {} /* Boot-time Initialization tables */ 146 | .init_array : {} /* C++ constructor tables */ 147 | .mspabi.exidx : {} /* C++ constructor tables */ 148 | .mspabi.extab : {} /* C++ constructor tables */ 149 | .const : {} /* Constant data */ 150 | } 151 | 152 | GROUP(EXECUTABLE_MEMORY) 153 | { 154 | .text : {} /* Code */ 155 | .text:_isr : {} /* Code ISRs */ 156 | } 157 | } > FRAM 158 | 159 | #ifdef __TI_COMPILER_VERSION__ 160 | #if __TI_COMPILER_VERSION__ >= 15009000 161 | .TI.ramfunc : {} load=FRAM, run=RAM, table(BINIT) 162 | #endif 163 | #endif 164 | 165 | .jtagsignature : {} > JTAGSIGNATURE 166 | .bslsignature : {} > BSLSIGNATURE 167 | 168 | .cio : {} > RAM /* C I/O buffer */ 169 | .sysmem : {} > RAM /* Dynamic memory allocation area */ 170 | .bss : {} > RAM /* Global & static vars */ 171 | .data : {} > RAM /* Global & static vars */ 172 | .TI.noinit : {} > RAM /* For #pragma noinit */ 173 | .stack : {} > RAM (HIGH) /* Software system stack */ 174 | 175 | /* MSP430 INFO memory segments */ 176 | .info : type = NOINIT{} > INFO 177 | 178 | 179 | /* MSP430 interrupt vectors */ 180 | 181 | .int00 : {} > INT00 182 | .int01 : {} > INT01 183 | .int02 : {} > INT02 184 | .int03 : {} > INT03 185 | .int04 : {} > INT04 186 | .int05 : {} > INT05 187 | .int06 : {} > INT06 188 | .int07 : {} > INT07 189 | .int08 : {} > INT08 190 | .int09 : {} > INT09 191 | .int10 : {} > INT10 192 | .int11 : {} > INT11 193 | .int12 : {} > INT12 194 | .int13 : {} > INT13 195 | .int14 : {} > INT14 196 | .int15 : {} > INT15 197 | .int16 : {} > INT16 198 | .int17 : {} > INT17 199 | .int18 : {} > INT18 200 | .int19 : {} > INT19 201 | .int20 : {} > INT20 202 | .int21 : {} > INT21 203 | .int22 : {} > INT22 204 | .int23 : {} > INT23 205 | .int24 : {} > INT24 206 | .int25 : {} > INT25 207 | .int26 : {} > INT26 208 | .int27 : {} > INT27 209 | .int28 : {} > INT28 210 | .int29 : {} > INT29 211 | .int30 : {} > INT30 212 | .int31 : {} > INT31 213 | .int32 : {} > INT32 214 | .int33 : {} > INT33 215 | .int34 : {} > INT34 216 | .int35 : {} > INT35 217 | .int36 : {} > INT36 218 | .int37 : {} > INT37 219 | .int38 : {} > INT38 220 | .int39 : {} > INT39 221 | .int40 : {} > INT40 222 | .int41 : {} > INT41 223 | .int42 : {} > INT42 224 | .int43 : {} > INT43 225 | .int44 : {} > INT44 226 | .int45 : {} > INT45 227 | PORT2 : { * ( .int46 ) } > INT46 type = VECT_INIT 228 | PORT1 : { * ( .int47 ) } > INT47 type = VECT_INIT 229 | ADC : { * ( .int48 ) } > INT48 type = VECT_INIT 230 | EUSCI_B0 : { * ( .int49 ) } > INT49 type = VECT_INIT 231 | EUSCI_A0 : { * ( .int50 ) } > INT50 type = VECT_INIT 232 | WDT : { * ( .int51 ) } > INT51 type = VECT_INIT 233 | RTC : { * ( .int52 ) } > INT52 type = VECT_INIT 234 | TIMER1_A1 : { * ( .int53 ) } > INT53 type = VECT_INIT 235 | TIMER1_A0 : { * ( .int54 ) } > INT54 type = VECT_INIT 236 | TIMER0_A1 : { * ( .int55 ) } > INT55 type = VECT_INIT 237 | TIMER0_A0 : { * ( .int56 ) } > INT56 type = VECT_INIT 238 | UNMI : { * ( .int57 ) } > INT57 type = VECT_INIT 239 | SYSNMI : { * ( .int58 ) } > INT58 type = VECT_INIT 240 | .reset : {} > RESET /* MSP430 reset vector */ 241 | 242 | } 243 | 244 | /****************************************************************************/ 245 | /* INCLUDE PERIPHERALS MEMORY MAP */ 246 | /****************************************************************************/ 247 | 248 | -l msp430fr2422.cmd 249 | 250 | 251 | -------------------------------------------------------------------------------- /firmware/AuraRing-Rx-top/lnk_msp430fr2422.cmd: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * Copyright (C) 2012 - 2018 Texas Instruments Incorporated - http://www.ti.com/ 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the 15 | * distribution. 16 | * 17 | * Neither the name of Texas Instruments Incorporated nor the names of 18 | * its contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * Default linker command file for Texas Instruments MSP430FR2422 34 | * 35 | *****************************************************************************/ 36 | 37 | /******************************************************************************/ 38 | /* */ 39 | /* Usage: lnk430 -o -m lnk.cmd */ 40 | /* cl430 -z -o -m lnk.cmd */ 41 | /* */ 42 | /*----------------------------------------------------------------------------*/ 43 | /* These linker options are for command line linking only. For IDE linking, */ 44 | /* you should set your linker options in Project Properties */ 45 | /* -c LINK USING C CONVENTIONS */ 46 | /* -stack 0x0100 SOFTWARE STACK SIZE */ 47 | /* -heap 0x0100 HEAP AREA SIZE */ 48 | /* */ 49 | /*----------------------------------------------------------------------------*/ 50 | /* 1.206 */ 51 | /*----------------------------------------------------------------------------*/ 52 | 53 | /****************************************************************************/ 54 | /* SPECIFY THE SYSTEM MEMORY MAP */ 55 | /****************************************************************************/ 56 | 57 | MEMORY 58 | { 59 | BSL0 : origin = 0x1000, length = 0x800 60 | INFO : origin = 0x1800, length = 0x100 61 | RAM : origin = 0x2000, length = 0x800 62 | FRAM : origin = 0xE300, length = 0x1C80 63 | BSL1 : origin = 0xFFC00, length = 0x400 64 | JTAGSIGNATURE : origin = 0xFF80, length = 0x0004, fill = 0xFFFF 65 | BSLSIGNATURE : origin = 0xFF84, length = 0x0004, fill = 0xFFFF 66 | INT00 : origin = 0xFF88, length = 0x0002 67 | INT01 : origin = 0xFF8A, length = 0x0002 68 | INT02 : origin = 0xFF8C, length = 0x0002 69 | INT03 : origin = 0xFF8E, length = 0x0002 70 | INT04 : origin = 0xFF90, length = 0x0002 71 | INT05 : origin = 0xFF92, length = 0x0002 72 | INT06 : origin = 0xFF94, length = 0x0002 73 | INT07 : origin = 0xFF96, length = 0x0002 74 | INT08 : origin = 0xFF98, length = 0x0002 75 | INT09 : origin = 0xFF9A, length = 0x0002 76 | INT10 : origin = 0xFF9C, length = 0x0002 77 | INT11 : origin = 0xFF9E, length = 0x0002 78 | INT12 : origin = 0xFFA0, length = 0x0002 79 | INT13 : origin = 0xFFA2, length = 0x0002 80 | INT14 : origin = 0xFFA4, length = 0x0002 81 | INT15 : origin = 0xFFA6, length = 0x0002 82 | INT16 : origin = 0xFFA8, length = 0x0002 83 | INT17 : origin = 0xFFAA, length = 0x0002 84 | INT18 : origin = 0xFFAC, length = 0x0002 85 | INT19 : origin = 0xFFAE, length = 0x0002 86 | INT20 : origin = 0xFFB0, length = 0x0002 87 | INT21 : origin = 0xFFB2, length = 0x0002 88 | INT22 : origin = 0xFFB4, length = 0x0002 89 | INT23 : origin = 0xFFB6, length = 0x0002 90 | INT24 : origin = 0xFFB8, length = 0x0002 91 | INT25 : origin = 0xFFBA, length = 0x0002 92 | INT26 : origin = 0xFFBC, length = 0x0002 93 | INT27 : origin = 0xFFBE, length = 0x0002 94 | INT28 : origin = 0xFFC0, length = 0x0002 95 | INT29 : origin = 0xFFC2, length = 0x0002 96 | INT30 : origin = 0xFFC4, length = 0x0002 97 | INT31 : origin = 0xFFC6, length = 0x0002 98 | INT32 : origin = 0xFFC8, length = 0x0002 99 | INT33 : origin = 0xFFCA, length = 0x0002 100 | INT34 : origin = 0xFFCC, length = 0x0002 101 | INT35 : origin = 0xFFCE, length = 0x0002 102 | INT36 : origin = 0xFFD0, length = 0x0002 103 | INT37 : origin = 0xFFD2, length = 0x0002 104 | INT38 : origin = 0xFFD4, length = 0x0002 105 | INT39 : origin = 0xFFD6, length = 0x0002 106 | INT40 : origin = 0xFFD8, length = 0x0002 107 | INT41 : origin = 0xFFDA, length = 0x0002 108 | INT42 : origin = 0xFFDC, length = 0x0002 109 | INT43 : origin = 0xFFDE, length = 0x0002 110 | INT44 : origin = 0xFFE0, length = 0x0002 111 | INT45 : origin = 0xFFE2, length = 0x0002 112 | INT46 : origin = 0xFFE4, length = 0x0002 113 | INT47 : origin = 0xFFE6, length = 0x0002 114 | INT48 : origin = 0xFFE8, length = 0x0002 115 | INT49 : origin = 0xFFEA, length = 0x0002 116 | INT50 : origin = 0xFFEC, length = 0x0002 117 | INT51 : origin = 0xFFEE, length = 0x0002 118 | INT52 : origin = 0xFFF0, length = 0x0002 119 | INT53 : origin = 0xFFF2, length = 0x0002 120 | INT54 : origin = 0xFFF4, length = 0x0002 121 | INT55 : origin = 0xFFF6, length = 0x0002 122 | INT56 : origin = 0xFFF8, length = 0x0002 123 | INT57 : origin = 0xFFFA, length = 0x0002 124 | INT58 : origin = 0xFFFC, length = 0x0002 125 | RESET : origin = 0xFFFE, length = 0x0002 126 | } 127 | 128 | /****************************************************************************/ 129 | /* SPECIFY THE SECTIONS ALLOCATION INTO MEMORY */ 130 | /****************************************************************************/ 131 | 132 | SECTIONS 133 | { 134 | GROUP(ALL_FRAM) 135 | { 136 | GROUP(READ_WRITE_MEMORY) 137 | { 138 | .TI.persistent : {} /* For #pragma persistent */ 139 | } 140 | 141 | GROUP(READ_ONLY_MEMORY) 142 | { 143 | .cinit : {} /* Initialization tables */ 144 | .pinit : {} /* C++ constructor tables */ 145 | .binit : {} /* Boot-time Initialization tables */ 146 | .init_array : {} /* C++ constructor tables */ 147 | .mspabi.exidx : {} /* C++ constructor tables */ 148 | .mspabi.extab : {} /* C++ constructor tables */ 149 | .const : {} /* Constant data */ 150 | } 151 | 152 | GROUP(EXECUTABLE_MEMORY) 153 | { 154 | .text : {} /* Code */ 155 | .text:_isr : {} /* Code ISRs */ 156 | } 157 | } > FRAM 158 | 159 | #ifdef __TI_COMPILER_VERSION__ 160 | #if __TI_COMPILER_VERSION__ >= 15009000 161 | .TI.ramfunc : {} load=FRAM, run=RAM, table(BINIT) 162 | #endif 163 | #endif 164 | 165 | .jtagsignature : {} > JTAGSIGNATURE 166 | .bslsignature : {} > BSLSIGNATURE 167 | 168 | .cio : {} > RAM /* C I/O buffer */ 169 | .sysmem : {} > RAM /* Dynamic memory allocation area */ 170 | .bss : {} > RAM /* Global & static vars */ 171 | .data : {} > RAM /* Global & static vars */ 172 | .TI.noinit : {} > RAM /* For #pragma noinit */ 173 | .stack : {} > RAM (HIGH) /* Software system stack */ 174 | 175 | /* MSP430 INFO memory segments */ 176 | .info : type = NOINIT{} > INFO 177 | 178 | 179 | /* MSP430 interrupt vectors */ 180 | 181 | .int00 : {} > INT00 182 | .int01 : {} > INT01 183 | .int02 : {} > INT02 184 | .int03 : {} > INT03 185 | .int04 : {} > INT04 186 | .int05 : {} > INT05 187 | .int06 : {} > INT06 188 | .int07 : {} > INT07 189 | .int08 : {} > INT08 190 | .int09 : {} > INT09 191 | .int10 : {} > INT10 192 | .int11 : {} > INT11 193 | .int12 : {} > INT12 194 | .int13 : {} > INT13 195 | .int14 : {} > INT14 196 | .int15 : {} > INT15 197 | .int16 : {} > INT16 198 | .int17 : {} > INT17 199 | .int18 : {} > INT18 200 | .int19 : {} > INT19 201 | .int20 : {} > INT20 202 | .int21 : {} > INT21 203 | .int22 : {} > INT22 204 | .int23 : {} > INT23 205 | .int24 : {} > INT24 206 | .int25 : {} > INT25 207 | .int26 : {} > INT26 208 | .int27 : {} > INT27 209 | .int28 : {} > INT28 210 | .int29 : {} > INT29 211 | .int30 : {} > INT30 212 | .int31 : {} > INT31 213 | .int32 : {} > INT32 214 | .int33 : {} > INT33 215 | .int34 : {} > INT34 216 | .int35 : {} > INT35 217 | .int36 : {} > INT36 218 | .int37 : {} > INT37 219 | .int38 : {} > INT38 220 | .int39 : {} > INT39 221 | .int40 : {} > INT40 222 | .int41 : {} > INT41 223 | .int42 : {} > INT42 224 | .int43 : {} > INT43 225 | .int44 : {} > INT44 226 | .int45 : {} > INT45 227 | PORT2 : { * ( .int46 ) } > INT46 type = VECT_INIT 228 | PORT1 : { * ( .int47 ) } > INT47 type = VECT_INIT 229 | ADC : { * ( .int48 ) } > INT48 type = VECT_INIT 230 | EUSCI_B0 : { * ( .int49 ) } > INT49 type = VECT_INIT 231 | EUSCI_A0 : { * ( .int50 ) } > INT50 type = VECT_INIT 232 | WDT : { * ( .int51 ) } > INT51 type = VECT_INIT 233 | RTC : { * ( .int52 ) } > INT52 type = VECT_INIT 234 | TIMER1_A1 : { * ( .int53 ) } > INT53 type = VECT_INIT 235 | TIMER1_A0 : { * ( .int54 ) } > INT54 type = VECT_INIT 236 | TIMER0_A1 : { * ( .int55 ) } > INT55 type = VECT_INIT 237 | TIMER0_A0 : { * ( .int56 ) } > INT56 type = VECT_INIT 238 | UNMI : { * ( .int57 ) } > INT57 type = VECT_INIT 239 | SYSNMI : { * ( .int58 ) } > INT58 type = VECT_INIT 240 | .reset : {} > RESET /* MSP430 reset vector */ 241 | 242 | } 243 | 244 | /****************************************************************************/ 245 | /* INCLUDE PERIPHERALS MEMORY MAP */ 246 | /****************************************************************************/ 247 | 248 | -l msp430fr2422.cmd 249 | 250 | 251 | -------------------------------------------------------------------------------- /cring/mag-inverse/file_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "file_utils.h" 2 | #include "settings.h" 3 | #include 4 | #include 5 | 6 | vector parseCSVFile(string fileName, string trial, char delimiter, unsigned int limit) 7 | { 8 | string full_filename = fileName + "__" + trial + ".csv"; 9 | ifstream file(full_filename); 10 | 11 | vector data; 12 | unsigned int row = 0; 13 | std::string line; 14 | while (!file.eof()) { 15 | std::getline(file, line); 16 | if (line == "") { 17 | break; 18 | } 19 | std::stringstream iss(line); 20 | vector parsed_line; 21 | while (!iss.eof()) 22 | { 23 | std::string val; 24 | std::getline(iss, val, delimiter); 25 | parsed_line.push_back(stod(val)); 26 | } 27 | /*if (parsed_line.size() < (9 + 3 + 4)) { 28 | break; 29 | }*/ 30 | unsigned int i = 0; 31 | DataFrame frame; 32 | for (i = 0; i < 9; i++) { 33 | frame.sensors[i] = parsed_line[i]; 34 | } 35 | #if USE_POS 36 | frame.ring_pos = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 37 | i += 3; 38 | 39 | frame.ring_q = Eigen::Quaterniond(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]); 40 | i += 4; 41 | #endif 42 | i += 3; // wrist pos 43 | i += 4; // wrist rot 44 | #ifdef USE_PALM 45 | frame.palm_pos = Eigen::Vector3(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 46 | i += 3; 47 | frame.palm_q = Eigen::Quaternion(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]); 48 | i += 4; 49 | #endif 50 | /*#ifdef USE_MARKERS 51 | vector markers(parsed_line.begin() + i, parsed_line.end()); 52 | frame.markers = markers; 53 | #endif*/ 54 | 55 | if (parsed_line.size() > i) { 56 | for (int j = 0; j < 4; j++) { 57 | frame.joint_angles[j] = parsed_line[i + j]; 58 | } 59 | } 60 | data.push_back(frame); 61 | 62 | row++; 63 | if ((limit > 0) && (row > limit)) { 64 | break; 65 | } 66 | } 67 | 68 | if (data.size() == 0) { 69 | printf("File not found: %s\n", full_filename.c_str()); 70 | cin.get(); 71 | exit(1); 72 | } 73 | return data; 74 | } 75 | 76 | 77 | vector parseProjectedCSVFile(string fileName, string trial, char delimiter, unsigned int limit) 78 | { 79 | string full_filename = fileName + "__" + trial + ".csv"; 80 | ifstream file(full_filename); 81 | 82 | vector data; 83 | unsigned int row = 0; 84 | std::string line; 85 | while (!file.eof()) { 86 | std::getline(file, line); 87 | if (line == "") { 88 | break; 89 | } 90 | std::stringstream iss(line); 91 | vector parsed_line; 92 | while (!iss.eof()) 93 | { 94 | std::string val; 95 | std::getline(iss, val, delimiter); 96 | try { 97 | parsed_line.push_back(stod(val)); 98 | } 99 | catch (std::invalid_argument&) { 100 | continue; 101 | } 102 | catch (std::out_of_range&) { 103 | break; 104 | } 105 | } 106 | 107 | unsigned int i = 0; 108 | DataFrame frame; 109 | 110 | frame.ring_pos = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 111 | i += 3; 112 | 113 | frame.ring_q = Eigen::Quaterniond(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]).conjugate(); 114 | i += 4; 115 | 116 | 117 | data.push_back(frame); 118 | 119 | row++; 120 | if ((limit > 0) && (row > limit)) { 121 | break; 122 | } 123 | } 124 | return data; 125 | } 126 | 127 | CalibrationModel parseCalibratedFile(string fileName, string trial, char delimiter = ' ') 128 | { 129 | ifstream file(fileName + "__" + trial + ".csv"); 130 | CalibrationModel calibrated = {}; 131 | int row = 0; 132 | std::string line; 133 | while (!file.eof()) { 134 | std::getline(file, line); 135 | if (line == "") { 136 | break; 137 | } 138 | std::stringstream iss(line); 139 | vector parsed_line; 140 | while (!iss.eof()) 141 | { 142 | std::string val; 143 | std::getline(iss, val, delimiter); 144 | parsed_line.push_back(stof(val)); 145 | } 146 | if (parsed_line.size() < (1 + 3 + 3 + 3 + 3 + 3 + 4 + 3 + 4 + 3 + 3 + 4)) { 147 | break; 148 | } 149 | unsigned int i = 0; 150 | CoilCalibrationModel frame; 151 | frame.global_gain = parsed_line[0]; 152 | i += 1; 153 | frame.gain = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 154 | i += 3; 155 | frame.per_channel_gain = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 156 | i += 3; 157 | frame.bias = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 158 | i += 3; 159 | frame.noise = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 160 | i += 3; 161 | frame.sensor_offset = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 162 | i += 3; 163 | frame.sensor_rot_offset = Eigen::Quaterniond(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]); 164 | i += 4; 165 | frame.ring_offset = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 166 | i += 3; 167 | frame.ring_rot_offset = Eigen::Quaterniond(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]); 168 | i += 4; 169 | frame.crosstalk = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2] }; 170 | i += 3; 171 | frame.base_sensor_offset = Eigen::Vector3d(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]); 172 | i += 3; 173 | frame.base_sensor_rot_offset = Eigen::Quaterniond(parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3]); 174 | i += 4; 175 | if (row == 0) { 176 | calibrated.coil1 = frame; 177 | } 178 | else if(row == 1) 179 | { 180 | calibrated.coil2 = frame; 181 | } 182 | else 183 | { 184 | calibrated.coil3 = frame; 185 | } 186 | row++; 187 | } 188 | return calibrated; 189 | } 190 | 191 | void writePerFrameResults(string fileName, string trial, const Eigen::Vector3d& ring_pos, const Eigen::Quaterniond& ring_rot, const Eigen::Vector3d& ring_pos_actual, const Eigen::Quaterniond& ring_rot_actual, const per_frame_opt_parameters& hand_model, 192 | bool newFile) 193 | { 194 | // file pointer 195 | fstream fout; 196 | 197 | if (newFile) { 198 | fout.open(fileName + "__" + trial + ".csv", ios::out); 199 | newFile = 0; 200 | } 201 | else { 202 | // opens an existing csv file or creates a new file. 203 | fout.open(fileName + "__" + trial + ".csv", ios::app); 204 | } 205 | 206 | fout << ring_pos[0] << " " << ring_pos[1] << " " << ring_pos[2] << " "; 207 | fout << ring_rot.w() << " " << ring_rot.x() << " " << ring_rot.y() << " " << ring_rot.z() << " "; 208 | fout << ring_pos_actual[0] << " " << ring_pos_actual[1] << " " << ring_pos_actual[2] << " "; 209 | fout << ring_rot_actual.w() << " " << ring_rot_actual.x() << " " << ring_rot_actual.y() << " " << ring_rot_actual.z() << " "; 210 | #if USE_JOINT 211 | fout << hand_model.joint_angles[0] << " " << hand_model.joint_angles[1] << " " << hand_model.joint_angles[2] << " " << hand_model.joint_angles[3] << " "; 212 | fout << hand_model.hand_model.wrist_offset[0] << " " << hand_model.hand_model.wrist_offset[1] << " " << hand_model.hand_model.wrist_offset[2] << " "; 213 | fout << hand_model.hand_model.wrist_offset_rot.w() << " " << hand_model.hand_model.wrist_offset_rot.x() << " " << hand_model.hand_model.wrist_offset_rot.y() << " " << hand_model.hand_model.wrist_offset_rot.z() << " "; 214 | fout << hand_model.hand_model.bone_lengths[0] << " " << hand_model.hand_model.bone_lengths[1] << " "; 215 | fout << hand_model.slack_pos[0] << " " << hand_model.slack_pos[1] << " " << hand_model.slack_pos[2] << " "; 216 | fout << hand_model.slack_rot.w() << " " << hand_model.slack_rot.x() << " " << hand_model.slack_rot.y() << " " << hand_model.slack_rot.z(); 217 | #endif 218 | fout << endl; 219 | fout.close(); 220 | } 221 | 222 | void writeCalibration(string fileName, string trial, const CoilCalibrationModel& coil, bool newFile) 223 | { 224 | // file pointer 225 | fstream fout; 226 | if (newFile) { 227 | fout.open(fileName + "__" + trial + ".csv", ios::out); 228 | } 229 | else { 230 | // opens an existing csv file or creates a new file. 231 | fout.open(fileName + "__" + trial + ".csv", ios::app); 232 | } 233 | fout << coil.global_gain << " " << coil.gain[0] << " " << coil.gain[1] << " " << coil.gain[2] << " " << coil.per_channel_gain[0] << " "; 234 | fout << coil.per_channel_gain[1] << " " << coil.per_channel_gain[2] << " " << coil.bias[0] << " " << coil.bias[1] << " " << coil.bias[2] << " "; 235 | fout << coil.noise[0] << " " << coil.noise[1] << " " << coil.noise[2] << " " << coil.sensor_offset[0] << " " << coil.sensor_offset[1] << " " << coil.sensor_offset[2] << " "; 236 | fout << coil.sensor_rot_offset.w() << " " << coil.sensor_rot_offset.x() << " " << coil.sensor_rot_offset.y() << " " << coil.sensor_rot_offset.z() << " "; 237 | fout << coil.ring_offset[0] << " " << coil.ring_offset[1] << " " << coil.ring_offset[2] << " "; 238 | fout << coil.ring_rot_offset.w() << " " << coil.ring_rot_offset.x() << " " << coil.ring_rot_offset.y() << " " << coil.ring_rot_offset.z() << " "; 239 | fout << coil.crosstalk[0] << " " << coil.crosstalk[1] << " " << coil.crosstalk[2] << " "; 240 | fout << coil.base_sensor_offset[0] << " " << coil.base_sensor_offset[1] << " " << coil.base_sensor_offset[2] << " "; 241 | fout << coil.base_sensor_rot_offset.w() << " " << coil.base_sensor_rot_offset.x() << " " << coil.base_sensor_rot_offset.y() << " " << coil.base_sensor_rot_offset.z(); 242 | fout << endl; 243 | 244 | fout.close(); 245 | } 246 | 247 | void writeHandModel(string fileName, string trial, const HandModel& hand_model) 248 | { 249 | // file pointer 250 | fstream fout; 251 | 252 | // opens an existing csv file or creates a new file. 253 | fout.open(fileName + "__" + trial + ".csv", ios::out); 254 | 255 | string name; 256 | fout << "wrist_len" << " " << "finger_len" << " " << "wrist_offset_x" << " " << "wrist_offset_y" << " " << "wrist_offset_z" << " " << "rx_rot_offset_k" << " " << "rx_rot_offset_x" << " " << "rx_rot_offset_y" << " " << "rx_rot_offset_z" << " " 257 | << "palm_rot_offset_k" << " " << "palm_rot_offset_x" << " " << "palm_rot_offset_y" << " " << "palm_rot_offset_z" << " " 258 | << "palm_offset_x" << " " << "palm_offset_y" << " " << "palm_offset_z" << " " 259 | << "base_slack_x" << " " << "base_slack_y" << " " << "base_slack_z" << " " << "base_slack_rot_w" << " " << "base_slack_rot_x" << " " << "base_slack_rot_y" << " " << "base_slack_rot_z" << " " 260 | << "cmc_alpha" <<"\n"; 261 | 262 | fout << hand_model.bone_lengths[0] << " " << hand_model.bone_lengths[1] << " "; 263 | fout << hand_model.wrist_offset[0] << " " << hand_model.wrist_offset[1] << " " << hand_model.wrist_offset[2] << " "; 264 | fout << hand_model.wrist_offset_rot.w() << " " << hand_model.wrist_offset_rot.x() << " " << hand_model.wrist_offset_rot.y() << " " << hand_model.wrist_offset_rot.z() << " "; 265 | fout << hand_model.palm_offset.x() << " " << hand_model.palm_offset.y() << " " << hand_model.palm_offset.z() << " "; 266 | fout << hand_model.palm_offset_rot.w() << " " << hand_model.palm_offset_rot.x() << " " << hand_model.palm_offset_rot.y() << " " << hand_model.palm_offset_rot.z() << " "; 267 | fout << hand_model.base_slack_pos[0] << " " << hand_model.base_slack_pos[1] << " " << hand_model.base_slack_pos[2] << " "; 268 | fout << hand_model.base_slack_rot.w() << " " << hand_model.base_slack_rot.x() << " " << hand_model.base_slack_rot.y() << " " << hand_model.base_slack_rot.z() << " "; 269 | fout << hand_model.cmc_alpha << "\n"; 270 | 271 | fout.close(); 272 | } 273 | 274 | void writeCSVFile(string fileName, string trial, vector> angles, vector> joints, vector raw_data) 275 | { 276 | // file pointer 277 | fstream fout; 278 | hand_model_data data; 279 | // opens an existing csv file or creates a new file. 280 | fout.open(fileName + "__" + trial + ".csv", ios::out); 281 | 282 | string name; 283 | fout << "wrist_theta wrist_phi finger_theta finger_phi "; 284 | fout << "ring_x ring_y ring_z ring_qw ring_qx ring_qy ring_qz "; 285 | fout << "knuckle_x knuckle_y knuckle_z knuckle_qw knuckle_qx knuckle_qy knuckle_qz "; 286 | fout << "wrist_x wrist_y wrist_z wrist_qw wrist_qx wrist_qy wrist_qz "; 287 | fout << "palm_x palm_y palm_z palm_qw palm_qx palm_qy palm_qz\n"; 288 | for (unsigned int i = 0; i < angles.size(); i++) { 289 | // Insert the data to file 290 | fout << angles[i][0] << " " << angles[i][1] << " " << angles[i][2] << " " << angles[i][3]; 291 | data = joints[i]; 292 | fout << " " << data.ring_pos[0] << " " << data.ring_pos[1] << " " << data.ring_pos[2]; 293 | fout << " " << data.ring_rot.w() << " " << data.ring_rot.x() << " " << data.ring_rot.y() << " " << data.ring_rot.z(); 294 | fout << " " << data.knuckle_pos[0] << " " << data.knuckle_pos[1] << " " << data.knuckle_pos[2]; 295 | fout << " " << data.knuckle_rot.w() << " " << data.knuckle_rot.x() << " " << data.knuckle_rot.y() << " " << data.knuckle_rot.z(); 296 | fout << " " << data.wrist_pos[0] << " " << data.wrist_pos[1] << " " << data.wrist_pos[2]; 297 | fout << " " << data.wrist_rot.w() << " " << data.wrist_rot.x() << " " << data.wrist_rot.y() << " " << data.wrist_rot.z(); 298 | fout << " " << data.palm_pos[0] << " " << data.palm_pos[1] << " " << data.palm_pos[2]; 299 | fout << " " << data.palm_rot.w() << " " << data.palm_rot.x() << " " << data.palm_rot.y() << " " << data.palm_rot.z(); 300 | fout << "\n"; 301 | } 302 | fout.close(); 303 | } 304 | 305 | 306 | HandModel parseHandModel(string fileName, string trial) { 307 | ifstream file(fileName + "__" + trial + ".csv"); 308 | if (file.fail()) { 309 | std::cout << "Hand model file does not exist"; 310 | throw std::runtime_error("Hand model file does not exist"); 311 | } 312 | HandModel data; 313 | int row = 0; 314 | std::string line; 315 | while (!file.eof()) { 316 | std::getline(file, line); 317 | if (line == "" || line[0] == 'w') { 318 | continue; 319 | } 320 | std::stringstream iss(line); 321 | vector parsed_line; 322 | while (!iss.eof()) 323 | { 324 | std::string val; 325 | std::getline(iss, val, ' '); 326 | parsed_line.push_back(stof(val)); 327 | } 328 | unsigned int i = 0; 329 | data.bone_lengths = { parsed_line[i], parsed_line[i + 1] }; 330 | i += 2; 331 | data.wrist_offset = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2] }; 332 | i += 3; 333 | data.wrist_offset_rot = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3] }; 334 | i += 4; 335 | data.palm_offset = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2]}; 336 | i += 3; 337 | data.palm_offset_rot = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3] }; 338 | i += 4; 339 | data.base_slack_pos = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2] }; 340 | i += 3; 341 | data.base_slack_rot = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3] }; 342 | i += 4; 343 | data.cmc_alpha = parsed_line[i]; 344 | } 345 | return data; 346 | } 347 | 348 | vector> parseMatlab(string fileName, string trial) { 349 | ifstream file(fileName + "__" + trial + ".csv"); 350 | vector> data; 351 | int row = 0; 352 | std::string line; 353 | while (!file.eof()) { 354 | std::getline(file, line); 355 | if (line == "") { 356 | break; 357 | } 358 | std::stringstream iss(line); 359 | vector parsed_line; 360 | while (!iss.eof()) 361 | { 362 | std::string val; 363 | std::getline(iss, val, ','); 364 | parsed_line.push_back(stof(val)); 365 | } 366 | unsigned int i = 4; 367 | array frame; 368 | frame = { parsed_line[i], parsed_line[i + 1], parsed_line[i + 2], parsed_line[i + 3] }; 369 | data.push_back(frame); 370 | if (row % 1000 == 0) { 371 | cout << row << "'s parsed" << endl; 372 | } 373 | row++; 374 | } 375 | return data; 376 | } --------------------------------------------------------------------------------