From ce19eda3d749f0ade35c57139934eda18b57cb55 Mon Sep 17 00:00:00 2001 From: ersaraujo Date: Fri, 22 Nov 2024 15:27:44 -0300 Subject: [PATCH] Robot firmware release 2024 --- .clang-format | 83 ++ .github/pull_request_template.md | 21 + .github/workflows/build_firmware.yml | 47 + .gitignore | 5 + .gitmodules | 3 + README.md | 30 + assets/graph.png | Bin 0 -> 38826 bytes lib/CurrentSensor/CurrentSensor.cpp | 58 ++ lib/CurrentSensor/CurrentSensor.h | 42 + lib/Debugger/Debugger.cpp | 685 +++++++++++++ lib/Debugger/Debugger.h | 81 ++ .../EthCommunication.cpp | 220 +++++ lib/EthernetCommunication/EthCommunication.h | 68 ++ lib/Kicker/kicker.cpp | 159 +++ lib/Kicker/kicker.h | 47 + lib/Kinematics/kinematics.cpp | 161 +++ lib/Kinematics/kinematics.h | 65 ++ lib/MotionControl/FastPWM/.hg_archival.txt | 5 + .../FastPWM/Device/FastPWM_KLXX_K20D50M.cpp | 54 ++ .../FastPWM/Device/FastPWM_KSDK.cpp | 87 ++ .../FastPWM/Device/FastPWM_LPC11XX.cpp | 104 ++ .../FastPWM/Device/FastPWM_LPC1768.cpp | 34 + .../FastPWM/Device/FastPWM_LPC_SCT.cpp | 64 ++ .../FastPWM/Device/FastPWM_STM_TIM.cpp | 76 ++ .../FastPWM/Device/FastPWM_STM_TIM_PinOut.cpp | 152 +++ lib/MotionControl/FastPWM/FastPWM.h | 154 +++ lib/MotionControl/FastPWM/FastPWM_common.cpp | 105 ++ lib/MotionControl/MotionControl.cpp | 332 +++++++ lib/MotionControl/MotionControl.h | 76 ++ .../PID_Controller/DriverBLDC/DriverBLDC.cpp | 80 ++ .../PID_Controller/DriverBLDC/DriverBLDC.h | 30 + .../PID_Controller/Encoder/Encoder.h | 27 + .../PID_Controller/Encoder/QEI/QEI.cpp | 211 ++++ .../PID_Controller/Encoder/QEI/QEI.h | 77 ++ .../Encoder/QEInterruption/QEInterruption.cpp | 120 +++ .../Encoder/QEInterruption/QEInterruption.h | 88 ++ .../Encoder/QEInterruption/readme.md | 158 +++ .../PID_Controller/PID_Controller.cpp | 279 ++++++ .../PID_Controller/PID_Controller.h | 139 +++ lib/Navigation/Navigation.cpp | 199 ++++ lib/Navigation/Navigation.h | 46 + lib/Odometry/MPU6050/MPU6050.cpp | 373 +++++++ lib/Odometry/MPU6050/MPU6050.h | 542 +++++++++++ lib/Odometry/Odometry.cpp | 187 ++++ lib/Odometry/Odometry.h | 65 ++ lib/Utils/NeoPixel/.hg_archival.txt | 5 + lib/Utils/NeoPixel/.library.json | 38 + lib/Utils/NeoPixel/colorspace.cpp | 163 ++++ lib/Utils/NeoPixel/colorspace.h | 60 ++ lib/Utils/NeoPixel/library.json | 19 + lib/Utils/NeoPixel/neopixel.cpp | 94 ++ lib/Utils/NeoPixel/neopixel.h | 65 ++ lib/Utils/NeoPixel/status.cpp | 92 ++ lib/Utils/NeoPixel/status.h | 35 + lib/Utils/defines.h | 22 + lib/Utils/mathematics.cpp | 63 ++ lib/Utils/mathematics.h | 21 + lib/Utils/ports_v1.2.h | 116 +++ lib/Utils/utils.cpp | 153 +++ lib/Utils/utils.h | 145 +++ lib/nRF24Communication/.gitignore | 1 + lib/nRF24Communication/README.md | 16 + lib/nRF24Communication/archive-readme.md | 266 +++++ lib/nRF24Communication/commConfig.h | 297 ++++++ lib/nRF24Communication/commTypes.h | 179 ++++ lib/nRF24Communication/nRF24Communication.cpp | 391 ++++++++ lib/nRF24Communication/nRF24Communication.h | 101 ++ .../nRF24L01P/nRF24L01P.cpp | 914 ++++++++++++++++++ lib/nRF24Communication/nRF24L01P/nRF24L01P.h | 775 +++++++++++++++ lib/proto/CommTypes.proto | 76 ++ lib/readme.txt | 41 + mbed_app.json | 7 + platformio.ini | 26 + src/main.cpp | 170 ++++ test/KickSpeed/.gitignore | 3 + test/KickSpeed/lib/README | 46 + test/KickSpeed/lib/Sseg/Sseg.cpp | 372 +++++++ test/KickSpeed/lib/Sseg/Sseg.h | 380 ++++++++ test/KickSpeed/platformio.ini | 18 + test/KickSpeed/src/main.cpp | 66 ++ 80 files changed, 10875 insertions(+) create mode 100644 .clang-format create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/build_firmware.yml create mode 100644 .gitignore create mode 100644 .gitmodules create mode 100644 README.md create mode 100644 assets/graph.png create mode 100644 lib/CurrentSensor/CurrentSensor.cpp create mode 100644 lib/CurrentSensor/CurrentSensor.h create mode 100644 lib/Debugger/Debugger.cpp create mode 100644 lib/Debugger/Debugger.h create mode 100644 lib/EthernetCommunication/EthCommunication.cpp create mode 100644 lib/EthernetCommunication/EthCommunication.h create mode 100644 lib/Kicker/kicker.cpp create mode 100644 lib/Kicker/kicker.h create mode 100644 lib/Kinematics/kinematics.cpp create mode 100644 lib/Kinematics/kinematics.h create mode 100644 lib/MotionControl/FastPWM/.hg_archival.txt create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_KLXX_K20D50M.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_KSDK.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_LPC11XX.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_LPC1768.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_LPC_SCT.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM.cpp create mode 100644 lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM_PinOut.cpp create mode 100644 lib/MotionControl/FastPWM/FastPWM.h create mode 100644 lib/MotionControl/FastPWM/FastPWM_common.cpp create mode 100644 lib/MotionControl/MotionControl.cpp create mode 100644 lib/MotionControl/MotionControl.h create mode 100644 lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.cpp create mode 100644 lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.h create mode 100644 lib/MotionControl/PID_Controller/Encoder/Encoder.h create mode 100644 lib/MotionControl/PID_Controller/Encoder/QEI/QEI.cpp create mode 100644 lib/MotionControl/PID_Controller/Encoder/QEI/QEI.h create mode 100644 lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.cpp create mode 100644 lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.h create mode 100644 lib/MotionControl/PID_Controller/Encoder/QEInterruption/readme.md create mode 100644 lib/MotionControl/PID_Controller/PID_Controller.cpp create mode 100644 lib/MotionControl/PID_Controller/PID_Controller.h create mode 100644 lib/Navigation/Navigation.cpp create mode 100644 lib/Navigation/Navigation.h create mode 100644 lib/Odometry/MPU6050/MPU6050.cpp create mode 100644 lib/Odometry/MPU6050/MPU6050.h create mode 100644 lib/Odometry/Odometry.cpp create mode 100644 lib/Odometry/Odometry.h create mode 100644 lib/Utils/NeoPixel/.hg_archival.txt create mode 100644 lib/Utils/NeoPixel/.library.json create mode 100644 lib/Utils/NeoPixel/colorspace.cpp create mode 100644 lib/Utils/NeoPixel/colorspace.h create mode 100644 lib/Utils/NeoPixel/library.json create mode 100644 lib/Utils/NeoPixel/neopixel.cpp create mode 100644 lib/Utils/NeoPixel/neopixel.h create mode 100644 lib/Utils/NeoPixel/status.cpp create mode 100644 lib/Utils/NeoPixel/status.h create mode 100644 lib/Utils/defines.h create mode 100644 lib/Utils/mathematics.cpp create mode 100644 lib/Utils/mathematics.h create mode 100644 lib/Utils/ports_v1.2.h create mode 100644 lib/Utils/utils.cpp create mode 100644 lib/Utils/utils.h create mode 100644 lib/nRF24Communication/.gitignore create mode 100644 lib/nRF24Communication/README.md create mode 100644 lib/nRF24Communication/archive-readme.md create mode 100644 lib/nRF24Communication/commConfig.h create mode 100644 lib/nRF24Communication/commTypes.h create mode 100644 lib/nRF24Communication/nRF24Communication.cpp create mode 100644 lib/nRF24Communication/nRF24Communication.h create mode 100644 lib/nRF24Communication/nRF24L01P/nRF24L01P.cpp create mode 100644 lib/nRF24Communication/nRF24L01P/nRF24L01P.h create mode 100644 lib/proto/CommTypes.proto create mode 100644 lib/readme.txt create mode 100644 mbed_app.json create mode 100644 platformio.ini create mode 100644 src/main.cpp create mode 100644 test/KickSpeed/.gitignore create mode 100644 test/KickSpeed/lib/README create mode 100644 test/KickSpeed/lib/Sseg/Sseg.cpp create mode 100644 test/KickSpeed/lib/Sseg/Sseg.h create mode 100644 test/KickSpeed/platformio.ini create mode 100644 test/KickSpeed/src/main.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..4507cb6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,83 @@ +# using clang-format 10 + +Language: Cpp +Standard: c++14 +# BasedOnStyle: LLVM + +UseTab: Never +IndentWidth: 2 +ColumnLimit: 100 + +# class preferences +AccessModifierOffset: -1 + +# aligns +AlignEscapedNewlines: Right +AlignAfterOpenBracket: Align +AlignOperands: true +AlignTrailingComments: true +AlignConsecutiveMacros: true + +# allows +AllowAllArgumentsOnNextLine: false +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: true + +# AllowShortEnumsOnASingleLine: false # clang-format 12 option. + +AllowShortFunctionsOnASingleLine: false +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: None +AllowShortLoopsOnASingleLine: false + +# always +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: true + +# bin packs +BinPackArguments: false +BinPackParameters: false + +# breaks +BreakStringLiterals: false +BreakBeforeBinaryOperators: None +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: AfterColon +BreakConstructorInitializersBeforeComma: true + +ConstructorInitializerAllOnOneLineOrOnePerLine: true + +MaxEmptyLinesToKeep: 1 + +PointerAlignment: Left + +SortIncludes: false +SortUsingDeclarations: false + +# spaces +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInConditionalStatement: false +SpacesInParentheses: false +SpacesInSquareBrackets: false + +CompactNamespaces: false +NamespaceIndentation: All + +IndentCaseLabels: true + +IndentPPDirectives: BeforeHash diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..c3ca54e --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,21 @@ +## Description + +Please include a summary of the change and if applicable the issue is fixed. Please also include the context of the PR. + +## Type of change + +Please delete options that are not relevant. + +- [ ] Bug fix (non-breaking change which fixes an issue) +- [ ] New feature (non-breaking change which adds functionality) +- [ ] Breaking change (fix or feature that expects additional changes) +- [ ] This change requires a documentation update + +## How Has This Been Tested? + +Please describe the tests that you ran to verify your changes. +Also, check the steps you've tested below: +**Test Septs**: +* Custom tested: +* Locally tested (tested in robot's firmware): +* Online tested (tested in SSL full network): diff --git a/.github/workflows/build_firmware.yml b/.github/workflows/build_firmware.yml new file mode 100644 index 0000000..2bfea07 --- /dev/null +++ b/.github/workflows/build_firmware.yml @@ -0,0 +1,47 @@ +# This is a basic workflow to help you get started with Actions + +name: PlatformIO CI + +# Controls when the action will run. Triggers the workflow on push or pull request +# events but only for the master branch +on: + push: + branches: [ master, validation ] + pull_request: + branches: [ master ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + + build: + # The type of runner that the job will run on + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-latest] + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + with: + token: ${{ secrets.RC_PAT_TOKEN_ACTIONS_ONLY }} + submodules: recursive + + # Install Python + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + + # Installing pip and platformIO + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install platformio + + - name: Run PlatformIO + run: | + platformio run + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9929b8a --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.DS_Store +.pio/* +.vscode/* +.pyc +**/*.csv \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..a32db87 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "lib/nRF24Communication"] + path = lib/nRF24Communication + url = git@github.com:robocin/communication-embedded.git diff --git a/README.md b/README.md new file mode 100644 index 0000000..20f6ec4 --- /dev/null +++ b/README.md @@ -0,0 +1,30 @@ +# ssl-embedded + +![PlatformIO CI](https://github.com/robocin/ssl-embedded/workflows/PlatformIO%20CI/badge.svg?branch=master) + + + +- Firmware + - Robot codes with folder for which version. + - Board: [NUCLEO-F767ZI](https://os.mbed.com/platforms/ST-Nucleo-F767ZI/) + - API: [MbedOS](https://os.mbed.com/docs/v5.10/apis/index.html) + - Software IDE: [Visual Studio Code](https://code.visualstudio.com/) + - Programming Extension: [PlatformIO](https://platformio.org/platformio-ide), installed with VS Code package manager. + - Updating Firmware: [instructions here](https://os.mbed.com/teams/ST/wiki/Nucleo-Firmware) + +- Tests + - Folder with all tests and auxiliar softwares. + +## Dependencies +### - nRF24Communication lib +It's submodule of [communication-embedded](https://github.com/robocin/communication-embedded/). +- So, to clone this repository please use: + + git clone --recurse-submodules -j8 +- Whener you want to update the nRF24Communication lib to the latest version on [communication-embedded](https://github.com/robocin/communication-embedded/) (master), use: + + git submodule update --init --recursive + +### - ST STM32 Platform +The [ST STM32](https://docs.platformio.org/en/latest/platforms/ststm32.html) is a device platform responsible for interface mbed to ARM boards, so it's necessary to install at the PlatformIO extension. + diff --git a/assets/graph.png b/assets/graph.png new file mode 100644 index 0000000000000000000000000000000000000000..c1dcd82c3d6dd1e0f98743f7c762ce3a0288e463 GIT binary patch literal 38826 zcmbrmbzGEN_dd)FLnA2|Al)4b(j_TKNJ_VWbT>mtHzGWl0&yBol??`biSKo z9G~y+{P+49VBnrR_S$=2>so7F%TT39($_F=Vh9WF;ghlpJi$ES{MnAu+@n8yFDCurPEO8X6dM^fEJII=HHYghZ$q_%N^p3>3B$o!TL`-RR?T7z8<7Qe;njBNnQfPMQkx z{KmG{Y=$PbMy70T)^=B?APKthgSXbEPKFe2*3WDl`Q3!5{`mwycz^Y0b}EX0KH_90 zM5U>qL?L18U`lb1?JgS!l`tj+1%;r4i5b7j1F2u9gWrUxo;o?%@w2nLy1KHta8@$s>9aItf7v4T&qI=b698M?9BI8y(-$nWbsFm*I`u&{Hou(hGMx~`#-t+SI5 z71h;^{`~v-oThFT|K7>Q@z=1x0NJmOuye9;u>ZL>I92fKul!0DZl=#PA6Qre^MHE@ zb8_6}7X0Ug|L4%ZxBPjknxml%=m;1nEq7u`P)e1l%BD87cAP2llwpJ76Y#6g(~9*`+r}8RHCN-(p5>X{^OVoxS}vJ zjVSd0eUag(b2U=y^vM6mF&a@*Xxe|7gaQ(cuJmdKpUsT!zYeE!!TbKpRxy6kupIeq z^c&pq|8-dD6~g?#Mka%x21a%Z41Xp2Ux#z#XSn}sWHdAQ@DQ>Ev%k#0?T6T38(A1H zw{qBDRoidc3BSQEm;=}PbzO?@=(UI6JmVOYseB8!XFqL{;W;xALaAET{@4vdQ01@? z_H43Bna6sZMmma=^E!H=egjSs3-_QohZv__JQ#LpSBu_s~}ZoY63{qSYX z?L!xvG1?=+A4|7_r!MEh*%MSKt%mb)3WJ9pFcCJbrYuzbTErAL(I>0yB^dDyBDarr zjB9R-Fzv;9scuss9NlTZ3S#pZlc_Eatd z=^;vsw%d)su0YpWHphtHmO+OqQOr1RPV&|CtF5Ufjz?O_zIdax-tR8{YX-J3;mJHV zLV~!7$MwR$Ure306+O<9Gi4x(XyNb>H5I>ES4>L$Yc^mnkn-_g^V+g?zhtk>d+W9D zpps{A-I*ftQkVbtBhKj3+8sBUW+`dj_s$UXJn2FBiAGXOMZ^@XpMV`|U*rCJ8fg4R z(G`+;5`npIamXdGQ)t)5ykkY|l1l*-Ta2 zay`$W26p_?`(5d`aTs7lk0m;@|Buo<{`h4#Lp2*AN3|iObD$r z_FOtrUdZzx0fLNrbIEYQuPd;~fu0ZN7gnJ|;kUTWWa8-*B5vNY z_UGyGIN;`esj|`PLM8*_y;6%zYPlWk4QEDgf`gF3gEi@Xzxn&Y&|zSTD8a?JEOXQ= zbG-!4v@ytqYh&Ax|3G6O`rg*KzE5iTC_~^(1i0|M&)(+%w)7hU)W=p2OBt&z@BZtf4G4Bgzji}%6R%WM#RAC zFE2b-20z3rdvquUWIY=yY2(BFHBvvAe>74>Buh+ny7FvGDB%!KkCZ05VWCT(Sg}EV)}3uTjE@5_W#Hqm=LvQ~!qI?LfrqEUQfEwbWdhOqtm42sm;a z?L%V4-0j!3W%ApvwHu>;VU=hHL6z%PYMJFwml928-<#C1z!=HTI-e-2gP)_1&W`j6{rjhwM?s-)n^mUZ7jkl`7<^%4$E~p6Ek;ic%~^O#kP3 z5=7V7K4!`%a^;RzSWjd+E`3f_pCvk8S)3uNgs1#k>+l%bBF{q`M$K)7nQUclDY;^< zR`Cku;eU}b2GP65o8?@`<8Tr^uzCXSnJEf~;{5QiqGBbi-Tz#q|Jyhmri$uSLg~WwFZ(Xuu%5BrThW*eo%H*qDHeRn!H|Q7WH?W1g z`Jj(WE_}T%`h-ULos^nbh{IwJqgJU|;(hzM`*~q25Il}The2Dt=&(Q}w;iru zdpM8>{v^_hqziU7eL!R?o>U?4cCz;&U+0qwkIf{5MxkCJ06Ceqh+DLZse*Bo63@T* zNR5*HbFv=^3;o3&89yv!e@&CF1lj(qhf7J& z$3Hhf?Q^93)-WRG_j*SpI95U4o`P@U`EVz2NPSqLj^rwbC$?M(d$AYb$5+alM56fr!1Iw3p; z=&0|l_<6uan3z!*hzMw8@N%lZC5ne6`jOMbIG>KlHPK*9CuTUl`LzgV9>WTBYN?2R zXsCFzsbq#co2njKJ0&+uz(9X}->m0mW3)oPj#zBWeS4$A2FN%UsXMA61eMullY5GdPR%QGbw!E zu5ZqrU;3Ov*mCs43}dS|#j(RD$bU`@fe z>{$um@El)QL^VhV@V)#*cUaOo>I4o| z71Km~*J`$7HV zlMdcxWLAJWW6P$Eb10F$kIMo=cI#HoenKF2jNM3S3shEO2d!dat`v zdu?dp-c$f);a_O{KKjxtrsls6G@8|YX*E)C3!TfX(}48HAI6NR!7%xp+PR#5eEBJM zSrSb+TmHVo_mGI&_vAK)V_3C$*u^^we%`v)8hUz==c7M89fKouq^Pgt&3J`t! zAT-l@qN2p>*x`8LrM}m@0m?7kYa@ju<#aOFr|&QR%TZhjIWB(>If83;Wgwf&;k!)e z?R&XpRwJKDv`Q3zZUQ|~3c$Zr^3+*h@mRlpHo9$tN_MxB1^#R0PEs3O zG+1BwZtAUk>0RIkz{4Qudr?~=^94Uw;s%lSPq(>E1O>pD_*OE!Yx(QDg}s%5`u+`r zwd~_|X=pVcj6_5I!7ntSRQNe)!OV$k^5?=C@<#v?P0vsERX#bGcD~_rG-&JO`uT)g zHzDimcp`dU02O-$cpUK0=c%RgZxS4PH?UGo^P!XeX8A`HkhAHwOWP3em>-+BF-)O8 z=yVFNRpnl=tb8OLWB-Zd?9_PFQrL{jC4pD*M-;iprfD74*yb`h`QlJyjCE}JpQs9U z19E<;eJO%l;1N(mR;WQoD(v;3;@KG0RV;Ei?JH8%-gokIYMMA83>kp%?tj*zpZw$~ zJvF3hZxl3J)1C3#C63ahU;3_HE`8&d16Rt|Dt)r?O(|cK)7G@Z=jUhK7~z1GGMnxl zY)mWwwxKuv&?WreFw`sBPlK5dhfI*6#&t_w1m=y)>iLrj&0K?F60Fqg$r-z0w!Am6?eU)dS>k|wgTel2-WbQADAQ@(8O$3_JUfTiow671M;1f_qkf zTJ)PFh*oRET~mzz1jp!%P`y0)(Hhr6lkOL+nQgruKOMz4PUt7cr5B!uo1GwNn=X!Z ztok{`tM7Pcuv+z3MYhwzoq$?xM-~Z0PcHnGx7M zngJ{Tqy_lM^!~bhB?bujd6>*-AS1AYt!Z}MYNy!VWZs(3%dHx&8{=7RMdp2E(Vl!J z9e68KHI?lPntm|xc%+0u;2_X@>BfEsQ2=8CWubZ>+zZ9( zbkXF+fISdp&b3Iv!(-I@!+9xyk!n?5ZSO8TtiE4WZ(Dm?9(KjP$i4SsuMz{mw@`gh z$%=E?7$0MVVBTP3;dA)DJz|voC`I)G@PKjM8mE;w)jV|tmF!2XASjQAqF{Ca_PoA( zUx4xP`-s6@wTFv+X@%nt(v;AQ@I^zvH2C@sja)Iqxt35N2gnTb_e9m^HhYBNZ zam&5FYcY8AfdB`$qP`1Y82nk@K~nm)67KEqO9T4i@72ujpO0D`Ue&!R%qg*^6( z9#*F609tVS?$h*>u#b9#v~o18+7)}1OxRL6TL3PGncN;h8|6f6`erY&gZ^4r6x%Q= z)Ov=8aelUh_nlW0?min;!kelCq{&}sc_d$p`p07L^e)$`_>B-Uf-1+3N?r*!cO8a^!GYh$@UNByG~dDdp`n#A5}3@U?Vm_&K{qOvG{`^ z`fC4V0cMt4xNLs7IhCc9As)y0m`;s#{DDUtjqG6NIf!7moL5zkce~k9tz7zGu}H zOD%1(GuP@b8VvyL1<0D@P!(Z|2`DA~j>LZ>Qf(T@*M0Mi9&N^>O8OfEKtN)q!b=|iKQubC^zg1o!XoxPf;{O&o;!x!?5lpO9O zVK^p%;FFC~ubv~6CKuHB$ z)iZOfqiQEUnKT_}97V;>2-IM%-;=*4GZlErOxQ6^Tk-VC;Lh)Yr&V2jOhjMQk z%hqPFWKry~4H9FRHP|r7zv-nd=j4C7<{F(bBT9nz5xe-s`FylUzO3M1P;h0&ZG7Mr zjyfWZcmvWzzjwr{z3~I!$z16NXa@#el^Z3}Tw%k-s!GQ8_fEp#l(6fJaScvF7QS0` zhl4c0ESDAC0Z9xLF4rC6Nz@nuxGmm=%E$M{%j@-;iqnIQ$7ycOk6=k<0&##$jahqFSq?ER zeDmADy`x2XKbM-rB8U8nCc1-QQn?i*d55BKWJG&VB7T6`P!~`&Fh^l!;I^N8;GHY% zypFO?lfil$EFz+GaTsP8i2CyQ4HgWE_izOSW*%d?1R)*~=Oq8m5dKQtxHr4~NwkFQdo!&jbV2qHWb0cr%T;n%m4tFPbj zR&l6)P?5A22pxrZ^yj;p+o{ss?++tpwQTW2JRqy#M%*!6A1&@GhYBz~#e(diKuGYD z@P}`Pv?DEq4V{ZKs~cp&t?%NSLj)u#l1xPVD{b``n7Ba7_mlnn^SN-Om>I0@kjwKe zUn!!|Zv^>8)RNUsD-llr1`|K2y&FB`37y-Lw_%|;97%mDLtd0ZiUF!0h@)0U7S^jk#fFpO!xucwmWM}}P(gSq$?R7lhCgMM*2r$=M^k?F0+NSHP z+ux4b$>-H-q6`+@4J1zj(+pek03K3H~@|pc+K#u6eGGUl%ur4V?TF1^~)<} z=p+&zGZLXa%obY)FCK!4%0MWtJ;P?V(obs&!4vWEdf98dKI-0B8xZ&odJ|WNCXP;F zb-ARw3?L-4ixNe;$l@p=8`drr28y6#K}9qx5n3I z8?R&hCYqvtu=<>@IzH1Y<{0Zi1J3U*og6uowr6zd{H!UE$1w@yo@3wQs*3EaPJB0k z5)CBr)W;8G zD{35ieMuLyQI<($O6h!d55L=wq5Km%UrTHy~ayu8gUqv$y-VXyKI<=y{J8?L2pYZPo2k&eBkyr`EXL)pL zTom@gQa|zK-!^I;nHrMHf28-xL2^=yXur&6Zx-BySep5jodN?Ol`0HLr-jj+JPDHW zFmwO=7{2HGFI97t8V!jxHj=SG*sRm4z=$Tzma*%naGH|Gm>v;7D*>EV~JM@8bE zrw7*A8)yg6P0r17y;zA$1N1Na3m~XJ@#DF~| zP`%Ejw5~!yj|=0O_n;D-qau zhqy|7n{>v-v1oC1rU+EhJbI&9zQ2^_J3eH-sr`~oH-QLt5-kkkD*XizSbgx?`zjsB zrpw=Y{bG9Nz3Jt}xuw^(qO>I!}3`f-#qqL8L@B#xiJXou17RKQ;#}A$ssxJfd)yp zN8M5L1;o3cEn>+QxUR)#{$8w4%9NZDT}Uh-L-=9x)(jzTW6KR|7QI@{luE0SM=5DS zp6acNc7ydbeW{BA1_&fmL-;`>UY{!*iedshcCLc!GzX*2!28G@($%f_i~1E0|M{^m z&a(qkTbVWI5-Cb?*T{u+&V@SSXu~}gw=0{*TFCpLsdogEfs0o=Bd&R5LXkeBq3@m57Ud+L&rtw&Dp@uGyPhCOGL?d3EZ6yh zamm%)Ic(&Mltso{s%CxsLP=KbOzxzgKCM7SgSDtgzg4v=_m^#r|aB_{LUsL1-cHq-(?e{WGNCfltcl4mq~2*&Qr+>E15(> zW)L;tg9eF%-EkFV$>GB*(jyujpF2r!c2LsX*OSttMao6;~?* zs$@g~TA$e7^4CPJ>(hk)xrMP* zb@Y-CZ+J7G*Lxi2l8T%KTM7UMsgjv6y;bDL|D!#YaQBe5VLCB1jwT`hQwgdnjnWA6 zM1$zPHgTG59Kv){ar9}ge3(&5XeW}-_L?jwolsJYfqxZ| zk;x#y3&1MxuJ|pa7Y5k0f$9#Te2_}Cyupv!8VU{>2zVTdzbr4^NRprNWL0!hhd){Z zhFBs1=e$!tp8fNnIh1%qb7(k>gH^Y>Oob|)^dwl#I^-LQ!EnA-Zdy@DW6K>v8IG@q zcZ?Br5)s%s0pVCMOtuya1ZEZW*TatZrQ6mSzEwA7+`f0U& z6|U0F#&J_nnaysHU* zqB!XUlb;luvB8)bbtI-nQ};naNM(QiQ*a6Pw4!waNNaB2cj4 z9a%5f_8*6TUcFb_XP0)XAC&eZeGZ>a{rI@=IUVIKB~o3vI_n~`sF-4#kbFGAQrYeP znzQKqN7W%BSR)O!TmpmCnr7sX_PJJX0w)Bq znxVJS9(IpzI0m>B#&{>iwKf{4|B&{NO-v3~azVGEyVm2XHs5ML?B4fmPAZZkXH=m{ zM4=n-pfZl4POl_lQOiwGPORCGu!lD0=8g3uvv?eCex&2wKIUa+k0D>@3&ZB;jBNbWn>Azs)K-`&eP6S) z`%*vQ2#%cS8SHCE^QN2QI{TsNNKx1|bNHsEt2S~Y!QVEq0N-5on`bQ0k{7VIG>V}oJM3QKvfEbo{32S+Ddjb# zrenhJKfDD!l)feH9&7Va8;xyn zrDOiXGDg{_H_lDbR#%Cic!w7LFLO4huV1~ZPsG^DU5Lu3p;Ypc%GLHiJ$!QrX>>9v=ubLmHWlNod z1kMK<*$r2Wh^#zAN}=`@LJX72Wex~ZZDdTETGG(Zg^5^EsjWvl^8!p=cdr5%bWUhr zn$XMSk#iPWeBEDOs@kW&XzQ8<-4&`zn-b z>wY&m92$E;Y2yOn&BFAgjCVO+9kF@@fS##y+zdV=qZ#M~T3N63#W zJoQS{*qJ!*ZHSP8`rWFD`qT$}Vdj3q%R~BB`sEMNcSUs+X3iase0dK?F4YcI@`rI4 zvQF1?P+gOrbgwM6YSmS>+fgFElN1vfbV6Do^55{k5N+T%%=^@I zhpmbbaI63|gQ4`qn@TuU;}|G#$zB4|zUKndvKuevY%XWUk$7+Eex5YXTMJcp;ybx` z`{vj-LJR&J@p1g5Sw|#lzckn!GPf*Xz7gEj!TZmAm_d)y>O49vUmZ2meA*sCsTs#- zC>9Gpxq#wQ5ln57XMtT z^P-KHR(Jt)V2rJPpD*o0qqEjrb!qg(FYWE4)%<#6|B#PJX zWYU`4Y!Xl20VJFYqV`Fk0!Ho75VD5)g9?%I1^;)UlrF;PkG`6nD6ttOEQ?)hn=kLtFvoP1lpyCELrv^Gs3i2USh$k|a1+?uy> z4wSj&;F)WU^mB16+7e z7Tuh?$Y(3pYxGJcgRGDH*zQ=MJD^Mb817B%*pgNsF`WHcYvu@dK(KT)_#AtT725~P zKM|DWGVe3d`Br82>2B=lyobZa_Bkt#9Um4T4p;-U0`XPgw4rE|n2p2`XneA)fS3 z(cz_j^e_(iaJ(^0b|C)RAiU$;+t&7B;7VhB^qpJhL6BTWDiZth-A%D9klFZn%Tu{>hVoh=iDa=WZGNNnwV%zfYjIleY za#&@OSE*fi42_>`mR78v+QF98!DHqn#{OEZYg;u~hB$Skz4%dMF9>vOES7A`r;PL>)> z-vd4Z3j#S5fAPNN5{~3{;oXqWQ5)Zix`I3LT&NK~TX^f4e!2wt{vXeaj3IP3=y!wF zq^fwAH|_|1B$2!J%sSXd)WXkLo@4*Pjd)+!8djKre}FYkasP+&MQ88LifA=84!9b$ zXR0`ou4gwx2O%e5GTRi?#jY~4aV-sGUe)HGhf5#tE}%e=Gji<9#z{!egSe2yE{YN< zyrzGA?|hffS4MsY;;F-L+1AwpxWcBjUIye@n!twqeub zWc@gg0v%&ij>y>m*azGRMUJV35aIF?@0mrYUmztV#GC%>d-`*U%M2v=J-jwpHw8Ki z3u$K{24MmUashP_l|OZpf4djE^QRZA+PUV)zPvHGHFpsy7}Ks*;RZ{nQ)D{N({)72 z*4dyp2wDsf(+mt-|Bw^BIGBx2ouP*J1=X>&*@bCBo?fFOW@pb1_cLOQTQt^fcb|I> zj}0tEaBZ)!t9?kIc^E6-rNN%}<=p|#F$>-RyjvGk<0iL)_-N?Ew+eeKm1y8h` zF%QFhQqI)B%?>LSx(hY)i!HUAH;|vm4e_g~^&3x2>z8VyCQvAh-&Gz_dRg!CZEiJ# z>+BFU>%o$b&oFYNHt!3%#N?Z77=+KxpAME}7@UdVc$G4sWFdGZ>Xq-!c$7B{4eexj z2g9_PnlO!iRI|DOzG99bRLN6jWlYJ7;IF7>eZ35KpV;mie}NvX`Q%xdu4 z6B^air9ykUt3?zFLJ6^^rK+NP>i&E;=)R6BlGU$i9uFuF$3~I=*k7&V)EKb2R&F)& z0x0*o!5l`+SZIc}b=C5z`_VhUQ@qM5I<53$R4j3CnMV6~{pjU&u_5XxG)7-m%+zWn zy^c#rDzDo0V*gays&~!iRFf}g#V49fXfW_rycv2vF`oRFXQ*4bw!;VepBD4GR~5ad zQWS-3e$=&pC+Q-S-!6+o)jkumH?Hr?nN~}#wwc16dcMiqpHb`AEe-NGKoq0MFzfFp zTUar9Q@|zvwL!DExS~qNa--$_In8u^rO{gc?Hdk;st>Qhe7OyS3uYa^HAS?1j-YMaw_ zh5Zf-eHO0QipfeA4hnk;R7>Mx+108C+c}@YC{rcnFmvRBLEzPI63**xx5rX$cCo!hBM(vH5)%kf^$)z7Lm^QUK-iYL~EAz4|-UL%vut~TyP!pPNt0@^d-B# z`U{MZk@WL(-Jz||_mY%=!10NKaEfG#v`}hU^pN-ZBob{1Porx#yDc~F9GxF~zMOyf z$Gn|o&*&slrKV+s$08ShJyd?Dd>?*Xy?=8eTKof?|IU#4=fVls!%sXfuXS-wk`|Ss zoP?&R=EH9Us8p7gqV8ZXPeQ~wpLVH@a z*veOL+Yz1~4Vm(kUZ`We7RIURD+-@`>ZxtJW@~WzZfi(7NI0&ON8~9c#t{Leh3w|N znO?c-@~uxlmR_)3u02cIpRV?Kxcbu80Crls*zP_-rY|co!Bl&(;U^ksj2f+`h3^UB zyh(x1v-~tpC)Y@J;t@vZhWwElgx*#}{!~wx@i0`LTtHXqfIQ2g5yf~4UM>}iGCZis##2d;+0h1`z|_7{`aliR;KJKpvee}bYd^O{!Y zXuItLPC^c0nYdD#!0FkD87n@U`2G2k4f>Rn2W8gb$7f^7nldgo?!1|-a%tALJVLiU z`M~}hmt2gT|Gew~Mm*PxvUCZx8abQRH#y7ViIO8pDcX(q*DP z?vgwm<(YwMpp{0X2&g+O_8LCoilQFRKcyhQxUq!Pj=p$wk~U(b&h$pWoA~9K?0AI5 z&?BIcy68cvS<}=Na%{ScQV)%%E9t%wcV220LJtNZMh7}pV(?3#e^vD0GZaN*6_Rsj zD_GwLcOG}?T#N*)EexCt4Pg}{V`iO?K3zQM71&)}U|yM;)jnS9 zGc#CMmlKx?AYaEDg^1Vi-z0j4iC58WC#i7OsdI`{En#$}@YO%jCjP9M8THZj zsv9K%!>$wY2uCL1nP#0bn|{?ZR)XB~A3tkh{-BL6kYd)@@PAA}89E2L-zUii&aHlj zqKi8aRpL2;TmvkH(&~2w_@77|6{_W6raW=$^I4~D(4BbWA22Omh(=&-1(B0XcKsbS zvix)jJjB;n*!Q`aoCrQvf7T0x>$?-#3DgakIu4$Y81K@l%TeW{De|6Ue~$=6@G_c_ z%#0~9EHU4vboonv?&F7iE;Bh9PGj} z?Wrf;e?Anj80pB$Xia>rNGfS}UNtfBLagy!tJbG4t7)5PR}y;vV9}$Q56{bgX9WML zqKkgP@H*cZ*`4a}x#*cz=BE3Sa_Sr>Hh(668v7h$y(f_yW=~8iJ4-;`eWt%8dmw0A zVTpZXvsZBj9e+|^yZJLJmQOM}N*K}h$o1=#gv5_x=1jwHjr52SN48wzGzp+f<@`uG z%^ujc!0Ab*SpwRi$=m(XHrH!pp*&FZQP-`{LZ+aR$PeS0Hgj7fdAp6cHOl_kG1jM@L+5f! zGVVw6vI#WGAzBy|6!Wyn%&qa#imKCKp;&D^%1N27w%4}NYcPDwk{JsKCCVt9i*=RF z{B?FQLD?7vcoO|V6y|iT%VV3V+AJgSDiW4mAeV92nUtLH4s4OM8I=DvGkZXLfw zGot{jx|jKX^#h1(bORzX?FVxU1qM_plNE#zJxbOjzhivy|XxIzdQea>neT@3JRnb;-Z0WDSxRV|9k=w1)j*|zT5wnu78ag z4-dGf|LHpY+ZZn3iHe#V0qOg92*2OM{eS`S@F@S*q5eJwAt<^^M%}?l{WUf}SA4ha z8Mv!+NZnsnBY}rxib6N>5Yb~I7e~$d!y)+8auGKheDWr7gqh z;nt18WI=a_W1ScWTrqLaDU zxSd!~|2hFW7HqqxP_tP4N;FIEduFeBlc4?2QouvKvH+@$(NpYMkQ)YVAw<~<-rJ36 zS1l)u>iJ2r+2TQ%ZN={bK(R&^loY>%M|}ZGuia611R|aRRcAaK)ltJlsrd*<2zO_carZOmDQi`1K#lM@Ar-r1_6j_b1HCpu~ zajtH$T*m^n@BPl=qJckiZWsX8is8KQ^Kk|s&ZBp;_~dh5eboT~nrWy(Wnc9L61mS& zMm>vc98K&$-JFX7K$@F30BWQvE?eJNKRzjs>q=y14i5nOcew!6YbJfELUH`gmX4fS z^F};V~53dn=Exbf_kvw=#~yP@GY(wyH$aZ=dKd;SmsnCt_sC zPN0o@9Pef!gpdkQqi7_%BdF_m^RsG@`M7)#-1( z#DUh9&LqxvS5jXf@t|)w+YJxIy`(eg0CFK_UfXY~i#tGMnaZ}fMQB3;rt+{IO%6M0 zSRfwp!cjf#-kkz$JWc(iEHE#NkLQm7R$7u4oM4Z<0PQyD`4473&&jHBF_gcSw@2mUFYb+uIvK05xYd!Ym z)R0ZtSe%K0#N?u$i1mpoBk}fO(qF&;8cz1HDKoWk=l6{rs=ZHL;GyoIJ@Bgcv8#Pz z;(4T+hkO&eP89`^M5|8)NH*e%oKb*A54*gYq#TJEqnS{HIbpYL1K-7JAY;`S67)G6 z&A}nu1&S1=R!NX2_?~QA&si{w#OwmP(A9S!MGiJZnv4hX!Vv4TA#<2tBOEpgGqsj(m-$joZ#!F6+(l@;sF|QXvo3Pbr91V^CGz1@fxA{E$dLVax7tkXvFJ zSZ)r+&0ABtzP~>9SiL~!LmONew3g}CpR6)Fp92N%YJJ}uzQ>WWwBz5t*Z|G^cc9XU zyV4b7liQS)`kZ^T;?a#(-*K%1T^(u{hYgpE;rM$Pn`|*)1u$Qzw}wsswhO^u;oPQW;VtZmYCO71s$V0fPa`PopkqWtaR zxbV(B^E7X|D_z#>S^+D(_&(6VnRC|hI7f-HCSM6j^n(!*M+gCJ7>!&)$7~lDoeSO% z7`8}UrS-)7Ihy+?u@Qa|ct-@uA8}OYd~MZfk?D%9N5qJ2Hk<&%dt|8;$db0FN~Px zesCh{Bu_exm=2bp{xUV)3Euawg!l|f={Mopm|cc*5kv~|@A0=m2iPu`%ev-1izubP z{Z;1;BlC<)9Jl!+_(Mn=Mix#2X3>S;T>B)>xN|Ubo4*5S1yHtrrwCMnYHTb@Do&e| zx71J=hAWhjc6d4HCg~Zf(kcAn(EBWb`eUe0j4$GjJs-y-hL)Mz0VN@`@EtOKCz*6j zF@8Er^qXO)maJGoJ)mnhp%tskQcMd*&?@xS?FK~97os)k_Aq+?P&oZ!W(y}Do*o$V zc37kB#+RypT{uDj^uXXj4^ilHIL_Mxn^`!P+A~p^XTLEdaWrS%@$Y21j!pmQO_8G& z;(-Hr9I3-z;}%So>vU%DZn$12Pj*0n-ZkFr`J0foFE`|$I)8Uqr`Z9hnJY`yZ!m5} zB!8XKV}HzJ$|H(DLk2-}JFoXikp$8tje)ygq7^IpayGKK_==yfkxHbG7y=s`(IdEs zcrmb1eG*t=3K|yW+~Ho?ifQUL)Jk&?Lhu?37_M_F(I9e>iLb{vEOcQQX~-vWh}k1H zZ!8OZZiM*X;a(jnEOhyS2qZ0_y2(qocE2JlTSoxMp)_A1m;mYmfxwfz{Ua}+*O02c zruiLv}k&a`BMA)l}^O#%TGr3&}`cx8nl0pC7j2Ai6_$`K`ulyUs;VAp2@8q25J&RKv zSa1}mde=vz$Qr*jO=V<~eE)ZMsv`Efr`~CUl)>O^XOztJknP@O* zP5H8E1e$RYxFEwAX7H7w7o>a+45s2haO)ITvW~z|Z|dy|M7-c;a*YGsIE)&ViKc_4@^Yg@^`=VX(OxYrdqlP-wK@y3Nb(wn-^NqK! zQ@x*X#xh6yxM-0K17ZH*VC^Zo|5&2}q)SxD<>MPMPXkM0M=b_DJZ4xJbp-h#xqzN1 zUQ&WMv{4pmT0=0q=uML&x)qDpA>%hOgPs{qhs#ZG-;~g&rNp85+|^!5ClZWm#R(65 zrWFm>*x!W*zC(-Z+C}NkdJsgQQZ$-;Da#gC<-IbVXcfm`L6vyf)ymvoIN%z`=t*~6 z?TS{pX7|D#MD->^b7~F;gIcn$coMx-HVOHPJR9y>j>*Jj8*M9@PbU4tzrQ^K(KTL` zggT+KBcbMT%Y(@Q3dco$@wuCXjHQUXB>ounS~EE58s(fgM|sAsC@rLZR)K0orOiM9 zVXbiB8b#j5VHKQ(be+V`QFL1hU4F}O7Rl8gzqihM#pF)e?6+&b!1eew`hf3m} zL~249KzW>Jo|JhfX|QQ;ie)j~%p+?oU-T=XgX1kyw2C*M$kx2SjPzhqWQy@2*(Y2A z(ckLpdsXj0O>?XT*TB`$(~&gYsZt%v52T|T<9R;17xP;Y-|_o;djQ#n#|++z#iFn+ zGz2w$_5Zc^)^AmPUH33?Xps&@I;1gVehloUTdzo=9ps)>WcW!T&RG%%`=v##0Iu64S2=S9Rm+5b^LVA0}!-btX%eQ1#ziOOo$ zS(H$g;}&GUdJ(tD=`w>qxpM7=WeQMGT#?QcvCmVj^8PDJ`Z$Onu%L#e#Q{?Fk#d^G=Oa2dMwLKN#{0UJhRXn3!c zkXoneN>I3BTnmc>Fk&_~W~sgXnR3!;mk zDFhY7`thjU`O#~lM10Hjm=FO8W3nZ-;eVz^B>N*u=u^0ZlxVIK)M%)8`^+z(Ou$|+o zlrZ_61yTTIKJTU^xRpE}GWW}fhY(@Ot%Y&Bm-u@Z)-wb;$LWyvVrenq#IHsYHoiuc zC?H6LR&2u4xs@kF58z7)1o|5KEWMt>Ny+c+eSE~$g7}u_2EkPTYUp^z3qePf_Z^fC zUk;|s>}djBrY2g-D-%VUO2TD;fTy`F!4bGk$ieZYrTzW`Y;F-N<=g z?NtcX-?pn0x@_-%>~XyOcv|3s$Nt?ZzxUah@6#T=w@w0Ad?xv zLMlK+)Nkj^@6*RCa0UWrR0B$}{x{f-s_ff6viu0GMwepp#_SymAmK!#8={ruqKUnSg|kZ;9F6rkK)W0r#m`N0nJ-LVJ18t*MghL*1qCu(ES`g#n8UMr zTSMcpGzRt;L30qu=mp12muq4^xz23|K_=+cwWbQ8NVb1!*ivae_>d<{N(iMm(L|>! z6J1wrIW&NUCo56h7WM4d=872US?PxfJ*EH3*)C4vXT zOgiQB!M+`iw?Zd*-b=eEtNNKeaGTK5C6_37wCe!s%_9(N`iOXv`*#JeYmpSA=-zsB z)#n*Tzlz5!;`f zzB_)v+W(7Hb)a>7nP{L+aa>@`M~u(k(Wsim7rE4i7BPJxz4kIz#A*7HCI7*G3`BZ) z0an04U9^`E*grt3c$J$Uc)@>J9gZgZCd2PWyZrZuUiw;sH(ClEC9BJ?GR-cIHZDfa z-cYp@JY5jggR8h_7=ND$B9Cw^N&Ky%5XH~E#W9?_-pbJCX?4jQ+l%Zs=#Y)yD9x|F z4JhPg>04wU)-wjtA0}=+?e!-m&V=(vLM}#f=9fCoR7fio9lVBmVP2gZwHUcR@Q=o5 zQp>y-iwSZEzzI>u$8+c|Tc0dSXskmGNAs_|CyQ3xZgYz3^1Y4Et^T*jZ;qt?CV{Ed zqr8vJi4)Yp*{)6gofv(rq!``c7^6}ihN$aWJqEuoh@FQRiZ+j-#r%Y^=n{?=g0B|5 z3Xg#WkW^56WlpX5%8N#7srdplUzkqkWOW$7v)6o$nqe5gZ~oN}-EOX1MX3`fv0QPZ zvGCY&vTv3y<$>vLcuOVdtI1{8YP1XKet-N$*luUV1L3n+zoNnH`~+EVYKT+Lvl6z+ zZvNdK4|QQC!jjm_6=RWJY$9#okHul=N$S9af1IMV84ZkAzL!~bfw`BQerOm4gnLN9;ReY3n6 z$cRnDxLEPJNp?8CXV|ku@8e16ixZ)l9P_S+j@!GNl}3}u>IU7XRI*FE*Z3$(wm#2& zlxZ^CBHht`Fcw7<*1V*5Bh2A@wV;(g^QF1{lbFAw>34gKSlnj>%YW3^&e;7Ocw=}j zQN8rpiRE<-e+98j($iq$Bb2*B=bi$3`TLNZj~9E*dVs!NqOY^S{Ad;H19#$v$y-u! zW<3Q%#BrjD4{$L<^@b^u^ycjRu5=DwV#P?1KIf^ny1`>`PPVQz!aP{Fl^3`M^_IRL zwCo&gnGR6(_Z|uN^w>m~MBwLBK=ZH+pr-hk+k9gc&KN!y7FQdyyG&FuIQe{^M39jW_=O}OT zJ~_8w#1tKVcl40)FmDz=`L@KhOuJFTv-+fJS*OJf=mVuou72+G?)j7kiCIcwvMA&= zG=l@0S*50ZzrNk4+gB*!iNTXnnvUh@%5lzBIoP{-$`DAdT#;E;F3AwnUp8>6DkU9qK&!TTEpQx@TT9@#V$ga>6k-#m08bQru}YF( z!7|sXmM!^#s-7qqDXRxGqoXDXj}CiQE)u!qZr(N)Ucm5Sx|=zrS1};FK^rm>ue9uv zc={c&ABWyMbA8%w>Edop@X330tnK^ zHM&ZGX`wK&9f}b^->5>9$yP4V;hI`{aa!BZ+1Rc64B-p%PC7kqdEvW6JO?cQOO$zE zgvvf~L6}NoTw01-VJ|gZqo{XCuKbNXo=iidhjt5 z$(T#Fv|()}B?5J7U31muF$?TQXn_1(4=o`y;(@k@Yc)veU>C30JVO}b=y-K*kf!z7 zP19K?@x8r%lQsi|@@`fUid`w{5CPiGRM{Z`n~p96u0wdMwf~qH6K-TYlL4zsf$;}) z*(fO9mQWOj4sx9D?U0X5;*+e0g2gzQ?EbcMJR6M+e@Sis1C&6W2uqK z)v-oZIDH`wh0uR7Xmu|*pcV;KcUUsKMv;pF`+V<|L0uuUvevR6JRO2N0gJjCgP22G z%lS-Ac*Gj4OuPIZZsAdCl#u`osH&HNn_}}gyhtLXK9K}^Jqygew%|jhs9Q6&J(Fhq z9$PspUsJlw((7DsM9#H1dUiOQcz5G>N*aq`*r&3>OP4m*)Z;J<=rsK7wzxsq1Hn%! zob81jkJ{N3-f;_rUA*~0S;C@4^#f10)m?j;7hWcTBkh8_Z6gtL&6a6zM;2hOW?^;# z??d?YvAZqkb!y(L6{!vyv@Mi;OWy68Ab%M%;gNi=V0-N2vr{LAa%6}1)~W)DxoWx5 zeJVz`JBZPvzl`A%f`5@F_vaoGnE@=5afB9-_g`wm=ZZB$_D#<_S1X5kH$ITRCP_5mMAHDCgJ9~gj2kMu5TS?lwvfkbH4hTAO5=f-gW*UrIYHO9SY49X@u@-&>Xz(Bi^#P;vWRI zmv&9)_#rH0b#~LN)Opv-h4pjavZZd@jTVVHOrFSN**h^g&_0tTj2aF6@^z=AX|+SR zjiS?mqumL~tkZu1#jM{`lqsN-RzyR+LnZ4)?w5B6@aIMx{=@eb1fJO5C&V=O*k2mL z%$P+G_)^L6=8?>VZRBk`eJgHEdwWNoI|$@hh94?~9HLTw0l&M!%g2z$mZar)R09?Oq`? zzlmCku)TM9@RkX*92rXZ{fr&$fyB@>LLqA8lRnAniNp4;O?J=s%53G<#GzUngn80sI)qa!>F|h3lHozu6e@@~ zD^8ub4*wPBEm5vCyWT;R8VBYev&*p~B!Q52C`>dS}WA?ZblEK{6|4&m-d)BbRu4xV~l zZifek(AM62!_);@Xto>qP3G^pgeRh!Yjb*q%r`j=OH0Tj%j@IVVjp+pkO$}y51=E_ z!_!5i7MBt*?pQp?(qkf;XrAdlrJnAFtJ&<&&V2A{!AE8=7$;&-J%<`^|9 z?AC$}-T1dijM^*56;$dK6+B`DxCI`IOt#;1J4 zxfjx-WAfFFS%_K0Rp`iL%rusIdzZ39B-uIhv2Q%!UR}L z#q4jr!r4u(A3C`c%8$MO952aX0SIfUd1UJXfFQYmqN$i7xpE1{RIbAK^LwE^Qa4&a z-Nm*27_{sHa4(G)doI3PH0*#7$e>eOtXO%u1!%vascirXJU=rL6_uhFWYjEW1}`sO zz~y)T)pUB<)1do~fb#~3HXJ4^Rl`@ci=4Q2DC~IG7n@~f09kZgAEA^tZuS6d3^N@{)xJ31d+hheb$=`f0ji_#47AOh^-j z^Y6JAc1vfQp$2ggZR$ypd19|Nj8)azkqn2r`3oN{g31bY%R66_R6&7u*O17)+U<8W z>#x@jp(51NgscPEo7{QdwR%3V=dJZ+j6azqM3nxjNzk7YH z-}AkvZI%P&_ecCUoP7^1Kv8?rg&yuNLaNdS1=7a7pqqLXpuyJJfD-!`ALWLTr<3B) z^1F)V+g*KNQiddauf77PG7bre?f}#g;?#>YWLmw?-+LO7LPM^rt;W@onanzYhTfz# zQZ4B{>0+M%T=;7sm{bMheKQEddJ3q@V#M}vV1VmAAR%#ay2}O-M$FdGmSQ@)o8jxN zmaQ-Kqol>2ORcYUMC6Z+piMNoO^ynkGx+zE>FT3XYTZ$WZFL@YHZLU^7IjKtk;~c6 z$u_(CIK-a*UM(st#ZsknOB#NmZ6Md=d{a4_&8Z(y@4qA9J52Qj3IATbQt32uBMu=D zjX-sR7*4*iGo{pjqQ~bxSLFG@l&rZ3vZu70HE7QXw7Bl$02Nt@gSBCMH%vkd2bcCY zK7iZ~6#fu@dIEv892HtoRaxp$XSh>D`Ff6u+nuJ}2?E=SAo5OSI`J1q8g#lqZ(z30 z-oCKo{5wFV<7~bFrYlS~mGLJ)WhWuVKy&0B=XZcTfslx2M)|$DmQNGAhFmrX0FtdT z&WEV+B<7o#P8{Vym(O9$yHT2sm3e>kYpF&-RxRzIY!# zakz;yi|>>%YH*>QbhHCWZ}k~7d@};By{i>!{%*RyTdw!(I@YbyC`LP3w<<1j*v@r^ z@4qK9sYAZWOqT*Mr#R3bpr2G@Qf6fz3lv;>0kWS1BzyW)D1gdgUmiK0hZ`+m-!n25 z!wKPB#gGENr}yP{o$F`k0Yt5Pmf}}$|KO}hbVu;L;BRX2fW}LrjYlV+KTpMLJ8hWL z@e)qs9{6`8fGQtsP`-u()h!X1KDOVruMjnG%6~{_h_r-e81BGyz11JoTTqZej1_E` zS{G_LkK`BlKs&T&tu1exdP)tVhTd_!L3rQE#<;=)d<1QCPOuE_<1%-G54Urr|}pbv%4B2eWm6(^8PZ{~)F)h100gL!+y9 zAc3i6_G#ot`b^l-!)2;fv|_JgBWS|cGi#NQnhz%DmZl)$btc21BWOLWf7zmmWM_}M z9;|iWJqhU*F)`Q0{Q{s&6&ke%zEg75+piq6l5?Q3oAY-MKR%Ljp`mTLve-b-lHVzp zUBGAf1F`Q=V%+ix=z%$S=f7M1NyERBLHcp(gDuV8e5|Z5%eTL|I)6^?um@U4ijFwV z`lfRa-~4a4O)=!r+*2K={dsDHMEcGnay;Iu_EhqsPS8}({@81fgD#UjCdg0%0}0WU zgk9+ zp<4ct{md69kyK;PfXbMp`@`naFit5EcDJc?JaZC_Qu+xS#K1m3vf{EB;x2@+IgAIK;TvLvg@U6;k*OBLM45p>gBykZ z8$ugxdRh>i@?wR`5jwa80m7UM$JG?KesJ}Fv|qSD3(){~zy`4(3JgWF2Ix5pz|>D0 zTPm_3K%?u34=5NF&H)&W#_%Atmo*l%+_e2k#lvtt^hevia79qp?X9|M31M?2q#;UF zRs>!xZ7D&~tGz(k9%~V1p=ICEk5X81zlKO^o5rS4eq9L!hum-*IJu?*YHY}!0nKk9 znpgDtDNYuxN)Dgbi%o7N5z+INL9!o-18bnIr@ZEqc5wwEHXZ^36YduB>ys_*RmDK= zBDoJW%mJ1XN#%e;Z#7$8;Ftn+{QUb>K;wVlgLl0?hO}TqkCSFkHL2W?XE*AS6v~fL zvN%TJB?Ssu*{5l>iuFYrC5$jhGi{Fh-P+JyVU5w6qQC~SdVIsC&rk@{&*|@QO%h@i z@O&tC;&6DrT2BmD^;3RrhVZ(YL371GBg-`p{3^08d$A+v?1(dU@hktumGKelYO`vvxtbKd{bmX)VdqM@aUnG!R1gfXAJy-+`)TfF3S;)Z37jynyAP zX+m6Cv~D(H9_v`6+K9&TgZusuMD)#*xUHfq^Br>J#n>gZ6z7}VhLzhHA_?5SWE}ZUJ#{= zZ#DWGhABU9w}adR!7Zf!|5#%<$bf#k1a|~CoS12#E71oC{@3cuL#&V+bdT2vwG7>F z@CE6d*GI(dI@Z4~bWCC|H^Mzb_(4fQCw8`X@>(1x41NmE+}}f_PXXvR1Q!{$hW`W_ z8H{o$_3`b++2VwEJyQ zCA>}3J<1uf(JT#Vnll%$Vr^d;v7~NSWJiSU9nEGi_VoxR)JpWneP*q4F;>0C9F1)p zLAbe#w>%x;7!Gdj-S~pyphaeGo02Lx^FC*A8cHz-4)#hp0O)arJ)N5bj?o1Wm<7-) zg9nry44aqDYuHTQxS;5VS-->r?^Kr2z6K%0c)r` zILfi@&)~&rD&p2)2n7x#1E^fF<5n<$N$dUq)mG-N)erTd|D@d1*Hw%J#NwPoH@$#j zAj5rDz?^(uh0mZ)(;bHvK_;SNCtOkUAd=H)X~?JY%a<(Qu#1jQ@(y`JnrsF#yZ4v+ zj+wrh{jOwyUkq$FMg72wFwPbayS#s$IK_`S%OgE2ycok~Z}H+OIM2z&nLHPHWhz4( zamaBOMDub{XK=G*^@#8t*x!5_k1LaPn~d`>g90wJ{Ia(}j@1XWab9(MKvU4@wx@^@ z8jS|Uzh#XNW;pD&_mhquKe?171(IM6p}<`Xj(asQ{L2B;6L`@lvzLxB(jz=vAx5 zoFKx+AAKR>Lt1n_{d|nM^x(FwMbN*hVV?ZS&9g78bEX&yS(R)i1;I3G@%UdHalVaB zOT5OYdta~e=3sbGrBNanJKBuulA^Ldp1yy&4*Mcwm+RVhD9MN4Waz--rtD{(Xxo~{ zZR0Ih+pY)UrqFgPb_+(J3v+9(;$%MgS`L?_WLw_QdzeZM7iLbaVz9_a&f#%m`uu_9 zaL;VG{wI(fhx!(;2nBysZ1Mm{5@J<#z`0UA!V|XQyMLW3r2aJ4Add-?U69Y){V#PG|RC)g-u2vGAonkUF7ef)&r z%71qHEHhul!A2=zI_q0Gg-Qay~c!^y*Hrf1&bIGb087Q=G2k7jTQ zY(;$zs#m_ojdO^oKwUhF`*T(v*dSMIsVzB&-}b*Qvk8-0Q)S8Ua#|oMt2^YGy2!;y zc*MoHAieiMYpqHW4j$61O|fH-ZC^@YCPk49ZMA%iFCFaKtG6^!N;0cQSs=4%hJ=gj zwGDbZ2PR?!px~i+whP4YyoA>%!IQbEE>DE09&CS;Go4ziT3uVXTm=b$+u=!pXW8iP z`>4mV6OkW^sB_d?rVdu9YjQ3EBV_6{7FG{F_a7`=?dJIABCFJS>lF~H=`H@gu1zB7 zYf%1Hrk7xJ`;bmORD)-2lHsyiKI4h-7sKY+;oHfRmR!du!vf8UR<*{q%H|m#=(=2! zlgh5YFmi89#1!sek6CD2GBNPQ_-%ht)&hR%;dJa>Vzl%)dpD_;H_Vvpf0EOeGck^5*Dy zQfNOYmCo(*veu!2|JAkAap=dU+^(*J7rS&k`jO!JDXKr3dir)Xl7x8deKaoQCnW-j zXdu{P?V3nH|FB3Y-jfnL0$Qs-;!nEprkN7Tq`wjBt#(9X$%dT_nw@78{90`Rkci{@+Ea7CehZyGzN%InA8}Tl zf?{rax4dWC-N~q=6{FZxRDnk3Wu)mfRoFZr_Vw(*RuK3IQ#R`(tE zG(OzLm7VwBzcD{ zgEe2y^%noJV!PPYJ^0<(1)4%(ZG~`hLDoa_ff&G<={=$yrz!P%wCJbh*)~5np^zch zdgWi~*eiQj%$nLnVImswaG2gRY34a>j_W}U1L{P@`y#|RA;d?M^acc>zJA>dbCD9L0`*Xr<2_C`D847==gv{d2KOmWp#(G>Cu=r zu1MNxfz?GB+uX3GjU@tF*0n^g%@XUc`5qflx|Z1%#yP6m1_<8Ka(*xM>c(0+?^?^U zd(@P{hKo5GrlKYE`$xB)Br%D(QoG#UIiD>Km+Bg5D^%&)w9t8HLAy0os0)I7l`&KrlC_ri zk=rhPJUH88`jYWI8RzrgJf)BMcLy21hjk5drLbS(H9R(%$oNW23LkRo^rX;ZbEw3m zrDtH+JlX5*iPy86(N%KumIWreK`x>C-Av1{pyHXzw{6HSy#j-!4PEZsT~6o1hm%+d1BuKevM4r`Wn6DOg+1Pv(9yY zsX~JA)!92S30;RRDLD8tB2WVTCMv-Q+af5SPrV}GvgKt|5p46CpULg7|QVAzv~Qn2t=7xcDlEpy-}#e(B03=VieuMhtqi0LQ)YX9Iyen=LWg}A4eZLWLbQVB#-qrpS4_nyInezr5U0zB9Q!Xb!IWVYdN zLc?CuUgyqes!VNF@C7O*i0B2V-}{R1l-%p=XYi>6-8mkG$z^M%9e13Am>9JG z`~sux(=>9|yaDGHS#x^vCCcyL=CFXf=qxz=VU?|To?`xp3 zFEI4E+P*E)>t}vl1II1wJL@xwv3JOkr#>dou|S@#fBkWNA`H+Uq5&Hs*JB^%J%fXT z;|MHN8g6ec2bs|TH8_5Gd6}Dy1W63U9&G7Uv1>ZaANS}iZ!aaDjon<*z2=cws0x2{ z#q$NzdHK15d8lj;6slqUXiTd(!D1oLgxq$aTz!YhJ%c}2*MVRAWRP<-9^n}|v$dXi z?KWnQY_H0T5=!Hy(hh2N*6OjK%(wGWxv4m(P4bOzfa9umvSrVn{^*S(sg z^y|19vk{wkfV2r)hd}Yu=xjWn$K7!i=^+q%)?? zdH=~lp;k@6ME>EMD;rC{nYJ}(GtR}k2dBm@)byjvZ!aqJ`b1z`TM^Qjm&*o&cQS_U zTmqLKd`( zER<>hn>lA6K9GCee23L zH@93I;^0~BB;Y&^YjE7?q%GL7yeJFg`W^dWNXDSB$D8-1r`tL;%Vte9Yxsl5H;q0z zbm2)W-pSfn8`#F5iY5vu1zRS78onJx8`;Gq$GeVizs}}9E^?OOIa617Bx8KMjsYZu zDyb%-qs=jchk~E{Cn!P{Q3@6?C4Se&`Z?Q~AbaVaO;T}9XAi~ik91)rZ8qK(rCcoq zM?A}0nV<-8Z902(kMo&+n6qFvl@z#sdzQ;3uEH*rWF<=jhj;TS1LE+k_nSD)-DQ^av9J5qeM)@3B<^ z1_bxXc;)r=!}+{3`C@+&YDkl&Lyb@@FZ`rXdW%OT>vG~glTcg)!!%<@OY1&L95 zd#lgo%Ju0IpI!zqllbXHaMkpD%Fy$|n9piRGNa@%ffE5>tOw{m@#<(E(D6t9#+6-L zs^7raMt;4pADyCefc)z<+E>90zXcBW@B2&PDX)o&DMAikuel5=p}ZJ~dz^5%L%ewr z*ik{8D{P z^C)SHFY;!pZ%$Xd^>t6{mr*w)Cs_xwGwrZ|v~Va3)VsrDRSP*~-qOzau-kZR0(QmsxNr9HuJ0?&rO#rowuyCu)(@o%>63etk3}+3n#+4YQ6#z=X=!-GA@lOMBE=I-P z)C!#e=HyD6+ad=%38~==M8bzQrI)wmGW5 zi4vE9?c`$Za}a0r&Tjdq`E1vp$r;iOkE`PX4h=u->qSm?uydIY(2IX24`yS!)BPhE)tkYWksc*Q;WP_h9fQsD;lYovx2|O?1p=iQ{3?RfBzFQG*D~ z>4J^ua>EXU@j|gO!0^nTDAm&eR2}mGw1?8ffCOUYli5SUV{Q6U#8TM^oO}+>e!3xl zPZsv7ZO|BI0*pg6N!!`#UI-Ove=|lvO!fn2JIDA5Ic@UR6yiE6>snaJ-0PXA-`bI3 zS>2zs=I#fDuYdlK>wZSpC=L2LM!^j(na;~*_X;Uq^-&Ex4GQu861MeZXZELSIjW*x z5_s(wnZJMkej?T6VX1;QXwdXns{rWpe0tlpS=Pqd z-uV>k8D=Uz;J>a3mVqkXoUJtm#+vCuzX$o|pOo_7KRCcC8+)AQvll3J;#rW*iW>MG zW1*r3AEs76!S*3UwyFNH?&A!w32gg)XYW9ORnM-iCn5=$cLPmq1wQo4_9dp%dA#F{ z%ne&JEdy^{`LoZ<+4KcB2QfkN`|-^{hObS&jZl}1OwuViF`!g@f`rl9a=Wl4JBZHLinJbT2waPP- z{3&W}T%JP=6a|n2?pIs-r2E}?s4f6gw`2~y)E=6V3Jhm?MABQaCLznQB%;(9vT-#~ zU0+*Zcv<(70!CWR{;6X*CQjs%0xL)S`#~ro%Ylj)F0bs=0|E7{aKTJf0&6_qw6sis ziToHcn2u2|=WdkVEJbADwHlL@c=|LKXsnu}KW7Oi8=pc2hwL~Iig<}f;Bj`!aayIq?i&19sp zE!EAv^8!GZ0~Bw8Ao1awAIivl7?v1tX&zY)-#aUi(T)=D*XNfIWux%Vzla6&$3C!4 z4XVXVouMU{)xIYdaL%{Fd66Z7 zPz{a7vIICG+cmy)-kwzlurnoKU*o2YIf4NEspgGflUhzX6+iWVjj`2ZF}|ScsX^ZdJC2wIr@VjEeDKeTAB&*$^UDt>n{=B zCpnxBHc}QZDC7BOdWnfvy7XDk_+r5xhQM0A4tq-<>Hwufj26QtqPm-x!O*`|9ulNP7pOB<)5&I;R1`*6bK569Lxm? z%y375o#KxUqzP&+dLJqXM!@9EK@EHbKuhLJFfbda(Vo24_|LBezU59UT*sRx`yEv` z-D=CIf?==Smd84Eui{{CnEAeXO|EfZj?8)pU95i`E7cPm?ZB1!kC_h}w{%J|K9phi z`(&!2qPJJ700^P+I9Lg(k_VLHSdb6(37^h`Oe=$L4JJ+x>zi5Q=@rggpgW3D7I521 zHT@bX4h%(;MhYJN&#*~nf#s}%O1!ih6%};=P@sMka&HO%_C*5)+a5R1WKs-t^k`r- z(9cxXZ2U#g9jN;c5+i|#?V1Ek^=Ld7LHEH*RuEY$?Rb7*c6tBeM=@I?;4;^pdUS@yzd$4t za4-hV)U=>YjMKOq6%7q73bd3Z8(ad2-1;{zAt4>$o)eK0SAdQ(P9WV;cfdqn#rDZ+@89BLw@VF6lR5a@5k$@+S z22gSsvM3;mnY{#IhrQ$dLT1NXK%wuce@FU1Z<7JG035rB`PakjkHiAftHBoU0ooh} z!|dOO2^sc)c|l{)>ivmkc0v5#yL|5E=*OUfz?!FlbPwf$&DC2l(!H~3E4i*omNS*P zmh9p>BI}+KR$xQ?6$>bTNmH1OSth(!fUiwpw;^_yt^8}vNRxspOu;gY0iQsKHdLnt zcIfwy_{XkH5ZmP7;P857B*oP(cAG^@zST^=A&?%a1ZtP__#5DfSf9yRUIhS@Ff6>l zFc%7igY_ZQY)qMf)++^HFv{^Dd+X;K7NS-XK8}UiLA+|gO8TF8UXSi>$}XPLY*XcK zuK=thLaNpQ*_QBn(!iinm|=A|V=4DZ5|;(<9E_dae*^s5>TRW#h}jLHNs}G3WqJa| zR-`39J1>9V!vglh2SKC`Q94{D}0)J*R=CY1msdA2pXD9*XjSUna~)tQwO45 zU^JZco=)W;;(duL)F}D=+W$>`-@`fI?Qm>^F(R)t?GD+|K)NCEY!F3)ku_DKt6c&V zl?u)+H{XCc7IweyiSxSh`Pt71)o7k?R8S}u`noMoa;lE;xgk#hNagQhUF2^BGX9jn z`^^^D9SOe?3LFA~9a*wLze&I6bh%N*_DPZtOs5beWwRLopj(_6Oy(W);0*$fDBUmZ zF*#}I20^5$e)btq1_s0Y7u~>N|JN9pu(Wg-L*j7oqIe_ldM=T`r{eHJd$54@pZhNB z`=9xlyY9B&rls5%=lC20CnA=OaWB(m>W)TW#@X{;FtdsjrWxS;yBjojB{2}-HBX}` z!Ms!p;)V#b7R0!3zp(70vU5M!4imza-<_KS_8w0VxAMl|A@cQ%#lMJ?dhASUNS?S*F@{?XjxfV zu+uhtMz-F@fZ!Jto>*cY87y>A!uaFkqVA6WOdhV;cH-DBYmX{#V|H~ysp+#uV zW#Q!HEFR5OMtdI(o>W|zo}PZOah)PUK=r9{3^EpOvKdu{$7?ch0IY@9%8#(VfZ;=T z3)Mds&3y$5x#-je4${O5%c>gcr7-XUd(Q&iZXf2p%m8c!M7fmR{`r-576h^f@*~OW zW%_A`0Xk$HNq^>Ie*3S%6k4ZW11iAeS?r(7!$VZ)mn%9Tq}MPL9hfgCzD86(&YzoO z&s3NMsTFJCp#O7eICwr(cU$%>5PLq^x7_SY-GvkiT7=opMqq<9R!~ieZmuW%*XtMS zHUUe`;fN>)P~Q(y@9ruc1@%o>A7BHgcf}VXy6^v$)|HPml&}lUIGzpl$9`>)LSoX_2R@_T5+X7H_|} zscjt=u2Cc-msfsbmj-LD>IbHWLX%OBe*&U=Ce$_Nutx(82_2tZ>E}`JIN6yR{XY?; z`>~;qpU%?Sep*U=Pcv1b;WZ`D@^K7#|6X2>ZRZ2*p_XY8fKo|3h`JYgMGzmMDRT0) z;j{!%`>PFFrT4~vGcTyG{pM*M-R9-Isyet%raL7iC2Ze>@on>7s<8g~zF0W0N;WLH zN#?WPNid;(a=VY?5lzt?bY&sK@%Qa1x1W2>onH!dV2m$A%FyUw8pt5aH(P4;;RLQy z+?hY(|Ez&UigM5-n)Kv#Ha9p2%vM|d#C+!J^&su`%;Dx^_ckBZ)4#9Lp$K|-T6=if zpkx2z(?By_V&-v)MoID!0Mfx?rvg*{eyt?fD~F_&uA$d|(b z<8OL!T#N@6lG2irZ)#HhWX8}8bjxq=K=WbJtRL9;drPSLy*OWrJ>V#vEEeGEMIGs|=D$f! z#@4(SFtfk;-C%XO^{Lza%0Gh!f*=_~x!an)V5D-HzHr2S8v0C4u(A+5wuDy?%Bg5%%m%Hx>Fz(!xYo0GCppb{tU+^HTE~}3k}a2iVs|?i zk_M}`8By_zkIzpU^Ge-a_)sh+z%>8kC=IG{cy<+;8DKvZ$mkS+tyE|N#(b-f4sh-? z5&@Y+Jg7Oz0VAfCW{m>XD;T&7OoHTp@%$E|{Q2pFTpCFf)8F-7|15J{F3s$)ageSI zH1+i(VhJtBd#f_xWuVc~M=GM#0wdRRuo4Gz>&aow>=K~8t`y;9R^jX()tLbK=AFM0Pf7e)j5dx0Q&VWk(Zq;I>ccGbt zvqMpCCn!U#z*N0}+KtCblvEDL*|P&(qW(OckdjyvnFs;IO4OUgmA4TH>*yO*3V+1EWd?HnR5mkGSO9JxL;#1>Xi&m!@Mbk=Wd>O_?)GK| zkBvqFI5z<2G;wgWpq@Q=A1ef(HfeNom0v&^^d2qd3+0EH1RTx3i%YB!mD!+LLVx$D zv9}4xl5yHEHmfy6G;tlL7zS|7hu}SixkmrOn)>=xdeor9*<-bjU~T+Q{SsT+%pa7V zWDHxydWjO15D-cGz0U`jg<;ZZtAdwGCj#aH;c1y0-^Z}}ZwarJIL!{``HQaCdX-i0 z209zkSi85U}djV}E0S!MqS`hd?n`2ipDUqM?hehykSxrj3OU7mkZNKJ2kUyuIh|KK6>bbH4~ z9^O&X^Q^l-Md*$#gr!s8Z3Hd_ErB(3O7xP1JfM&4DZMF zyH~t)!ZFThlTRf`H(2V0^oMaNpya6!e~ywG5*FU7GX4{-;Y187|#+ z`S>PwEw0l~YP%EB7EikA+3r5ki~j_;&nL~m3_|IGqH|^YiU}6EMpQYA2F>nLG56T^ zp52H9G;(=%vqB)~4wD=JC*h43X7chz;2=p%3(KTovkwIV769$fo`?h?kAuY7tL94! zG`pEW6zk^%e+TfszkS=y)O{KdOcQs_?C;U!^L9XX&+Nlh{yqz#KRCMe0W-X<=Bcum zY_}dx_5XgDoN%Zsn|IY5oZum>YykW;x9#k8C=-|Ml-?iQT_94zA4JVQQ0v{&HCITY zT^hU{S~N%c=LIl1Y)9CP73wnyjsw%V#L}RX`)s=40&lTCP~DYsB31lBc}*4|5Zksl z$9*J~&K)XVDFk=36WbvI>dI7nvuxsO4)%Z6K&m;EX4|*$#4TI?FLdudGtV%R|ALxp zFDA}nn9u90sMq!QtZB&K1_2$RjpgORfBew-_pQp{6VLkPd-J%2`=~1`M?Mm8{J{O&5!SH?`q)je|m}4|A_|&x570>Tff<%x6~M9Y+DqaKBo&7yQixJpv~jT zxq#&ESzh6eRN!;xvx7cUEhlERoJh5j7wXpE%2v-N!jB)h{#`+K2%DOv2`)ubVXRnN z!q_(Zf-b*J5zt!ab17>>@#KRQ-um1<4h43Rn7eyD8Y9d?5%_^U*yL|KY(?B0Z=cPJ2YX=LxmQ_r<^0KlMjDMXAPQ$PQqylyp$k|?} z{Tjan!+P{H*YWNyIpOV~HQx-ZRUowHs9kHD1i&demsgUdcmMAm40Sbg-~UXM3wF3{ z_%}d$Z|SLQJnv^v;NjhwzYC7=8Mq_u0ks+a+{T;03`~F8eU*CXY^WVw$HzJ6&`$16Y&W5W#efs;!fBjDV zGteKWL);7b8-4!utG755iOZtH$;tocCZNwA?UfM?-ai-r=U!J*;8M3jHrD?>hTYu; zssrv`|29hgUl;%Tt9#_&Qa6Xi1q}Z@!9U;Y73}VW&pGdc_CI$3;rsu8kpHhY;(v>h YF`Sv#nEJa64*ZdMrYKn|VHoiL0K>^+-v9sr literal 0 HcmV?d00001 diff --git a/lib/CurrentSensor/CurrentSensor.cpp b/lib/CurrentSensor/CurrentSensor.cpp new file mode 100644 index 0000000..83e4819 --- /dev/null +++ b/lib/CurrentSensor/CurrentSensor.cpp @@ -0,0 +1,58 @@ +#include "CurrentSensor.h" + +CurrentSensor::CurrentSensor(){}; + +void CurrentSensor::setMuxWiremap(Wiremap& wiremap) { + this->A2.write(wiremap.a2); + this->A1.write(wiremap.a1); + this->A0.write(wiremap.a0); +} + +double CurrentSensor::calculateCurrentFromVoltage(Voltage voltage) { + double vref = voltage.first * 3.3; + double vout = voltage.second * 3.3; + + return (vout - vref) / SENSITIVITY; +} + +Voltage CurrentSensor::readVoltageFromMuxOutput() { + return std::make_pair(vReference.read(), vOut.read()); +} + +Motors CurrentSensor::getMotorsCurrent() { + Voltage voltage; + Motors motorCurrent; + + // Motor 1 + this->setMuxWiremap(this->motorOneWiremap); + voltage = this->readVoltageFromMuxOutput(); + motorCurrent.m1 = this->calculateCurrentFromVoltage(voltage); + + // Motor 2 + this->setMuxWiremap(this->motorTwoWiremap); + voltage = this->readVoltageFromMuxOutput(); + motorCurrent.m2 = this->calculateCurrentFromVoltage(voltage); + + // Motor 3 + this->setMuxWiremap(this->motorThreeWiremap); + voltage = this->readVoltageFromMuxOutput(); + motorCurrent.m3 = this->calculateCurrentFromVoltage(voltage); + + // Motor 4 + this->setMuxWiremap(this->motorFourWiremap); + voltage = this->readVoltageFromMuxOutput(); + motorCurrent.m4 = this->calculateCurrentFromVoltage(voltage); + + return motorCurrent; +} + +double CurrentSensor::getDribblerCurrent() { + // Setup wiremap to dribbler output + this->setMuxWiremap(this->dribblerWiremap); + + // Read output + Voltage voltage = this->readVoltageFromMuxOutput(); + + // Do not use utils getCurrent, refactor this later on + return this->calculateCurrentFromVoltage(voltage); +} \ No newline at end of file diff --git a/lib/CurrentSensor/CurrentSensor.h b/lib/CurrentSensor/CurrentSensor.h new file mode 100644 index 0000000..5bc3e62 --- /dev/null +++ b/lib/CurrentSensor/CurrentSensor.h @@ -0,0 +1,42 @@ +#ifndef CURRENT_SENSOR_H +#define CURRENT_SENSOR_H + +#include + +#define SENSITIVITY 0.025 + +using Voltage = std::pair; + +class CurrentSensor { + public: + class Wiremap { + public: + int a0; + int a1; + int a2; + + Wiremap(int a0, int a1, int a2) : a0(a0), a1(a1), a2(a2){}; + }; + + CurrentSensor(); + double getDribblerCurrent(); + Motors getMotorsCurrent(); + + private: + DigitalOut A2 = CURRENT_SENSOR_A2; + DigitalOut A1 = CURRENT_SENSOR_A1; + DigitalOut A0 = CURRENT_SENSOR_A0; + AnalogIn vReference = PIN_CURR_REF; + AnalogIn vOut = PIN_CURR_OUT; + Wiremap motorOneWiremap{0, 0, 1}; + Wiremap motorTwoWiremap{0, 1, 1}; + Wiremap motorThreeWiremap{1, 1, 1}; + Wiremap motorFourWiremap{1, 0, 0}; + Wiremap dribblerWiremap{0, 1, 0}; + + void setMuxWiremap(Wiremap& wiremap); + double calculateCurrentFromVoltage(Voltage voltage); + Voltage readVoltageFromMuxOutput(); +}; + +#endif // CURRENT_SENSOR_H \ No newline at end of file diff --git a/lib/Debugger/Debugger.cpp b/lib/Debugger/Debugger.cpp new file mode 100644 index 0000000..8ad581f --- /dev/null +++ b/lib/Debugger/Debugger.cpp @@ -0,0 +1,685 @@ +#include "Debugger.h" +#include +#include + +Debugger::Debugger(MotionControl* motionControl, + Odometry* odometry, + Kicker* kicker, + nRF24Communication* radio_recv, + nRF24Communication* radio_send, + CurrentSensor* current_sensor) { + + _motion = motionControl; + _odometry = odometry; + _kick = kicker; + _radio_recv = radio_recv; + _radio_send = radio_send; + _current_sensor = current_sensor; +} + +Debugger::Debugger(MotionControl* motionControl, + Odometry* odometry, + Kicker* kicker, + nRF24Communication* radio_recv, + nRF24Communication* radio_send) { + _motion = motionControl; + _odometry = odometry; + _kick = kicker; + _radio_recv = radio_recv; + _radio_send = radio_send; +} + +void Debugger::processRobot(ExecMode robotMode) { + // to do: check if any robotMode is broken + if (robotMode == ExecMode::GAME_ON) { + printf("Starting game...\n"); + } else if (robotMode == ExecMode::TEST) { + runAllTests(); + } else if (robotMode == ExecMode::VISION_BLACKOUT) { + printf("Starting Vision Blackout...\n"); + } else if (robotMode == ExecMode::TEST_BASICS) { + testRobotBasics(30); + } else if (robotMode == ExecMode::TEST_PWM) { + testRobotMotors(); + } else if (robotMode == ExecMode::TEST_PID) { + testRobotPID(); + } else if (robotMode == ExecMode::TEST_KICK) { + testKick(); + } else if (robotMode == ExecMode::TEST_DRIBBLER) { + testDribblerPWM(-80); // default: -50 + } else if (robotMode == ExecMode::TEST_ODOMETRY) { + testOdometry(); + } else if (robotMode == ExecMode::TEST_MPU) { + _odometry->init(); + testMPU(); + } else if (robotMode == ExecMode::TEST_IR) { + testIR(); + } else if (robotMode == ExecMode::TEST_RECEIVER_BALL) { + testReceiveBall(); + } else if (robotMode == ExecMode::TEST_CURRENT) { + testCurrent(); + } else if (robotMode == ExecMode::GET_CURRENT) { + getMotorsCurrent(); + } else { + printf("Unknown robot mode: %d\n", static_cast(robotMode)); + } +} + +void Debugger::getMotorsCurrent(double maxPWM, double minPWM, uint8_t step) { + while (utils::PBT1) + ThisThread::sleep_for(300ms); + + Motors motorsCurrent; + double pwm = minPWM; + + printf("PWM_VALUE,M1_CURRENT,M2_CURRENT,M3_CURRENT,M4_CURRENT\n"); + + while (1) { + _motion->moveRobotPWM(pwm); + motorsCurrent = _current_sensor->getMotorsCurrent(); + printf("%f,%f,%f,%f,%f\n", + pwm, + motorsCurrent.m1, + motorsCurrent.m2, + motorsCurrent.m3, + motorsCurrent.m4); + + if (!utils::PBT2) { + while (utils::PBT1) + ; + ThisThread::sleep_for(500ms); + pwm += step; + if (pwm > maxPWM) { + pwm = minPWM; + } + } + } +} + +void Debugger::testCurrent() { + while (1) { + Motors motorsCurrent = _current_sensor->getMotorsCurrent(); + printf("M1: %f - M2: %f - M3: %f - M4: %f\n", + motorsCurrent.m1, + motorsCurrent.m2, + motorsCurrent.m3, + motorsCurrent.m4); + ThisThread::sleep_for(100ms); + } +} + +void Debugger::printMotorsSpeed() { + _motion->printMotorsSpeed(); +} +void Debugger::printKickerBasics() { + printf("IR ADC: %f | ", _kick->readIR()); + printf("Kicker Load: %f\n", _kick->getLoad()); +} +void Debugger::printRadioConnection() { + printf("nRF Connected? %d\n", (int) _radio_recv->compareChannel(SSL_1_ROBOT_RECV_CH)); +} +void Debugger::printRadiosConnections() { + printf("nRF 1: %d | nRF 2: %d\n", + (int) _radio_recv->compareChannel(SSL_1_ROBOT_RECV_CH), + (int) _radio_send->compareChannel(SSL_2_ROBOT_SEND_CH)); +} +void Debugger::printRadioDetails() { + _radio_recv->printDetails(); +} +void Debugger::printRadioReceivedInfos() { + + printf("Updated ?\n"); + Vector v; + KickFlags kick; + v = _radio_recv->getVectorSpeed(); + _radio_recv->getKick(kick); + printf("Vx = %.3f\n", v.x); + printf("Vy = %.3f\n", v.y); + printf("W = %.3f\n", v.w); + printf("Front = %d\n", kick.front); + printf("Chip = %d\n", kick.chip); + printf("Charge = %d\n", kick.charge); + printf("Strength = %.1f\n", kick.kickStrength); + printf("Dribbler = %d\n", kick.dribbler); + printf("Dribbler Speed = %f\n", kick.dribblerSpeed); + printf("\n"); +} +void Debugger::printBattery() { + printf("Battery: %.3f ----> %.3f V!\n", utils::BATT.read(), utils::getBattery()); +} +void Debugger::printSwitchs() { + printf("SW> 1: %d | 2: %d | 3: %d | 4: %d | ID: %d - \n", + utils::SW1.read(), + utils::SW2.read(), + utils::SW3.read(), + utils::SW4.read(), + utils::getRobotId()); +} +void Debugger::printButtons() { + printf("BT 1: %d | BT 2: %d\n", utils::PBT1.read(), utils::PBT2.read()); +} +void Debugger::printRobotBasics() { + printBattery(); + printSwitchs(); + printButtons(); + printf("\n"); + printRadioConnection(); + printMotorsSpeed(); + printKickerBasics(); +} + +void Debugger::runAllTests() { + + // Signal robotMode TEST + Status::sendRoundColor(Color::WHITE, 100); + Status::sendRoundColor(Color::PINK, 100); + Status::sendRoundColor(Color::WHITE, 100); + + // beep to signal the start of the tests + utils::beep(180); + + ExecMode currentMode = ExecMode::TEST_PWM; + while (1) { + // signals transition + Status::send(Color::WHITE, ALL_LEDS); + + if (currentMode == ExecMode::TEST_PWM) { + printf("Testing PWM\n"); + currentMode = testRobotMotors(); + } else if (currentMode == ExecMode::TEST_PID) { + printf("Testing PID\n"); + currentMode = testRobotPID(); + } else if (currentMode == ExecMode::TEST_IR) { + printf("Testing IR\n"); + currentMode = testIR(); + } else if (currentMode == ExecMode::TEST_KICK) { + printf("Testing Kicker\n"); + currentMode = testKick(); + } else { + printf("Unknown robot mode: %d\n", (int) currentMode); + } + } +} + +void Debugger::testRobotBasics(int v) { + // TODO: Reactivate MPU when all robots receive the MPU interface. + // utils::initMPU(); + while (1) { + if (!utils::PBT1) { + _motion->moveRobotPWM(v); + v = -v; + } else if (!utils::PBT2) { + _motion->stopRobot(); + } + printRobotBasics(); + // utils::printMPU(); + ThisThread::sleep_for(300ms); + } +} + +ExecMode Debugger::testRobotMotors(bool start, double limitSpeed, uint8_t repeats) { + Status::send(Color::PINK, ALL_LEDS); + + // Wait for release + while (!utils::PBT1) + ; + while (!utils::PBT2) + ; + ThisThread::sleep_for(10ms); // debounce + + bool running = start; + int speed = 1; + int v = speed; + int step = 1; + int count = 0; + Timer motor_pid; + motor_pid.start(); + ButtonState PBT1State = ButtonState::UNPRESSED; + ButtonState PBT2State = ButtonState::UNPRESSED; + + printf("Press PBT1 to start, PBT2 to stop\n"); + while (1) { + ThisThread::sleep_for(1ms); + + PBT1State = utils::pressedFor(utils::PBT1, 2000); + PBT2State = utils::pressedFor(utils::PBT2, 2000); + + if (PBT1State == ButtonState::HELD) { + _motion->stopRobot(); + return ExecMode::TEST_IR; + } + if (PBT2State == ButtonState::HELD) { + _motion->stopRobot(); + return ExecMode::TEST_PID; + } + + if (PBT1State == ButtonState::RELEASED) { + running = true; + printf("Starting...\n"); + ThisThread::sleep_for(200ms); + } + if (PBT2State == ButtonState::RELEASED) { + printf("Stopping...\n"); + ThisThread::sleep_for(200ms); + running = false; + _motion->stopRobot(); + } + + // Move motors + if (!running) + continue; + + _motion->moveRobotPWM(v); + + // Update speed + if (utils::timerRead(motor_pid) < 300) + continue; + + printf("%d -> ", v); + printMotorsSpeed(); + motor_pid.reset(); + v += step; + + // Change direction + if (v < limitSpeed && v > -limitSpeed) + continue; + + _motion->stopRobot(); + ThisThread::sleep_for(2s); + v = 0; + step = -step; + if (count < 3) { + count++; + } + + // Stop if its the first test + if (count != 2) + continue; + + printf("Completed first test\n"); + printf("Press PBT1 to start looping"); + running = false; + } +} + +ExecMode Debugger::testIR() { + Status::send(Color::BLUE, ALL_LEDS); + + // Wait for release + while (!utils::PBT1) + ; + while (!utils::PBT2) + ; + ThisThread::sleep_for(10ms); // debounce + + Timer beepTime; + beepTime.start(); + bool shouldBeep = true; + ButtonState PBT1State = ButtonState::UNPRESSED; + ButtonState PBT2State = ButtonState::UNPRESSED; + + printf("Press PBT1 to turn on beep, PBT2 to turn off beep\n"); + while (1) { + ThisThread::sleep_for(100ms); + + PBT1State = utils::pressedFor(utils::PBT1, 2000); + PBT2State = utils::pressedFor(utils::PBT2, 2000); + + if (PBT1State == ButtonState::HELD) + return ExecMode::TEST_KICK; + if (PBT2State == ButtonState::HELD) + return ExecMode::TEST_PWM; + + if (PBT1State == ButtonState::RELEASED) { + shouldBeep = true; + printf("Turn on beep\n"); + ThisThread::sleep_for(100ms); + } + if (PBT2State == ButtonState::RELEASED) { + shouldBeep = false; + printf("Turn off beep\n"); + ThisThread::sleep_for(100ms); + } + + printf("IR ADC: %f | with ball: %d\n", _kick->readIR(), (_kick->readIR() < IR_THRESHOLD)); + + // Beep + if (!shouldBeep) + continue; + + // Detect ball + if (_kick->readIR() > IR_THRESHOLD) + continue; + + // Delay between beeps + if (utils::timerRead(beepTime) < 300) + continue; + + utils::beep(70); + beepTime.start(); + } +} + +long long now() { + return std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()) + .count(); +} + +void Debugger::testReceiveBall() { + const double INITIAL_SPEED = 80; // velocidade do dribbler + const double SECONDS = 1.3; // segundos para dominar a bola + + const double MICROSECONDS = SECONDS * 1e6; + + bool ir_was_activacted = false; + double current_speed = -INITIAL_SPEED; + long long started = now(); + + while (true) { + if (ir_was_activacted) { + current_speed = + -std::max(0, + INITIAL_SPEED * (MICROSECONDS - (now() - started)) / MICROSECONDS); + } else { + started = now(); + } + _motion->moveMotorPWM(5, current_speed); + if (_kick->readIR() < IR_THRESHOLD) { + ir_was_activacted = true; + } + } + + printf("%.3f ", (double) current_speed); + _motion->printDriblerSpeed(); + ThisThread::sleep_for(70ms); +} + +ExecMode Debugger::testRobotPID(bool start, double limitSpeed, uint8_t repeats, uint32_t delay) { + Status::send(Color::YELLOW, ALL_LEDS); + + // Wait for release + while (!utils::PBT1) + ; + while (!utils::PBT2) + ; + ThisThread::sleep_for(10ms); // debounce + + bool running = start; + int count = 0; + double v = limitSpeed; + Timer motor_pid; + motor_pid.start(); + ButtonState PBT1State = ButtonState::UNPRESSED; + ButtonState PBT2State = ButtonState::UNPRESSED; + + printf("Press PBT1 to start, PBT2 to stop\n"); + while (1) { + PBT1State = utils::pressedFor(utils::PBT1, 2000); + PBT2State = utils::pressedFor(utils::PBT2, 2000); + + if (PBT1State == ButtonState::HELD) { + _motion->stopRobot(); + return ExecMode::TEST_PWM; + } + if (PBT2State == ButtonState::HELD) { + _motion->stopRobot(); + return ExecMode::TEST_KICK; + } + + if (PBT1State == ButtonState::RELEASED) { + running = true; + motor_pid.reset(); + printf("Starting...\n"); + ThisThread::sleep_for(200ms); + } + if (PBT2State == ButtonState::RELEASED) { + printf("Stopping...\n"); + ThisThread::sleep_for(200ms); + running = false; + _motion->stopRobot(); + } + + // Move motors + if (!running) + continue; + + Vector desiredVector(0, 0, r_w[count]); + + _motion->accelRobot(desiredVector); + _motion->printDebugPID(desiredVector, true); // print with debug + + // Update speed + if (utils::timerRead(motor_pid) < delay) + continue; + + v = -v; + count++; + motor_pid.reset(); + + if (count == repeats) { + count = 0; + running = false; + _motion->stopRobot(); + ThisThread::sleep_for(200ms); + printf("Test completed\n"); + } + } +} + +ExecMode Debugger::testKick(bool start) { + Status::send(Color::GREEN, ALL_LEDS); + + // Wait for release + while (!utils::PBT1) + ; + while (!utils::PBT2) + ; + ThisThread::sleep_for(10ms); // debounce + + int kicked = 0; + KickFlags toKick; + bool running = start; + toKick.charge = true; + toKick.kickStrength = 7.0; // max strength + float kickLoad = MAX_CHARGE - MAX_CHARGE_MARGIN; + ButtonState PBT1State = ButtonState::UNPRESSED; + ButtonState PBT2State = ButtonState::UNPRESSED; + + printf("Press PBT1 to charge and kick, PBT2 to discharge\n"); + while (1) { + printKickerBasics(); + ThisThread::sleep_for(100ms); + + PBT1State = utils::pressedFor(utils::PBT1, 2000); + PBT2State = utils::pressedFor(utils::PBT2, 2000); + + if (PBT1State == ButtonState::HELD) { + printf("Discharging...\n"); + for (int i = 0; i < 5; i++) { + _kick->discharge(); + ThisThread::sleep_for(500ms); + } + return ExecMode::TEST_PID; + } + if (PBT2State == ButtonState::HELD) { + printf("Discharging...\n"); + for (int i = 0; i < 5; i++) { + _kick->discharge(); + ThisThread::sleep_for(500ms); + } + return ExecMode::TEST_IR; + } + + if (PBT1State == ButtonState::RELEASED) { + running = true; + } + if (PBT2State == ButtonState::RELEASED) { + printf("Discharging...\n"); + for (int i = 0; i < 5; i++) { + _kick->discharge(); + ThisThread::sleep_for(500ms); + } + } + + // // Kick + // if (!running) continue; + + printf("Charging...\n"); + while (_kick->getLoad() < kickLoad) + _kick->update(toKick, DEFAULT_MIN_CHARGE, MAX_CHARGE); + _kick->stopCharge(); + ThisThread::sleep_for(200ms); + + if (kicked == 0 && _kick->readIR() < IR_THRESHOLD) { + printf("\n\n >>>>> FRONT <<<<<< \n\n"); + _kick->front(toKick.kickStrength); + kicked = 1; + } else if (kicked == 1 && _kick->readIR() < IR_THRESHOLD) { + printf("\n\n >>>>> CHIP <<<<<< \n\n"); + _kick->chip(toKick.kickStrength); + kicked = 0; + } + + running = false; + } +} + +void Debugger::testDribblerPWM(float pwm) { + pwm = pwm < (-5) ? pwm : DEFAULT_DRIBLER_SPEED; + + while (utils::PBT1) + ; + + while (1) { + _motion->moveMotorPWM(5, pwm); + + if (!utils::PBT2) { + _motion->stopMotor(5); + ThisThread::sleep_for(100ms); + while (utils::PBT1) + ; + } + ThisThread::sleep_for(70ms); + } +} + +void Debugger::testOdometry() { + Timer sampleTime; + Vector v; + Vector move; + unsigned int count = 0; + unsigned int totalTime = 0; + v.x = 0.5; + // v.y = 0.2; + // v.w = 5; + while (utils::PBT1) + ; + ThisThread::sleep_for(200ms); + + sampleTime.start(); + while (1) { + _motion->moveRobot(v); + if (!utils::PBT1) { + v.x = -v.x; + v.y = -v.y; + v.w = -v.w; + ThisThread::sleep_for(100ms); + } else if (!utils::PBT2) { + _motion->stopRobot(); + totalTime = 0; + while (utils::PBT1) + ; + _odometry->updatePosition(); + sampleTime.reset(); + } + if (utils::timerRead(sampleTime) > 500) { + count++; + float time = utils::timerRead(sampleTime); + totalTime += time; + move = v * (totalTime / 1000); + _motion->printMotorsSpeed(); + printf("Count: %d - Time: %f (ms)\n", count, time); + printf("Theory: X: %lf- Y: %lf - W: %lf\n", move.x, move.y, move.w); + _odometry->printMovement(); + printf("\n\n"); + sampleTime.reset(); + } + Vector currentPos = _odometry->getCurrentPosition(); + if (currentPos.x >= 0.8) { + totalTime += utils::timerRead(sampleTime); + move = v * (totalTime / 1000.0); + for (int i = 0; i < 20; i++) { + _motion->breakRobot(5); + ThisThread::sleep_for(5ms); + } + _motion->stopRobot(); + printf("Theory: X: %lf- Y: %lf - W: %lf\n", move.x, move.y, move.w); + _odometry->printCurrentPosition(); + printf("\n\n"); + totalTime = 0; + while (utils::PBT1) + ; + _odometry->updatePosition(); + ThisThread::sleep_for(200ms); + sampleTime.reset(); + } + ThisThread::sleep_for(5ms); + } +} + +void Debugger::testPosition() { + Timer sampleTime; + Vector move; + unsigned int count = 0; + unsigned int totalTime = 0; + + while (utils::PBT1) + ; + ThisThread::sleep_for(200ms); + + sampleTime.start(); + while (1) { + _motion->stopRobot(); + if (!utils::PBT1) { + _motion->stopRobot(); + printf("Theory: X: %lf- Y: %lf - W: %lf\n", move.x, move.y, move.w); + _odometry->printSpeedIntPosition(); + _odometry->printCurrentPosition(); + printf("\n\n"); + totalTime = 0; + ThisThread::sleep_for(200ms); + while (utils::PBT1) + ; + _odometry->updatePosition(); + ThisThread::sleep_for(200ms); + sampleTime.reset(); + } else if (!utils::PBT2) { + while (utils::PBT1) + ; + _odometry->updatePosition(); + sampleTime.reset(); + } + if (utils::timerRead(sampleTime) > 500) { + count++; + float time = utils::timerRead(sampleTime); + totalTime += time; + _motion->printMotorsSpeed(); + _odometry->printIntegralMovement(); + _odometry->printMovement(); + printf("\n\n"); + sampleTime.reset(); + } + ThisThread::sleep_for(5ms); + } +} + +void Debugger::testMPU() { + double angle; + while (1) { + _odometry->getGyroAngle(angle); + printf("Angle: %lf\n", angle); + ThisThread::sleep_for(50ms); + _odometry->processGyro(); + } +} \ No newline at end of file diff --git a/lib/Debugger/Debugger.h b/lib/Debugger/Debugger.h new file mode 100644 index 0000000..e2a073f --- /dev/null +++ b/lib/Debugger/Debugger.h @@ -0,0 +1,81 @@ +#ifndef DEBUGGER_H +#define DEBUGGER_H + +#include "utils.h" +#include +#include +#include +#include +#include +#include + +class Debugger { + public: + Debugger(MotionControl* motionControl, + Odometry* odometry, + Kicker* kicker, + nRF24Communication* radio_recv, + nRF24Communication* radio_send); + Debugger(MotionControl* motionControl, + Odometry* odometry, + Kicker* kicker, + nRF24Communication* radio_recv, + nRF24Communication* radio_send, + CurrentSensor* current_sensor); + + void printRobotBasics(); + void printMotorsSpeed(); + void printKickerBasics(); + void printRadioConnection(); + void printRadiosConnections(); + void printRadioDetails(); + void printRadioReceivedInfos(); + void printMPU(); + void processRobot(ExecMode robotMode); + void printBattery(); + void printSwitchs(); + void printButtons(); + + void getMotorsCurrent(double maxPWM = 30., + double minPWM = 10., + uint8_t step = 10); + + void runAllTests(); + void testRobotBasics(int v = 30); + ExecMode testRobotMotors(bool start = false, double limitSpeed = 15, uint8_t repeats = 1); + ExecMode testRobotPID(bool start = false, + double limitSpeed = 40.0, + uint8_t repeats = 30, + uint32_t delay = 3000); + ExecMode testKick(bool start = false); + void testDribblerPWM(float pwm); + void testOdometry(); + void testPosition(); + void testMPU(); + ExecMode testIR(); + void testReceiveBall(); + void testCurrent(); + + private: + MotionControl* _motion; + Kicker* _kick; + Odometry* _odometry; + nRF24Communication* _radio_recv; + nRF24Communication* _radio_send; + CurrentSensor* _current_sensor; + + double r_vx[30] = {-1.8237, -1.0451, 1.3245, -2.0714, 1.8870, 1.0135, -0.0501, 0.3455, + -1.1560, -0.1811, 2.0376, 0.2059, 0.0930, -1.1810, -0.0488, 0.5459, + 0.7882, -0.4597, -0.5833, 2.1471, -2.0339, 1.6947, 1.8185, 1.3032, + -1.7657, -1.0478, -0.7244, 0.7908, -1.5992, 0.9734}; + double r_vy[30] = {-1.7302, 0.6765, -0.0256, 1.2278, 0.9462, 1.7764, 1.7201, -0.7297, + 0.8745, -1.3296, -2.0656, 1.0739, 0.0001, -0.0883, 1.7808, 0.4834, + 0.5177, 1.5815, 1.3442, 0.3376, -1.3951, -1.1443, 1.7007, -2.0738, + -0.0444, -1.4611, 2.1062, 0.9359, 0.0021, -0.1272}; + double r_w[30] = {-4.4038, 1.8197, -4.5757, -4.2855, 0.2165, -4.0327, 3.1815, 3.1755, + 2.2244, -3.5013, 1.5961, 0.1859, 4.7297, 1.4899, 3.0033, -0.4620, + -0.6761, 3.2531, -4.1653, -3.6683, -3.2661, -1.0906, 3.3138, 3.0336, + -4.3953, -1.0074, 0.2688, -0.8320, 1.5686, 1.2797}; +}; + +#endif /* DEBUGGER_H */ diff --git a/lib/EthernetCommunication/EthCommunication.cpp b/lib/EthernetCommunication/EthCommunication.cpp new file mode 100644 index 0000000..c583f1f --- /dev/null +++ b/lib/EthernetCommunication/EthCommunication.cpp @@ -0,0 +1,220 @@ +#include "EthCommunication.h" + +EthCommunication::EthCommunication(const char* ip, + const char* netmask, + const char* gateway, + int port) : + _socketAddress(ip, port) { + _ip = ip; + _port = port; + _lastMsgType = msgType::NONE; + _eth.set_network(_ip, netmask, gateway); + _eth.set_blocking(false); + _isActive = false; +} + +bool EthCommunication::setup() { + if (!_eth.connect()) { + printf("Ethernet Connected! Listening on Port: %d\n", _port); + _socket.open(&_eth); + _socket.bind(_port); + _socket.set_timeout(0); + _isActive = true; + } else { + printf("Ethernet Connection Has Failed!\n"); + } + return _isActive; +} + +uint16_t EthCommunication::update_last_message() { + // only updates last received msg if buffer is not empty + uint16_t msgLen = 0; + uint8_t recvMsgCheck[protoPositionSSL_size]; + std::memset(recvMsgCheck, 0, sizeof(_lastMsg)); + // checks buffer until it has no message + while (true) { + uint16_t checkMsgLen = _socket.recvfrom(&_socketAddress, recvMsgCheck, sizeof(_lastMsg)); + if (checkMsgLen == EMPTY_MESSAGE_LENGTH) { + return msgLen; + } else { + msgLen = checkMsgLen; + // copies not-empty message to _lastMsg + std::memcpy(this->_lastMsg, recvMsgCheck, msgLen); + // noMsgTimer.reset(); // reset no msg timer if msg is valid + } + } +} + +void EthCommunication::decode_proto_position_msg(protoPositionSSL protoMessage) { + this->_lastMsgType = msgType::POSITION; + this->_packetPos.v.x = protoMessage.x; + this->_packetPos.v.y = protoMessage.y; + this->_packetPos.v.w = protoMessage.w; + this->_packetPos.resetOdometry = protoMessage.resetOdometry; + this->_packetPos.maxSpeed = protoMessage.max_speed; + this->_packetPos.minSpeed = protoMessage.min_speed; + this->_packetPos.type = static_cast(protoMessage.posType); + this->_kick.front = static_cast(protoMessage.front); + this->_kick.chip = static_cast(protoMessage.chip); + this->_kick.charge = static_cast(protoMessage.charge); + this->_kick.kickStrength = protoMessage.kickStrength / 10; + this->_kick.dribbler = static_cast(protoMessage.dribbler); + this->_kick.dribblerSpeed = protoMessage.dribSpeed / 10; +} + +void EthCommunication::decode_proto_motors_msg(protoMotorsPWMSSL protoMessage) { + this->_lastMsgType = msgType::SSL_MOTORS_PWM; + this->_packetMotors.m1 = protoMessage.m1; + this->_packetMotors.m2 = protoMessage.m2; + this->_packetMotors.m3 = protoMessage.m3; + this->_packetMotors.m4 = protoMessage.m4; +} + +bool EthCommunication::eth_recv() { + if (!_isActive) { + return false; + } + uint16_t msgLen = this->update_last_message(); + protoPositionSSL protoMessage = protoPositionSSL_init_zero; + if (msgLen == 0) + return false; + else { + pb_istream_t recvStream = pb_istream_from_buffer(_lastMsg, msgLen); + if (pb_decode(&recvStream, &protoPositionSSL_msg, &protoMessage)) { + this->decode_proto_position_msg(protoMessage); + } + return true; + } +} + +msgType EthCommunication::get_last_msg_type() { + return this->_lastMsgType; +} + +void EthCommunication::get_position(RobotPosition& pos) { + pos = _packetPos; +} + +void EthCommunication::get_motors_pwm(Motors& motors) { + motors = _packetMotors; +} + +void EthCommunication::get_kick(KickFlags& isKick) { + isKick.front = _kick.front; + isKick.chip = _kick.chip; + isKick.charge = _kick.charge; + isKick.kickStrength = _kick.kickStrength; + isKick.dribbler = _kick.dribbler; + isKick.bypassIR = (_kick.front | _kick.chip) & _kick.charge; + isKick.dribbler = _kick.dribbler; + isKick.dribblerSpeed = _kick.dribblerSpeed; +} + +protoOdometrySSL EthCommunication::encode_proto_odometry(RobotInfo robotInfo, + Vector robotOdometry, + Vector robotSpeed) { + protoOdometrySSL protoMessage; + protoMessage = protoOdometrySSL_init_zero; + protoMessage.x = static_cast(robotOdometry.x); + protoMessage.y = static_cast(robotOdometry.y); + protoMessage.w = static_cast(robotOdometry.w); + protoMessage.hasBall = static_cast(robotInfo.ball); + protoMessage.kickLoad = static_cast(robotInfo.kickLoad); + protoMessage.battery = static_cast(robotInfo.battery); + protoMessage.count = static_cast(robotInfo.count); + protoMessage.vision_x = static_cast(robotInfo.v.x); + protoMessage.vision_y = static_cast(robotInfo.v.y); + protoMessage.vision_w = static_cast(robotInfo.v.w); + protoMessage.vx = static_cast(robotSpeed.x); + protoMessage.vy = static_cast(robotSpeed.y); + protoMessage.vw = static_cast(robotSpeed.w); + + return protoMessage; +} + +protoMotorsDataSSL EthCommunication::encode_proto_motors_data(Motors currentMotorsSpeed, + Motors motorsPWM, + Motors desiredMotorsSpeed, + double msgTime) { + protoMotorsDataSSL protoMessage; + protoMessage = protoMotorsDataSSL_init_zero; + protoMessage.current_m1 = static_cast(currentMotorsSpeed.m1); + protoMessage.current_m2 = static_cast(currentMotorsSpeed.m2); + protoMessage.current_m3 = static_cast(currentMotorsSpeed.m3); + protoMessage.current_m4 = static_cast(currentMotorsSpeed.m4); + protoMessage.pwm_m1 = static_cast(motorsPWM.m1); + protoMessage.pwm_m2 = static_cast(motorsPWM.m2); + protoMessage.pwm_m3 = static_cast(motorsPWM.m3); + protoMessage.pwm_m4 = static_cast(motorsPWM.m4); + protoMessage.desired_m1 = static_cast(desiredMotorsSpeed.m1); + protoMessage.desired_m2 = static_cast(desiredMotorsSpeed.m2); + protoMessage.desired_m3 = static_cast(desiredMotorsSpeed.m3); + protoMessage.desired_m4 = static_cast(desiredMotorsSpeed.m4); + protoMessage.msgTime = static_cast(msgTime); + + return protoMessage; +} + +void EthCommunication::eth_sendto(const char* ip, + uint16_t port, + RobotInfo robotInfo, + Vector robotOdometry, + Vector robotSpeed) { + if (!_isActive) + return; + uint8_t buffer[protoOdometrySSL_size]; + size_t message_length; + + protoOdometrySSL protoMessage = this->encode_proto_odometry(robotInfo, robotOdometry, robotSpeed); + + pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + + if (pb_encode(&stream, &protoOdometrySSL_msg, &protoMessage)) { + // message receiver breaks if full buffer is sent, so use stream.bytes_written property to limit + // length + message_length = stream.bytes_written; + SocketAddress target(ip, port); + // packets are decoded by protobuf, not nanopb, so send buffer, not nanopb stream + _socket.sendto(target, buffer, message_length); + } else { + printf("encoding failed!\n"); + } +} + +void EthCommunication::eth_sendto(const char* ip, + uint16_t port, + Motors currentMotorsSpeed, + Motors motorsPWM, + Motors desiredMotorsSpeed, + double msgTime) { + if (!_isActive) + return; + uint8_t buffer[protoMotorsDataSSL_size]; + size_t message_length; + + protoMotorsDataSSL protoMessage = this->encode_proto_motors_data(currentMotorsSpeed, + motorsPWM, + desiredMotorsSpeed, + msgTime); + + pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + + if (pb_encode(&stream, &protoMotorsDataSSL_msg, &protoMessage)) { + // message receiver breaks if full buffer is sent, so use stream.bytes_written property to limit + // length + message_length = stream.bytes_written; + SocketAddress target(ip, port); + // packets are decoded by protobuf, not nanopb, so send buffer, not nanopb stream + _socket.sendto(target, buffer, message_length); + } else { + printf("encoding failed!\n"); + } +} + +void EthCommunication::printProtoPositionMsg(protoPositionSSL protoMessage) { + printf("x: %f, y: %f, w: %f, type: %d\n", + protoMessage.x, + protoMessage.y, + protoMessage.w, + protoMessage.posType); +} diff --git a/lib/EthernetCommunication/EthCommunication.h b/lib/EthernetCommunication/EthCommunication.h new file mode 100644 index 0000000..7b64b4c --- /dev/null +++ b/lib/EthernetCommunication/EthCommunication.h @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#define EMPTY_MESSAGE_LENGTH 62535 + +class EthCommunication { + public: + // Class constructor + EthCommunication(const char* ip, const char* netmask, const char* gateway, int port); + + // Class destructor + // ~EthCommunication(); + + bool setup(); + + // FOR RECEIVING MESSAGES + uint16_t update_last_message(); + void decode_proto_position_msg(protoPositionSSL protoMessage); + void decode_proto_motors_msg(protoMotorsPWMSSL protoMessage); + bool eth_recv(); + void get_position(RobotPosition& pos); + void get_motors_pwm(Motors& motors); + msgType get_last_msg_type(); + void get_kick(KickFlags& isKick); + + // FOR SENDING MESSAGES + protoOdometrySSL encode_proto_odometry(RobotInfo robotInfo, + Vector robotOdometry, + Vector robotSpeed); + protoMotorsDataSSL encode_proto_motors_data(Motors currentMotorsSpeed, + Motors motorsPWM, + Motors desiredMotorsSpeed, + double msgTime); + void eth_sendto(const char* ip, + uint16_t port, + RobotInfo robotInfo, + Vector robotOdometry, + Vector robotSpeed); + void eth_sendto(const char* ip, + uint16_t port, + Motors currentMotorsSpeed, + Motors motorsPWM, + Motors desiredMotorsSpeed, + double msgTime); + + // FOR DEBUGGING + void printProtoPositionMsg(protoPositionSSL protoMessage); + + private: + bool _isActive; + const char* _ip; + int _port; + EthernetInterface _eth; + UDPSocket _socket; + SocketAddress _socketAddress; + uint8_t _lastMsg[protoPositionSSL_size]; + msgType _lastMsgType; + RobotPosition _packetPos; + KickFlags _kick; + Motors _packetMotors; +}; diff --git a/lib/Kicker/kicker.cpp b/lib/Kicker/kicker.cpp new file mode 100644 index 0000000..5ab8b62 --- /dev/null +++ b/lib/Kicker/kicker.cpp @@ -0,0 +1,159 @@ +#include "kicker.h" + +Kicker::Kicker(PinName charge, + PinName load, + PinName front, + PinName chip, + PinName IR_LED, + PinName IR_ADC) : + _charge(charge, 0), + _load(load), + _front(front, 1), + _chip(chip, 1), + _irLED(IR_LED), + _irADC(IR_ADC) { + this->stopKick(); + this->stopCharge(); + _irLED.write(0); +} + +void Kicker::init() { + + while(this->getLoad() > NO_LOAD){ + this->discharge(); + ThisThread::sleep_for(200ms); + } + this->stopCharge(); + kickerTimeOut.start(); +} + +void Kicker::update(KickFlags& flagsKick, float minLevel, float maxLevel) { + + flagsKick.ball = (this->readIR() < IR_THRESHOLD); + + if (flagsKick.front || flagsKick.chip) { + if ((flagsKick.ball || flagsKick.bypassIR) && this->getLoad() > minLevel && + utils::timerRead(kickerTimeOut) > KICKER_TIMEOUT) { + // Kick with Front or Chip + this->kick(flagsKick.front, flagsKick.chip, flagsKick.kickStrength); + this->clear(flagsKick); + } else { + // Just Charge + this->charge(maxLevel); + this->stopKick(); + } + } else if (flagsKick.charge) { + // Just Charge + this->charge(maxLevel); + this->stopKick(); + } else { + this->stopCharge(); + this->stopKick(); + } + + if (utils::timerRead(kickerTimeOut) > KICKER_TIMEOUT) { + kickerTimeOut.stop(); + } +} + +void Kicker::clear(KickFlags& isKick) { + isKick.front = false; + isKick.chip = false; + isKick.charge = false; + isKick.dribbler = false; + isKick.ball = (this->readIR() < IR_THRESHOLD); +} + +bool Kicker::isCharging(KickFlags& isKick) { + return isKick.charge || isKick.front || isKick.chip; +} + +float Kicker::getLoad() { + return (_load.read() * 3.3) / MAX_CAP_V_LOAD; +} + +void Kicker::charge(float level) { + this->stopKick(); + currentLoad = (_load.read() * 3.3) / MAX_CAP_V_LOAD; + + if (currentLoad < level) + _charge.write(1); + else + _charge.write(0); +} + +void Kicker::stopCharge() { + _charge.write(0); + wait_us(10); +} + +void Kicker::discharge() { + this->stopCharge(); + _front.write(0); + wait_us(1000); + _front.write(1); +} + +void Kicker::kick(bool kickFront, bool kickChip, float strength) { + kickerTimeOut.reset(); + kickerTimeOut.start(); + + if (kickFront) { + this->front(strength); + } else if (kickChip) { + this->chip(strength); + } +} + +void Kicker::front(float strength) { + _charge.write(0); + wait_us(1); + _front.write(0); // Activate Front + wait_us(int(1000 * fmax(strength, 1))); + _front.write(1); // Activate Front +} + +void Kicker::chip(float strength) { + _charge.write(0); + wait_us(1); + _chip.write(0); // Activate Chip + wait_us(int(1000 * fmax(strength, 1))); + _chip.write(1); +} + +void Kicker::stopKick() { + _front.write(1); + _chip.write(1); +} + +float Kicker::readIR() { + _irLED.write(1); + wait_us(70); + _irReading = _irADC.read(); + wait_us(1); + _irLED.write(0); + return _irReading; +} + +void Kicker::printKick(KickFlags& isKick) { + printf( + "Kicker - Load: %f | Ball: %d | IR %f | Front: %d | Chip: %d | Charge: %d | BypassIR: %d | Strength: %f \n", + this->getLoad(), + isKick.ball, + this->readIR(), + isKick.front, + isKick.chip, + isKick.charge, + isKick.bypassIR, + isKick.kickStrength); + printf("Kicker - Dribbler:%d | Speed: %f \n", isKick.dribbler, isKick.dribblerSpeed); +} + +void Kicker::getKickerInfo(double& kickLoad, bool& ball) { + kickLoad = this->getLoad(); + ball = (this->readIR() < IR_THRESHOLD); +} + +double Kicker::getDribblerSpeed(double speed) { + return -(speed * (MAX_DRIBBLER_SPEED - MIN_DRIBBLER_SPEED) / 100 + MIN_DRIBBLER_SPEED); +} diff --git a/lib/Kicker/kicker.h b/lib/Kicker/kicker.h new file mode 100644 index 0000000..afe194c --- /dev/null +++ b/lib/Kicker/kicker.h @@ -0,0 +1,47 @@ +#ifndef KICKER_H +#define KICKER_H + +#include "mbed.h" +#include + +class Kicker { + public: + Kicker(PinName charge_kick, + PinName load_kick, + PinName front_kick, + PinName chip_kick, + PinName IR_LED, + PinName IR_ADC); + void init(); + void update(KickFlags& flagsKick, float minLevel, float maxLevel); + void charge(float level = 0.5); + void stopCharge(); + void discharge(); + float getLoad(); + void clear(KickFlags& isKick); + bool isCharging(KickFlags& isKick); + float readIR(); + void kick(bool kickFront, bool kickChip, float strength); + void front(float strength); + void chip(float strength); + void stopKick(); + void printKick(KickFlags& isKick); + void getKickerInfo(double& kickLoad, bool& ball); + double getDribblerSpeed(double speed); + + private: + DigitalOut _charge; + AnalogIn _load; + DigitalOut _front; + DigitalOut _chip; + + DigitalOut _irLED; + AnalogIn _irADC; + float _irReading = 0; + Timer kickerTimeOut; + + bool hysteresis_flag = false; + float currentLoad = 0; +}; + +#endif /* KICKER_H */ diff --git a/lib/Kinematics/kinematics.cpp b/lib/Kinematics/kinematics.cpp new file mode 100644 index 0000000..00ec1e7 --- /dev/null +++ b/lib/Kinematics/kinematics.cpp @@ -0,0 +1,161 @@ +#include "kinematics.h" +#include + +Kinematics::Kinematics() { + + /** + * Attention: + * 1. The formal matrix calculetes motors in anti-clock order. + * 2. The robot board uses motors in clock order. + */ + + // Matrix for calculating Inverse Kinematics + i_kinematic_m1.push_back(sin(angle_m1) / wheel_radius); // vx + i_kinematic_m1.push_back(cos(angle_m1) / wheel_radius); // vy + i_kinematic_m1.push_back(robot_radius / wheel_radius); // w + + i_kinematic_m2.push_back(sin(angle_m2) / wheel_radius); // vx + i_kinematic_m2.push_back(-1 * cos(angle_m2) / wheel_radius); // vy + i_kinematic_m2.push_back(robot_radius / wheel_radius); // w + + i_kinematic_m3.push_back(-1 * sin(angle_m3) / wheel_radius); // vx + i_kinematic_m3.push_back(-1 * cos(angle_m3) / wheel_radius); // vy + i_kinematic_m3.push_back(robot_radius / wheel_radius); // w + + i_kinematic_m4.push_back(-1 * sin(angle_m4) / wheel_radius); // vx + i_kinematic_m4.push_back(cos(angle_m4) / wheel_radius); // vy + i_kinematic_m4.push_back(robot_radius / wheel_radius); // w + + // Matrix for calculating Direct Kinematics + kinematic_x.push_back(0.34641 * wheel_radius); // m1(robot) + kinematic_x.push_back(0.282843 * wheel_radius); // m2(robot) + kinematic_x.push_back(-0.282843 * wheel_radius); // m3(robot) + kinematic_x.push_back(-0.34641 * wheel_radius); // m4(robot) + + kinematic_y.push_back(0.414214 * wheel_radius); // m1(robot) + kinematic_y.push_back(-0.414216 * wheel_radius); // m2(robot) + kinematic_y.push_back(-0.414214 * wheel_radius); // m3(robot) + kinematic_y.push_back(0.414214 * wheel_radius); // m4(robot) + +#if defined(DIRECT_TRANSMISSION) + kinematic_w.push_back(3.56751789 * wheel_radius); // m1(robot) + kinematic_w.push_back(2.52261609 * wheel_radius); // m2(robot) + kinematic_w.push_back(2.52261609 * wheel_radius); // m3(robot) + kinematic_w.push_back(3.56751789 * wheel_radius); // m4(robot) +#else + kinematic_w.push_back(3.616966 * wheel_radius); // m1(robot) + kinematic_w.push_back(2.556874 * wheel_radius); // m2(robot) + kinematic_w.push_back(2.556874 * wheel_radius); // m3(robot) + kinematic_w.push_back(3.616966 * wheel_radius); // m4(robot) +#endif + + opt_kinematic_x.push_back(opt_parameters[0] * opt_parameters[12]); // m1(robot) + opt_kinematic_x.push_back(opt_parameters[1] * opt_parameters[12]); // m2(robot) + opt_kinematic_x.push_back(opt_parameters[2] * opt_parameters[12]); // m3(robot) + opt_kinematic_x.push_back(opt_parameters[3] * opt_parameters[12]); // m4(robot) + + opt_kinematic_y.push_back(opt_parameters[4] * opt_parameters[12]); // m1(robot) + opt_kinematic_y.push_back(opt_parameters[5] * opt_parameters[12]); // m2(robot) + opt_kinematic_y.push_back(opt_parameters[6] * opt_parameters[12]); // m3(robot) + opt_kinematic_y.push_back(opt_parameters[7] * opt_parameters[12]); // m4(robot) + + opt_kinematic_w.push_back(opt_parameters[8] * opt_parameters[12]); // m1(robot) + opt_kinematic_w.push_back(opt_parameters[9] * opt_parameters[12]); // m2(robot) + opt_kinematic_w.push_back(opt_parameters[10] * opt_parameters[12]); // m3(robot) + opt_kinematic_w.push_back(opt_parameters[11] * opt_parameters[12]); // m4(robot) +} + +Motors Kinematics::convertToWheel(double vx, double vy, double w) { + Motors m; + m.m1 = ((i_kinematic_m1[0] * vx) + (i_kinematic_m1[1] * vy) + (i_kinematic_m1[2] * w)); + m.m2 = ((i_kinematic_m2[0] * vx) + (i_kinematic_m2[1] * vy) + (i_kinematic_m2[2] * w)); + m.m3 = ((i_kinematic_m3[0] * vx) + (i_kinematic_m3[1] * vy) + (i_kinematic_m3[2] * w)); + m.m4 = ((i_kinematic_m4[0] * vx) + (i_kinematic_m4[1] * vy) + (i_kinematic_m4[2] * w)); + return m; +} + +Motors Kinematics::convertToWheel(Vector v) { + return this->convertToWheel(v.x, v.y, v.w); +} + +Motors Kinematics::clearSpeeds() { + Motors m; + m.m1 = 0; + m.m2 = 0; + m.m3 = 0; + m.m4 = 0; + return m; +} + +Vector Kinematics::convertToVector(Motors m) { + Vector v; + v.x = ((opt_kinematic_x[0] * m.m1) + (opt_kinematic_x[1] * m.m2) + (opt_kinematic_x[2] * m.m3) + + (opt_kinematic_x[3] * m.m4)); + v.y = ((opt_kinematic_y[0] * m.m1) + (opt_kinematic_y[1] * m.m2) + (opt_kinematic_y[2] * m.m3) + + (opt_kinematic_y[3] * m.m4)); + v.w = ((opt_kinematic_w[0] * m.m1) + (opt_kinematic_w[1] * m.m2) + (opt_kinematic_w[2] * m.m3) + + (opt_kinematic_w[3] * m.m4)); + v.x = fabs(v.x) < 0.0001 ? 0 : v.x; + v.y = fabs(v.y) < 0.0001 ? 0 : v.y; + v.w = fabs(v.w) < 0.0001 ? 0 : v.w; + + return v; +} + +Vector Kinematics::convertToVectorOriginal(Motors m) { + Vector v; + v.x = ((kinematic_x[0] * m.m1) + (kinematic_x[1] * m.m2) + (kinematic_x[2] * m.m3) + + (kinematic_x[3] * m.m4)); + v.y = ((kinematic_y[0] * m.m1) + (kinematic_y[1] * m.m2) + (kinematic_y[2] * m.m3) + + (kinematic_y[3] * m.m4)); + v.w = ((kinematic_w[0] * m.m1) + (kinematic_w[1] * m.m2) + (kinematic_w[2] * m.m3) + + (kinematic_w[3] * m.m4)); + v.x = fabs(v.x) < 0.0001 ? 0 : v.x; + v.y = fabs(v.y) < 0.0001 ? 0 : v.y; + v.w = fabs(v.w) < 0.0001 ? 0 : v.w; + + return v; +} + +Vector Kinematics::rotateToLocal(double robot_w, Vector v) { + Vector l; + l.x = v.x * cos(robot_w) + v.y * sin(robot_w); + l.y = -v.x * sin(robot_w) + v.y * cos(robot_w); + l.w = v.w; + return l; +} + +Vector Kinematics::rotateToGlobal(double robot_w, Vector v) { + Vector g; + g.x = v.x * cos(robot_w + v.w / 2) - v.y * sin(robot_w + v.w / 2); + g.y = v.x * sin(robot_w + v.w / 2) + v.y * cos(robot_w + v.w / 2); + g.w = v.w; + return g; +} + +Vector Kinematics::rotateVector(double rotateAngle, Vector v) { + Vector g; + g.x = v.x * cos(rotateAngle) - v.y * sin(rotateAngle); + g.y = v.x * sin(rotateAngle) + v.y * cos(rotateAngle); + g.w = v.w; + return g; +} + +Vector Kinematics::clearVector() { + Vector v; + v.x = 0; + v.y = 0; + v.w = 0; + return v; +} + +void Kinematics::validateSpeeds(Vector v) { + Motors m = this->convertToWheel(v); + printf("v-M1: %.3lf | v-M2: %.3lf | v-M3: %.3lf | v-M4: %.3lf\n", m.m1, m.m2, m.m3, m.m4); + Vector v_converted = this->convertToVector(m); + printf("Sent Vect: v-x: %.3lf | v-y: %.3lf | v-w: %.3lf\n", v.x, v.y, v.w); + printf("Converted: v-x: %.3lf | v-y: %.3lf | v-w: %.3lf\n\n\n", + v_converted.x, + v_converted.y, + v_converted.w); +} diff --git a/lib/Kinematics/kinematics.h b/lib/Kinematics/kinematics.h new file mode 100644 index 0000000..b5a3982 --- /dev/null +++ b/lib/Kinematics/kinematics.h @@ -0,0 +1,65 @@ +#ifndef KINEMATICS_H +#define KINEMATICS_H + +#include "mbed.h" +#define _USE_MATH_DEFINES +#include +#include + +class Kinematics { + public: + Kinematics(); + Motors convertToWheel(double vx, double vy, double w); + Motors convertToWheel(Vector v); + Motors clearSpeeds(); + Vector rotateToLocal(double robot_w, Vector v); + Vector rotateToGlobal(double robot_w, Vector v); + Vector rotateVector(double rotateAngle, Vector v); + Vector convertToVector(Motors m); + Vector convertToVectorOriginal(Motors m); + Vector clearVector(); + void validateSpeeds(Vector v); + + private: +#if defined(DIRECT_TRANSMISSION) + double wheel_radius = 0.03005; + double robot_radius = 0.08105; +#else + double robot_radius = 0.02475; + double half_axis = 0.0831; +#endif + double angle_m1 = M_PI / 3; + double angle_m2 = M_PI / 4; + double angle_m3 = M_PI / 4; + double angle_m4 = M_PI / 3; + + std::vector i_kinematic_m1; + std::vector i_kinematic_m2; + std::vector i_kinematic_m3; + std::vector i_kinematic_m4; + + std::vector kinematic_x; + std::vector kinematic_y; + std::vector kinematic_w; + + std::vector opt_kinematic_x; + std::vector opt_kinematic_y; + std::vector opt_kinematic_w; + + // ROBOT 0 second optimization (23-01-10) + std::vector opt_parameters = std::vector{0.42447140713655457, + 0.35482242703016686, + -0.1823307411279881, + -0.274779656546075, + 0.25745996970744844, + -0.4438060201135028, + -0.35102689244950486, + 0.23426282528626358, + 3.611082557939859, + 2.6806232596725357, + 2.5552490512551853, + 3.5933353127553165, + 0.022848272302565545}; +}; + +#endif /* KINEMATICS_H */ diff --git a/lib/MotionControl/FastPWM/.hg_archival.txt b/lib/MotionControl/FastPWM/.hg_archival.txt new file mode 100644 index 0000000..79b6600 --- /dev/null +++ b/lib/MotionControl/FastPWM/.hg_archival.txt @@ -0,0 +1,5 @@ +repo: f8c1b0ad5371b8b01d73cc6fab47459e6e0ba7c1 +node: c0b2265cff9c510e35914d7a62d3977aa805694e +branch: default +latesttag: null +latesttagdistance: 35 diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_KLXX_K20D50M.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_KLXX_K20D50M.cpp new file mode 100644 index 0000000..c7ce072 --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_KLXX_K20D50M.cpp @@ -0,0 +1,54 @@ +#if defined(TARGET_KLXX) || defined(TARGET_K20D50M) + + #include "FastPWM.h" + +void FastPWM::initFastPWM(void) { + bits = 16; +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + *(_pwm.CnV) = ticks; +} + +void FastPWM::period_ticks(uint32_t ticks) { + *(_pwm.MOD) = ticks - 1; +} + +uint32_t FastPWM::getPeriod(void) { + return *(_pwm.MOD) + 1; +} + +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + + // Yes this is ugly, yes I should feel bad about it + volatile uint32_t* TPM_SC = _pwm.MOD - 2; + + const char prescalers[] = {1, 2, 4, 8, 16, 32, 64, 128}; + + // If prescaler is 0, return current one + if (reqScale == 0) + return (prescalers[(*TPM_SC) & 0x07]); + + uint32_t retval = 0; + char bin; + + for (bin = 0; bin < 8; bin++) { + retval = prescalers[bin]; + if (retval >= reqScale) + break; + } + if (bin == 8) + bin = 7; + + // Clear lower 5 bits, write new value: + char clockbits = *TPM_SC & (3 << 3); + + // For some reason clearing them takes some effort + while ((*TPM_SC & 0x1F) != 0) + *TPM_SC &= ~0x1F; + + *TPM_SC = bin + clockbits; + + return retval; +} +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_KSDK.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_KSDK.cpp new file mode 100644 index 0000000..ee83d7a --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_KSDK.cpp @@ -0,0 +1,87 @@ +#if defined(TARGET_KPSDK_MCUS) + + #include "FastPWM.h" + #include "fsl_ftm.h" + + #define PWM_CNV (*(((fastpwm_struct*) fast_obj)->CnV)) + #define PWM_MOD (*(((fastpwm_struct*) fast_obj)->MOD)) + #define PWM_SC (*(((fastpwm_struct*) fast_obj)->SC)) + #define PWM_SYNC (*(((fastpwm_struct*) fast_obj)->SYNC)) + +typedef struct { + __IO uint32_t* CnV; + __IO uint32_t* MOD; + __IO uint32_t* SC; + __IO uint32_t* SYNC; +} fastpwm_struct; + +static uint32_t pwm_prescaler; +static FTM_Type* const ftm_addrs[] = FTM_BASE_PTRS; + +void FastPWM::initFastPWM(void) { + fast_obj = new fastpwm_struct; + bits = 16; + + pwm_prescaler = SystemCoreClock / CLOCK_GetFreq(kCLOCK_BusClk); + ; + + unsigned int ch_n = (_pwm.pwm_name & 0xF); + FTM_Type* ftm = ftm_addrs[_pwm.pwm_name >> TPM_SHIFT]; + + ((fastpwm_struct*) fast_obj)->CnV = &ftm->CONTROLS[ch_n].CnV; + ((fastpwm_struct*) fast_obj)->MOD = &ftm->MOD; + ((fastpwm_struct*) fast_obj)->SC = &ftm->SC; + ((fastpwm_struct*) fast_obj)->SYNC = &ftm->SYNC; + + // Do not clear counter when writing new value, set end of period as loading value + ftm->SYNCONF &= ~FTM_SYNCONF_SWRSTCNT_MASK; + ftm->SYNC |= FTM_SYNC_CNTMAX_MASK; +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + PWM_CNV = ticks; + PWM_SYNC |= FTM_SYNC_SWSYNC_MASK; +} + +void FastPWM::period_ticks(uint32_t ticks) { + PWM_MOD = ticks - 1; + PWM_SYNC |= FTM_SYNC_SWSYNC_MASK; +} + +uint32_t FastPWM::getPeriod(void) { + return PWM_MOD + 1; +} + +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + + uint32_t prescalers[] = {1, 2, 4, 8, 16, 32, 64, 128}; + + for (int i = 0; i < 8; i++) + prescalers[i] = prescalers[i] * pwm_prescaler; + + // If prescaler is 0, return current one + if (reqScale == 0) + return (prescalers[(PWM_SC) &0x07]); + + uint32_t retval = 0; + char bin; + + for (bin = 0; bin < 8; bin++) { + retval = prescalers[bin]; + if (retval >= reqScale) + break; + } + if (bin == 8) + bin = 7; + + // Clear lower 5 bits, write new value: + char clockbits = PWM_SC & (3 << 3); + + // For some reason clearing them takes some effort + while ((PWM_SC & 0x1F) != 0) + PWM_SC &= ~0x1F; + + PWM_SC = bin + clockbits; + return retval; +} +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_LPC11XX.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_LPC11XX.cpp new file mode 100644 index 0000000..fe732c0 --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_LPC11XX.cpp @@ -0,0 +1,104 @@ +#if defined(TARGET_LPC11UXX) || defined(TARGET_LPC11XX_11CXX) + + #include "FastPWM.h" + + #define PWM_MR (*(((fastpwm_struct*) fast_obj)->MR)) + #define PWM_TIMER (((fastpwm_struct*) fast_obj)->timer) + +typedef struct { + uint8_t timer; + uint8_t mr; +} timer_mr; + + #ifdef TARGET_LPC11UXX +typedef struct { + __IO uint32_t* MR; + LPC_CTxxBx_Type* timer; +} fastpwm_struct; + +static timer_mr pwm_timer_map[11] = { + {0, 0}, + {0, 1}, + {0, 2}, + {1, 0}, + {1, 1}, + {2, 0}, + {2, 1}, + {2, 2}, + {3, 0}, + {3, 1}, + {3, 2}, +}; + +static LPC_CTxxBx_Type* Timers[4] = {LPC_CT16B0, LPC_CT16B1, LPC_CT32B0, LPC_CT32B1}; + #else // LPC11XX +typedef struct { + __IO uint32_t* MR; + LPC_TMR_TypeDef* timer; +} fastpwm_struct; + +static timer_mr pwm_timer_map[5] = { + {0, 0}, /* CT16B0, MR0 */ + {0, 1}, /* CT16B0, MR1 */ + + {1, 0}, /* CT16B1, MR0 */ + {1, 1}, /* CT16B1, MR1 */ + + {2, 2}, /* CT32B0, MR2 */ +}; + +static LPC_TMR_TypeDef* Timers[3] = {LPC_TMR16B0, LPC_TMR16B1, LPC_TMR32B0}; + #endif + +void FastPWM::initFastPWM(void) { + fast_obj = new fastpwm_struct; + timer_mr tid = pwm_timer_map[_pwm.pwm]; + PWM_TIMER = Timers[tid.timer]; + (((fastpwm_struct*) fast_obj)->MR) = &PWM_TIMER->MR[tid.mr]; + + if (tid.timer < 2) + // 16-bit timer + bits = 16; + else + // 32-bit timer + bits = 32; +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + if (ticks) + PWM_MR = PWM_TIMER->MR3 - ticks; // They inverted PWM on the 11u24 + else + PWM_MR = 0xFFFFFFFF; // If MR3 = ticks 1 clock cycle wide errors appear, this prevents that + // (unless MR3 = max). +} + +void FastPWM::period_ticks(uint32_t ticks) { + PWM_TIMER->TCR = 0x02; + PWM_TIMER->MR3 = ticks; + PWM_TIMER->TCR = 0x01; +} + +uint32_t FastPWM::getPeriod(void) { + return PWM_TIMER->MR3; +} + +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + // If 32-bit, disable auto-scaling, return 1 + if (bits == 32) { + dynamicPrescaler = false; + return 1; + } + + // Else 16-bit timer: + if (reqScale == 0) + // Return prescaler + return PWM_TIMER->PR + 1; + if (reqScale > (uint32_t) (1 << 16)) + reqScale = 1 << 16; + // Else set prescaler, we have to substract one from reqScale since a 0 in PCVAL is prescaler of 1 + PWM_TIMER->PR = reqScale - 1; + + return reqScale; +} + +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_LPC1768.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_LPC1768.cpp new file mode 100644 index 0000000..517c42c --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_LPC1768.cpp @@ -0,0 +1,34 @@ +#ifdef TARGET_LPC176X + + #include "FastPWM.h" + +void FastPWM::initFastPWM(void) { + // Set clock source + LPC_SC->PCLKSEL0 |= 1 << 12; + bits = 32; +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + *(_pwm.MR) = ticks; + LPC_PWM1->LER |= 1 << _pwm.pwm; +} + +void FastPWM::period_ticks(uint32_t ticks) { + LPC_PWM1->MR0 = ticks; + LPC_PWM1->LER |= 1 << 0; +} + +uint32_t FastPWM::getPeriod(void) { + return LPC_PWM1->MR0; +} + +// Maybe implemented later, but needing to change the prescaler for a 32-bit +// timer used in PWM mode is kinda unlikely. +// If you really need to do it, rejoice, you can make it run so slow a period is over 40,000 year +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + // Disable dynamic prescaling + dynamicPrescaler = false; + + return 1; +} +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_LPC_SCT.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_LPC_SCT.cpp new file mode 100644 index 0000000..0102097 --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_LPC_SCT.cpp @@ -0,0 +1,64 @@ +// For targets which use the SCT +#if defined(TARGET_LPC81X) || defined(TARGET_LPC82X) + + #ifdef TARGET_LPC82X + #define CTRL_U CTRL + #endif + + #include "FastPWM.h" + +void FastPWM::initFastPWM(void) { + // Mbed uses the timer as a single unified 32-bit timer, who are we to argue with this, and it is + // easier + bits = 32; + + #ifdef TARGET_LPC82X + // The mbed lib uses the PWM peripheral slightly different, which is irritating. This sets it bck + // to the LPC81X + _pwm.pwm->EVENT[_pwm.pwm_ch + 1].CTRL = (1 << 12) | (_pwm.pwm_ch + 1); // Event_n on Match_n + _pwm.pwm->EVENT[_pwm.pwm_ch + 1].STATE = 0xFFFFFFFF; // All states + _pwm.pwm->OUT[_pwm.pwm_ch].SET = (1 << 0); // All PWM channels are SET on Event_0 + _pwm.pwm->OUT[_pwm.pwm_ch].CLR = (1 << (_pwm.pwm_ch + 1)); // PWM ch is CLRed on Event_(ch+1) + #endif + + // With 32-bit we fix prescaler to 1 + _pwm.pwm->CTRL_U |= (1 << 2) | (1 << 3); + _pwm.pwm->CTRL_U &= ~(0x7F << 5); + _pwm.pwm->CTRL_U &= ~(1 << 2); +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + #ifdef TARGET_LPC81X + _pwm.pwm->MATCHREL[_pwm.pwm_ch + 1].U = ticks; + #else + _pwm.pwm->MATCHREL[_pwm.pwm_ch + 1] = ticks; + #endif +} + +void FastPWM::period_ticks(uint32_t ticks) { + #ifdef TARGET_LPC81X + _pwm.pwm->MATCHREL[0].U = ticks; + #else + _pwm.pwm->MATCHREL[0] = ticks; + #endif +} + +uint32_t FastPWM::getPeriod(void) { + #ifdef TARGET_LPC81X + return _pwm.pwm->MATCHREL[0].U; + #else + return _pwm.pwm->MATCHREL[0]; + #endif +} + +// Maybe implemented later, but needing to change the prescaler for a 32-bit +// timer used in PWM mode is kinda unlikely. +// If you really need to do it, rejoice, you can make it run so slow a period is over 40,000 year +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + // Disable dynamic prescaling + dynamicPrescaler = false; + + return 1; +} + +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM.cpp new file mode 100644 index 0000000..199939b --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM.cpp @@ -0,0 +1,76 @@ +// This should (hopefully) work on all STM targets which use TIM timers for PWM + +#ifdef TARGET_STM + + #include "FastPWM.h" + +typedef __IO uint32_t* CHANNEL_P_T; + +typedef struct { + CHANNEL_P_T channel; + uint32_t clk_prescaler; +} fastpwm_struct; + + #define PWM_CHANNEL ((((fastpwm_struct*) fast_obj)->channel)) + #define PWM_CLK_PRESCALER ((((fastpwm_struct*) fast_obj)->clk_prescaler)) + #define PWM_TIMER ((TIM_TypeDef*) _pwm.pwm) + + #if defined(TARGET_STM32F0) || defined(TARGET_STM32F1) || defined(TARGET_STM32L1) +extern __IO uint32_t* getChannel(TIM_TypeDef* pwm, PinName pin); + #endif + +void FastPWM::initFastPWM(void) { + fast_obj = new fastpwm_struct; + #if defined(TARGET_STM32F0) || defined(TARGET_STM32F1) || defined(TARGET_STM32L1) + PWM_CHANNEL = getChannel(PWM_TIMER, _pwm.pin); + #else + PWM_CHANNEL = (&PWM_TIMER->CCR1 + _pwm.channel - 1); + #endif + + // Depending on the timer and the internal bus it is connected to, each STM timer + // can have a fixed prescaler from the clock, especially the faster devices. + // In order not to have to hardcode this in, we use knowledge that mbed lib sets + // default period to 20ms to reverse engineer the prescaler from this. + uint32_t current_hz = SystemCoreClock / (PWM_TIMER->PSC + 1) / (PWM_TIMER->ARR + 1); + PWM_CLK_PRESCALER = (current_hz + 1) / 50; // 50Hz is magic number it should be, +1 is to handle + // possible rounding errors in mbed setup + + // Sanity check in case a target does something different + if ((PWM_CLK_PRESCALER == 0) || (PWM_CLK_PRESCALER > 16)) { + PWM_CLK_PRESCALER = 1; + } + + // Enable PWM period syncing for glitch free result + PWM_TIMER->CR1 |= TIM_CR1_ARPE; + + bits = 16; +} + +void FastPWM::pulsewidth_ticks(uint32_t ticks) { + *PWM_CHANNEL = ticks; +} + +void FastPWM::period_ticks(uint32_t ticks) { + PWM_TIMER->ARR = ticks - 1; +} + +uint32_t FastPWM::getPeriod(void) { + return PWM_TIMER->ARR + 1; +} + +uint32_t FastPWM::setPrescaler(uint32_t reqScale) { + if (reqScale == 0) { + // Return prescaler + return (PWM_TIMER->PSC + 1) * PWM_CLK_PRESCALER; + } + if (reqScale > (uint32_t) (PWM_CLK_PRESCALER << 16)) { + reqScale = PWM_CLK_PRESCALER << 16; + } + // Else set prescaler, we have to substract one from reqScale since a 0 in PCVAL is prescaler of 1 + // Take into account PWM_CLK_PRESCALER, we need to make sure reqScale is always rounded up + PWM_TIMER->PSC = (reqScale + PWM_CLK_PRESCALER - 1) / PWM_CLK_PRESCALER - 1; + + return setPrescaler(0); +} + +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM_PinOut.cpp b/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM_PinOut.cpp new file mode 100644 index 0000000..fa37346 --- /dev/null +++ b/lib/MotionControl/FastPWM/Device/FastPWM_STM_TIM_PinOut.cpp @@ -0,0 +1,152 @@ +#include "mbed.h" + +#if defined(TARGET_NUCLEO_F030R8) || (TARGET_DISCO_F051R8) +__IO uint32_t* getChannel(TIM_TypeDef* pwm, PinName pin) { + switch (pin) { + // Channels 1 + case PA_4: + case PA_6: + case PB_1: + case PB_4: + case PB_8: + case PB_9: + case PB_14: + case PC_6: + case PB_6: + case PB_7: return &pwm->CCR1; + + // Channels 2 + case PA_7: + case PB_5: + case PC_7: return &pwm->CCR2; + + // Channels 3 + case PB_0: + case PC_8: return &pwm->CCR3; + + // Channels 4 + case PC_9: return &pwm->CCR4; + } + return NULL; +} +#endif + +#if defined(TARGET_NUCLEO_F103RB) || (TARGET_DISCO_F100RB) +__IO uint32_t* getChannel(TIM_TypeDef* pwm, PinName pin) { + switch (pin) { + // Channels 1 : PWMx/1 + case PA_6: + case PA_8: + case PA_15: + case PB_4: + case PC_6: + case PB_13: return &pwm->CCR1; + + // Channels 2 : PWMx/2 + case PA_1: + case PA_7: + case PA_9: + case PB_3: + case PB_5: + case PC_7: + case PB_14: return &pwm->CCR2; + + // Channels 3 : PWMx/3 + case PA_2: + case PA_10: + case PB_0: + case PB_10: + case PC_8: + case PB_15: return &pwm->CCR3; + + // Channels 4 : PWMx/4 + case PA_3: + case PA_11: + case PB_1: + case PB_11: + case PC_9: return &pwm->CCR4; + } + return NULL; +} +#endif + +#if defined TARGET_NUCLEO_F072RB +__IO uint32_t* getChannel(TIM_TypeDef* pwm, PinName pin) { + switch (pin) { + // Channels 1 : PWMx/1 + case PA_2: + case PA_6: + case PA_4: + case PA_7: + case PA_8: + case PB_1: + case PB_4: + case PB_8: + case PB_9: + case PB_14: + case PC_6: + // Channels 1N : PWMx/1N + case PA_1: + case PB_6: + case PB_7: + case PB_13: return &pwm->CCR1; + + // Channels 2 : PWMx/2 + case PA_3: + case PA_9: + case PB_5: + case PC_7: + case PB_15: return &pwm->CCR2; + + // Channels 3 : PWMx/3 + case PA_10: + case PB_0: + case PC_8: return &pwm->CCR3; + + // Channels 4 : PWMx/4 + case PA_11: + case PC_9: return &pwm->CCR4; + } + return NULL; +} +#endif + +#if defined(TARGET_NUCLEO_L152RE) +__IO uint32_t* getChannel(TIM_TypeDef* pwm, PinName pin) { + switch (pin) { + // Channels 1 : PWMx/1 + case PA_6: + case PB_4: + case PB_12: + case PB_13: + case PC_6: return &pwm->CCR1; + + // Channels 2 : PWMx/2 + case PA_1: + case PA_7: + case PB_3: + case PB_5: + case PB_14: + case PB_7: + case PC_7: return &pwm->CCR2; + + // Channels 3 : PWMx/3 + case PA_2: + case PB_0: + case PB_8: + case PB_10: + case PC_8: return &pwm->CCR3; + + // Channels 4 : PWMx/4 + case PA_3: + case PB_1: + case PB_9: + case PB_11: + case PC_9: return &pwm->CCR4; + default: + /* NOP */ + break; + } + return NULL; +} +#endif diff --git a/lib/MotionControl/FastPWM/FastPWM.h b/lib/MotionControl/FastPWM/FastPWM.h new file mode 100644 index 0000000..37b647f --- /dev/null +++ b/lib/MotionControl/FastPWM/FastPWM.h @@ -0,0 +1,154 @@ +/* + .---. _....._ + / p `\ .-""`: :`"-. + |__ - | ,' . ' ', + ._> \ /: : ; :, + '-. '\`. . : ' \ + `. | .'._.' '._.' '._.'. | + `;-\. : : ' '/,__, + .-'`'._ ' . : _.'.__.' + ((((-'/ `";--..:..--;"` \ + .' / \ \ + jgs ((((-' ((((-' + +Yeah ASCII art turtle more fun than copyright stuff +*/ + +#include "mbed.h" + +#ifndef FASTPWM_H + #define FASTPWM_H + +/** Library that allows faster and/or higher resolution PWM output + * + * Library can directly replace standard mbed PWM library. + * + * Contrary to the default mbed library, this library takes doubles instead of floats. The compiler + * will autocast if needed, but do take into account it is done for a reason, your accuracy will + * otherwise be limitted by the floating point precision. + */ +class FastPWM : public PwmOut { + public: + /** + * Create a FastPWM object connected to the specified pin + * + * @param pin - PWM pin to connect to + * @param prescaler - Clock prescaler, -1 is dynamic (default), 0 is bit random, everything else + * normal + */ + FastPWM(PinName pin, int prescaler = -1); + ~FastPWM(); + + /** + * Set the PWM period, specified in seconds (double), keeping the pulsewidth the same. + */ + void period(double seconds); + + /** + * Set the PWM period, specified in milli-seconds (int), keeping the pulsewidth the same. + */ + void period_ms(int ms); + + /** + * Set the PWM period, specified in micro-seconds (int), keeping the pulsewidth the same. + */ + void period_us(int us); + + /** + * Set the PWM period, specified in micro-seconds (double), keeping the pulsewidth the same. + */ + void period_us(double us); + + /** + * Set the PWM period, specified in clock ticks, keeping _pulse width_ the same. + * + * This function can be used if low overhead is required. Do take into account the result is + * board (clock frequency) dependent, and this does not keep an equal duty cycle! + */ + void period_ticks(uint32_t ticks); + + /** + * Set the PWM pulsewidth, specified in seconds (double), keeping the period the same. + */ + void pulsewidth(double seconds); + + /** + * Set the PWM pulsewidth, specified in milli-seconds (int), keeping the period the same. + */ + void pulsewidth_ms(int ms); + + /** + * Set the PWM pulsewidth, specified in micro-seconds (int), keeping the period the same. + */ + void pulsewidth_us(int us); + + /** + * Set the PWM pulsewidth, specified in micro-seconds (double), keeping the period the same. + */ + void pulsewidth_us(double us); + + /** + * Set the PWM period, specified in clock ticks, keeping the period the same. + * + * This function can be used if low overhead is required. Do take into account the result is + * board (clock frequency) dependent! + */ + void pulsewidth_ticks(uint32_t ticks); + + /** + * Set the ouput duty-cycle, specified as a percentage (double) + * + * @param duty - A double value representing the output duty-cycle, specified as a percentage. The + * value should lie between 0.0 (representing on 0%) and 1.0 (representing on 100%). + */ + void write(double duty); + + /** + * Return the ouput duty-cycle, specified as a percentage (double) + * + * @param return - A double value representing the output duty-cycle, specified as a percentage. + */ + double read(void); + + /** + * An operator shorthand for write() + */ + FastPWM& operator=(double value); + + /** + * An operator shorthand for read() + */ + operator double(); + + /** + * Set the PWM prescaler + * + * The period of all PWM pins on the same PWM unit have to be reset after using this! + * + * @param value - The required prescaler. Special values: 0 = lock current prescaler, -1 = use + * dynamic prescaler + * @param return - The prescaler which was set (can differ from requested prescaler if not + * possible) + */ + int prescaler(int value); + + private: + void initFastPWM(void); + + uint32_t setPrescaler(uint32_t reqScale); + int calcPrescaler(uint64_t clocks); + uint32_t getPeriod(void); + + void updateTicks(uint32_t prescaler); + uint32_t bits; + + double _duty; + + double dticks, dticks_us; + int iticks_ms, iticks_us; + + bool dynamicPrescaler; + + void* fast_obj; +}; +#endif \ No newline at end of file diff --git a/lib/MotionControl/FastPWM/FastPWM_common.cpp b/lib/MotionControl/FastPWM/FastPWM_common.cpp new file mode 100644 index 0000000..1f84426 --- /dev/null +++ b/lib/MotionControl/FastPWM/FastPWM_common.cpp @@ -0,0 +1,105 @@ +#include "FastPWM.h" + +FastPWM::FastPWM(PinName pin, int prescaler) : PwmOut(pin) { + fast_obj = NULL; + initFastPWM(); + this->prescaler(prescaler); + + // Set duty cycle on 0%, period on 20ms + period(0.02); + write(0); +} + +FastPWM::~FastPWM(void) { + if (fast_obj != NULL) + delete static_cast(fast_obj); +} + +void FastPWM::period(double seconds) { + if (dynamicPrescaler) + calcPrescaler((uint64_t) (seconds * (double) SystemCoreClock)); + + period_ticks(seconds * dticks + 0.5); +} + +void FastPWM::period_ms(int ms) { + if (dynamicPrescaler) + calcPrescaler(ms * (SystemCoreClock / 1000)); + + period_ticks(ms * iticks_ms); +} + +void FastPWM::period_us(int us) { + if (dynamicPrescaler) + calcPrescaler(us * (SystemCoreClock / 1000000)); + + period_ticks(us * iticks_us); +} + +void FastPWM::period_us(double us) { + if (dynamicPrescaler) + calcPrescaler((uint64_t) (us * (double) (SystemCoreClock / 1000000))); + + period_ticks(us * dticks_us + 0.5); +} + +void FastPWM::pulsewidth(double seconds) { + pulsewidth_ticks(seconds * dticks + 0.5); +} + +void FastPWM::pulsewidth_ms(int ms) { + pulsewidth_ticks(ms * iticks_ms); +} + +void FastPWM::pulsewidth_us(int us) { + pulsewidth_ticks(us * iticks_us); +} + +void FastPWM::pulsewidth_us(double us) { + pulsewidth_ticks(us * dticks_us + 0.5); +} + +void FastPWM::write(double duty) { + _duty = duty; + pulsewidth_ticks(duty * getPeriod()); +} + +double FastPWM::read(void) { + return _duty; +} + +FastPWM& FastPWM::operator=(double value) { + write(value); + return (*this); +} + +FastPWM::operator double() { + return _duty; +} + +int FastPWM::prescaler(int value) { + int retval; + if (value == -1) { + dynamicPrescaler = true; + value = 0; + } else + dynamicPrescaler = false; + + retval = setPrescaler(value); + updateTicks(retval); + return retval; +} + +void FastPWM::updateTicks(uint32_t prescaler) { + dticks = SystemCoreClock / (double) prescaler; + dticks_us = dticks / 1000000.0f; + iticks_us = (int) (dticks_us + 0.5); + iticks_ms = (int) (dticks_us * 1000.0 + 0.5); +} + +int FastPWM::calcPrescaler(uint64_t clocks) { + uint32_t scale = (clocks >> bits) + 1; + uint32_t retval = setPrescaler(scale); + updateTicks(retval); + return retval; +} \ No newline at end of file diff --git a/lib/MotionControl/MotionControl.cpp b/lib/MotionControl/MotionControl.cpp new file mode 100644 index 0000000..ad7a750 --- /dev/null +++ b/lib/MotionControl/MotionControl.cpp @@ -0,0 +1,332 @@ +#include "MotionControl.h" + +MotionControl::MotionControl(Kinematics* kinematics) : + _M1(M1_PWM, + M1_DIR, + M1_RST, + M1_MODE, + M1_CST, + M1_BRK, + TIM1, + MOTOR_ENCODER_PULSES, + 2), // sampling time 2ms + _M2(M2_PWM, M2_DIR, M2_RST, M2_MODE, M2_CST, M2_BRK, TIM2, MOTOR_ENCODER_PULSES, 2), + _M3(M3_PWM, M3_DIR, M3_RST, M3_MODE, M3_CST, M3_BRK, TIM8, MOTOR_ENCODER_PULSES, 2), + _M4(M4_PWM, M4_DIR, M4_RST, M4_MODE, M4_CST, M4_BRK, TIM3, MOTOR_ENCODER_PULSES, 2), + _M5(M5_PWM, M5_DIR, M5_RST, M5_MODE, M5_CST, M5_BRK, M5_TAC, M5_DIRO, 6, 70, DRIBBLER) { + _kinematics = kinematics; +} + +void MotionControl::init() { + _M1.init(); + _M2.init(); + _M3.init(); + _M4.init(); + _M5.init(); +} + +Motors MotionControl::moveRobot(Vector desiredVector) { + _motorSpeeds = this->convertWheelSpeed(desiredVector); + moveRobotPID(_motorSpeeds.m1, _motorSpeeds.m2, _motorSpeeds.m3, _motorSpeeds.m4); + return _motorSpeeds; +} + +Motors MotionControl::accelRobot(Vector desiredVector) { + Vector currVector; + + this->getRobotSpeedOriginal(currVector); + + Vector diffVector = desiredVector - currVector; + double diffNorm = sqrt(diffVector.x * diffVector.x + diffVector.y * diffVector.y); + if (diffNorm < 0.001) + return this->moveRobot(desiredVector); + // For bypassing acceleration limitation, use line below: + return this->moveRobot(desiredVector); + + double diffMax = 0.1; + double alpha = std::min((diffNorm), diffMax) / (diffNorm); + + desiredVector.x = currVector.x + diffVector.x * alpha; + desiredVector.y = currVector.y + diffVector.y * alpha; + + // return this->moveRobot(desiredVector); +} + +Motors MotionControl::accelRobot(Vector desiredVector, Vector currentPosition) { + // Rotate vector with past robot's movement + Vector fixDesiredVector = _kinematics->rotateVector(-currentPosition.w, desiredVector); + return this->accelRobot(fixDesiredVector); +} + +Motors MotionControl::slowRobot() { + // Slow down robot + Vector currVector; + return this->accelRobot(currVector); +} + +void MotionControl::moveRobotPID(double m1, double m2, double m3, double m4) { + _M1.updateDesiredSpeed(m1); + _M2.updateDesiredSpeed(m2); + _M3.updateDesiredSpeed(m3); + _M4.updateDesiredSpeed(m4); +} + +void MotionControl::moveRobotPID(double speed) { + moveRobotPID(speed, speed, speed, speed); +} + +void MotionControl::moveRobotPWM(double m1, double m2, double m3, double m4) { + _M1.runPWM(m1); + _M2.runPWM(m2); + _M3.runPWM(m3); + _M4.runPWM(m4); +} + +void MotionControl::moveRobotPWM(double speed) { + _M1.runPWM(speed); + _M2.runPWM(speed); + _M3.runPWM(speed); + _M4.runPWM(speed); +} + +void MotionControl::moveMotorPID(uint8_t idx, double speed) { + if (idx == 1) { + _M1.updateDesiredSpeed(speed); + } else if (idx == 2) { + _M2.updateDesiredSpeed(speed); + } else if (idx == 3) { + _M3.updateDesiredSpeed(speed); + } else if (idx == 4) { + _M4.updateDesiredSpeed(speed); + } else if (idx == 5) { + _M5.runPID(speed, DRIBBLER); + } +} + +void MotionControl::moveMotorPWM(uint8_t idx, double speed) { + if (idx == 1) { + _M1.runPWM(speed); + } else if (idx == 2) { + _M2.runPWM(speed); + } else if (idx == 3) { + _M3.runPWM(speed); + } else if (idx == 4) { + _M4.runPWM(speed); + } else if (idx == 5) { + _M5.runPWM(speed); + } +} + +void MotionControl::updateDribbler(KickFlags& isKick) { + // Control Dribbler + if (isKick.dribbler) { + double d_speed = + fabs(isKick.dribblerSpeed) >= 0.1 ? isKick.dribblerSpeed : DEFAULT_DRIBLER_SPEED; + this->moveMotorPWM(5, d_speed); + } else { + this->stopMotor(5); + } +} + +void MotionControl::getMotorsInfo(RobotInfo& telemetry) { + telemetry.m.m1 = _M1.get_speed_rad_s(); + telemetry.m.m2 = _M2.get_speed_rad_s(); + telemetry.m.m3 = _M3.get_speed_rad_s(); + telemetry.m.m4 = _M4.get_speed_rad_s(); + telemetry.dribbler = _M5.get_speed_rad_s(); +} + +void MotionControl::getMotorsSpeed(Motors& m) { + m.m1 = _M1.get_speed_rad_s(); + m.m2 = _M2.get_speed_rad_s(); + m.m3 = _M3.get_speed_rad_s(); + m.m4 = _M4.get_speed_rad_s(); +} + +void MotionControl::getMotorsDesiredSpeed(Motors& m) { + m.m1 = _M1.get_desired_speed_rad_s(); + m.m2 = _M2.get_desired_speed_rad_s(); + m.m3 = _M3.get_desired_speed_rad_s(); + m.m4 = _M4.get_desired_speed_rad_s(); +} + +void MotionControl::getMotorsPWM(Motors& m) { + m.m1 = _M1.get_current_pwm(); + m.m2 = _M2.get_current_pwm(); + m.m3 = _M3.get_current_pwm(); + m.m4 = _M4.get_current_pwm(); +} + +bool MotionControl::robotIsMoving() { + Motors m; + this->getMotorsSpeed(m); + if (m.m1 == 0 && m.m2 == 0 && m.m3 == 0 && m.m4 == 0) + return false; + return true; +} + +double MotionControl::getMotorSpeed(uint8_t idx) { + double speed = 0; + if (idx == 1) { + speed = _M1.get_speed_rad_s(); + } else if (idx == 2) { + speed = _M2.get_speed_rad_s(); + } else if (idx == 3) { + speed = _M3.get_speed_rad_s(); + } else if (idx == 4) { + speed = _M4.get_speed_rad_s(); + } else if (idx == 5) { + speed = _M5.get_speed_rad_s(); + } + return speed; +} + +void MotionControl::getRobotSpeed(Vector& v) { + Motors speeds; + this->getMotorsSpeed(speeds); + v = _kinematics->convertToVector(speeds); +} + +void MotionControl::getRobotSpeedOriginal(Vector& v) { + Motors speeds; + this->getMotorsSpeed(speeds); + v = _kinematics->convertToVectorOriginal(speeds); +} + +void MotionControl::getRobotSpeed(Vector& v, bool useOptimizedParamaters) { + if (useOptimizedParamaters) { + this->getRobotSpeed(v); + } else { + this->getRobotSpeedOriginal(v); + } +} + +void MotionControl::stopRobot() { + _M1.stop(); + _M2.stop(); + _M3.stop(); + _M4.stop(); + _M5.stop(); +} + +void MotionControl::stopMotor(uint8_t idx) { + if (idx == 1) { + _M1.stop(); + } else if (idx == 2) { + _M2.stop(); + } else if (idx == 3) { + _M3.stop(); + } else if (idx == 4) { + _M4.stop(); + } else if (idx == 5) { + _M5.stop(); + } +} + +void MotionControl::breakRobot(double haltSpeed) { + if (this->shouldStop(_M1.get_speed_rad_s(), haltSpeed)) + _M1.runPID(0); + else + _M1.stop(); + if (this->shouldStop(_M2.get_speed_rad_s(), haltSpeed)) + _M2.runPID(0); + else + _M2.stop(); + if (this->shouldStop(_M3.get_speed_rad_s(), haltSpeed)) + _M3.runPID(0); + else + _M3.stop(); + if (this->shouldStop(_M4.get_speed_rad_s(), haltSpeed)) + _M4.runPID(0); + else + _M4.stop(); + + _motorSpeeds = _kinematics->clearSpeeds(); +} + +Motors MotionControl::convertWheelSpeed(double vx, double vy, double w) { + return _kinematics->convertToWheel(vx, vy, w); +} + +Motors MotionControl::convertWheelSpeed(Vector& desiredVector) { + return _kinematics->convertToWheel(desiredVector.x, desiredVector.y, desiredVector.w); +} + +void MotionControl::printMotorsSpeed() { + printf("v-M1: %.3lf | v-M2: %.3lf | v-M3: %.3lf | v-M4: %.3lf\n", + _M1.get_speed_rad_s(), + _M2.get_speed_rad_s(), + _M3.get_speed_rad_s(), + _M4.get_speed_rad_s()); +} + +void MotionControl::printDesiredSpeed(Vector motionSpeed) { + printf("vx: %.3lf | vy: %.3lf | w: %.3lf\n", motionSpeed.x, motionSpeed.y, motionSpeed.w); +} + +void MotionControl::printDebugPID(double desired) { + printf("v %.3lf M1 %.3lf M2 %.3lf M3 %.3lf M4 %.3lf\n", + desired, + _M1.get_speed_rad_s(), + _M2.get_speed_rad_s(), + _M3.get_speed_rad_s(), + _M4.get_speed_rad_s()); +} + +void MotionControl::printDebugPID(Vector desired, bool debug) { + Motors desiredSpeeds = _kinematics->convertToWheel(desired.x, desired.y, desired.w); + if (debug) { + printf( + "V1 %.3lf | V2 %.3lf | V3 %.3lf | V4 %.3lf || M1 %.3lf | M2 %.3lf | M3 %.3lf | M4 %.3lf\n", + desiredSpeeds.m1, + desiredSpeeds.m2, + desiredSpeeds.m3, + desiredSpeeds.m4, + _M1.get_speed_rad_s(), + _M2.get_speed_rad_s(), + _M3.get_speed_rad_s(), + _M4.get_speed_rad_s()); + } else { + printf("%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf,%.3lf\n", + desiredSpeeds.m1, + desiredSpeeds.m2, + desiredSpeeds.m3, + desiredSpeeds.m4, + _M1.get_speed_rad_s(), + _M2.get_speed_rad_s(), + _M3.get_speed_rad_s(), + _M4.get_speed_rad_s()); + } +} + +bool MotionControl::shouldStop(double motorSpeed, double haltSpeed) { + if (fabs(motorSpeed) > haltSpeed) + return true; + else + return false; +} + +void MotionControl::printDriblerSpeed() { + printf("%.3f\n", _M5.dribbler_get_speed()); +} + +void MotionControl::validateNavigation() { + Vector v_test; + v_test.x = 1; + _kinematics->validateSpeeds(v_test); +} + +void MotionControl::moveIsLocked(bool robotMoveIsLocked) { + _M1.motorIsLocked(robotMoveIsLocked); + _M2.motorIsLocked(robotMoveIsLocked); + _M3.motorIsLocked(robotMoveIsLocked); + _M4.motorIsLocked(robotMoveIsLocked); +} + +void MotionControl::printMotorsPWM() { + printf("M1 %.3lf M2 %.3lf M3 %.3lf M4 %.3lf\n", + _M1.get_current_pwm(), + _M2.get_current_pwm(), + _M3.get_current_pwm(), + _M4.get_current_pwm()); +} diff --git a/lib/MotionControl/MotionControl.h b/lib/MotionControl/MotionControl.h new file mode 100644 index 0000000..ed8dbe5 --- /dev/null +++ b/lib/MotionControl/MotionControl.h @@ -0,0 +1,76 @@ +#ifndef MOTION_CONTROL_H +#define MOTION_CONTROL_H + +#include "mbed.h" +#include +#include +#include +#include +#include + +#define HALT_SPEED 8 + +#ifdef USING_NEW_MOTOR + #define MOTOR_ENCODER_PULSES 2048 +#else + #define MOTOR_ENCODER_PULSES 1024 +#endif + +class MotionControl { + public: + MotionControl(Kinematics* kinematics); + + void init(); + + Motors moveRobot(Vector desiredVector); + Motors accelRobot(Vector desiredVector); + Motors accelRobot(Vector desiredVector, Vector currentPosition); + Motors slowRobot(); + void moveRobotPID(double m1, double m2, double m3, double m4); + void moveRobotPID(double speed); + void moveRobotPWM(double m1, double m2, double m3, double m4); + void moveRobotPWM(double speed); + void moveMotorPID(uint8_t idx, double speed); + void moveMotorPWM(uint8_t idx, double speed); + void updateDribbler(KickFlags& isKick); + void getMotorsInfo(RobotInfo& telemetry); + void getMotorsSpeed(Motors& m); + void getMotorsDesiredSpeed(Motors& m); + void getMotorsPWM(Motors& m); + double getMotorSpeed(uint8_t idx); + + void getRobotSpeed(Vector& v); + void getRobotSpeedOriginal(Vector& v); + void getRobotSpeed(Vector& v, bool useOptimizedParamaters); + + bool robotIsMoving(); + void stopRobot(); + void stopMotor(uint8_t idx); + void breakRobot(double haltSpeed = HALT_SPEED); + + Motors convertWheelSpeed(double vx, double vy, double w); + Motors convertWheelSpeed(Vector& desiredVector); + + void printMotorsSpeed(); + void printMotorsPWM(); + void printDebugPID(double desired); + void printDebugPID(Vector desired, bool debug = false); + void printDesiredSpeed(Vector motionSpeed); + void printDriblerSpeed(); + void validateNavigation(); + void moveIsLocked(bool robotMoveIsLocked); + + private: + PID_Controller _M1; + PID_Controller _M2; + PID_Controller _M3; + PID_Controller _M4; + PID_Controller _M5; + + Kinematics* _kinematics; + Motors _motorSpeeds; + + static bool shouldStop(double motorSpeed, double haltSpeed); +}; + +#endif // MOTION_CONTROL_H diff --git a/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.cpp b/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.cpp new file mode 100644 index 0000000..406108f --- /dev/null +++ b/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.cpp @@ -0,0 +1,80 @@ +#include "DriverBLDC.h" +#include "utils.h" + +// PWM - DIR - RST - MODE - COAST - BRK +double map(double x, double in_min, double in_max, double out_min, double out_max) { + double out = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + return out; +} + +DriverBLDC::DriverBLDC(PinName pwm, + PinName dir, + PinName rst, + PinName mode, + PinName coast, + PinName brk) : + _m(pwm), + _dir(dir), + _rst(rst), + _mode(mode), + _coast(coast), + _break(brk) { + _rst.write(0); // Reset Motor + _mode.write(1); // Low Decay (Tiggers we trust) + _coast.write(0); // Coast + _break.write(1); // ! Break + _m.period_us(PWM_PULSE_WIDTH_US); +} + +void DriverBLDC::init() { + _coast.write(0); + _rst.write(1); + _m.write(0); +} + +void DriverBLDC::reset() { + run(0); + _coast.write(1); + _rst.write(0); +} + +int DriverBLDC::getRst() { + return _rst.read(); +} + +double DriverBLDC::run(double speed) { + _coast.write(1); + double send_pwm; + if (_mode == 0) { + // PWM: 1 Counter clockwise <-> 0 Closkwise + _dir.write(0); + send_pwm = map(speed, -100, 100, 1.0, 0); + } + // default: mode==1 + else { + // Dir chooses side. + if (speed < 0) { +// invert motor direction if using direct transmission +#if defined(DIRECT_TRANSMISSION) + _dir.write(1); +#else + _dir.write(0); +#endif + speed *= -1; + } else { +// invert motor direction if using direct transmission +#if defined(DIRECT_TRANSMISSION) + _dir.write(0); +#else + _dir.write(1); +#endif + } + send_pwm = map(speed, 0, 100, 0, 1.0); + } + + _m.write(send_pwm); + return send_pwm; +} +void DriverBLDC::stop() { + _coast.write(0); +} diff --git a/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.h b/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.h new file mode 100644 index 0000000..5f02566 --- /dev/null +++ b/lib/MotionControl/PID_Controller/DriverBLDC/DriverBLDC.h @@ -0,0 +1,30 @@ +#include "mbed.h" +#include "FastPWM.h" + +#ifndef DriverlBLDC_H + #define DriverlBLDC_H + + #define PWM_FREQUENCY 50000 + #define PWM_PULSE_WIDTH (1.0 / PWM_FREQUENCY) + #define PWM_PULSE_WIDTH_US (PWM_PULSE_WIDTH * 1000000) + +class DriverBLDC { + public: + DriverBLDC(PinName pwm, PinName dir, PinName rst, PinName mode, PinName coast, PinName brk); + + void init(void); + double run(double speed); + void stop(void); + void reset(); + int getRst(); + + private: + FastPWM _m; + DigitalOut _dir; + DigitalOut _rst; + DigitalOut _mode; + DigitalOut _coast; + DigitalOut _break; +}; + +#endif \ No newline at end of file diff --git a/lib/MotionControl/PID_Controller/Encoder/Encoder.h b/lib/MotionControl/PID_Controller/Encoder/Encoder.h new file mode 100644 index 0000000..978c1e8 --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/Encoder.h @@ -0,0 +1,27 @@ +#ifndef Encoder_H +#define Encoder_H + +#include "mbed.h" +#include "utils.h" + +/** + * Quadrature Encoder Interface. + */ +class Encoder { + typedef enum Encoding { X2_ENCODING, X4_ENCODING } Encoding; + + public: + virtual void init(void); + + virtual void resetPulses(void); + + virtual int getPulses(void); + + virtual double getFrequency(void); + + virtual double getSpeed(); + + virtual void frequency(void); +}; + +#endif /* Encoder_H */ diff --git a/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.cpp b/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.cpp new file mode 100644 index 0000000..3dd6d3a --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.cpp @@ -0,0 +1,211 @@ +#include "QEI.h" +#include + +QEI::QEI(int pulsesPerRev, int tsample, TIM_TypeDef* timer) { + pulsesPerRev_ = pulsesPerRev; + tsample_ = tsample; + timer_ = timer; +} + +void QEI::init(void) { + // Configure Timers and Frequency callback + init_enc_timer(this->timer_); + this->resetPulses(); +} + +void QEI::frequency(void) { + // Motor spin frequency in Hz + frequency_ = double((this->getPulses() * (1000.0 / tsample_)) / (4.0 * pulsesPerRev_)); + this->resetPulses(); +} + +double QEI::getFrequency(void) { + return frequency_; +} + +double QEI::getSpeed() { + return 2.0 * M_PI * (MOTOR_GEAR / WHEEL_GEAR) * this->getFrequency(); +} + +void QEI::resetPulses(void) { + // Reset Timer and Pulses counter + this->timer_->CNT = PULSES_RESET; + pulses_ = 0; +} + +int QEI::getPulses(void) { + pulses_ = PULSES_RESET - this->timer_->CNT; + return pulses_; +} + +// Choosing Timer to Init. +void QEI::init_enc_timer(TIM_TypeDef* timer) { + if (timer == TIM1) + init_enc_timer1(); + else if (timer == TIM2) + init_enc_timer2(); + else if (timer == TIM8) + init_enc_timer8(); + else if (timer == TIM3) + init_enc_timer3(); +} + +void QEI::init_enc_timer1() // PE_9 PE_11 - Encoder of M1 +{ + // Enable registers manipulation for Timer1, GPIOE + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOEEN; + + // // GPIO Init for E ports 9 + GPIOE->MODER |= GPIO_MODER_MODER11_1 | GPIO_MODER_MODER9_1; // Alternate function mode MODERy: 10 + GPIOE->OTYPER |= GPIO_OTYPER_OT_9 | GPIO_OTYPER_OT_11; // Output open-drain OTy: 1 + GPIOE->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR9_0 | GPIO_OSPEEDER_OSPEEDR11_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOE->AFR[1] |= GPIO_AFRH_AFRH1_0 | GPIO_AFRH_AFRH3_0; // Alternate function 1 AFRy: 0001 + + TIM1->CR1 = 0; + TIM1->CR2 = 0; + TIM1->PSC = 0; + TIM1->ARR = 0xFFFF; + TIM1->RCR = 0; + // IC1 mapped to TI1, IC2 mapped to TI2, filter = 0x0F on both, no prescaler + TIM1->CCMR1 = TIM_CCMR1_CC1S_0 | (0x0F << 4) | TIM_CCMR1_CC2S_0 | (0x0F << 12); +// set TIM_CCER_CC1P for inverting polarity -> reads positive velocity for motor in clockwise +// direction +#if defined(DIRECT_TRANSMISSION) + TIM1->CCER = + TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC1P; // enable input capture, rising polarity +#else + TIM1->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; // enable input capture, rising polarity +#endif + TIM1->DIER = 0; + TIM1->SMCR = 3; // encoder mode 3, count all flanks + TIM1->CR1 = TIM_CR1_CEN; +} + +void QEI::init_enc_timer2() // PA_5 PB_3 - Encoder of M2 +{ + // Enable registers manipulation for Timer2, GPIOA GPIOB + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; + + // // GPIO Init for A ports 5 + GPIOA->MODER |= GPIO_MODER_MODER5_1; // Alternate function mode MODERy: 10 + GPIOA->OTYPER |= GPIO_OTYPER_OT_5; // Output open-drain OTy: 1 + GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR5_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOA->AFR[0] |= GPIO_AFRH_AFRH5_0; // Alternate function 1 AFRy: 0001 + + // printf("TIMER 2.A:\n - MODER: %X\n - OTYPER: %X\n - OSPEEDR: %X\n - AFR: %X\n", GPIOA->MODER, + // GPIOA->OTYPER, GPIOA->OSPEEDR, GPIOA->AFR[0]); + // // GPIO Init for B ports 3 + GPIOB->MODER |= GPIO_MODER_MODER3_1; // Alternate function mode MODERy: 10 + GPIOB->OTYPER |= GPIO_OTYPER_OT_3; // Output open-drain OTy: 1 + GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOB->AFR[0] |= GPIO_AFRH_AFRH3_0; // Alternate function 1 AFRy: 0001 + // printf("TIMER 2.B:\n - MODER: %X\n - OTYPER: %X\n - OSPEEDR: %X\n - AFR: %X\n", GPIOB->MODER, + // GPIOB->OTYPER, GPIOB->OSPEEDR, GPIOB->AFR[0]); + + TIM2->CR1 = 0; + TIM2->CR2 = 0; + TIM2->PSC = 0; + TIM2->ARR = 0xFFFF; + TIM2->RCR = 0; + // IC1 mapped to TI1, IC2 mapped to TI2, filter = 0x0F on both, no prescaler + TIM2->CCMR1 = TIM_CCMR1_CC1S_0 | (0x0F << 4) | TIM_CCMR1_CC2S_0 | (0x0F << 12); +// set TIM_CCER_CC1P for inverting polarity -> reads positive velocity for motor in clockwise +// direction +#if defined(DIRECT_TRANSMISSION) + TIM2->CCER = + TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC1P; // enable input capture, rising polarity +#else + TIM2->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; // enable input capture, rising polarity +#endif + TIM2->DIER = 0; + TIM2->SMCR = 3; // encoder mode 3, count all flanks + TIM2->CR1 = TIM_CR1_CEN; +} + +void QEI::init_enc_timer8() // PC_6 PC_7 - Encoder of M3 +{ + // Enable registers manipulation for Timer8 and GPIOC + RCC->APB2ENR |= RCC_APB2ENR_TIM8EN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; + + // // GPIO Init for C ports 6 and 7 + GPIOC->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1; // Alternate function mode MODERy: 10 + GPIOC->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7; // Output open-drain OTy: 1 + GPIOC->OSPEEDR |= + GPIO_OSPEEDER_OSPEEDR6_0 | GPIO_OSPEEDER_OSPEEDR7_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOC->AFR[0] |= GPIO_AFRH_AFRH6_1 | GPIO_AFRH_AFRH6_0 | GPIO_AFRH_AFRH7_1 | + GPIO_AFRH_AFRH7_0; // Alternate function 3 AFRy: 0011 + + // printf("TIMER 8:\n - MODER: %X\n - OTYPER: %X\n - OSPEEDR: %X\n - AFR: %X\n", GPIOC->MODER, + // GPIOC->OTYPER, GPIOC->OSPEEDR, GPIOC->AFR[0]); + TIM8->CR1 = 0; + TIM8->CR2 = 0; + TIM8->PSC = 0; + TIM8->ARR = 0xFFFF; + TIM8->RCR = 0; + // IC1 mapped to TI1, IC2 mapped to TI2, filter = 0x0F on both, no prescaler + TIM8->CCMR1 = TIM_CCMR1_CC1S_0 | (0x0F << 4) | TIM_CCMR1_CC2S_0 | (0x0F << 12); +// set TIM_CCER_CC1P for inverting polarity -> reads positive velocity for motor in clockwise +// direction +#if defined(DIRECT_TRANSMISSION) + TIM8->CCER = + TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC1P; // enable input capture, rising polarity +#else + TIM8->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; // enable input capture, rising polarity +#endif + TIM8->DIER = 0; + TIM8->SMCR = 3; // encoder mode 3, count all flanks + TIM8->CR1 = TIM_CR1_CEN; +} + +void QEI::init_enc_timer3() // PA_6 PB_5 - Encoder of M4 +{ + // Enable registers manipulation for Timer3 and GPIOA + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; + + // // GPIO Init for A port 6 + GPIOA->MODER |= GPIO_MODER_MODER6_1; // Alternate function mode MODERy: 10 + GPIOA->OTYPER |= GPIO_OTYPER_OT_6; // Output open-drain OTy: 1 + GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOA->AFR[0] |= GPIO_AFRH_AFRH6_1; // Alternate function 2 AFRy: 0010 + + // printf("TIMER 3.A:\n - MODER: %X\n - OTYPER: %X\n - OSPEEDR: %X\n - AFR: %X\n", GPIOA->MODER, + // GPIOA->OTYPER, GPIOA->OSPEEDR, GPIOA->AFR[0]); + // // GPIO Init for B port 5 + GPIOB->MODER |= GPIO_MODER_MODER5_1; // Alternate function mode MODERy: 10 + GPIOB->OTYPER |= GPIO_OTYPER_OT_5; // Output open-drain OTy: 1 + GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR5_0; // Medium Speed OSPEEDRy: 01 + // PUPDR - No pull-up, pull-down PUPDRy: 00 + GPIOB->AFR[0] |= GPIO_AFRH_AFRH5_1; // Alternate function 2 AFRy: 0010 + + // printf("TIMER 3.B:\n - MODER: %X\n - OTYPER: %X\n - OSPEEDR: %X\n - AFR: %X\n", GPIOB->MODER, + // GPIOB->OTYPER, GPIOB->OSPEEDR, GPIOB->AFR[0]); + TIM3->CR1 = 0; + TIM3->CR2 = 0; + TIM3->PSC = 0; + TIM3->ARR = 0xFFFF; + TIM3->RCR = 0; + // IC1 mapped to TI1, IC2 mapped to TI2, filter = 0x0F on both, no prescaler + TIM3->CCMR1 = TIM_CCMR1_CC1S_0 | (0x0F << 4) | TIM_CCMR1_CC2S_0 | (0x0F << 12); +// set TIM_CCER_CC1P for inverting polarity -> reads positive velocity for motor in clockwise +// direction +#if defined(DIRECT_TRANSMISSION) + TIM3->CCER = + TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC1P; // enable input capture, rising polarity +#else + TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; // enable input capture, rising polarity +#endif + TIM3->DIER = 0; + TIM3->SMCR = 3; // encoder mode 3, count all flanks + TIM3->CR1 = TIM_CR1_CEN; +} diff --git a/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.h b/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.h new file mode 100644 index 0000000..17bb092 --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/QEI/QEI.h @@ -0,0 +1,77 @@ +/* + * Quadrature Encoder Interface. + + * This interface can also use X4 encoding which calculates the pulse count + * based on reading the current state after each rising and falling edge of + * either channel. + * + * +-----+ +-----+ +-----+ + * Channel A | | | | | | + * ---+ +-----+ +-----+ +----- + * ^ ^ ^ ^ ^ + * ^ +-----+ ^ +-----+ ^ +-----+ + * Channel B ^ | ^ | ^ | ^ | ^ | | + * ------+ ^ +-----+ ^ +-----+ +-- + * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + */ + +#ifndef QEI_H +#define QEI_H + +#include "Encoder.h" + +#define PULSES_RESET 32767 + +/** + * Quadrature Encoder Interface. + */ +class QEI : public Encoder { + + public: + QEI(int pulsesPerRev, int tsample, TIM_TypeDef* timer); + + /** + * Init the encoder. + */ + void init(void); + + /** + * Reset the encoder: sets the pulses and revolutions count to zero. + */ + void resetPulses(void); + + /** + * @return Number of pulses which have occured in tSample. + */ + int getPulses(void); + + /** + * @return Frequency in Hz. + */ + double getFrequency(void); + + /** + * @return Wheel speed. + */ + double getSpeed(); + + /* Update the rotation frequecy count */ + void frequency(void); + + private: + static void init_enc_timer(TIM_TypeDef* timer); + static void init_enc_timer1(); // PE_9 PA_9 - Encoder of M1 + static void init_enc_timer2(); // PA_5 PB_3 - Encoder of M2 + static void init_enc_timer8(); // PC_6 PC_7 - Encoder of M3 + static void init_enc_timer3(); // PA_6 PB_5 - Encoder of M4 + + TIM_TypeDef* timer_; + int pulsesPerRev_; + + volatile int pulses_; + volatile double frequency_; + int tsample_; +}; + +#endif /* QEI_H */ diff --git a/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.cpp b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.cpp new file mode 100644 index 0000000..487d12b --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.cpp @@ -0,0 +1,120 @@ +#include "QEInterruption.h" + +#include "mbed.h" +#include "utils.h" + +QEInterruption::QEInterruption(PinName channelA, + PinName channelB, + int pulsesPerRev, + int tsample, + Encoding encoding) : + channelA_(channelA), + channelB_(channelB) { + pulses_ = 0; + pulsesPerRev_ = pulsesPerRev; + encoding_ = encoding; + tsample_ = tsample; +} + +void QEInterruption::init(void) { + // Workout what the current state is. + int chanA = channelA_.read(); + int chanB = channelB_.read(); + + // 2-bit state. + currState_ = (chanA << 1) | (chanB); + prevState_ = currState_; + + // X1 encoders uses rise interrupts on channel A, and X2 or tacho/diro rise and fall. + channelA_.rise(callback(this, &QEInterruption::encode)); + if (encoding_ != X1_ENCODING) + channelA_.fall(callback(this, &QEInterruption::encode)); + + /* If we're using X4 encoding, then attach interrupts to channel B. + * Remember of setting channelB_ as Interrupt Pin (on .h). + */ + // if (encoding_ == X4_ENCODING) + // { + // channelB_.rise(callback(this, &QEInterruption::encode)); + // channelB_.fall(callback(this, &QEInterruption::encode)); + // } +} + +void QEInterruption::frequency(void) { + if (encoding_ == TAC_DIRO_ENCODING) { + frequency_ = double((this->getPulses() * (1.0 / tsample_)) / (1.0 * pulsesPerRev_)); + } else if (encoding_ == X1_ENCODING) { + frequency_ = double((this->getPulses() * (1.0 / tsample_)) / (1.0 * pulsesPerRev_)); + } else if (encoding_ == X2_ENCODING) { + frequency_ = double((this->getPulses() * (1.0 / tsample_)) / (2.0 * pulsesPerRev_)); + } + // else if (encoding_ == X4_ENCODING) + // frequency_ = double((this->getPulses() * (1.0 / tsample_)) / (4.0 * pulsesPerRev_)); + // utils::pc.printf("Pulses %d\n", this->getPulses()); + this->resetPulses(); +} + +double QEInterruption::getFrequency(void) { + return frequency_; +} + +double QEInterruption::getSpeed() { + return 2.0 * M_PI * (DRIBLER_BAR_GEAR / DRIBLER_GEAR) * this->getFrequency(); +} + +void QEInterruption::resetPulses(void) { + pulses_ = 0; +} + +int QEInterruption::getCurrentState(void) { + + return currState_; +} + +int QEInterruption::getPulses(void) { + return pulses_; +} + +void QEInterruption::encode(void) { + // int change = 0; + int chanA = channelA_.read(); + int chanB = channelB_.read(); + currState_ = (chanA << 1) | (chanB); + + if (encoding_ == TAC_DIRO_ENCODING) { + if (chanB) + pulses_++; // 11->01->11->01 is counter clockwise rotation or "forward" + else + pulses_--; // 10->00->10->00 is clockwise rotation or "backward" + } else if (encoding_ == X1_ENCODING) { + if (chanB) + pulses_--; + else + pulses_++; + } else if (encoding_ == X2_ENCODING) { + // 11->00->11->00 is counter clockwise rotation or "forward". + if ((prevState_ == 0x3 && currState_ == 0x0) || (prevState_ == 0x0 && currState_ == 0x3)) { + pulses_++; + } + // 10->01->10->01 is clockwise rotation or "backward". + else if ((prevState_ == 0x2 && currState_ == 0x1) || (prevState_ == 0x1 && currState_ == 0x2)) { + pulses_--; + } + } + + // else if (encoding_ == X4_ENCODING) { + // //Entered a new valid state. + // if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { + // //2 bit state. Right hand bit of prev XOR left hand bit of current + // //gives 0 if clockwise rotation and 1 if counter clockwise rotation. + // change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); + // if (change == 0) { + // change = -1; + // } + // pulses_ -= change; + // } + // } + + prevState_ = currState_; + return; +} diff --git a/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.h b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.h new file mode 100644 index 0000000..c36772f --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/QEInterruption.h @@ -0,0 +1,88 @@ + +// --- TODO: Test it at the Dribbler Motor !!!!! ---- + +#ifndef QEInterruption_H +#define QEInterruption_H + +#include "Encoder.h" + +/* Defines */ +#define PREV_MASK 0x1 // Mask for the previous state in determining direction +#define CURR_MASK 0x2 // Mask for the current state in determining direction +#define INVALID 0x3 // XORing two states where both bits have changed. + +/* Quadrature Encoder Interruption */ +class QEInterruption : public Encoder { + + public: + typedef enum Encoding { TAC_DIRO_ENCODING, X1_ENCODING, X2_ENCODING, X4_ENCODING } Encoding; + + /** + * Reads the current values on channel A and channel B to determine the initial state. + * + * Attaches the encode function to the rise/fall interrupt edges of channels A and B. + * + * Attaches the frequenci callback function. + * + * @param channelA mbed pin for channel A input. + * @param channelB mbed pin for channel B input. + * @param pulsesPerRev Number of pulses in one revolution. + * @param encoding The encoding to use. + */ + + QEInterruption(PinName channelA, + PinName channelB, + int pulsesPerRev, + int tsample, + Encoding encoding = TAC_DIRO_ENCODING); + + void init(void); + + /* Reset the encoder: Sets the pulses and revolutions count to zero. */ + void resetPulses(void); + + /* Read the state of the encoder. + * + * @return The current state of the encoder as a 2-bit number, where: + * bit 1 = The reading from channel B + * bit 2 = The reading from channel A */ + int getCurrentState(void); + + /* Read the number of pulses recorded by the encoder. + * @return Number of pulses which have occured. + */ + int getPulses(void); + + /* @return pre-calculated Frequency in Hz. */ + double getFrequency(void); + + /* @return Speed of wheel in rad/s by calculated frequency. */ + double getSpeed(); + + /* @return Calculates the wheel frequency based on encoded pulses. */ + void frequency(void); + + private: + /* Called on every rising/falling edge of channels A/B. + * + * Determines whether a pulse forward or backward has occured. + */ + void encode(void); + + Encoding encoding_; + + Ticker freqTicker_; + + InterruptIn channelA_; + DigitalIn channelB_; + + int pulsesPerRev_; + int prevState_; + int currState_; + + volatile int pulses_; + volatile double frequency_; + int tsample_; +}; + +#endif /* QEI_H */ diff --git a/lib/MotionControl/PID_Controller/Encoder/QEInterruption/readme.md b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/readme.md new file mode 100644 index 0000000..bad4022 --- /dev/null +++ b/lib/MotionControl/PID_Controller/Encoder/QEInterruption/readme.md @@ -0,0 +1,158 @@ + + +#Quadrature Encoder Interface. + + A quadrature encoder consists of two code tracks on a disc which are 90 + degrees out of phase. It can be used to determine how far a wheel has + rotated, relative to a known starting position. + + Only one code track changes at a time leading to a more robust system than + a single track, because any jitter around any edge won't cause a state + change as the other track will remain constant. + + Encoders can be a homebrew affair, consisting of infrared emitters/receivers + and paper code tracks consisting of alternating black and white sections; + alternatively, complete disk and PCB emitter/receiver encoder systems can + be bought, but the interface, regardless of implementation is the same. + + +-----+ +-----+ +-----+ + Channel A | ^ | | | | | + ---+ ^ +-----+ +-----+ +----- + ^ ^ + ^ +-----+ +-----+ +-----+ + Channel B ^ | | | | | | + ------+ +-----+ +-----+ +----- + ^ ^ + ^ ^ + 90deg + + The interface uses X2 encoding by default which calculates the pulse count + based on reading the current state after each rising and falling edge of + channel A. + + +-----+ +-----+ +-----+ + Channel A | | | | | | + ---+ +-----+ +-----+ +----- + ^ ^ ^ ^ ^ + ^ +-----+ ^ +-----+ ^ +-----+ + Channel B ^ | ^ | ^ | ^ | ^ | | + ------+ ^ +-----+ ^ +-----+ +-- + ^ ^ ^ ^ ^ + ^ ^ ^ ^ ^ + Pulse count 0 1 2 3 4 5 ... + + This interface can also use X4 encoding which calculates the pulse count + based on reading the current state after each rising and falling edge of + either channel. + + +-----+ +-----+ +-----+ + Channel A | | | | | | + ---+ +-----+ +-----+ +----- + ^ ^ ^ ^ ^ + ^ +-----+ ^ +-----+ ^ +-----+ + Channel B ^ | ^ | ^ | ^ | ^ | | + ------+ ^ +-----+ ^ +-----+ +-- + ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + Pulse count 0 1 2 3 4 5 6 7 8 9 ... + + It defaults + + An optional index channel can be used which determines when a full + revolution has occured. + + If a 4 pules per revolution encoder was used, with X4 encoding, + the following would be observed. + + +-----+ +-----+ +-----+ + Channel A | | | | | | + ---+ +-----+ +-----+ +----- + ^ ^ ^ ^ ^ + ^ +-----+ ^ +-----+ ^ +-----+ + Channel B ^ | ^ | ^ | ^ | ^ | | + ------+ ^ +-----+ ^ +-----+ +-- + ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + ^ ^ ^ +--+ ^ ^ +--+ ^ + ^ ^ ^ | | ^ ^ | | ^ + Index ------------+ +--------+ +----------- + ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ + Pulse count 0 1 2 3 4 5 6 7 8 9 ... + Rev. count 0 1 2 + + + Linear position can be calculated by: + + (pulse count / X N) (1 / PPI) + + Where X is encoding type [e.g. X4 encoding => X=44], N is the number of + pulses per revolution, and PPI is pulses per inch, or the equivalent for + any other unit of displacement. PPI can be calculated by taking the + circumference of the wheel or encoder disk and dividing it by the number + of pulses per revolution. + +#How it Works + +-------------+ +| TAC_DIRO Encoding | ++-------------+ + +Counter clockwise rotation: + +11 -> 01 -> 11 -> 01 -> ... + +Clockwise rotation: + +10 -> 00 -> 10 -> 00 ->... + + +-------------+ +| X1 Encoding | ++-------------+ + +Counter clockwise rotation: + +10 -> 10 -> ... + +Clockwise rotation: + +11 -> 11 ->... + + +-------------+ +| X2 Encoding | ++-------------+ + +When observing states two patterns will appear: + +Counter clockwise rotation: + +10 -> 01 -> 10 -> 01 -> ... + +Clockwise rotation: + +11 -> 00 -> 11 -> 00 -> ... + +We consider counter clockwise rotation to be "forward" and +counter clockwise to be "backward". Therefore pulse count will increase +during counter clockwise rotation and decrease during clockwise rotation. + ++-------------+ +| X4 Encoding | ++-------------+ + +There are four possible states for a quadrature encoder which correspond to +2-bit gray code. + +A state change is only valid if of only one bit has changed. +A state change is invalid if both bits have changed. + +Clockwise Rotation -> + + 00 01 11 10 00 + +<- Counter Clockwise Rotation + +If we observe any valid state changes going from left to right, we have +moved one pulse clockwise [we will consider this "backward" or "negative"]. + +If we observe any valid state changes going from right to left we have +moved one pulse counter clockwise [we will consider this "forward" or +"positive"]. diff --git a/lib/MotionControl/PID_Controller/PID_Controller.cpp b/lib/MotionControl/PID_Controller/PID_Controller.cpp new file mode 100644 index 0000000..0f3d460 --- /dev/null +++ b/lib/MotionControl/PID_Controller/PID_Controller.cpp @@ -0,0 +1,279 @@ + +#include "PID_Controller.h" + +PID_Controller::PID_Controller(PinName pwm, + PinName dir, + PinName rst, + PinName mode, + PinName cst, + PinName brk, + TIM_TypeDef* timer, + int pulsesPerRev, + int tsample, + MotorType M_TYPE) : + motor(pwm, dir, rst, mode, cst, brk), + last_error(MAX_ERROR_SIZE, 0) { + enc = new QEI(pulsesPerRev, tsample, timer); + pin_pwm = pwm; + _tsample = tsample; + if (M_TYPE == DRIBBLER) + dribblerConfig(); +} + +PID_Controller::PID_Controller(PinName pwm, + PinName dir, + PinName rst, + PinName mode, + PinName cst, + PinName brk, + PinName channelA, + PinName channelB, + int pulsesPerRev, + int tsample, + MotorType M_TYPE) : + motor(pwm, dir, rst, mode, cst, brk), + last_error(MAX_ERROR_SIZE, 0) { + enc = new QEInterruption(channelA, channelB, pulsesPerRev, tsample); + pin_pwm = pwm; + _tsample = tsample; + if (M_TYPE == DRIBBLER) + dribblerConfig(); +} + +void PID_Controller::init() { + motor.init(); + enc->init(); + _controlTicker.attach(callback(this, &PID_Controller::update), chrono::milliseconds(_tsample)); + _controlTimeout.start(); +} + +void PID_Controller::reset() { + _desired_rad_s = 0; + _stopControl = true; + motor.reset(); +} + +void PID_Controller::stop() { + _desired_rad_s = 0; + _stopControl = true; + motor_timer.reset(); + motor.stop(); +} + +// Callback that updates the encoder frequency and PID control. +void PID_Controller::update() { + enc->frequency(); + if (_stopControl || + (!motor_error && utils::timerMillisExpired(_controlTimeout, NRF_NO_MSG_TIME))) { + stop(); + } else { + _pwm = runPID(_desired_rad_s); + } +} + +void PID_Controller::updateDesiredSpeed(double desired_rad_s) { + if (!motor_error) { + _stopControl = false; + _desired_rad_s = desired_rad_s; + _controlTimeout.reset(); + debug_if(DEBUG, + "PID_Controller::updateDesiredSpeed: %lf, Flag: %d, Timeout: %lf\n", + _desired_rad_s, + _stopControl, + utils::timerMillisRead(_controlTimeout)); + } else { + _desired_rad_s = 0; + if (motor_error_armed_warning) { + utils::beep(300); + motor_error_armed_warning = false; + } + } +} + +double PID_Controller::runPID(double desired_rad_s, MotorType M_TYPE) { + if (fabs(desired_rad_s) < MOTOR_MIN_SPEED_RAD_S && desired_rad_s != 0) { + desired_rad_s = 0; + } + // Updates de Encoder + double pwm; + if (M_TYPE == DIRECTION) + pwm = adjust_rad_s_to_pwm_pi(desired_rad_s); // Comment to use OLD PID + else + pwm = dribbler_adjust_rad_s_to_pwm(desired_rad_s); + + is_motor_running(desired_rad_s); + if (!motor_error) { + motor.run(pwm); + } else { + if (pin_pwm == M1_PWM) { + Status::send(FAIL, MOTOR_1_LED); + } else if (pin_pwm == M2_PWM) { + Status::send(FAIL, MOTOR_2_LED); + } else if (pin_pwm == M3_PWM) { + Status::send(FAIL, MOTOR_3_LED); + } else if (pin_pwm == M4_PWM) { + Status::send(FAIL, MOTOR_4_LED); + } else if (pin_pwm == M5_PWM) { + Status::send(FAIL, DRIBBLER_LED); + } + motor.stop(); + } + return pwm; +} + +void PID_Controller::runPWM(double desired_pwm) { + _pwm = desired_pwm; + motor.run(desired_pwm); + _speed_rad_s = enc->getSpeed(); +} + +double PID_Controller::adjust_rad_s_to_pwm(double desired_rad_s) { + _speed_rad_s = enc->getSpeed(); + + if ((alpha > 0) && (fabs(desired_rad_s - _speed_rad_s) > accel_diff_speed)) { + desired_rad_s = (alpha * desired_rad_s) + ((1 - alpha) * _speed_rad_s); + } + + double adjust_rad_s = pid(desired_rad_s, _speed_rad_s); + + double adjust_pwm = + utils::rad_s_to_pwm(fmin(fmax(adjust_rad_s, -max_adjust_speed), max_adjust_speed)); + double desired_pwm = utils::rad_s_to_pwm(desired_rad_s); + + return desired_pwm + adjust_pwm; +} + +double PID_Controller::adjust_rad_s_to_pwm_pi(double desired_rad_s) { + _speed_rad_s = enc->getSpeed(); + double adjust_pwm; + adjust_pwm = pi(desired_rad_s, _speed_rad_s); + + double MAX_PWM_ALLOWED = (motor_is_locked) ? LOCKED_MAX_PWM_ALLOWED : STD_MAX_PWM_ALLOWED; + + adjust_pwm = std::min(adjust_pwm, MAX_PWM_ALLOWED); + adjust_pwm = std::max(adjust_pwm, -MAX_PWM_ALLOWED); + + return adjust_pwm; +} + +double PID_Controller::dribbler_adjust_rad_s_to_pwm(double desired_rad_s) { + + _speed_rad_s = dribbler_get_speed(); + + if ((alpha > 0) && (fabs(desired_rad_s - _speed_rad_s) > accel_diff_speed)) { + desired_rad_s = (alpha * desired_rad_s) + ((1 - alpha) * _speed_rad_s); + } + + double adjust_rad_s = pid(desired_rad_s, _speed_rad_s); + double adjusted_rad_s = + desired_rad_s + fmin(fmax(adjust_rad_s, -max_adjust_speed), max_adjust_speed); + + return 0; +} + +double PID_Controller::pid(double desired, double measured) { + double error = desired - measured; + + double proportional = Kp * error; + double derivative = Kd * (error - last_error.back()); + + last_error.push_back(error); + if (last_error.size() > MAX_ERROR_SIZE) { + last_error.erase(last_error.begin()); + } + + double sum_of_errors = std::accumulate(last_error.begin(), last_error.end(), 0); + double integrative = Ki * sum_of_errors; + + return proportional + integrative + derivative; +} + +double PID_Controller::pi(double desired, double measured) { + double error = desired - measured; // erro em rad/s + + if(motor_error) { + errSum = 0; + } + + P = Kp * error; + I = Ki * errSum; + D = Kd * (error - derivative_last_error); + derivative_last_error = error; + + double pwm = P + I + D; + + errSum += error; + errSum = std::min(errSum, MAX_I); + errSum = std::max(errSum, -MAX_I); + + return pwm; +} + +void PID_Controller::is_motor_running(double desired_rad_s) { + if ((fabs(desired_rad_s) >= MOTOR_MIN_SPEED_RAD_S) && + (fabs(_speed_rad_s) <= MOTOR_STOP_SPEED_RAD_S)) { + if (!motor_error) // TIMER NOT STARTED + { + motor_timer.start(); + } else { + if (utils::timerRead(motor_timer) > 5 * MOTOR_TIMEOUT && + motor_error_rst < 5) { + motor_timer.reset(); + motor_timer.stop(); + motor_error_rst++; + motor_error = false; + } + } + } else { + motor_error = false; + motor_timer.stop(); + motor_timer.reset(); + } + + if (utils::timerRead(motor_timer) > MOTOR_TIMEOUT) { + motor_error = true; + motor_error_armed_warning = true; + } + return; +} + +double PID_Controller::get_speed_pwm() { + _speed_rad_s = enc->getSpeed(); + return utils::rad_s_to_pwm(_speed_rad_s); +} + +double PID_Controller::get_speed_rad_s() { + _speed_rad_s = enc->getSpeed(); + return _speed_rad_s; +} + +double PID_Controller::get_desired_speed_rad_s() { + return _desired_rad_s; +} + +double PID_Controller::get_current_pwm() { + return _pwm; +} + +bool PID_Controller::get_motor_error() { + return motor_error; +} + +double PID_Controller::dribbler_get_speed() { + return enc->getSpeed(); +} + +void PID_Controller::dribblerConfig() { + + accel_diff_speed = M5_accel_diff_speed; + max_adjust_speed = M5_max_adjust_speed; + + alpha = M5_alpha; + Kp = M5_Kp; + Ki = M5_Ki; + Kd = M5_Kd; +} + +void PID_Controller::motorIsLocked(bool robotMoveIsLocked) { + motor_is_locked = robotMoveIsLocked; +} diff --git a/lib/MotionControl/PID_Controller/PID_Controller.h b/lib/MotionControl/PID_Controller/PID_Controller.h new file mode 100644 index 0000000..13f01cf --- /dev/null +++ b/lib/MotionControl/PID_Controller/PID_Controller.h @@ -0,0 +1,139 @@ +#ifndef PID_CONTROLLER +#define PID_CONTROLLER + +#include "mbed.h" +#include "Encoder/Encoder.h" +#include "Encoder/QEI/QEI.h" +#include "Encoder/QEInterruption/QEInterruption.h" +#include "DriverBLDC/DriverBLDC.h" +#include "utils.h" +#include +#include +#include + +#define MAX_ERROR_SIZE 2 // Number of PID errors accumulation. + +enum MotorType { DRIBBLER, DIRECTION }; + +class PID_Controller { + public: + /** Using motor with Timer Encoder + * @param tsample Encoder sampling time, between speeds calculations + */ + PID_Controller(PinName pwm, + PinName dir, + PinName rst, + PinName mode, + PinName cst, + PinName brk, + TIM_TypeDef* timer, + int pulsesPerRev, + int tsample, + MotorType M_TYPE = DIRECTION); + + /** Using motor with Interruption Encoder + * @param tsample Encoder sampling time, between speeds calculations + */ + PID_Controller(PinName pwm, + PinName dir, + PinName rst, + PinName mode, + PinName cst, + PinName brk, + PinName channelA, + PinName channelB, + int pulsesPerRev, + int tsample, + MotorType M_TYPE = DIRECTION); + + void init(); + void stop(); + void reset(); + void runPWM(double desired_pwm); + void updateDesiredSpeed(double desired_rad_s); + void motorIsLocked(bool robotMoveIsLocked); + double runPID(double desired_rad_s, MotorType M_TYPE = DIRECTION); + double runPID_PWM(double desired_pwm); + int getMotorRst(); + + // Calculate the necessary PWM given an desired speed. + double + adjust_pwm_to_pwm(double desired_pwm); // Speed in PWM (mapped from no loaded motor direction) + double adjust_rad_s_to_pwm(double desired_rad_s); // Speed in rad/s + double dribbler_adjust_rad_s_to_pwm( + double desired_rad_s); // Speed in PWM (mapped from no loaded motor dribbler) + double adjust_rad_s_to_pwm_pi(double desired_rad_s); // Speed in PWM (from PI controller) + + double get_speed_pwm(); // Wheel speed in rad/s converted to PWM (mapped from no loaded motor) + double get_speed_rad_s(); // Wheel speed in rad/s + double get_desired_speed_rad_s(); + double get_current_pwm(); + double get_motion_rad(); + bool get_motor_error(); + + // Motor encoder by interruption + double dribbler_get_speed(); // current mensured Bar Dribbler speed in rad/s + + private: + void update(); // Update Encoder reads and PID control + void is_motor_running(double desired_rad_s); // Checks if motor is properly running + double pid(double desired, double measured); // With desired and measured outputs control signal + double pi(double desired, double measured); // With desired and measured outputs control signal + void dribblerConfig(); // configurate constants for dribbler motor + + Ticker _controlTicker; + Timer _controlTimeout; + bool _stopControl = true; + int _tsample; + Encoder* enc; + DriverBLDC motor; + std::vector last_error; + + PinName pin_pwm; + + double _pwm; + double _speed_rad_s = 0; + double _desired_rad_s = 0; + + double alpha = 0.2; + + // For control direction motor + int accel_diff_speed = 25; + int max_adjust_speed = 40; + + double P = 0; + double errSum = 0; + double I = 0; + const double MAX_I = STD_MAX_PWM_ALLOWED / Ki; // (170/Ki) + double derivative_last_error = 0; + double D = 0; + +#ifdef USING_NEW_MOTOR + double Kp = 1.157; // PID Control constants + double Ki = 0.0549; // PID Control constants + double Kd = 0; // PID Control constants +#else + double Kp = 1.391; // PID Control constants + double Ki = 0.0296; // PID Control constants + double Kd = 0; // PID Control constants +#endif + + // For control dribbler motor + const int M5_accel_diff_speed = 25; + const int M5_max_adjust_speed = 150; + + double M5_alpha = 0.25; + double M5_Kp = 2.0; + double M5_Ki = 0; + double M5_Kd = 0.02; + + Timer motor_timer; + const int MOTOR_STOP_SPEED_RAD_S = 1; + const float MOTOR_TIMEOUT = 1000.0; + long motor_error_rst = 0; + bool motor_error = false; + bool motor_error_armed_warning = false; + bool motor_is_locked = false; +}; + +#endif /* PID_CONTROLLER */ diff --git a/lib/Navigation/Navigation.cpp b/lib/Navigation/Navigation.cpp new file mode 100644 index 0000000..0959b89 --- /dev/null +++ b/lib/Navigation/Navigation.cpp @@ -0,0 +1,199 @@ + +#include "Navigation.h" + +Navigation::Navigation(Kinematics* kinematics, MotionControl* motion, Odometry* odometry) { + _kinematics = kinematics; + _motion = motion; + _odometry = odometry; +} + +void Navigation::update(Vector& motionSpeed, Vector radioSpeed) { + motionSpeed = radioSpeed; +} + +void Navigation::update(RobotPosition lastPositionPacket) { + // updates source position or target position + if (lastPositionPacket.type == PositionType::source) { + _odometry->updatePosition(lastPositionPacket.v); + } else { + this->_target = lastPositionPacket; + } +} + +void Navigation::move(Vector& motionSpeed) { + // updates desired speed based on current position, target position and trajectory type + if (this->_target.type == PositionType::motionControl) { + this->motionControlOld(motionSpeed, _odometry->getCurrentPosition()); + } else if (this->_target.type == PositionType::rotateInPoint) { + this->rotateInPoint(motionSpeed, _odometry->getCurrentPosition()); + } else if (this->_target.type == PositionType::rotateControl) { + this->rotateControl(motionSpeed, _odometry->getCurrentPosition()); + } +} + +void Navigation::rotateControl(Vector& speed, Vector robot, float targetAngle, float angleKp) { + float dtheta = math::smallestAngleDiff(targetAngle, robot.w); + float kp = angleKp > 0.01f ? angleKp : ANGLE_KP; + + speed.x = 0.0; + speed.y = 0.0; + speed.w = (kp * dtheta); + return; +} + +void Navigation::rotateControl(Vector& speed, Vector robot) { + return this->rotateControl(speed, robot, this->_target.v.w, ANGLE_KP); +} + +void Navigation::motionControlOld(Vector& speed, + Vector robot, + Vector target, + float maxSpeed, + float minSpeed) { + float dist = math::distance(robot, target); + + // If Player send max speed, this max speed has to be respected, otherwise use + // 1 m/s. + maxSpeed = maxSpeed > 0.001 ? maxSpeed : 1; + + maxSpeed = math::map(dist, 0, 1.5, minSpeed, maxSpeed); + + if ((math::distance(robot, target) > 0.05)) { + // Uses an angle Proportional,to get the right angle using only angular + // speed. and then use linear speed to get into the point. + float dx = target.x - robot.x; + float dy = target.y - robot.y; + + float theta_v = atan2(dy, dx); + + float dtheta = math::smallestAngleDiff(target.w, robot.w); + + // Proportional to prioritize the angle correction + float v_prop = fabs(math::smallestAngleDiff(dtheta, M_PI - 0.1)) * (maxSpeed / (M_PI - 0.1)); + + // Global speed + Vector global(v_prop * cos(theta_v), v_prop * sin(theta_v), (ANGLE_KP * dtheta)); + + // Rotate to robot speed. + speed = _kinematics->rotateToLocal(robot.w, global); + } else { + return this->rotateControl(speed, robot, target.w, ANGLE_KP); + } + + return; +} + +void Navigation::motionControlOld(Vector& speed, Vector robot) { + RobotPosition pos = this->_target; + this->motionControlOld(speed, robot, pos.v, pos.maxSpeed, pos.minSpeed); +} + +void Navigation::rotateInPoint(Vector& speed, + Vector robot, + Vector target, + float maxSpeed, + float propSpeedFactor, + bool rotateInClockWise, + float orbitRadius, + float angleKp, + float approachKp) { + + float robotRadius = math::distance(robot, target); + float dtheta = math::smallestAngleDiff(target.w, robot.w); + float kp = angleKp > 0.01f ? angleKp : ANGLE_KP; + // Rotate in the smaller angle + float clockSignal = rotateInClockWise ? 1 : -1; + float velocity = maxSpeed; + velocity *= propSpeedFactor; + + speed.x = approachKp * (robotRadius - orbitRadius); + speed.y = clockSignal * velocity; + speed.w = (-(clockSignal * velocity) / (orbitRadius)) + (kp * dtheta); + + return; +} + +void Navigation::rotateInPoint(Vector& speed, + Vector robot, + Vector target, + float maxSpeed, + float minSpeed) { + + // vetor (robô => ponto de rotação) + Vector diffVector = target - robot; + diffVector.w = atan2(diffVector.y, diffVector.x); + float R = math::distance(target, robot); + + // erro de orientação: + float alpha = math::smallestAngleDiff(diffVector.w, robot.w); + // quanto falta girar em torno do ponto de referência: + float dtheta = math::smallestAngleDiff(target.w, diffVector.w); + + if (fabs(dtheta + alpha) < MIN_ROTATE_ANGLE * M_PI / 180 && fabs(R - INNER_ORBIT_RADIUS) < 0.03 && + fabs(alpha) < 2 * MIN_ROTATE_ANGLE * M_PI / 180) { + speed = Vector(0, 0, 0); + return; + } + + // Reduce maxSpeed to prioritize the angle correction + float v_prop = fabs(math::smallestAngleDiff(alpha, M_PI - 0.0001)) / (M_PI - 0.0001); + maxSpeed = v_prop * maxSpeed; + + // proportional desired radius: + float desired_radius; + desired_radius = math::map(fabs(dtheta), + 0, + TRASITION_ROTATE_ANGLE * M_PI / 180, + INNER_ORBIT_RADIUS, + OUTER_ORBIT_RADIUS); + + // radial velocity proportional to difference between current and desired radius: + float Rdiff = desired_radius - R; + float v_r = math::map(fabs(Rdiff), 0, maxSpeed / 2, 0, maxSpeed); + if (Rdiff > 0) { + v_r = -v_r; + } + + // tangential velocity proportional to current radius and limits resulting translational speed to + // maxSpeed: + float vy_prop = 1 - math::map(fabs(Rdiff), 0, (OUTER_ORBIT_RADIUS - INNER_ORBIT_RADIUS), 0, 1); + float vy_max2 = maxSpeed * maxSpeed - v_r * v_r; + float vy_max = vy_max2 > 0.001 ? sqrt(vy_max2) : 0; + + // vy mapped according to dtheta: + float v_tan = vy_prop * math::map(fabs(dtheta), 0, M_PI, 0, vy_max); + if (dtheta > 0) { + v_tan = -v_tan; + } + + // w for rotating around reference point and compensating orientation error: + float w = -v_tan / R + ANGLE_KP * alpha; + + // project v_r and v_tan on correct directions: + float local_vx = v_r * cos(alpha) - v_tan * sin(alpha); + float local_vy = v_r * sin(alpha) + v_tan * cos(alpha); + + // rotate to robot orientation: + Vector local(local_vx, local_vy, w); + speed = local; +} + +void Navigation::rotateInPoint(Vector& speed, Vector robot) { + RobotPosition pos = this->_target; + this->rotateInPoint(speed, robot, pos.v, pos.maxSpeed, pos.minSpeed); +} + +void Navigation::printTarget() { + RobotPosition pos = this->_target; + printf("Type: %d | X: %lf, Y: %lf, W: %lf | max: %lf, min %lf\n", + pos.type, + pos.v.x, + pos.v.y, + pos.v.w, + pos.maxSpeed, + pos.minSpeed); +} + +void Navigation::printVelocity(Vector speed) { + printf("Vx: %lf, Vy: %lf, w: %lf\n", speed.x, speed.y, speed.w); +} diff --git a/lib/Navigation/Navigation.h b/lib/Navigation/Navigation.h new file mode 100644 index 0000000..e1c1a84 --- /dev/null +++ b/lib/Navigation/Navigation.h @@ -0,0 +1,46 @@ +#ifndef NAVIGATION_H +#define NAVIGATION_H +#define MIN_ROTATE_ANGLE 5 // degrees +#define TRASITION_ROTATE_ANGLE 180 // degrees (ideal: 180º) +#define OUTER_ORBIT_RADIUS 0.160 // meters +#define INNER_ORBIT_RADIUS 0.090 // meters (ideal: 90mm) + +#include "mbed.h" +#include +#include +#include +#include + +class Navigation { + public: + Navigation(Kinematics* kinematics, MotionControl* motion, Odometry* odometry); + + void update(Vector& motionSpeed, Vector radioSpeed); + void update(RobotPosition lastTargetPacket); + void move(Vector& motionSpeed); + void rotateControl(Vector& speed, Vector robot, float targetAngle, float angleKp); + void rotateControl(Vector& speed, Vector robot); + void motionControlOld(Vector& speed, Vector robot, Vector target, float maxSpeed, float minSpeed); + void motionControlOld(Vector& speed, Vector robot); + void rotateInPoint(Vector& speed, + Vector robot, + Vector target, + float maxSpeed, + float propSpeedFactor, + bool rotateInClockWise, + float orbitRadius, + float angleKp, + float approachKp); + void rotateInPoint(Vector& speed, Vector robot); + void rotateInPoint(Vector& speed, Vector robot, Vector target, float maxSpeed, float minSpeed); + void printTarget(); + void printVelocity(Vector speed); + + private: + Kinematics* _kinematics; + MotionControl* _motion; + Odometry* _odometry; + RobotPosition _target; +}; + +#endif /* NAVIGATION_H */ diff --git a/lib/Odometry/MPU6050/MPU6050.cpp b/lib/Odometry/MPU6050/MPU6050.cpp new file mode 100644 index 0000000..f90f06e --- /dev/null +++ b/lib/Odometry/MPU6050/MPU6050.cpp @@ -0,0 +1,373 @@ +/** + * @author Crispin Mukalay + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * @section DESCRIPTION + * + * MPU-6050 triple-axis MEMS gyroscope and triple-axis MEMS accelerometer. + * + * Datasheet: + * + * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf + */ + +/** + * Includes + */ +#include "MPU6050.h" +#include + +MPU6050::MPU6050(PinName sda, PinName scl) : i2c_(sda, scl) { + // 400kHz, fast mode. + i2c_.frequency(400000); +} + +bool MPU6050::initialize() { + if (this->getWhoAmI() != 0x68) { + printf("ERROR: MPU6050 not found...\n"); + return false; + } + + // The device will come up in sleep mode upon power-up. + this->setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC); // Disable sleep mode + ThisThread::sleep_for(1ms); + + // Full scale, +/-2000°/s, 16.4LSB°/s. + this->setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Gyroscope self-test trigger off. + _gyro_scale = 16.4; + ThisThread::sleep_for(1ms); + + // Full scale, +/-16g, 2048LSB/g. + this->setAccelConfig(ACC_ST_OFF, AFS_16g); // Accelerometer self-test trigger off. + _accel_scale = 2048.0; + ThisThread::sleep_for(20ms); + return true; +} + +void MPU6050::selfTest(uint8_t* TestReadings) { + + char buffer[4]; + uint8_t Xa1, Xa2, Ya1, Ya2, Za1, Za2; + + buffer[0] = SingleByteRead(SELF_TEST_X_REG); + buffer[1] = SingleByteRead(SELF_TEST_Y_REG); + buffer[2] = SingleByteRead(SELF_TEST_Z_REG); + buffer[3] = SingleByteRead(SELF_TEST_A_REG); + + // Split the bytes + Xa1 = (int) buffer[0] >> 5 & 0x07; + Ya1 = (int) buffer[1] >> 5 & 0x07; + Za1 = (int) buffer[2] >> 5 & 0x07; + + Xa2 = (int) buffer[3] & 0x30; + Ya2 = (int) buffer[3] & 0x0C; + Za2 = (int) buffer[3] & 0x03; + + TestReadings[0] = Xa1 | Xa2; + TestReadings[1] = Ya1 | Ya2; + TestReadings[2] = Za1 | Za2; + TestReadings[3] = (int) buffer[0] & 0x1F; + TestReadings[4] = (int) buffer[1] & 0x1F; + TestReadings[5] = (int) buffer[2] & 0x1F; +} + +void MPU6050::setSampleRate(uint8_t Divider) { + + SingleByteWrite(SMPLRT_DIV_REG, Divider); +} + +uint8_t MPU6050::getSampleRate(void) { + + return SingleByteRead(SMPLRT_DIV_REG); +} + +void MPU6050::setFSYNCConfig(uint8_t Conf) { + + uint8_t conf_L; + uint8_t conf_H; + uint8_t conf_HL; + + conf_L = SingleByteRead(CONFIG_REG) & 0x07; + conf_H = Conf << 3; + conf_HL = conf_H | conf_L; + + SingleByteWrite(CONFIG_REG, conf_HL); +} + +uint8_t MPU6050::getFSYNCConfig(void) { + + return (int) SingleByteRead(CONFIG_REG) >> 3 & 0x07; +} + +void MPU6050::setDLPFConfig(uint8_t Conf) { + + uint8_t conf_L; + uint8_t conf_H; + uint8_t conf_HL; + + conf_L = Conf; + conf_H = SingleByteRead(CONFIG_REG) & 0x38; + conf_HL = conf_H | conf_L; + + SingleByteWrite(CONFIG_REG, conf_HL); +} + +uint8_t MPU6050::getDLPFConfig(void) { + + return (int) SingleByteRead(CONFIG_REG) & 0x07; +} + +void MPU6050::setGyroConfig(uint8_t GyroST, uint8_t Scale) { + + SingleByteWrite(GYRO_CONFIG_REG, GyroST | Scale); +} + +uint8_t MPU6050::getGyroConfig(void) { + + return (int) SingleByteRead(GYRO_CONFIG_REG); +} + +void MPU6050::setAccelConfig(uint8_t AccST, uint8_t Scale) { + + SingleByteWrite(ACCEL_CONFIG_REG, AccST | Scale); +} + +uint8_t MPU6050::getAccelConfig(void) { + + return (int) SingleByteRead(ACCEL_CONFIG_REG); +} + +void MPU6050::setFIFO_Enable(uint8_t Setting) { + + SingleByteWrite(FIFO_EN_REG, Setting); +} + +uint8_t MPU6050::getFIFO_Enable(void) { + + return (int) SingleByteRead(FIFO_EN_REG); +} + +void MPU6050::readAccelRaw(int16_t* accReadings) { + char ACCEL_OUT_buffer[6]; + + // Optimization of Read + multiByteRead(ACCEL_XOUT_H_REG, ACCEL_OUT_buffer, 6); + // ACCEL_OUT_buffer[0] = SingleByteRead(ACCEL_XOUT_H_REG); + // ACCEL_OUT_buffer[1] = SingleByteRead(ACCEL_XOUT_L_REG); + // ACCEL_OUT_buffer[2] = SingleByteRead(ACCEL_YOUT_H_REG); + // ACCEL_OUT_buffer[3] = SingleByteRead(ACCEL_YOUT_L_REG); + // ACCEL_OUT_buffer[4] = SingleByteRead(ACCEL_ZOUT_H_REG); + // ACCEL_OUT_buffer[5] = SingleByteRead(ACCEL_ZOUT_L_REG); + + accReadings[0] = (int) ACCEL_OUT_buffer[0] << 8 | (int) ACCEL_OUT_buffer[1]; + accReadings[1] = (int) ACCEL_OUT_buffer[2] << 8 | (int) ACCEL_OUT_buffer[3]; + accReadings[2] = (int) ACCEL_OUT_buffer[4] << 8 | (int) ACCEL_OUT_buffer[5]; +} + +void MPU6050::readAccel(double* accReadings) { + + int16_t accel[3] = {0, 0, 0}; + this->readAccelRaw(accel); + + accReadings[0] = ((int) accel[0]) / _accel_scale; + accReadings[1] = ((int) accel[1]) / _accel_scale; + accReadings[2] = ((int) accel[2]) / _accel_scale; +} + +void MPU6050::readTemp(int16_t* TempReadings) { + + char TEMP_OUT_buffer[2]; + + TEMP_OUT_buffer[0] = SingleByteRead(TEMP_OUT_H_REG); + TEMP_OUT_buffer[1] = SingleByteRead(TEMP_OUT_L_REG); + + TempReadings[0] = (int) TEMP_OUT_buffer[0] << 8 | (int) TEMP_OUT_buffer[1]; +} + +void MPU6050::readGyroRaw(int16_t* gyroReadings) { + + char GYRO_OUT_buffer[6]; + + // Optimization of Read + multiByteRead(GYRO_XOUT_H_REG, GYRO_OUT_buffer, 6); + // GYRO_OUT_buffer[0] = SingleByteRead(GYRO_XOUT_H_REG); + // GYRO_OUT_buffer[1] = SingleByteRead(GYRO_XOUT_L_REG); + // GYRO_OUT_buffer[2] = SingleByteRead(GYRO_YOUT_H_REG); + // GYRO_OUT_buffer[3] = SingleByteRead(GYRO_YOUT_L_REG); + // GYRO_OUT_buffer[4] = SingleByteRead(GYRO_ZOUT_H_REG); + // GYRO_OUT_buffer[5] = SingleByteRead(GYRO_ZOUT_L_REG); + + gyroReadings[0] = (int) GYRO_OUT_buffer[0] << 8 | (int) GYRO_OUT_buffer[1]; + gyroReadings[1] = (int) GYRO_OUT_buffer[2] << 8 | (int) GYRO_OUT_buffer[3]; + gyroReadings[2] = (int) GYRO_OUT_buffer[4] << 8 | (int) GYRO_OUT_buffer[5]; +} + +void MPU6050::readGyro(double* gyroReadings) { + int16_t gyro[3] = {0, 0, 0}; + this->readGyroRaw(gyro); + gyroReadings[0] = (int) gyro[0] / _gyro_scale; + gyroReadings[1] = (int) gyro[1] / _gyro_scale; + gyroReadings[2] = (int) gyro[2] / _gyro_scale; +} + +void MPU6050::sigPathReset(uint8_t ResVal) { + + SingleByteWrite(SIGNAL_PATH_RESET_REG, ResVal); +} + +void MPU6050::setUserCtl(uint8_t Settings) { + + SingleByteWrite(USER_CTRL_REG, Settings); +} + +uint8_t MPU6050::getUserCtl(void) { + + return (int) SingleByteRead(USER_CTRL_REG); +} + +void MPU6050::setPowerCtl_1(uint8_t DevRes, + uint8_t Sleep, + uint8_t Cycle, + uint8_t Temp, + uint8_t Clock) { + + uint8_t powerSetting; + + powerSetting = DevRes | Sleep; + powerSetting = powerSetting | Cycle; + powerSetting = powerSetting | Temp; + powerSetting = powerSetting | Clock; + + SingleByteWrite(PWR_MGMT_1_REG, powerSetting); +} + +uint8_t MPU6050::getPowerCtl_1(void) { + + return (int) SingleByteRead(PWR_MGMT_1_REG); +} + +void MPU6050::setPowerCtl_2(uint8_t Conf) { + + SingleByteWrite(PWR_MGMT_2_REG, Conf); +} + +uint8_t MPU6050::getPowerCtl_2(void) { + + return (int) SingleByteRead(PWR_MGMT_2_REG); +} + +uint16_t MPU6050::getFIFOCount(void) { + + uint16_t FIFOCount_HL, FIFOCount_L, FIFOCount_H; + + FIFOCount_L = SingleByteRead(FIFO_COUNTL_REG); + FIFOCount_H = SingleByteRead(FIFO_COUNTH_REG); + + FIFOCount_HL = FIFOCount_H << 8 | FIFOCount_L; + + return FIFOCount_HL; +} + +void MPU6050::FIFODataWrite(uint8_t Data) { + + SingleByteWrite(FIFO_R_W_REG, Data); +} + +uint8_t MPU6050::FIFODataRead(void) { + + return (int) SingleByteRead(FIFO_R_W_REG); +} + +uint8_t MPU6050::getWhoAmI(void) { + + // WhoAmI Register address. + return SingleByteRead(WHO_AM_I_REG); +} + +char MPU6050::SingleByteRead(char address) { + + /****info on the i2c_.read*** + address : 8-bit I2C slave address [ addr | 1 ] + data : Pointer to the byte-array to read data into + length : Number of bytes to read + repeated : Repeated start, true - don't send stop at end + returns : 0 on success (ack), or non-0 on failure (nack) + */ + + char tx = address; // Address of register being accessed + char output; // Data read from the register + + i2c_.write((MPU6050_I2C_ADDRESS << 1) & 0xFE, &tx, 1); // Access register indicated by the address + i2c_.read((MPU6050_I2C_ADDRESS << 1) | 0x01, + &output, + 1); // Read data from the register indicated by address and store it in output variable + + // i2c_.write(MPU6050_I2C_ADDRESS, &tx, 1); //Access register indicated by the address + // i2c_.read(MPU6050_I2C_ADDRESS, &output, 1); //Read data from the register indicated by + // address and store it in output variable + + return output; +} + +uint8_t MPU6050::SingleByteWrite(char address, char data) { + + /****info on the i2c_.write*** + address : 8-bit I2C slave address [ addr | 0 ] + data : Pointer to the byte-array data to send + length : Number of bytes to send + repeated : Repeated start, true - do not send stop at end + returns : 0 on success (ack), or non-0 on failure (nack) + */ + + int ack = 0; + char tx[2]; // Two bytes to send during write operation, address and data + + tx[0] = address; // Address of register being accessed + tx[1] = data; // data to write to the register + + return ack | i2c_.write((MPU6050_I2C_ADDRESS << 1) & 0xFE, tx, 2); // Bitwise OR +} + +void MPU6050::multiByteRead(char address, char* output, int size) { + + i2c_.write((MPU6050_I2C_ADDRESS << 1) & 0xFE, + &address, + 1); // Access register indicated by the address + i2c_.read( + (MPU6050_I2C_ADDRESS << 1) | 0x01, + output, + size); // Read data from the register indicated by address and store it in output variable +} + +uint8_t MPU6050::multiByteWrite(char address, char* ptr_data, int size) { + + int ack; + ack = i2c_.write((MPU6050_I2C_ADDRESS << 1) & 0xFE, + &address, + 1); // Access register indicated by the address + + return ack | i2c_.write((MPU6050_I2C_ADDRESS << 1) & 0xFE, + ptr_data, + size); // Write data to the register indicated by address +} diff --git a/lib/Odometry/MPU6050/MPU6050.h b/lib/Odometry/MPU6050/MPU6050.h new file mode 100644 index 0000000..f129660 --- /dev/null +++ b/lib/Odometry/MPU6050/MPU6050.h @@ -0,0 +1,542 @@ +/** + * @author Crispin Mukalay + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * @section DESCRIPTION + * + * MPU-6050 triple-axis MEMS gyroscope and triple-axis MEMS accelerometer. + * + * Datasheet: + * + * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf + */ + +#ifndef MPU6050_H +#define MPU6050_H + +/** + * Includes + */ +#include "mbed.h" + +/** + * Defines + */ +#define MPU6050_I2C_ADDRESS 0x68 // 7-bit address of slave device (MPU6050). + +//----------- +// Registers +//----------- +#define SELF_TEST_X_REG 0x0D +#define SELF_TEST_Y_REG 0x0E +#define SELF_TEST_Z_REG 0x0F +#define SELF_TEST_A_REG 0x10 +#define SMPLRT_DIV_REG 0x19 +#define CONFIG_REG 0x1A +#define GYRO_CONFIG_REG 0x1B +#define ACCEL_CONFIG_REG 0x1C +#define FIFO_EN_REG 0x23 +#define I2C_MST_CTRL_REG 0x24 +#define I2C_SLV0_ADDR_REG 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL_REG 0x27 +#define I2C_SLV1_ADDR_REG 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL_REG 0x2A +#define I2C_SLV2_ADDR_REG 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL_REG 0x2D +#define I2C_SLV3_ADDR_REG 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL_REG 0x30 +#define I2C_SLV4_ADDR_REG 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO_REG 0x33 +#define I2C_SLV4_CTRL_REG 0x34 +#define I2C_SLV4_DI_REG 0x35 +#define I2C_MST_STATUS_REG 0x36 +#define INT_PIN_CFG_REG 0x37 +#define INT_ENABLE_REG 0x38 +#define INT_STATUS_REG 0x3A +#define ACCEL_XOUT_H_REG 0x3B +#define ACCEL_XOUT_L_REG 0x3C +#define ACCEL_YOUT_H_REG 0x3D +#define ACCEL_YOUT_L_REG 0x3E +#define ACCEL_ZOUT_H_REG 0x3F +#define ACCEL_ZOUT_L_REG 0x40 +#define TEMP_OUT_H_REG 0x41 +#define TEMP_OUT_L_REG 0x42 +#define GYRO_XOUT_H_REG 0x43 +#define GYRO_XOUT_L_REG 0x44 +#define GYRO_YOUT_H_REG 0x45 +#define GYRO_YOUT_L_REG 0x46 +#define GYRO_ZOUT_H_REG 0x47 +#define GYRO_ZOUT_L_REG 0x48 +#define EXT_SENS_DATA_00_REG 0x49 +#define EXT_SENS_DATA_01_REG 0x4A +#define EXT_SENS_DATA_02_REG 0x4B +#define EXT_SENS_DATA_03_REG 0x4C +#define EXT_SENS_DATA_04_REG 0x4D +#define EXT_SENS_DATA_05_REG 0x4E +#define EXT_SENS_DATA_06_REG 0x4F +#define EXT_SENS_DATA_07_REG 0x50 +#define EXT_SENS_DATA_08_REG 0x51 +#define EXT_SENS_DATA_09_REG 0x52 +#define EXT_SENS_DATA_10_REG 0x53 +#define EXT_SENS_DATA_11_REG 0x54 +#define EXT_SENS_DATA_12_REG 0x55 +#define EXT_SENS_DATA_13_REG 0x56 +#define EXT_SENS_DATA_14_REG 0x57 +#define EXT_SENS_DATA_15_REG 0x58 +#define EXT_SENS_DATA_16_REG 0x59 +#define EXT_SENS_DATA_17_REG 0x5A +#define EXT_SENS_DATA_18_REG 0x5B +#define EXT_SENS_DATA_19_REG 0x5C +#define EXT_SENS_DATA_20_REG 0x5D +#define EXT_SENS_DATA_21_REG 0x5E +#define EXT_SENS_DATA_22_REG 0x5F +#define EXT_SENS_DATA_23_REG 0x60 +#define I2C_SLV0_DO_REG 0x63 +#define I2C_SLV1_DO_REG 0x64 +#define I2C_SLV2_DO_REG 0x65 +#define I2C_SLV3_DO_REG 0x66 +#define I2C_MST_DELAY_CTRL_REG 0x67 +#define SIGNAL_PATH_RESET_REG 0x68 +#define USER_CTRL_REG 0x6A +#define PWR_MGMT_1_REG 0x6B +#define PWR_MGMT_2_REG 0x6C +#define FIFO_COUNTH_REG 0x72 +#define FIFO_COUNTL_REG 0x73 +#define FIFO_R_W_REG 0x74 +#define WHO_AM_I_REG 0x75 + +//---------------------------------------------- +// External Frame Synchronization Configuration +//---------------------------------------------- +#define FSYNC_DISABLE 0x00 +#define FSYNC_TEMP_OUT_L 0x01 +#define FSYNC_GYRO_XOUT_L 0x02 +#define FSYNC_GYRO_YOUT_L 0x03 +#define FSYNC_GYRO_ZOUT_L 0x04 +#define FSYNC_ACCEL_XOUT_L 0x05 +#define FSYNC_ACCEL_YOUT_L 0x06 +#define FSYNC_ACCEL_ZOUT_L 0x07 + +//--------------------------------------- +// Digital Low Pass Filter Configuration +//--------------------------------------- +#define DLPF_260_256 0x00 +#define DLPF_184_188 0x01 +#define DLPF_94_98 0x02 +#define DLPF_44_42 0x03 +#define DLPF_21_20 0x04 +#define DLPF_10_10 0x05 +#define DLPF_5_5 0x06 + +//------------------------------------ +// Gyroscope Selft-Test Configuration +//------------------------------------ +#define GFS_DISABLE 0x00 +#define GFS_250dps 0x00 +#define GFS_500dps 0x08 +#define GFS_1000dps 0x10 +#define GFS_2000dps 0x18 +#define GYRO_ST_ON 0xE0 +#define GYRO_ST_OFF 0x00 + +//---------------------------------------- +// Accelerometer Selft-Test Configuration +//---------------------------------------- +#define AFS_DSIABLE 0x00 +#define AFS_2g 0x00 +#define AFS_4g 0x08 +#define AFS_8g 0x10 +#define AFS_16g 0x18 +#define ACC_ST_ON 0xE0 +#define ACC_ST_OFF 0x00 + +//---------------------------------------------- +// Sensor Measurements To Load In FIFO Register +//---------------------------------------------- +#define TEMP_FIFO_EN 0x80 +#define GYRO_FIFO_EN 0x70 +#define ACCEL_FIFO_EN 0x08 +#define SLV2_FIFO_EN 0x04 +#define SLV1_FIFO_EN 0x02 +#define SLV0_FIFO_EN 0x01 +#define SLVZ_FIFO_EN 0x07 +#define TEMPGYROACCEL_FIFO_EN 0xF8 +#define GYROACCEL_FIFO_EN 0x78 +#define TEMPGYROACCELSLVZ_FIFO_EN 0xFF + +//--------------------------- +// Signal Path Reset Values +//--------------------------- +#define GYRRO_RESET 0x04 +#define ACCEL_RESET 0x02 +#define TEMP_RESET 0x01 +#define ALL_RESET 0x07 + +//------------------------------- +// User Control settings +//------------------------------- +#define FIFO_EN 0x40 +#define FIFO_DIS 0x00 +#define I2C_MST_EN 0x60 +#define I2C_MST_DIS 0x00 +#define FIFO_RESET 0x04 +#define I2C_MST_RESET 0x02 +#define SIG_COND_RESET 0x41 + +//--------------------------------- +// Power Management Setings One +//--------------------------------- +#define DEVICE_RESET 0x80 +#define SEEP 0x40 +#define CYCLE 0x20 +#define TEMP_DIS 0x08 +#define INT_8MHz_OSC 0x00 +#define PLL_GYROX_REF 0x01 +#define PLL_GYROY_REF 0x02 +#define PLL_GYROZ_REF 0x03 +#define PLL_EXT_32KHz_REF 0x04 +#define PLL_EXT_19KHz_REF 0x05 + +//--------------------------------- +// Power Management Settings Two +//--------------------------------- +#define LP_WAKE_CTRL_1Hz 0x00 +#define LP_WAKE_CTRL_5Hz 0x40 +#define LP_WAKE_CTRL_20Hz 0x80 +#define LP_WAKE_CTRL_40Hz 0xC0 + +/** + * MPU6050 triple axis digital gyroscope and triple axis digital accelerometer. + */ +class MPU6050 { + + public: + /** + * + * @param sda - mbed pin to use for the SDA I2C line. + * @param scl - mbed pin to use for the SCL I2C line. + */ + MPU6050(PinName sda, PinName scl); + + bool initialize(); + + /** + * Self-test permits users to test the mechanical and electrical portions of the MPU6050. + * + * @return Contents of the self-test registers. + * Array of XA_TEST, YA_TEST, ZA_TEST, XG_TEST, YG_TEST and ZG_TEST. + */ + void selfTest(uint8_t* TestReadings); + + /** + * Set Sample Rate for the MPU-6050 senor output, FIFO, and DMP registers. + * + * @param Divider - The sample rate divider as a number from 0x00 - 0xFF. + * + */ + void setSampleRate(uint8_t Divider); + + /** + * Get Sample Rate for the MPU-6050 senor output, FIFO, and DMP registers. + * + * @return Sample rate. + * + */ + uint8_t getSampleRate(void); + + /** + * Set the external Frame Synchronization (FSYNC) pin sampling. + * + * @param Conf - The configuration setting as a number from 0x00 - 0xFF. + * + */ + void setFSYNCConfig(uint8_t Conf); + + /** + * Get the Frame Synchronization (FSYNC) configuration. + * + * @return Current Frame Synchronization (FSYNC) configuration. + * + */ + uint8_t getFSYNCConfig(void); + + /** + * Set the Digital Low Pass Filter (DLPF) setting for both gyro and accelemometer. + * + * @param Conf - The configuration setting as a number from 0-7. + * + */ + void setDLPFConfig(uint8_t Conf); + + /** + * Get the Digital Low Pass Filter (DLPF) setting for both gyro and accelemometer. + * + * @return Current Digital Low Pass Filter (DLPF) setting. + * + */ + uint8_t getDLPFConfig(void); + + /** + * Set the gyroscope self-test trigger and scale range configuration. + * + * @param GyroST - Trigger Gyroscope self-test setting. GYRO_ST_ON or GYRO_ST_OFF. + * @param Scale - The full scale range setting for the gyroscope GFS_DISABLE, GFS_250dps, + * GFS_500dps, GFS_1000dps, GFS_2000dps. + * + */ + void setGyroConfig(uint8_t GyroST, uint8_t Scale); + + /** + * Get the gyroscope self-test trigger and scale range configuration. + * + * @return Current gyroscope self-test configuration. + * + */ + uint8_t getGyroConfig(void); + + /** + * Set the gyroscope self-test trigger and scale range configuration. + * + * @param AccST - Trigger Accelerometer self-test setting. ACC_ST_ON or ACC_ST_OFF. + * @param Scale - The full scale range setting for the accelemometer AFS_DSIABLE, AFS_2g, AFS_4g, + * AFS_8g, AFS_16g. + * + */ + void setAccelConfig(uint8_t AccST, uint8_t Scale); + + /** + * Get the accelerometer self-test trigger and scale range configuration. + * + * @return Current accelerometer self-test configuration. + * + */ + uint8_t getAccelConfig(void); + + /** + * Set the setting for sensor measurements to be loaded to the FIFO register + * + * @param Setting - The configuration settings which determines the sensor measurements loaded + * into the FIFO register. TEMP_FIFO_EN, GYRO_FIFO_EN, ACCEL_FIFO_EN, SLV2_FIFO_EN, SLV1_FIFO_EN, + * SLV0_FIFO_EN, SLVZ_FIFO_EN, TEMPGYROACCEL_FIFO_EN, GYROACCEL_FIFO_EN, + * TEMPGYROACCELSLVZ_FIFO_EN. + */ + void setFIFO_Enable(uint8_t Setting); + + /** + * Get the setting for sensor measurements to be loaded to the FIFO register. + * + * @return Current setting which determines the sensor measurements loaded into the FIFO register. + * + */ + uint8_t getFIFO_Enable(void); + + /** + * The most recent accelemometer measurements. + * + * @return Contents of Accelerometer measurement registers. + * Array of ACCEL_XOUT, ACCEL_YOUT, ACCEL_ZOUT as 16-bit 2's complement values. + */ + void readAccelRaw(int16_t* accReadings); + + /** + * The most recent accelemometer measurements. + * + * @return Contents of Accelerometer measurement registers. + * Array of ACCEL_XOUT, ACCEL_YOUT, ACCEL_ZOUT converted to g (m/(s*s)). + */ + void readAccel(double* accReadings); + + /** + * The most recent temperature measurements. + * + * @return TempReadings - Contents Temperature measurement registers. + * TEMP_OUT as 16-bit 2's complement value. + */ + void readTemp(int16_t* TempReadings); + + /** + * The most recent gyroscope measurements. + * + * @return Contents of Gyroscope measurement registers. + * Array of GYRO_XOUT, GYRO_YOUT, GYRO_ZOUT as 16-bit 2's complement values. + */ + void readGyroRaw(int16_t* gyroReadings); + + /** + * The most recent gyroscope measurements. + * + * @return Contents of Gyroscope measurement registers. + * Array of GYRO_XOUT, GYRO_YOUT, GYRO_ZOUT as 16-bit 2's complement values. + */ + void readGyro(double* gyroReadings); + + /** + * Resets the analog and digital paths of the gyroscope, accelemometer and temperature sensors. + * + * @param ResVal - The reset settings GYRO_RESET, ACCEL_RESET, TEMP_RESET, ALL_RESET. + * + */ + void sigPathReset(uint8_t ResVal); + + /** + * Enable or disable FIFO buffer, I2C Master Mode, and primary I2C interface. + * + * @param Settings - The configuration settings for user control register. + * FIFO_EN, FIFO_DIS, I2C_MST_EN, I2C_MST_DIS, FIFO_RESET, I2C_MST_RESET, + * SIG_COND_RESET. + * + */ + void setUserCtl(uint8_t Settings); + + /** + * Get the user setting for FIFO buffer, I2C Master Mode, and primary I2C interface. + * + * @return Current user settings for FIFO buffer, I2C Master Mode, and primary I2C interface. + * + */ + uint8_t getUserCtl(void); + + /** + * Set the power mode control settings. + * + * @param DevRes - Resets all internal resgisters to their default values. DEVICE_RESET or 0x00. + * @param Sleep - Puts the MPU6050 into sleep mode. SEEP or 0x00. + * @param Cycle - Cycles MPU6050 between sleep mode and waking up to take a single sample of data + * from active sensors at a rate determined by LP_WAKE_CTRL register. CYCLE or 0x00. + * @param Temp - Disables the temperature sensor. TEMP_DIS or 0x00. + * @param Clock - Specifies the clock source of the device. + * Settings INT_8MHz_OSC, PLL_GYROX_REF, PLL_GYROY_REF, PLL_GYROZ_REF, + * PLL_EXT_32KHz_REF, PLL_EXT_19KHz_REF. + * + */ + void setPowerCtl_1(uint8_t DevRes, uint8_t Sleep, uint8_t Cycle, uint8_t Temp, uint8_t Clock); + + /** + * Get the power mode and clock source. + * + * @return Current power mode control settings. + * + */ + uint8_t getPowerCtl_1(void); + + /** + * Set frequency of wake-ups in Accelerometer Only Low Power Mode, as well as individual gyroscope + * and accelemometer axes standby configuration. + * + * @param Conf - Wake-up and axes standby configuration. + * Configurations LP_WAKE_CTRL_1Hz, LP_WAKE_CTRL_5Hz, LP_WAKE_CTRL_20Hz, + * LP_WAKE_CTRL_40Hz. + * + */ + void setPowerCtl_2(uint8_t Conf); + + /** + * Get frequency of wake-ups in Accelerometer Only Low Power Mode, as well as individual gyroscope + * and accelemometer axes standby status. + * + * @return Current wake-up and individual axes standby status. + * + */ + uint8_t getPowerCtl_2(void); + + /** + * Read the number of samples currenly in the FIFO buffer. + * + * @return Current number of samples in the FIFO buffer. + * + */ + uint16_t getFIFOCount(void); + + /** + * This register is used to write data to the FIFO buffer. + * + * @param Data - Data to write to the FIFO buffer. Value from 0x00 - 0xFF. + * + */ + void FIFODataWrite(uint8_t Data); + + /** + * This register is used to read data from the FIFO buffer. + * + * @return Data located in the FIFO buffer. + * + */ + uint8_t FIFODataRead(void); + + /** + * Get the identity of the device. + * + * @return + * + */ + uint8_t getWhoAmI(void); + + private: + I2C i2c_; + double _accel_scale = 2048.0; + double _gyro_scale = 16.4; + + /** + * Read one byte from a register on the device. + * + * @param: - the address to be read from + * + * @return: the value of the data read + */ + char SingleByteRead(char address); + + /** + * Write one byte to a register on the device. + * + * @param: + - address of the register to write to. + - the value of the data to store + */ + uint8_t SingleByteWrite(char address, char data); + + /** + * Read several consecutive bytes on the device and store them in a given location. + * + * @param startAddress: The address of the first register to read from. + * @param ptr_output: a pointer to the location to store the data being read + * @param size: The number of bytes to read. + */ + void multiByteRead(char startAddress, char* ptr_output, int size); + + /** + * Write several consecutive bytes on the device. + * + * @param startAddress: The address of the first register to write to. + * @param ptr_data: Pointer to a location which contains the data to write. + * @param size: The number of bytes to write. + */ + uint8_t multiByteWrite(char startAddress, char* ptr_data, int size); +}; + +#endif /* MPU6050_H */ diff --git a/lib/Odometry/Odometry.cpp b/lib/Odometry/Odometry.cpp new file mode 100644 index 0000000..4f81605 --- /dev/null +++ b/lib/Odometry/Odometry.cpp @@ -0,0 +1,187 @@ + +#include "Odometry.h" +#include "MPU6050/MPU6050.h" +#include + +Odometry::Odometry(MotionControl* motion, Kinematics* kinematics, int tsample, int tGyroSample) : + _MPU(MPU_SDA, MPU_SCL) { + _motion = motion; + _kinematics = kinematics; + _tsample = tsample; + _tGyroSample = tGyroSample; +} + +void Odometry::init() { + + _activeIMU = _MPU.initialize(); + + freqUpdate.attach(callback(this, &Odometry::update), chrono::milliseconds(_tsample)); + if (_activeIMU) { + _gyroFlag = false; + // calibrateGyroOffset(); + gyroUpdate.attach(callback(this, &Odometry::updateGyroFlag), + chrono::milliseconds(_tGyroSample)); + } + + gyroTimer.start(); +} + +void Odometry::calibrateGyroOffset() { + double gyroRead[3] = {0, 0, 0}; + double k = 0.05; + int count = 0; + while (1) { + getAllGyroRead(gyroRead); + if (fabs(gyroRead[0]) < GYRO_TOLERANCE && fabs(gyroRead[1]) < GYRO_TOLERANCE && + fabs(gyroRead[2]) < GYRO_TOLERANCE) { + if (count > 10) + break; + count++; + } + _gyroOffset[0] += gyroRead[0] * k; + _gyroOffset[1] += gyroRead[1] * k; + _gyroOffset[2] += gyroRead[2] * k; + + utils::beep(_tGyroSample, 370, 0.1); + debug_if(DEBUG, + "Odometry: Calibrating GyroOffset [0]: %lf, [1]: %lf, [2]: %lf\n", + _gyroOffset[0], + _gyroOffset[1], + _gyroOffset[2]); + } + utils::beep(100, 370, 0.1); + printf("Odometry GyroOffset [0]: %lf, [1]: %lf, [2]: %lf\n", + _gyroOffset[0], + _gyroOffset[1], + _gyroOffset[2]); + utils::beep(100, 370, 0.1); +} + +double Odometry::getGyroRead() { + double gyro[3] = {0, 0, 0}; + _MPU.readGyro(gyro); + return gyro[2] - _gyroOffset[2]; +} + +void Odometry::getAllGyroRead(double* gyroRead) { + double gyro[3] = {0, 0, 0}; + _MPU.readGyro(gyro); + gyroRead[0] = gyro[0] - _gyroOffset[0]; + gyroRead[1] = gyro[1] - _gyroOffset[1]; + gyroRead[2] = gyro[2] - _gyroOffset[2]; +} + +void Odometry::update() { + Vector move; + _motion->getRobotSpeed(move, false); + move = (move * double(_tsample / 1000.0)); + + // Using Gyro for delta W + if (_activeIMU) { + // Only use Gyro angle if IMU active. + double deltaGyro = getGyroMove(); + _gyroAngle += deltaGyro; + limitW(_gyroAngle); + move.w = deltaGyro; + } + _currentPosition = _currentPosition + _kinematics->rotateToGlobal(_currentPosition.w, move); + limitW(_currentPosition); + _movement = _currentPosition - _initialPosition; + + _currentOdometry = _currentOdometry + _kinematics->rotateToGlobal(_currentOdometry.w, move); + limitW(_currentOdometry); +} + +void Odometry::updateGyroFlag() { + _gyroFlag = true; +} + +void Odometry::processGyro() { + // Should read Gyro? + if (_gyroFlag && _activeIMU) { + double accumulator = 0.0; + for (int i = 0; i < _nGyroSample; i++) { + accumulator += getGyroRead(); + } + accumulator /= (_nGyroSample / 1.0); + accumulator = accumulator * (M_PI / 180.0); // TO RAD + if (fabs(accumulator) < 0.03) { + accumulator = 0; + } + double gyroElapsedTime = utils::timerRead(gyroTimer); + _gyroMove += (accumulator * double(gyroElapsedTime / 1000.0)); + + gyroTimer.reset(); + _gyroFlag = false; // Clear Gyro Flag + } +} + +void Odometry::updatePosition(Vector pos) { + _initialPosition = pos; + _currentPosition = pos; + _movement = Vector(); + _gyroAngle = pos.w; +} + +void Odometry::resetPosition() { + _currentPosition = _initialPosition; + _movement = Vector(); + _gyroAngle = 0; +} + +void Odometry::updatePosition() { + Vector pos; + this->updatePosition(pos); +} + +void Odometry::getCurrentPosition(Vector& v) { + v = _currentPosition; +} + +Vector Odometry::getCurrentPosition() { + return _currentPosition; +} + +Vector Odometry::getInitialPosition() { + return _initialPosition; +} + +Vector Odometry::getMovement() { + return this->_movement; +} + +Vector Odometry::getCurrentOdometry() { + return _currentOdometry; +} + +void Odometry::getGyroAngle(double& w) { + w = _gyroAngle; +} + +double Odometry::getGyroMove() { + double w = _gyroMove; + _gyroMove = 0; // Resets gyro move. + return w; +} + +void Odometry::limitW(double& w) { + if (w > M_PI) + w -= 2 * M_PI; + else if (w < -M_PI) + w += 2 * M_PI; +} + +void Odometry::limitW(Vector& v) { + this->limitW(v.w); +} + +void Odometry::printCurrentPosition() { + printf("Current X: %lf - Y: %lf - W: %lf \n", + _currentPosition.x, + _currentPosition.y, + _currentPosition.w); +} + +void Odometry::printMovement() { + printf("Movement X: %lf - Y: %lf - W: %lf \n", _movement.x, _movement.y, _movement.w); +} diff --git a/lib/Odometry/Odometry.h b/lib/Odometry/Odometry.h new file mode 100644 index 0000000..9b51b3b --- /dev/null +++ b/lib/Odometry/Odometry.h @@ -0,0 +1,65 @@ +#ifndef ODOMETRY_H +#define ODOMETRY_H + +#include "mbed.h" +#include +#include +#include + +class Odometry { + public: + Odometry(MotionControl* motion, Kinematics* kinematics, int tsample, int gyroSample); + void init(); + void updatePosition(Vector pos); + void updatePosition(); + void resetPosition(); + void getCurrentPosition(Vector& v); + void getSpeedIntPosition(Vector& v); + Vector getMovement(); + Vector getInitialPosition(); + Vector getCurrentPosition(); + Vector getCurrentOdometry(); + Vector getSpeedIntPosition(); + + void getGyroAngle(double& w); // Absolute angle + double getGyroRead(); + void getAllGyroRead(double* gyroRead); + double getGyroMove(); // Relative angular movement + + void printCurrentPosition(); + void printSpeedIntPosition(); + void printMovement(); + void printIntegralMovement(); + void processGyro(); + + private: + void update(); + void updateGyroFlag(); + void calibrateGyroOffset(); + void limitW(Vector& v); + void limitW(double& w); + + Ticker freqUpdate; + int _tsample; + + bool _gyroFlag = true; + bool _activeIMU = false; + Ticker gyroUpdate; + Timer gyroTimer; + int _tGyroSample; + double _nGyroSample = 3.0; + + double _gyroMove = 0; + double _gyroAngle = 0; + std::vector _gyroOffset = {0, 0, 0}; + + Vector _initialPosition; + Vector _currentPosition; + Vector _movement; + Vector _currentOdometry; + MPU6050 _MPU; + MotionControl* _motion; + Kinematics* _kinematics; +}; + +#endif /* ODOMETRY_H */ diff --git a/lib/Utils/NeoPixel/.hg_archival.txt b/lib/Utils/NeoPixel/.hg_archival.txt new file mode 100644 index 0000000..65a9e1c --- /dev/null +++ b/lib/Utils/NeoPixel/.hg_archival.txt @@ -0,0 +1,5 @@ +repo: a81364d9a67b340d268d76b81352b45ff64da6af +node: a81364d9a67b340d268d76b81352b45ff64da6af +branch: default +latesttag: null +latesttagdistance: 1 diff --git a/lib/Utils/NeoPixel/.library.json b/lib/Utils/NeoPixel/.library.json new file mode 100644 index 0000000..de2da6a --- /dev/null +++ b/lib/Utils/NeoPixel/.library.json @@ -0,0 +1,38 @@ +{ + "description": "Simple neopixel (WS2812) library, tuned for stm32 (L432) at 80 MHz Should be compatible with any stm32, different clock speed may require timing adjustments in neopixel.c", + "repository": { + "url": "https://os.mbed.com/users/MightyPork/code/NeoPixel/", + "type": "hg" + }, + "platforms": [ + "atmelsam", + "freescalekinetis", + "maxim32", + "nordicnrf51", + "nordicnrf52", + "nxplpc", + "siliconlabsefm32", + "ststm32", + "teensy", + "wiznet7500" + ], + "frameworks": [ + "mbed" + ], + "version": "a81364d9a6", + "authors": [ + { + "url": "https://os.mbed.com/users/MightyPork/", + "maintainer": false, + "email": null, + "name": "Ond\u0159ej Hru\u0161ka" + } + ], + "keywords": [ + "stm32", + "ws2812b", + "neopixel" + ], + "id": 4280, + "name": "NeoPixel" +} \ No newline at end of file diff --git a/lib/Utils/NeoPixel/colorspace.cpp b/lib/Utils/NeoPixel/colorspace.cpp new file mode 100644 index 0000000..4cbe4db --- /dev/null +++ b/lib/Utils/NeoPixel/colorspace.cpp @@ -0,0 +1,163 @@ +#include "mbed.h" +#include "colorspace.h" +#include "math.h" + +#define max(a, b) ((a) > (b) ? (a) : (b)) +#define min(a, b) ((a) > (b) ? (b) : (a)) + +static float threeway_max(float a, float b, float c) { + return max(a, max(b, c)); +} + +static float threeway_min(float a, float b, float c) { + return min(a, min(b, c)); +} + +static float hue2rgb(float p, float q, float t) { + if (t < 0) + t += 1; + if (t > 1) + t -= 1; + if (t < 1 / 6.0f) + return p + (q - p) * 6 * t; + if (t < 1 / 2.0f) + return q; + if (t < 2 / 3.0f) + return p + (q - p) * (2 / 3.0f - t) * 6; + return p; +} + +void hsl2rgb(const FloatHSL* hsl, FloatRGB* rgb) { + float h = hsl->h; + float s = hsl->s; + float l = hsl->l; + + if (s == 0) { + rgb->r = rgb->g = rgb->b = l; // achromatic + } else { + float q = l < 0.5f ? l * (1 + s) : l + s - l * s; + float p = 2 * l - q; + rgb->r = hue2rgb(p, q, h + 1 / 3.0f); + rgb->g = hue2rgb(p, q, h); + rgb->b = hue2rgb(p, q, h - 1 / 3.0f); + } +} + +void hsv2rgb(const FloatHSV* hsv, FloatRGB* rgb) { + float r = 0, g = 0, b = 0; + + int i = floor(hsv->h * 6); + float f = hsv->h * 6 - i; + float p = hsv->v * (1 - hsv->s); + float q = hsv->v * (1 - f * hsv->s); + float t = hsv->v * (1 - (1 - f) * hsv->s); + + switch (i % 6) { + case 0: + r = hsv->v; + g = t; + b = p; + break; + case 1: + r = q; + g = hsv->v; + b = p; + break; + case 2: + r = p; + g = hsv->v; + b = t; + break; + case 3: + r = p; + g = q; + b = hsv->v; + break; + case 4: + r = t; + g = p; + b = hsv->v; + break; + case 5: + r = hsv->v; + g = p; + b = q; + break; + } + + rgb->r = r; + rgb->g = g; + rgb->b = b; +} + +void rgb2hsl(const FloatRGB* rgb, FloatHSL* hsl) { + float rd = rgb->r; + float gd = rgb->g; + float bd = rgb->b; + float max = threeway_max(rd, gd, bd); + float min = threeway_min(rd, gd, bd); + float h = 0, s, l = (max + min) / 2.0f; + + if (max == min) { + h = s = 0; // achromatic + } else { + float d = max - min; + s = l > 0.5f ? d / (2 - max - min) : d / (max + min); + if (max == rd) { + h = (gd - bd) / d + (gd < bd ? 6 : 0); + } else if (max == gd) { + h = (bd - rd) / d + 2; + } else if (max == bd) { + h = (rd - gd) / d + 4; + } + h /= 6; + } + hsl->h = h; + hsl->s = s; + hsl->l = l; +} + +/** Convert from HSV to HSL (this is possibly wrong) */ +void hsv2hsl(const FloatHSV* hsv, FloatHSL* hsl) { + float l = (2 - hsv->s) * hsv->v / 2.0f; + float s = hsv->s; + + if (l != 0) { + if (l == 1) { + s = 0; + } else if (l < 0.5f) { + s = s * hsv->v / (l * 2); + } else { + s = s * hsv->v / (2 - l * 2); + } + } + + hsl->h = hsv->h; + hsl->s = s; + hsl->l = l; +} + +/** Convert from HSL to HSV */ +void hsl2hsv(const FloatHSL* hsl, FloatHSV* hsv) { + float sat = hsv->s * ((hsl->l < 0.5f) ? hsl->l : (1 - hsl->l)); + hsv->s = 2 * sat / (hsl->l + sat); + hsv->h = hsl->h; + hsv->v = hsl->l + sat; +} + +uint32_t hsl2hex(const FloatHSL* hsl) { + FloatRGB rgb; + hsl2rgb(hsl, &rgb); + return rgb2hex(&rgb); +} + +uint32_t hsv2hex(const FloatHSV* hsv) { + FloatRGB rgb; + hsv2rgb(hsv, &rgb); + return rgb2hex(&rgb); +} + +uint32_t rgb2hex(FloatRGB* rgb) { + return ((int) floor(rgb->r * 255 + 0.5f) << 16) | ((int) floor(rgb->g * 255 + 0.5f) << 8) | + ((int) floor(rgb->b * 255 + 0.5f)); +} diff --git a/lib/Utils/NeoPixel/colorspace.h b/lib/Utils/NeoPixel/colorspace.h new file mode 100644 index 0000000..1e41ef6 --- /dev/null +++ b/lib/Utils/NeoPixel/colorspace.h @@ -0,0 +1,60 @@ +#ifndef HSL_H +#define HSL_H + +// +// Utilities for working with colors in different color spaces +// + +#include "mbed.h" + +// all values 0 to 1 + +// adapted from: https://github.com/ratkins/RGBConverter + +struct FloatRGB { + float r; + float g; + float b; +}; + +struct FloatHSL { + float h; + float s; + float l; +}; + +struct FloatHSV { + float h; + float s; + float v; +}; + +// --- Converting to RGB hex --- + +/** Convert HSL to RGB hex */ +uint32_t hsl2hex(const FloatHSL* hsl); + +/** Convert HSV to RGB hex */ +uint32_t hsv2hex(const FloatHSV* hsv); + +/** Convert RGB to RGB hex */ +uint32_t rgb2hex(FloatRGB* rgb); + +// --- Itner-space conversion functions --- + +/** Convert HSL to RGB */ +void hsl2rgb(const FloatHSL* hsl, FloatRGB* rgb); + +/** Convert RGB to HSL */ +void rgb2hsl(const FloatRGB* rgb, FloatHSL* hsl); + +/** Convert from HSV to HSL */ +void hsv2hsl(const FloatHSV* hsv, FloatHSL* hsl); + +/** Convert from HSL to HSV */ +void hsl2hsv(const FloatHSL* hsl, FloatHSV* hsv); + +/** Convert HSV to RGB ("handy" algo) */ +void hsv2rgb(const FloatHSV* hsv, FloatRGB* rgb); + +#endif /* HSL_H */ \ No newline at end of file diff --git a/lib/Utils/NeoPixel/library.json b/lib/Utils/NeoPixel/library.json new file mode 100644 index 0000000..001cbbc --- /dev/null +++ b/lib/Utils/NeoPixel/library.json @@ -0,0 +1,19 @@ +{ + "authors": { + "name": "Ondřej Hruška", + "url": "https://os.mbed.com/users/MightyPork/" + }, + "description": "Simple neopixel (WS2812) library, tuned for stm32 (L432) at 80 MHz Should be compatible with any stm32, different clock speed may require timing adjustments in neopixel.c", + "frameworks": "mbed", + "keywords": [ + "NeoPixel", + "stm32", + "WS2812B" + ], + "name": "NeoPixel", + "platforms": "*", + "repository": { + "type": "hg", + "url": "https://os.mbed.com/users/MightyPork/code/NeoPixel/" + } +} \ No newline at end of file diff --git a/lib/Utils/NeoPixel/neopixel.cpp b/lib/Utils/NeoPixel/neopixel.cpp new file mode 100644 index 0000000..d1375ae --- /dev/null +++ b/lib/Utils/NeoPixel/neopixel.cpp @@ -0,0 +1,94 @@ +#include "mbed.h" +#include "neopixel.h" + +NeoPixelOut::NeoPixelOut(PinName pin, bool normalize, double scale) : DigitalOut(pin) { + normalize = normalize; + global_scale = scale; +} + +// The timing should be approximately 800ns/300ns, 300ns/800ns +void NeoPixelOut::byte(register uint32_t byte) { + for (int i = 0; i < 8; i++) { + gpio_write(&gpio, 1); + // duty cycle determines bit value + if (byte & 0x80) { + // one + for (int j = 0; j < 73; j++) + __ASM("NOP\n\t"); + + gpio_write(&gpio, 0); + for (int j = 0; j < 36; j++) + __ASM("NOP\n\t"); + } else { + // zero + for (int j = 0; j < 36; j++) + __ASM("NOP\n\t"); + + gpio_write(&gpio, 0); + for (int j = 0; j < 73; j++) + __ASM("NOP\n\t"); + } + + byte = byte << 1; // shift to next bit + } +} + +void NeoPixelOut::send(Pixel* colors, uint32_t count, bool flipwait) { + // Disable interrupts in the critical section + __disable_irq(); + + Pixel* rgb; + float fr, fg, fb; + for (unsigned int i = 0; i < count; i++) { + rgb = colors++; + fr = (int) rgb->r; + fg = (int) rgb->g; + fb = (int) rgb->b; + + if (normalize) { + float scale = 255.0f / (fr + fg + fb); + fr *= scale; + fg *= scale; + fb *= scale; + } + + fr *= global_scale; + fg *= global_scale; + fb *= global_scale; + + if (fr > 255) + fr = 255; + if (fg > 255) + fg = 255; + if (fb > 255) + fb = 255; + if (fr < 0) + fr = 0; + if (fg < 0) + fg = 0; + if (fb < 0) + fb = 0; + +// Black magic to fix distorted timing +#ifdef __HAL_FLASH_INSTRUCTION_CACHE_DISABLE + __HAL_FLASH_INSTRUCTION_CACHE_DISABLE(); +#endif + + byte((int) fg); + byte((int) fr); + byte((int) fb); + +#ifdef __HAL_FLASH_INSTRUCTION_CACHE_ENABLE + __HAL_FLASH_INSTRUCTION_CACHE_ENABLE(); +#endif + } + + __enable_irq(); + + if (flipwait) + flip(); +} + +void NeoPixelOut::flip(void) { + wait_us(50); +} diff --git a/lib/Utils/NeoPixel/neopixel.h b/lib/Utils/NeoPixel/neopixel.h new file mode 100644 index 0000000..d368ea6 --- /dev/null +++ b/lib/Utils/NeoPixel/neopixel.h @@ -0,0 +1,65 @@ +#ifndef NEOPIXEL_H +#define NEOPIXEL_H +#include "mbed.h" + +/* +// Example + +NeoPixelOut npx(D12); + +int main() { + wait(0.2); // wait for HSE to stabilize + + npx.global_scale = 1.0f; // Adjust brightness + npx.normalize = true; // Equalize brightness to make r + g + b = 255 + + Pixel strip[6]; + strip[0].hex = 0xFF0000; + strip[1].hex = 0xFFFF00; + strip[2].hex = 0x00FF00; + strip[3].hex = 0x00FFFF; + strip[4].hex = 0x0000FF; + strip[5].hex = 0xFF00FF; + + npx.send(strip, 6); + + while(1); +} +*/ + +/** + * @brief Struct for easy manipulation of RGB colors. + * + * Set components in the xrgb.r (etc.) and you will get + * the hex in xrgb.num. + */ +union Pixel { + /** Struct for access to individual color components */ + struct __attribute__((packed)) { + uint8_t b; + uint8_t g; + uint8_t r; + uint8_t a; // unused + }; + + /** RGB color as a single uint32_t */ + uint32_t hex; +}; + +class NeoPixelOut : DigitalOut { + private: + void byte(uint32_t b); + + public: + bool normalize; + float global_scale; + + NeoPixelOut(PinName pin, bool normalize = false, double scale = 1.0); + + void send(Pixel* colors, uint32_t count, bool flipwait = true); + + /** Wait long enough to make the colors show up */ + void flip(void); +}; + +#endif /* NEOPIXEL_H */ \ No newline at end of file diff --git a/lib/Utils/NeoPixel/status.cpp b/lib/Utils/NeoPixel/status.cpp new file mode 100644 index 0000000..e5097f4 --- /dev/null +++ b/lib/Utils/NeoPixel/status.cpp @@ -0,0 +1,92 @@ +#include "status.h" + +static NeoPixelOut* neoPixel; +static Pixel* ledStrip; +static unsigned char numLeds; + +void Status::init(PinName pin, bool normalize, double scale, unsigned char NLed) { + numLeds = NLed; + + ledStrip = new Pixel[numLeds]; + neoPixel = new NeoPixelOut(pin, normalize, scale); + + for (int i = 0; i < numLeds; i++) { + ledStrip[i].hex = 0x000000; + } + neoPixel->send(ledStrip, numLeds); +} + +void Status::sendDefault() { + Pixel* ledStripp = new Pixel[numLeds]; + for (int i = 0; i < numLeds; i++) { + ledStripp[i].hex = 0xFF0000; + } + neoPixel->send(ledStripp, numLeds); +} + +void Status::send(int hexColor, LED_POSITION motor) { + if (motor <= DRIBBLER_LED) { + ledStrip[(int) motor].hex = hexColor; + } else { + for (int i = 0; i < numLeds; ++i) + ledStrip[i].hex = hexColor; + } + neoPixel->send(ledStrip, numLeds); +} + +void Status::send(CODE code, LED_POSITION motor) { + if (motor <= DRIBBLER_LED) { + ledStrip[(int) motor].hex = getColor(code); + } else { + setStripColor(code, ledStrip); + } +} +void Status::sendAllColor(int hex, int delay) { + clearColors(); + for (int j = 0; j < numLeds; j++) { + ledStrip[j].hex = hex; + neoPixel->send(ledStrip, numLeds); + } + ThisThread::sleep_for(chrono::milliseconds(delay)); + clearColors(); +} +void Status::sendRoundColor(int hex, int delay) { + for (int j = 0; j < numLeds; j++) { + clearColors(); + ledStrip[j].hex = hex; + neoPixel->send(ledStrip, numLeds); + ThisThread::sleep_for(chrono::milliseconds(delay)); + } + clearColors(); +} +void Status::clearColors() { + for (int i = 0; i < numLeds; ++i) { + ledStrip[i].hex = 0x000000; + } + neoPixel->send(ledStrip, numLeds); +} +int Status::getColor(CODE code) { + return 0x00FF00; +} + +void Status::setStripColor(CODE code, Pixel* ledStrip) { + int color = 0x000000; + for (int i = 0; i < numLeds; ++i) { + if (code == FAIL) { + color = 0xFF0000; + } else if (code == INIT) { + color = 0xFFFFFF; + } else if (code == NOT_CONNECT) { + color = 0xFF00FF; + } else if (code == CLEAR) { + color = 0x000000; + } else if (code == RUNNING) { + color = 0x00FF00; + } else if (code == BOOTING) { + color = 0xFFFF00; + } else + color = 0x000000; + + ledStrip[i].hex = color; + } +} diff --git a/lib/Utils/NeoPixel/status.h b/lib/Utils/NeoPixel/status.h new file mode 100644 index 0000000..371345c --- /dev/null +++ b/lib/Utils/NeoPixel/status.h @@ -0,0 +1,35 @@ +#ifndef STATUS_H +#define STATUS_H + +#include "mbed.h" +#include "neopixel.h" +#include "colorspace.h" + +enum LED_POSITION { + MOTOR_1_LED = 0, + MOTOR_2_LED, + MOTOR_3_LED, + MOTOR_4_LED, + DRIBBLER_LED, + ALL_LEDS +}; + +enum CODE { INIT = 0, FAIL, NOT_CONNECT, CLEAR, RUNNING, WAIT, BOOTING }; + +class Status { + public: + static void + init(PinName pin, bool normalize = false, double scale = 0.5, unsigned char NLeds = 5); + static void send(CODE status, LED_POSITION motor = ALL_LEDS); + static void send(int hexColor, LED_POSITION motor = ALL_LEDS); + static void sendAllColor(int hex, int delay); + static void sendRoundColor(int hexColor, int delay); + static void clearColors(); + static void sendDefault(); + + private: + static int getColor(CODE status); + static void setStripColor(CODE status, Pixel* strip); +}; + +#endif diff --git a/lib/Utils/defines.h b/lib/Utils/defines.h new file mode 100644 index 0000000..9037a88 --- /dev/null +++ b/lib/Utils/defines.h @@ -0,0 +1,22 @@ +#ifndef DEFINES_H +#define DEFINES_H + +#include +#include +#include +#include + +const char* ip = "199.0.1.1"; +const char* ip_pc = "199.0.1.2"; +const char* netmask = "255.255.0.0"; +const char* gateway = "199.199.0.0"; +const int port = 9600; +const int port_pc = 9601; + +NetworkType nrfNetwork; +uint8_t pipeNum = 0; +packetGeneric packetSend; +packetGeneric packetFull; +packetBStConfig packetConfig; + +#endif /* DEFINES_H */ \ No newline at end of file diff --git a/lib/Utils/mathematics.cpp b/lib/Utils/mathematics.cpp new file mode 100644 index 0000000..114e119 --- /dev/null +++ b/lib/Utils/mathematics.cpp @@ -0,0 +1,63 @@ +#include "mathematics.h" + +namespace math { + /** + * @brief Returns a max of X in the parameters bound. + * + * @param x + * @param in_min + * @param in_max + * @param out_min + * @param out_max + * @return double + */ + double map(double x, double in_min, double in_max, double out_min, double out_max) { + if (x <= in_min) + return out_min; + if (x >= in_max) + return out_max; + + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } + + /** + * @brief Returns Euclidian distace of two points. + * + * @param a + * @param b + * @return double + */ + double distance(Vector a, Vector b) { + return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2)); + } + + /** + * @brief Returns a angle between 0 and 2 * PI. + * + * @param angle + * @return double + */ + double to_positive_angle(double angle) { + return fmod(angle + 2 * M_PI, 2 * M_PI); + } + + /** + * @brief Returns the smalles difference of two angles. + * + * @param target + * @param source + * @return double + */ + double smallestAngleDiff(double target, double source) { + double a; + a = to_positive_angle(target) - to_positive_angle(source); + + if (a > M_PI) + a = a - 2 * M_PI; + + else if (a < -M_PI) + a = a + 2 * M_PI; + + return a; + } +} // namespace math diff --git a/lib/Utils/mathematics.h b/lib/Utils/mathematics.h new file mode 100644 index 0000000..49b4cad --- /dev/null +++ b/lib/Utils/mathematics.h @@ -0,0 +1,21 @@ +#ifndef MATHEMATICS_H +#define MATHEMATICS_H + +#include +#include + +#define M_PI 3.14159265358979323846 + +namespace math { + + double map(double x, double in_min, double in_max, double out_min, double out_max); + + double distance(Vector a, Vector b); + + double to_positive_angle(double angle); + + double smallestAngleDiff(double target, double source); + +} // namespace math + +#endif // MATHEMATICS_H diff --git a/lib/Utils/ports_v1.2.h b/lib/Utils/ports_v1.2.h new file mode 100644 index 0000000..d8cda91 --- /dev/null +++ b/lib/Utils/ports_v1.2.h @@ -0,0 +1,116 @@ +// NOTES +// PC_6 and PD_6 cannot be InterruptIn + +#define M1_PWM PD_12 // TIM4 (CH1) +#define M1_DIR PB_8 +#define M1_RST PE_15 +#define M1_MODE PE_14 +#define M1_CST PE_12 +#define M1_BRK PE_10 +// #define M1_ENCA PE_9 // TIM1 (CH1) +// #define M1_ENCB PF_13 +#define M1_FF1 PF_14 +#define M1_FF2 PE_11 + +#define M2_PWM PD_13 // TIM4 (CH2) +#define M2_DIR PB_11 +#define M2_RST PE_15 +#define M2_MODE PE_14 +#define M2_CST PE_12 +#define M2_BRK PE_10 +// #define M2_ENCA PE_5 // TIM9 (CH1) +// #define M2_ENCB PE_7 +#define M2_FF1 PG_9 +#define M2_FF2 PG_14 + +#define M3_PWM PD_14 // TIM4 (CH3) +#define M3_DIR PC_0 +#define M3_RST PE_15 +#define M3_MODE PE_14 +#define M3_CST PE_12 +#define M3_BRK PE_10 +// #define M3_ENCA PC_6 // TIM3 (CH1) +// #define M3_ENCB PB_15 +#define M3_FF1 PF_5 +#define M3_FF2 PF_10 + +#define M4_PWM PD_15 // TIM4 (CH4) +#define M4_DIR PC_2 +#define M4_RST PE_15 +#define M4_MODE PE_14 +#define M4_CST PE_12 +#define M4_BRK PE_10 +// #define M4_ENCA PF_8 // TIM13 (CH1) +#define M4_FF1 PD_5 +#define M4_FF2 PD_4 + +#define M5_PWM PF_9 // TIM14 (CH1) +#define M5_DIR PD_0 +#define M5_RST PB_13 +#define M5_MODE PB_12 +#define M5_CST PA_15 +#define M5_BRK PC_9 // * +// #define M5_FF1 PB_2 +// #define M5_FF2 PE_3 +#define M5_TAC PD_7 // * +#define M5_DIRO PF_2 // * + +#define PIN_CHARGE PE_0 +#define PIN_CAP_LOAD PA_3 +#define PIN_CHIP PG_3 +#define PIN_FRONT PB_0 +#define PIN_FAULT PD_11 + +#define NRF_R_MOSI PC_12 +#define NRF_R_MISO PC_11 +#define NRF_R_SCK PC_10 +#define NRF_R_CSN PG_0 +#define NRF_R_CE PF_3 +#define NRF_R_IRQ PH_1 +#define NRF_R_VCC PF_15 + +#define NRF_S_MOSI PC_12 +#define NRF_S_MISO PC_11 +#define NRF_S_SCK PC_10 +#define NRF_S_CSN PF_12 +#define NRF_S_CE PE_13 +#define NRF_S_IRQ PG_1 +#define NRF_S_VCC PA_0 + +// #define MPU_MOSI PC_3 // PA_7 // TIM1 +// #define MPU_MISO PB_14 // PA_6 // TIM3 +// #define MPU_SCK PB_10 // PA_5 // TIM2 +// #define MPU_CS PD_2 +// #define MPU_IRQ PG_2 + +// ******* EXTERNAL MPU 6050 ****** +// Needs to cut PF_0 PIN in main board!!! +#define MPU_SDA PF_0 // I2C +#define MPU_SCL PF_1 // I2C + +#define PIN_BUZZER PF_7 // TIM11 (CH1) +#define PIN_RGB PD_1 +#define PIN_BATT PF_4 + +#define PIN_IR_LED PB_4 +#define PIN_IR_ADC PA_4 +// #define PIN_TEMP PC_2 + +#define PIN_SELECTOR_1 PE_6 +#define PIN_SELECTOR_2 PE_2 +#define PIN_SELECTOR_3 PD_3 +#define PIN_SELECTOR_4 PE_4 + +#define PIN_PB1 PB_1 +#define PIN_PB2 PB_2 + +#define PIN_CURR_REF PC_3 +#define PIN_CURR_OUT PF_5 + +// Current Sensor +#define PIN_CURR_REF PC_3 +#define PIN_CURR_OUT PF_5 + +#define CURRENT_SENSOR_A0 PD_5 +#define CURRENT_SENSOR_A1 PD_6 +#define CURRENT_SENSOR_A2 PD_7 \ No newline at end of file diff --git a/lib/Utils/utils.cpp b/lib/Utils/utils.cpp new file mode 100644 index 0000000..87fbb1e --- /dev/null +++ b/lib/Utils/utils.cpp @@ -0,0 +1,153 @@ +#include "utils.h" + +namespace utils { + double param_angular_pos = 6.222; + double param_angular_neg = 6.222; + double param_linear_pos = 57.71; + double param_linear_neg = -60.51; + + // To control M5 + // 9.4 - 22 PWM == -13.44 ~ -314.159 RAD/S + double param_linear_r1 = 210.9059206; + double param_angular_r1 = -23.8665873; + // 22 - 38 PWM == -314.159 ~ -688.158 RAD/S + double param_linear_r2 = 200.089625; + double param_angular_r2 = -23.3749375; + // 38 - 53 PWM == -688.158 ~ -1002.318 RAD/S + double param_linear_r3 = 107.714; + double param_angular_r3 = -20.944; + // 53 - 73 PWM == -1002.318 ~ -1436.150 RAD/S + double param_linear_r4 = 127.3368; + double param_angular_r4 = -20.600; + // 73 PWM == -1436.150 RAD/S + double param_linear_r5 = 140.3368; + double param_angular_r5 = -20.320; + + DigitalIn PBT1(PIN_PB1); + DigitalIn PBT2(PIN_PB2); + DigitalIn SW1(PIN_SELECTOR_1); + DigitalIn SW2(PIN_SELECTOR_2); + DigitalIn SW3(PIN_SELECTOR_3); + DigitalIn SW4(PIN_SELECTOR_4); + PwmOut Buzzer(PIN_BUZZER); + + AnalogIn BATT(PIN_BATT); + + void initRobot() { + printf("Booting Robot....id %d\n", getRobotId()); + +#ifdef USING_NEW_MOTOR + Buzzer.period_us(DEFAULT_BUZZER_CYCLE); + Status::init(PIN_RGB); + Status::send(Color::BLUE, DRIBBLER_LED); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + ThisThread::sleep_for(80ms); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + ThisThread::sleep_for(80ms); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + Status::clearColors(); +#else + printf("! Old motor control loaded !\n"); + Buzzer.period_us(390); + Status::init(PIN_RGB); + Status::send(Color::BLUE, DRIBBLER_LED); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + ThisThread::sleep_for(80ms); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + ThisThread::sleep_for(80ms); + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendAllColor(Color::GREEN, 80); + Buzzer.write(0); + Status::clearColors(); +#endif + } + + bool timerMillisExpired(const Timer& timer, double time) { + return timerRead(timer) > time; + } + + double timerMillisRead(const Timer& timer) { + return timerRead(timer); + } + + ButtonState pressedFor(DigitalIn button, double time) { + if (button) + return ButtonState::UNPRESSED; + + Timer timer; + timer.start(); + + while (!button) { + if (timerMillisExpired(timer, time)) { + utils::beep(180); + return ButtonState::HELD; + } + } + return ButtonState::RELEASED; + } + + // New line equation to control M1, M2, M3, M4 + double rad_s_to_pwm_pi(double rad_s) { + double param_angular = param_angular_pos; + double param_linear = param_linear_pos; + + if (fabs(rad_s) < MOTOR_MIN_SPEED_RAD_S) { + return 0; + } + if (rad_s < 0) { + param_angular = param_angular_neg; + param_linear = param_linear_neg; + } + return (param_angular * rad_s) + param_linear; + } + + double last_battery = ((BATT.read() * 3.3) * 54) / 10.0; + double getBattery() { + last_battery = (last_battery + (((BATT.read() * 3.3) * 54) / 10.0)) / 2.0; + return last_battery; + } + void checkBattery() { + while (utils::getBattery() < 15.5) { + Buzzer.write(DEFAULT_BUZZER_FREQUENCY); + Status::sendRoundColor(Color::RED, 100); + Buzzer.write(0); + ThisThread::sleep_for(500ms); + } + } + int getRobotId() { + unsigned int _robotId = 0; + _robotId += (SW1.read()); + _robotId += (SW2.read() * 2); + _robotId += (SW3.read() * 4); + _robotId += (SW4.read() * 8); + if (_robotId <= 15) + return _robotId; + else + return 16; + } + void beep(int time, int cycleUs, double frequency) { + bool cycleUpdated = false; + if (cycleUs != DEFAULT_BUZZER_CYCLE) { + Buzzer.period_us(cycleUs); + cycleUpdated = true; + } + + Buzzer.write(frequency); + ThisThread::sleep_for(chrono::milliseconds(time)); + Buzzer.write(0); + + if (cycleUpdated) { + Buzzer.period_us(DEFAULT_BUZZER_CYCLE); + } + } +} // namespace utils diff --git a/lib/Utils/utils.h b/lib/Utils/utils.h new file mode 100644 index 0000000..90bdfdb --- /dev/null +++ b/lib/Utils/utils.h @@ -0,0 +1,145 @@ +#ifndef UTILS_H +#define UTILS_H + +#define DIRECT_TRANSMISSION + +#define USING_NEW_MOTOR + +#include "mathematics.h" +#include +#include +#include +#include +#include + +#define DEBUG false +#define DEFAULT_PCKT + +#define NAVIGATION_MAX_LINEAR_ACCEL 2.4 +#define NAVIGATION_BREAK_DECAY 2.11 +#define NAVIGATION_DECAY 0.09 +#define NAVIGATION_CYCLE_STEP 0.16 + +#define DEFAULT_TOLERANCE_TO_TARGET 0.035 +#define SMALL_TOLERANCE_TO_TARGET 0.01 + +#define NRF_NO_MSG_TIME 300 +#define NRF_HALT_TIME 1000 +#define DEFAULT_FEEDBACK_TIME 100 +#define ODOMETRY_TIME 100 + +#define GYRO_TOLERANCE 0.1 + +#define DRIBLER_BAR_GEAR 50 +#define DRIBLER_GEAR 26 +#define MIN_DRIBBLER_SPEED 0 +#define MAX_DRIBBLER_SPEED 720 +#define DEFAULT_DRIBLER_SPEED (-50) +#define MOTOR_MIN_SPEED_RAD_S 1.567 +#define STD_MAX_PWM_ALLOWED 80.0 +#define LOCKED_MAX_PWM_ALLOWED 30.0 + +#define ROBOT_ACCEL 1 + +#if defined(DIRECT_TRANSMISSION) + #define MOTOR_GEAR 1 // Direct transmission + #define WHEEL_GEAR 1 // Direct transmission +#else + #define MOTOR_GEAR 18.0 + #define WHEEL_GEAR 60.0 +#endif + +#define DEFAULT_BUZZER_CYCLE 370 +#define DEFAULT_BUZZER_FREQUENCY 0.5 +#define MAX_CHARGE 0.75 +#define MAX_CHARGE_MARGIN 0.05 +#define DEFAULT_MIN_CHARGE 0.65 +#define BALL_PLACEMENT_MIN_CHARGE 0.3 +#define MAX_CAP_V_LOAD 2.8 +#define IR_THRESHOLD 0.15 +#define RANGINGSENSOR_THRESHOLD 80 +#define KICKER_TIMEOUT 500 +#define NO_LOAD 0.1 + +#define ANGLE_KP 2.0 + +#define GYRO_TSAMPLE 5 +#define ODOMETRY_TSAMPLE 5 + +enum Color { + RED = 0xFF0000, + GREEN = 0x00FF00, + BLUE = 0x0000FF, + YELLOW = 0xFFFF00, + CYAN = 0x00FFFF, + PINK = 0xFF00FF, + CLEAN = 0x000000, + WHITE = 0xFFFFFF, +}; + +enum class ButtonState { + UNPRESSED, + RELEASED, + HELD +}; + +enum class ExecMode { + GAME_ON, + TEST, + VISION_BLACKOUT, + TEST_BASICS, + TEST_PWM, + TEST_PID, + TEST_KICK, + TEST_DRIBBLER, + TEST_ODOMETRY, + TEST_MPU, + TEST_IR, + TEST_RECEIVER_BALL, + TEST_RANGING_SENSOR, + TEST_CURRENT, + GET_CURRENT, +}; + +namespace utils { + + extern double last_battery; + void initRobot(); + double rad_s_to_pwm(double rad_s); + double rad_s_to_pwm_pi(double rad_s); + double pwm_to_rad_s(double pwm); + double getBattery(); + int getRobotId(); + void checkBattery(); + void + beep(int time, int cycleUs = DEFAULT_BUZZER_CYCLE, double frequency = DEFAULT_BUZZER_FREQUENCY); + bool shouldStop(double motorSpeed, double haltSpeed); + + /** Note: Use this function to make the code more readable + * and prioritize the milliseconds unit for time conditionals + * @param ToDuration Reading time unit, based on the chrono namespace types + * @param T Type of variable returned, when omitted the default is to return in + * float + * @param timer Timer for reading + */ + template + T timerRead(const Timer& timer) { + return chrono::duration_cast>( + timer.elapsed_time()) + .count(); + } + bool timerMillisExpired(const Timer& timer, double time); + double timerMillisRead(const Timer& timer); + ButtonState pressedFor(DigitalIn button, double time); + + extern DigitalIn PBT1; + extern DigitalIn PBT2; + extern DigitalIn SW1; + extern DigitalIn SW2; + extern DigitalIn SW3; + extern DigitalIn SW4; + extern PwmOut Buzzer; + extern AnalogIn BATT; +} // namespace utils + +#endif diff --git a/lib/nRF24Communication/.gitignore b/lib/nRF24Communication/.gitignore new file mode 100644 index 0000000..496ee2c --- /dev/null +++ b/lib/nRF24Communication/.gitignore @@ -0,0 +1 @@ +.DS_Store \ No newline at end of file diff --git a/lib/nRF24Communication/README.md b/lib/nRF24Communication/README.md new file mode 100644 index 0000000..f80de12 --- /dev/null +++ b/lib/nRF24Communication/README.md @@ -0,0 +1,16 @@ +# communication-embedded + +It's a library that provides a high-level interface to communicate soccer robots with a control station using nRF24l01+ module. + +## Notes +Every update here affects the reposities below and its' dependents: +- [base-station](https://github.com/robocin/base-station/). +- [ssl-embedded](https://github.com/robocin/ssl-embedded/). +- [communication-software](https://github.com/robocin/communication-software/). + +So, whenever changes are made at the `master`, go to the dependents repository and update the submodule: + + git submodule update --init --recursive + +Make sure to update the changes into the dependent repository. + diff --git a/lib/nRF24Communication/archive-readme.md b/lib/nRF24Communication/archive-readme.md new file mode 100644 index 0000000..cbfad3a --- /dev/null +++ b/lib/nRF24Communication/archive-readme.md @@ -0,0 +1,266 @@ +# Biblioteca nRF24Communication + Abaixo tem-se a documentação da biblioteca nRF24Communication, onde usamos as funções da biblioteca [Biblioteca RF24](http://tmrh20.github.io/RF24/) aplicado a robótica. + +# Um exemplo de código +No código a seguir você configura um nrf que recebe mensagens: + +```cpp +#include // incluindo a lib + +bool justUpdated = false; +nRF24Communication radio = nRF24Communication(A0,10); // parâmetros pinCE,pinCS - portas do nRF24L01 + + +void setup(){ + radio.setup(1, 400); // configura o radio com id 1 e tempo de espera máximo entre uma mensagem e outra, fica configurado para receber mensagem + Serial.begin(9600); + +} + + +void loop(){ + justUpdated = radio.updateInfo(); // verifica se tem mensagem nova + + if(justUpdated){ + Serial.println(radio.getLeftMotorSpeed()); + Serial.println(radio.getRightMotorSpeed()); + } + +} + +``` + +# Fluxo Do Código +![Fluxo do código](../imgs/robo_comm.png) + +# Tipos de Mensagens +Atualmente temos **seis** tipos de mensagens mais **um** tipo básico usado para indentificação geral das mensagens. + +**>> TypeSpeed** + +Tipo de mensagem que envia para o robô as velocidades dos seus motores e pode pedir para o robô retornar algum dado através do campo flags. + +| Campos | Quantidade de Bits| Função | +|-----------|-----------------------|-------------------------------------------------------------------------| +|typeMsg |`4 bits` |Indentificar o tipo de mensagem | +|id |`4 bits` |Indentificar para que robô a mensagem está sendo enviada | +|leftSpeed |`8 bits` | Velocidade do motor esquerdo | +|rightSpeed |`8 bits` | Velocidade do motor direito | +|flags |`8 bits` | Tipos de retorno que o robô pode devolver | + +**>> TypePositions** + +| Campos | Quantidade de Bits| Função | +|-----------|-----------------------|---------------------------------------| +|typeMsg |`4 bits` |Indentificar o tipo de mensagem | +|id |`4 bits` |Indentificar para que robô a mensagem está sendo enviada | +|curPosX |`8 bits` | Posição X atual do robô| +|curPosY |`8 bits` | Posição Y atual do robô| +|curAngle |`16 bits` | Ângulo atual do robô| +|objPosX |`8 bits` | Posição X objetivo do robô| +|objPosY |`8 bits` | Posição Y objetivo do robô| +|objAngle |`16 bits` | Ângulo objetivo do robô| +|flags |`8 bits` | Tipos de retorno que o robô pode devolver| + +**>> TypePID** + +| Campos | Quantidade de Bits| Função | +|-----------|-----------------------|---------------------------------------| +|typeMsg |`4 bits` |Indentificar o tipo de mensagem | +|id |`4 bits` |Indentificar para que robô a mensagem está sendo enviada | +|kp |`16 bits` | Constante proporcional| +|ki |`16 bits` | Constante integral| +|kd |`16 bits` | Constante derivativo| +|alpha |`16 bits` | Aceleração| +|flags |`8 bits` | Tipos de retorno que o robô pode devolver| + +**>> TypeRetPosition** + +| Campos | Quantidade de Bits| Função | +|-----------|-----------------------|---------------------------------------| +|typeMsg |`4 bits` |Indentificar o tipo de mensagem | +|id |`4 bits` |Indentificar para que robô a mensagem está sendo enviada | +|curPosX |`8 bits` | Posição X atual do robô| +|curPosY |`8 bits` | Posição Y atual do robô| +|curAngle |`16 bits` | Ângulo atual do robô| +|end |`8 bits` | Final de mensagem | + +**>> TypeRetBattery** + +| Campos | Quantidade de Bits| Função | +|-----------|-----------------------|---------------------------------------| +|typeMsg |`4 bits` |Indentificar o tipo de mensagem | +|id |`4 bits` |Indentificar para que robô a mensagem está sendo enviada | +|batteryLevel |`8 bits` | Nível da Bateria| +|curAngle |`16 bits` | Ângulo atual do robô| +|end |`8 bits` | Final de mensagem | + +# Construção dos Tipos +Nessa seção explicaremos como é feita a construção de um tipo e como você poderá criar um novo tipo. + +Para começar usemos como exemplo o tipo **TypeSpeed**: +```cpp + typedef struct typeSpeed{ + uint8_t typeMsg:4; + uint8_t id:4; + int8_t leftSpeed:8; + int8_t rightSpeed:8; + uint8_t flags:8; + } __attribute__((packed)) TypeSpeed; + + typedef union msgSpeed{ + unsigned char encoded[4]; + TypeSpeed decoded; + } MsgSpeed; +``` +Inicialmente criamos uma struct e usamos o conceito de [Bit Field](http://en.cppreference.com/w/cpp/language/bit_field) onde conseguimos declarar um tipo com uma quantidade restrita de bits, portanto para cada variavél existente na struct é determinado o número de bits que vamos usar nesta e por fim a struct funciona como um grande tipo de dados com uma quantidade de bits. A flag "__attribute__((packed))" evita com que o gcc complete bits, ou seja, dessa forma dizemos ao compilador que ele não precisa preecher com bits aleatórios pois queremos aquele tipo específico. + +Portanto cabe agora explicar o uso do union, como o [nrf24l01](http://tmrh20.github.io/RF24/classRF24.html#a4cd4c198a47704db20b6b5cf0731cd58) na biblioteca base que usamos para envio usa-se ponteiro o que fazemos nesso ponto é transformar nosso tipo recém criado em um ponteiro através da união. Para isso declaramos dentro do union uma variável com o tipo da struct que criamos e uma variavél que é um vetor com a quantidade de bytes que usamos.Assim, quando modificamos um o outro é modificado consequentemente. + +```cpp + . + . + . + MsgSpeed M1; + + M1.decoded.typeMsg = 10; + M1.decoded.id = 2; + + // Isso é equivalente ao que eu fiza acima: + M1.encoded [1] = 162; + + // 10 = 1010 + // 02 = 0010 + // 162 = 10100010 | Em um byte temos o typeMsg+id + . + . + . + +``` + +# Funções +Atualmente, temos as seguintes funções na biblioteca de comunicação: + +**>> nRF24Communication(int pinCE, int pinCSN)** + +Construtor da classe, recebe como parâmetros o pino do CE e do CSN. +Inicializa as velocidades dos motores como 0, e define flags como falsas. +Define os pinos do CE e CSN a partir dos parâmetros de entrada. + +**>> setup(int roboSwitches, unsigned long watchdog_millis_time)** + +Função de setar os parâmetros iniciais. +Chama uma instância de RF24. +Atualiza o ID do robô baseado no switch (updateRobotId()). +Atualiza o momento da última recepção de informações (momento atual), e define essa atualização como true. +Atualiza o tempo do watchdog a partir do parâmetro de entrada. +Chama a função para configurar a comunicação RF(_configure()). + +**>> updateInfo()** + +Começa a ouvir pelo NRF. +Caso o chip esteja conectado, enquanto tiver informação disponível, lê as informações recebidas pelo NRF e as salva em uma variável codificada. Também salva o tipo de mensagem. +Se a mensagem for para esse robô, dependendo do tipo de mensagem, atualiza as informações do robô: velocidade (dos dois motores), posição (coordenadas atuais do robô e da bola), e PID (kp, kd, ki e alpha). +Atualiza também o tempo de recepção da última informação. +Caso o chip não esteja conectado, pára de ouvir para reconfigurar, e na próxima vez tenta novamente. + +**>> sendInfo(const void * buf,uint8_t len)** + +Para de ouvir, e manda informação pelo NRF. + +**>> getLeftMotorSpeed()** + +Retorna a velocidade do motor esquerdo. + +**>> getRightMotorSpeed()** + +Retorna a velocidade do motor direito. + +**>> getCurX()** + +Retorna a coordenada X atual do robô. + +**>> getCurY()** + +Retorna a coordenada Y atual do robô. + +**>> getCurAngle()** + +Retorna o ângulo atual do robô. + +**>> getObjX()** + +Retorna a coordenada X atual da bola. + +**>> getObjY()** + +Retorna a coordenada Y atual da bola. + +**>> getObjAngle()** + +Retorna o ângulo atual da bola. + +**>> shallSendBatteryLevel()** + +Retorna o booleano que diz deve mandar ou não o nível da bateria, de acordo com as últimas flags recebidas. + +**>> updateRobotId(int roboSwID)** + +Atualiza o ID do robô a partir da informação do switch. + +**>> getRobotId()** + +Retorna o ID do robô. + +**>> sendBatteryLevel(float battery)** + +Pega o nível de bateria do robô e manda para o PC através do NRF. + +**>> sendPositions(int PosX,int PosY, float Angle)** + +Pega as informações da posição do robô. + +**>> timeupWatchdog()** + +Diz se o tempo que passou desde a última atualização passou o tempo limite estabelecido. + +**>> getTypeOfMessage()** + +Retorna o tipo de mensagem. + +**>> getKP()** + +Retorna o parâmetro kp do PID. + +**>> getKD()** + +Retorna o parâmetro kd do PID. + +**>> getAlpha()** + +Retorna o parâmetro alpha do PID. + +# Funções Privadas +As funções privadas da biblioteca de comunicação são: + +**>> _configure()** + +Configura os parâmetros da comunicação RF, utilizando as funções da biblioteca RF24: setPALevel, setChannel, setAutoAck, setPayloadSize, openWritingPipe, openReadingPipe. +Começa a receber pelo NRF (_receive()); + +**>> _receive()** + +Começa a ouvir pelo NRF, através da própria função do RF24: startListening. + + +**>> _send()** + +Pára de ouvir pelo NRF para mandar, através da função do RF24: stopListening. + +**>> _setFlags(uint8_t value)** + +Atualiza as flags de mandar nível da bateria, mandar posições, “kick” e “dribbler”. + +**>> _setFlagsToFalse()** + +Coloca todas as flags para “false”. diff --git a/lib/nRF24Communication/commConfig.h b/lib/nRF24Communication/commConfig.h new file mode 100644 index 0000000..5f46182 --- /dev/null +++ b/lib/nRF24Communication/commConfig.h @@ -0,0 +1,297 @@ +#ifndef COMM_CONFIG_H +#define COMM_CONFIG_H + +// v3.1 + +#include +#include + +/* +Base frequency 2400 +Highest frequency 2525 +Channels 126 + +Channel to frequency = 2400 + CHANNEL_NUM +*/ + +// NRF Config Defitions +#define SSL_1_BASE_SEND_CH 103 +#define SSL_2_BASE_RECV_CH 114 + +#define SSL_1_ROBOT_RECV_CH 103 +#define SSL_2_ROBOT_SEND_CH 114 + +#define SSL_ADDR_1 0xABAAADA99ALL +#define SSL_ADDR_2 0xADAADC9A4BLL + +#define VSS_BASE_RECV_CHANNEL 110 +#define VSS_BASE_SEND_CHANNEL 108 + +#define VSS_ROBOT_RECV_CHANNEL 108 +#define VSS_ROBOT_SEND_CHANNEL 110 + +#define VSS_ADDR_1 0x752FAF0A9ALL +#define VSS_ADDR_2 0x5D4BFBC2BBLL + +#define ACK_RADIO 0 +#define NRF_MAX_PAYLOAD 32 + +// PAYLOAD DEFINITIONS +#define BST_CONFIG_LENGTH 21 + +#define VSS_PAYLOAD_LENGTH 7 +#define VSS_SPEED_LENGTH VSS_PAYLOAD_LENGTH +#define VSS_TELEMETRY_LENGTH VSS_PAYLOAD_LENGTH + +#define SSL_PAYLOAD_LENGTH DEFAULT_PAYLOAD_LENGTH // 15 // +#define SSL_SPEED_LENGTH DEFAULT_PAYLOAD_LENGTH // 12 // +#define POSITION_LENGTH DEFAULT_PAYLOAD_LENGTH // 9 // +#define TELEMETRY_LENGTH DEFAULT_PAYLOAD_LENGTH // 13 // +#define ODOMETRY_LENGTH DEFAULT_PAYLOAD_LENGTH // 11 // + +#define DEFAULT_PAYLOAD_LENGTH 20 + +#pragma pack(push, 1) + +enum class msgType { + NONE = -1, + BST_CONFIG, + VSS_SPEED, + SSL_SPEED, + TELEMETRY, + VSS_TELEMETRY, + ODOMETRY, + POSITION, + SSL_MOTORS_PWM, + BST_REQ_CONFIG +}; + +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int64_t rest_a : 64; + int64_t rest_b : 64; + int64_t rest_c : 64; + int64_t rest_d : 56; +} packetTypeGeneric; + +typedef union { + unsigned char encoded[NRF_MAX_PAYLOAD]; + packetTypeGeneric decoded; +} packetGeneric; + +/** + * Structure to configure the Base Station, + * This type sends: + * - Message type + * - Duplex communication? + * - Team (follow NetworkType) + * - nRF Address 1 + * - nRF Address 2 + * - Payload of nRF messages + * - Channel of nRF send. + * - Channel of nRF recv. + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + bool duplex : 1; + uint8_t team : 4; + uint64_t addr1 : 64; + uint64_t addr2 : 64; + uint8_t payload : 8; + uint8_t channel1 : 8; + uint8_t channel2 : 8; + uint8_t free : 3; + +} packetTypeBStConfig; + +typedef union { + unsigned char encoded[BST_CONFIG_LENGTH]; + packetTypeBStConfig decoded; +} packetBStConfig; + +/** + * Structure for sending speeds to bi-directional robot, + * This type sends: + * - Message type + * - Robot Id + * - The left and right motor speeds + * - + * - One byte of free for optional flags. + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int32_t m1 : 18; // (-131.072 <-> 131.071 rad/s or -1.31072 <-> 1.31072 pwm) + // (clamp(-1.00000, 1.00000) // left motor speed + int32_t m2 : 18; // (-132.072 <-> 131.071 rad/s or -1.31072 <-> 1.31072 pwm) + // (clamp(-1.00000, 1.00000) // right motor speed + bool isPWM : 1; // Bit indication for speed type (00 -> rad/s, 01 -> pwm) + uint16_t free : 11; + +} packetTypeSpeedVSS; + +typedef union packetSpeedVSS { + unsigned char encoded[VSS_SPEED_LENGTH]; + packetTypeSpeedVSS decoded; +} packetSpeedVSS; + +/* + * Structure for sending speeds omni-directional robot, + * This type sends: + * - Message type + * - Robot Id + * - Vx, Vy and Vw of the robot. + * - Type and Strength of Kick. + * - Dribbler flag and its speed. + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int32_t vx : 20; + int32_t vy : 20; + int32_t vw : 20; + uint8_t front : 1; + uint8_t chip : 1; + uint8_t charge : 1; + uint8_t kickStrength : 8; + uint8_t dribbler : 1; + int16_t dribblerSpeed : 11; + uint8_t command : 8; + uint8_t robotLockedToMove : 1; + uint8_t criticalMoveTurbo : 1; + uint64_t free_1 : 59; + +} packetTypeSpeedSSL; + +typedef union packetSpeedSSL { + unsigned char encoded[SSL_SPEED_LENGTH]; + packetTypeSpeedSSL decoded; +} packetSpeedSSL; + +/* + * Structure for sending position to a robot, + * This type sends: + * - Message type + * - Robot Id + * - x, y and w of the robot. + * - Source or Destiny position. + * - .... + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int16_t x : 16; // -32.767 - 32.767 m + int16_t y : 16; // -32.767 - 32.767 m + int16_t w : 16; // 0 - 6.5535 rad + // motion parameters + uint16_t maxSpeed : 12; // 0 - 4.095 m/s + uint16_t minSpeed : 12; // 0 - 4.095 m/s + uint16_t rotateKp : 10; // 0 - 10.23 + uint8_t usingPropSpeed : 1; + uint16_t minDistanceToPropSpeed : 12; // 0 - 4.095 m + uint8_t clockwise : 1; // In goToPoint it is High Acceleration flag + uint16_t orbitRadius : 12; // 0 - 4.095 m // Custom Acceleration in goToPoint + uint16_t approachKp : 10; // 0 - 10.23 + uint8_t positionType : 3; + // Kick Options + uint8_t front : 1; + uint8_t chip : 1; + uint8_t charge : 1; + uint8_t strength : 8; + uint8_t dribbler : 1; + int16_t dribblerSpeed : 11; + uint8_t command : 8; +} packetTypePosition; + +typedef union packetPosition { + unsigned char encoded[POSITION_LENGTH]; + packetTypePosition decoded; +} packetPosition; + +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + uint16_t current_m1 : 12; // 0 - 50.00 A + uint16_t current_m2 : 12; // 0 - 50.00 A + uint16_t current_m3 : 12; // 0 - 50.00 A + uint16_t current_m4 : 12; // 0 - 50.00 A + int16_t dribbler : 15; // -1638.3 - 1638.3 rad/s + uint8_t kickLoad : 8; // 0 - 2.55 + bool ball : 1; // 0 | 1 + uint8_t battery : 8; // 0 - 25.5 V + int16_t m1 : 16; // -327.67 - 327.67 m/s + int16_t m2 : 16; // -327.67 - 327.67 m/s + int16_t m3 : 16; // -327.67 - 327.67 m/s + int16_t m4 : 16; // -327.67 - 327.67 m/s + uint8_t pcktCount : 8; + +} packetTypeTelemetry; + +typedef union packetTelemetry { + unsigned char encoded[TELEMETRY_LENGTH]; + packetTypeTelemetry decoded; +} packetTelemetry; + +/** + * Structure for sending speeds to bi-directional robot, + * This type sends: + * - Message type + * - Robot Id + * - The left and right motor speeds + * - One byte of free for optional flags. + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int32_t m1 : 18; + int32_t m2 : 18; + uint8_t battery : 8; // 0 - 12.8 V + uint8_t free : 4; + +} packetTypeVSSTelemetry; + +typedef union packetVSSTelemetry { + unsigned char encoded[VSS_TELEMETRY_LENGTH]; + packetTypeVSSTelemetry decoded; +} packetVSSTelemetry; + +/* + * Structure for send robot basic status and position, + * This type sends: + * - Message type + * - Robot Id + * - X, Y and W of the robot. + * - Dribbler speed and its speed. + * - Kick capacitor load. + * - Ball on robot? + * - Battery load. + */ +typedef struct { + uint8_t typeMsg : 4; + uint8_t id : 4; + int16_t x : 16; // -32.767 - 32.767 m + int16_t y : 16; // -32.767 - 32.767 m + int16_t w : 16; // 0 - 6.5535 rad + int16_t dribbler : 15; // -1638.3 - 1638.3 rad/s + uint8_t kickLoad : 8; // 0 - 2.55 + bool ball : 1; + uint8_t battery : 8; // 0 - 25.5 V + int16_t m1 : 16; // -327.67 - 327.67 m/s + int16_t m2 : 16; // -327.67 - 327.67 m/s + int16_t m3 : 16; // -327.67 - 327.67 m/s + int16_t m4 : 16; // -327.67 - 327.67 m/s + uint8_t pcktCount : 8; + +} packetTypeOdometry; + +typedef union packetOdometry { + unsigned char encoded[ODOMETRY_LENGTH]; + packetTypeOdometry decoded; +} packetOdometry; + +// restoring the standard alignment +#pragma pack(pop) + +#endif // COMM_CONFIG_H diff --git a/lib/nRF24Communication/commTypes.h b/lib/nRF24Communication/commTypes.h new file mode 100644 index 0000000..e0406b2 --- /dev/null +++ b/lib/nRF24Communication/commTypes.h @@ -0,0 +1,179 @@ +#ifndef COMM_TYPES_H +#define COMM_TYPES_H + +/************* AUXILIAR TYPES ************/ +#include "commConfig.h" +#include + +enum class RadioFunction { receiver, sender }; + +/* + * Network type / catergory configuration + * Attencion: Limited in 15 + */ +enum class NetworkType { unknown = 0, generic, ssl, vss }; + +/* + * Type of the packet position. + * Attention: Limited in 8 + */ +enum class PositionType { unknown = 0, source, stop, motionControl, rotateControl, rotateInPoint }; + +enum class refereeCommand { + halt = 0, + stop = 4, + forceStart = 11, + ballPlacementYellow = 16, + ballPlacementBlue = 17 +}; + +typedef struct { + uint8_t payload; + uint64_t addr[2]; + bool ack; + uint8_t receiveChannel; + uint8_t sendChannel; + uint8_t pipeNum; + bool reConfig; + RadioFunction function; +} NetworkConfig; + +typedef struct Motors { + double m1 = 0; + double m2 = 0; + double m3 = 0; + double m4 = 0; + + Motors() { + m1 = 0; + m2 = 0; + m3 = 0; + m4 = 0; + } + + inline Motors operator+(Motors a) { + Motors b; + b.m1 = m1 + a.m1; + b.m2 = m2 + a.m2; + b.m3 = m3 + a.m3; + b.m4 = m4 + a.m4; + return b; + } +} Motors; + +typedef struct Vector { + double x = 0; + double y = 0; + double w = 0; + + Vector() { + x = 0; + y = 0; + w = 0; + } + + Vector(double _x, double _y, double _w) { + x = _x; + y = _y; + w = _w; + } + + inline Vector operator+(Vector a) { + Vector b; + b.x = x + a.x; + b.y = y + a.y; + b.w = w + a.w; + return b; + } + + inline Vector operator-(Vector a) { + Vector b; + b.x = x - a.x; + b.y = y - a.y; + b.w = w - a.w; + return b; + } + + inline Vector operator*(double a) { + Vector b; + b.x = x * a; + b.y = y * a; + b.w = w * a; + return b; + } +} Vector; + +typedef struct KickFlags { + bool front = false; + bool chip = false; + bool charge = false; + float kickStrength = 0; + bool ball = false; + bool dribbler = false; + bool bypassIR = false; + float dribblerSpeed = 0; + + KickFlags& operator=(const KickFlags& a) { + front = a.front; + chip = a.chip; + charge = a.charge; + kickStrength = a.kickStrength; + ball = a.ball; + dribbler = a.dribbler; + bypassIR = a.bypassIR; + dribblerSpeed = a.dribblerSpeed; + return *this; + } +} KickFlags; + +typedef struct RobotPosition { + Vector v; + bool resetOdometry{}; + PositionType type = PositionType::unknown; + double maxSpeed{}; + double minSpeed{}; + double rotateKp{}; + bool usingPropSpeed{}; + double minDistanceToPropSpeed{}; + bool rotateInClockWise{}; + double orbitRadius{}; + double approachKp{}; + + RobotPosition& operator=(const RobotPosition& a) { + resetOdometry = a.resetOdometry; + v = a.v; + type = a.type; + maxSpeed = a.maxSpeed; + minSpeed = a.minSpeed; + rotateKp = a.rotateKp; + usingPropSpeed = a.usingPropSpeed; + minDistanceToPropSpeed = a.minDistanceToPropSpeed; + rotateInClockWise = a.rotateInClockWise; + orbitRadius = a.orbitRadius; + approachKp = a.approachKp; + return *this; + } +} RobotPosition; + +typedef struct { + int id = -1; + msgType type; + Motors m; + Motors current; + Vector v; + double dribbler = 0; + double kickLoad = 0; + bool ball = false; + double battery = 0; + uint8_t count = 0; +} RobotInfo; + +typedef struct { + int id = -1; + msgType type; + double m1; + double m2; + double battery = 0; +} VSSRobotInfo; + +#endif // COMM_TYPES_H diff --git a/lib/nRF24Communication/nRF24Communication.cpp b/lib/nRF24Communication/nRF24Communication.cpp new file mode 100644 index 0000000..212ce7e --- /dev/null +++ b/lib/nRF24Communication/nRF24Communication.cpp @@ -0,0 +1,391 @@ +#include "nRF24Communication.h" +#include "commTypes.h" + +nRF24Communication::nRF24Communication(PinName pinMOSI, + PinName pinMISO, + PinName pinSCK, + PinName pinCE, + PinName pinCSN, + PinName pinVCC, + NetworkType network, + RadioFunction function) : + _radio(pinMOSI, pinMISO, pinSCK, pinCSN, pinCE), + _vcc(pinVCC) { + this->_vcc = 1; + + this->_config.function = function; + this->_network(network); + this->disable(); +} + +int nRF24Communication::setup(int robotSwitches) { + if (this->updateRobotId(robotSwitches) < 0) { + return -1; + } + this->enable(); + this->_configure(); + this->disable(); + return 0; +} + +bool nRF24Communication::resetRadio() { + this->enable(); + this->_resetRadio(); + wait_us(1000); + this->_configure(); + bool connected = this->_radio.getPALevel() == RF24_PA_MAX; + this->disable(); + return connected; +} + +bool nRF24Communication::compareChannel(uint8_t channel) { + return this->_radio.compareChannel(channel); +} + +void nRF24Communication::_network(NetworkType network) { + this->_config.pipeNum = 0; + this->_config.reConfig = false; + this->_config.ack = ACK_RADIO; + + if (network == NetworkType::ssl) { + this->_config.payload = SSL_PAYLOAD_LENGTH; + this->_config.receiveChannel = SSL_1_ROBOT_RECV_CH; + this->_config.sendChannel = SSL_2_ROBOT_SEND_CH; + + this->_config.addr[0] = SSL_ADDR_1; + this->_config.addr[1] = SSL_ADDR_2; + } else if (network == NetworkType::vss) { + this->_config.payload = VSS_PAYLOAD_LENGTH; + this->_config.receiveChannel = VSS_ROBOT_RECV_CHANNEL; + this->_config.sendChannel = VSS_ROBOT_SEND_CHANNEL; + + this->_config.addr[0] = VSS_ADDR_1; + this->_config.addr[1] = VSS_ADDR_2; + } +} + +void nRF24Communication::_configure() { + this->_radio.begin(); // Start the nRF24 module + this->_radio.setPALevel(RF24_PA_MAX); + this->_radio.setDataRate(RF24_2MBPS); + this->_radio.setAutoAck(_config.ack); + this->_radio.setPayloadSize(_config.payload); + if (_config.function == RadioFunction::receiver) { + // Init communication as receiver + this->_radio.openWritingPipe(_config.addr[1]); + this->_radio.openReadingPipe(1, _config.addr[0]); + this->_radio.setChannel(this->_config.receiveChannel); + this->_radio.startListening(); + } else if (_config.function == RadioFunction::sender) { + this->_radio.openWritingPipe(_config.addr[0]); + this->_radio.openReadingPipe(1, _config.addr[1]); + this->_radio.setChannel(this->_config.sendChannel); + this->_radio.stopListening(); + } + // this->_radio.printDetails(); // Radio Details +} + +void nRF24Communication::_resetRadio() { + this->_vcc = 0; + wait_us(1000); + this->_vcc = 1; + wait_us(1000); +} + +void nRF24Communication::_receive() { + this->_radio.startListening(); +} + +void nRF24Communication::_send() { + this->_radio.stopListening(); +} + +void nRF24Communication::enable() { + this->_radio.csn(LOW); + wait_us(10); +} + +void nRF24Communication::disable() { + this->_radio.csn(HIGH); + wait_us(5); +} + +int nRF24Communication::updateRobotId(int robotSwID) { + if (robotSwID < 0 || robotSwID > 15) + return -1; + else + this->_robotId = robotSwID; + return this->_robotId; +} + +int nRF24Communication::getRobotId() { + return this->_robotId; +} + +void nRF24Communication::printDetails() { + this->_radio.printDetails(); +} + +bool nRF24Communication::radioStillConfigured() { + return this->_radio.getPALevel() == RF24_PA_MAX && this->_radio.getCRCLength() == RF24_CRC_16 && + this->compareChannel(this->_config.receiveChannel); +} + +msgType nRF24Communication::getPacketType() { + return this->_lastPacketType; +} + +bool nRF24Communication::updatePacket() { + this->enable(); + if (radioStillConfigured()) { + // printDetails(); + while (this->_radio.available(&(_config.pipeNum))) { + this->_radio.read(&(this->_rx.encoded), _config.payload); + // this->showBitsReceived(_config.payload,this->_rx.encoded); // Debug + + if (this->_rx.decoded.id == this->_robotId) { + // Save the message type + this->_lastPacketType = static_cast(this->_rx.decoded.typeMsg); + // According to the type of message assign variables + if (this->_lastPacketType == msgType::VSS_SPEED) { + // VSS + this->clearVSSData(); + std::memcpy(this->_mVSS.encoded, + this->_rx.encoded, + VSS_SPEED_LENGTH); // require std::, eventual error in copy + this->_isPWM = static_cast(this->_mVSS.decoded.isPWM); + if (_isPWM) { + this->_motorSpeed.m1 = static_cast((this->_mVSS.decoded.m1) / 100000.0); + this->_motorSpeed.m2 = static_cast((this->_mVSS.decoded.m2) / 100000.0); + } else { + this->_motorSpeed.m1 = static_cast((this->_mVSS.decoded.m1) / 1000.0); + this->_motorSpeed.m2 = static_cast((this->_mVSS.decoded.m2) / 1000.0); + } + } else if (this->_lastPacketType == msgType::SSL_SPEED) { + // SSL + this->clearSSLDataSpeed(); + this->clearSSLDataKick(); + std::memcpy(this->_mSSL.encoded, + this->_rx.encoded, + SSL_SPEED_LENGTH); // require std::, eventual error in copy + this->_gameState = static_cast(this->_mSSL.decoded.command); + this->_v.x = static_cast((this->_mSSL.decoded.vx) / 10000.0); + this->_v.y = static_cast((this->_mSSL.decoded.vy) / 10000.0); + this->_v.w = static_cast((this->_mSSL.decoded.vw) / 10000.0); + this->_kick.front = static_cast(this->_mSSL.decoded.front); + this->_kick.chip = static_cast(this->_mSSL.decoded.chip); + this->_kick.charge = static_cast(this->_mSSL.decoded.charge); + this->_kick.kickStrength = static_cast((this->_mSSL.decoded.kickStrength) / 10.0); + this->_kick.dribbler = static_cast(this->_mSSL.decoded.dribbler); + this->_kick.dribblerSpeed = + static_cast((this->_mSSL.decoded.dribblerSpeed) / 10.0); + this->_moveIsLocked = static_cast(this->_mSSL.decoded.robotLockedToMove); + this->_criticalMoveTurbo = static_cast(this->_mSSL.decoded.criticalMoveTurbo); + } else if (this->_lastPacketType == msgType::POSITION) { + this->clearSSLDataPosition(); + this->clearSSLDataKick(); + std::memcpy(this->_mPostion.encoded, this->_rx.encoded, POSITION_LENGTH); + this->_pos.type = static_cast((this->_mPostion.decoded.positionType)); + + this->_pos.v.x = static_cast((this->_mPostion.decoded.x) / 1000.0); + this->_pos.v.y = static_cast((this->_mPostion.decoded.y) / 1000.0); + this->_pos.v.w = static_cast((this->_mPostion.decoded.w) / 10000.0); + this->_pos.maxSpeed = static_cast((this->_mPostion.decoded.maxSpeed) / 1000.0); + this->_pos.minSpeed = static_cast((this->_mPostion.decoded.minSpeed) / 1000.0); + this->_pos.rotateKp = static_cast((this->_mPostion.decoded.rotateKp) / 100.0); + this->_pos.usingPropSpeed = static_cast(this->_mPostion.decoded.usingPropSpeed); + this->_pos.minDistanceToPropSpeed = + static_cast((this->_mPostion.decoded.minDistanceToPropSpeed) / 1000.0); + this->_pos.rotateInClockWise = static_cast(this->_mPostion.decoded.clockwise); + this->_pos.orbitRadius = + static_cast((this->_mPostion.decoded.orbitRadius) / 1000.0); + this->_pos.approachKp = static_cast((this->_mPostion.decoded.approachKp) / 100.0); + this->_kick.front = static_cast(this->_mPostion.decoded.front); + this->_kick.chip = static_cast(this->_mPostion.decoded.chip); + this->_kick.charge = static_cast(this->_mPostion.decoded.charge); + this->_kick.kickStrength = static_cast((this->_mPostion.decoded.strength) / 10.0); + this->_kick.dribbler = static_cast(this->_mPostion.decoded.dribbler); + this->_kick.dribblerSpeed = + static_cast((this->_mPostion.decoded.dribblerSpeed) / 10.0); + } else { + break; + } + this->disable(); + return true; + } else { + break; + } + } + } else { + this->_resetRadio(); + this->_configure(); + utils::beep(100); // warning reset signal + printf("reseting receive radio...\n"); + } + this->disable(); + return false; +} + +bool nRF24Communication::sendTelemetryPacket(RobotInfo telemetry) { + + this->_mTelemetry.decoded.typeMsg = static_cast(msgType::TELEMETRY); + this->_mTelemetry.decoded.id = static_cast(this->getRobotId()); + this->_mTelemetry.decoded.current_m1 = static_cast(telemetry.current.m1 * 100); + this->_mTelemetry.decoded.current_m2 = static_cast(telemetry.current.m2 * 100); + this->_mTelemetry.decoded.current_m3 = static_cast(telemetry.current.m3 * 100); + this->_mTelemetry.decoded.current_m4 = static_cast(telemetry.current.m4 * 100); + this->_mTelemetry.decoded.dribbler = static_cast(telemetry.dribbler * 10); + this->_mTelemetry.decoded.kickLoad = static_cast(telemetry.kickLoad * 100); + this->_mTelemetry.decoded.ball = static_cast(telemetry.ball); + this->_mTelemetry.decoded.battery = static_cast(telemetry.battery * 10); + this->_mTelemetry.decoded.m1 = static_cast(telemetry.m.m1 * 100); + this->_mTelemetry.decoded.m2 = static_cast(telemetry.m.m2 * 100); + this->_mTelemetry.decoded.m3 = static_cast(telemetry.m.m3 * 100); + this->_mTelemetry.decoded.m4 = static_cast(telemetry.m.m4 * 100); + this->_mTelemetry.decoded.pcktCount = static_cast(telemetry.count); + this->enable(); + bool answer = this->_radio.write(this->_mTelemetry.encoded, TELEMETRY_LENGTH); + this->disable(); + return answer; +} + +bool nRF24Communication::sendVSSTelemetryPacket(VSSRobotInfo telemetry) { + + this->_mTelemetryVSS.decoded.typeMsg = static_cast(msgType::VSS_TELEMETRY); + this->_mTelemetryVSS.decoded.id = static_cast(this->getRobotId()); + this->_mTelemetryVSS.decoded.m1 = static_cast(telemetry.m1 * 1000); + this->_mTelemetryVSS.decoded.m2 = static_cast(telemetry.m2 * 1000); + this->_mTelemetryVSS.decoded.battery = static_cast(telemetry.battery * 10); + this->enable(); + bool answer = this->_radio.write(this->_mTelemetryVSS.encoded, VSS_TELEMETRY_LENGTH); + this->disable(); + return answer; +} + +bool nRF24Communication::sendOdometryPacket(RobotInfo odometry) { + this->_mOdometry.decoded.typeMsg = static_cast(msgType::ODOMETRY); + this->_mOdometry.decoded.id = static_cast(this->getRobotId()); + this->_mOdometry.decoded.x = static_cast(odometry.v.x * 1000); + this->_mOdometry.decoded.y = static_cast(odometry.v.y * 1000); + this->_mOdometry.decoded.w = static_cast(odometry.v.w * 10000); + this->_mOdometry.decoded.dribbler = static_cast(odometry.dribbler * 10); + this->_mOdometry.decoded.kickLoad = static_cast(odometry.kickLoad * 100); + this->_mOdometry.decoded.ball = static_cast(odometry.ball); + this->_mOdometry.decoded.battery = static_cast(odometry.battery * 10); + this->_mOdometry.decoded.m1 = static_cast(odometry.m.m1 * 100); + this->_mOdometry.decoded.m2 = static_cast(odometry.m.m2 * 100); + this->_mOdometry.decoded.m3 = static_cast(odometry.m.m3 * 100); + this->_mOdometry.decoded.m4 = static_cast(odometry.m.m4 * 100); + this->_mOdometry.decoded.pcktCount = static_cast(odometry.count); + this->enable(); + bool answer = this->_radio.write(this->_mOdometry.encoded, ODOMETRY_LENGTH); + this->disable(); + return answer; +} + +void nRF24Communication::getDifferentialSpeed(Motors& mSpeed) { + mSpeed.m1 = this->_motorSpeed.m1; + mSpeed.m2 = this->_motorSpeed.m2; +} + +void nRF24Communication::clearVSSData() { + this->_isPWM = 0; + this->_motorSpeed.m1 = 0; + this->_motorSpeed.m2 = 0; +} + +refereeCommand nRF24Communication::getGameState() { + return this->_gameState; +} + +Vector nRF24Communication::getVectorSpeed() { + return this->_v; +} + +void nRF24Communication::clearSSLDataSpeed() { + this->_v.x = 0; + this->_v.y = 0; + this->_v.w = 0; + this->_moveIsLocked = false; + this->_criticalMoveTurbo = false; +} + +void nRF24Communication::clearSSLDataPosition() { + + this->_pos.v = Vector(); + this->_pos.type = PositionType::unknown; + this->_pos.maxSpeed = 0; + this->_pos.minSpeed = 0; + this->_pos.rotateKp = 0; + this->_pos.usingPropSpeed = false; + this->_pos.minDistanceToPropSpeed = 0; + this->_pos.rotateInClockWise = false; + this->_pos.orbitRadius = 0; + this->_pos.approachKp = 0; +} + +void nRF24Communication::clearSSLDataKick() { + this->_kick.front = false; + this->_kick.chip = false; + this->_kick.charge = false; + this->_kick.bypassIR = false; + this->_kick.kickStrength = 0; + this->_kick.dribbler = false; + this->_kick.dribblerSpeed = 0; +} + +void nRF24Communication::getPosition(RobotPosition& pos) { + pos = _pos; +} + +RobotPosition nRF24Communication::getLastPosition() { + return _pos; +} + +void nRF24Communication::getKick(KickFlags& isKick) { + isKick.front = _kick.front; + isKick.chip = _kick.chip; + isKick.charge = _kick.charge; + isKick.kickStrength = _kick.kickStrength; + isKick.dribbler = _kick.dribbler; + isKick.bypassIR = (_kick.front | _kick.chip) & _kick.charge; + isKick.dribbler = _kick.dribbler; + isKick.dribblerSpeed = _kick.dribblerSpeed; +} + +void nRF24Communication::showBitsReceived(int payload, unsigned char* data) { + // A sequencia de campos segue o da declaracao da estrutura em commConfig.h, mas os bits estao + // em little endian + for (int i = 0; i < payload; i++) { + int val = data[i]; + int mask = 1; + for (int j = 0; j < 8; j++) { + if (mask & val) { + printf("1"); + } else { + printf("0"); + } + mask = mask << 1; + } + printf("|"); + } + printf("\n"); +} + +float nRF24Communication::getKP() { + return _kp; +} +float nRF24Communication::getKD() { + return _kd; +} +float nRF24Communication::getAlpha() { + return _alpha; +} + +bool nRF24Communication::robotMoveIsLocked() { + return _moveIsLocked; +} + +bool nRF24Communication::robotMoveCriticalTurbo() { + return _criticalMoveTurbo; +} \ No newline at end of file diff --git a/lib/nRF24Communication/nRF24Communication.h b/lib/nRF24Communication/nRF24Communication.h new file mode 100644 index 0000000..ccab0b1 --- /dev/null +++ b/lib/nRF24Communication/nRF24Communication.h @@ -0,0 +1,101 @@ +#ifndef NRF24_COMMUNICATION_H +#define NRF24_COMMUNICATION_H + +#include +#include // Call SPI library so you can communicate with the nRF24L01+ +#include // nRF2401 libarary found at https://github.com/tmrh20/RF24/ +#include +#include +#include "utils.h" + +class nRF24Communication { + public: + // Class constructor + nRF24Communication(PinName pinMOSI, + PinName pinMISO, + PinName pinSCK, + PinName pinCE, + PinName pinCSN, + PinName pinVCC, + NetworkType network, + RadioFunction function); + + // Class destructor + // ~nRF24Communication(); + + int setup(int robotSwitches); + bool resetRadio(); + bool compareChannel(uint8_t channel); + int updateRobotId(int robotSwIR2_D); + int getRobotId(); + void printDetails(); + bool radioStillConfigured(); + + bool updatePacket(); + msgType getPacketType(); + void showBitsReceived(int payload, unsigned char* data); + void enable(); + void disable(); + + // VSS Info + void getDifferentialSpeed(Motors& mSpeed); + void clearVSSData(); + + // SSL Info + Vector getVectorSpeed(); + bool sendTelemetryPacket(RobotInfo telemetry); + bool sendOdometryPacket(RobotInfo odometry); + bool sendVSSTelemetryPacket(VSSRobotInfo telemetryVSS); + void clearSSLDataSpeed(); + void clearSSLDataPosition(); + void clearSSLDataKick(); + void getKick(KickFlags& isKick); + void getPosition(RobotPosition& pos); + refereeCommand getGameState(); + RobotPosition getLastPosition(); + bool robotMoveIsLocked(); + bool robotMoveCriticalTurbo(); + + // Aux Info + float getKP(); + float getKD(); + float getAlpha(); + + /******** CREATED PUBLIC RX FOR ETHERNET CONFIG ***********/ + packetGeneric _public_rx; + + private: + RF24 _radio; + DigitalOut _vcc; + + private: + void _network(NetworkType network); + void _configure(); + void _resetRadio(); + void _receive(); + void _send(); + + packetSpeedVSS _mVSS; + packetSpeedSSL _mSSL; + packetTelemetry _mTelemetry; + packetVSSTelemetry _mTelemetryVSS; + packetOdometry _mOdometry; + packetPosition _mPostion; + packetGeneric _rx; + + NetworkConfig _config; + char _robotId; + msgType _typeMsg; + refereeCommand _gameState; + msgType _lastPacketType; + Motors _motorSpeed; + bool _isPWM; + Vector _v; + bool _moveIsLocked; + bool _criticalMoveTurbo; + KickFlags _kick; + RobotPosition _pos; + float _kp, _kd, _ki, _alpha; +}; + +#endif // NRF_COMM diff --git a/lib/nRF24Communication/nRF24L01P/nRF24L01P.cpp b/lib/nRF24Communication/nRF24L01P/nRF24L01P.cpp new file mode 100644 index 0000000..2ab1f32 --- /dev/null +++ b/lib/nRF24Communication/nRF24L01P/nRF24L01P.cpp @@ -0,0 +1,914 @@ +/* + Copyright (C) 2011 J. Coliz + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + version 2 as published by the Free Software Foundation. + */ + +#include "nRF24L01P.h" + +/****************************************************************************/ + +void RF24::csn(int mode) { + // Minimum ideal spi bus speed is 2x data rate + // If we assume 2Mbs data rate and 16Mhz clock, a + // divider of 4 is the minimum we want. + // CLK:BUS 8Mhz:2Mhz, 16Mhz:4Mhz, or 20Mhz:5Mhz + //#ifdef ARDUINO + // spi.setBitOrder(MSBFIRST); + // spi.setDataMode(spi_MODE0); + // spi.setClockDivider(spi_CLOCK_DIV4); + //#endif + // digitalWrite(csn_pin,mode); + csn_pin = mode; +} + +/****************************************************************************/ + +void RF24::ce(int level) { + // digitalWrite(ce_pin,level); + ce_pin = level; +} + +/****************************************************************************/ + +uint8_t RF24::read_register(uint8_t reg, uint8_t* buf, uint8_t len) { + uint8_t status; + + csn(LOW); + status = spi.write(R_REGISTER | (REGISTER_MASK & reg)); + while (len--) + *buf++ = spi.write(0xff); + + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::read_register(uint8_t reg) { + csn(LOW); + spi.write(R_REGISTER | (REGISTER_MASK & reg)); + uint8_t result = spi.write(0xff); + + csn(HIGH); + return result; +} + +/****************************************************************************/ + +uint8_t RF24::write_register(uint8_t reg, const uint8_t* buf, uint8_t len) { + uint8_t status; + + csn(LOW); + status = spi.write(W_REGISTER | (REGISTER_MASK & reg)); + while (len--) + spi.write(*buf++); + + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::write_register(uint8_t reg, uint8_t value) { + uint8_t status; + + // IF_SERIAL_DEBUG(printf(("write_register(%02x,%02x)\r\n"),reg,value)); + + csn(LOW); + status = spi.write(W_REGISTER | (REGISTER_MASK & reg)); + spi.write(value); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::write_payload(const void* buf, uint8_t len) { + uint8_t status; + + const uint8_t* current = reinterpret_cast(buf); + + uint8_t data_len = min(len, payload_size); + uint8_t blank_len = dynamic_payloads_enabled ? 0 : payload_size - data_len; + + // printf("[Writing %u bytes %u blanks]",data_len,blank_len); + + csn(LOW); + status = spi.write(W_TX_PAYLOAD); + while (data_len--) + spi.write(*current++); + while (blank_len--) + spi.write(0); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::read_payload(void* buf, uint8_t len) { + uint8_t status; + uint8_t* current = reinterpret_cast(buf); + + uint8_t data_len = min(len, payload_size); + uint8_t blank_len = dynamic_payloads_enabled ? 0 : payload_size - data_len; + + // printf("[Reading %u bytes %u blanks]",data_len,blank_len); + + csn(LOW); + status = spi.write(R_RX_PAYLOAD); + while (data_len--) + *current++ = spi.write(0xff); + while (blank_len--) + spi.write(0xff); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::flush_rx(void) { + uint8_t status; + + csn(LOW); + status = spi.write(FLUSH_RX); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::flush_tx(void) { + uint8_t status; + + csn(LOW); + status = spi.write(FLUSH_TX); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +uint8_t RF24::get_status(void) { + uint8_t status; + + csn(LOW); + status = spi.write(NOP); + csn(HIGH); + + return status; +} + +/****************************************************************************/ + +void RF24::print_status(uint8_t status) { + printf(("STATUS\t\t = 0x%02x RX_DR=%x TX_DS=%x MAX_RT=%x RX_P_NO=%x TX_FULL=%x\r\n"), + status, + (status & _BV(RX_DR)) ? 1 : 0, + (status & _BV(TX_DS)) ? 1 : 0, + (status & _BV(MAX_RT)) ? 1 : 0, + ((status >> RX_P_NO) & 7), + (status & _BV(TX_FULL)) ? 1 : 0); +} + +/****************************************************************************/ + +void RF24::print_observe_tx(uint8_t value) { + printf(("OBSERVE_TX=%02x: POLS_CNT=%x ARC_CNT=%x\r\n"), + value, + (value >> PLOS_CNT) & 15, + (value >> ARC_CNT) & 15); +} + +/****************************************************************************/ + +void RF24::print_byte_register(const char* name, uint8_t reg, uint8_t qty) { + // char extra_tab = strlen(name) < 8 ? '\t' : 0; + printf("%s =", name); + while (qty--) + printf((" 0x%02x"), read_register(reg++)); + printf(("\r\n")); +} + +/****************************************************************************/ + +void RF24::print_address_register(const char* name, uint8_t reg, uint8_t qty) { + // char extra_tab = strlen_P(name) < 8 ? '\t' : 0; + printf("%s =", name); + + while (qty--) { + uint8_t buffer[5]; + read_register(reg++, buffer, sizeof buffer); + + printf((" 0x")); + uint8_t* bufptr = buffer + sizeof buffer; + while (--bufptr >= buffer) + printf(("%02x"), *bufptr); + } + + printf(("\r\n")); +} + +/****************************************************************************/ + +RF24::RF24(PinName mosi, PinName miso, PinName sck, PinName _csnpin, PinName _cepin) : + ce_pin(_cepin), + csn_pin(_csnpin), + wide_band(true), + p_variant(false), + payload_size(32), + ack_payload_available(false), + dynamic_payloads_enabled(false), + pipe0_reading_address(0), + spi(mosi, miso, sck) { + spi.frequency(10000000); // 10 Mbit, the maximum transfer rate for the spi bus + spi.format(8, 0); // 8-bit, ClockPhase = 0, ClockPolarity = 0 +} + +/****************************************************************************/ + +void RF24::setChannel(uint8_t channel) { + // TODO: This method could take advantage of the 'wide_band' calculation + // done in setChannel() to require certain channel spacing. + + const uint8_t max_channel = 127; + write_register(RF_CH, min(channel, max_channel)); +} + +/****************************************************************************/ + +void RF24::setPayloadSize(uint8_t size) { + const uint8_t max_payload_size = 32; + payload_size = min(size, max_payload_size); +} + +/****************************************************************************/ + +uint8_t RF24::getPayloadSize(void) { + return payload_size; +} + +/****************************************************************************/ + +static const char rf24_datarate_e_str_0[] = "1MBPS"; +static const char rf24_datarate_e_str_1[] = "2MBPS"; +static const char rf24_datarate_e_str_2[] = "250KBPS"; +static const char* const rf24_datarate_e_str_P[] = { + rf24_datarate_e_str_0, + rf24_datarate_e_str_1, + rf24_datarate_e_str_2, +}; +static const char rf24_model_e_str_0[] = "nRF24L01"; +static const char rf24_model_e_str_1[] = "nRF24L01+"; +static const char* const rf24_model_e_str_P[] = { + rf24_model_e_str_0, + rf24_model_e_str_1, +}; +static const char rf24_crclength_e_str_0[] = "Disabled"; +static const char rf24_crclength_e_str_1[] = "8 bits"; +static const char rf24_crclength_e_str_2[] = "16 bits"; +static const char* const rf24_crclength_e_str_P[] = { + rf24_crclength_e_str_0, + rf24_crclength_e_str_1, + rf24_crclength_e_str_2, +}; +static const char rf24_pa_dbm_e_str_0[] = "PA_MIN"; +static const char rf24_pa_dbm_e_str_1[] = "PA_LOW"; +static const char rf24_pa_dbm_e_str_2[] = "PA_MED"; +static const char rf24_pa_dbm_e_str_3[] = "PA_HIGH"; +static const char* const rf24_pa_dbm_e_str_P[] = { + rf24_pa_dbm_e_str_0, + rf24_pa_dbm_e_str_1, + rf24_pa_dbm_e_str_2, + rf24_pa_dbm_e_str_3, +}; + +void RF24::printDetails(void) { + print_status(get_status()); + + print_address_register(("RX_ADDR_P0-1"), RX_ADDR_P0, 2); + print_byte_register(("RX_ADDR_P2-5"), RX_ADDR_P2, 4); + print_address_register(("TX_ADDR"), TX_ADDR); + + print_byte_register(("RX_PW_P0-6"), RX_PW_P0, 6); + print_byte_register(("EN_AA"), EN_AA); + print_byte_register(("EN_RXADDR"), EN_RXADDR); + print_byte_register(("RF_CH"), RF_CH); + print_byte_register(("RF_SETUP"), RF_SETUP); + print_byte_register(("CONFIG"), CONFIG); + print_byte_register(("DYNPD/FEATURE"), DYNPD, 2); + + printf(("Data Rate\t = %s\r\n"), rf24_datarate_e_str_P[getDataRate()]); + printf(("Model\t\t = %s\r\n"), rf24_model_e_str_P[isPVariant()]); + printf(("CRC Length\t = %s\r\n"), rf24_crclength_e_str_P[getCRCLength()]); + printf(("PA Power\t = %s\r\n"), rf24_pa_dbm_e_str_P[getPALevel()]); +} + +bool RF24::compareChannel(uint8_t channel) { + uint8_t reg = read_register(RF_CH); + if (reg == channel) { + return true; + } else + return false; +} + +/****************************************************************************/ + +void RF24::begin(void) { + // Initialize pins + // pinMode(ce_pin,OUTPUT); + // pinMode(csn_pin,OUTPUT); + + // Initialize spi bus + // spi.begin(); + mainTimer.start(); + + ce(LOW); + csn(HIGH); + + // Must allow the radio time to settle else configuration bits will not necessarily stick. + // This is actually only required following power up but some settling time also appears to + // be required after resets too. For full coverage, we'll always assume the worst. + // Enabling 16b CRC is by far the most obvious case if the wrong timing is used - or skipped. + // Technically we require 4.5ms + 14us as a worst case. We'll just call it 5ms for good measure. + // WARNING: wait_ms is based on P-variant whereby non-P *may* require different timing. + wait_us(5000); + + // Set 1500uS (minimum for 32B payload in ESB@250KBPS) timeouts, to make testing a little easier + // WARNING: If this is ever lowered, either 250KBS mode with AA is broken or maximum packet + // sizes must never be used. See documentation for a more complete explanation. + write_register(SETUP_RETR, (4 << ARD) | (15 << ARC)); + + // Restore our default PA level + setPALevel(RF24_PA_MAX); + + // Determine if this is a p or non-p RF24 module and then + // reset our data rate back to default value. This works + // because a non-P variant won't allow the data rate to + // be set to 250Kbps. + if (setDataRate(RF24_250KBPS)) { + p_variant = true; + } + + // Then set the data rate to the slowest (and most reliable) speed supported by all + // hardware. + setDataRate(RF24_1MBPS); + + // Initialize CRC and request 2-byte (16bit) CRC + setCRCLength(RF24_CRC_16); + + // Disable dynamic payloads, to match dynamic_payloads_enabled setting + write_register(DYNPD, 0); + + // Reset current status + // Notice reset and flush is the last thing we do + write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT)); + + // Set up default configuration. Callers can always change it later. + // This channel should be universally safe and not bleed over into adjacent + // spectrum. + setChannel(76); + + // Flush buffers + flush_rx(); + flush_tx(); + + // set EN_RXADDRR to 0 to fix pipe 0 from receiving + write_register(EN_RXADDR, 0); +} + +/****************************************************************************/ + +void RF24::startListening(void) { + write_register(CONFIG, read_register(CONFIG) | _BV(PWR_UP) | _BV(PRIM_RX)); + write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT)); + + // Restore the pipe0 adddress, if exists + if (pipe0_reading_address) + write_register(RX_ADDR_P0, reinterpret_cast(&pipe0_reading_address), 5); + + // Flush buffers + flush_rx(); + flush_tx(); + + // Go! + ce(HIGH); + + // wait for the radio to come up (130us actually only needed) + // wait_msMicroseconds(130); + wait_us(130); +} + +/****************************************************************************/ + +void RF24::stopListening(void) { + ce(LOW); + flush_tx(); + flush_rx(); +} + +/****************************************************************************/ + +void RF24::powerDown(void) { + write_register(CONFIG, read_register(CONFIG) & ~_BV(PWR_UP)); +} + +/****************************************************************************/ + +void RF24::powerUp(void) { + write_register(CONFIG, read_register(CONFIG) | _BV(PWR_UP)); +} + +/******************************************************************/ + +bool RF24::write(const void* buf, uint8_t len) { + bool result = false; + + // Begin the write + startWrite(buf, len); + + // ------------ + // At this point we could return from a non-blocking write, and then call + // the rest after an interrupt + + // Instead, we are going to block here until we get TX_DS (transmission completed and ack'd) + // or MAX_RT (maximum retries, transmission failed). Also, we'll timeout in case the radio + // is flaky and we get neither. + + // IN the end, the send should be blocking. It comes back in 60ms worst case, or much faster + // if I tighted up the retry logic. (Default settings will be 1500us. + // Monitor the send + uint8_t observe_tx; + uint8_t status; + uint32_t sent_at = chrono::duration_cast(mainTimer.elapsed_time()).count(); + const uint32_t timeout = 500 * 1000; // us to wait for timeout + do { + status = read_register(OBSERVE_TX, &observe_tx, 1); + // IF_SERIAL_DEBUG(Serial.print(observe_tx,HEX)); + } while (!(status & (_BV(TX_DS) | _BV(MAX_RT))) && + ((chrono::duration_cast(mainTimer.elapsed_time()).count() - + sent_at) < timeout)); + + // The part above is what you could recreate with your own interrupt handler, + // and then call this when you got an interrupt + // ------------ + + // Call this when you get an interrupt + // The status tells us three things + // * The send was successful (TX_DS) + // * The send failed, too many retries (MAX_RT) + // * There is an ack packet waiting (RX_DR) + bool tx_ok, tx_fail; + whatHappened(tx_ok, tx_fail, ack_payload_available); + + // printf("%u%u%u\r\n",tx_ok,tx_fail,ack_payload_available); + + result = tx_ok; + // IF_SERIAL_DEBUG(Serial.print(result?"...OK.":"...Failed")); + + // Handle the ack packet + if (ack_payload_available) { + ack_payload_length = getDynamicPayloadSize(); + // IF_SERIAL_DEBUG(Serial.print("[AckPacket]/")); + // IF_SERIAL_DEBUG(Serial.println(ack_payload_length,DEC)); + } + + // Yay, we are done. + + // Power down + // powerDown(); + + // Flush buffers (Is this a relic of past experimentation, and not needed anymore? + // flush_tx(); + + return result; +} +/****************************************************************************/ + +void RF24::startWrite(const void* buf, uint8_t len) { + // Transmitter power-up + write_register(CONFIG, (read_register(CONFIG) | _BV(PWR_UP)) & ~_BV(PRIM_RX)); + // wait_msMicroseconds(150); + wait_us(130); + + // Send the payload + write_payload(buf, len); + + // Allons! + ce(HIGH); + // wait_msMicroseconds(15); + wait_us(15); + ce(LOW); +} + +/****************************************************************************/ + +uint8_t RF24::getDynamicPayloadSize(void) { + uint8_t result = 0; + + csn(LOW); + spi.write(R_RX_PL_WID); + result = spi.write(0xff); + csn(HIGH); + + return result; +} + +/****************************************************************************/ + +bool RF24::available(void) { + return available(NULL); +} + +/****************************************************************************/ + +bool RF24::available(uint8_t* pipe_num) { + uint8_t status = get_status(); + + // Too noisy, enable if you really want lots o data!! + // IF_SERIAL_DEBUG(print_status(status)); + + bool result = (status & _BV(RX_DR)); + + if (result) { + // If the caller wants the pipe number, include that + if (pipe_num) + *pipe_num = (status >> RX_P_NO) & 7; + + // Clear the status bit + + // ??? Should this REALLY be cleared now? Or wait until we + // actually READ the payload? + + write_register(STATUS, _BV(RX_DR)); + + // Handle ack payload receipt + if (status & _BV(TX_DS)) { + write_register(STATUS, _BV(TX_DS)); + } + } + + return result; +} + +/****************************************************************************/ + +bool RF24::read(void* buf, uint8_t len) { + // Fetch the payload + read_payload(buf, len); + + // was this the last of the data available? + return read_register(FIFO_STATUS) & _BV(RX_EMPTY); +} + +/****************************************************************************/ + +void RF24::whatHappened(bool& tx_ok, bool& tx_fail, bool& rx_ready) { + // Read the status & reset the status in one easy call + // Or is that such a good idea? + uint8_t status = write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT)); + + // Report to the user what happened + tx_ok = status & _BV(TX_DS); + tx_fail = status & _BV(MAX_RT); + rx_ready = status & _BV(RX_DR); +} + +/****************************************************************************/ + +void RF24::openWritingPipe(uint64_t value) { + // Note that AVR 8-bit uC's store this LSB first, and the NRF24L01(+) + // expects it LSB first too, so we're good. + + write_register(RX_ADDR_P0, reinterpret_cast(&value), 5); + write_register(TX_ADDR, reinterpret_cast(&value), 5); + + const uint8_t max_payload_size = 32; + write_register(RX_PW_P0, min(payload_size, max_payload_size)); + + flush_tx(); +} + +/****************************************************************************/ + +static const uint8_t child_pipe[] = + {RX_ADDR_P0, RX_ADDR_P1, RX_ADDR_P2, RX_ADDR_P3, RX_ADDR_P4, RX_ADDR_P5}; +static const uint8_t child_payload_size[] = + {RX_PW_P0, RX_PW_P1, RX_PW_P2, RX_PW_P3, RX_PW_P4, RX_PW_P5}; +static const uint8_t child_pipe_enable[] = {ERX_P0, ERX_P1, ERX_P2, ERX_P3, ERX_P4, ERX_P5}; + +void RF24::openReadingPipe(uint8_t child, uint64_t address) { + // If this is pipe 0, cache the address. This is needed because + // openWritingPipe() will overwrite the pipe 0 address, so + // startListening() will have to restore it. + if (child == 0) + pipe0_reading_address = address; + + if (child <= 6) { + // For pipes 2-5, only write the LSB + if (child < 2) + write_register(child_pipe[child], reinterpret_cast(&address), 5); + else + write_register(child_pipe[child], reinterpret_cast(&address), 1); + + write_register(child_payload_size[child], payload_size); + + // Note it would be more efficient to set all of the bits for all open + // pipes at once. However, I thought it would make the calling code + // more simple to do it this way. + write_register(EN_RXADDR, read_register(EN_RXADDR) | _BV(child_pipe_enable[child])); + } +} + +/****************************************************************************/ + +void RF24::toggle_features(void) { + csn(LOW); + spi.write(ACTIVATE); + spi.write(0x73); + csn(HIGH); +} + +/****************************************************************************/ + +void RF24::enableDynamicPayloads(void) { + // Enable dynamic payload throughout the system + write_register(FEATURE, read_register(FEATURE) | _BV(EN_DPL)); + + // If it didn't work, the features are not enabled + if (!read_register(FEATURE)) { + // So enable them and try again + toggle_features(); + write_register(FEATURE, read_register(FEATURE) | _BV(EN_DPL)); + } + + // IF_SERIAL_DEBUG(printf("FEATURE=%i\r\n",read_register(FEATURE))); + + // Enable dynamic payload on all pipes + // + // Not sure the use case of only having dynamic payload on certain + // pipes, so the library does not support it. + write_register(DYNPD, + read_register(DYNPD) | _BV(DPL_P5) | _BV(DPL_P4) | _BV(DPL_P3) | _BV(DPL_P2) | + _BV(DPL_P1) | _BV(DPL_P0)); + + dynamic_payloads_enabled = true; +} + +/****************************************************************************/ + +void RF24::enableAckPayload(void) { + // + // enable ack payload and dynamic payload features + // + + write_register(FEATURE, read_register(FEATURE) | _BV(EN_ACK_PAY) | _BV(EN_DPL)); + + // If it didn't work, the features are not enabled + if (!read_register(FEATURE)) { + // So enable them and try again + toggle_features(); + write_register(FEATURE, read_register(FEATURE) | _BV(EN_ACK_PAY) | _BV(EN_DPL)); + } + + // IF_SERIAL_DEBUG(printf("FEATURE=%i\r\n",read_register(FEATURE))); + + // + // Enable dynamic payload on pipes 0 & 1 + // + + write_register(DYNPD, read_register(DYNPD) | _BV(DPL_P1) | _BV(DPL_P0)); +} + +/****************************************************************************/ + +void RF24::writeAckPayload(uint8_t pipe, const void* buf, uint8_t len) { + const uint8_t* current = reinterpret_cast(buf); + + csn(LOW); + spi.write(W_ACK_PAYLOAD | (pipe & 7)); + const uint8_t max_payload_size = 32; + uint8_t data_len = min(len, max_payload_size); + while (data_len--) + spi.write(*current++); + + csn(HIGH); +} + +/****************************************************************************/ + +bool RF24::isAckPayloadAvailable(void) { + bool result = ack_payload_available; + ack_payload_available = false; + return result; +} + +/****************************************************************************/ + +bool RF24::isPVariant(void) { + return p_variant; +} + +/****************************************************************************/ + +void RF24::setAutoAck(bool enable) { + if (enable) + write_register(EN_AA, 63); + else + write_register(EN_AA, 0); +} + +/****************************************************************************/ + +void RF24::setAutoAck(uint8_t pipe, bool enable) { + if (pipe <= 6) { + uint8_t en_aa = read_register(EN_AA); + if (enable) { + en_aa |= _BV(pipe); + } else { + en_aa &= ~_BV(pipe); + } + write_register(EN_AA, en_aa); + } +} + +/****************************************************************************/ + +bool RF24::testCarrier(void) { + return (read_register(CD) & 1); +} + +/****************************************************************************/ + +bool RF24::testRPD(void) { + return (read_register(RPD) & 1); +} + +/****************************************************************************/ + +void RF24::setPALevel(rf24_pa_dbm_e level) { + uint8_t setup = read_register(RF_SETUP); + setup &= ~(_BV(RF_PWR_LOW) | _BV(RF_PWR_HIGH)); + + // switch uses RAM (evil!) + if (level == RF24_PA_MAX) { + setup |= (_BV(RF_PWR_LOW) | _BV(RF_PWR_HIGH)); + } else if (level == RF24_PA_HIGH) { + setup |= _BV(RF_PWR_HIGH); + } else if (level == RF24_PA_LOW) { + setup |= _BV(RF_PWR_LOW); + } else if (level == RF24_PA_MIN) { + // nothing + } else if (level == RF24_PA_ERROR) { + // On error, go to maximum PA + setup |= (_BV(RF_PWR_LOW) | _BV(RF_PWR_HIGH)); + } + + write_register(RF_SETUP, setup); +} + +/****************************************************************************/ + +rf24_pa_dbm_e RF24::getPALevel(void) { + rf24_pa_dbm_e result = RF24_PA_ERROR; + uint8_t power = read_register(RF_SETUP) & (_BV(RF_PWR_LOW) | _BV(RF_PWR_HIGH)); + + // switch uses RAM (evil!) + if (power == (_BV(RF_PWR_LOW) | _BV(RF_PWR_HIGH))) { + result = RF24_PA_MAX; + } else if (power == _BV(RF_PWR_HIGH)) { + result = RF24_PA_HIGH; + } else if (power == _BV(RF_PWR_LOW)) { + result = RF24_PA_LOW; + } else { + result = RF24_PA_MIN; + } + + return result; +} + +/****************************************************************************/ + +bool RF24::setDataRate(rf24_datarate_e speed) { + bool result = false; + uint8_t setup = read_register(RF_SETUP); + + // HIGH and LOW '00' is 1Mbs - our default + wide_band = false; + setup &= ~(_BV(RF_DR_LOW) | _BV(RF_DR_HIGH)); + if (speed == RF24_250KBPS) { + // Must set the RF_DR_LOW to 1; RF_DR_HIGH (used to be RF_DR) is already 0 + // Making it '10'. + wide_band = false; + setup |= _BV(RF_DR_LOW); + } else { + // Set 2Mbs, RF_DR (RF_DR_HIGH) is set 1 + // Making it '01' + if (speed == RF24_2MBPS) { + wide_band = true; + setup |= _BV(RF_DR_HIGH); + } else { + // 1Mbs + wide_band = false; + } + } + write_register(RF_SETUP, setup); + + // Verify our result + if (read_register(RF_SETUP) == setup) { + result = true; + } else { + wide_band = false; + } + + return result; +} + +/****************************************************************************/ + +rf24_datarate_e RF24::getDataRate(void) { + rf24_datarate_e result; + uint8_t dr = read_register(RF_SETUP) & (_BV(RF_DR_LOW) | _BV(RF_DR_HIGH)); + + // switch uses RAM (evil!) + // Order matters in our case below + if (dr == _BV(RF_DR_LOW)) { + // '10' = 250KBPS + result = RF24_250KBPS; + } else if (dr == _BV(RF_DR_HIGH)) { + // '01' = 2MBPS + result = RF24_2MBPS; + } else { + // '00' = 1MBPS + result = RF24_1MBPS; + } + return result; +} + +/****************************************************************************/ +bool RF24::isChipConnected() { + uint8_t setup = read_register(SETUP_AW); + if (setup >= 1 && setup <= 3) { + return true; + } + + return false; +} +/****************************************************************************/ +void RF24::setCRCLength(rf24_crclength_e length) { + uint8_t config = read_register(CONFIG) & ~(_BV(CRCO) | _BV(EN_CRC)); + + if (length == RF24_CRC_DISABLED) { + // Do nothing, we turned it off above. + } else if (length == RF24_CRC_8) { + config |= _BV(EN_CRC); + } else { + config |= _BV(EN_CRC); + config |= _BV(CRCO); + } + write_register(CONFIG, config); +} + +/****************************************************************************/ + +rf24_crclength_e RF24::getCRCLength(void) { + rf24_crclength_e result = RF24_CRC_DISABLED; + uint8_t config = read_register(CONFIG) & (_BV(CRCO) | _BV(EN_CRC)); + + if (config & _BV(EN_CRC)) { + if (config & _BV(CRCO)) + result = RF24_CRC_16; + else + result = RF24_CRC_8; + } + + return result; +} + +/****************************************************************************/ + +void RF24::disableCRC(void) { + uint8_t disable = read_register(CONFIG) & ~_BV(EN_CRC); + write_register(CONFIG, disable); +} + +/****************************************************************************/ +void RF24::setRetries(uint8_t delay, uint8_t count) { + write_register(SETUP_RETR, (delay & 0xf) << ARD | (count & 0xf) << ARC); +} + +uint8_t RF24::min(uint8_t a, uint8_t b) { + if (a < b) + return a; + else + return b; +} diff --git a/lib/nRF24Communication/nRF24L01P/nRF24L01P.h b/lib/nRF24Communication/nRF24L01P/nRF24L01P.h new file mode 100644 index 0000000..5dcf811 --- /dev/null +++ b/lib/nRF24Communication/nRF24L01P/nRF24L01P.h @@ -0,0 +1,775 @@ +/* + Copyright (c) 2007 Stefan Engelke + + Permission is hereby granted, free of charge, to any person + obtaining a copy of this software and associated documentation + files (the "Software"), to deal in the Software without + restriction, including without limitation the rights to use, copy, + modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be + included in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT + HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +/* Memory Map */ +#define CONFIG 0x00 +#define EN_AA 0x01 +#define EN_RXADDR 0x02 +#define SETUP_AW 0x03 +#define SETUP_RETR 0x04 +#define RF_CH 0x05 +#define RF_SETUP 0x06 +#define STATUS 0x07 +#define OBSERVE_TX 0x08 +#define CD 0x09 +#define RX_ADDR_P0 0x0A +#define RX_ADDR_P1 0x0B +#define RX_ADDR_P2 0x0C +#define RX_ADDR_P3 0x0D +#define RX_ADDR_P4 0x0E +#define RX_ADDR_P5 0x0F +#define TX_ADDR 0x10 +#define RX_PW_P0 0x11 +#define RX_PW_P1 0x12 +#define RX_PW_P2 0x13 +#define RX_PW_P3 0x14 +#define RX_PW_P4 0x15 +#define RX_PW_P5 0x16 +#define FIFO_STATUS 0x17 +#define DYNPD 0x1C +#define FEATURE 0x1D + +/* Bit Mnemonics */ +#define MASK_RX_DR 6 +#define MASK_TX_DS 5 +#define MASK_MAX_RT 4 +#define EN_CRC 3 +#define CRCO 2 +#define PWR_UP 1 +#define PRIM_RX 0 +#define ENAA_P5 5 +#define ENAA_P4 4 +#define ENAA_P3 3 +#define ENAA_P2 2 +#define ENAA_P1 1 +#define ENAA_P0 0 +#define ERX_P5 5 +#define ERX_P4 4 +#define ERX_P3 3 +#define ERX_P2 2 +#define ERX_P1 1 +#define ERX_P0 0 +#define AW 0 +#define ARD 4 +#define ARC 0 +#define PLL_LOCK 4 +#define RF_DR 3 +#define RF_PWR 6 +#define RX_DR 6 +#define TX_DS 5 +#define MAX_RT 4 +#define RX_P_NO 1 +#define TX_FULL 0 +#define PLOS_CNT 4 +#define ARC_CNT 0 +#define TX_REUSE 6 +#define FIFO_FULL 5 +#define TX_EMPTY 4 +#define RX_FULL 1 +#define RX_EMPTY 0 +#define DPL_P5 5 +#define DPL_P4 4 +#define DPL_P3 3 +#define DPL_P2 2 +#define DPL_P1 1 +#define DPL_P0 0 +#define EN_DPL 2 +#define EN_ACK_PAY 1 +#define EN_DYN_ACK 0 + +/* Instruction Mnemonics */ +#define R_REGISTER 0x00 +#define W_REGISTER 0x20 +#define REGISTER_MASK 0x1F +#define ACTIVATE 0x50 +#define R_RX_PL_WID 0x60 +#define R_RX_PAYLOAD 0x61 +#define W_TX_PAYLOAD 0xA0 +#define W_ACK_PAYLOAD 0xA8 +#define FLUSH_TX 0xE1 +#define FLUSH_RX 0xE2 +#define REUSE_TX_PL 0xE3 +#define NOP 0xFF + +/* Non-P omissions */ +#define LNA_HCURR 0 + +/* P model memory Map */ +#define RPD 0x09 + +/* P model bit Mnemonics */ +#define RF_DR_LOW 5 +#define RF_DR_HIGH 3 +#define RF_PWR_LOW 1 +#define RF_PWR_HIGH 2 + +#define HIGH 1 +#define LOW 0 +#define _BV(n) (1 << n) + +/* + Copyright (C) 2011 J. Coliz + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + version 2 as published by the Free Software Foundation. + */ + +/** + * @file RF24.h + * + * Class declaration for RF24 and helper enums + */ + +#ifndef __RF24_H__ + #define __RF24_H__ + + #include + +/** + * Power Amplifier level. + * + * For use with setPALevel() + */ +typedef enum { + RF24_PA_MIN = 0, + RF24_PA_LOW, + RF24_PA_HIGH, + RF24_PA_MAX, + RF24_PA_ERROR +} rf24_pa_dbm_e; + +/** + * Data rate. How fast data moves through the air. + * + * For use with setDataRate() + */ +typedef enum { RF24_1MBPS = 0, RF24_2MBPS, RF24_250KBPS } rf24_datarate_e; + +/** + * CRC Length. How big (if any) of a CRC is included. + * + * For use with setCRCLength() + */ +typedef enum { RF24_CRC_DISABLED = 0, RF24_CRC_8, RF24_CRC_16 } rf24_crclength_e; + +/** + * Driver for nRF24L01(+) 2.4GHz Wireless Transceiver + */ + +class RF24 { + private: + DigitalOut ce_pin; /**< "Chip Enable" pin, activates the RX or TX role */ + DigitalOut csn_pin; /**< SPI Chip select */ + bool wide_band; /* 2Mbs data rate in use? */ + bool p_variant; /* False for RF24L01 and true for RF24L01P */ + uint8_t payload_size; /**< Fixed size of payloads */ + bool ack_payload_available; /**< Whether there is an ack payload waiting */ + bool dynamic_payloads_enabled; /**< Whether dynamic payloads are enabled. */ + uint8_t ack_payload_length; /**< Dynamic size of pending ack payload. */ + uint64_t pipe0_reading_address; /**< Last address set on pipe 0 for reading. */ + SPI spi; + Timer mainTimer; + + protected: + /** + * Set chip enable + * + * @param level HIGH to actively begin transmission or LOW to put in standby. Please see data + * sheet for a much more detailed description of this pin. + */ + void ce(int level); + /** + * Read a chunk of data in from a register + * + * @param reg Which register. Use constants from nRF24L01.h + * @param buf Where to put the data + * @param len How many bytes of data to transfer + * @return Current value of status register + */ + uint8_t read_register(uint8_t reg, uint8_t* buf, uint8_t len); + + /** + * Read single byte from a register + * + * @param reg Which register. Use constants from nRF24L01.h + * @return Current value of register @p reg + */ + uint8_t read_register(uint8_t reg); + + /** + * Write a chunk of data to a register + * + * @param reg Which register. Use constants from nRF24L01.h + * @param buf Where to get the data + * @param len How many bytes of data to transfer + * @return Current value of status register + */ + uint8_t write_register(uint8_t reg, const uint8_t* buf, uint8_t len); + + /** + * Write a single byte to a register + * + * @param reg Which register. Use constants from nRF24L01.h + * @param value The new value to write + * @return Current value of status register + */ + uint8_t write_register(uint8_t reg, uint8_t value); + + /** + * Write the transmit payload + * + * The size of data written is the fixed payload size, see getPayloadSize() + * + * @param buf Where to get the data + * @param len Number of bytes to be sent + * @return Current value of status register + */ + uint8_t write_payload(const void* buf, uint8_t len); + + /** + * Read the receive payload + * + * The size of data read is the fixed payload size, see getPayloadSize() + * + * @param buf Where to put the data + * @param len Maximum number of bytes to read + * @return Current value of status register + */ + uint8_t read_payload(void* buf, uint8_t len); + + /** + * Decode and print the given status to stdout + * + * @param status Status value to print + * + * @warning Does nothing if stdout is not defined. See fdevopen in stdio.h + */ + void print_status(uint8_t status); + + /** + * Decode and print the given 'observe_tx' value to stdout + * + * @param value The observe_tx value to print + * + * @warning Does nothing if stdout is not defined. See fdevopen in stdio.h + */ + void print_observe_tx(uint8_t value); + + /** + * Print the name and value of an 8-bit register to stdout + * + * Optionally it can print some quantity of successive + * registers on the same line. This is useful for printing a group + * of related registers on one line. + * + * @param name Name of the register + * @param reg Which register. Use constants from nRF24L01.h + * @param qty How many successive registers to print + */ + void print_byte_register(const char* name, uint8_t reg, uint8_t qty = 1); + + /** + * Print the name and value of a 40-bit address register to stdout + * + * Optionally it can print some quantity of successive + * registers on the same line. This is useful for printing a group + * of related registers on one line. + * + * @param name Name of the register + * @param reg Which register. Use constants from nRF24L01.h + * @param qty How many successive registers to print + */ + void print_address_register(const char* name, uint8_t reg, uint8_t qty = 1); + + /** + * Turn on or off the special features of the chip + * + * The chip has certain 'features' which are only available when the 'features' + * are enabled. See the datasheet for details. + */ + void toggle_features(void); + /**@}*/ + + public: + /** + * @name Primary public interface + * + * These are the main methods you need to operate the chip + */ + /**@{*/ + + /** + * Constructor + * + * Creates a new instance of this driver. Before using, you create an instance + * and send in the unique pins that this chip is connected to. + * + * @param _cepin The pin attached to Chip Enable on the RF module + * @param _cspin The pin attached to Chip Select + */ + RF24(PinName mosi, PinName miso, PinName sck, PinName _csnpin, PinName _cepin); + + /** + * Begin operation of the chip + * + * Call this in setup(), before calling any other methods. + */ + void begin(void); + + /** + * Set chip select pin + * + * Running SPI bus at PI_CLOCK_DIV2 so we don't waste time transferring data + * and best of all, we make use of the radio's FIFO buffers. A lower speed + * means we're less likely to effectively leverage our FIFOs and pay a higher + * AVR runtime cost as toll. + * + * @param mode HIGH to take this unit off the SPI bus, LOW to put it on + */ + void csn(int mode); + + /** + * Retrieve the current status of the chip + * + * @return Current value of status register + */ + uint8_t get_status(void); + + /** + * Empty the receive buffer + * + * @return Current value of status register + */ + uint8_t flush_rx(void); + + /** + * Empty the transmit buffer + * + * @return Current value of status register + */ + uint8_t flush_tx(void); + + /** + * Start listening on the pipes opened for reading. + * + * Be sure to call openReadingPipe() first. Do not call write() while + * in this mode, without first calling stopListening(). Call + * isAvailable() to check for incoming traffic, and read() to get it. + */ + void startListening(void); + + /** + * Stop listening for incoming messages + * + * Do this before calling write(). + */ + void stopListening(void); + + /** + * Write to the open writing pipe + * + * Be sure to call openWritingPipe() first to set the destination + * of where to write to. + * + * This blocks until the message is successfully acknowledged by + * the receiver or the timeout/retransmit maxima are reached. In + * the current configuration, the max delay here is 60ms. + * + * The maximum size of data written is the fixed payload size, see + * getPayloadSize(). However, you can write less, and the remainder + * will just be filled with zeroes. + * + * @param buf Pointer to the data to be sent + * @param len Number of bytes to be sent + * @return True if the payload was delivered successfully false if not + */ + bool write(const void* buf, uint8_t len); + + /** + * Test whether there are bytes available to be read + * + * @return True if there is a payload available, false if none is + */ + bool available(void); + + /** + * Read the payload + * + * Return the last payload received + * + * The size of data read is the fixed payload size, see getPayloadSize() + * + * @note I specifically chose 'void*' as a data type to make it easier + * for beginners to use. No casting needed. + * + * @param buf Pointer to a buffer where the data should be written + * @param len Maximum number of bytes to read into the buffer + * @return True if the payload was delivered successfully false if not + */ + bool read(void* buf, uint8_t len); + + /** + * Open a pipe for writing + * + * Only one pipe can be open at once, but you can change the pipe + * you'll listen to. Do not call this while actively listening. + * Remember to stopListening() first. + * + * Addresses are 40-bit hex values, e.g.: + * + * @code + * openWritingPipe(0xF0F0F0F0F0); + * @endcode + * + * @param address The 40-bit address of the pipe to open. This can be + * any value whatsoever, as long as you are the only one writing to it + * and only one other radio is listening to it. Coordinate these pipe + * addresses amongst nodes on the network. + */ + void openWritingPipe(uint64_t address); + + /** + * Open a pipe for reading + * + * Up to 6 pipes can be open for reading at once. Open all the + * reading pipes, and then call startListening(). + * + * @see openWritingPipe + * + * @warning Pipes 1-5 should share the first 32 bits. + * Only the least significant byte should be unique, e.g. + * @code + * openReadingPipe(1,0xF0F0F0F0AA); + * openReadingPipe(2,0xF0F0F0F066); + * @endcode + * + * @warning Pipe 0 is also used by the writing pipe. So if you open + * pipe 0 for reading, and then startListening(), it will overwrite the + * writing pipe. Ergo, do an openWritingPipe() again before write(). + * + * @todo Enforce the restriction that pipes 1-5 must share the top 32 bits + * + * @param number Which pipe# to open, 0-5. + * @param address The 40-bit address of the pipe to open. + */ + void openReadingPipe(uint8_t number, uint64_t address); + + /**@}*/ + /** + * @name Optional Configurators + * + * Methods you can use to get or set the configuration of the chip. + * None are required. Calling begin() sets up a reasonable set of + * defaults. + */ + /**@{*/ + /** + * Set the number and delay of retries upon failed submit + * + * @param delay How long to wait between each retry, in multiples of 250us, + * max is 15. 0 means 250us, 15 means 4000us. + * @param count How many retries before giving up, max 15 + */ + void setRetries(uint8_t delay, uint8_t count); + + /** + * Set RF communication channel + * + * @param channel Which RF channel to communicate on, 0-127 + */ + void setChannel(uint8_t channel); + + /** + * Set Static Payload Size + * + * This implementation uses a pre-stablished fixed payload size for all + * transmissions. If this method is never called, the driver will always + * transmit the maximum payload size (32 bytes), no matter how much + * was sent to write(). + * + * @todo Implement variable-sized payloads feature + * + * @param size The number of bytes in the payload + */ + void setPayloadSize(uint8_t size); + + /** + * Get Static Payload Size + * + * @see setPayloadSize() + * + * @return The number of bytes in the payload + */ + uint8_t getPayloadSize(void); + + /** + * Get Dynamic Payload Size + * + * For dynamic payloads, this pulls the size of the payload off + * the chip + * + * @return Payload length of last-received dynamic payload + */ + uint8_t getDynamicPayloadSize(void); + + /** + * Enable custom payloads on the acknowledge packets + * + * Ack payloads are a handy way to return data back to senders without + * manually changing the radio modes on both units. + * + * @see examples/pingpair_pl/pingpair_pl.pde + */ + void enableAckPayload(void); + + /** + * Enable dynamically-sized payloads + * + * This way you don't always have to send large packets just to send them + * once in a while. This enables dynamic payloads on ALL pipes. + * + * @see examples/pingpair_pl/pingpair_dyn.pde + */ + void enableDynamicPayloads(void); + + /** + * Determine whether the hardware is an nRF24L01+ or not. + * + * @return true if the hardware is nRF24L01+ (or compatible) and false + * if its not. + */ + bool isPVariant(void); + + /** + * Enable or disable auto-acknowlede packets + * + * This is enabled by default, so it's only needed if you want to turn + * it off for some reason. + * + * @param enable Whether to enable (true) or disable (false) auto-acks + */ + void setAutoAck(bool enable); + + /** + * Enable or disable auto-acknowlede packets on a per pipeline basis. + * + * AA is enabled by default, so it's only needed if you want to turn + * it off/on for some reason on a per pipeline basis. + * + * @param pipe Which pipeline to modify + * @param enable Whether to enable (true) or disable (false) auto-acks + */ + void setAutoAck(uint8_t pipe, bool enable); + + /** + * Set Power Amplifier (PA) level to one of four levels. + * Relative mnemonics have been used to allow for future PA level + * changes. According to 6.5 of the nRF24L01+ specification sheet, + * they translate to: RF24_PA_MIN=-18dBm, RF24_PA_LOW=-12dBm, + * RF24_PA_MED=-6dBM, and RF24_PA_HIGH=0dBm. + * + * @param level Desired PA level. + */ + void setPALevel(rf24_pa_dbm_e level); + + /** + * Fetches the current PA level. + * + * @return Returns a value from the rf24_pa_dbm_e enum describing + * the current PA setting. Please remember, all values represented + * by the enum mnemonics are negative dBm. See setPALevel for + * return value descriptions. + */ + rf24_pa_dbm_e getPALevel(void); + + /** + * Set the transmission data rate + * + * @warning setting RF24_250KBPS will fail for non-plus units + * + * @param speed RF24_250KBPS for 250kbs, RF24_1MBPS for 1Mbps, or RF24_2MBPS for 2Mbps + * @return true if the change was successful + */ + bool setDataRate(rf24_datarate_e speed); + + /** + * Fetches the transmission data rate + * + * @return Returns the hardware's currently configured datarate. The value + * is one of 250kbs, RF24_1MBPS for 1Mbps, or RF24_2MBPS, as defined in the + * rf24_datarate_e enum. + */ + rf24_datarate_e getDataRate(void); + + /** + * Set the CRC length + * + * @param length RF24_CRC_8 for 8-bit or RF24_CRC_16 for 16-bit + */ + void setCRCLength(rf24_crclength_e length); + + /** + * Get the CRC length + * + * @return RF24_DISABLED if disabled or RF24_CRC_8 for 8-bit or RF24_CRC_16 for 16-bit + */ + rf24_crclength_e getCRCLength(void); + + /** + * Disable CRC validation + * + */ + void disableCRC(void); + + /**@}*/ + /** + * @name Advanced Operation + * + * Methods you can use to drive the chip in more advanced ways + */ + /**@{*/ + + /** + * Print a giant block of debugging information to stdout + * + * @warning Does nothing if stdout is not defined. See fdevopen in stdio.h + */ + void printDetails(void); + + bool compareChannel(uint8_t channel); + + /** + * Enter low-power mode + * + * To return to normal power mode, either write() some data or + * startListening, or powerUp(). + */ + void powerDown(void); + + /** + * Leave low-power mode - making radio more responsive + * + * To return to low power mode, call powerDown(). + */ + void powerUp(void); + + /** + * Test whether there are bytes available to be read + * + * Use this version to discover on which pipe the message + * arrived. + * + * @param[out] pipe_num Which pipe has the payload available + * @return True if there is a payload available, false if none is + */ + bool available(uint8_t* pipe_num); + + /** + * Non-blocking write to the open writing pipe + * + * Just like write(), but it returns immediately. To find out what happened + * to the send, catch the IRQ and then call whatHappened(). + * + * @see write() + * @see whatHappened() + * + * @param buf Pointer to the data to be sent + * @param len Number of bytes to be sent + * @return True if the payload was delivered successfully false if not + */ + void startWrite(const void* buf, uint8_t len); + + /** + * Write an ack payload for the specified pipe + * + * The next time a message is received on @p pipe, the data in @p buf will + * be sent back in the acknowledgement. + * + * @warning According to the data sheet, only three of these can be pending + * at any time. I have not tested this. + * + * @param pipe Which pipe# (typically 1-5) will get this response. + * @param buf Pointer to data that is sent + * @param len Length of the data to send, up to 32 bytes max. Not affected + * by the static payload set by setPayloadSize(). + */ + void writeAckPayload(uint8_t pipe, const void* buf, uint8_t len); + + /** + * Determine if an ack payload was received in the most recent call to + * write(). + * + * Call read() to retrieve the ack payload. + * + * @warning Calling this function clears the internal flag which indicates + * a payload is available. If it returns true, you must read the packet + * out as the very next interaction with the radio, or the results are + * undefined. + * + * @return True if an ack payload is available. + */ + bool isAckPayloadAvailable(void); + + /** + * Call this when you get an interrupt to find out why + * + * Tells you what caused the interrupt, and clears the state of + * interrupts. + * + * @param[out] tx_ok The send was successful (TX_DS) + * @param[out] tx_fail The send failed, too many retries (MAX_RT) + * @param[out] rx_ready There is a message waiting to be read (RX_DS) + */ + void whatHappened(bool& tx_ok, bool& tx_fail, bool& rx_ready); + + bool isChipConnected(void); + + /** + * Test whether there was a carrier on the line for the + * previous listening period. + * + * Useful to check for interference on the current channel. + * + * @return true if was carrier, false if not + */ + bool testCarrier(void); + + /** + * Test whether a signal (carrier or otherwise) greater than + * or equal to -64dBm is present on the channel. Valid only + * on nRF24L01P (+) hardware. On nRF24L01, use testCarrier(). + * + * Useful to check for interference on the current channel and + * channel hopping strategies. + * + * @return true if signal => -64dBm, false if not + */ + bool testRPD(void); + + uint8_t min(uint8_t, uint8_t); +}; + +#endif // __RF24_H__ +// vim:ai:cin:sts=2 sw=2 ft=cpp diff --git a/lib/proto/CommTypes.proto b/lib/proto/CommTypes.proto new file mode 100644 index 0000000..9975cef --- /dev/null +++ b/lib/proto/CommTypes.proto @@ -0,0 +1,76 @@ +syntax = "proto3"; + +message protoSpeedSSL{ + double vx = 1; + double vy = 2; + double vw = 3; + bool front = 4; + bool chip = 5; + bool charge = 6; + double kickStrength = 7 ; + bool dribbler = 8; + double dribSpeed = 9; +}; + +message protoOdometrySSL{ + double x = 1; + double y = 2; + double w = 3; + bool hasBall = 4; + double kickLoad = 5; + double battery = 6; + int32 count = 7; + double vision_x = 8; + double vision_y = 9; + double vision_w = 10; + double vx = 11; + double vy = 12; + double vw = 13; +}; + +message protoPositionSSL{ + double x = 1; + double y = 2; + double w = 3; + enum PosType{ + unknown = 0; + source = 1; + stop = 2; + driveToTarget = 3; + rotateOnSelf = 4; + rotateInPoint = 5; + } + PosType posType = 4; + bool front = 5; + bool chip = 6; + bool charge = 7; + double kickStrength = 8 ; + bool dribbler = 9; + double dribSpeed = 10; + bool resetOdometry = 11; + double min_speed = 12; + double max_speed = 13; +}; + +message protoMotorsPWMSSL{ + double m1 = 1; + double m2 = 2; + double m3 = 3; + double m4 = 4; +} + +message protoMotorsDataSSL{ + double current_m1 = 1; + double current_m2 = 2; + double current_m3 = 3; + double current_m4 = 4; + double pwm_m1 = 5; + double pwm_m2 = 6; + double pwm_m3 = 7; + double pwm_m4 = 8; + double desired_m1 = 9; + double desired_m2 = 10; + double desired_m3 = 11; + double desired_m4 = 12; + double msgTime = 13; +} \ No newline at end of file diff --git a/lib/readme.txt b/lib/readme.txt new file mode 100644 index 0000000..cfa16df --- /dev/null +++ b/lib/readme.txt @@ -0,0 +1,41 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link them to executable files. + +The source code of each library should be placed in separate directories, like +"lib/private_lib/[here are source files]". + +For example, see the structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- readme.txt --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/mbed_app.json b/mbed_app.json new file mode 100644 index 0000000..5c16dc5 --- /dev/null +++ b/mbed_app.json @@ -0,0 +1,7 @@ +{ + "target_overrides": { + "*": { + "target.printf_lib": "std" + } + } +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..5d30f77 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,26 @@ +; 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:nucleo_f767zi] +platform = ststm32 +board = nucleo_f767zi +framework = mbed +monitor_speed = 230400 +upload_protocol = mbed + +lib_deps = nanopb/Nanopb@^0.4.6 + +custom_nanopb_protos = + + +build_flags = + -std=gnu++11 -O2 + -I .pio/libdeps/nucleo_f767zi/Nanopb + -I .pio/build/nucleo_f767zi/nanopb/generated-src +build_unflags = -std=c++98 -std=gnu++98 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..bf1c1f8 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,170 @@ +// RobôCIn SSL - v1.4 +#include "defines.h" +#include "utils.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define ROBOT_MODE (ExecMode::GAME_ON) + +nRF24Communication radio_recv(NRF_R_MOSI, + NRF_R_MISO, + NRF_R_SCK, + NRF_R_CE, + NRF_R_CSN, + NRF_R_VCC, + NetworkType::ssl, + RadioFunction::receiver); + +nRF24Communication radio_send(NRF_S_MOSI, + NRF_S_MISO, + NRF_S_SCK, + NRF_S_CE, + NRF_S_CSN, + NRF_S_VCC, + NetworkType::ssl, + RadioFunction::sender); + +EthCommunication eth_communication(ip, netmask, gateway, port); + +Kicker kick(PIN_CHARGE, PIN_CAP_LOAD, PIN_FRONT, PIN_CHIP, PIN_IR_LED, PIN_IR_ADC); + +KickFlags isKick; +RobotInfo robotInfo; +Vector motionSpeed; +Vector currentSpeed; +Kinematics kinematics; +RobotPosition packetPos; +CurrentSensor currentSensor; + +MotionControl motion(&kinematics); +Odometry odometry(&motion, &kinematics, ODOMETRY_TSAMPLE, GYRO_TSAMPLE); // Odm at 5ms, Gyro as 5ms +Navigation navigation(&kinematics, &motion, &odometry); +Debugger debbuger(&motion, &odometry, &kick, &radio_recv, &radio_send, ¤tSensor); + +static BufferedSerial serial_port(USBTX, USBRX, 230400); +FileHandle* mbed::mbed_override_console(int fd) { + return &serial_port; +} + +Timer msgTimeout; +Timer msgTelemetry; + +uint8_t pcktCount = 0; + +int main() { + kick.init(); + utils::initRobot(); + utils::checkBattery(); + + motion.init(); + + radio_recv.setup(utils::getRobotId()); + radio_send.setup(utils::getRobotId()); + radio_recv.enable(); + + msgType recvType = msgType::NONE; + + ThisThread::sleep_for(100ms); + Status::clearColors(); + msgTimeout.start(); + msgTelemetry.start(); + + debbuger.processRobot(ROBOT_MODE); + + // Init odometry after debugger for avoiding calibration beep + odometry.init(); + + // GAME_ON + while (1) { + // Process Gyroscope + odometry.processGyro(); + + // New packet? + bool newPacket = false; + + // Tries to receive radio message! + if (radio_recv.updatePacket() && radio_recv.getGameState() != refereeCommand::halt) { + recvType = radio_recv.getPacketType(); + msgTimeout.reset(); + newPacket = true; + } + + // Find the speed to navigate robot. + if (recvType == msgType::SSL_SPEED) { + // Check movement lock + motion.moveIsLocked(radio_recv.robotMoveIsLocked()); + // Update desired speed + navigation.update(motionSpeed, radio_recv.getVectorSpeed()); + } else if (recvType == msgType::POSITION) { + if (newPacket) { + navigation.update(radio_recv.getLastPosition()); + } + navigation.move(motionSpeed); + } + + if (utils::timerMillisExpired(msgTimeout, NRF_NO_MSG_TIME)) { + if (utils::timerMillisExpired(msgTimeout, NRF_NO_MSG_TIME + NRF_HALT_TIME)) { + // Coast robot after 1s of break. + motion.stopRobot(); + if (utils::pressedFor(utils::PBT1, 2000) == ButtonState::HELD || + utils::pressedFor(utils::PBT2, 2000) == ButtonState::HELD) { + debbuger.processRobot(ExecMode::TEST); + } + } else { + // Brake robot. + motion.accelRobot(Vector(0, 0, 0)); + } + kick.stopCharge(); + } else { + // Move robot with speed + motion.accelRobot(motionSpeed); + // Control Kicker + radio_recv.getKick(isKick); + kick.update(isKick, + ((radio_recv.getGameState() == refereeCommand::ballPlacementYellow || + radio_recv.getGameState() == refereeCommand::ballPlacementBlue) ? + BALL_PLACEMENT_MIN_CHARGE : + DEFAULT_MIN_CHARGE), + MAX_CHARGE); + // Control Dribbler + motion.updateDribbler(isKick); + } + +#ifdef DEFAULT_PCKT + // Sending Telemetry + if (utils::timerRead(msgTelemetry) > DEFAULT_FEEDBACK_TIME) { + motion.getMotorsInfo(robotInfo); + robotInfo.current = currentSensor.getMotorsCurrent(); + kick.getKickerInfo(robotInfo.kickLoad, robotInfo.ball); + robotInfo.battery = utils::getBattery(); + robotInfo.type = msgType::TELEMETRY; + robotInfo.count = pcktCount++; + radio_send.sendTelemetryPacket(robotInfo); + msgTelemetry.reset(); + } +#endif +#ifdef ODOMETRY_PCKT + // Sending Odometry + if (utils::timerRead(msgTelemetry) > ODOMETRY_TIME) { + // Get robot position + odometry.getCurrentPosition(robotInfo.v); + // Get motors speed + motion.getMotorsInfo(robotInfo); + motion.getRobotSpeed(currentSpeed); + kick.getKickerInfo(robotInfo.kickLoad, robotInfo.ball); + robotInfo.battery = utils::getBattery(); + robotInfo.type = msgType::ODOMETRY; + robotInfo.count = pcktCount++; + radio_send.sendOdometryPacket(robotInfo); + msgTelemetry.reset(); + } +#endif + } +} diff --git a/test/KickSpeed/.gitignore b/test/KickSpeed/.gitignore new file mode 100644 index 0000000..862cbb3 --- /dev/null +++ b/test/KickSpeed/.gitignore @@ -0,0 +1,3 @@ +.DS_Store +.pio/* +.vscode/* \ No newline at end of file diff --git a/test/KickSpeed/lib/README b/test/KickSpeed/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/test/KickSpeed/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/test/KickSpeed/lib/Sseg/Sseg.cpp b/test/KickSpeed/lib/Sseg/Sseg.cpp new file mode 100644 index 0000000..b2f1081 --- /dev/null +++ b/test/KickSpeed/lib/Sseg/Sseg.cpp @@ -0,0 +1,372 @@ +/* + Sseg.cpp - mbed library for 2/4/8 digit seven segment LED driver. + Copyright 2013,2014,2015 by morecat_lab + + base on Dots library. + Copyright 2010 arms22. All right reserved. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +*/ + +#include "Sseg.h" +#include + +const int Sseg::numConv[] = {NUM_PAT_0, + NUM_PAT_1, + NUM_PAT_2, + NUM_PAT_3, + NUM_PAT_4, + NUM_PAT_5, + NUM_PAT_6, + NUM_PAT_7, + NUM_PAT_8, + NUM_PAT_9, + NUM_PAT_A, + NUM_PAT_B, + NUM_PAT_C, + NUM_PAT_D, + NUM_PAT_E, + NUM_PAT_F}; + +// 2 digit +Sseg::Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2) { + _numOfDigs = 2; + PinName segPins[8] = {a, b, c, d, e, f, g, dp}; + PinName digPins[2] = {d1, d2}; + + for (int i = 0; i < 8; i++) { + _segPins[i] = new DigitalOut(segPins[i]); + } + + for (int i = 0; i < _numOfDigs; i++) { + _digPins[i] = new DigitalOut(digPins[i]); + } + + _updateInterval = (8333 / 2); + _zeroSupress = true; + _kcommon = false; + _sinkDriver = false; +} + +// 4 digit +Sseg::Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2, + PinName d3, + PinName d4) { + _numOfDigs = 4; + PinName segPins[8] = {a, b, c, d, e, f, g, dp}; + PinName digPins[4] = {d1, d2, d3, d4}; + + for (int i = 0; i < 8; i++) { + _segPins[i] = new DigitalOut(segPins[i]); + } + + for (int i = 0; i < _numOfDigs; i++) { + _digPins[i] = new DigitalOut(digPins[i]); + } + _updateInterval = (8333 / 4); + _zeroSupress = true; + _kcommon = false; + _sinkDriver = false; +} + +// 8 digit +Sseg::Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2, + PinName d3, + PinName d4, + PinName d5, + PinName d6, + PinName d7, + PinName d8) { + _numOfDigs = 8; + PinName segPins[8] = {a, b, c, d, e, f, g, dp}; + PinName digPins[8] = {d1, d2, d3, d4, d5, d6, d7, d8}; + + for (int i = 0; i < 8; i++) { + _segPins[i] = new DigitalOut(segPins[i]); + } + + for (int i = 0; i < _numOfDigs; i++) { + _digPins[i] = new DigitalOut(digPins[i]); + } + _updateInterval = (8333 / 8); + _zeroSupress = true; + _kcommon = false; + _sinkDriver = false; +} + +void Sseg::begin(void) { + timer.start(); + clear(); +} + +void Sseg::setAcommon(void) { + _kcommon = false; +} + +void Sseg::setKcommon(void) { + _kcommon = true; +} + +void Sseg::setSinkDriver(void) { + _sinkDriver = true; +} + +char Sseg::segCh(char i) { + return Sseg::numConv[i]; +} + +void Sseg::setDot(int d) { + _buffer[d] |= 0x01; +} + +void Sseg::clearDot(int d) { + _buffer[d] &= 0xfe; +} + +void Sseg::writeNum(int n) { + if (_numOfDigs == 2) { + writeNum2(n); + } else if (_numOfDigs == 4) { + writeNum4(n); + } else if (_numOfDigs == 8) { + writeNum8((long) n); + } +} + +void Sseg::writeNum2(int n) { + if (n < 100) { + _buffer[0] = segCh((n % 100) / 10); + _buffer[1] = segCh(n % 10); + } else { + _buffer[0] = _buffer[1] = 0x02; // overflow + } +} + +void Sseg::writeNum4(int n) { + if (n < 10000) { + _buffer[0] = segCh((n % 10000) / 1000); + _buffer[1] = segCh((n % 1000) / 100); + _buffer[2] = segCh((n % 100) / 10); + _buffer[3] = segCh(n % 10); + } else { + _buffer[0] = _buffer[1] = _buffer[2] = _buffer[3] = 0x02; // overflow + } +} + +void Sseg::writeNum8(int n) { + _buffer[0] = segCh((n % 100000000) / 10000000); + _buffer[1] = segCh((n % 10000000) / 1000000); + _buffer[2] = segCh((n % 1000000) / 100000); + _buffer[3] = segCh((n % 100000) / 10000); + _buffer[4] = segCh((n % 10000) / 1000); + _buffer[5] = segCh((n % 1000) / 100); + _buffer[6] = segCh((n % 100) / 10); + _buffer[7] = segCh(n % 10); +} + +void Sseg::writeNum(char d1, char d2) { + _buffer[0] = segCh(d1); + _buffer[1] = segCh(d2); +} + +void Sseg::writeNum(char d1, char d2, char d3, char d4) { + _buffer[0] = segCh(d1); + _buffer[1] = segCh(d2); + _buffer[2] = segCh(d3); + _buffer[3] = segCh(d4); +} + +void Sseg::writeNum(char d1, char d2, char d3, char d4, char d5, char d6, char d7, char d8) { + _buffer[0] = segCh(d1); + _buffer[1] = segCh(d2); + _buffer[2] = segCh(d3); + _buffer[3] = segCh(d4); + _buffer[4] = segCh(d5); + _buffer[5] = segCh(d6); + _buffer[6] = segCh(d7); + _buffer[7] = segCh(d8); +} + +void Sseg::writeHex(int n) { + if (_numOfDigs == 2) { + _buffer[0] = segCh((n >> 4) & 0xf); + _buffer[1] = segCh(n & 0xf); + } else if (_numOfDigs == 4) { + _buffer[0] = segCh((n >> 12) & 0xf); + _buffer[1] = segCh((n >> 8) & 0xf); + _buffer[2] = segCh((n >> 4) & 0xf); + _buffer[3] = segCh(n & 0xf); + } +} + +void Sseg::writeHex(long n) { + _buffer[0] = segCh((n >> 28) & 0xf); + _buffer[1] = segCh((n >> 24) & 0xf); + _buffer[2] = segCh((n >> 20) & 0xf); + _buffer[3] = segCh((n >> 16) & 0xf); + _buffer[4] = segCh((n >> 12) & 0xf); + _buffer[5] = segCh((n >> 8) & 0xf); + _buffer[6] = segCh((n >> 4) & 0xf); + _buffer[7] = segCh(n & 0xf); +} + +void Sseg::setZeroSupress(bool t) { + _zeroSupress = t; +} + +void Sseg::supressZero() { + int i; + if (_zeroSupress) { + for (i = 0; i < (_numOfDigs - 1); i++) { + if (_buffer[i] == segCh(0)) { + _buffer[i] = _buffer[i] & 0x1; + } else { + break; + } + } + } +} + +void Sseg::writeRawData(char d1, char d2) { + _buffer[0] = d1; + _buffer[1] = d2; +} + +void Sseg::writeRawData(char d1, char d2, char d3, char d4) { + _buffer[0] = d1; + _buffer[1] = d2; + _buffer[2] = d3; + _buffer[3] = d4; +} + +void Sseg::writeRawData(char d1, char d2, char d3, char d4, char d5, char d6, char d7, char d8) { + _buffer[0] = d1; + _buffer[1] = d2; + _buffer[2] = d3; + _buffer[3] = d4; + _buffer[4] = d5; + _buffer[5] = d6; + _buffer[6] = d7; + _buffer[7] = d8; +} + +void Sseg::write(uint8_t x, uint8_t y, uint8_t value) { + uint8_t tmp, msk; + tmp = _buffer[y]; + msk = 0x80 >> x; + tmp = tmp & ~msk; + tmp = tmp | (value ? msk : 0); + _buffer[y] = tmp; +} + +void Sseg::write(uint8_t d, uint8_t value) { + _buffer[d] = value; +} + +void Sseg::write(uint8_t y, const uint8_t values[], uint8_t size) { + uint8_t i; + for (i = 0; i < size; i++) + _buffer[(y++) & 0x07] = values[i]; +} + +void Sseg::clear(void) { + int i; + for (i = 0; i < 8; i++) { + _buffer[i] = 0; + } + _dig = _numOfDigs - 1; +} + +void Sseg::turnOff(void) { + _digPins[_dig]->write((_kcommon) ? 1 : 0); +} + +void Sseg::turnOn(void) { + _digPins[_dig]->write((_kcommon) ? 0 : 1); +} + +void Sseg::updateSeg(void) { + char i, data, mask; + if ((++_dig) >= _numOfDigs) + _dig = 0; + data = _buffer[_dig]; + mask = 0x80; + for (i = 0; i < 8; i++) { + if (data & mask) { + _segPins[i]->write(((_kcommon) || (_sinkDriver)) ? 1 : 0); + } else { + _segPins[i]->write(((_kcommon) || (_sinkDriver)) ? 0 : 1); + } + mask >>= 1; + } +} + +bool Sseg::update(void) { + int t = timer.read_us(); + bool sync = false; + if ((t - _lastUpdateTime) > _updateInterval) { + turnOff(); + updateSeg(); + turnOn(); + _lastUpdateTime = t; + sync = (_dig == 0); + } + return sync; +} + +void Sseg::updateWithDelay(int ms) { + timer.reset(); // to avoid overflow 32bit counter (~=30min) + int start = timer.read_ms(); + do { + bool sync = update(); + if (sync) { + int t = timer.read_ms(); + if ((t - start) >= ms) { + break; + } + } + } while (1); +} + +void Sseg::updateOnce(void) { + uint8_t i; + _dig = _numOfDigs - 1; + turnOff(); + for (i = 0; i < _numOfDigs; i++) { + updateSeg(); + turnOn(); + wait_us(1000); // wait 1ms + turnOff(); + } +} + +// EOF diff --git a/test/KickSpeed/lib/Sseg/Sseg.h b/test/KickSpeed/lib/Sseg/Sseg.h new file mode 100644 index 0000000..4029ca4 --- /dev/null +++ b/test/KickSpeed/lib/Sseg/Sseg.h @@ -0,0 +1,380 @@ +/* + SSeg.cpp - mbed library for 2/4/8 digit seven segment LED driver. + Copyright 2013,2014,2015 by morecat_lab + + based on Dots library. + Copyright 2010 arms22. All right reserved. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +*/ + +#ifndef SSEG_H +#define SSEG_H + +#include "mbed.h" +#include + +#define NUM_PAT_0 0xfc +#define NUM_PAT_1 0x60 +#define NUM_PAT_2 0xda +#define NUM_PAT_3 0xf2 +#define NUM_PAT_4 0x66 +#define NUM_PAT_5 0xb6 +#define NUM_PAT_6 0xbe +#define NUM_PAT_7 0xe0 +#define NUM_PAT_8 0xfe +#define NUM_PAT_9 0xf6 +#define NUM_PAT_A 0xee +#define NUM_PAT_B 0x3e +#define NUM_PAT_C 0x9c +#define NUM_PAT_D 0x7a +#define NUM_PAT_E 0x9e +#define NUM_PAT_F 0x8e + +/** + * Seven segment LED driver library + */ +class Sseg { + private: + unsigned long _lastUpdateTime; + int _updateInterval; + DigitalOut* _segPins[8]; // changed to use array + DigitalOut* _digPins[8]; // changed to use array + char _buffer[8]; + int _numOfDigs; + int _dig; // support 4 or 8 + bool _zeroSupress; + bool _kcommon; // Cathode-common flag + bool _sinkDriver; // invert segment drive, if true + void initConv(void); + Timer timer; + + public: + static const int numConv[16]; + /** + * create an 2 digit seven segment driver + * + * @param PinName a Pin No for segment A + * @param PinName b Pin No for segment B + * @param PinName c Pin No for segment C + * @param PinName d Pin No for segment D + * @param PinName e Pin No for segment E + * @param PinName f Pin No for segment F + * @param PinName g Pin No for segment G + * @param PinName dp Pin No for segment DP + * @param PinName d1 Pin No for dight 1 + * @param PinName d2 Pin No for dight 2 +     */ + Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2); + + /** + * create an 4 digit seven segment driver + * + * @param PinName a Pin No for segment A + * @param PinName b Pin No for segment B + * @param PinName c Pin No for segment C + * @param PinName d Pin No for segment D + * @param PinName e Pin No for segment E + * @param PinName f Pin No for segment F + * @param PinName g Pin No for segment G + * @param PinName dp Pin No for segment DP + * @param PinName d1 Pin No for dight 1 + * @param PinName d2 Pin No for dight 2 + * @param PinName d3 Pin No for dight 3 + * @param PinName d4 Pin No for dight 4 + */ + Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2, + PinName d3, + PinName d4); + + /** + * create an 8 digit seven segment driver + * + * @param PinName a Pin No for segment A + * @param PinName b Pin No for segment B + * @param PinName c Pin No for segment C + * @param PinName d Pin No for segment D + * @param PinName e Pin No for segment E + * @param PinName f Pin No for segment F + * @param PinName g Pin No for segment G + * @param PinName dp Pin No for segment DP + * @param PinName d1 Pin No for dight 1 + * @param PinName d2 Pin No for dight 2 + * @param PinName d3 Pin No for dight 3 + * @param PinName d4 Pin No for dight 4 + * @param PinName d5 Pin No for dight 5 + * @param PinName d6 Pin No for dight 6 + * @param PinName d7 Pin No for dight 7 + * @param PinName d8 Pin No for dight 8 + */ + Sseg(PinName a, + PinName b, + PinName c, + PinName d, + PinName e, + PinName f, + PinName g, + PinName dp, + PinName d1, + PinName d2, + PinName d3, + PinName d4, + PinName d5, + PinName d6, + PinName d7, + PinName d8); + + /** + * start driver + */ + void begin(void); + + /** + * use Kathode Common LED + */ + void setKcommon(void); + + /** + * use Anode Common LED (default) + */ + void setAcommon(void); + + /** + * sink driver + */ + void setSinkDriver(void); + + /** + * get a charcter pattern from a number + * + * @param i number + * + * @returns bit pattern of number i + * + */ + char segCh(char i); + + /** + * turn on DP + * + * @param d dight + * + */ + void setDot(int d); + + /** + * turn off DP + * + * @param d dight + * + */ + void clearDot(int d); + + /** + * write a number to LED + * + * @param d number + * + */ + void writeNum(int n); + + /** + * write a number to 2 dight LED + * + * @param n number + * + */ + void writeNum2(int n); + + /** + * write a number to 4 dight LED + * + * @param n number + * + */ + void writeNum4(int n); + + /** + * write a number to 8 dight LED + * + * @param n number + * + */ + void writeNum8(int n); + + /** + * write numbers to each dight of 2 dight LED + * + * @param d1 digit 1 number + * @param d2 digit 2 number + * + */ + void writeNum(char d1, char d2); + + /** + * write numbers to each dight of 4 dight LED + * + * @param d1 digit 1 number + * @param d2 digit 2 number + * @param d3 digit 3 number + * @param d4 digit 4 number + * + */ + void writeNum(char d1, char d2, char d3, char d4); + + /** + * write numbers to each dight of 8 dight LED + * + * @param d1 digit 1 number + * @param d2 digit 2 number + * @param d3 digit 3 number + * @param d4 digit 4 number + * @param d5 digit 5 number + * @param d6 digit 6 number + * @param d7 digit 7 number + * @param d8 digit 8 number + * + */ + void writeNum(char d1, char d2, char d3, char d4, char d5, char d6, char d7, char d8); + + /** + * zero supress: tell driver not to display 0 in the left + * + */ + void supressZero(); + + /** + * control zero supress bit + * + * @param t, 1:supress on, 0:supress off + * + */ + void setZeroSupress(bool t); + + /** + * write hex number to LED + * + * @param n number + * + */ + void writeHex(int n); + + /** + * write hex number to LED + * + * @param n (long)number + * + */ + void writeHex(long n); + + /** + * write patterns to each dight of 2 dight LED + * + * @param d1 digit 1 pattern + * @param d2 digit 2 pattern + * + */ + void writeRawData(char d1, char d2); + + /** + * write patterns to each dight of 4 dight LED + * + * @param d1 digit 1 pattern + * @param d2 digit 2 pattern + * @param d3 digit 3 pattern + * @param d4 digit 4 pattern + * + */ + void writeRawData(char d1, char d2, char d3, char d4); + + /** + * write patterns to each dight of 8 dight LED + * + * @param d1 digit 1 pattern + * @param d2 digit 2 pattern + * @param d3 digit 3 pattern + * @param d4 digit 4 pattern + * @param d5 digit 5 pattern + * @param d6 digit 6 pattern + * @param d7 digit 7 pattern + * @param d8 digit 8 pattern + * + */ + void writeRawData(char d1, char d2, char d3, char d4, char d5, char d6, char d7, char d8); + + void write(uint8_t x, uint8_t y, uint8_t value); + + /** + * write patterns to a dight + * + * @param d digit + * + * @param value pattern + * + */ + void write(uint8_t d, uint8_t value); + void write(uint8_t y, const uint8_t values[], uint8_t size); + + /** + * Clear LED buffer + */ + void clear(void); + + /** + * Turn off LED + */ + void turnOff(void); + + /** + * Turn on LED + */ + void turnOn(void); + + /** + * Update One dight of LED + */ + void updateSeg(void); + + /** + * Update LED (by internal clock) + * + * @returns sync = 1, if digit == 0 + * + */ + bool update(void); + + /** + * keep updating LED for specified period + * + * @param ms period (ms) + * + */ + void updateWithDelay(int ms); + + /** + * Update LED Once with 1ms delay + */ + void updateOnce(void); +}; + +#endif // SSEG.h diff --git a/test/KickSpeed/platformio.ini b/test/KickSpeed/platformio.ini new file mode 100644 index 0000000..964d4d7 --- /dev/null +++ b/test/KickSpeed/platformio.ini @@ -0,0 +1,18 @@ +;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:lpc1768] +platform = nxplpc +board = lpc1768 +framework = mbed +monitor_speed = 9600 +build_flags = -std=gnu++11 -O2 +build_unflags = -std=c++98 -std=gnu++98 + diff --git a/test/KickSpeed/src/main.cpp b/test/KickSpeed/src/main.cpp new file mode 100644 index 0000000..c15594a --- /dev/null +++ b/test/KickSpeed/src/main.cpp @@ -0,0 +1,66 @@ +#include +#include + +Serial pc(USBTX, USBRX); +Timer delta; + +DigitalOut irLed_1(p15); +AnalogIn irAdc_1(p16); + +DigitalOut irLed_2(p17); +AnalogIn irAdc_2(p18); + +// 7seg +#define pinA p19 +#define pinB p20 +#define pinC p21 +#define pinD p22 +#define pinE p23 +#define pinF p24 +#define pinG p25 +#define pinDP p30 +#define pinD1 p26 +#define pinD2 p27 +#define pinD3 p28 +#define pinD4 p29 + +Sseg Setseg = Sseg(pinA, pinB, pinC, pinD, pinE, pinF, pinG, pinDP, pinD1, pinD2, pinD3, pinD4); + +int main() { + irLed_1.write(1); + irLed_2.write(1); + double delay; + double speed; + int speedToDisplay; + Setseg.begin(); + + Setseg.writeNum(0); + while (1) { + + while (irAdc_1.read() > 0.01) { + Setseg.update(); + } + delta.start(); + + while (irAdc_2.read() > 0.01) { + Setseg.update(); + } + delay = delta.read_ms(); + delay = delay / 1000; + speed = (0.1 / delay); + speedToDisplay = (int) (speed * 1000); + + pc.printf("Speed(m/s): %f \n", speed); + + // overflow case + if (speedToDisplay > 9999) { + Setseg.writeRawData(NUM_PAT_F, NUM_PAT_F, NUM_PAT_F, NUM_PAT_F); + } else { + Setseg.writeNum(speedToDisplay); + Setseg.setDot(0); + } + + delta.stop(); + delta.reset(); + } +}