diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..5cf43f5 --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,15 @@ +[target.xtensa-esp32-none-elf] +runner = "espflash flash --monitor" + +[env] +ESP_LOG="INFO" + +[build] +rustflags = [ + "-C", "link-arg=-nostartfiles", +] + +target = "xtensa-esp32-none-elf" + +[unstable] +build-std = ["alloc", "core"] diff --git a/.clangd b/.clangd deleted file mode 100644 index 284a8c7..0000000 --- a/.clangd +++ /dev/null @@ -1,10 +0,0 @@ -Diagnostics: - UnusedIncludes: Strict - ClangTidy: - Add: [bugprone-*, cert-*, modernize-*, performance-*, misc-unused-*] -CompileFlags: - Add: - - '-Wall' - - '-Wextra' - - '-std=c++2a' - diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml new file mode 100644 index 0000000..4ae7ae8 --- /dev/null +++ b/.github/workflows/rust.yml @@ -0,0 +1,28 @@ +name: Build, test and lint + +on: + push: + branches: [ "main-rust" ] + pull_request: + branches: [ "main-rust" ] + +env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + CARGO_TERM_COLOR: always + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Install Rust for Xtensa + uses: esp-rs/xtensa-toolchain@v1.5 + with: + default: true + ldproxy: true + + - name: Build project + run: cargo build --verbose + - name: Run clippy + run: cargo clippy --no-deps -- -Dwarnings diff --git a/.gitignore b/.gitignore index 115685c..ba7fe33 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,24 @@ -.pio -.vscode/* -compile_commands.json -.cache/* -.kateproject.build +# Generated by Cargo +# will have compiled files and executables +debug/ +target/ + +# These are backup files generated by rustfmt +**/*.rs.bk + +# MSVC Windows builds of rustc generate these, which store debugging information +*.pdb + +# RustRover +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +.idea/ + + +# ignore platformio's build folder +.pio/ + +.cache/ +/compile_commands.json diff --git a/.vscode/extensions.json b/.vscode/extensions.json deleted file mode 100644 index 080e70d..0000000 --- a/.vscode/extensions.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ], - "unwantedRecommendations": [ - "ms-vscode.cpptools-extension-pack" - ] -} diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 0000000..9ef4aa0 --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,1189 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "anyhow" +version = "1.0.96" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6b964d184e89d9b6b67dd2715bc8e74cf3107fb2b529990c90cf517326150bf4" + +[[package]] +name = "archer" +version = "1.3.1" +dependencies = [ + "embassy-executor", + "embassy-time", + "embedded-hal 1.0.0", + "embedded-hal-bus", + "esp-alloc", + "esp-backtrace", + "esp-hal", + "esp-hal-embassy", + "esp-println", + "libm", + "log", + "mma8x5x", + "pololu_tic", + "thiserror", +] + +[[package]] +name = "autocfg" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" + +[[package]] +name = "basic-toml" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "823388e228f614e9558c6804262db37960ec8821856535f5c3f59913140558f8" +dependencies = [ + "serde", +] + +[[package]] +name = "bitfield" +version = "0.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f798d2d157e547aa99aab0967df39edd0b70307312b6f8bd2848e6abe40896e0" + +[[package]] +name = "bitflags" +version = "2.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36" + +[[package]] +name = "bytemuck" +version = "1.21.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825" +dependencies = [ + "num-traits", +] + +[[package]] +name = "critical-section" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" + +[[package]] +name = "darling" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn", +] + +[[package]] +name = "darling_macro" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +dependencies = [ + "darling_core", + "quote", + "syn", +] + +[[package]] +name = "delegate" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "297806318ef30ad066b15792a8372858020ae3ca2e414ee6c2133b1eb9e9e945" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "document-features" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" +dependencies = [ + "litrs", +] + +[[package]] +name = "embassy-embedded-hal" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41fea5ef5bed4d3468dfd44f5c9fa4cda8f54c86d4fb4ae683eacf9d39e2ea12" +dependencies = [ + "embassy-futures", + "embassy-sync", + "embassy-time", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-storage", + "embedded-storage-async", + "nb 1.1.0", +] + +[[package]] +name = "embassy-executor" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6" +dependencies = [ + "critical-section", + "document-features", + "embassy-executor-macros", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "embassy-futures" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067" + +[[package]] +name = "embassy-sync" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" +dependencies = [ + "cfg-if", + "critical-section", + "embedded-io-async", + "futures-sink", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-time" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f820157f198ada183ad62e0a66f554c610cdcd1a9f27d4b316358103ced7a1f8" +dependencies = [ + "cfg-if", + "critical-section", + "document-features", + "embassy-time-driver", + "embassy-time-queue-utils", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-util", +] + +[[package]] +name = "embassy-time-driver" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba" +dependencies = [ + "document-features", +] + +[[package]] +name = "embassy-time-queue-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83" +dependencies = [ + "embassy-executor", + "heapless", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-bus" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "513e0b3a8fb7d3013a8ae17a834283f170deaf7d0eeab0a7c1a36ad4dd356d22" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "embedded-io", +] + +[[package]] +name = "embedded-storage" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" + +[[package]] +name = "embedded-storage-async" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" +dependencies = [ + "embedded-storage", +] + +[[package]] +name = "enum-as-inner" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1e6a265c649f3f5979b601d26f1d05ada116434c87741c9493cb56218f76cbc" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "enumset" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d07a4b049558765cef5f0c1a273c3fc57084d768b44d2f98127aef4cceb17293" +dependencies = [ + "enumset_derive", +] + +[[package]] +name = "enumset_derive" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "59c3b24c345d8c314966bdc1832f6c2635bfcce8e7cf363bd115987bba2ee242" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "esp-alloc" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "408c0d4c7f51efb256af18641047b0d83b58a485be9edf5a559da796abef0b63" +dependencies = [ + "cfg-if", + "critical-section", + "document-features", + "enumset", + "linked_list_allocator", +] + +[[package]] +name = "esp-backtrace" +version = "0.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c83ca63fd02ca40644ae91ae63362e3a6e7f53458f6c1356decf892343d2418" +dependencies = [ + "esp-build", + "esp-println", + "semihosting", +] + +[[package]] +name = "esp-build" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8aa1c8f9954c9506699cf1ca10a2adcc226ff10b6ae3cb9e875cf2c6a0b9a372" +dependencies = [ + "quote", + "syn", + "termcolor", +] + +[[package]] +name = "esp-config" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd428a3b4b2975772f24eabea123d45cf6a5e28020396ed5dcb331ee3a904046" +dependencies = [ + "document-features", +] + +[[package]] +name = "esp-hal" +version = "0.23.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a573175c540fd1d21a9cf07b0dee286b5a8f4cfde4b35da0f4f4657de7942c45" +dependencies = [ + "basic-toml", + "bitfield", + "bitflags", + "bytemuck", + "cfg-if", + "chrono", + "critical-section", + "delegate", + "document-features", + "embassy-embedded-hal", + "embassy-futures", + "embassy-sync", + "embedded-can", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "enumset", + "esp-build", + "esp-config", + "esp-hal-procmacros", + "esp-metadata", + "esp-riscv-rt", + "esp32", + "fugit", + "instability", + "nb 1.1.0", + "paste", + "portable-atomic", + "rand_core", + "riscv", + "serde", + "strum", + "ufmt-write", + "void", + "xtensa-lx", + "xtensa-lx-rt", +] + +[[package]] +name = "esp-hal-embassy" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4cea15ef146c7689fede0c3a7d765e07b1eb22ef462f1203a137dacc615b031a" +dependencies = [ + "critical-section", + "document-features", + "embassy-executor", + "embassy-sync", + "embassy-time", + "embassy-time-driver", + "embassy-time-queue-utils", + "esp-build", + "esp-config", + "esp-hal", + "esp-hal-procmacros", + "esp-metadata", + "portable-atomic", + "static_cell", +] + +[[package]] +name = "esp-hal-procmacros" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e4a3297005c2b31cd00e2ba50037edc9bddf99da3afe1c97a2d1b0165a312eab" +dependencies = [ + "darling", + "document-features", + "litrs", + "proc-macro-crate", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "esp-metadata" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb15c17e50f4cccb0d88305c19eae2d5533d750f0a05b6a05f1c99864974758e" +dependencies = [ + "anyhow", + "basic-toml", + "serde", + "strum", +] + +[[package]] +name = "esp-println" +version = "0.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "645e54eb592ca0a3d60213b1695e2a5fc0b51ca6d693c08d6983857224a629ed" +dependencies = [ + "critical-section", + "esp-build", + "log", + "portable-atomic", +] + +[[package]] +name = "esp-riscv-rt" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94aca65db6157aa5f42d9df6595b21462f28207ca4230b799aa3620352ef6a72" +dependencies = [ + "document-features", + "riscv", + "riscv-rt-macros", +] + +[[package]] +name = "esp32" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19d3bff1d268a4b8d34b494c0e88466cd59a827bb330189773db299ff525ea13" +dependencies = [ + "critical-section", + "vcell", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "fugit" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17186ad64927d5ac8f02c1e77ccefa08ccd9eaa314d5a4772278aa204a22f7e7" +dependencies = [ + "gcd", +] + +[[package]] +name = "futures-core" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e" + +[[package]] +name = "futures-sink" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7" + +[[package]] +name = "futures-task" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988" + +[[package]] +name = "futures-util" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "gcd" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d758ba1b47b00caf47f24925c0074ecb20d6dfcffe7f6d53395c0465674841a" + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hashbrown" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "indexmap" +version = "2.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8c9c992b02b5b4c94ea26e32fe5bccb7aa7d9f390ab5c1221ff895bc7ea8b652" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "indoc" +version = "2.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" + +[[package]] +name = "instability" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bf9fed6d91cfb734e7476a06bde8300a1b94e217e1b523b6f0cd1a01998c71d" +dependencies = [ + "darling", + "indoc", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + +[[package]] +name = "linked_list_allocator" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9afa463f5405ee81cdb9cc2baf37e08ec7e4c8209442b5d72c04cfb2cd6e6286" + +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "log" +version = "0.4.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30bde2b3dc3671ae49d8e2e9f044c7c005836e7a023ee57cffa25ab82764bb9e" + +[[package]] +name = "memchr" +version = "2.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" + +[[package]] +name = "minijinja" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cff7b8df5e85e30b87c2b0b3f58ba3a87b68e133738bf512a7713769326dbca9" +dependencies = [ + "serde", +] + +[[package]] +name = "mma8x5x" +version = "0.1.1" +source = "git+https://github.com/eldruin/mma8x5x-rs.git#40342c348ed68f7eabe8b5c9e5e812f2ec149ed3" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "num-derive" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed3955f1a9c7c0c15e092f9c887db08b1fc683305fdf6eb6684f22555355e202" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pin-project-lite" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "pololu_tic" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67f9d38ddca616bd591f95669aa7346ecd73c30b0ec11357d215d65c58c90219" +dependencies = [ + "embedded-hal 1.0.0", + "num-derive", + "num-traits", + "thiserror", +] + +[[package]] +name = "portable-atomic" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" + +[[package]] +name = "proc-macro-crate" +version = "3.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ecf48c7ca261d60b74ab1a7b20da18bede46776b2e55535cb958eb595c5fa7b" +dependencies = [ + "toml_edit", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "proc-macro2" +version = "1.0.93" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r0" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd7a31eed1591dcbc95d92ad7161908e72f4677f8fabf2a32ca49b4237cbf211" + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" + +[[package]] +name = "riscv" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5ea8ff73d3720bdd0a97925f0bf79ad2744b6da8ff36be3840c48ac81191d7a7" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "paste", + "riscv-macros", + "riscv-pac", +] + +[[package]] +name = "riscv-macros" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f265be5d634272320a7de94cea15c22a3bfdd4eb42eb43edc528415f066a1f25" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "riscv-pac" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8188909339ccc0c68cfb5a04648313f09621e8b87dc03095454f1a11f6c5d436" + +[[package]] +name = "riscv-rt-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30f19a85fe107b65031e0ba8ec60c34c2494069fe910d6c297f5e7cb5a6f76d0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "rustversion" +version = "1.0.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f7c45b9784283f1b2e7fb61b42047c2fd678ef0960d4f6f1eba131594cc369d4" + +[[package]] +name = "semihosting" +version = "0.1.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "407ec8d274cd77556e9c2bef886df91eb3447b4059e603d6fb19f85e6452e275" + +[[package]] +name = "serde" +version = "1.0.218" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8dfc9d19bdbf6d17e22319da49161d5d0108e4188e8b680aef6299eed22df60" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.218" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f09503e191f4e797cb8aac08e9a4a4695c5edf6a2e70e376d961ddd5c969f82b" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_spanned" +version = "0.6.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87607cb1398ed59d48732e575a4c28a7a8ebf2454b964fe3f224f2afc07909e1" +dependencies = [ + "serde", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "static_cell" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" +dependencies = [ + "portable-atomic", +] + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "strum" +version = "0.26.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fec0f0aef304996cf250b31b5a10dee7980c85da9d759361292b8bca5a18f06" +dependencies = [ + "strum_macros", +] + +[[package]] +name = "strum_macros" +version = "0.26.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn", +] + +[[package]] +name = "syn" +version = "2.0.98" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "36147f1a48ae0ec2b5b3bc5b537d267457555a10dc06f3dbc8cb11ba3006d3b1" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "termcolor" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "thiserror" +version = "2.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d452f284b73e6d76dd36758a0c8684b1d5be31f92b89d07fd5822175732206fc" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "26afc1baea8a989337eeb52b6e72a039780ce45c3edfcc9c5b9d112feeb173c2" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "toml" +version = "0.8.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd87a5cdd6ffab733b2f74bc4fd7ee5fff6634124999ac278c35fc78c6120148" +dependencies = [ + "serde", + "serde_spanned", + "toml_datetime", + "toml_edit", +] + +[[package]] +name = "toml_datetime" +version = "0.6.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0dd7358ecb8fc2f8d014bf86f6f638ce72ba252a2c3a2572f2a795f1d23efb41" +dependencies = [ + "serde", +] + +[[package]] +name = "toml_edit" +version = "0.22.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17b4795ff5edd201c7cd6dca065ae59972ce77d1b80fa0a84d94950ece7d1474" +dependencies = [ + "indexmap", + "serde", + "serde_spanned", + "toml_datetime", + "winnow", +] + +[[package]] +name = "ufmt-write" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69" + +[[package]] +name = "unicode-ident" +version = "1.0.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "00e2473a93778eb0bad35909dff6a10d28e63f792f16ed15e404fca9d5eeedbe" + +[[package]] +name = "vcell" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "winapi-util" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "winnow" +version = "0.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e7f4ea97f6f78012141bcdb6a216b2609f0979ada50b20ca5b52dde2eac2bb1" +dependencies = [ + "memchr", +] + +[[package]] +name = "xtensa-lx" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "51cbb46c78cfd284c9378070ab90bae9d14d38b3766cb853a97c0a137f736d5b" +dependencies = [ + "critical-section", + "document-features", +] + +[[package]] +name = "xtensa-lx-rt" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "689c2ef159d9cd4fc9503603e9999968a84a30db9bde0f0f880d0cceea0190a9" +dependencies = [ + "anyhow", + "document-features", + "enum-as-inner", + "minijinja", + "r0", + "serde", + "strum", + "toml", + "xtensa-lx", + "xtensa-lx-rt-proc-macros", +] + +[[package]] +name = "xtensa-lx-rt-proc-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11277b1e4cbb7ffe44678c668518b249c843c81df249b8f096701757bc50d7ee" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn", +] diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..da4b774 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,44 @@ +[package] +edition = "2024" +name = "archer" +version = "1.3.1" + +[[bin]] +name = "archer" +test = false +bench = false + +[dependencies] +embassy-executor = { version = "0.7", features = ["task-arena-size-20480"] } +embassy-time = { version = "0.4", features = ["generic-queue-8"] } +esp-alloc = { version = "0.6" } +esp-backtrace = { version = "0.15", features = [ + "esp32", + "exception-handler", + "panic-handler", + "println", +] } +esp-hal = { version = "0.23", features = ["esp32", "unstable"] } +esp-hal-embassy = { version = "0.6", features = ["esp32"] } +esp-println = { version = "0.13", features = ["esp32", "log"] } +pololu_tic = "0.1" +log = "0.4" +embedded-hal-bus = "0.3" +embedded-hal = "1.0" +mma8x5x = { git = "https://github.com/eldruin/mma8x5x-rs.git" } +libm = "0.2" +thiserror = { version = "2.0", default-features = false } + +[profile.dev] +# Rust debug is too slow. +# For debug builds always builds with some optimization +opt-level = "s" + +[profile.release] +codegen-units = 1 # LLVM can perform better optimizations using a single thread +debug = 2 +debug-assertions = false +incremental = false +lto = 'fat' +opt-level = 's' +overflow-checks = false diff --git a/PROTOCOL.md b/PROTOCOL.md index 2e035d0..e3e0d68 100644 --- a/PROTOCOL.md +++ b/PROTOCOL.md @@ -15,7 +15,7 @@ Positive and negative must be specified. Commands respond with `OK\n` if successful, `ERR \n` if not. Some commands respond with return arguments after the `OK` and before the `\n`. -# Version 1.1.1 +# Version 1.3.1 ## Types command: `ABCD` # 4 ASCII characters @@ -49,6 +49,12 @@ Args: Description: Sets horizontal calibration position to current position. +### `MOVC` +Args: `[UP, DN, LT, RT, SV, SH]` + +Description: Moves in the direction specified until stopped. `SV` is "Stop +Vertical", and `SH` is "Stop Horizontal" + ### `MOVV` Args: `integer` @@ -85,6 +91,20 @@ Returns: `integer` Description: Gets speed for both axes. +### `VERS` +Args: + +Returns: `string` + +Description: Gets the current version of the software. + +### `GETC` +Args: + +Returns: `string` + +Description: Returns `true` or `false` depending on the current calibration status. + ### `HALT` Args: diff --git a/README.md b/README.md index 274ca7d..3992ed5 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Tracker +# ARCHER (Alt-azimuth Rate Calculator for High-speed Ephemeris Ranging) This is the repository for the embedded code for UNL Rocketry's tracker project. Information about the communication protocol used for the tracker can be found diff --git a/build.rs b/build.rs new file mode 100644 index 0000000..e3f3ed6 --- /dev/null +++ b/build.rs @@ -0,0 +1,3 @@ +fn main() { + println!("cargo:rustc-link-arg=-Tlinkall.x"); +} diff --git a/platformio.ini b/platformio.ini deleted file mode 100644 index 8fd864a..0000000 --- a/platformio.ini +++ /dev/null @@ -1,23 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:stable] -platform = espressif32 -board = esp32dev -framework = arduino -lib_deps = - pololu/Tic@^2.2.0 - adafruit/Adafruit MMA8451 Library@^1.2.3 -monitor_speed = 115200 -monitor_echo = yes -build_flags = - -Wall - -Wextra - -std=c++2a diff --git a/rust-toolchain.toml b/rust-toolchain.toml new file mode 100644 index 0000000..a2f5ab5 --- /dev/null +++ b/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "esp" diff --git a/src/commands.rs b/src/commands.rs new file mode 100644 index 0000000..de3fe8e --- /dev/null +++ b/src/commands.rs @@ -0,0 +1,235 @@ +use crate::{ + SPEED_DEFAULT_HORIZONTAL, SPEED_DEFAULT_VERTICAL, SPEED_MAX_HORIZONTAL, SPEED_MAX_VERTICAL, + STEPS_PER_DEGREE_HORIZONTAL, STEPS_PER_DEGREE_VERTICAL, calibrate_vertical, get_delta_angle, + get_relative_angle, +}; +use alloc::format; +use alloc::string::{String, ToString}; +use esp_println::println; +use mma8x5x::ic::Mma8451; +use mma8x5x::{Mma8x5x, mode}; +use pololu_tic::{TicBase, TicI2C}; + +#[derive(Debug, thiserror::Error)] +pub enum ParseErr { + #[error("the command was empty")] + Empty, + #[error("the command provided was invalid")] + InvalidCommand, + #[error("the argument provided was invalid")] + InvalidArgument, + #[error("the number could not be parsed")] + InvalidNumber, + #[error("not enough arguments were provided")] + TooFewArguments, + #[error("the motor controllers experienced an error")] + InternalError(#[from] pololu_tic::TicHandlerError), + #[error("the vertical position has not been calibrated.")] + Uncalibrated, +} + +const BLACKLIST: &[&str] = &["DVER", "DHOR"]; + +pub async fn parse_command( + motor_vertical: &mut TicI2C, + motor_horizontal: &mut TicI2C, + accel: &mut Option>, + input: &str, + is_calibrated: &mut bool, +) -> Result { + let input = input.to_ascii_uppercase(); + let mut arguments = input.split_whitespace().peekable(); + + if !*is_calibrated && arguments.peek().is_some_and(|a| BLACKLIST.contains(a)) { + return Err(ParseErr::Uncalibrated); + } + + match arguments.next().ok_or(ParseErr::Empty)? { + "DVER" => { + let target_pos = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n.clamp(0.0, 90.0), + _ => return Err(ParseErr::InvalidNumber), + }; + + motor_vertical + .set_target_position((target_pos * STEPS_PER_DEGREE_VERTICAL as f32) as i32)?; + } + "DHOR" => { + let target_pos = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n.clamp(-180.0, 180.0), + _ => return Err(ParseErr::InvalidNumber), + }; + let angle_steps = get_delta_angle(get_relative_angle(motor_horizontal), target_pos) + * STEPS_PER_DEGREE_HORIZONTAL as f32; + let move_to = motor_horizontal.current_position()? as f32 + angle_steps; + motor_horizontal.set_target_position(move_to as i32)?; + } + "CALV" => match arguments.next() { + Some("SET") => { + motor_vertical.halt_and_set_position(0)?; + *is_calibrated = true; + } + _ => match accel { + Some(accel) => { + calibrate_vertical(motor_vertical, accel).await; + *is_calibrated = true + } + None => return Err(ParseErr::InvalidCommand), + }, + }, + "CALH" => { + motor_horizontal.halt_and_set_position(0)?; + } + "MOVC" => match arguments.next() { + Some("UP") => { + motor_vertical.set_target_velocity(SPEED_DEFAULT_HORIZONTAL/2)?; + } + Some("DN") => { + motor_vertical.set_target_velocity(-SPEED_DEFAULT_HORIZONTAL/2)?; + } + Some("LT") => { + motor_horizontal.set_target_velocity(SPEED_DEFAULT_HORIZONTAL/2)?; + } + Some("RT") => { + motor_horizontal.set_target_velocity(-SPEED_DEFAULT_HORIZONTAL/2)?; + } + Some("SV") => { + motor_vertical.set_target_velocity(0)?; + } + Some("SH") => { + motor_horizontal.set_target_velocity(0)?; + } + _ => return Err(ParseErr::InvalidCommand), + } + "MOVV" => { + let steps_to_move = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n, + Err(_) => Err(ParseErr::TooFewArguments)?, + }; + let current_position = motor_vertical.current_position()? as f32; + let move_to = current_position + steps_to_move; + motor_vertical.set_target_position(move_to as i32)?; + } + "MOVH" => { + let steps_to_move = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n, + Err(_) => Err(ParseErr::TooFewArguments)?, + }; + let current_position = motor_horizontal.current_position()? as f32; + let move_to = current_position - steps_to_move; + motor_horizontal.set_target_position(move_to as i32)?; + } + "GETP" => { + let vertical_position: f32 = + motor_vertical.current_position()? as f32 / STEPS_PER_DEGREE_VERTICAL as f32; + let mut horizontal_position: f32 = + motor_horizontal.current_position()? as f32 / STEPS_PER_DEGREE_HORIZONTAL as f32; + while horizontal_position > 180.0 { + horizontal_position -= 360.0; + } + while horizontal_position < -180.0 { + horizontal_position += 360.0; + } + return Ok(format!("{} {}", vertical_position, horizontal_position)); + } + "INFO" => { + let command_list = [ + "DVER INT", + "DHOR INT", + "CALV {SET}", + "CALH", + "MOVC [UP DN LT RT SV SH]", + "MOVV INT", + "MOVH INT", + "GETP", + "GETC", + "VERS", + "SSPD INT {VER INT} {HOR INT} {RST}", + "GSPD", + "HALT", + "INFO", + ]; + + for command in command_list { + println!(" {}", command); + } + } + "GETC" => { + return Ok(is_calibrated.to_string()); + } + "VERS" => { + return Ok(env!("CARGO_PKG_VERSION").to_string()); + } + "SSPD" => match arguments.next() { + Some("VER") => { + let new_speed = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n, + Err(_) => Err(ParseErr::TooFewArguments)?, + }; + motor_vertical + .set_max_speed(new_speed.clamp(0.0, SPEED_MAX_VERTICAL as f32) as u32)?; + } + Some("HOR") => { + let new_speed = match arguments + .next() + .ok_or(ParseErr::TooFewArguments)? + .parse::() + { + Ok(n) => n, + Err(_) => Err(ParseErr::TooFewArguments)?, + }; + motor_horizontal + .set_max_speed(new_speed.clamp(0.0, SPEED_MAX_HORIZONTAL as f32) as u32)?; + } + Some("RST") => { + motor_vertical.set_max_speed(SPEED_DEFAULT_VERTICAL as u32)?; + motor_horizontal.set_max_speed(SPEED_DEFAULT_HORIZONTAL as u32)?; + } + Some(int) => { + let new_speed = match int.parse::() { + Ok(n) => n, + Err(_) => Err(ParseErr::TooFewArguments)?, + }; + motor_vertical + .set_max_speed(new_speed.clamp(0.0, SPEED_MAX_VERTICAL as f32) as u32)?; + motor_horizontal + .set_max_speed(new_speed.clamp(0.0, SPEED_MAX_HORIZONTAL as f32) as u32)?; + } + _ => return Err(ParseErr::InvalidArgument), + }, + "GSPD" => { + return Ok(format!( + "{} {}", + motor_vertical.max_speed()?, + motor_horizontal.max_speed()? + )); + } + "HALT" => { + motor_vertical.halt_and_hold()?; + motor_horizontal.halt_and_hold()? + } + _ => Err(ParseErr::InvalidCommand)?, + } + + Ok("".to_string()) +} diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 5599dbe..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,371 +0,0 @@ -#include -#include -#include -#include -#include - -// ##### TOP LEVEL STUFF ##### // -// ########################### // - - -// #### PIN DEFINITIONS #### // -const int SDA_PIN = 18; -const int SCL_PIN = 19; - -// #### MOTOR DRIVERS #### // -TicI2C motorHorizontal(14); -TicI2C motorVertical(15); - -// Steps per degree for the motor drivers at the default stepping -const int TIC_STEPS_PER_DEGREE_VERTICAL = 24; -const int TIC_STEPS_PER_DEGREE_HORIZONTAL = 126; -const int TIC_SPEED_VERYSLOW = 500000; //only used on CALV so no need to add a second one -const int TIC_SPEED_DEFAULT_VERTICAL = 7000000; -const int TIC_SPEED_DEFAULT_HORIZONTAL = 7000000; -const int TIC_SPEED_MAX_VERTICAL = 7000000; -const int TIC_SPEED_MAX_HORIZONTAL = 7000000; - -const int TIC_DECEL_DEFAULT = 2000000; - -// #### ACCELEROMETER #### // -Adafruit_MMA8451 mma8451 = Adafruit_MMA8451(); - -// Offsets calculated manually from accelerometer data -const int ACC_OFFSET_X = 263; -const int ACC_OFFSET_Y = -194; -const int ACC_OFFSET_Z = 142; - -// ##### END OF TOP LEVEL STUFF ##### // -// ################################## // - -auto calculatePitch() -> double { - mma8451.read(); - double x = double(mma8451.y - ACC_OFFSET_Y) / 4096; - double y = double(mma8451.x - ACC_OFFSET_X) / 4096; - double z = double(mma8451.z - ACC_OFFSET_Z) / 4096; - - // Calculate pitch from acceleration data - return atan2(-x, pow(y, 2) + pow(z, 2) ) * 57.29577951; -} - -void setupMotor(TicI2C motor, String whichMotor) { - motor.setProduct(TicProduct::Tic36v4); - motor.setCurrentLimit(2000); - motor.setAgcFrequencyLimit(TicAgcFrequencyLimit::F675Hz); - motor.haltAndSetPosition(0); - Serial.println(motor.getMaxAccel()); - motor.setMaxDecel(TIC_DECEL_DEFAULT); - motor.setMaxAccel(TIC_DECEL_DEFAULT); - if (whichMotor == "Vert") { - motor.setMaxSpeed(TIC_SPEED_DEFAULT_VERTICAL); - } else if (whichMotor == "Horiz") { - motor.setMaxSpeed(TIC_SPEED_DEFAULT_HORIZONTAL); - } else - motor.setMaxSpeed(min(TIC_SPEED_DEFAULT_VERTICAL, TIC_SPEED_DEFAULT_HORIZONTAL)); - motor.setStepMode(TicStepMode::Full); - - motor.exitSafeStart(); -} - -void setup() { - Serial.begin(115200); - Serial.setTimeout(10); - - while (!Serial) delay(10); - - Wire.begin(SDA_PIN, SCL_PIN); - - delay(1000); - - Serial.println("Starting MMA8451"); - if (!mma8451.begin()) { - Serial.println("Failed to start MMA8451!"); - } else { - Serial.println("MMA8451 found!"); - } - - mma8451.setRange(MMA8451_RANGE_2_G); - mma8451.setDataRate(mma8451_dataRate_t::MMA8451_DATARATE_50_HZ); - - // Set up motor driver(s) - setupMotor(motorVertical, "Vert"); - setupMotor(motorHorizontal, "Horiz"); -} - -void calibrateVertical() { - const float ZERO_CAL = 0.2; - int targetVelocity; - motorVertical.setMaxDecel(5000000); - motorVertical.setMaxAccel(5000000); - motorVertical.setStepMode(TicStepMode::Microstep8); - - // Find the zero position - while (true) { - double pitchSum = 0; - for (int i = 0; i < 20; i++) { - double pitch = calculatePitch(); - pitchSum += pitch; - delay(20); - } - pitchSum /= 20; - - // Prevents movement from erroring out - motorVertical.resetCommandTimeout(); - - if (fabs(pitchSum - ZERO_CAL) < 3.0) { - targetVelocity = -TIC_SPEED_VERYSLOW; - delay(50); - } else { - targetVelocity = -TIC_SPEED_DEFAULT_VERTICAL; - } - - if (pitchSum <= -0.02) { - motorVertical.setTargetVelocity(targetVelocity); - } else if (pitchSum >= 0.02) { - motorVertical.setTargetVelocity(-targetVelocity); - } else { - motorVertical.haltAndSetPosition(0); - break; - } - delay(200); - motorVertical.setTargetVelocity(0); - } - motorVertical.setMaxDecel(TIC_DECEL_DEFAULT); - motorVertical.setMaxAccel(TIC_DECEL_DEFAULT); - motorVertical.setStepMode(TicStepMode::Full); -} - -float getDeltaAngle(float curr_angle, float new_angle) { - // Calculate most optimal difference in current and destination angle - float diff = fmod((new_angle - curr_angle + 180), (float) 360) - 180; - if (diff < -180) { - return diff + 360; // if angle less than -180, switch direction - } else { - return diff; - } -} - -void parseCommand(String &input) { - if (input.length() == 1) { - Serial.println("ERR"); - } - - int indicies[32] = {}; - int indexIndicies = 0; - - int lastIndex = 0; - - while (true) { - int nextIndex = input.indexOf(' ', lastIndex); - if (nextIndex == -1) { - break; - } - - indicies[indexIndicies] = nextIndex; - indexIndicies += 1; - - lastIndex = nextIndex + 1; - } - - String command = input.substring(0, indicies[0]); - command.toUpperCase(); - command.trim(); - Serial.print("Got command \""); - Serial.print(command); - Serial.println("\""); - - if (command == "DVER") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - if (indexIndicies == 1) { - Serial.println("ERR"); - return; - } - - float position = arg1.toFloat(); - if (fabs(position) > 90) { - Serial.println("ERR"); - return; - } - - motorVertical.setTargetPosition((int32_t) (position * float(TIC_STEPS_PER_DEGREE_VERTICAL))); - - } else if (command == "DHOR") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - - if (indexIndicies == 1) { - Serial.println("ERR"); - return; - } - - float position = arg1.toFloat(); - if (fabs(position) > 180) { - Serial.println("ERR"); - return; - } - - // Get current position in steps - float curr_pos = (float) motorHorizontal.getCurrentPosition() / (float) TIC_STEPS_PER_DEGREE_HORIZONTAL; - - while (curr_pos > 180) { - curr_pos -= 360; - } - while (curr_pos < -180) { - curr_pos += 360; - } - - // Find the number of steps to get to destination - int32_t angle_steps = (getDeltaAngle(curr_pos, position) * TIC_STEPS_PER_DEGREE_HORIZONTAL); - - motorHorizontal.setTargetPosition(motorHorizontal.getCurrentPosition() + angle_steps); - - } else if (command == "CALV") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - - if (arg1 == "SET") { - motorVertical.haltAndSetPosition(0); - } else if (indicies[1] == 0) { - calibrateVertical(); - } else { - Serial.println("ERR"); - return; - } - - } else if (command == "CALH") { - motorHorizontal.haltAndSetPosition(0); - - } else if (command == "MOVV") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - - if (indexIndicies == 1) { - Serial.println("ERR"); - return; - } - - auto steps_to_move = (int32_t) arg1.toInt(); - int32_t current_position = motorVertical.getCurrentPosition(); - int32_t move_to = current_position + steps_to_move; - motorVertical.setTargetPosition(move_to); - - } else if (command == "MOVH") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - - if (indexIndicies == 1) { - Serial.println("ERR"); - return; - } - - auto steps_to_move = (int32_t) arg1.toInt(); - int32_t current_position = motorHorizontal.getCurrentPosition(); - int32_t move_to = current_position + steps_to_move; - motorHorizontal.setTargetPosition(move_to); - - } else if (command == "GETP") { - float vertical_position = (float) motorVertical.getCurrentPosition() / (float) TIC_STEPS_PER_DEGREE_VERTICAL; - float horizontal_position = (float) motorHorizontal.getCurrentPosition() / (float) TIC_STEPS_PER_DEGREE_HORIZONTAL; - while (horizontal_position > 180) { - horizontal_position -= 360; - } - while (horizontal_position < -180) { - horizontal_position += 360; - } - Serial.printf("OK %g %g\n",vertical_position, horizontal_position); - - } else if (command == "INFO") { - - Serial.println("Command List:"); - String command_list[] = {"DVER INT", "DHOR INT", "CALV {SET}", "CALH", "MOVV INT", "MOVH INT", "GETP", "SSPD INT {VER INT} {HOR INT} {RST}", "GSPD"}; - for (String &command : command_list) { - Serial.print(" "); - Serial.println(command); - } - - } else if (command == "SSPD") { - String arg1 = input.substring(indicies[0], indicies[1]); - arg1.trim(); - - int64_t new_speed; - - if (indexIndicies == 3) { - String arg2 = input.substring(indicies[1], indicies[2]); - arg2.trim(); - new_speed = arg2.toInt(); - } else if (indexIndicies == 1 || ((arg1 == "VER" || arg1 == "HOR") && indexIndicies != 3)) { - Serial.println("ERR"); - return; - } - - if (arg1 == "VER") { - if (new_speed > TIC_SPEED_MAX_VERTICAL) { - new_speed = TIC_SPEED_MAX_VERTICAL; - } - motorVertical.setMaxSpeed(new_speed); - } else if (arg1 == "HOR") { - if (new_speed > TIC_SPEED_MAX_HORIZONTAL) { - new_speed = TIC_SPEED_MAX_HORIZONTAL; - } - motorHorizontal.setMaxSpeed(new_speed); - } else if (arg1 == "RST") { - motorVertical.setMaxSpeed(TIC_SPEED_DEFAULT_VERTICAL); - motorHorizontal.setMaxSpeed(TIC_SPEED_DEFAULT_HORIZONTAL); - } else if (indicies[1] == 0) { - motorVertical.setMaxSpeed(new_speed); - motorHorizontal.setMaxSpeed(new_speed); - } else { - Serial.println("ERR"); - return; - } - - } else if (command == "GSPD") { - uint32_t vertical_speed; - uint32_t Horizontal_speed; - - vertical_speed = motorVertical.getMaxSpeed(); - Horizontal_speed = motorHorizontal.getMaxSpeed(); - - Serial.printf("OK %i %i\n",vertical_speed, Horizontal_speed); - return; - - } else { - Serial.println("ERR"); - return; - } - - Serial.println("OK"); -} - -String commandString = ""; -void loop() { - // Prevents movement from erroring out - motorHorizontal.resetCommandTimeout(); - motorVertical.resetCommandTimeout(); - - if (Serial.available() > 0) { - // Read in a string until a newline, not including the newline - int byte = Serial.read(); - if (byte == '\n') { - // The command string has been terminated - commandString += ' '; - parseCommand(commandString); - commandString.clear(); - } else if (byte == '\b') { - // Backspace, delete the last character in the string. Unless it's empty - // then scream ig - if (commandString.length() != 0) { - commandString.remove(commandString.length() - 1); - Serial.print(" \b"); - } - } else if (byte != 0xFF) { - // This is a regular valid character, add it to the string - commandString += (char) byte; - } - } - - //Serial.printf("%i\n", motorVertical.getCurrentPosition()); - - delay(10); -} diff --git a/src/main.rs b/src/main.rs new file mode 100644 index 0000000..bf93f90 --- /dev/null +++ b/src/main.rs @@ -0,0 +1,331 @@ +#![no_main] +#![no_std] + +mod commands; +use commands::parse_command; + +use alloc::string::String; +use core::cell::RefCell; +use embedded_hal_bus::i2c::RefCellDevice; +use esp_backtrace as _; +use esp_hal::{ + clock::CpuClock, + i2c::{self, master::I2c}, +}; +use esp_println::{print, println}; + +use embassy_executor::Spawner; +use embassy_time::{Duration, Timer}; +use log::{error, info}; +use mma8x5x::{GScale, Mma8x5x, OutputDataRate, PowerMode, ic::Mma8451, mode}; +use pololu_tic::{TicHandlerError, TicI2C, TicProduct, TicStepMode, base::TicBase}; + +extern crate alloc; + +const STEPS_PER_DEGREE_VERTICAL: u32 = + (23.3 * tic_step_mult(DEFAULT_STEP_MODE_VERTICAL) as f32) as u32; +const STEPS_PER_DEGREE_HORIZONTAL: u32 = + (126.0 * tic_step_mult(DEFAULT_STEP_MODE_HORIZONTAL) as f32) as u32; +const SPEED_VERYSLOW: i32 = 20000 * tic_step_mult(DEFAULT_STEP_MODE_VERTICAL) as i32; //only used on CALV so no need to add a second one +const SPEED_DEFAULT_VERTICAL: i32 = 15000000 * tic_step_mult(DEFAULT_STEP_MODE_VERTICAL) as i32; +const SPEED_DEFAULT_HORIZONTAL: i32 = 10000000 * tic_step_mult(DEFAULT_STEP_MODE_HORIZONTAL) as i32; +const SPEED_MAX_VERTICAL: u32 = 15000000 * tic_step_mult(DEFAULT_STEP_MODE_VERTICAL) as u32; +const SPEED_MAX_HORIZONTAL: u32 = 10000000 * tic_step_mult(DEFAULT_STEP_MODE_HORIZONTAL) as u32; + +const TIC_DECEL_DEFAULT_VERTICAL: u32 = + 400000 * (tic_step_mult(DEFAULT_STEP_MODE_VERTICAL) as u32 / 2); +const TIC_DECEL_DEFAULT_HORIZONTAL: u32 = + 300000 * tic_step_mult(DEFAULT_STEP_MODE_HORIZONTAL) as u32; + +const DEFAULT_CURRENT: u16 = 1024; + +const DEFAULT_STEP_MODE_VERTICAL: TicStepMode = TicStepMode::Microstep16; +const DEFAULT_STEP_MODE_HORIZONTAL: TicStepMode = TicStepMode::Microstep8; + +pub const fn tic_step_mult(step_mode: TicStepMode) -> u16 { + match step_mode { + TicStepMode::Full => 1, + TicStepMode::Half => 2, + TicStepMode::Microstep2_100p => 2, + TicStepMode::Microstep4 => 4, + TicStepMode::Microstep8 => 8, + TicStepMode::Microstep16 => 16, + TicStepMode::Microstep32 => 32, + TicStepMode::Microstep64 => 64, + TicStepMode::Microstep128 => 128, + TicStepMode::Microstep256 => 256, + } +} + +// Offsets calculated manually from accelerometer data +const ACC_OFFSET_X: i16 = 53 / 8; +const ACC_OFFSET_Y: i16 = 83 / 8; +const ACC_OFFSET_Z: i16 = -154 / 8; + +#[esp_hal_embassy::main] +async fn main(spawner: Spawner) { + let config = esp_hal::Config::default().with_cpu_clock(CpuClock::_80MHz); + let peripherals = esp_hal::init(config); + esp_alloc::heap_allocator!(72 * 1024); + esp_println::logger::init_logger_from_env(); + let timer0 = esp_hal::timer::timg::TimerGroup::new(peripherals.TIMG1); + esp_hal_embassy::init(timer0.timer0); + info!("Embassy initialized!"); + + let sda = peripherals.GPIO18; + let scl = peripherals.GPIO19; + + let _ = spawner; + + let i2c_bus = I2c::new( + peripherals.I2C0, + esp_hal::i2c::master::Config::default().with_timeout(i2c::master::BusTimeout::Maximum), + ) + .unwrap() + .with_sda(sda) + .with_scl(scl) + .into_async(); + let i2c_bus = RefCell::new(i2c_bus); + + let mut motor_horizontal = + pololu_tic::TicI2C::new_with_address(RefCellDevice::new(&i2c_bus), TicProduct::Tic36v4, 14); + let mut motor_vertical = + pololu_tic::TicI2C::new_with_address(RefCellDevice::new(&i2c_bus), TicProduct::Tic36v4, 15); + + let mut accelerometer = Mma8x5x::new_mma8451( + RefCellDevice::new(&i2c_bus), + mma8x5x::SlaveAddr::Alternative(true), + ); + let _ = accelerometer.disable_auto_sleep(); + let _ = accelerometer.set_scale(GScale::G2); + let _ = accelerometer.set_data_rate(OutputDataRate::Hz50); + let _ = accelerometer.set_wake_power_mode(PowerMode::HighResolution); + let _ = accelerometer.set_read_mode(mma8x5x::ReadMode::Normal); + let _ = accelerometer.set_offset_correction( + ACC_OFFSET_X as i8, + ACC_OFFSET_Y as i8, + ACC_OFFSET_Z as i8, + ); + + let (tx_pin, rx_pin) = (peripherals.GPIO1, peripherals.GPIO3); + let config = esp_hal::uart::Config::default().with_rx_fifo_full_threshold(64); + + let mut uart0 = esp_hal::uart::Uart::new(peripherals.UART0, config) + .unwrap() + .with_tx(tx_pin) + .with_rx(rx_pin) + .into_async(); + + uart0.set_at_cmd(esp_hal::uart::AtCmdConfig::default().with_cmd_char(0x04)); + + let mut accelerometer = if let Ok(a) = accelerometer.into_active() { + info!("MMA8451 set up!!"); + Some(a) + } else { + None + }; + + setup_motor(&mut motor_horizontal, MotorAxis::Horizontal) + .expect("Horizontal motor setup error"); + setup_motor(&mut motor_vertical, MotorAxis::Vertical).expect("Vertical motor setup error"); + info!("Motors set up!!"); + + let mut is_calibrated = false; + + let mut buffer = [0; 1]; + let mut command_string = String::new(); + + loop { + Timer::after(Duration::from_millis(1)).await; + + while motor_horizontal.reset_command_timeout().is_err() { + error!("Horizontal motor communication failure, attempting reconnection"); + motor_horizontal = + pololu_tic::TicI2C::new_with_address(RefCellDevice::new(&i2c_bus), TicProduct::Tic36v4, 14); + + let _ = setup_motor(&mut motor_horizontal, MotorAxis::Horizontal); + Timer::after(Duration::from_secs(1)).await; + } + + while motor_vertical.reset_command_timeout().is_err() { + error!("Vertical motor communication failure, attempting reconnection"); + motor_vertical = + pololu_tic::TicI2C::new_with_address(RefCellDevice::new(&i2c_bus), TicProduct::Tic36v4, 15); + + let _ = setup_motor(&mut motor_vertical, MotorAxis::Vertical); + Timer::after(Duration::from_secs(1)).await; + } + + let count = uart0.read_buffered_bytes(&mut buffer).unwrap(); + + // If there were no bytes read, don't try to use them + if count == 0 { + continue; + } + + if buffer[0] == b'\x1B' { + command_string.clear(); + match parse_command( + &mut motor_vertical, + &mut motor_horizontal, + &mut accelerometer, + "HALT ", + &mut is_calibrated, + ) + .await + { + Ok(_) => print!("OK SOFTWARE E-STOP (ESC RECIEVED)\n"), + Err(e) => print!("ERR: {:?}, {}\n", e, e), + } + continue; + } + + if buffer[0] == b'\r' || buffer[0] == b'\n' { + println!(); + command_string += " "; + + match parse_command( + &mut motor_vertical, + &mut motor_horizontal, + &mut accelerometer, + &command_string, + &mut is_calibrated, + ) + .await + { + Ok(s) => print!("OK {}\n", s), + Err(e) => print!("ERR: {:?}, {}\n", e, e), + } + + command_string.clear(); + } else if buffer[0] == b'\x08' { + if !command_string.is_empty() { + command_string.remove(command_string.len() - 1); + print!("\x08 \x08"); + } + } else if buffer[0] != 0xFF { + print!("{}", buffer[0] as char); + command_string.push(buffer[0] as char); + } + } +} + +#[derive(PartialEq, Eq)] +enum MotorAxis { + Horizontal, + Vertical, +} + +/// Calculates pitch from MMA8451 data +fn calculate_pitch( + accel: &mut Mma8x5x, +) -> f32 { + let data = accel.read().unwrap(); + let x = data.y; + let y = data.x; + let z = data.z; + + libm::atan2f(-x, libm::powf(y, 2.0) + libm::powf(z, 2.0)) + * (180.0 / core::f64::consts::PI as f32) +} + +/// Function to set up motors +fn setup_motor( + motor: &mut TicI2C, + motor_axis: MotorAxis, +) -> Result<(), TicHandlerError> { + motor.set_current_limit(DEFAULT_CURRENT)?; + motor.halt_and_set_position(0)?; + + match motor_axis { + MotorAxis::Vertical => { + motor.set_max_decel(TIC_DECEL_DEFAULT_VERTICAL)?; + motor.set_max_accel(TIC_DECEL_DEFAULT_VERTICAL)?; + motor.set_max_speed(SPEED_MAX_VERTICAL)?; + motor.set_step_mode(DEFAULT_STEP_MODE_VERTICAL)?; + } + MotorAxis::Horizontal => { + motor.set_max_decel(TIC_DECEL_DEFAULT_HORIZONTAL)?; + motor.set_max_accel(TIC_DECEL_DEFAULT_HORIZONTAL)?; + motor.set_max_speed(SPEED_MAX_HORIZONTAL)?; + motor.set_step_mode(DEFAULT_STEP_MODE_HORIZONTAL)?; + } + } + + motor.exit_safe_start()?; + + Ok(()) +} + +async fn calibrate_vertical( + motor: &mut TicI2C, + accel: &mut Mma8x5x, +) { + const ZERO_CAL: f64 = 0.2; + let mut target_velocity: i32; + motor.set_max_decel(5000000).unwrap(); + motor.set_max_accel(5000000).unwrap(); + motor.set_step_mode(TicStepMode::Microstep8).unwrap(); + + //find zero + loop { + Timer::after(Duration::from_millis(100)).await; + let mut pitch_sum: f64 = 0.0; + for _i in 0..20 { + let pitch: f64 = calculate_pitch(accel) as f64; + pitch_sum += pitch; + Timer::after(Duration::from_millis(20)).await; + } + pitch_sum /= 20.0; + + // Prevents movement from erroring out + motor + .reset_command_timeout() + .expect("Motor horizontal communication failure"); + + // Slow down after reaching within 0.5 degrees + if f64::abs(pitch_sum - ZERO_CAL) < 0.5 { + target_velocity = -SPEED_VERYSLOW; + Timer::after(Duration::from_millis(50)).await; + } else { + target_velocity = -7000000; + } + + if pitch_sum <= -0.02 { + motor.set_target_velocity(target_velocity).unwrap(); + } else if pitch_sum >= 0.02 { + motor.set_target_velocity(-target_velocity).unwrap(); + } else { + motor.halt_and_set_position(0).unwrap(); + break; + } + Timer::after(Duration::from_millis(200)).await; + motor.set_target_velocity(0).unwrap(); + } + motor.set_max_decel(TIC_DECEL_DEFAULT_VERTICAL).unwrap(); + motor.set_max_accel(TIC_DECEL_DEFAULT_VERTICAL).unwrap(); + motor.set_step_mode(DEFAULT_STEP_MODE_VERTICAL).unwrap(); +} + +/// Calculate most optimal difference in current and destination angle +fn get_delta_angle(curr_angle: f32, new_angle: f32) -> f32 { + let diff: f32 = ((new_angle - curr_angle + 180.0) % 360.0) - 180.0; + if diff < -180.0 { + diff + 360.0 //if angle less than -180 switch directions + } else { + diff + } +} + +fn get_relative_angle(motor: &mut TicI2C) -> f32 { + let mut curr_angle: f32 = + motor.current_position().unwrap() as f32 / STEPS_PER_DEGREE_HORIZONTAL as f32; + + while curr_angle > 180.0 { + curr_angle -= 360.0; + } + while curr_angle < 180.0 { + curr_angle += 360.0; + } + curr_angle +}