├── LICENSE ├── README.md ├── java └── GridMapGL │ ├── .classpath │ ├── .gitignore │ ├── .project │ ├── .settings │ ├── org.eclipse.buildship.core.prefs │ └── org.eclipse.jdt.core.prefs │ ├── LICENSE │ ├── build.gradle │ ├── gradle.properties │ ├── gradle │ └── wrapper │ │ ├── gradle-wrapper.jar │ │ └── gradle-wrapper.properties │ ├── gradlew │ ├── gradlew.bat │ ├── imgui.ini │ ├── license.txt │ ├── res │ └── shaders │ │ └── basic.shader │ ├── settings.gradle │ └── src │ ├── imgui.ini │ └── main │ └── java │ └── com │ └── fmsz │ └── gridmapgl │ ├── app │ ├── DataEventHandler.java │ ├── DataRecorder.java │ ├── GridMapApp.java │ ├── IApplication.java │ ├── ObjectSerializer.java │ └── Util.java │ ├── conn │ ├── ConnectionManager.java │ ├── ConnectionThread.java │ ├── IConnection.java │ ├── NetworkConnection.java │ └── SerialConnection.java │ ├── core │ └── Main2.java │ ├── graphics │ ├── Camera.java │ ├── Color.java │ ├── PrimitiveRenderer.java │ ├── ShapeRenderer.java │ └── gl │ │ ├── Shader.java │ │ ├── VertexArray.java │ │ ├── VertexBuffer.java │ │ └── VertexBufferLayout.java │ ├── math │ ├── MathUtil.java │ └── Transform.java │ └── slam │ ├── GridMap.java │ ├── GridMapLoader.java │ ├── Observation.java │ ├── Odometry.java │ ├── ParticleFilter.java │ ├── Pose.java │ ├── RayIterator.java │ ├── Robot.java │ ├── SLAM.java │ ├── SensorModel.java │ └── TimeFrame.java └── robot ├── .gitignore ├── ARDUINO_SKETCH ├── ARDUINO_SKETCH.ino ├── ARDUINO_SKETCH.ino.bak └── encoder_functions.ino ├── esp32 ├── .gitignore ├── TFmini.h ├── encoder.cpp ├── encoder.h ├── esp32.ino ├── motors.cpp ├── motors.h ├── pid.cpp ├── pid.h ├── pins.h ├── sensor.cpp ├── sensor.h └── wifi_settings.example.h └── teensy ├── TFmini.h └── teensy.ino /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Gridmap SLAM Robot 2 | 3 | This is the source code and some documentation for my differential drive robot. I've written a backend in Java using OpenGL to visualize the LIDAR readings and control the robot. The lidar unit is home made using a single [TFMini](http://www.benewake.com/en/tfmini.html) sensor mounted on a rotating gear. The gear is driven by a stepper motor through a timing belt, and the signals are transfered using a cheap slip ring. Compared to the two [VL53L1X](https://www.pololu.com/product/3415) laser ToF distance sensors previously used, the TFMini sensor uses a strong IR LED (and no laser) and can measure up to 12 meters at a rate of 100 Hz. 4 | 5 | The robot is currently equipped with an ESP32 controlling the LIDAR stepper motor and the two DC motors for driving, as well as counting the motor encoder pulses and sending them back to the software running on the computer. The encoder count is then used as odometry control input to the SLAM algorithm. Communication with the host machine is done through WiFi which, together with a LiPo battery, makes the robot completely wireless! 6 | 7 | In the future i intend to possibly move away from my own implementation of the SLAM algorithm to an existing one and maybe even to ROS. 8 | 9 | [Here](https://photos.app.goo.gl/9LCzzc31TMR4rb7r5) is a link to an older video of the robot in action. As you can see, the implementation kind of works, but still need much improvement to be really useable. My goal is to eventually have a robot that can wander around autonomously, creating a map of the environment! 10 | 11 | ## Useful links and resources on the subject 12 | 13 | ### SLAM 14 | 15 | * [tinySLAM](https://openslam-org.github.io/tinyslam.html) 16 | 17 | * [ROS: mrpt_icp_slam_2d](http://wiki.ros.org/mrpt_icp_slam_2d) 18 | 19 | * [Field and Service Robotics: "beam endpoint model"](https://books.google.se/books?id=G09sCQAAQBAJ&pg=PA107&hl=sv&source=gbs_selected_pages&cad=3#v=onepage&q=beam%20endpoint%20model&f=false) 20 | 21 | * [Probabilistic robotics / Sebastian Thrun, Wolfram Burgard, Dieter Fox]() - an excelent book on the subject. 22 | 23 | * YouTube: [SLAM Course - WS13/14](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_) - great video recordings of a course on robotics and SLAM. Held by [Cyrill Stachniss](https://www.youtube.com/channel/UCi1TC2fLRvgBQNe-T4dp8Eg) at the University of Freiburg, Germany and contains a lot of references to the book above. 24 | 25 | * YouTube: [SLAM Lectures](https://www.youtube.com/playlist?list=PLpUPoM7Rgzi_7YWn14Va2FODh7LzADBSm) - another series of videos explaining how to implement a "simple" land mark based SLAM algorithm in python by [Claus Brenner](https://www.youtube.com/channel/UCQoNsqW4v8uvrpWxnIabStg). 26 | 27 | ### ESP32 28 | 29 | * [ESP32 Arduino Core](https://github.com/espressif/arduino-esp32) 30 | * [Static IP](https://randomnerdtutorials.com/esp32-static-fixed-ip-address-arduino-ide/) 31 | * [Soft AP](https://randomnerdtutorials.com/esp32-access-point-ap-web-server/) 32 | * [Board Pinout (using 30 GPIO version)](https://randomnerdtutorials.com/getting-started-with-esp32/) 33 | * [PWM](https://randomnerdtutorials.com/esp32-pwm-arduino-ide/) 34 | * [ESP32 Exception decoder](https://github.com/me-no-dev/EspExceptionDecoder]) 35 | * [ESP-IDF Programming Guide about FreeRTOS](https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/system/freertos.html) 36 | -------------------------------------------------------------------------------- /java/GridMapGL/.classpath: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /java/GridMapGL/.gitignore: -------------------------------------------------------------------------------- 1 | /.gradle/ 2 | /bin/ 3 | /build/ 4 | /maps/* 5 | 6 | .vscode -------------------------------------------------------------------------------- /java/GridMapGL/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | GridMapGL 4 | Project GridMapGL created by Buildship. 5 | 6 | 7 | 8 | 9 | org.eclipse.jdt.core.javabuilder 10 | 11 | 12 | 13 | 14 | org.eclipse.buildship.core.gradleprojectbuilder 15 | 16 | 17 | 18 | 19 | org.jetbrains.kotlin.ui.kotlinBuilder 20 | 21 | 22 | 23 | 24 | 25 | org.eclipse.jdt.core.javanature 26 | org.eclipse.buildship.core.gradleprojectnature 27 | org.jetbrains.kotlin.core.kotlinNature 28 | 29 | 30 | 31 | kotlin_bin 32 | 2 33 | org.jetbrains.kotlin.core.filesystem:/GridMapGL/kotlin_bin 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /java/GridMapGL/.settings/org.eclipse.buildship.core.prefs: -------------------------------------------------------------------------------- 1 | connection.project.dir= 2 | eclipse.preferences.version=1 3 | -------------------------------------------------------------------------------- /java/GridMapGL/.settings/org.eclipse.jdt.core.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | org.eclipse.jdt.core.builder.resourceCopyExclusionFilter=*.kt 3 | org.eclipse.jdt.core.compiler.codegen.targetPlatform=11 4 | org.eclipse.jdt.core.compiler.compliance=11 5 | org.eclipse.jdt.core.compiler.source=11 6 | -------------------------------------------------------------------------------- /java/GridMapGL/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /java/GridMapGL/build.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | plugins { 4 | id 'java' 5 | id "org.jetbrains.kotlin.jvm" version "1.4.30" 6 | id 'application' 7 | id 'eclipse' 8 | } 9 | 10 | // this is for LWJGL (OBS: update this to the same one as kotlin-imgui uses) 11 | project.ext.lwjglVersion = "3.2.3" 12 | 13 | switch ( OperatingSystem.current() ) { 14 | case OperatingSystem.WINDOWS: 15 | project.ext.lwjglNatives = "natives-windows" 16 | break 17 | case OperatingSystem.LINUX: 18 | project.ext.lwjglNatives = "natives-linux" 19 | break 20 | case OperatingSystem.MAC_OS: 21 | project.ext.lwjglNatives = "natives-macos" 22 | break 23 | } 24 | 25 | eclipse { 26 | classpath { 27 | downloadJavadoc = true 28 | downloadSources = true 29 | } 30 | } 31 | 32 | sourceSets { 33 | main.kotlin.srcDirs += 'src/main/java' 34 | } 35 | 36 | distZip { 37 | into(project.name) { 38 | from '.' 39 | include 'res/*' 40 | } 41 | } 42 | 43 | // In this section you declare where to find the dependencies of your project 44 | repositories { 45 | // Use jcenter for resolving your dependencies. 46 | // You can declare any Maven/Ivy/file repository here. 47 | jcenter() 48 | 49 | mavenCentral() 50 | maven { url "https://dl.bintray.com/kotlin/kotlin-dev" } 51 | maven { url "https://oss.sonatype.org/content/repositories/snapshots/" } 52 | maven { url 'https://jitpack.io' } 53 | } 54 | 55 | dependencies { 56 | implementation "org.jetbrains.kotlin:kotlin-stdlib" 57 | 58 | // This dependency is exported to consumers, that is to say found on their compile classpath. 59 | api 'org.apache.commons:commons-math3:3.6.1' 60 | 61 | // for serial communication 62 | implementation 'com.fazecast:jSerialComm:[2.0.0,3.0.0)' 63 | 64 | // uses the kotlin-graphics/imgui library, but exclude vulkan dependencies 65 | ["gl", "glfw", "core"].each { 66 | implementation "com.github.kotlin-graphics.imgui:$it:v1.79" 67 | } 68 | 69 | // 2019-04-21/2019-11-24/2020-03-30: theese are required since kotlin-graphics/imgui does not use the following depencendies 70 | // with the "api" keyword, and does thus not export the dependencies for use with this application. 71 | // That is why the following depencendies are needed directly 72 | // OBS: Make sure the hashes are the same as the ones used in imgui release specified above) 73 | /*implementation 'com.github.kotlin-graphics:gln:1d4d577573c83aeec84a43a217db47b789b78f0d' 74 | implementation 'com.github.kotlin-graphics.glm:glm:6048c31425ae6110258e4b42165f1e636f8b5603' 75 | implementation ('com.github.kotlin-graphics:uno-sdk:cb9073be566b5ca09e3a6d2a8edfb661a721c751') { 76 | exclude group: 'org.lwjgl' 77 | }*/ 78 | 79 | 80 | implementation 'com.github.kotlin-graphics:gln:3e59f9ea' 81 | implementation 'com.github.kotlin-graphics:glm:a4185eec' 82 | implementation ('com.github.kotlin-graphics:uno-sdk:2b06aa78') { 83 | exclude group: 'org.lwjgl' 84 | } 85 | 86 | implementation "org.lwjgl:lwjgl:$lwjglVersion" 87 | implementation "org.lwjgl:lwjgl-glfw:$lwjglVersion" 88 | implementation "org.lwjgl:lwjgl-remotery:$lwjglVersion" 89 | implementation "org.lwjgl:lwjgl-jemalloc:$lwjglVersion" 90 | implementation "org.lwjgl:lwjgl-jawt:$lwjglVersion" 91 | implementation "org.lwjgl:lwjgl-bgfx:$lwjglVersion" 92 | implementation "org.lwjgl:lwjgl-openal:$lwjglVersion" 93 | implementation "org.lwjgl:lwjgl-opengl:$lwjglVersion" 94 | implementation "org.lwjgl:lwjgl-opengles:$lwjglVersion" 95 | implementation "org.lwjgl:lwjgl-vulkan:$lwjglVersion" 96 | implementation "org.lwjgl:lwjgl-stb:$lwjglVersion" 97 | 98 | // Look up which modules and versions of LWJGL are required and add setup the approriate natives. 99 | configurations.implementation.dependencies.forEach { 100 | if (it.group == "org.lwjgl" && it.name != "lwjgl-jawt" && it.name != "lwjgl-vulkan") { 101 | runtimeOnly "org.lwjgl:${it.name}:${it.version}:${lwjglNatives}" 102 | } 103 | } 104 | } 105 | 106 | application { 107 | //applicationDefaultJvmArgs = ['-Dorg.lwjgl.util.Debug=true'] 108 | } 109 | 110 | 111 | mainClassName = 'com.fmsz.gridmapgl.core.Main2' // Main class with main method 112 | -------------------------------------------------------------------------------- /java/GridMapGL/gradle.properties: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Copyright 2018 Anton Berneving 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | ############################################################################### 16 | # wont specify the java home anyomore. Gradle will have to take it from the system path instead 17 | #org.gradle.java.home=C:\\Program Files\\Java\\jdk1.8.0_66 18 | #org.gradle.java.home=C:\\Program Files\\Java\\jdk_current 19 | 20 | # use parallel execution 21 | org.gradle.parallel=true 22 | 23 | #rmdir jdk_current 24 | #mklink /J jdk_current "C:\Program Files\Java\jdk1.8.0_172" -------------------------------------------------------------------------------- /java/GridMapGL/gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antbern/gridmap-slam-robot/abb842733181fefba036ddb0a8f75a7828ff9b07/java/GridMapGL/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /java/GridMapGL/gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=wrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=wrapper/dists 6 | -------------------------------------------------------------------------------- /java/GridMapGL/gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | ############################################################################## 4 | ## 5 | ## Gradle start up script for UN*X 6 | ## 7 | ############################################################################## 8 | 9 | # Attempt to set APP_HOME 10 | # Resolve links: $0 may be a link 11 | PRG="$0" 12 | # Need this for relative symlinks. 13 | while [ -h "$PRG" ] ; do 14 | ls=`ls -ld "$PRG"` 15 | link=`expr "$ls" : '.*-> \(.*\)$'` 16 | if expr "$link" : '/.*' > /dev/null; then 17 | PRG="$link" 18 | else 19 | PRG=`dirname "$PRG"`"/$link" 20 | fi 21 | done 22 | SAVED="`pwd`" 23 | cd "`dirname \"$PRG\"`/" >/dev/null 24 | APP_HOME="`pwd -P`" 25 | cd "$SAVED" >/dev/null 26 | 27 | APP_NAME="Gradle" 28 | APP_BASE_NAME=`basename "$0"` 29 | 30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 31 | DEFAULT_JVM_OPTS='"-Xmx64m"' 32 | 33 | # Use the maximum available, or set MAX_FD != -1 to use that value. 34 | MAX_FD="maximum" 35 | 36 | warn () { 37 | echo "$*" 38 | } 39 | 40 | die () { 41 | echo 42 | echo "$*" 43 | echo 44 | exit 1 45 | } 46 | 47 | # OS specific support (must be 'true' or 'false'). 48 | cygwin=false 49 | msys=false 50 | darwin=false 51 | nonstop=false 52 | case "`uname`" in 53 | CYGWIN* ) 54 | cygwin=true 55 | ;; 56 | Darwin* ) 57 | darwin=true 58 | ;; 59 | MINGW* ) 60 | msys=true 61 | ;; 62 | NONSTOP* ) 63 | nonstop=true 64 | ;; 65 | esac 66 | 67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 68 | 69 | # Determine the Java command to use to start the JVM. 70 | if [ -n "$JAVA_HOME" ] ; then 71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 72 | # IBM's JDK on AIX uses strange locations for the executables 73 | JAVACMD="$JAVA_HOME/jre/sh/java" 74 | else 75 | JAVACMD="$JAVA_HOME/bin/java" 76 | fi 77 | if [ ! -x "$JAVACMD" ] ; then 78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 79 | 80 | Please set the JAVA_HOME variable in your environment to match the 81 | location of your Java installation." 82 | fi 83 | else 84 | JAVACMD="java" 85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 86 | 87 | Please set the JAVA_HOME variable in your environment to match the 88 | location of your Java installation." 89 | fi 90 | 91 | # Increase the maximum file descriptors if we can. 92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 93 | MAX_FD_LIMIT=`ulimit -H -n` 94 | if [ $? -eq 0 ] ; then 95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 96 | MAX_FD="$MAX_FD_LIMIT" 97 | fi 98 | ulimit -n $MAX_FD 99 | if [ $? -ne 0 ] ; then 100 | warn "Could not set maximum file descriptor limit: $MAX_FD" 101 | fi 102 | else 103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 104 | fi 105 | fi 106 | 107 | # For Darwin, add options to specify how the application appears in the dock 108 | if $darwin; then 109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 110 | fi 111 | 112 | # For Cygwin, switch paths to Windows format before running java 113 | if $cygwin ; then 114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 116 | JAVACMD=`cygpath --unix "$JAVACMD"` 117 | 118 | # We build the pattern for arguments to be converted via cygpath 119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 120 | SEP="" 121 | for dir in $ROOTDIRSRAW ; do 122 | ROOTDIRS="$ROOTDIRS$SEP$dir" 123 | SEP="|" 124 | done 125 | OURCYGPATTERN="(^($ROOTDIRS))" 126 | # Add a user-defined pattern to the cygpath arguments 127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 129 | fi 130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 131 | i=0 132 | for arg in "$@" ; do 133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 135 | 136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 138 | else 139 | eval `echo args$i`="\"$arg\"" 140 | fi 141 | i=$((i+1)) 142 | done 143 | case $i in 144 | (0) set -- ;; 145 | (1) set -- "$args0" ;; 146 | (2) set -- "$args0" "$args1" ;; 147 | (3) set -- "$args0" "$args1" "$args2" ;; 148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;; 149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 154 | esac 155 | fi 156 | 157 | # Escape application args 158 | save () { 159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 160 | echo " " 161 | } 162 | APP_ARGS=$(save "$@") 163 | 164 | # Collect all arguments for the java command, following the shell quoting and substitution rules 165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 166 | 167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong 168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then 169 | cd "$(dirname "$0")" 170 | fi 171 | 172 | exec "$JAVACMD" "$@" 173 | -------------------------------------------------------------------------------- /java/GridMapGL/gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS="-Xmx64m" 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /java/GridMapGL/imgui.ini: -------------------------------------------------------------------------------- 1 | [Window][Debug##Default] 2 | Pos=60.0,60.0 3 | Size=400,400 4 | Collapsed=0 5 | 6 | [Window][Debug] 7 | Pos=10.0,10.0 8 | Size=255,82 9 | Collapsed=0 10 | 11 | [Window][Grid Map] 12 | Pos=10.0,100.0 13 | Size=255,428 14 | Collapsed=0 15 | 16 | [Window][Serial] 17 | Pos=1015.0,274.0 18 | Size=244,199 19 | Collapsed=0 20 | 21 | [Window][Recorder] 22 | Pos=1015.0,7.0 23 | Size=255,251 24 | Collapsed=0 25 | 26 | [Window][Controls] 27 | Pos=1015.0,428.0 28 | Size=255,290 29 | Collapsed=0 30 | 31 | [Window][Connection] 32 | Pos=1015.0,266.0 33 | Size=255,155 34 | Collapsed=0 35 | 36 | -------------------------------------------------------------------------------- /java/GridMapGL/license.txt: -------------------------------------------------------------------------------- 1 | Copyright ${date} Anton Berneving 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. -------------------------------------------------------------------------------- /java/GridMapGL/res/shaders/basic.shader: -------------------------------------------------------------------------------- 1 | #shader vertex 2 | #version 330 core 3 | 4 | 5 | layout(location = 0) in vec4 position; 6 | layout(location = 1) in vec4 color; 7 | 8 | uniform mat4 u_projModelView; 9 | 10 | varying vec4 v_Color; 11 | void main(){ 12 | // output the final vertex position 13 | gl_Position = u_projModelView * position; 14 | 15 | v_Color = vec4(color.xyz, 1.0); 16 | }; 17 | 18 | 19 | 20 | 21 | 22 | #shader fragment 23 | 24 | #version 330 core 25 | 26 | layout(location = 0) out vec4 color; 27 | //uniform vec4 u_Color; 28 | 29 | varying vec4 v_Color; 30 | void main(){ 31 | color = v_Color;//vec4(1.0, 1.0, 0.0, 1.0); 32 | }; -------------------------------------------------------------------------------- /java/GridMapGL/settings.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This settings file was generated by the Gradle 'init' task. 3 | * 4 | * The settings file is used to specify which projects to include in your build. 5 | * In a single project build this file can be empty or even removed. 6 | * 7 | * Detailed information about configuring a multi-project build in Gradle can be found 8 | * in the user guide at https://docs.gradle.org/4.3/userguide/multi_project_builds.html 9 | */ 10 | 11 | /* 12 | // To declare projects as part of a multi-project build use the 'include' method 13 | include 'shared' 14 | include 'api' 15 | include 'services:webservice' 16 | */ 17 | 18 | rootProject.name = 'GridMapGL' 19 | -------------------------------------------------------------------------------- /java/GridMapGL/src/imgui.ini: -------------------------------------------------------------------------------- 1 | [Window][Data] 2 | Pos=289,221 3 | Size=344,52 4 | Collapsed=0 5 | 6 | [Window][Debug##Default] 7 | Pos=785,114 8 | Size=400,400 9 | Collapsed=0 10 | 11 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/app/DataEventHandler.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.app; 17 | 18 | import java.util.ArrayList; 19 | import java.util.concurrent.ArrayBlockingQueue; 20 | 21 | import com.fmsz.gridmapgl.slam.TimeFrame; 22 | 23 | /** 24 | * Handles the robot data by using a publish/subscribe model. Tries to be thread safe by using thread safe queues for data and calling 25 | * subscribers only from the main thread. 26 | */ 27 | public class DataEventHandler { 28 | // static instance variable 29 | private static DataEventHandler instance = null; 30 | 31 | /** Returns the Singleton instance of the current DataEventHandler */ 32 | public static DataEventHandler getInstance() { 33 | if (instance == null) 34 | instance = new DataEventHandler(); 35 | 36 | return instance; 37 | } 38 | 39 | private final ArrayBlockingQueue frameQueue = new ArrayBlockingQueue<>(20); 40 | 41 | private ArrayList subscribers = new ArrayList<>(); 42 | 43 | // Private constructor 44 | private DataEventHandler() { 45 | } 46 | 47 | /** Interface for receiving events */ 48 | public interface IDataSubscriber { 49 | public void onHandleData(TimeFrame frame); 50 | } 51 | 52 | /** Subscribe to get notified when new data arrives */ 53 | public void subscribe(IDataSubscriber sub) { 54 | if (!subscribers.contains(sub)) 55 | subscribers.add(sub); 56 | } 57 | 58 | /** Remove subscription to get notified when new data arrives */ 59 | public void unSubscribe(IDataSubscriber sub) { 60 | subscribers.remove(sub); 61 | } 62 | 63 | /** Publish new data */ 64 | public void publish(TimeFrame frame) { 65 | try { 66 | frameQueue.put(frame); 67 | } catch (InterruptedException e) { 68 | e.printStackTrace(); 69 | } 70 | } 71 | 72 | /** 73 | * Checks if any new data events has happened and calls all subscribers accordingly. maxEvents is the maximum number of events to be processed at once or 0 for infinity. 74 | */ 75 | public void handleEvents(int maxEvents) { 76 | 77 | while (maxEvents-- > 0 && !frameQueue.isEmpty()) { 78 | try { 79 | TimeFrame frame = frameQueue.take(); 80 | 81 | for (IDataSubscriber sub : subscribers) 82 | sub.onHandleData(frame); 83 | 84 | } catch (InterruptedException e) { 85 | e.printStackTrace(); 86 | } 87 | } 88 | 89 | } 90 | 91 | } 92 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/app/IApplication.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.app; 17 | 18 | import com.fmsz.gridmapgl.graphics.Camera; 19 | 20 | import glm_.vec2.Vec2; 21 | 22 | public interface IApplication { 23 | public void init(); 24 | 25 | public void render(float delta, Camera cam); 26 | 27 | public abstract void keydown(); 28 | 29 | public abstract void keyup(); 30 | 31 | public void mouseMove(Vec2 mousePosition, boolean drag); 32 | 33 | public void mouseClick(Vec2 mousePosition); 34 | 35 | public void dispose(); 36 | } 37 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/app/ObjectSerializer.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.app; 17 | 18 | import java.io.DataInputStream; 19 | import java.io.DataOutputStream; 20 | import java.io.IOException; 21 | import java.util.List; 22 | 23 | import com.fmsz.gridmapgl.slam.GridMap; 24 | import com.fmsz.gridmapgl.slam.GridMap.GridMapData; 25 | import com.fmsz.gridmapgl.slam.Observation; 26 | import com.fmsz.gridmapgl.slam.Odometry; 27 | import com.fmsz.gridmapgl.slam.Observation.Measurement; 28 | 29 | import glm_.vec2.Vec2; 30 | 31 | public class ObjectSerializer { 32 | private ObjectSerializer() { 33 | }; 34 | 35 | ///////////////////////////////// ODOMETRY /////////////////////////////////////////////// 36 | public static void writeOdometry(DataOutputStream dos, Odometry o) throws IOException { 37 | // write values 38 | dos.writeDouble(o.dCenter); 39 | dos.writeDouble(o.dTheta); 40 | } 41 | 42 | public static Odometry readOdometry(DataInputStream dis) throws IOException { 43 | // read and return 44 | return new Odometry(dis.readDouble(), dis.readDouble()); 45 | } 46 | 47 | /////////////////////////////////// OBSERVATION //////////////////////////////////////////// 48 | public static void writeObservation(DataOutputStream dos, Observation o) throws IOException { 49 | List measurements = o.getMeasurements(); 50 | 51 | // write size first 52 | dos.writeShort(measurements.size()); 53 | 54 | // then all measurements 55 | for (Measurement m : measurements) 56 | writeMeasurement(dos, m); 57 | } 58 | 59 | public static Observation readObservation(DataInputStream dis) throws IOException { 60 | Observation obs = new Observation(); 61 | // read size first 62 | short length = dis.readShort(); 63 | 64 | // then read all measurements 65 | for (int i = 0; i < length; i++) 66 | obs.addMeasurement(readMeasurement(dis)); 67 | 68 | return obs; 69 | } 70 | 71 | ///////////////////////////////// MEASUREMENT ////////////////////////////////////////// 72 | public static void writeMeasurement(DataOutputStream dos, Measurement m) throws IOException { 73 | dos.writeDouble(m.angle); 74 | dos.writeDouble(m.distance); 75 | dos.writeBoolean(m.wasHit); 76 | } 77 | 78 | public static Measurement readMeasurement(DataInputStream dis) throws IOException { 79 | double angle = dis.readDouble(); 80 | double distance = dis.readDouble(); 81 | boolean wasHit = dis.readBoolean(); 82 | return new Measurement(angle, distance, wasHit); 83 | } 84 | 85 | ///////////////////////////////// GRID MAP //////////////////////////////////////////// 86 | public static void writeGridMap(DataOutputStream dos, GridMap map) throws IOException { 87 | // width, height, resolution, position, logData 88 | 89 | dos.writeFloat(map.getWorldSize().getX()); // map width (meters) 90 | dos.writeFloat(map.getWorldSize().getY()); // map height (meters) 91 | dos.writeFloat(map.getResolution()); // resolution (meters) 92 | dos.writeFloat(map.getPosition().getX()); // global map position x 93 | dos.writeFloat(map.getPosition().getY()); // global map position y 94 | } 95 | 96 | public static GridMap readGridMap(DataInputStream dis) throws IOException { 97 | // continue reading the parameters 98 | float width = dis.readFloat(); 99 | float height = dis.readFloat(); 100 | float resolution = dis.readFloat(); 101 | float posX = dis.readFloat(); 102 | float posY = dis.readFloat(); 103 | 104 | // create the map object: 105 | GridMap map = new GridMap(width, height, resolution, new Vec2(posX, posY)); 106 | 107 | return map; 108 | } 109 | 110 | ///////////////////////////////// GRID MAP DATA /////////////////////////////////////// 111 | public static void writeGridMapData(DataOutputStream dos, GridMapData map) throws IOException { 112 | // write all the data: 113 | for (int i = 0; i < map.logData.length; i++) { 114 | dos.writeDouble(map.logData[i]); 115 | } 116 | } 117 | 118 | public static GridMapData readGridMapData(DataInputStream dis, GridMap gridMap) throws IOException { 119 | // create new data object 120 | GridMapData map = gridMap.createMapData(null); 121 | 122 | // then load in the data from the file 123 | for (int i = 0; i < map.logData.length; i++) { 124 | map.logData[i] = dis.readDouble(); 125 | } 126 | 127 | return map; 128 | } 129 | 130 | } 131 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/conn/ConnectionManager.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.conn; 17 | 18 | import java.io.IOException; 19 | import java.util.ArrayList; 20 | import java.util.List; 21 | 22 | import glm_.vec2.Vec2; 23 | import glm_.vec4.Vec4; 24 | import imgui.Col; 25 | import imgui.Dir; 26 | import imgui.FocusedFlag; 27 | import imgui.ImGui; 28 | import imgui.MouseButton; 29 | import imgui.MutableProperty0; 30 | import imgui.SliderFlag; 31 | 32 | /** 33 | * This class handles the connection (only serial for now) to the robot. Has its own GUI for establishing the serial 34 | * connection to the correct port and with the correct speed settings 35 | * 36 | * @author Anton 37 | * 38 | */ 39 | public class ConnectionManager { 40 | private static final byte COMMAND_ONCE = 0x01; 41 | private static final byte COMMAND_ENABLE = 0x02; 42 | private static final byte COMMAND_DISABLE = 0x04; 43 | private static final byte COMMAND_HOME_SENSOR = 0x05; 44 | private static final byte COMMAND_SET_RES = 0x08; 45 | 46 | private ConnectionThread thread = null; 47 | 48 | private int[] sensorDegreeResolutions = { 2, 3, 4, 5, 8, 10, 15, 20, 30, 45 }; 49 | private List sensorDegreeResolutionNames = new ArrayList<>(); 50 | private int[] currentSelectedSensorDegreeResolution = { 2 }; 51 | 52 | // for selecting the type of connection to make 53 | private int[] selectedConn = { 1 }; 54 | private List selectedConnNames = new ArrayList<>(); 55 | private IConnection[] conn; 56 | 57 | // window open variables 58 | private MutableProperty0 connectionOpen = new MutableProperty0<>(true); 59 | private MutableProperty0 controlsOpen = new MutableProperty0<>(true); 60 | 61 | // Controls 62 | private final float[] selectedSpeed = { 10.0f }; 63 | private final Vec2 arrowPadding = new Vec2(30, 30); 64 | private Dir lastDirection = Dir.None; 65 | 66 | // Pid values 67 | private final float[] pidTuningP = { 2.5f /*0.55f*/ }; 68 | private final float[] pidTuningI = { 1.5f/*1.6445f*/ }; 69 | private final float[] pidTuningD = { 0/*0.01016f*/ }; 70 | private final float[] pidTuningTf = { 11.82f }; 71 | 72 | public ConnectionManager() { 73 | // create sensor rate stuff 74 | for (int sensorDegreeResolution : sensorDegreeResolutions) 75 | sensorDegreeResolutionNames.add(String.valueOf(sensorDegreeResolution)); 76 | 77 | // create connections 78 | conn = new IConnection[] { new SerialConnection(), new NetworkConnection() }; 79 | 80 | // add their names and initialize 81 | for (int i = 0; i < conn.length; i++) { 82 | selectedConnNames.add(conn[i].getName()); 83 | conn[i].init(); 84 | } 85 | 86 | } 87 | 88 | public void doGUI(ImGui imgui) { 89 | if (imgui.begin("Connection", connectionOpen, 0)) { 90 | 91 | // let user select connection to use 92 | imgui.combo("Connection", selectedConn, selectedConnNames, 5); 93 | 94 | int selected = selectedConn[0]; 95 | 96 | // let the selected connection draw its GUI 97 | conn[selected].doGUI(imgui); 98 | 99 | // button for connecting/disconnecting 100 | if (conn[selected].isConnected()) { 101 | if (imgui.button("Disconnect", new Vec2())) 102 | disconnect(conn[selected]); 103 | } else { 104 | if (imgui.button("Connect", new Vec2())) 105 | connect(conn[selected]); 106 | } 107 | } 108 | 109 | imgui.end(); 110 | 111 | // controls window 112 | if (imgui.begin("Controls", controlsOpen, 0)) { 113 | 114 | if (imgui.button("Single", new Vec2())) { 115 | sendCommand(COMMAND_ONCE); 116 | } 117 | imgui.sameLine(0, 4); 118 | if (imgui.button("Enable", new Vec2())) { 119 | sendCommand(COMMAND_ENABLE); 120 | } 121 | imgui.sameLine(0, 4); 122 | if (imgui.button("Disable", new Vec2())) { 123 | sendCommand(COMMAND_DISABLE); 124 | } 125 | imgui.sameLine(0, 4); 126 | if (imgui.button("Home", new Vec2())) { 127 | sendCommand(COMMAND_HOME_SENSOR); 128 | } 129 | 130 | if (imgui.combo("Res", currentSelectedSensorDegreeResolution, sensorDegreeResolutionNames, 7)) { 131 | int selectedResolution = sensorDegreeResolutions[currentSelectedSensorDegreeResolution[0]]; 132 | sendCommand(new byte[] { 0x08, (byte) selectedResolution }); 133 | } 134 | 135 | imgui.separator(); 136 | 137 | boolean controlsActive = imgui.isWindowFocused(FocusedFlag.ChildWindows); 138 | 139 | imgui.text("Status: %s", controlsActive ? "active" : "inactive"); 140 | if (controlsActive) 141 | imgui.pushStyleColor(Col.Button, new Vec4(0.5, 0.5, 0.5, 1)); 142 | 143 | Dir selectedDirection = Dir.None; 144 | // Up arrow 145 | imgui.newLine(); 146 | imgui.sameLine(45, 0); 147 | 148 | imgui.arrowButtonEx("11", Dir.Up, arrowPadding, 0); 149 | if (imgui.isItemHovered(0) && imgui.isMouseDown(MouseButton.Left)) { 150 | selectedDirection = Dir.Up; 151 | } 152 | 153 | // Left arrow 154 | imgui.arrowButtonEx("10", Dir.Left, arrowPadding, 0); 155 | if (imgui.isItemHovered(0) && imgui.isMouseDown(MouseButton.Left)) { 156 | selectedDirection = Dir.Left; 157 | } 158 | 159 | // middle button... 160 | // imgui.sameLine(41 + 4); 161 | // imgui.button("O", new Vec2(41-8, 41-8)); 162 | 163 | // Right arrow 164 | imgui.sameLine(82, 0); 165 | imgui.arrowButtonEx("13", Dir.Right, arrowPadding, 0); 166 | if (imgui.isItemHovered(0) && imgui.isMouseDown(MouseButton.Left)) { 167 | selectedDirection = Dir.Right; 168 | } 169 | 170 | // Down arrow 171 | imgui.newLine(); 172 | imgui.sameLine(45, 0); 173 | imgui.arrowButtonEx("12", Dir.Down, arrowPadding, 0); 174 | if (imgui.isItemHovered(0) && imgui.isMouseDown(MouseButton.Left)) { 175 | selectedDirection = Dir.Down; 176 | } 177 | 178 | if (controlsActive) 179 | imgui.popStyleColor(1); 180 | 181 | // speed select slider 182 | imgui.dragFloat("Speed", selectedSpeed, 0, 0.01f, 0.0f, 14.0f, "%.2f", SliderFlag.None.getI()); 183 | 184 | // only send to robot if there was a change 185 | if (selectedDirection != lastDirection) { 186 | lastDirection = selectedDirection; 187 | 188 | // update speeds 189 | float speedLeft = 0, speedRight = 0; 190 | float speed = selectedSpeed[0]; 191 | 192 | switch (selectedDirection) { 193 | case Up: 194 | speedLeft = speedRight = speed; 195 | break; 196 | case Down: 197 | speedLeft = speedRight = -speed; 198 | break; 199 | case Left: 200 | speedLeft = -speed; 201 | speedRight = speed; 202 | break; 203 | case Right: 204 | speedLeft = speed; 205 | speedRight = -speed; 206 | break; 207 | 208 | default: 209 | break; 210 | 211 | } 212 | 213 | // send speed values to robot 214 | sendFloat(0x10, speedLeft, speedRight); 215 | } 216 | 217 | // PID tuning sliders 218 | if (imgui.dragFloat("Kp", pidTuningP, 0, 0.001f, 0, 10, "%.4f", SliderFlag.None.getI())) { 219 | sendFloat(0x15, pidTuningP[0]); 220 | } 221 | if (imgui.dragFloat("Ki", pidTuningI, 0, 0.001f, 0, 10, "%.4f", SliderFlag.None.getI())) { 222 | sendFloat(0x16, pidTuningI[0]); 223 | } 224 | if (imgui.dragFloat("Kd", pidTuningD, 0, 0.001f, 0, 10, "%.4f", SliderFlag.None.getI())) { 225 | sendFloat(0x17, pidTuningD[0]); 226 | } 227 | if (imgui.dragFloat("Tf = 1/x", pidTuningTf, 0, 0.01f, 0, 100, "%.2f", SliderFlag.None.getI())) { 228 | sendFloat(0x18, pidTuningTf[0]); 229 | } 230 | } 231 | imgui.end(); 232 | 233 | } 234 | 235 | private void connect(IConnection conn) { 236 | // let the connection do the connecting 237 | conn.connect(); 238 | 239 | // if it was successful in connecting, start the thread reading from the streams 240 | if (conn.isConnected()) { 241 | thread = new ConnectionThread(conn.getInputStream()); 242 | thread.start(); 243 | 244 | // send the initial configuration parameters 245 | sendCommand(new byte[] { COMMAND_SET_RES, (byte) sensorDegreeResolutions[currentSelectedSensorDegreeResolution[0]] }); 246 | sendFloat(0x15, pidTuningP[0]); 247 | sendFloat(0x16, pidTuningI[0]); 248 | sendFloat(0x17, pidTuningD[0]); 249 | sendFloat(0x18, pidTuningTf[0]); 250 | 251 | } else { 252 | System.err.println("[ConnectionManager] Did not succeed in opening the connection!"); 253 | } 254 | 255 | } 256 | 257 | private void disconnect(IConnection conn) { 258 | // check if the connection is open 259 | if (conn.isConnected()) { 260 | 261 | // send disable command first 262 | sendCommand(COMMAND_DISABLE); 263 | 264 | // stop the reading thread 265 | thread.interrupt(); 266 | try { 267 | thread.join(); 268 | } catch (InterruptedException e) { 269 | e.printStackTrace(); 270 | } 271 | thread = null; 272 | 273 | // let it disconnect 274 | conn.disconnect(); 275 | } 276 | } 277 | 278 | /** Writes the given bytes to the robot */ 279 | private void sendCommand(byte... command) { 280 | IConnection c = conn[selectedConn[0]]; 281 | if (c.isConnected()) { 282 | try { 283 | c.getOutputStream().write(command, 0, command.length); 284 | c.getOutputStream().flush(); 285 | } catch (IOException e) { 286 | e.printStackTrace(); 287 | } 288 | } 289 | } 290 | 291 | private void sendFloat(int command, float... values) { 292 | 293 | byte[] commandBuffer = new byte[1+values.length*4]; 294 | commandBuffer[0] = (byte) command; 295 | for (int i = 0; i < values.length; i++) { 296 | 297 | int bits = Float.floatToIntBits(values[i]); 298 | 299 | commandBuffer[1+i*4 + 0] = (byte) ((bits >> 24) & 0xff); 300 | commandBuffer[1+i*4 + 1] = (byte) ((bits >> 16) & 0xff); 301 | commandBuffer[1+i*4 + 2] = (byte) ((bits >> 8) & 0xff); 302 | commandBuffer[1+i*4 + 3] = (byte) (bits & 0xff); 303 | } 304 | 305 | sendCommand(commandBuffer); 306 | 307 | } 308 | 309 | public void dispose() { 310 | disconnect(conn[selectedConn[0]]); 311 | } 312 | } 313 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/conn/ConnectionThread.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.conn; 17 | 18 | import java.io.DataInputStream; 19 | import java.io.IOException; 20 | import java.io.InputStream; 21 | 22 | import com.fmsz.gridmapgl.app.DataEventHandler; 23 | import com.fmsz.gridmapgl.math.MathUtil; 24 | import com.fmsz.gridmapgl.slam.Observation; 25 | import com.fmsz.gridmapgl.slam.Odometry; 26 | import com.fmsz.gridmapgl.slam.Robot; 27 | import com.fmsz.gridmapgl.slam.SensorModel; 28 | import com.fmsz.gridmapgl.slam.TimeFrame; 29 | 30 | public class ConnectionThread extends Thread { 31 | 32 | private InputStream is; 33 | private Observation currentObservation; 34 | 35 | public ConnectionThread(InputStream is) { 36 | this.is = is; 37 | currentObservation = new Observation(); 38 | } 39 | 40 | @Override 41 | public void run() { 42 | 43 | DataInputStream dis = new DataInputStream(this.is); 44 | 45 | while (!isInterrupted()) { 46 | try { 47 | // do we have the 5 bytes needed for a packet? 48 | if (dis.available() >= 8) { 49 | // Note that we check against 0xAA55 here (and not 0x55AA as in the ESP code) due to differences in endianness 50 | if (dis.readShort() == (short)0xAA55) { 51 | short steps = dis.readShort(); 52 | short frontDistance = dis.readShort(); 53 | short backDistance = dis.readShort(); 54 | 55 | // reorganize bytes (convert from little to big endian) 56 | steps = (short) ((steps & 0xff) << 8 | (steps >> 8) & 0xff); 57 | frontDistance = (short) ((frontDistance & 0xff) << 8 | (frontDistance >> 8) & 0xff); 58 | backDistance = (short) ((backDistance & 0xff) << 8 | (backDistance >> 8) & 0xff); 59 | 60 | System.out.println(steps + "-> " + frontDistance + ", " + backDistance); 61 | 62 | // if this is a "new packet" indicator? 63 | if (steps < 0) { 64 | // this packet also holds the odometry information 65 | 66 | // post the old packet, start creating a new one 67 | DataEventHandler.getInstance() 68 | .publish(new TimeFrame(currentObservation, new Odometry(frontDistance, backDistance))); 69 | currentObservation = new Observation(); 70 | 71 | } else { 72 | // add a measurement for the front sensor 73 | float rad = steps / (float) Robot.SENSOR_STEPS_PER_REVOLUTION * MathUtil.PI2; 74 | // correct for the fact that the sensor is oriented with an angle offset 75 | rad += Robot.SENSOR_ANGLE_OFFSET; 76 | 77 | float dist = frontDistance / 1000f; 78 | if (dist < 0) 79 | currentObservation.addMeasurement(rad, SensorModel.SENSOR_MAX_RANGE, false); 80 | else 81 | currentObservation.addMeasurement(rad, dist, true); 82 | 83 | /* 84 | // add a measurement for the back sensor (+180 deg) 85 | rad += MathUtil.PI; 86 | dist = backDistance / 1000f; 87 | if (dist < 0) 88 | currentObservation.addMeasurement(rad, SensorModel.SENSOR_MAX_RANGE, false); 89 | else 90 | currentObservation.addMeasurement(rad, dist, true); 91 | */ 92 | } 93 | 94 | } 95 | } 96 | 97 | } catch (IOException ioe) { 98 | ioe.printStackTrace(); 99 | break; 100 | } 101 | } 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/conn/IConnection.java: -------------------------------------------------------------------------------- 1 | package com.fmsz.gridmapgl.conn; 2 | 3 | import java.io.InputStream; 4 | import java.io.OutputStream; 5 | 6 | import imgui.ImGui; 7 | 8 | public interface IConnection { 9 | 10 | /** Returns the name for this connection */ 11 | public String getName(); 12 | 13 | /** Initializes this connection */ 14 | public void init(); 15 | 16 | /** Lets this connection draw some kind of GUI using ImGUI */ 17 | public void doGUI(ImGui imgui); 18 | 19 | /** Called when the connection is to be made. This is done before the receiving thread is created. */ 20 | public void connect(); 21 | 22 | /** Called when the connection is to be closed. This is done after the receiving thread is closed. */ 23 | public void disconnect(); 24 | 25 | /** returns whether or not this connection is connected */ 26 | public boolean isConnected(); 27 | 28 | /** returns the active InputStream associated with this connection */ 29 | public InputStream getInputStream(); 30 | 31 | /** returns the active OutputStream associated with this connection */ 32 | public OutputStream getOutputStream(); 33 | 34 | /** Invoked when this object is destroyed (often because of the application terminating) */ 35 | // public void dispose(); 36 | } 37 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/conn/NetworkConnection.java: -------------------------------------------------------------------------------- 1 | package com.fmsz.gridmapgl.conn; 2 | 3 | import java.io.IOException; 4 | import java.io.InputStream; 5 | import java.io.OutputStream; 6 | import java.net.Socket; 7 | 8 | import imgui.ImGui; 9 | 10 | public class NetworkConnection implements IConnection { 11 | // GUI Stuff 12 | private byte[] hostBuff = new byte[64]; 13 | private byte[] portBuff = new byte[32]; 14 | 15 | // connection stuff 16 | private static final String defaultHost = "esp32robot.local"; 17 | private static final String defaultPort = "5555"; 18 | 19 | private Socket socket = null; 20 | private InputStream is; 21 | private OutputStream os; 22 | 23 | @Override 24 | public String getName() { 25 | return "Network"; 26 | } 27 | 28 | @Override 29 | public void init() { 30 | // fill buffer with default host name 31 | System.arraycopy(defaultHost.getBytes(), 0, hostBuff, 0, defaultHost.length()); 32 | System.arraycopy(defaultPort.getBytes(), 0, portBuff, 0, defaultPort.length()); 33 | } 34 | 35 | @Override 36 | public void doGUI(ImGui imgui) { 37 | // very simple ui for now 38 | imgui.inputText("Host", hostBuff, 0, null, null); 39 | imgui.inputText("Port", portBuff, 0, null, null); 40 | 41 | } 42 | 43 | @Override 44 | public void connect() { 45 | // get the entered host name 46 | String host = new String(hostBuff).trim(); 47 | String portStr = new String(portBuff).trim(); 48 | 49 | System.out.println("Connection to: " + host + ":" + portStr); 50 | try { 51 | // try to parse the port as an integer 52 | int port = Integer.parseInt(portStr); 53 | 54 | // try to create a new socket 55 | socket = new Socket(host, port); 56 | 57 | // store local pointers to the respective streams 58 | is = socket.getInputStream(); 59 | os = socket.getOutputStream(); 60 | 61 | } catch (NumberFormatException nfe) { 62 | System.err.println("[NetworkConnection] Ill-formed port number!"); 63 | } catch (IOException ioe) { 64 | ioe.printStackTrace(); 65 | } 66 | 67 | } 68 | 69 | @Override 70 | public void disconnect() { 71 | if (!isConnected()) 72 | return; 73 | 74 | // try to close the socket 75 | try { 76 | socket.close(); 77 | 78 | } catch (IOException ioe) { 79 | ioe.printStackTrace(); 80 | } finally { 81 | is = null; 82 | os = null; 83 | socket = null; 84 | } 85 | } 86 | 87 | @Override 88 | public boolean isConnected() { 89 | return socket != null; 90 | } 91 | 92 | @Override 93 | public InputStream getInputStream() { 94 | return is; 95 | } 96 | 97 | @Override 98 | public OutputStream getOutputStream() { 99 | return os; 100 | } 101 | 102 | } 103 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/conn/SerialConnection.java: -------------------------------------------------------------------------------- 1 | package com.fmsz.gridmapgl.conn; 2 | 3 | import java.io.InputStream; 4 | import java.io.OutputStream; 5 | import java.util.ArrayList; 6 | import java.util.List; 7 | 8 | import com.fazecast.jSerialComm.SerialPort; 9 | 10 | import glm_.vec2.Vec2; 11 | import imgui.ImGui; 12 | 13 | public class SerialConnection implements IConnection { 14 | 15 | private int[] currentSelectedPortName = new int[] { 0 }; 16 | private List portNames = new ArrayList<>(); 17 | 18 | private int[] currentSelectedBaudRate = new int[] { 7 }; 19 | private int[] baudRates = new int[] { 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200 }; 20 | private List baudRateNames = new ArrayList<>(); 21 | 22 | private SerialPort[] availablePorts = null; 23 | private SerialPort currentPort = null; 24 | 25 | @Override 26 | public String getName() { 27 | return "Serial"; 28 | } 29 | 30 | @Override 31 | public void init() { 32 | 33 | // create baud rate stuff 34 | for (int baudRate : baudRates) 35 | baudRateNames.add(String.valueOf(baudRate)); 36 | 37 | refreshList(); 38 | } 39 | 40 | @Override 41 | public void doGUI(ImGui imgui) { 42 | // drop-down menu for selecting a port 43 | imgui.combo("Port", currentSelectedPortName, portNames, 5); 44 | imgui.combo("Baud", currentSelectedBaudRate, baudRateNames, 7); 45 | 46 | if (imgui.button("Refresh", new Vec2())) { 47 | refreshList(); 48 | } 49 | imgui.sameLine(0, 4); 50 | if (imgui.button("Reset", new Vec2())) { 51 | if (currentPort != null) { 52 | currentPort.clearDTR(); 53 | currentPort.setDTR(); 54 | currentPort.clearDTR(); 55 | } 56 | } 57 | } 58 | 59 | private void refreshList() { 60 | 61 | // fetch available ports 62 | availablePorts = SerialPort.getCommPorts(); 63 | 64 | // clear current names 65 | portNames.clear(); 66 | 67 | // add each available ports name to the selectable list 68 | for (SerialPort port : availablePorts) { 69 | portNames.add(port.getSystemPortName() + "(" + port.getPortDescription() + ")"); 70 | } 71 | 72 | // set the preselected one to the last one 73 | currentSelectedPortName[0] = availablePorts.length - 1; 74 | } 75 | 76 | @Override 77 | public void connect() { 78 | if (currentSelectedPortName[0] != -1) 79 | connect(availablePorts[currentSelectedPortName[0]], baudRates[currentSelectedBaudRate[0]]); 80 | } 81 | 82 | private void connect(SerialPort port, int baudRate) { 83 | if (currentPort != null) 84 | return; 85 | 86 | // deactivate RTS line to avoid reset on the Arduino (does not currently work) 87 | port.setBaudRate(baudRate); 88 | 89 | port.clearRTS(); 90 | 91 | // try to open the port 92 | if (!port.openPort()) { 93 | // there was an error! 94 | System.err.println("[SerialConnection] There was an error opening the port " + port.getSystemPortName() + " (" + port.getDescriptivePortName() + ")"); 95 | return; 96 | } 97 | 98 | // this is now our active port 99 | currentPort = port; 100 | 101 | // wait for robot to become ready...( could probably be skipped) 102 | /* 103 | try { 104 | Thread.sleep(2500); 105 | } catch (InterruptedException ie) { 106 | ie.printStackTrace(); 107 | } 108 | */ 109 | } 110 | 111 | @Override 112 | public void disconnect() { 113 | 114 | if (currentPort != null) { 115 | 116 | // try to close port 117 | if (!currentPort.closePort()) 118 | throw new IllegalStateException("SerialPort: Could not close port " + currentPort.getSystemPortName()); 119 | 120 | currentPort = null; 121 | } 122 | } 123 | 124 | @Override 125 | public boolean isConnected() { 126 | return currentPort != null; 127 | } 128 | 129 | @Override 130 | public InputStream getInputStream() { 131 | return currentPort.getInputStream(); 132 | } 133 | 134 | @Override 135 | public OutputStream getOutputStream() { 136 | return currentPort.getOutputStream(); 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/core/Main2.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.core; 17 | 18 | import static org.lwjgl.glfw.GLFW.GLFW_MOUSE_BUTTON_LEFT; 19 | import static org.lwjgl.glfw.GLFW.GLFW_PRESS; 20 | import static org.lwjgl.glfw.GLFW.GLFW_RELEASE; 21 | import static org.lwjgl.glfw.GLFW.glfwGetTime; 22 | import static org.lwjgl.opengl.GL11.*; 23 | 24 | import java.io.IOException; 25 | 26 | import org.lwjgl.glfw.GLFW; 27 | 28 | import com.fmsz.gridmapgl.app.GridMapApp; 29 | import com.fmsz.gridmapgl.app.IApplication; 30 | import com.fmsz.gridmapgl.graphics.Camera; 31 | 32 | import glm_.vec2.Vec2; 33 | import glm_.vec2.Vec2d; 34 | import glm_.vec2.Vec2i; 35 | import glm_.vec4.Vec4; 36 | import imgui.Cond; 37 | import imgui.classes.Context; 38 | import imgui.impl.gl.ImplGL3; 39 | import imgui.classes.IO; 40 | import imgui.ImGui; 41 | import imgui.MutableProperty0; 42 | import imgui.impl.glfw.ImplGlfw; 43 | import kotlin.Unit; 44 | import kotlin.jvm.functions.Function1; 45 | import kotlin.jvm.functions.Function3; 46 | import uno.glfw.GlfwWindow; 47 | import uno.glfw.VSync; 48 | import uno.glfw.windowHint.Profile; 49 | 50 | 51 | import static gln.GlnKt.glClearColor; 52 | import static gln.GlnKt.glViewport; 53 | 54 | public class Main2 { 55 | 56 | public static void main(String[] args) throws IOException { 57 | /* 58 | * ClassLoader cl = ClassLoader.getSystemClassLoader(); 59 | * 60 | * URL[] urls = ((URLClassLoader)cl).getURLs(); 61 | * 62 | * for(URL url: urls){ System.out.println(url.getFile()); } 63 | * 64 | * 65 | * System.out.println(ClassLoader.getSystemResource("config/imgui.ini")); 66 | * //System.out.println(ClassLoader.getSystemClassLoader().getResources("Main2").hasMoreElements()); 67 | * System.out.println(ClassLoader.getSystemResource( 68 | * "C:\\Users\\Anton\\Dropbox\\Programmering\\Java\\Projekt\\GridMapGL\\bin\\imgui.ini")); 69 | * 70 | */ 71 | // System.out.println(new 72 | // File(Paths.get("C:\\Users\\Anton\\Dropbox\\Programmering\\Java\\Projekt\\GridMapGL\\bin\\imgui.ini").toUri())); 73 | 74 | /* 75 | int size = 10; 76 | //@formatter:off 77 | float[] in = new float[] { 78 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 79 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 80 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 81 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 82 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 83 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 84 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 85 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 86 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 87 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 88 | }; 89 | //@formatter:on 90 | 91 | print(in, size); 92 | 93 | float[] out = new float[in.length]; 94 | 95 | System.out.println("Sum:"); 96 | print(Util.doBoxBlur(size, size, in, out, 1), size); 97 | 98 | System.out.println("out:"); 99 | print(out, size); 100 | 101 | Vec3 v1 = new Vec3(1, 2, 3); 102 | Vec3 v2 = new Vec3(4, 5, 6); 103 | 104 | System.out.println(v1.times(v2)); 105 | 106 | Mat3 m = new Mat3(); 107 | 108 | */ 109 | // System.out.println(Arrays.toString(Util.generateGaussianKernel(2, 5))); 110 | // System.out.println(Arrays.toString(Util.generateGaussianKernelIntegrate(2, 5))); 111 | 112 | // System.out.println(MathUtil.angleDiff(-Math.PI, Math.PI / 4)); 113 | // System.out.println(3*Math.PI / 4); 114 | new Main2().run(); 115 | 116 | } 117 | /* 118 | private static void print(float[] data, int width) { 119 | for (int i = 0; i < data.length; i++) { 120 | if (i % width == 0) 121 | System.out.println(); 122 | System.out.print(String.format("%5.2f : ", data[i])); 123 | } 124 | System.out.println(); 125 | } 126 | */ 127 | 128 | // The window handle 129 | private GlfwWindow window; 130 | private uno.glfw.glfw glfw = uno.glfw.glfw.INSTANCE; 131 | 132 | private ImplGlfw implGlfw; 133 | private ImplGL3 implGl3; 134 | private ImGui igui = ImGui.INSTANCE; 135 | private IO io; 136 | private Context ctx; 137 | 138 | // for handling input smoothly 139 | private Vec2 currentMouseWorldPosition = new Vec2(); 140 | private Vec2 currentMousePosition = new Vec2(); 141 | private Vec2 tmp = new Vec2(); 142 | private boolean mousePressed = false; 143 | private boolean mouseDragged = false; 144 | 145 | // keep track of the current window size (in pixels) 146 | private Vec2 currentWindowSize = new Vec2(1280, 720); 147 | 148 | // the application 149 | private IApplication app = null; 150 | 151 | // the camera and stuff 152 | private Camera cam; 153 | 154 | // keep track of the last update time 155 | private double lastTime; 156 | 157 | private Vec4 clearColor = new Vec4(0.45f, 0.55f, 0.6f, 1f); 158 | 159 | private MutableProperty0 debugOpen = new MutableProperty0<>(true); 160 | 161 | public void run() { 162 | 163 | // initialize stuff 164 | init(); 165 | 166 | // call the main loop until window is closed 167 | window.loop((MemoryStack) -> { 168 | loop(); 169 | }); 170 | 171 | // cleanup 172 | dispose(); 173 | 174 | } 175 | 176 | private void init() { 177 | ////// PLATFORM SETUP ////// 178 | 179 | // initialize glfw 180 | GLFW.glfwSetErrorCallback((error, description) -> System.out.println("Glfw Error " + error + ": " + description)); 181 | glfw.init("3.3", Profile.core, true); 182 | 183 | // create GLFW window 184 | window = new GlfwWindow(1280, 720, "GridMapGL using ImGUI", null, new Vec2i(Integer.MIN_VALUE), true); 185 | window.init(true); 186 | 187 | // Enable vsync 188 | glfw.setSwapInterval(VSync.ON); 189 | 190 | // create context 191 | ctx = new Context(null); 192 | 193 | // set correct .ini file 194 | io = igui.getIo(); 195 | // TODO: take this back once bug has been fixed! 196 | // io.setIniFilename("res/config/imgui.ini"); 197 | 198 | // Setup style 199 | igui.styleColorsDark(null); 200 | 201 | // Setup Platform/Renderer bindings 202 | implGlfw = new ImplGlfw(window, true, null); 203 | implGl3 = new ImplGL3(); 204 | 205 | 206 | /////// APPLICATION SPECIFIC SETUP //////// 207 | 208 | // create the view camera 209 | cam = new Camera(); 210 | 211 | // set up input handling 212 | 213 | window.setCursorPosCB(new Function1() { 214 | @Override 215 | public Unit invoke(Vec2 pos) { 216 | // if imgui wants the mouse, let it have it :) 217 | if (io.getWantCaptureMouse()) 218 | return null; 219 | 220 | // is this a "drag" event? 221 | if (mousePressed) { 222 | 223 | // only pan if the CTRL key is not down 224 | if (!io.getKeyCtrl()) { 225 | // yes! do panning: 226 | tmp.put(pos); 227 | tmp.minusAssign(currentMousePosition); 228 | tmp.timesAssign(1, -1); 229 | 230 | // tmp is now the dx and dy 231 | cam.pan(tmp); 232 | } 233 | 234 | mouseDragged = true; 235 | 236 | } 237 | 238 | // save current mouse position 239 | currentMousePosition.put(pos); 240 | 241 | // save current mouse position i world coordinates 242 | currentMouseWorldPosition.put(pos); 243 | cam.unproject(currentMouseWorldPosition); 244 | 245 | // if this was not a dragging, let the app know 246 | if (!mouseDragged || io.getKeyCtrl()) 247 | app.mouseMove(currentMouseWorldPosition, mouseDragged); 248 | 249 | return null; 250 | } 251 | }); 252 | 253 | window.setMouseButtonCB(new Function3() { 254 | @Override 255 | public Unit invoke(Integer button, Integer action, Integer flags) { 256 | // if imgui wants the mouse, let it have it :) 257 | if (io.getWantCaptureMouse()) 258 | return null; 259 | 260 | if (button == GLFW_MOUSE_BUTTON_LEFT) { 261 | mousePressed = action == GLFW_PRESS; 262 | 263 | if (action == GLFW_RELEASE) { 264 | if (!mouseDragged) 265 | app.mouseClick(currentMouseWorldPosition); 266 | 267 | mouseDragged = false; 268 | 269 | } 270 | 271 | } 272 | return null; 273 | } 274 | }); 275 | 276 | window.setScrollCB(new Function1() { 277 | @Override 278 | public Unit invoke(Vec2d amount) { 279 | // if imgui wants the mouse, let it have it :) 280 | if (io.getWantCaptureMouse()) 281 | return null; 282 | 283 | cam.zoom(currentMousePosition, amount.getY().floatValue()); 284 | return null; 285 | } 286 | }); 287 | 288 | window.setFramebufferSizeCB(new Function1() { 289 | public Unit invoke(Vec2i size) { 290 | // store current window size 291 | currentWindowSize.put(size); 292 | 293 | // call cameras resize-method 294 | cam.resize(currentWindowSize); 295 | return null; 296 | }; 297 | }); 298 | 299 | cam.resize(currentWindowSize); 300 | 301 | app = new GridMapApp(); 302 | app.init(); 303 | 304 | lastTime = glfwGetTime(); 305 | 306 | } 307 | 308 | private void loop() { 309 | 310 | implGl3.newFrame(); 311 | implGlfw.newFrame(); 312 | igui.newFrame(); 313 | 314 | // some default behavior (TODO: probablit not needed anymore) 315 | igui.setWindowCollapsed(false, Cond.FirstUseEver); 316 | igui.setWindowSize(new Vec2(0, 0), Cond.FirstUseEver); 317 | igui.setNextWindowCollapsed(false, Cond.FirstUseEver); 318 | 319 | // Rendering 320 | glViewport(window.getFramebufferSize()); 321 | glClearColor(clearColor); 322 | glClear(GL_COLOR_BUFFER_BIT); 323 | glFrontFace(GL_CCW); 324 | glEnable(GL_CULL_FACE); 325 | 326 | // let the camera recalculate stuff if needed 327 | cam.update(); 328 | 329 | double currentTime = glfwGetTime(); 330 | 331 | float delta = (float) (currentTime - lastTime); 332 | 333 | // let the app do its thing: update, rendering etc. 334 | app.render(delta, cam); 335 | 336 | float updateTime = (float) (glfwGetTime() - currentTime); 337 | 338 | lastTime = currentTime; 339 | 340 | 341 | // show some debug info 342 | if (igui.begin("Debug", debugOpen, 0)) { 343 | igui.text("App Delta: %.2f ms (%.2f FPS)", delta * 1000, 1.0f / delta); 344 | igui.text("App Update: %.2f ms (%.2f FPS)", updateTime * 1000, 1.0f / updateTime); 345 | igui.text("Mouse: [%.2f, %.2f] m", currentMouseWorldPosition.getX(), currentMouseWorldPosition.getY()); 346 | 347 | igui.end(); 348 | } 349 | 350 | // render ImGUI 351 | igui.render(); 352 | glViewport(window.getFramebufferSize()); 353 | implGl3.renderDrawData(igui.getDrawData()); 354 | 355 | // this is from the example, not sure if necessary 356 | //if (ImguiKt.getDEBUG()) 357 | // gln.GlnKt.checkError("loop", true); 358 | 359 | } 360 | 361 | private void dispose() { 362 | app.dispose(); 363 | 364 | // terminate stuff (this should be done lastly!) 365 | implGlfw.shutdown(); 366 | implGl3.shutdown(); 367 | ctx.destroy(); 368 | 369 | window.destroy(); 370 | glfw.terminate(); 371 | 372 | } 373 | } -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/Camera.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics; 17 | 18 | import glm_.glm; 19 | import glm_.mat4x4.Mat4; 20 | import glm_.vec2.Vec2; 21 | 22 | /** 23 | * The Camera class is responsible for creating the correct MVP-matrix to show the desired area of the "world" on 24 | * screen. 25 | * 26 | * Inspiration taken from libgdx's Camera and OrthographicCamera classes. 27 | * https://github.com/libgdx/libgdx/blob/master/gdx/src/com/badlogic/gdx/graphics/Camera.java 28 | * https://github.com/libgdx/libgdx/blob/master/gdx/src/com/badlogic/gdx/graphics/OrthographicCamera.java 29 | * 30 | * @author Anton 31 | * 32 | */ 33 | public class Camera { 34 | 35 | private Mat4 projection = new Mat4(); 36 | 37 | public Mat4 combined = new Mat4(); 38 | public Mat4 invCombined = new Mat4(); 39 | 40 | // stores where the current center of the camera is 41 | public Vec2 position = new Vec2(0, 0); 42 | public float zoom = 1f; 43 | public Mat4 view = new Mat4(); 44 | 45 | private boolean hasChanged = true; 46 | 47 | float viewportWidth, viewportHeight; 48 | 49 | private Vec2 currentScreenSize = new Vec2(); 50 | 51 | public Camera() { 52 | 53 | } 54 | 55 | public void centerAt(Vec2 pos) { 56 | position.put(pos); 57 | } 58 | 59 | public void pan(Vec2 screenChange) { 60 | // convert the change from screen units to viewport units 61 | screenChange.divAssign(currentScreenSize); 62 | screenChange.timesAssign(viewportWidth, viewportHeight); 63 | screenChange.timesAssign(zoom); 64 | 65 | // apply the change 66 | position.plusAssign(screenChange); 67 | hasChanged = true; 68 | } 69 | 70 | public void zoom(Vec2 mousePos, float amount) { 71 | zoom *= (1 - 0.1f * amount); 72 | 73 | // clamp zoom 74 | if (zoom < 0.1) 75 | zoom = 0.1f; 76 | 77 | hasChanged = true; 78 | } 79 | 80 | public void resize(Vec2 newSize) { 81 | currentScreenSize.put(newSize); 82 | 83 | // recalculate the size of the viewport here 84 | viewportWidth = 10f; 85 | viewportHeight = viewportWidth * currentScreenSize.getY() / currentScreenSize.getX(); 86 | 87 | // need to recalculate the projection matrix here... 88 | 89 | // float height = 10f; 90 | // float width = height * newSize.x / newSize.y; 91 | // projection = (glm.INSTANCE.ortho(-width / 2, width / 2, -height / 2, height / 2)); 92 | // projection = (glm.INSTANCE.ortho(0, newSize.x, 0, newSize.y)); 93 | // invProj = projection.inverse(); 94 | 95 | hasChanged = true; 96 | } 97 | 98 | public Vec2 unproject(Vec2 screenCoords) { 99 | 100 | screenCoords.put(screenCoords.getX() / currentScreenSize.getX() * viewportWidth * zoom, (currentScreenSize.getY() - screenCoords.getY() - 1) / currentScreenSize.getY() * viewportHeight * zoom); 101 | 102 | // adjust for the viewport size 103 | screenCoords.minusAssign(viewportWidth * zoom / 2, viewportHeight * zoom / 2); 104 | 105 | // adjust for the fact that the center of the screen is at "position" 106 | screenCoords.minusAssign(position); 107 | 108 | return screenCoords; 109 | } 110 | 111 | public void update() { 112 | if (!hasChanged) 113 | return; 114 | 115 | hasChanged = false; 116 | 117 | // recreate the projection matrix 118 | projection = (glm.INSTANCE.ortho(zoom * -viewportWidth / 2, zoom * viewportWidth / 2, zoom * -viewportHeight / 2, zoom * viewportHeight / 2)); 119 | 120 | // recreate the view matrix 121 | //@formatter:off 122 | view.put( 123 | 1, 0, 0, position.getX(), 124 | 0, 1, 0, position.getY(), 125 | 0, 0, 1, 0, 126 | 0, 0, 0, 1 127 | ); 128 | view.transposeAssign(); 129 | //@formatter:on 130 | // calculate the combined transformation 131 | combined.put(projection); 132 | combined.timesAssign(view); 133 | 134 | // combined.inverse(invCombined); 135 | 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/Color.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics; 17 | 18 | import glm_.vec4.Vec4; 19 | 20 | /** 21 | * Holds a single color value 22 | * 23 | */ 24 | public class Color { 25 | // some standard predefined colors 26 | public static final Color RED = new Color(1, 0, 0); 27 | public static final Color GREEN = new Color(0, 1, 0); 28 | public static final Color BLUE = new Color(0, 0, 1); 29 | public static final Color YELLOW = new Color(1, 1, 0); 30 | public static final Color MAGENTA = new Color(1, 0, 1); 31 | public static final Color TEAL = new Color(0, 1, 1); 32 | 33 | public static final Color WHITE = new Color(1, 1, 1); 34 | public static final Color BLACK = new Color(0, 0, 0); 35 | // public static final Color MAGENTA = new Color(1, 0, 0); 36 | 37 | public float r, g, b, a; 38 | 39 | public Color(float r, float g, float b) { 40 | this(r, g, b, 1); 41 | } 42 | 43 | public Color(float r, float g, float b, float a) { 44 | this.r = r; 45 | this.g = g; 46 | this.b = b; 47 | this.a = a; 48 | } 49 | 50 | public Color(Vec4 color) { 51 | this.r = color.getX(); 52 | this.g = color.getY(); 53 | this.b = color.getZ(); 54 | this.a = color.getW(); 55 | } 56 | 57 | /** https://github.com/libgdx/libgdx/blob/master/gdx/src/com/badlogic/gdx/utils/NumberUtils.java */ 58 | public float toFloatBits() { 59 | return colorToFloatBits(r, g, b, a); 60 | } 61 | 62 | public static float colorToFloatBits(float r, float g, float b, float a) { 63 | int colorI = ((int) (255 * a) << 24) | ((int) (255 * b) << 16) | ((int) (255 * g) << 8) | ((int) (255 * r)); 64 | 65 | return Float.intBitsToFloat(colorI & 0xfeffffff); 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/PrimitiveRenderer.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics; 17 | 18 | import glm_.mat4x4.Mat4; 19 | import glm_.vec3.Vec3; 20 | import glm_.vec4.Vec4; 21 | 22 | import static org.lwjgl.opengl.GL11.*; 23 | //import static org.lwjgl.opengl.GL15.*; 24 | 25 | import com.fmsz.gridmapgl.graphics.gl.Shader; 26 | import com.fmsz.gridmapgl.graphics.gl.VertexArray; 27 | import com.fmsz.gridmapgl.graphics.gl.VertexBuffer; 28 | import com.fmsz.gridmapgl.graphics.gl.VertexBufferLayout; 29 | 30 | public class PrimitiveRenderer { 31 | 32 | // the layout used by this renderer 33 | private final VertexBufferLayout layout = new VertexBufferLayout().push(GL_FLOAT, 3, false).push(GL_UNSIGNED_BYTE, 4, true); 34 | 35 | // the vertex array, vertex buffer and shader used by this renderer 36 | private VertexArray va; 37 | private VertexBuffer vb; 38 | private Shader shader; 39 | 40 | // stores and keeps track of vertices to draw 41 | private float vertices[]; 42 | private final int maxVertices; 43 | private int vertexCount = 0; 44 | private int index = 0; 45 | 46 | // the projection*model*view matrix 47 | private final Mat4 projModelView = new Mat4(); 48 | 49 | // the primitive type being drawn 50 | private int primitiveType = 0; 51 | 52 | public PrimitiveRenderer(int maxVertices) { 53 | this.maxVertices = maxVertices; 54 | 55 | // load our shader 56 | shader = new Shader("res/shaders/basic.shader"); 57 | shader.bind(); 58 | 59 | // create a vertex buffer to hold our data (size in bytes) 60 | vb = new VertexBuffer(maxVertices * layout.getStride()); 61 | 62 | // create our view of the vertices (size in float = size in bytes / 4) 63 | vertices = new float[maxVertices * 4]; 64 | 65 | // create vertex array and combine our vertex buffer with the layout 66 | va = new VertexArray(); 67 | va.addBuffer(vb, layout); 68 | 69 | } 70 | 71 | public void setMVP(Mat4 projModelView) { 72 | this.projModelView.put(projModelView); 73 | } 74 | 75 | // begins a new drawing session of the specified primitive type 76 | public void begin(int primitiveType) { 77 | this.primitiveType = primitiveType; 78 | } 79 | 80 | public void vertex(Vec3 vertex) { 81 | vertex(vertex.getX(), vertex.getY(), vertex.getZ()); 82 | } 83 | 84 | public void vertex(float x, float y, float z) { 85 | // if the buffer is full, do a "flush" 86 | if (vertexCount >= maxVertices) { 87 | end(); 88 | begin(primitiveType); 89 | } 90 | 91 | vertices[index + 0] = x; 92 | vertices[index + 1] = y; 93 | vertices[index + 2] = z; 94 | 95 | index += 4;// 7; 96 | vertexCount++; 97 | } 98 | 99 | public void color(Vec4 color) { 100 | color(color.getX(), color.getY(), color.getZ(), color.getW()); 101 | } 102 | 103 | public void color(Color color) { 104 | color(color.r, color.g, color.b, color.a); 105 | } 106 | 107 | public void color(float r, float g, float b) { 108 | color(r, g, b, 1.0f); 109 | } 110 | 111 | public void color(float r, float g, float b, float a) { 112 | 113 | // int colorI = ((int) (255 * a) << 24) | ((int) (255 * b) << 16) | ((int) (255 * g) << 8) | ((int) (255 * r)); 114 | 115 | color(Color.colorToFloatBits(r, g, b, a));// Float.intBitsToFloat(colorI & 0xfeffffff); 116 | 117 | /* 118 | vertices[index + 3] = r; 119 | vertices[index + 4] = g; 120 | vertices[index + 5] = b; 121 | vertices[index + 6] = a; 122 | */ 123 | } 124 | 125 | public void color(float colorBits) { 126 | vertices[index + 3] = colorBits; 127 | } 128 | 129 | public void end() { 130 | 131 | // use our shader 132 | shader.bind(); 133 | if (projModelView != null) 134 | shader.setUniformMat4("u_projModelView", projModelView); 135 | 136 | // upload our data 137 | vb.bind(); 138 | vb.setVertices(vertices, 0, index); 139 | 140 | // do the actual drawing using a draw call 141 | va.bind(); 142 | glDrawArrays(primitiveType, 0, vertexCount); 143 | 144 | // reset state 145 | vertexCount = 0; 146 | index = 0; 147 | } 148 | 149 | public int getVertexCount() { 150 | return vertexCount; 151 | } 152 | 153 | public int getMaxVertices() { 154 | return maxVertices; 155 | } 156 | 157 | public void dispose() { 158 | va.dispose(); 159 | vb.dispose(); 160 | shader.dispose(); 161 | } 162 | 163 | } 164 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/ShapeRenderer.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics; 17 | 18 | import static org.lwjgl.opengl.GL11.GL_LINES; 19 | import static org.lwjgl.opengl.GL11.GL_POINTS; 20 | import static org.lwjgl.opengl.GL11.GL_TRIANGLES; 21 | 22 | import com.fmsz.gridmapgl.math.MathUtil; 23 | 24 | import glm_.mat4x4.Mat4; 25 | //import glm_.vec2.Vec2; 26 | import glm_.vec2.*; 27 | 28 | /** 29 | * Implements the rendering interface in IRenderer using the primitive renderer (a lot faster than immediate mode) 30 | * 31 | * Lots of inspiration taken from 32 | * https://github.com/libgdx/libgdx/blob/master/gdx/src/com/badlogic/gdx/graphics/glutils/ShapeRenderer.java 33 | */ 34 | public class ShapeRenderer { 35 | public static enum ShapeType { 36 | POINT(GL_POINTS), LINE(GL_LINES), FILLED(GL_TRIANGLES); 37 | 38 | int primitiveType; 39 | 40 | private ShapeType(int primitiveType) { 41 | this.primitiveType = primitiveType; 42 | } 43 | } 44 | 45 | private ShapeType currentShapeType = null; 46 | 47 | private PrimitiveRenderer renderer; 48 | 49 | public ShapeRenderer() { 50 | renderer = new PrimitiveRenderer(1000); 51 | } 52 | 53 | public void line(Vec2 start, Vec2 end, Color color) { 54 | line(start.getX(), start.getY(), end.getX(), end.getY(), color); 55 | } 56 | 57 | public void line(float x1, float y1, float x2, float y2, Color color) { 58 | check(ShapeType.LINE, ShapeType.POINT, 2); 59 | renderer.color(color); 60 | renderer.vertex(x1, y1, 0); 61 | 62 | renderer.color(color); 63 | renderer.vertex(x2, y2, 0); 64 | } 65 | 66 | public void begin(ShapeType type) { 67 | currentShapeType = type; 68 | renderer.begin(currentShapeType.primitiveType); 69 | } 70 | 71 | public void end() { 72 | renderer.end(); 73 | currentShapeType = null; 74 | } 75 | 76 | public void rect(Vec2 pos, Vec2 size, Color color) { 77 | rect(pos.getX(), pos.getY(), size.getX(), size.getY(), color.r, color.b, color.g, color.a); 78 | } 79 | 80 | public void rect(float x, float y, float width, float height, float r, float g, float b, float a) { 81 | rect(x, y, width, height, Color.colorToFloatBits(r, g, b, a)); 82 | } 83 | 84 | public void rect(float x, float y, float width, float height, float colorBits) { 85 | check(ShapeType.LINE, ShapeType.FILLED, 8); 86 | 87 | if (currentShapeType == ShapeType.LINE) { 88 | renderer.color(colorBits); 89 | renderer.vertex(x, y, 0); 90 | renderer.color(colorBits); 91 | renderer.vertex(x + width, y, 0); 92 | 93 | renderer.color(colorBits); 94 | renderer.vertex(x + width, y, 0); 95 | renderer.color(colorBits); 96 | renderer.vertex(x + width, y + height, 0); 97 | 98 | renderer.color(colorBits); 99 | renderer.vertex(x + width, y + height, 0); 100 | renderer.color(colorBits); 101 | renderer.vertex(x, y + height, 0); 102 | 103 | renderer.color(colorBits); 104 | renderer.vertex(x, y + height, 0); 105 | renderer.color(colorBits); 106 | renderer.vertex(x, y, 0); 107 | } else { 108 | renderer.color(colorBits); 109 | renderer.vertex(x, y, 0); 110 | renderer.color(colorBits); 111 | renderer.vertex(x + width, y, 0); 112 | renderer.color(colorBits); 113 | renderer.vertex(x + width, y + height, 0); 114 | 115 | renderer.color(colorBits); 116 | renderer.vertex(x + width, y + height, 0); 117 | renderer.color(colorBits); 118 | renderer.vertex(x, y + height, 0); 119 | renderer.color(colorBits); 120 | renderer.vertex(x, y, 0); 121 | } 122 | 123 | } 124 | 125 | // draws a circle at the specified position 126 | 127 | public void circle(Vec2 pos, float radius, Color color) { 128 | circle(pos.getX(), pos.getY(), radius, color); 129 | } 130 | 131 | public void circle(float x, float y, float radius, Color color) { 132 | // calculate the number of segments needed for a "good" circle 133 | int numberOfSegments = Math.max(1, (int) (/*6*/ 12 * (float) Math.cbrt(radius))); // 10;// (int) (circumference / 20); 134 | circle(x, y, radius, color, numberOfSegments); 135 | 136 | } 137 | 138 | public void circle(float x, float y, float radius, Color color, int numberOfSegments) { 139 | // pre compute color bits 140 | final float colorBits = color.toFloatBits(); 141 | 142 | // the angle between each circle segment 143 | double anglePerSegment = 2 * Math.PI / numberOfSegments; 144 | 145 | // precompute sin and cos 146 | float c = (float) Math.cos(anglePerSegment); 147 | float s = (float) Math.sin(anglePerSegment); 148 | 149 | // starting point 150 | float pointX = radius; 151 | float pointY = 0; 152 | 153 | if (currentShapeType == ShapeType.LINE) { 154 | check(ShapeType.LINE, null, numberOfSegments * 2 + 2); 155 | // place one vertex for each segment 156 | for (int i = 0; i < numberOfSegments; i++) { 157 | // place a point 158 | // glVertex2f(pointX, pointY); 159 | renderer.color(colorBits); 160 | renderer.vertex(pointX + x, pointY + y, 0); 161 | 162 | // rotate point using the "rotation matrix" multiplication to get to the next 163 | float pointXtmp = c * pointX - s * pointY; 164 | pointY = s * pointX + c * pointY; 165 | pointX = pointXtmp; 166 | 167 | renderer.color(colorBits); 168 | renderer.vertex(pointX + x, pointY + y, 0); 169 | 170 | } 171 | // add last point too 172 | renderer.color(colorBits); 173 | renderer.vertex(pointX + x, pointY + y, 0); 174 | 175 | } else { 176 | check(ShapeType.LINE, ShapeType.FILLED, numberOfSegments * 3 + 3); 177 | // place one vertex for each segment 178 | for (int i = 0; i < numberOfSegments; i++) { 179 | renderer.color(colorBits); 180 | renderer.vertex(x, y, 0); 181 | 182 | renderer.color(colorBits); 183 | renderer.vertex(pointX + x, pointY + y, 0); 184 | 185 | // rotate point using the "rotation matrix" multiplication to get to the next 186 | float pointXtmp = c * pointX - s * pointY; 187 | pointY = s * pointX + c * pointY; 188 | pointX = pointXtmp; 189 | 190 | renderer.color(colorBits); 191 | renderer.vertex(pointX + x, pointY + y, 0); 192 | 193 | } 194 | // add last point too 195 | renderer.color(colorBits); 196 | renderer.vertex(x, y, 0); 197 | renderer.color(colorBits); 198 | renderer.vertex(pointX + x, pointY + y, 0); 199 | } 200 | 201 | renderer.color(colorBits); 202 | renderer.vertex(x, y, 0); 203 | 204 | } 205 | 206 | // pre-computed sine and cosine values for the "back-wing" of the arrow 207 | private static final float arrowAngle = (float) (MathUtil.DEG_TO_RAD * 45); 208 | private static final float aSin = MathUtil.sin(arrowAngle); 209 | private static final float aCos = MathUtil.cos(arrowAngle); 210 | 211 | public void arrow(float x, float y, float rot, float radius, Color color) { 212 | // pre-compute color bits 213 | final float colorBits = color.toFloatBits(); 214 | 215 | // pre compute sin and cos for the rotation 216 | float c = (float) Math.cos(rot); 217 | float s = (float) Math.sin(rot); 218 | 219 | // Used Wolfram Alpha for the following trigonometric identities for the corner points: 220 | // cos(t+pi-a) = -sin(a)sin(t)-cos(a)cos(t) 221 | // sin(t+pi-a) = sin(a)cos(t)-cos(a)sin(t) 222 | // cos(t+pi+a) = sin(a)sin(t)-cos(a)cos(t) 223 | // sin(t+pi+a) = sin(a)-cos(t)-cos(a)sin(t) 224 | 225 | // pre compute the factors above for the position of the corner points 226 | float leftCos = (-aSin * s - aCos * c); 227 | float leftSin = (aSin * c - aCos * s); 228 | float rightCos = (aSin * s - aCos * c); 229 | float rightSin = (aSin * -c - aCos * s); 230 | 231 | if (currentShapeType == ShapeType.FILLED) { 232 | check(ShapeType.FILLED, null, 3 * 2); 233 | 234 | // front 235 | renderer.color(colorBits); 236 | renderer.vertex(x + c * radius, y + s * radius, 0); 237 | 238 | // back left 239 | renderer.color(colorBits); 240 | renderer.vertex(x + leftCos * radius, y + leftSin * radius, 0); 241 | 242 | // back middle 243 | renderer.color(colorBits); 244 | renderer.vertex(x - c * (radius / 3), y - s * (radius / 3), 0); 245 | 246 | // back middle (again, starting a new triangle) 247 | renderer.color(colorBits); 248 | renderer.vertex(x - c * (radius / 3), y - s * (radius / 3), 0); 249 | 250 | // back right 251 | renderer.color(colorBits); 252 | renderer.vertex(x + rightCos * radius, y + rightSin * radius, 0); 253 | 254 | // front 255 | renderer.color(colorBits); 256 | renderer.vertex(x + c * radius, y + s * radius, 0); 257 | 258 | } else { 259 | check(ShapeType.LINE, ShapeType.POINT, 4 * 2); 260 | 261 | // front 262 | renderer.color(colorBits); 263 | renderer.vertex(x + c * radius, y + s * radius, 0); 264 | 265 | // back left 266 | renderer.color(colorBits); 267 | renderer.vertex(x + leftCos * radius, y + leftSin * radius, 0); 268 | 269 | // back left (again, starting a new line) 270 | renderer.color(colorBits); 271 | renderer.vertex(x + leftCos * radius, y + leftSin * radius, 0); 272 | 273 | // back middle 274 | renderer.color(colorBits); 275 | renderer.vertex(x - c * (radius / 3), y - s * (radius / 3), 0); 276 | 277 | // back middle (again, starting a new line) 278 | renderer.color(colorBits); 279 | renderer.vertex(x - c * (radius / 3), y - s * (radius / 3), 0); 280 | 281 | // back right 282 | renderer.color(colorBits); 283 | renderer.vertex(x + rightCos * radius, y + rightSin * radius, 0); 284 | 285 | // back right (again, starting a new line) 286 | renderer.color(colorBits); 287 | renderer.vertex(x + rightCos * radius, y + rightSin * radius, 0); 288 | 289 | // front 290 | renderer.color(colorBits); 291 | renderer.vertex(x + c * radius, y + s * radius, 0); 292 | } 293 | 294 | } 295 | 296 | private void check(ShapeType desiredShapeType, ShapeType other, int numberOfNewVertices) { 297 | if (currentShapeType == null) 298 | throw new IllegalStateException("ShapeRenderer: Begin must be called first!"); 299 | 300 | // do we need to "restart" ? 301 | if (currentShapeType != desiredShapeType && currentShapeType != other) { 302 | end(); 303 | begin(desiredShapeType); 304 | } else if (renderer.getVertexCount() + numberOfNewVertices > renderer.getMaxVertices()) { 305 | ShapeType type = currentShapeType; 306 | end(); 307 | begin(type); 308 | } 309 | } 310 | 311 | public void setMVP(Mat4 mvp) { 312 | renderer.setMVP(mvp); 313 | } 314 | 315 | public void dispose() { 316 | renderer.dispose(); 317 | } 318 | 319 | } 320 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/gl/Shader.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics.gl; 17 | 18 | import static org.lwjgl.opengl.GL11.GL_FALSE; 19 | 20 | import java.io.IOException; 21 | import java.nio.FloatBuffer; 22 | import java.nio.file.Files; 23 | import java.nio.file.Paths; 24 | import java.util.HashMap; 25 | import java.util.List; 26 | 27 | import org.lwjgl.BufferUtils; 28 | 29 | import glm_.mat4x4.Mat4; 30 | 31 | import static org.lwjgl.opengl.GL20.*; 32 | 33 | public class Shader { 34 | 35 | private enum ShaderType { 36 | NONE(-1), VERTEX(0), FRAGMENT(1); 37 | 38 | int index; 39 | 40 | private ShaderType(int index) { 41 | this.index = index; 42 | } 43 | } 44 | 45 | private final int id; 46 | private HashMap cachedUniforms = new HashMap<>(); 47 | 48 | private FloatBuffer mat4Buffer = BufferUtils.createFloatBuffer(16); 49 | 50 | public Shader(String filename) { 51 | // read file 52 | String[] source = parseShaderFile(filename); 53 | 54 | // create the shader program 55 | this.id = createShader(source[ShaderType.VERTEX.index], source[ShaderType.FRAGMENT.index]); 56 | } 57 | 58 | private int createShader(String vertexShader, String fragmentShader) { 59 | // create program 60 | int program = glCreateProgram(); 61 | 62 | // create vertex shader 63 | int vs = compileShader(vertexShader, GL_VERTEX_SHADER); 64 | int fs = compileShader(fragmentShader, GL_FRAGMENT_SHADER); 65 | 66 | // attach our shaders to the program 67 | glAttachShader(program, vs); 68 | glAttachShader(program, fs); 69 | 70 | glLinkProgram(program); 71 | glValidateProgram(program); 72 | 73 | // delete the shaders, they are stored in our program now 74 | glDeleteShader(vs); 75 | glDeleteShader(fs); 76 | 77 | return program; 78 | } 79 | 80 | private int compileShader(String source, int shaderType) { 81 | // create vertex shader 82 | int id = glCreateShader(shaderType); 83 | glShaderSource(id, source); 84 | glCompileShader(id); 85 | 86 | // error handling 87 | int[] result = new int[1]; 88 | glGetShaderiv(id, GL_COMPILE_STATUS, result); 89 | 90 | if (result[0] == GL_FALSE) { 91 | 92 | // get error message 93 | String error = glGetShaderInfoLog(id); 94 | 95 | System.out.println("Failed to compile " + (shaderType == GL_VERTEX_SHADER ? "vertex" : "fragment") + " shader"); 96 | System.out.println(error); 97 | 98 | glDeleteShader(id); 99 | return 0; 100 | } 101 | 102 | return id; 103 | } 104 | 105 | public void setUniform4f(String name, float v0, float v1, float v2, float v3) { 106 | glUniform4f(getUniformLocation(name), v0, v1, v2, v3); 107 | } 108 | 109 | public void setUniformMat4(String name, Mat4 matrix4f) { 110 | matrix4f.to(mat4Buffer); 111 | glUniformMatrix4fv(getUniformLocation(name), false, mat4Buffer); 112 | } 113 | 114 | /** 115 | * Retrieves the uniform location based on its name 116 | * 117 | * @param name 118 | * the name of the uniform 119 | * @return the location of the specified uniform 120 | */ 121 | private int getUniformLocation(String name) { 122 | if (cachedUniforms.containsKey(name)) 123 | return cachedUniforms.get(name); 124 | 125 | int location = glGetUniformLocation(id, name); 126 | if (location == -1) 127 | System.out.println("Warning: uniform " + name + " does not exist!"); 128 | 129 | cachedUniforms.put(name, location); 130 | 131 | return location; 132 | } 133 | 134 | public void bind() { 135 | glUseProgram(id); 136 | } 137 | 138 | public void unbind() { 139 | glUseProgram(0); 140 | } 141 | 142 | public void dispose() { 143 | glDeleteProgram(id); 144 | } 145 | 146 | private static String[] parseShaderFile(String filename) { 147 | 148 | List lines = null; 149 | 150 | try { 151 | lines = Files.readAllLines(Paths.get(filename)); 152 | } catch (IOException e) { 153 | e.printStackTrace(); 154 | return null; 155 | } 156 | 157 | ShaderType current = ShaderType.NONE; 158 | 159 | String[] contents = new String[] { "", "" }; 160 | 161 | for (String line : lines) { 162 | if (line.startsWith("#shader")) { 163 | if (line.contains("vertex")) { 164 | // set mode to vertex 165 | current = ShaderType.VERTEX; 166 | } else if (line.contains("fragment")) { 167 | // set mode to vertex 168 | current = ShaderType.FRAGMENT; 169 | } 170 | } else { 171 | // add the read content 172 | contents[current.index] += line + "\n"; 173 | } 174 | } 175 | 176 | return contents; 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/gl/VertexArray.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics.gl; 17 | 18 | //import static org.lwjgl.opengl.GL11.*; 19 | //import static org.lwjgl.opengl.GL15.*; 20 | import static org.lwjgl.opengl.GL20.*; 21 | import static org.lwjgl.opengl.GL30.*; 22 | 23 | import java.util.ArrayList; 24 | 25 | import com.fmsz.gridmapgl.graphics.gl.VertexBufferLayout.LayoutElement; 26 | 27 | public class VertexArray { 28 | 29 | private int vaoId = -1; 30 | 31 | public VertexArray() { 32 | // generate buffer 33 | vaoId = glGenVertexArrays(); 34 | glBindVertexArray(vaoId); 35 | 36 | } 37 | 38 | /** 39 | * Adds a buffer to this VertexArray together with the specified layout 40 | * 41 | */ 42 | public void addBuffer(VertexBuffer vb, VertexBufferLayout layout) { 43 | bind(); 44 | vb.bind(); 45 | 46 | // attach the layout (this binds the layout and VBO to the VAO) 47 | 48 | int offset = 0; 49 | 50 | ArrayList elements = layout.getElements(); 51 | for (int i = 0; i < elements.size(); i++) { 52 | LayoutElement element = elements.get(i); 53 | 54 | glEnableVertexAttribArray(i); 55 | glVertexAttribPointer(i, element.count, element.type, element.normalized, layout.getStride(), offset); 56 | 57 | offset += element.count * LayoutElement.getGLSize(element.type); 58 | } 59 | 60 | } 61 | 62 | public void bind() { 63 | glBindVertexArray(vaoId); 64 | } 65 | 66 | public void unbind() { 67 | glBindVertexArray(0); 68 | } 69 | 70 | public void dispose() { 71 | glDeleteVertexArrays(vaoId); 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/gl/VertexBuffer.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics.gl; 17 | 18 | import java.nio.ByteBuffer; 19 | import java.nio.FloatBuffer; 20 | 21 | import org.lwjgl.BufferUtils; 22 | 23 | //import static org.lwjgl.opengl.GL11.*; 24 | import static org.lwjgl.opengl.GL15.*; 25 | //import static org.lwjgl.opengl.GL20.*; 26 | //import static org.lwjgl.opengl.GL30.*; 27 | 28 | /** 29 | * A class wrapping an OpenGL VertexBuffer. It's basically a buffer of data. 30 | * 31 | */ 32 | public class VertexBuffer { 33 | 34 | // a byte buffer for the data and FloatBuffer view of it 35 | private ByteBuffer byteBuffer; 36 | private FloatBuffer buffer; 37 | 38 | private int bufferId = -1; 39 | private boolean isBound = false; 40 | 41 | public VertexBuffer(int size) { 42 | // 3 is the size of the attributes 43 | byteBuffer = BufferUtils.createByteBuffer(size); 44 | buffer = byteBuffer.asFloatBuffer(); 45 | 46 | // buffer.flip(); 47 | // byteBuffer.flip(); 48 | 49 | // generate buffer 50 | bufferId = glGenBuffers(); 51 | 52 | } 53 | 54 | public void setVertices(float[] vertices) { 55 | setVertices(vertices, 0, vertices.length); 56 | } 57 | 58 | public void setVertices(float[] vertices, int offset, int count) { 59 | buffer.position(0); 60 | buffer.limit(count); 61 | buffer.put(vertices, offset, count); 62 | 63 | 64 | // send new vertices to GPU 65 | if (!isBound) 66 | bind(); 67 | glBufferData(GL_ARRAY_BUFFER, byteBuffer, GL_DYNAMIC_DRAW); 68 | } 69 | 70 | public void bind() { 71 | glBindBuffer(GL_ARRAY_BUFFER, bufferId); 72 | isBound = true; 73 | } 74 | 75 | public void unbind() { 76 | glBindBuffer(GL_ARRAY_BUFFER, 0); 77 | isBound = false; 78 | } 79 | 80 | public void dispose() { 81 | glDeleteBuffers(bufferId); 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/graphics/gl/VertexBufferLayout.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.graphics.gl; 17 | 18 | import java.util.ArrayList; 19 | 20 | import static org.lwjgl.opengl.GL11.*; 21 | 22 | /** 23 | * Holds the Layout of the VertexBuffer 24 | * 25 | * @author Anton 26 | * 27 | */ 28 | public class VertexBufferLayout { 29 | static class LayoutElement { 30 | int type; // OpenGl type 31 | int count; // Number of the type 32 | boolean normalized; 33 | 34 | public LayoutElement(int type, int count, boolean normalized) { 35 | this.type = type; 36 | this.count = count; 37 | this.normalized = normalized; 38 | } 39 | 40 | public static int getGLSize(int glType) { 41 | switch (glType) { 42 | case GL_FLOAT: 43 | return 4; 44 | case GL_UNSIGNED_INT: 45 | return 4; 46 | case GL_UNSIGNED_BYTE: 47 | return 1; 48 | } 49 | return 0; 50 | } 51 | 52 | } 53 | 54 | private ArrayList elements = new ArrayList<>(); 55 | 56 | // keep track of the stride (the total size of the vertex) 57 | private int stride = 0; 58 | 59 | public VertexBufferLayout pushf(int count) { 60 | elements.add(new LayoutElement(GL_FLOAT, count, false)); 61 | stride += LayoutElement.getGLSize(GL_FLOAT) * count; 62 | return this; 63 | } 64 | 65 | public VertexBufferLayout pushui(int count) { 66 | elements.add(new LayoutElement(GL_UNSIGNED_INT, count, false)); 67 | stride += LayoutElement.getGLSize(GL_UNSIGNED_INT) * count; 68 | return this; 69 | } 70 | 71 | public VertexBufferLayout pushuc(int count) { 72 | elements.add(new LayoutElement(GL_UNSIGNED_BYTE, count, true)); 73 | stride += LayoutElement.getGLSize(GL_UNSIGNED_BYTE) * count; 74 | return this; 75 | } 76 | 77 | public VertexBufferLayout push(int gltype, int count, boolean normailze) { 78 | elements.add(new LayoutElement(gltype, count, normailze)); 79 | stride += LayoutElement.getGLSize(gltype) * count; 80 | return this; 81 | } 82 | 83 | public int getStride() { 84 | return stride; 85 | } 86 | 87 | public ArrayList getElements() { 88 | return elements; 89 | } 90 | 91 | } 92 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/math/MathUtil.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.math; 17 | 18 | import org.apache.commons.math3.util.FastMath; 19 | 20 | public class MathUtil { 21 | public static final float PI = (float) Math.PI; 22 | public static final float PI2 = (float) Math.PI * 2; 23 | 24 | public static final double RAD_TO_DEG = 180.0 / Math.PI; 25 | public static final double DEG_TO_RAD = Math.PI / 180.0; 26 | 27 | /** 28 | * Methods for calculating sin and cos with float values instead of using double 29 | */ 30 | public static float sin(float radians) { 31 | return (float) FastMath.sin(radians); 32 | } 33 | 34 | public static double sin(double radians) { 35 | return FastMath.sin(radians); 36 | } 37 | 38 | public static float cos(float radians) { 39 | return (float) FastMath.cos(radians); 40 | } 41 | 42 | public static double cos(double radians) { 43 | return FastMath.cos(radians); 44 | } 45 | 46 | public static float atan2(float y, float x) { 47 | return (float) FastMath.atan2(y, x); 48 | } 49 | 50 | public static double atan2(double y, double x) { 51 | return FastMath.atan2(y, x); 52 | } 53 | 54 | // TODO: check this, do we want angles in the range [-PI, PI] instead of [0, 2PI] ??? 55 | public static double angleDiff(double a, double b) { 56 | double dif = ((b - a) % (2 * Math.PI)); // in range 57 | if (a > b) 58 | dif += 2 * Math.PI; 59 | if (dif >= Math.PI) 60 | dif = -(dif - 2 * Math.PI); 61 | 62 | return dif; 63 | } 64 | 65 | public static double angleConstrain(double a) { 66 | while (a < Math.PI) 67 | a += Math.PI * 2; 68 | while (a > Math.PI) 69 | a -= Math.PI * 2; 70 | 71 | return a; 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/math/Transform.java: -------------------------------------------------------------------------------- 1 | package com.fmsz.gridmapgl.math; 2 | 3 | import com.fmsz.gridmapgl.slam.Pose; 4 | 5 | public abstract class Transform { 6 | 7 | public abstract double transformX(double x, double y); 8 | 9 | public abstract double transformY(double x, double y); 10 | 11 | // creates a transform that converts from local robot coordinates to world coordinates based 12 | // on the given robot pose 13 | public static Transform fromRobotToWorld(Pose p) { 14 | // pre-compute sin and cos for speed 15 | double cos = MathUtil.cos(p.theta); 16 | double sin = MathUtil.sin(p.theta); 17 | 18 | // return new Transform object with specific transform 19 | return new Transform() { 20 | 21 | @Override 22 | public double transformX(double x, double y) { 23 | return x * cos - y * sin + p.x; 24 | } 25 | 26 | @Override 27 | public double transformY(double x, double y) { 28 | return x * sin + y * cos + p.y; 29 | } 30 | 31 | }; 32 | } 33 | 34 | } 35 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/GridMapLoader.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | import java.io.DataInputStream; 19 | import java.io.DataOutputStream; 20 | import java.io.FileInputStream; 21 | import java.io.FileOutputStream; 22 | import java.io.IOException; 23 | 24 | import com.fmsz.gridmapgl.app.ObjectSerializer; 25 | import com.fmsz.gridmapgl.slam.GridMap.GridMapData; 26 | 27 | public class GridMapLoader { 28 | private static FileOutputStream fos = null; 29 | private static DataOutputStream dos = null; 30 | 31 | public static void beginSave(String filename) { 32 | 33 | try { 34 | fos = new FileOutputStream(filename, false); 35 | 36 | // create data output stream 37 | dos = new DataOutputStream(fos); 38 | 39 | // write header 40 | dos.writeByte(0xff); 41 | 42 | } catch (IOException e) { 43 | System.err.println("Error opening file: " + filename); 44 | e.printStackTrace(); 45 | } 46 | } 47 | 48 | public static void saveGridMap(GridMap map) { 49 | try { 50 | 51 | ObjectSerializer.writeGridMap(dos, map); 52 | 53 | } catch (IOException e) { 54 | 55 | e.printStackTrace(); 56 | } 57 | 58 | } 59 | 60 | public static void saveGridMapData(GridMapData map) { 61 | try { 62 | ObjectSerializer.writeGridMapData(dos, map); 63 | 64 | } catch (IOException e) { 65 | e.printStackTrace(); 66 | } 67 | } 68 | 69 | public static void endSave() { 70 | try { 71 | dos.flush(); 72 | fos.close(); 73 | 74 | dos = null; 75 | fos = null; 76 | } catch (IOException e) { 77 | e.printStackTrace(); 78 | } 79 | 80 | } 81 | 82 | private static FileInputStream fis = null; 83 | private static DataInputStream dis = null; 84 | 85 | public static void beginLoad(String filename) { 86 | try { 87 | 88 | // open the file 89 | fis = new FileInputStream(filename); 90 | dis = new DataInputStream(fis); 91 | 92 | // correct "header" byte? 93 | byte b; 94 | if ((b = dis.readByte()) != (byte) 0xff) { 95 | dis.close(); 96 | throw new IllegalStateException("Error opening file, header byte is not correct! Wanted " + 0xff + ", got " + b); 97 | } 98 | 99 | } catch (IOException e) { 100 | System.err.println("Error opening file " + filename); 101 | e.printStackTrace(); 102 | } 103 | } 104 | 105 | public static GridMap loadGridMap() { 106 | try { 107 | 108 | return ObjectSerializer.readGridMap(dis); 109 | 110 | } catch (IOException e) { 111 | e.printStackTrace(); 112 | } 113 | return null; 114 | } 115 | 116 | public static GridMapData loadGridMapData(GridMap gridMap) { 117 | 118 | try { 119 | return ObjectSerializer.readGridMapData(dis, gridMap); 120 | } catch (IOException e) { 121 | e.printStackTrace(); 122 | } 123 | 124 | return null; 125 | 126 | } 127 | 128 | public static void endLoad() { 129 | try { 130 | dis.close(); 131 | fis.close(); 132 | } catch (IOException e) { 133 | e.printStackTrace(); 134 | } 135 | } 136 | 137 | } 138 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/Observation.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | import java.util.ArrayList; 19 | import java.util.List; 20 | 21 | import com.fmsz.gridmapgl.math.MathUtil; 22 | 23 | /** 24 | * Class containing all data for a single lidar scan (a complete revolution) 25 | * 26 | * @author Anton 27 | * 28 | */ 29 | public class Observation { 30 | 31 | /** 32 | * Class for storing a single measurement 33 | * 34 | * @author Anton 35 | * 36 | */ 37 | public static class Measurement { 38 | public double angle, distance; 39 | public boolean wasHit; 40 | 41 | public double localX, localY; 42 | 43 | /** Constructs a new measurement based on the given angle and distance in robot's local coordinate frame */ 44 | public Measurement(double angle, double distance, boolean wasHit) { 45 | this.angle = angle; 46 | this.distance = distance; 47 | this.wasHit = wasHit; 48 | 49 | this.localX = distance * MathUtil.cos(angle); 50 | this.localY = distance * MathUtil.sin(angle); 51 | } 52 | 53 | /** Constructs a new measurement based on a given pose and world coordinates of end point */ 54 | public Measurement(float x, float y, boolean wasHit, Pose p) { 55 | float dx = x - p.x; 56 | float dy = y - p.y; 57 | float a = MathUtil.atan2(dy, dx); 58 | 59 | this.distance = Math.sqrt(dx * dx + dy * dy); 60 | this.angle = a - p.theta; 61 | this.wasHit = wasHit; 62 | 63 | // TODO: Probably not correct ^^ 64 | this.localX = dx; 65 | this.localY = dy; 66 | } 67 | 68 | /** Constructs a new measurement based on coordinates given i robot's local coordinate frame */ 69 | public Measurement(double x, double y, boolean wasHit, int dummy) { 70 | this.angle = MathUtil.atan2(y, x); 71 | this.distance = Math.sqrt(x * x + y * y); 72 | this.wasHit = wasHit; 73 | 74 | this.localX = x; 75 | this.localY = y; 76 | } 77 | 78 | } 79 | 80 | private ArrayList measurements; 81 | 82 | public Observation() { 83 | measurements = new ArrayList<>(); 84 | 85 | } 86 | 87 | public void addMeasurement(float angle, float distance, boolean wasHit) { 88 | measurements.add(new Measurement(angle, distance, wasHit)); 89 | } 90 | 91 | public void addMeasurement(Measurement m) { 92 | measurements.add(m); 93 | } 94 | 95 | public List getMeasurements() { 96 | return measurements; 97 | } 98 | 99 | public int getNumberOfMeasurements() { 100 | return measurements.size(); 101 | } 102 | 103 | public void reset() { 104 | measurements.clear(); 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/Odometry.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | import com.fmsz.gridmapgl.math.MathUtil; 19 | 20 | import org.apache.commons.math3.distribution.NormalDistribution; 21 | import org.apache.commons.math3.random.RandomGenerator; 22 | import org.apache.commons.math3.random.Well1024a; 23 | 24 | /** Class for storing the odometry measurement associated with this observation */ 25 | public class Odometry { 26 | // shared random generator for the normal distributions 27 | private static RandomGenerator rndGen = new Well1024a(); 28 | 29 | // used to add noise when applying to Pose 30 | private NormalDistribution ndCenter, ndTheta = new NormalDistribution(); 31 | 32 | public double dCenter, dTheta; 33 | private double dCenterSD, dThetaSD; 34 | 35 | public Odometry(double dCenter, double dTheta) { 36 | this.dCenter = dCenter; 37 | this.dTheta = dTheta; 38 | recalculateStdDev(); 39 | } 40 | 41 | public Odometry(int leftCount, int rightCount) { 42 | // http://faculty.salina.k-state.edu/tim/robotics_sg/Control/kinematics/odometry.html 43 | 44 | // convert encoder values to traveled distance 45 | double dLeft = (double) leftCount / Robot.MOTOR_STEPS_PER_REVOLUTION * MathUtil.PI * Robot.WHEEL_DIAMETER; 46 | double dRight = (double) rightCount / Robot.MOTOR_STEPS_PER_REVOLUTION * MathUtil.PI * Robot.WHEEL_DIAMETER; 47 | 48 | // the amount the center has moved 49 | dCenter = (dLeft + dRight) / 2; 50 | 51 | // the angle moved 52 | dTheta = (dRight - dLeft) / Robot.WHEEL_DISTANCE; 53 | 54 | recalculateStdDev(); 55 | } 56 | 57 | /** 58 | * Calculates the standard deviation used when applying random Gaussian noise to a Pose 59 | */ 60 | public void recalculateStdDev() { 61 | // calculate desired standard deviations, +/- 2SD contains 95.4% 62 | // basic principle: default base case + % of changed amount 63 | dCenterSD = (0.01 + Math.abs(dCenter) * 0.05) / 2; 64 | dThetaSD = 5 * MathUtil.DEG_TO_RAD + 0.1 * Math.abs(dTheta); 65 | 66 | ndCenter = new NormalDistribution(rndGen, dCenter, dCenterSD); 67 | ndTheta = new NormalDistribution(rndGen, dTheta, dThetaSD); 68 | 69 | } 70 | 71 | /** 72 | * Applies this Odometry to the given pose, thus updating it 73 | * 74 | * @param p 75 | * the Pose to change 76 | */ 77 | public void apply(Pose p) { 78 | 79 | // take a sample from this very simple motion model 80 | double d = ndCenter.sample(); 81 | double theta = ndTheta.sample(); 82 | 83 | // do not add noise when we have not moved! 84 | /* 85 | if (dCenter == 0) { 86 | d = 0; 87 | theta = 0; 88 | } 89 | */ 90 | 91 | // apply movement, angle first since this controls the direction of the traveled distance 92 | p.theta = (float) MathUtil.angleConstrain(p.theta + theta); 93 | p.x += MathUtil.cos(p.theta) * d; 94 | p.y += MathUtil.sin(p.theta) * d; 95 | 96 | } 97 | 98 | // Calculates the probability of being at pose p when starting at pose start given this odometry 99 | public double probabiliyOf(Pose start, Pose p) { 100 | // calculate moved distance 101 | double dist = Math.sqrt(start.x - p.x) * (start.x - p.x) + (start.y - p.y) * (start.y - p.y); 102 | return ndCenter.probability(dist) * ndTheta.probability(p.theta); 103 | } 104 | } -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/ParticleFilter.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | /** Class implementing the fundamental methods for creating a Particle Filter */ 19 | public class ParticleFilter { 20 | 21 | public static class Particle { 22 | // mandatory 23 | public double weight = 0; 24 | 25 | // particle members 26 | public Pose pose; 27 | 28 | public Particle(Particle other) { 29 | this.weight = other.weight; 30 | this.pose = new Pose(other.pose); 31 | } 32 | 33 | public Particle(double weight, Pose pose) { 34 | this.weight = weight; 35 | this.pose = pose; 36 | } 37 | 38 | } 39 | 40 | private Particle[] particles; 41 | private int numberOfParticles; 42 | 43 | public ParticleFilter(int numberOfParticles) { 44 | this.numberOfParticles = numberOfParticles; 45 | 46 | // create list to hold our particles 47 | particles = new Particle[numberOfParticles]; 48 | } 49 | 50 | public Particle[] getParticles() { 51 | return particles; 52 | } 53 | 54 | /** 55 | * Does the resampling step of the particle filter (i.e selects numberOfParticles particles with a random chance 56 | * proportional to their weight). Based on an implementation of the "low variance sampling" technique, see the book 57 | * "Probabilistic Robotics" p. 110 58 | */ 59 | public void resample() { 60 | 61 | Particle[] newParticles = new Particle[numberOfParticles]; 62 | int index = 0; 63 | 64 | double r = Math.random() * 1.0 / numberOfParticles; 65 | double c = particles[0].weight; 66 | int i = 1; 67 | 68 | for (int m = 1; m <= numberOfParticles; m++) { 69 | double U = r + (m - 1) * 1.0 / numberOfParticles; 70 | while (U > c) { 71 | i++; 72 | c += particles[i].weight; 73 | } 74 | // add the i:th particle to the new generation (note that this i a copy operation) 75 | newParticles[index++] = new Particle(particles[i]); 76 | 77 | } 78 | 79 | // make the new generation the current one 80 | particles = newParticles; 81 | 82 | } 83 | 84 | } 85 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/Pose.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | /** 19 | * Class for holding one possible robot pose 20 | */ 21 | public class Pose { 22 | public float x, y, theta; 23 | 24 | public Pose(float x, float y, float theta) { 25 | this.x = x; 26 | this.y = y; 27 | this.theta = theta; 28 | } 29 | 30 | public Pose(Pose copy) { 31 | this.x = copy.x; 32 | this.y = copy.y; 33 | this.theta = copy.theta; 34 | } 35 | 36 | @Override 37 | public String toString() { 38 | return String.format("[%.2f, %.2f] @ %.2f", x, y, Math.toDegrees(theta)); 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/RayIterator.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | import java.util.Iterator; 19 | 20 | import glm_.vec2.Vec2i; 21 | 22 | /** 23 | * A class for iterating over all grid cells that overlap with a specific line. 24 | * Lots of inspiration taken from {@link https://playtechs.blogspot.com/2007/03/raytracing-on-grid.html} 25 | * 26 | * @author Anton 27 | * 28 | */ 29 | public class RayIterator implements Iterator { 30 | 31 | private final Vec2i vector = new Vec2i(); 32 | private int x, y, width, height, x_inc, y_inc, n; 33 | private float dx, dy, error; 34 | 35 | /** 36 | * Construct a new {@link RayIterator} width the specified width and height boundaries 37 | * 38 | * @param width 39 | * the width of the grid 40 | * @param height 41 | * the height of the grid 42 | * 43 | */ 44 | public RayIterator(int width, int height) { 45 | // save parameters 46 | this.width = width; 47 | this.height = height; 48 | } 49 | 50 | /** 51 | * Initializes this {@link RayIterator} to iterate over the line specified. The start and end points are specified in grid coordinates, 52 | * with the origin at the bottom left. 53 | * 54 | * @param x0 55 | * the x-coordinate of the start point 56 | * @param y0 57 | * the y-coordinate of the start point 58 | * @param x1 59 | * the x-coordinate of the end point 60 | * @param y1 61 | * the y-coordinate of the end point 62 | * @param additionalSteps 63 | * an extra number of steps to perform after the end point has been reached 64 | */ 65 | public void init(float x0, float y0, float x1, float y1, int additionalSteps) { 66 | 67 | // set up stuff 68 | dx = Math.abs(x1 - x0); 69 | dy = Math.abs(y1 - y0); 70 | 71 | x = (int) Math.floor(x0); 72 | y = (int) Math.floor(y0); 73 | 74 | // start with at least one 75 | n = 1 + additionalSteps; 76 | 77 | // decide based on case 78 | if (dx == 0) { 79 | x_inc = 0; 80 | error = Float.POSITIVE_INFINITY; 81 | } else if (x1 > x0) { 82 | x_inc = 1; 83 | n += (int) (Math.floor(x1) - x); 84 | error = (float) ((Math.floor(x0) + 1 - x0) * dy); 85 | } else { 86 | x_inc = -1; 87 | n += x - (int) Math.floor(x1); 88 | error = (float) ((x0 - Math.floor(x0)) * dy); 89 | } 90 | 91 | if (dy == 0) { 92 | y_inc = 0; 93 | error -= Float.POSITIVE_INFINITY; 94 | } else if (y1 > y0) { 95 | y_inc = 1; 96 | n += (int) (Math.floor(y1)) - y; 97 | error -= (Math.floor(y0) + 1 - y0) * dx; 98 | } else { 99 | y_inc = -1; 100 | n += y - (int) (Math.floor(y1)); 101 | error -= (y0 - Math.floor(y0)) * dx; 102 | } 103 | 104 | } 105 | 106 | @Override 107 | public boolean hasNext() { 108 | return n > 0 && !(x < 0 || x >= width || y < 0 || y >= height); 109 | } 110 | 111 | @Override 112 | public Vec2i next() { 113 | // cell coordinates to return 114 | vector.put(x, y); 115 | 116 | // move to next position 117 | if (error > 0) { 118 | y += y_inc; 119 | error -= dx; 120 | } else { 121 | x += x_inc; 122 | error += dy; 123 | } 124 | 125 | // decrease number of cells 126 | n -= 1; 127 | 128 | // return cell coordinates 129 | return vector; 130 | } 131 | 132 | } 133 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/Robot.java: -------------------------------------------------------------------------------- 1 | package com.fmsz.gridmapgl.slam; 2 | 3 | import com.fmsz.gridmapgl.math.MathUtil; 4 | 5 | /** Stores robot configuration parameters */ 6 | public class Robot { 7 | /** The distance between the robots wheels [m] */ 8 | public static final double WHEEL_DISTANCE = 0.22; 9 | 10 | /** The diameter of robots wheels [m] */ 11 | public static final double WHEEL_DIAMETER = 0.063; 12 | 13 | /** Number of encoder steps per wheel revolution */ 14 | public static final int MOTOR_STEPS_PER_REVOLUTION = 32 * 30; 15 | 16 | /** Number of stepper motor steps per sensor revolution */ 17 | public static final int SENSOR_STEPS_PER_REVOLUTION = 360 * 2; 18 | 19 | /** The initial angle of the front sensor with respect to the forward direction of the robot */ 20 | public static final float SENSOR_ANGLE_OFFSET = -MathUtil.PI / 2; 21 | 22 | private Robot() { 23 | 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/SLAM.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | import java.util.ArrayList; 19 | 20 | import com.fmsz.gridmapgl.math.MathUtil; 21 | import com.fmsz.gridmapgl.slam.GridMap.GridMapData; 22 | 23 | import glm_.vec2.Vec2; 24 | 25 | /** Class implementing the SLAM algorithm using particle filters */ 26 | public class SLAM { 27 | private GridMap gridMap; 28 | 29 | // particle stuff 30 | public class Particle { 31 | public double weight; 32 | public Pose pose; 33 | public GridMapData m; 34 | 35 | public Particle(Pose x, GridMapData m) { 36 | this.pose = x; 37 | this.m = m; 38 | } 39 | 40 | // constructor that copies the other particle 41 | public Particle(Particle other) { 42 | this.weight = other.weight; 43 | this.pose = new Pose(other.pose); 44 | this.m = gridMap.createMapData(other.m); 45 | } 46 | 47 | } 48 | 49 | private ArrayList particles; 50 | private int numParticles = 500; 51 | 52 | private Particle strongestParticle = null; 53 | 54 | // private Random rand = new Random(); 55 | 56 | public SLAM() { 57 | gridMap = new GridMap(6.0f, 6.0f, 0.05f, new Vec2(-3.0f, -3.0f)); 58 | 59 | particles = new ArrayList<>(numParticles); 60 | 61 | reset(); 62 | } 63 | 64 | /** Resets all the particles to an initial Pose of (0, 0, 0) and with a blank map */ 65 | public void reset() { 66 | particles.clear(); 67 | for (int i = 0; i < numParticles; i++) { 68 | Particle p = new Particle(new Pose(0, 0, 0), gridMap.createMapData(null)); 69 | 70 | // uniform weight initially 71 | p.weight = 1.0 / numParticles; 72 | 73 | particles.add(p); 74 | } 75 | strongestParticle = particles.get(0); 76 | 77 | } 78 | 79 | /** The main SLAM update loop */ 80 | public double update(Observation z, Odometry u) { 81 | // skip map integration if this measurement involved a large rotation as the measurements are very uncertain 82 | boolean skipUpdate = Math.abs(u.dTheta) > MathUtil.DEG_TO_RAD * 30; 83 | 84 | strongestParticle = null; 85 | 86 | // do logic for all particles 87 | double weightSum = 0; 88 | for (Particle p : particles) { 89 | // first sample a new pose from the motion model based on the given controls (odometry) 90 | p.pose = sampleMotionModel(p.pose, u); 91 | 92 | // calculate the weight of this particle as p(z|x,m) 93 | gridMap.computeLikelihoodMap(p.m); 94 | 95 | // optimize pose position to maximize measurement likelihood 96 | //p.pose = gridMap.findBestPose(p.m, z, p.pose); 97 | p.pose = gridMap.findBestPoseOptim(p.m, z, u, p.pose); 98 | 99 | p.weight = gridMap.probabilityOf(p.m, z, p.pose); 100 | weightSum += p.weight; 101 | 102 | if (!skipUpdate) { 103 | 104 | // integrate the measurement into the particles map 105 | gridMap.integrateObservation(p.m, z, p.pose); 106 | 107 | } 108 | 109 | // store the particle with highest weight 110 | if (strongestParticle == null) 111 | strongestParticle = p; 112 | else { 113 | if (p.weight > strongestParticle.weight) 114 | strongestParticle = p; 115 | } 116 | 117 | } 118 | 119 | // normalize particle weights 120 | for (Particle p : particles) 121 | p.weight /= weightSum; 122 | 123 | // resample 124 | double neff = calculateNeff(); 125 | 126 | // if (neff < numParticles / 2) 127 | // resample(); 128 | 129 | return neff; 130 | 131 | } 132 | 133 | public void resample() { 134 | ArrayList newParticles = new ArrayList<>(numParticles); 135 | 136 | double r = Math.random() * 1.0 / numParticles; 137 | double c = particles.get(0).weight; 138 | int i = 0; 139 | 140 | for (int m = 1; m <= numParticles; m++) { 141 | double U = r + (m - 1) * 1.0 / numParticles; 142 | while (U > c) { 143 | i++; 144 | c += particles.get(i).weight; 145 | } 146 | // add the i:th particle to the new generation (note that this a copying operation) 147 | newParticles.add(new Particle(particles.get(i))); 148 | 149 | } 150 | 151 | // make the new generation the current one 152 | particles = newParticles; 153 | } 154 | 155 | private Pose sampleMotionModel(Pose x, Odometry u) { 156 | Pose p = new Pose(x); 157 | 158 | // apply odometry motion 159 | if (u != null) 160 | u.apply(p); 161 | 162 | return p; 163 | } 164 | 165 | public Pose getWeightedPose() { 166 | // calculate a weighted average of the position 167 | double xSum = 0, ySum = 0, thetaSum = 0, weightSum = 0; 168 | 169 | for (Particle p : particles) { 170 | xSum += p.pose.x * p.weight; 171 | ySum += p.pose.y * p.weight; 172 | thetaSum += MathUtil.angleConstrain(p.pose.theta) * p.weight; 173 | weightSum += p.weight; 174 | } 175 | 176 | return new Pose((float) (xSum / weightSum), (float) (ySum / weightSum), (float) (thetaSum / weightSum)); 177 | 178 | } 179 | 180 | public double calculateNeff() { 181 | double sum = 0; 182 | for (Particle p : particles) 183 | sum += p.weight; 184 | 185 | double sqaredSum = 0; 186 | for (Particle p : particles) 187 | sqaredSum += (p.weight / sum) * (p.weight / sum); 188 | 189 | return 1.0 / sqaredSum; 190 | } 191 | 192 | public ArrayList getParticles() { 193 | return particles; 194 | } 195 | 196 | public Particle getStrongestParticle() { 197 | return strongestParticle; 198 | } 199 | 200 | public GridMap getGridMap() { 201 | return gridMap; 202 | } 203 | 204 | } 205 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/SensorModel.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | public class SensorModel { 19 | /** Defines the max sensing range for the sensor */ 20 | public static final float SENSOR_MAX_RANGE = 10.0f; 21 | //public static final float SENSOR_NO_RESPONSE_THRESHHOLD = 8.0f; 22 | 23 | public static final double P_FREE = 0.30f; 24 | public static final double P_OCCUPPIED = 0.9f; 25 | public static final double P_PRIOR = 0.5f; 26 | 27 | /** 28 | * Returns the probability of the currentDistance being occupied, given the measured distance and whether or not it was 29 | * a hit. Also called the "inverse sensor model". Inspired by this video. 30 | */ 31 | public static double inverseSensorModel(float currentDistance, float measuredDistance, boolean wasHit, float hitTolerance) { 32 | if (!wasHit) 33 | return (currentDistance < measuredDistance ? P_FREE : P_PRIOR); 34 | 35 | if (currentDistance < measuredDistance - hitTolerance / 2) 36 | return P_FREE; 37 | if (currentDistance > measuredDistance + hitTolerance / 2) 38 | return P_PRIOR; 39 | 40 | return P_OCCUPPIED; 41 | } 42 | 43 | /** 44 | * An implementation of the above method, but using squared distances instead to increase performance (avoid a sqrt for each visited cell) 45 | * You need to provide your own maxDistSq = (measuredDistance + hitTolerance/2)^2 and minDistSq = (measuredDistance - hitTolerance/2)^2 46 | */ 47 | public static double inverseSensorModelSq(float currentDistanceSq, float measuredDistanceSq, boolean wasHit, float maxDistSq, float minDistSq) { 48 | if (!wasHit) 49 | return (currentDistanceSq < measuredDistanceSq ? P_FREE : P_PRIOR); 50 | 51 | if (currentDistanceSq < minDistSq) 52 | return P_FREE; 53 | if (currentDistanceSq > maxDistSq) 54 | return P_PRIOR; 55 | 56 | return P_OCCUPPIED; 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /java/GridMapGL/src/main/java/com/fmsz/gridmapgl/slam/TimeFrame.java: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 Anton Berneving 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | package com.fmsz.gridmapgl.slam; 17 | 18 | /** Holds one single time step of received data */ 19 | public class TimeFrame { 20 | public Observation z; 21 | public Odometry u; 22 | 23 | // optional timeStamp variable 24 | // public float timeStamp = -1; 25 | 26 | public TimeFrame(Observation z, Odometry u) { 27 | this.z = z; 28 | this.u = u; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /robot/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ -------------------------------------------------------------------------------- /robot/ARDUINO_SKETCH/encoder_functions.ino: -------------------------------------------------------------------------------- 1 | 2 | // interrupt routine called to update encoder counters 3 | void int_encoder_left(){ 4 | // if the states are the same, increment; otherwise decrement 5 | if((ENC_PORT & _BV(LEFT_ENCA_BIT)) >> LEFT_ENCA_BIT == (ENC_PORT & _BV(LEFT_ENCB_BIT)) >> LEFT_ENCB_BIT ){ 6 | motor_left.current_encoder_counter++; 7 | }else{ 8 | motor_left.current_encoder_counter--; 9 | } 10 | } 11 | 12 | // interrupt routine called to update encoder counters 13 | void int_encoder_right(){ 14 | // if the states are the same, increment; otherwise decrement 15 | if((ENC_PORT & _BV(RIGHT_ENCA_BIT)) >> RIGHT_ENCA_BIT == (ENC_PORT & _BV(RIGHT_ENCB_BIT)) >> RIGHT_ENCB_BIT ) 16 | motor_right.current_encoder_counter++; 17 | else 18 | motor_right.current_encoder_counter--; 19 | 20 | } 21 | 22 | double getMotorRotationSpeed(motor_t* motor, double dt){ 23 | 24 | // calculate difference in encoder counts since last time 25 | double position_delta = motor->current_encoder_counter - (double)motor->last_encoder_counter; 26 | 27 | // save current position 28 | motor->last_encoder_counter = motor->current_encoder_counter; 29 | 30 | // increase odometry counter 31 | motor->odometry_counter += position_delta; 32 | 33 | // calculate and return current speed in rad/s 34 | return (double) 2.0 * PI * position_delta / ENC_COUNTS_PER_REV / dt; 35 | 36 | } 37 | 38 | double getMotorPosition(motor_t* motor){ 39 | return (double)2.0 * PI * motor->current_encoder_counter / ENC_COUNTS_PER_REV; 40 | } -------------------------------------------------------------------------------- /robot/esp32/.gitignore: -------------------------------------------------------------------------------- 1 | # The build folder 2 | build/ 3 | 4 | # Don't commit the WiFi credentials 5 | wifi_settings.h 6 | -------------------------------------------------------------------------------- /robot/esp32/TFmini.h: -------------------------------------------------------------------------------- 1 | /* 2 | This library was taken from https://github.com/hideakitai/TFmini, 3 | and modified to add support for firmware version 16X of the TFMini (changed Short and added Medium distance mode constants) 4 | 5 | Original MIT License: 6 | 7 | Copyright (c) 2018 Hideaki Tai 8 | 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies of the Software, and to permit persons to whom the Software is 14 | furnished to do so, subject to the following conditions: 15 | 16 | The above copyright notice and this permission notice shall be included in all 17 | copies or substantial portions of the Software. 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #ifndef TFMINI_H 24 | #define TFMINI_H 25 | 26 | class TFmini 27 | { 28 | public: 29 | 30 | static const uint32_t DEFAULT_BAUDRATE = 115200; 31 | 32 | enum class OutputDataFormat { Standard = 0x01, Pixhawk = 0x04 }; 33 | enum class OutputDataUnit { MM = 0x00, CM = 0x01 }; 34 | enum class DetectionPattern { Auto = 0x00, Fixed = 0x01 }; 35 | enum class DistanceMode { Short = 0x00, Meduim = 0x03, Long = 0x07 }; 36 | enum class TriggerSource { Internal = 0x01, External = 0x00 }; 37 | enum class Baudrate 38 | { 39 | BAUD_9600, BAUD_14400, BAUD_19200, BAUD_38400, BAUD_56000, BAUD_57600, BAUD_115200, 40 | BAUD_128000, BAUD_230400, BAUD_256000, BAUD_460800, BAUD_500000, BAUD_512000 41 | }; 42 | 43 | void attach(Stream& s) { stream_ = &s; } 44 | 45 | bool available() 46 | { 47 | if (!stream_) return false; 48 | 49 | update(); 50 | if (b_available_) 51 | { 52 | b_available_ = false; 53 | return true; 54 | } 55 | else 56 | return false; 57 | } 58 | 59 | uint16_t getDistance() const { return packet_.distance.i; } 60 | uint16_t getStrength() const { return packet_.strength.i; } 61 | uint8_t getIntegrationTime() const { return packet_.int_time; } 62 | 63 | // default : Standard 64 | void setOutputDataFormat(const OutputDataFormat fmt) 65 | { 66 | format_ = fmt; 67 | configBegin(); 68 | sendHeader(); 69 | stream_->write((uint8_t)0x00); 70 | stream_->write((uint8_t)0x00); 71 | stream_->write((uint8_t)fmt); 72 | stream_->write((uint8_t)0x06); 73 | configEnd(); 74 | } 75 | 76 | // default : 10ms 77 | void setOutputDataPeriod(const uint16_t ms) 78 | { 79 | configBegin(); 80 | sendHeader(); 81 | stream_->write((uint8_t)((ms >> 8) & 0x00FF)); 82 | stream_->write((uint8_t)((ms >> 0) & 0x00FF)); 83 | stream_->write((uint8_t)0x00); 84 | stream_->write((uint8_t)0x07); 85 | configEnd(); 86 | } 87 | 88 | // default : cm 89 | void setOutputDataUnit(const OutputDataUnit unit) 90 | { 91 | configBegin(); 92 | sendHeader(); 93 | stream_->write((uint8_t)0x00); 94 | stream_->write((uint8_t)0x00); 95 | stream_->write((uint8_t)unit); 96 | stream_->write((uint8_t)0x1A); 97 | configEnd(); 98 | } 99 | 100 | // default : Auto 101 | void setDetectionPattern(const DetectionPattern pttr) 102 | { 103 | configBegin(); 104 | sendHeader(); 105 | stream_->write((uint8_t)0x00); 106 | stream_->write((uint8_t)0x00); 107 | stream_->write((uint8_t)pttr); 108 | stream_->write((uint8_t)0x14); 109 | configEnd(); 110 | } 111 | 112 | // usable when detection pattern is Fixed 113 | void setDistanceMode(const DistanceMode mode) 114 | { 115 | configBegin(); 116 | sendHeader(); 117 | stream_->write((uint8_t)0x00); 118 | stream_->write((uint8_t)0x00); 119 | stream_->write((uint8_t)mode); 120 | stream_->write((uint8_t)0x11); 121 | configEnd(); 122 | } 123 | 124 | // default : 12m 125 | void setRangeLimit(uint16_t mm) 126 | { 127 | configBegin(); 128 | sendHeader(); 129 | stream_->write((uint8_t)((mm >> 8) & 0x00FF)); 130 | stream_->write((uint8_t)((mm >> 0) & 0x00FF)); 131 | stream_->write((uint8_t)0x01); 132 | stream_->write((uint8_t)0x19); 133 | configEnd(); 134 | } 135 | 136 | void disableRangeLimit() 137 | { 138 | configBegin(); 139 | sendHeader(); 140 | stream_->write((uint8_t)0x00); 141 | stream_->write((uint8_t)0x00); 142 | stream_->write((uint8_t)0x00); 143 | stream_->write((uint8_t)0x19); 144 | configEnd(); 145 | } 146 | 147 | // default : low = 20(DEC), high & cm is undefined 148 | void setSignalStrengthThreshold(uint8_t low, uint16_t high, uint8_t cm) 149 | { 150 | configBegin(); 151 | // lower limit 152 | sendHeader(); 153 | stream_->write((uint8_t)low); 154 | stream_->write((uint8_t)0x00); 155 | stream_->write((uint8_t)0x00); 156 | stream_->write((uint8_t)0x20); 157 | // upper limit 158 | sendHeader(); 159 | stream_->write((uint8_t)((high >> 8) & 0x00FF)); 160 | stream_->write((uint8_t)((high >> 0) & 0x00FF)); 161 | stream_->write((uint8_t)cm); 162 | stream_->write((uint8_t)0x21); 163 | configEnd(); 164 | } 165 | 166 | // default : 115200 (0x06) 167 | void setBaudRate(Baudrate baud) 168 | { 169 | configBegin(); 170 | sendHeader(); 171 | stream_->write((uint8_t)0x00); 172 | stream_->write((uint8_t)0x00); 173 | stream_->write((uint8_t)baud); 174 | stream_->write((uint8_t)0x08); 175 | configEnd(); 176 | } 177 | 178 | // default : Internal (100Hz) 179 | void setTriggerSource(const TriggerSource trigger) 180 | { 181 | configBegin(); 182 | sendHeader(); 183 | stream_->write((uint8_t)0x00); 184 | stream_->write((uint8_t)0x00); 185 | stream_->write((uint8_t)trigger); 186 | stream_->write((uint8_t)0x40); 187 | configEnd(); 188 | } 189 | 190 | // reset all settings 191 | void resetSettings() 192 | { 193 | configBegin(); 194 | sendHeader(); 195 | stream_->write((uint8_t)0xFF); 196 | stream_->write((uint8_t)0xFF); 197 | stream_->write((uint8_t)0xFF); 198 | stream_->write((uint8_t)0xFF); 199 | configEnd(); 200 | } 201 | 202 | private: 203 | 204 | void sendHeader() 205 | { 206 | stream_->write((uint8_t)0x42); 207 | stream_->write((uint8_t)0x57); 208 | stream_->write((uint8_t)0x02); 209 | stream_->write((uint8_t)0x00); 210 | } 211 | 212 | void configBegin() 213 | { 214 | sendHeader(); 215 | stream_->write((uint8_t)0x00); 216 | stream_->write((uint8_t)0x00); 217 | stream_->write((uint8_t)0x01); 218 | stream_->write((uint8_t)0x02); 219 | } 220 | 221 | void configEnd() 222 | { 223 | sendHeader(); 224 | stream_->write((uint8_t)0x00); 225 | stream_->write((uint8_t)0x00); 226 | stream_->write((uint8_t)0x00); 227 | stream_->write((uint8_t)0x02); 228 | } 229 | 230 | void update() 231 | { 232 | while(stream_->available()) 233 | { 234 | uint8_t data = (uint8_t)stream_->read(); 235 | 236 | if (format_ == OutputDataFormat::Pixhawk) 237 | { 238 | Serial.println("Pixhawk Format NOT SUPPORTED YET"); 239 | return; 240 | } 241 | 242 | if (state_ != State::CHECKSUM) buffer_.sum += data; 243 | 244 | switch(state_) 245 | { 246 | case State::HEAD_L: 247 | { 248 | reset(); 249 | buffer_.sum = data; 250 | if (data == RECV_FRAME_HEADER) state_ = State::HEAD_H; 251 | break; 252 | } 253 | case State::HEAD_H: 254 | { 255 | if (data == RECV_FRAME_HEADER) state_ = State::DIST_L; 256 | else state_ = State::HEAD_L; 257 | break; 258 | } 259 | case State::DIST_L: 260 | { 261 | buffer_.distance.b[0] = data; 262 | state_ = State::DIST_H; 263 | break; 264 | } 265 | case State::DIST_H: 266 | { 267 | buffer_.distance.b[1] = data; 268 | state_ = State::STRENGTH_L; 269 | break; 270 | } 271 | case State::STRENGTH_L: 272 | { 273 | buffer_.strength.b[0] = data; 274 | state_ = State::STRENGTH_H; 275 | break; 276 | } 277 | case State::STRENGTH_H: 278 | { 279 | buffer_.strength.b[1] = data; 280 | state_ = State::INT_TIME; 281 | break; 282 | } 283 | case State::INT_TIME: 284 | { 285 | buffer_.int_time = data; 286 | state_ = State::RESERVED; 287 | break; 288 | } 289 | case State::RESERVED: 290 | { 291 | state_ = State::CHECKSUM; 292 | break; 293 | } 294 | case State::CHECKSUM: 295 | { 296 | if (buffer_.sum == data) 297 | { 298 | packet_ = buffer_; 299 | b_available_ = true; 300 | } 301 | else 302 | { 303 | b_available_ = false; 304 | } 305 | reset(); 306 | break; 307 | } 308 | default: 309 | { 310 | reset(); 311 | break; 312 | } 313 | } 314 | } 315 | } 316 | 317 | void reset() 318 | { 319 | buffer_.clear(); 320 | state_ = State::HEAD_L; 321 | } 322 | 323 | struct Packet 324 | { 325 | union { uint8_t b[2]; uint16_t i; } distance; 326 | union { uint8_t b[2]; uint16_t i; } strength; 327 | uint8_t int_time; 328 | uint8_t sum; 329 | 330 | void clear() { distance.i = strength.i = int_time = sum = 0; } 331 | }; 332 | 333 | enum class State 334 | { 335 | HEAD_L, HEAD_H, DIST_L, DIST_H, STRENGTH_L, STRENGTH_H, INT_TIME, RESERVED, CHECKSUM 336 | }; 337 | 338 | static const uint8_t RECV_FRAME_HEADER = 0x59; 339 | 340 | Packet packet_; 341 | Packet buffer_; 342 | State state_; 343 | 344 | bool b_available_; 345 | Stream* stream_; 346 | 347 | OutputDataFormat format_ { OutputDataFormat::Standard }; 348 | }; 349 | 350 | #endif // TFMINI_H -------------------------------------------------------------------------------- /robot/esp32/encoder.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | 3 | #include "encoder.h" 4 | #include "pins.h" 5 | 6 | // define variables here 7 | encoder_t encLeft, encRight; 8 | 9 | // Adapted from: https://github.com/espressif/esp-idf/blob/master/examples/peripherals/pcnt/main/pcnt_example_main.c 10 | void init_pcnt(encoder_t *enc) { 11 | 12 | pinMode(enc->encoderAPin, INPUT); 13 | pinMode(enc->encoderBPin, INPUT); 14 | 15 | pcnt_config_t pcnt_config = { 16 | // Set PCNT input signal and control GPIOs 17 | .pulse_gpio_num = enc->encoderAPin, 18 | .ctrl_gpio_num = enc->encoderBPin, 19 | 20 | 21 | // What to do when control input is low or high? 22 | .lctrl_mode = PCNT_MODE_REVERSE, // Reverse counting direction if low 23 | .hctrl_mode = PCNT_MODE_KEEP, // Keep the primary counter mode if high 24 | // What to do on the positive / negative edge of pulse input? 25 | .pos_mode = PCNT_COUNT_INC, // Count up on the positive edge 26 | .neg_mode = PCNT_COUNT_DIS, // Keep the counter value on the negative edge 27 | 28 | // Set the maximum and minimum limit values to watch (NOT USED!!) 29 | .counter_h_lim = PCNT_MAX_VAL, 30 | .counter_l_lim = PCNT_MIN_VAL, 31 | 32 | .unit = enc->pcnt_unit, 33 | .channel = enc->pcnt_channel, 34 | }; 35 | 36 | /* Initialize PCNT unit */ 37 | ESP_ERROR_CHECK( pcnt_unit_config(&pcnt_config) ); 38 | 39 | /* Configure and enable the input filter */ 40 | pcnt_set_filter_value(pcnt_config.unit, 100); 41 | pcnt_filter_enable(pcnt_config.unit); 42 | 43 | // lets go! 44 | pcnt_counter_clear(pcnt_config.unit); 45 | pcnt_counter_resume(pcnt_config.unit); 46 | } 47 | 48 | void initEncoders(){ 49 | 50 | // set up pin definitions 51 | encLeft.encoderAPin = MOTOR_LEFT_ENCA; 52 | encLeft.encoderBPin = MOTOR_LEFT_ENCB; 53 | encLeft.pcnt_unit = PCNT_UNIT_0; 54 | encLeft.pcnt_channel = PCNT_CHANNEL_0; 55 | 56 | encRight.encoderAPin = MOTOR_RIGHT_ENCA; 57 | encRight.encoderBPin = MOTOR_RIGHT_ENCB; 58 | encLeft.pcnt_unit = PCNT_UNIT_1; 59 | encLeft.pcnt_channel = PCNT_CHANNEL_0; 60 | 61 | // and initialize 62 | init_pcnt(&encLeft); 63 | init_pcnt(&encRight); 64 | 65 | } 66 | 67 | int16_t readEncoder(encoder_t* enc){ 68 | int16_t count; 69 | pcnt_get_counter_value(enc->pcnt_unit, &count); 70 | return count; 71 | } 72 | 73 | // resets the encoder 74 | void resetEncoder(encoder_t* enc){ 75 | pcnt_counter_clear(enc->pcnt_unit); 76 | } 77 | 78 | int16_t readAndResetEncoder(encoder_t* enc) { 79 | int16_t count; 80 | pcnt_get_counter_value(enc->pcnt_unit, &count); 81 | 82 | pcnt_counter_clear(enc->pcnt_unit); 83 | 84 | return count; 85 | } -------------------------------------------------------------------------------- /robot/esp32/encoder.h: -------------------------------------------------------------------------------- 1 | #ifndef ENCODER_H 2 | #define ENCODER_H 3 | 4 | #include 5 | 6 | #define PCNT_MAX_VAL (32767) 7 | #define PCNT_MIN_VAL (-32768) 8 | 9 | // a struct holding the encoder values 10 | typedef struct { 11 | int encoderAPin, encoderBPin; 12 | pcnt_unit_t pcnt_unit; 13 | pcnt_channel_t pcnt_channel; 14 | 15 | } encoder_t; 16 | 17 | // theese are defined in encoder.cpp 18 | extern encoder_t encLeft, encRight; 19 | 20 | void initEncoders(); 21 | void resetEncoder(encoder_t* enc); 22 | int16_t readEncoder(encoder_t* enc); 23 | int16_t readAndResetEncoder(encoder_t* enc); 24 | 25 | #endif // ENCODER_H 26 | -------------------------------------------------------------------------------- /robot/esp32/esp32.ino: -------------------------------------------------------------------------------- 1 | #include "WiFi.h" 2 | #include 3 | 4 | #include "pins.h" 5 | #include "encoder.h" 6 | #include "pid.h" 7 | #include "motors.h" 8 | #include "sensor.h" 9 | 10 | #include "mdns.h" 11 | 12 | #define SERVER_PORT 5555 13 | 14 | static const char* TAG = "Main"; 15 | 16 | //// VARIABLES //// 17 | 18 | // vairables for handling WiFi connection 19 | #include "wifi_settings.h" 20 | 21 | // webserver handling the connection on port 5555 22 | WiFiServer server(SERVER_PORT); 23 | 24 | sensor_loop_parameters_t sensorLoopParams; 25 | 26 | void setup() { 27 | // enable debug serial 28 | Serial.begin(115200); 29 | 30 | pinMode(BUILTIN_LED, OUTPUT); 31 | 32 | //we must initialize rorary encoder and motors first 33 | initEncoders(); 34 | initMotors(); 35 | initSensor(); 36 | 37 | // set up networked connection 38 | 39 | startWifi(); 40 | initialize_mdns(); 41 | } 42 | 43 | // initializes mDNS to make the device discoverable by others on the network, taken from the examples/protocols/mdns example in esp-idf 44 | void initialize_mdns() { 45 | // initialize mDNS 46 | ESP_ERROR_CHECK( mdns_init() ); 47 | 48 | // set mDNS hostname 49 | ESP_ERROR_CHECK( mdns_hostname_set("esp32robot") ); 50 | 51 | // set friendly name 52 | ESP_ERROR_CHECK( mdns_instance_name_set("ESP32 Robot") ); 53 | 54 | // finally advertise our service 55 | ESP_ERROR_CHECK( mdns_service_add(NULL, "_controller", "_tcp", SERVER_PORT, NULL, 0) ); 56 | 57 | Serial.println("Successfully registered mDNS service"); 58 | } 59 | 60 | void startWifi(){ 61 | // Connect to Wi-Fi network with SSID and password 62 | Serial.print("Connecting to "); 63 | Serial.println(ssid); 64 | WiFi.begin(ssid, password); 65 | 66 | int failureCounter = 15000 / 200; // 6 seconds 67 | while (WiFi.status() != WL_CONNECTED && --failureCounter > 0) { 68 | // blink built in LED to signal that we are trying to connect 69 | blinkBuiltinLED(1, 200); 70 | } 71 | 72 | // if failure, start internal Wifi Server instead 73 | if(failureCounter == 0){ 74 | 75 | // indicate using built in LED 76 | blinkBuiltinLED(10, 50); 77 | 78 | // start SoftAP instead 79 | WiFi.mode(WIFI_AP); 80 | WiFi.softAP(apSSID, apPwd); 81 | Serial.println("Wait 100 ms for AP_START..."); 82 | delay(100); 83 | 84 | //Serial.println("Set softAPConfig"); 85 | //WiFi.softAPConfig(local_IP, local_IP, subnet); 86 | 87 | 88 | IPAddress IP = WiFi.softAPIP(); 89 | Serial.print("AP IP address: "); 90 | Serial.println(IP); 91 | 92 | // indicate using built in LED 93 | blinkBuiltinLED(10, 50); 94 | } else { 95 | // Configures static IP address 96 | //if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { 97 | //Serial.println("STA Failed to configure"); 98 | //} 99 | 100 | // Print local IP address and start web server 101 | Serial.println(""); 102 | Serial.println("WiFi connected."); 103 | Serial.println("IP address: "); 104 | Serial.println(WiFi.localIP()); 105 | } 106 | 107 | Serial.println("Starting listening..."); 108 | 109 | 110 | // start server listening 111 | server.begin(); 112 | } 113 | 114 | 115 | void loop() { 116 | // Listen for incoming clients 117 | WiFiClient client = server.available(); 118 | 119 | // a new client connects? 120 | if (client) { 121 | Serial.println("Client Connected"); 122 | 123 | 124 | // home the sensor 125 | // homeSensor(); 126 | 127 | // start a new thread for the motors and for reading the sensors 128 | xTaskCreate(motorLoop, "motorLoop", 10000, NULL, 1, NULL); 129 | 130 | // create a queue for the sensor to receive messages on 131 | sensorLoopParams.queueHandle = xQueueCreate(10, sizeof(sensor_queue_item_t)); 132 | 133 | sensorLoopParams.client = &client; 134 | 135 | 136 | xTaskCreate(doSensorLoop, "sensorLoop", 10000, &sensorLoopParams, 2, NULL); 137 | 138 | // do regular loop to handle the incoming control messages 139 | while (client.connected()) { 140 | handleCommands(&sensorLoopParams); 141 | } 142 | 143 | Serial.println("Client Disconnected"); 144 | 145 | // stop motors and reset sensor 146 | stopMotorLoop(); 147 | resetSensor(); 148 | 149 | sensor_queue_item_t item = {TERMINATE, 0}; 150 | xQueueSendToBack(sensorLoopParams.queueHandle, &item, portMAX_DELAY); 151 | } 152 | } 153 | 154 | void blinkBuiltinLED(unsigned int times, unsigned int period){ 155 | period /= 2; 156 | for(int i = 0; i < times; i++){ 157 | digitalWrite(BUILTIN_LED, HIGH); 158 | delay(period); 159 | digitalWrite(BUILTIN_LED, LOW); 160 | delay(period); 161 | } 162 | } 163 | -------------------------------------------------------------------------------- /robot/esp32/motors.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | 3 | #include "motors.h" 4 | #include "pid.h" 5 | #include "pins.h" 6 | 7 | motor_t motor_left, motor_right; 8 | volatile bool running = false; 9 | 10 | // number of ticks required for the desired loop period length 11 | const TickType_t xFrequency = MOTOR_LOOP_PERIOD_US / portTICK_PERIOD_MS ; 12 | 13 | // variable holding the last wake time 14 | TickType_t xLastWakeTime; 15 | 16 | void initMotors(){ 17 | 18 | ///// SETUP PINS ///// 19 | pinMode(MOTOR_LEFT_EN, OUTPUT); 20 | digitalWrite(MOTOR_LEFT_EN, HIGH); 21 | pinMode(MOTOR_LEFT_DIRA, OUTPUT); 22 | pinMode(MOTOR_LEFT_DIRB, OUTPUT); 23 | 24 | pinMode(MOTOR_RIGHT_EN, OUTPUT); 25 | digitalWrite(MOTOR_RIGHT_EN, HIGH); 26 | pinMode(MOTOR_RIGHT_DIRA, OUTPUT); 27 | pinMode(MOTOR_RIGHT_DIRB, OUTPUT); 28 | 29 | 30 | // set up motors and their PID-regulators 31 | motor_left.pid.Kp = PID_KP; 32 | motor_left.pid.Ki = PID_KI; 33 | motor_left.pid.Kd = PID_KD; 34 | motor_left.pid.Tf = PID_TF; 35 | motor_left.pins = {MOTOR_LEFT_EN, MOTOR_LEFT_DIRA, MOTOR_LEFT_DIRB}; 36 | motor_left.enc = &encLeft; 37 | motor_left.pwm_channel = 0; 38 | 39 | motor_right.pid.Kp = PID_KP; 40 | motor_right.pid.Ki = PID_KI; 41 | motor_right.pid.Kd = PID_KD; 42 | motor_right.pid.Tf = PID_TF; 43 | motor_right.pins = {MOTOR_RIGHT_EN, MOTOR_RIGHT_DIRA, MOTOR_RIGHT_DIRB}; 44 | motor_right.enc = &encRight; 45 | motor_right.pwm_channel = 1; 46 | 47 | // set default speed reference 48 | motor_right.speed_reference = 0.0f; 49 | motor_left.speed_reference = 0.0f; 50 | 51 | 52 | // configure PWM chanels for controlling the motors 53 | ledcSetup(motor_left.pwm_channel, 5000, 8); 54 | ledcAttachPin(motor_left.pins.PIN_EN, motor_left.pwm_channel); 55 | 56 | ledcSetup(motor_right.pwm_channel, 5000, 8); 57 | ledcAttachPin(motor_right.pins.PIN_EN, motor_right.pwm_channel); 58 | 59 | } 60 | 61 | void resetMotors() { 62 | // reset speed reference and the speed PIDs 63 | motor_left.speed_reference = 0; 64 | motor_right.speed_reference = 0; 65 | reset_pid(&motor_left.pid); 66 | reset_pid(&motor_right.pid); 67 | 68 | // stop motors 69 | actuate_motor(&motor_left, 0); 70 | actuate_motor(&motor_right, 0); 71 | 72 | // reset the encoder values 73 | resetEncoder(motor_left.enc); 74 | resetEncoder(motor_right.enc); 75 | 76 | motor_left.odometry_counter = 0; 77 | motor_right.odometry_counter = 0; 78 | } 79 | 80 | void stopMotorLoop(){ 81 | running = false; 82 | } 83 | 84 | void motorLoop(void* parameter) { 85 | // initialize the time counter 86 | unsigned long last_timer_us = micros(); 87 | 88 | unsigned long timer_us = 0; 89 | double h = 0; 90 | 91 | resetMotors(); 92 | 93 | // discard first speed readings as they may be erroneous 94 | getMotorRotationSpeed(&motor_left, 0.1); 95 | getMotorRotationSpeed(&motor_right, 0.1); 96 | 97 | // save time when started (for use with vTaskDelayUntil) 98 | xLastWakeTime = xTaskGetTickCount (); 99 | 100 | running = true; 101 | while(running){ 102 | // get current time 103 | timer_us = micros(); 104 | 105 | // calculate elapsed time in seconds 106 | h = (double)(timer_us - last_timer_us) / 1000000.0; 107 | 108 | // store current time for next iteration 109 | last_timer_us = timer_us; 110 | 111 | handle_motor(&motor_left, h); 112 | handle_motor(&motor_right, h); 113 | 114 | // delay until the desired loop period is reached 115 | vTaskDelayUntil(&xLastWakeTime, xFrequency); 116 | } 117 | 118 | // we are exiting, stop motors 119 | resetMotors(); 120 | 121 | vTaskDelete(NULL); 122 | } 123 | 124 | void handle_motor(motor_t* motor, double h){ 125 | double speed = getMotorRotationSpeed(motor, h); 126 | 127 | // calculate error 128 | double e = motor->speed_reference - speed; 129 | 130 | // calculate control signal 131 | double u = calculate_pid(&motor->pid, e, h); 132 | 133 | // constrain control signal to within +-12 volts 134 | double saturated_u = constrain(u, -12.0, 12.0); 135 | 136 | //Serial.println(speed); 137 | 138 | // drive the motor with this signal 139 | actuate_motor(motor, saturated_u); 140 | 141 | } 142 | 143 | // motor function, input voltage in range -12 to 12 volts 144 | void actuate_motor(motor_t* motor, double u){ 145 | // cap u in the range -12 to 12 volts 146 | u = constrain(u, -12.0, 12.0); 147 | 148 | 149 | // theese small voltages will only make the motors whine anyway 150 | if( abs(u) < 0.6) 151 | u = 0; 152 | 153 | // convert voltage to pwm duty cycle 154 | u = 100.0 * (u / 12.0); 155 | 156 | // convert pwm duty cycle to raw value 157 | uint8_t PWM_VALUE = (uint8_t) abs(u * (double) 255 / 100.0 ); 158 | 159 | /* 160 | Serial.print(motor->pins.PIN_EN); 161 | Serial.print(":"); 162 | Serial.println(PWM_VALUE); 163 | */ 164 | 165 | //analogWrite(motor->pins.PIN_EN, PWM_VALUE); 166 | ledcWrite(motor->pwm_channel, PWM_VALUE); 167 | 168 | 169 | if(u >= 0){ 170 | // forward 171 | digitalWrite(motor->pins.PIN_DIRA, HIGH); 172 | digitalWrite(motor->pins.PIN_DIRB, LOW); 173 | }else{ 174 | // backward 175 | digitalWrite(motor->pins.PIN_DIRA, LOW); 176 | digitalWrite(motor->pins.PIN_DIRB, HIGH); 177 | } 178 | } 179 | 180 | double getMotorRotationSpeed(motor_t* motor, double dt){ 181 | 182 | double position_delta = (double) readAndResetEncoder(motor->enc); 183 | 184 | // increase odometry counter 185 | motor->odometry_counter += position_delta; 186 | 187 | // calculate and return current speed in rad/s 188 | return (double) 2.0 * PI * position_delta / ENC_COUNTS_PER_REV / dt; 189 | 190 | } -------------------------------------------------------------------------------- /robot/esp32/motors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef MOTORS_H 4 | #define MOTORS_H 5 | 6 | #include "encoder.h" 7 | #include "pid.h" 8 | 9 | 10 | #define MOTOR_LOOP_FREQ 100 11 | #define MOTOR_LOOP_PERIOD_US 1000/MOTOR_LOOP_FREQ 12 | 13 | // PID values for the motor controllers 14 | #define PID_KP 0.55276367534483 /* 0.610694929511361;//0.641817786149385 ;//6.458906368104240;//5.061601496636267;//3.286079178973016; */ 15 | #define PID_KI 1.64455966045303 /* 1.34329498731559;//1.169731890184110 ;//21.544597297186854;//59.064657944882540;//70.241507066863450; */ 16 | #define PID_KD 0.0101674410396297 /* 0.0220997968974464; */ 17 | #define PID_TF 1/11.8209539589613 /* 1/5.57670843490099; */ 18 | 19 | 20 | 21 | // struct defining the motor properties 22 | typedef struct { 23 | encoder_t* enc; 24 | uint8_t pwm_channel; 25 | int32_t odometry_counter = 0; 26 | PID_t pid; // PID controller for velocity control 27 | double speed_reference = 0; // the reference value used for the PID-controller 28 | struct{ 29 | char PIN_EN, PIN_DIRA, PIN_DIRB; 30 | } pins; // struct for the motors' pins 31 | 32 | } motor_t; 33 | 34 | // these are defined in motors.cpp 35 | extern motor_t motor_left, motor_right; 36 | 37 | 38 | void initMotors(); 39 | void resetMotors(); 40 | 41 | void stopMotorLoop(); 42 | 43 | void motorLoop(void* parameter); 44 | void handle_motor(motor_t* motor, double h); 45 | void actuate_motor(motor_t* motor, double u); 46 | double getMotorRotationSpeed(motor_t* motor, double dt); 47 | 48 | 49 | #endif // MOTORS_H -------------------------------------------------------------------------------- /robot/esp32/pid.cpp: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | 3 | // does the PID calculation and returns the new control output 4 | double calculate_pid(PID_t* pid, double error, double h){ 5 | // proportional part 6 | pid->P = pid->Kp * error; 7 | 8 | // derivative part 9 | pid->D = pid->Tf / (pid->Tf + h) * pid->D + pid->Kd / (pid->Tf + h) * (error - pid->e_old); 10 | 11 | // calculate output 12 | double u = pid->P + pid->I + pid->D; 13 | 14 | // integral part 15 | pid->I += pid->Ki * h * error; 16 | 17 | // save error 18 | pid->e_old = error; 19 | 20 | // return control signal 21 | return u; 22 | } 23 | 24 | void reset_pid(PID_t* pid){ 25 | pid->e_old = 0; 26 | pid->P = 0; 27 | pid->I = 0; 28 | pid->D = 0; 29 | } -------------------------------------------------------------------------------- /robot/esp32/pid.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef PID_H 4 | #define PID_H 5 | 6 | typedef struct { 7 | double Kp = 0.0; 8 | double Ki = 0.0; 9 | double Kd = 0.0; 10 | double Tf = 0.0; 11 | 12 | double P = 0.0; 13 | double I = 0.0; 14 | double D = 0.0; 15 | 16 | double e_old = 0.0; 17 | } PID_t; 18 | 19 | double calculate_pid(PID_t* pid, double error, double h); 20 | void reset_pid(PID_t* pid); 21 | 22 | #endif // PID_H -------------------------------------------------------------------------------- /robot/esp32/pins.h: -------------------------------------------------------------------------------- 1 | #ifndef PINS_H 2 | #define PINS_H 3 | 4 | #define BUILTIN_LED 2 5 | 6 | // Serial2 pins, cannot be changed 7 | #define RDX2 16 8 | #define TXD2 17 9 | 10 | // stepper motor defines 11 | #define STEPPER_EN 23 12 | #define STEPPER_DIR 27 13 | #define STEPPER_STEP 26 14 | #define STEPPER_SENSOR 34 15 | 16 | #define MICROSTEPPING 2 17 | #define STEPS_PER_ROTATION (360*MICROSTEPPING) 18 | 19 | // dc motor defines 20 | #define MOTOR_RIGHT_EN 21 21 | #define MOTOR_RIGHT_DIRA 19 22 | #define MOTOR_RIGHT_DIRB 18 23 | #define MOTOR_RIGHT_ENCA 32 24 | #define MOTOR_RIGHT_ENCB 35 25 | 26 | #define MOTOR_LEFT_EN 22 27 | #define MOTOR_LEFT_DIRA 5 28 | #define MOTOR_LEFT_DIRB 4 29 | #define MOTOR_LEFT_ENCA 25 30 | #define MOTOR_LEFT_ENCB 33 31 | 32 | 33 | // NEW: 480 34 | // #define ENC_COUNTS_PER_REV (32 * 30 * 2) 35 | #define ENC_COUNTS_PER_REV 480 36 | 37 | 38 | #endif // PINS_H -------------------------------------------------------------------------------- /robot/esp32/sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor.h" 2 | 3 | #include "Arduino.h" 4 | #include "WiFiClient.h" 5 | #include "TFmini.h" 6 | 7 | #include "pins.h" 8 | #include "motors.h" 9 | 10 | // struct holding a single measurement (note that the struct's variables must be aligned on a 2-byte boundary, or padding bytes will be added) 11 | typedef struct { 12 | short header = 0x55AA; 13 | short steps; 14 | short frontDistance, backDistance; 15 | } measurement_t; 16 | 17 | // buffer storing all measurements taken during this loop (with a maximum of STEPS_PER_ROTATION samples) 18 | measurement_t measurements[STEPS_PER_ROTATION]; 19 | int measurement_count = 0; 20 | 21 | // variables for the "LiDAR" unit 22 | TFmini tfmini; 23 | 24 | // private function prototypes 25 | void step_motor(unsigned short steps); 26 | 27 | void initSensor(){ 28 | // stepper motor pin definitions 29 | pinMode(STEPPER_EN, OUTPUT); 30 | pinMode(STEPPER_DIR, OUTPUT); 31 | pinMode(STEPPER_STEP, OUTPUT); 32 | pinMode(STEPPER_SENSOR, INPUT); 33 | 34 | // disable stepper motor 35 | digitalWrite(STEPPER_EN, HIGH); 36 | 37 | // select rotational direction 38 | digitalWrite(STEPPER_DIR, HIGH); 39 | 40 | ///// SETUP TFMini ///// 41 | Serial2.begin(TFmini::DEFAULT_BAUDRATE, SERIAL_8N1, RDX2, TXD2); 42 | 43 | delay(100); // Give a little time for things to start 44 | 45 | // Set to Standard Output mode 46 | tfmini.attach(Serial2); 47 | tfmini.setDetectionPattern(TFmini::DetectionPattern::Fixed); 48 | tfmini.setDistanceMode(TFmini::DistanceMode::Meduim); 49 | 50 | //////////////////////// 51 | } 52 | 53 | void resetSensor(){ 54 | // doOnce = 0; 55 | // doContinously = 0; 56 | } 57 | 58 | sensor_queue_item_t handleCommandsItem; 59 | 60 | void handleCommands(sensor_loop_parameters_t* params) { 61 | 62 | 63 | WiFiClient* stream = params->client; 64 | 65 | while(stream->available() > 0){ 66 | 67 | // read one byte 68 | char input = stream->read(); 69 | 70 | if(input == 0x01 || input == 'O'){ // "do once"-command? 71 | handleCommandsItem.command = ENABLE_ONCE; 72 | xQueueSendToBack( params->queueHandle, &handleCommandsItem, 0); 73 | 74 | 75 | }else if (input == 0x02 || input == 'E'){ // "enable continous"-command? 76 | handleCommandsItem.command = ENABLE_CONTINOUSLY; 77 | xQueueSendToBack( params->queueHandle, &handleCommandsItem, 0); 78 | }else if (input == 0x04 || input == 'D'){ // "disable continous"-command? 79 | handleCommandsItem.command = DISABLE; 80 | xQueueSendToBack( params->queueHandle, &handleCommandsItem, 0); 81 | }else if (input == 0x05 || input == 'H'){ // "home sensor"-command? 82 | handleCommandsItem.command = HOME_SENSOR; 83 | xQueueSendToBack( params->queueHandle, &handleCommandsItem, 0); 84 | }else if (input == 0x08){ // "set resolution"-command? 85 | char d; 86 | // wait for next byte 87 | while ((d = stream->read()) == -1); 88 | 89 | short next_steps = (short) (STEPS_PER_ROTATION * ((float)d / 360.0f)); 90 | 91 | handleCommandsItem.command = SET_STEP_LENGTH; 92 | handleCommandsItem.data = next_steps; 93 | xQueueSendToBack( params->queueHandle, &handleCommandsItem, 0); 94 | 95 | }else if (input == 0x10){ // "set motor speed" - command? 96 | motor_left.speed_reference = (double) readFloat(stream); 97 | motor_right.speed_reference = (double) readFloat(stream); 98 | 99 | // Serial.printf("New motor speed: %5.3f, %5.3f\n", motor_left.speed_reference, motor_right.speed_reference); 100 | 101 | } else if (input == 0x15) { // set K_P command? 102 | motor_left.pid.Kp = motor_right.pid.Kp = (double) readFloat(stream); 103 | } else if (input == 0x16) { // set K_I command? 104 | motor_left.pid.Ki = motor_right.pid.Ki = (double) readFloat(stream); 105 | }else if (input == 0x17) { // set K_D command? 106 | motor_left.pid.Kd = motor_right.pid.Kd = (double) readFloat(stream); 107 | }else if (input == 0x18) { // set T_f command? 108 | motor_left.pid.Tf = motor_right.pid.Tf = (double) readFloat(stream); 109 | } 110 | } 111 | } 112 | 113 | 114 | void doSensorLoop(void* parameter){ 115 | sensor_loop_parameters_t* params = (sensor_loop_parameters_t*) parameter; 116 | 117 | QueueHandle_t queue = params->queueHandle; 118 | WiFiClient* stream = params->client; 119 | 120 | // internal state variables 121 | bool doOnce = 0; 122 | bool doContinously = 0; 123 | unsigned short step_counter = 0, next_steps = 4; 124 | 125 | bool shouldTerminate = false; 126 | 127 | BaseType_t xStatus; 128 | sensor_queue_item_t item; 129 | while (!shouldTerminate){ 130 | 131 | // if we are not actively taking measurements, then wait indefinitely 132 | // otherwise don't wait for anything to appear in the queue 133 | 134 | TickType_t ticksToWait = doOnce ? 0 : portMAX_DELAY; 135 | // Serial.printf("Entering queue receive with %d ticksToWait\n", ticksToWait); 136 | 137 | xStatus = xQueueReceive(queue, &item, ticksToWait); 138 | 139 | // Serial.printf("Status: %d, doOnce=%d\n", xStatus, doOnce); 140 | if (xStatus == pdPASS) { 141 | // Serial.println("Got new message!"); 142 | switch (item.command) { 143 | case ENABLE_ONCE: 144 | doOnce = true; 145 | break; 146 | case ENABLE_CONTINOUSLY: 147 | doOnce = true; 148 | doContinously = true; 149 | break; 150 | case DISABLE: 151 | doContinously = false; 152 | break; 153 | case SET_STEP_LENGTH: 154 | next_steps = item.data; 155 | break; 156 | case HOME_SENSOR: 157 | homeSensor(); 158 | step_counter = 0; 159 | break; 160 | case TERMINATE: 161 | shouldTerminate = true; 162 | break; 163 | default: 164 | break; 165 | } 166 | } 167 | 168 | // Serial.printf("After: doOnce=%d, doContinously=%d\n", doOnce, doContinously); 169 | 170 | if (doOnce){ 171 | 172 | // make sure the motor is enabled 173 | digitalWrite(STEPPER_EN, LOW); 174 | 175 | // move the motor 176 | step_motor(next_steps); 177 | 178 | // increase the counter and "loop back" if we exceed the number of steps for one compleete sensor revolution 179 | step_counter += next_steps; 180 | 181 | // check if we have done a complete revolution 182 | if(step_counter > STEPS_PER_ROTATION){ 183 | step_counter -= STEPS_PER_ROTATION; 184 | 185 | // TODO: make this data be available to a secondary task that collates all the information 186 | 187 | // yes, send message (with odometry information) to indicate that 188 | measurements[measurement_count].steps = -1; 189 | measurements[measurement_count].frontDistance = motor_left.odometry_counter; 190 | measurements[measurement_count].backDistance = motor_right.odometry_counter; 191 | measurement_count++; 192 | 193 | // write all data to client 194 | stream->write((const uint8_t*) &measurements, sizeof(measurement_t) * measurement_count); 195 | 196 | // reset buffer counter 197 | measurement_count = 0; 198 | 199 | // reset odometry counters 200 | motor_left.odometry_counter = 0; 201 | motor_right.odometry_counter = 0; 202 | 203 | // this allows the code to either do repeated measurements (if doContinously = true) or only do a measurement once (if doContinously = false and doOnce is set to true once) 204 | doOnce = doContinously; 205 | 206 | // if stopped, disable the motor 207 | if(!doOnce) 208 | digitalWrite(STEPPER_EN, HIGH); 209 | } 210 | 211 | // Serial.println("Before reading TFMini"); 212 | 213 | // take reading 214 | while(!tfmini.available()); 215 | 216 | // Serial.println("After reading TFMini"); 217 | 218 | // save the reading to our buffer 219 | measurements[measurement_count].steps = step_counter; 220 | measurements[measurement_count].frontDistance = tfmini.getDistance() * 10; 221 | measurements[measurement_count].backDistance = tfmini.getStrength(); 222 | measurement_count++; 223 | 224 | } 225 | } 226 | 227 | digitalWrite(STEPPER_EN, HIGH); 228 | 229 | vTaskDelete(NULL); 230 | } 231 | 232 | // reads a single float value from the provided WiFiClient (blocking) 233 | float readFloat(WiFiClient* stream) { 234 | // wait for 4 bytes 235 | while(stream->available() < 4); 236 | 237 | // get the four bytes and convert them to a float 238 | char buffer[4]; 239 | buffer[3] = stream->read(); 240 | buffer[2] = stream->read(); 241 | buffer[1] = stream->read(); 242 | buffer[0] = stream->read(); 243 | 244 | return *((float*)&buffer); 245 | } 246 | 247 | void homeSensor(){ 248 | // enable stepper 249 | digitalWrite(STEPPER_EN, LOW); 250 | 251 | int smoothed = 0, smoothed16 = 0, newVal; 252 | 253 | // startup value 254 | smoothed = analogRead(STEPPER_SENSOR); 255 | smoothed16 = smoothed << 4; 256 | do{ 257 | // do some exponential filtering. See https://forum.arduino.cc/index.php?topic=445844.0 (last post) 258 | smoothed16 = smoothed16 - smoothed + analogRead(STEPPER_SENSOR); 259 | smoothed = smoothed16 >> 4; 260 | 261 | // step the motor 262 | step_motor(1); 263 | 264 | delayMicroseconds(1000); 265 | 266 | // get a new reading 267 | newVal = analogRead(STEPPER_SENSOR); 268 | 269 | //Serial.println(newVal); 270 | 271 | // continue while we see no "peak" in the sensor value 272 | }while(newVal - smoothed < 25); 273 | 274 | // disable stepper 275 | digitalWrite(STEPPER_EN, HIGH); 276 | } 277 | 278 | void step_motor(unsigned short steps){ 279 | for(unsigned short i = 0; i < steps; i++){ 280 | digitalWrite(STEPPER_STEP, HIGH); 281 | delayMicroseconds(800); 282 | digitalWrite(STEPPER_STEP, LOW); 283 | delayMicroseconds(800); 284 | } 285 | } 286 | -------------------------------------------------------------------------------- /robot/esp32/sensor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef SENSOR_H 4 | #define SENSOR_H 5 | 6 | #include "Arduino.h" 7 | #include "WiFiClient.h" 8 | 9 | 10 | enum sensor_command { 11 | ENABLE_ONCE, 12 | ENABLE_CONTINOUSLY, 13 | DISABLE, 14 | SET_STEP_LENGTH, 15 | HOME_SENSOR, 16 | TERMINATE 17 | }; 18 | 19 | typedef struct { 20 | sensor_command command; 21 | short data; 22 | } sensor_queue_item_t; 23 | 24 | typedef struct { 25 | QueueHandle_t queueHandle; 26 | WiFiClient* client; 27 | } sensor_loop_parameters_t; 28 | 29 | void initSensor(); 30 | void handleCommands(sensor_loop_parameters_t *params); 31 | void doSensorLoop(void* parameter); 32 | 33 | float readFloat(WiFiClient* stream); 34 | void homeSensor(); 35 | void resetSensor(); 36 | 37 | #endif // SENSOR_H -------------------------------------------------------------------------------- /robot/esp32/wifi_settings.example.h: -------------------------------------------------------------------------------- 1 | // This file contains the SSID and password information for your Wifi network 2 | 3 | const char* ssid = "SSID"; 4 | const char* password = "password"; 5 | 6 | 7 | // SSID and password for SoftAP Network 8 | const char* apSSID = "Robot"; 9 | const char* apPwd = "robotwifi"; -------------------------------------------------------------------------------- /robot/teensy/TFmini.h: -------------------------------------------------------------------------------- 1 | /* 2 | This library was taken from https://github.com/hideakitai/TFmini, 3 | and modified to add support for firmware version 16X of the TFMini (changed Short and added Medium distance mode constants) 4 | 5 | Original MIT License: 6 | 7 | Copyright (c) 2018 Hideaki Tai 8 | 9 | Permission is hereby granted, free of charge, to any person obtaining a copy 10 | of this software and associated documentation files (the "Software"), to deal 11 | in the Software without restriction, including without limitation the rights 12 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | copies of the Software, and to permit persons to whom the Software is 14 | furnished to do so, subject to the following conditions: 15 | 16 | The above copyright notice and this permission notice shall be included in all 17 | copies or substantial portions of the Software. 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #ifndef TFMINI_H 24 | #define TFMINI_H 25 | 26 | class TFmini 27 | { 28 | public: 29 | 30 | static const uint32_t DEFAULT_BAUDRATE = 115200; 31 | 32 | enum class OutputDataFormat { Standard = 0x01, Pixhawk = 0x04 }; 33 | enum class OutputDataUnit { MM = 0x00, CM = 0x01 }; 34 | enum class DetectionPattern { Auto = 0x00, Fixed = 0x01 }; 35 | enum class DistanceMode { Short = 0x00, Meduim = 0x03, Long = 0x07 }; 36 | enum class TriggerSource { Internal = 0x01, External = 0x00 }; 37 | enum class Baudrate 38 | { 39 | BAUD_9600, BAUD_14400, BAUD_19200, BAUD_38400, BAUD_56000, BAUD_57600, BAUD_115200, 40 | BAUD_128000, BAUD_230400, BAUD_256000, BAUD_460800, BAUD_500000, BAUD_512000 41 | }; 42 | 43 | void attach(Stream& s) { stream_ = &s; } 44 | 45 | bool available() 46 | { 47 | if (!stream_) return false; 48 | 49 | update(); 50 | if (b_available_) 51 | { 52 | b_available_ = false; 53 | return true; 54 | } 55 | else 56 | return false; 57 | } 58 | 59 | uint16_t getDistance() const { return packet_.distance.i; } 60 | uint16_t getStrength() const { return packet_.strength.i; } 61 | uint8_t getIntegrationTime() const { return packet_.int_time; } 62 | 63 | // default : Standard 64 | void setOutputDataFormat(const OutputDataFormat fmt) 65 | { 66 | format_ = fmt; 67 | configBegin(); 68 | sendHeader(); 69 | stream_->write((uint8_t)0x00); 70 | stream_->write((uint8_t)0x00); 71 | stream_->write((uint8_t)fmt); 72 | stream_->write((uint8_t)0x06); 73 | configEnd(); 74 | } 75 | 76 | // default : 10ms 77 | void setOutputDataPeriod(const uint16_t ms) 78 | { 79 | configBegin(); 80 | sendHeader(); 81 | stream_->write((uint8_t)((ms >> 8) & 0x00FF)); 82 | stream_->write((uint8_t)((ms >> 0) & 0x00FF)); 83 | stream_->write((uint8_t)0x00); 84 | stream_->write((uint8_t)0x07); 85 | configEnd(); 86 | } 87 | 88 | // default : cm 89 | void setOutputDataUnit(const OutputDataUnit unit) 90 | { 91 | configBegin(); 92 | sendHeader(); 93 | stream_->write((uint8_t)0x00); 94 | stream_->write((uint8_t)0x00); 95 | stream_->write((uint8_t)unit); 96 | stream_->write((uint8_t)0x1A); 97 | configEnd(); 98 | } 99 | 100 | // default : Auto 101 | void setDetectionPattern(const DetectionPattern pttr) 102 | { 103 | configBegin(); 104 | sendHeader(); 105 | stream_->write((uint8_t)0x00); 106 | stream_->write((uint8_t)0x00); 107 | stream_->write((uint8_t)pttr); 108 | stream_->write((uint8_t)0x14); 109 | configEnd(); 110 | } 111 | 112 | // usable when detection pattern is Fixed 113 | void setDistanceMode(const DistanceMode mode) 114 | { 115 | configBegin(); 116 | sendHeader(); 117 | stream_->write((uint8_t)0x00); 118 | stream_->write((uint8_t)0x00); 119 | stream_->write((uint8_t)mode); 120 | stream_->write((uint8_t)0x11); 121 | configEnd(); 122 | } 123 | 124 | // default : 12m 125 | void setRangeLimit(uint16_t mm) 126 | { 127 | configBegin(); 128 | sendHeader(); 129 | stream_->write((uint8_t)((mm >> 8) & 0x00FF)); 130 | stream_->write((uint8_t)((mm >> 0) & 0x00FF)); 131 | stream_->write((uint8_t)0x01); 132 | stream_->write((uint8_t)0x19); 133 | configEnd(); 134 | } 135 | 136 | void disableRangeLimit() 137 | { 138 | configBegin(); 139 | sendHeader(); 140 | stream_->write((uint8_t)0x00); 141 | stream_->write((uint8_t)0x00); 142 | stream_->write((uint8_t)0x00); 143 | stream_->write((uint8_t)0x19); 144 | configEnd(); 145 | } 146 | 147 | // default : low = 20(DEC), high & cm is undefined 148 | void setSignalStrengthThreshold(uint8_t low, uint16_t high, uint8_t cm) 149 | { 150 | configBegin(); 151 | // lower limit 152 | sendHeader(); 153 | stream_->write((uint8_t)low); 154 | stream_->write((uint8_t)0x00); 155 | stream_->write((uint8_t)0x00); 156 | stream_->write((uint8_t)0x20); 157 | // upper limit 158 | sendHeader(); 159 | stream_->write((uint8_t)((high >> 8) & 0x00FF)); 160 | stream_->write((uint8_t)((high >> 0) & 0x00FF)); 161 | stream_->write((uint8_t)cm); 162 | stream_->write((uint8_t)0x21); 163 | configEnd(); 164 | } 165 | 166 | // default : 115200 (0x06) 167 | void setBaudRate(Baudrate baud) 168 | { 169 | configBegin(); 170 | sendHeader(); 171 | stream_->write((uint8_t)0x00); 172 | stream_->write((uint8_t)0x00); 173 | stream_->write((uint8_t)baud); 174 | stream_->write((uint8_t)0x08); 175 | configEnd(); 176 | } 177 | 178 | // default : Internal (100Hz) 179 | void setTriggerSource(const TriggerSource trigger) 180 | { 181 | configBegin(); 182 | sendHeader(); 183 | stream_->write((uint8_t)0x00); 184 | stream_->write((uint8_t)0x00); 185 | stream_->write((uint8_t)trigger); 186 | stream_->write((uint8_t)0x40); 187 | configEnd(); 188 | } 189 | 190 | // reset all settings 191 | void resetSettings() 192 | { 193 | configBegin(); 194 | sendHeader(); 195 | stream_->write((uint8_t)0xFF); 196 | stream_->write((uint8_t)0xFF); 197 | stream_->write((uint8_t)0xFF); 198 | stream_->write((uint8_t)0xFF); 199 | configEnd(); 200 | } 201 | 202 | private: 203 | 204 | void sendHeader() 205 | { 206 | stream_->write((uint8_t)0x42); 207 | stream_->write((uint8_t)0x57); 208 | stream_->write((uint8_t)0x02); 209 | stream_->write((uint8_t)0x00); 210 | } 211 | 212 | void configBegin() 213 | { 214 | sendHeader(); 215 | stream_->write((uint8_t)0x00); 216 | stream_->write((uint8_t)0x00); 217 | stream_->write((uint8_t)0x01); 218 | stream_->write((uint8_t)0x02); 219 | } 220 | 221 | void configEnd() 222 | { 223 | sendHeader(); 224 | stream_->write((uint8_t)0x00); 225 | stream_->write((uint8_t)0x00); 226 | stream_->write((uint8_t)0x00); 227 | stream_->write((uint8_t)0x02); 228 | } 229 | 230 | void update() 231 | { 232 | while(stream_->available()) 233 | { 234 | uint8_t data = (uint8_t)stream_->read(); 235 | 236 | if (format_ == OutputDataFormat::Pixhawk) 237 | { 238 | Serial.println("Pixhawk Format NOT SUPPORTED YET"); 239 | return; 240 | } 241 | 242 | if (state_ != State::CHECKSUM) buffer_.sum += data; 243 | 244 | switch(state_) 245 | { 246 | case State::HEAD_L: 247 | { 248 | reset(); 249 | buffer_.sum = data; 250 | if (data == RECV_FRAME_HEADER) state_ = State::HEAD_H; 251 | break; 252 | } 253 | case State::HEAD_H: 254 | { 255 | if (data == RECV_FRAME_HEADER) state_ = State::DIST_L; 256 | else state_ = State::HEAD_L; 257 | break; 258 | } 259 | case State::DIST_L: 260 | { 261 | buffer_.distance.b[0] = data; 262 | state_ = State::DIST_H; 263 | break; 264 | } 265 | case State::DIST_H: 266 | { 267 | buffer_.distance.b[1] = data; 268 | state_ = State::STRENGTH_L; 269 | break; 270 | } 271 | case State::STRENGTH_L: 272 | { 273 | buffer_.strength.b[0] = data; 274 | state_ = State::STRENGTH_H; 275 | break; 276 | } 277 | case State::STRENGTH_H: 278 | { 279 | buffer_.strength.b[1] = data; 280 | state_ = State::INT_TIME; 281 | break; 282 | } 283 | case State::INT_TIME: 284 | { 285 | buffer_.int_time = data; 286 | state_ = State::RESERVED; 287 | break; 288 | } 289 | case State::RESERVED: 290 | { 291 | state_ = State::CHECKSUM; 292 | break; 293 | } 294 | case State::CHECKSUM: 295 | { 296 | if (buffer_.sum == data) 297 | { 298 | packet_ = buffer_; 299 | b_available_ = true; 300 | } 301 | else 302 | { 303 | b_available_ = false; 304 | } 305 | reset(); 306 | break; 307 | } 308 | default: 309 | { 310 | reset(); 311 | break; 312 | } 313 | } 314 | } 315 | } 316 | 317 | void reset() 318 | { 319 | buffer_.clear(); 320 | state_ = State::HEAD_L; 321 | } 322 | 323 | struct Packet 324 | { 325 | union { uint8_t b[2]; uint16_t i; } distance; 326 | union { uint8_t b[2]; uint16_t i; } strength; 327 | uint8_t int_time; 328 | uint8_t sum; 329 | 330 | void clear() { distance.i = strength.i = int_time = sum = 0; } 331 | }; 332 | 333 | enum class State 334 | { 335 | HEAD_L, HEAD_H, DIST_L, DIST_H, STRENGTH_L, STRENGTH_H, INT_TIME, RESERVED, CHECKSUM 336 | }; 337 | 338 | static const uint8_t RECV_FRAME_HEADER = 0x59; 339 | 340 | Packet packet_; 341 | Packet buffer_; 342 | State state_; 343 | 344 | bool b_available_; 345 | Stream* stream_; 346 | 347 | OutputDataFormat format_ { OutputDataFormat::Standard }; 348 | }; 349 | 350 | #endif // TFMINI_H -------------------------------------------------------------------------------- /robot/teensy/teensy.ino: -------------------------------------------------------------------------------- 1 | 2 | //// LIBRARIES //// 3 | 4 | #include "Arduino.h" 5 | #include 6 | 7 | #include "TFmini.h" 8 | 9 | //// PIN DEFINES //// 10 | 11 | // stepper motor defines 12 | #define STEPPER_EN 12 13 | #define STEPPER_DIR 10 14 | #define STEPPER_STEP 11 15 | #define STEPPER_SENSOR A9 16 | 17 | #define MICROSTEPPING 2 18 | #define STEPS_PER_ROTATION (360*MICROSTEPPING) 19 | 20 | 21 | // dc motor defines 22 | #define MOTOR_RIGHT_EN 4 23 | #define MOTOR_RIGHT_DIRA 3 24 | #define MOTOR_RIGHT_DIRB 2 25 | #define MOTOR_RIGHT_ENCA 14 26 | #define MOTOR_RIGHT_ENCB 15 27 | 28 | #define MOTOR_LEFT_EN 5 29 | #define MOTOR_LEFT_DIRA 7 30 | #define MOTOR_LEFT_DIRB 6 31 | #define MOTOR_LEFT_ENCA 16 32 | #define MOTOR_LEFT_ENCB 17 33 | 34 | 35 | #define ENC_COUNTS_PER_REV (32 * 30 * 2) 36 | 37 | //// VARIABLES //// 38 | 39 | 40 | 41 | TFmini tfmini; 42 | 43 | 44 | char doOnce = 0; 45 | char doContinously = 0; 46 | 47 | unsigned short step_counter = 0, next_steps = 1; 48 | 49 | 50 | // PID values for the motor controllers 51 | double Kp = 0.55276367534483;//0.610694929511361;//0.641817786149385 ;//6.458906368104240;//5.061601496636267;//3.286079178973016; 52 | double Ki = 1.64455966045303;//1.34329498731559;//1.169731890184110 ;//21.544597297186854;//59.064657944882540;//70.241507066863450; 53 | double Kd = 0.0101674410396297;//0.0220997968974464; 54 | double Tf = 1/11.8209539589613;//1/5.57670843490099; 55 | 56 | 57 | typedef struct { 58 | double Kp = 0.0; 59 | double Ki = 0.0; 60 | double Kd = 0.0; 61 | double Tf = 0.0; 62 | 63 | double P = 0.0; 64 | double I = 0.0; 65 | double D = 0.0; 66 | 67 | double e_old = 0.0; 68 | } PID_t; 69 | 70 | // struct defining the motor properties 71 | typedef struct { 72 | Encoder* enc; 73 | int32_t current_encoder_counter = 0; // current encoder count 74 | int32_t last_encoder_counter = 0; // last encoder count, used for calculating rotational speed 75 | int32_t odometry_counter = 0; 76 | PID_t pid; // PID controller for velocity control 77 | double speed_reference = 0; // the reference value used for the PID-controller 78 | struct{ 79 | char PIN_EN, PIN_DIRA, PIN_DIRB; 80 | } pins; // struct for the motors' pins 81 | 82 | } motor_t; 83 | 84 | motor_t motor_left, motor_right; 85 | 86 | Encoder encLeft(MOTOR_LEFT_ENCA, MOTOR_LEFT_ENCB); 87 | Encoder encRight(MOTOR_RIGHT_ENCA, MOTOR_RIGHT_ENCB); 88 | 89 | 90 | double last_timer_us = 0, h = 0; 91 | 92 | void setup(){ 93 | 94 | // enable serial connection on USB port 95 | Serial.begin(115200); 96 | 97 | ///// SETUP PINS ///// 98 | // stepper motor 99 | pinMode(STEPPER_EN, OUTPUT); 100 | pinMode(STEPPER_DIR, OUTPUT); 101 | pinMode(STEPPER_STEP, OUTPUT); 102 | pinMode(STEPPER_SENSOR, INPUT); 103 | 104 | // disable stepper motor 105 | digitalWrite(STEPPER_EN, HIGH); 106 | 107 | // select rotational direction 108 | digitalWrite(STEPPER_DIR, HIGH); 109 | 110 | homeSensor(); 111 | 112 | //digitalWrite(STEPPER_EN, LOW); 113 | //doContinously = 1; 114 | //doOnce = 1; 115 | 116 | // dc motors 117 | 118 | ///// SETUP TFMini ///// 119 | Serial1.begin(TFmini::DEFAULT_BAUDRATE); 120 | tfmini.attach(Serial1); 121 | tfmini.setDetectionPattern(TFmini::DetectionPattern::Fixed); 122 | tfmini.setDistanceMode(TFmini::DistanceMode ::Meduim); 123 | 124 | 125 | ///////////// DC MOTORs /////////// 126 | pinMode(MOTOR_LEFT_EN, OUTPUT); 127 | digitalWrite(MOTOR_LEFT_EN, HIGH); 128 | pinMode(MOTOR_LEFT_DIRA, OUTPUT); 129 | pinMode(MOTOR_LEFT_DIRB, OUTPUT); 130 | 131 | pinMode(MOTOR_RIGHT_EN, OUTPUT); 132 | digitalWrite(MOTOR_RIGHT_EN, HIGH); 133 | pinMode(MOTOR_RIGHT_DIRA, OUTPUT); 134 | pinMode(MOTOR_RIGHT_DIRB, OUTPUT); 135 | 136 | // set up motors and their PID-regulators 137 | motor_left.pid.Kp = 0.55276367534483; 138 | motor_left.pid.Ki = 1.64455966045303; 139 | motor_left.pid.Kd = 0.0101674410396297; 140 | motor_left.pid.Tf = 1/11.8209539589613; 141 | motor_left.pins = {MOTOR_LEFT_EN, MOTOR_LEFT_DIRA, MOTOR_LEFT_DIRB}; 142 | motor_left.enc = &encLeft; 143 | 144 | motor_right.pid.Kp = 0.55276367534483; 145 | motor_right.pid.Ki = 1.64455966045303; 146 | motor_right.pid.Kd = 0.0101674410396297; 147 | motor_right.pid.Tf = 1/11.8209539589613; 148 | motor_right.pins = {MOTOR_RIGHT_EN, MOTOR_RIGHT_DIRA, MOTOR_RIGHT_DIRB}; 149 | motor_right.enc = &encRight; 150 | 151 | motor_right.speed_reference = 0.0f; 152 | motor_left.speed_reference = 0.0f; 153 | 154 | // initialize the time counter 155 | last_timer_us = micros(); 156 | 157 | /* 158 | digitalWrite(MOTOR_RIGHT_DIRA, HIGH); 159 | digitalWrite(MOTOR_RIGHT_DIRB, LOW); 160 | 161 | analogWrite(MOTOR_RIGHT_EN, HIGH); 162 | */ 163 | 164 | // while(true); 165 | } 166 | 167 | void loop(){ 168 | // get current time 169 | unsigned long timer_us = micros(); 170 | 171 | // calculate elapsed time in seconds 172 | h = (double)(timer_us - last_timer_us) / 1000000.0; 173 | 174 | // store current time for next iteration 175 | last_timer_us = timer_us; 176 | 177 | handle_motor(&motor_left, h); 178 | handle_motor(&motor_right, h); 179 | //Serial.println(encRight.read()); 180 | //actuate_motor(&motor_left, 3); 181 | //step_motor(10); 182 | /* 183 | if (tfmini.available()) { 184 | //Serial.print("distance : "); 185 | Serial.print(tfmini.getDistance()); 186 | Serial.print(","); 187 | Serial.println(tfmini.getStrength()); 188 | //Serial.print("int time : "); 189 | //Serial.println(tfmini.getIntegrationTime()); 190 | } 191 | */ 192 | 193 | // check if there are any incoming bytes on the serial port 194 | if(Serial.available() > 0){ 195 | // read one byte 196 | char input = Serial.read(); 197 | 198 | if(input == 0x01 || input == 'O'){ // "do once"-command? 199 | doOnce = 1; 200 | digitalWrite(STEPPER_EN, LOW); 201 | }else if (input == 0x02 || input == 'E'){ // "enable continous"-command? 202 | doContinously = 1; 203 | doOnce = 1; 204 | digitalWrite(STEPPER_EN, LOW); 205 | }else if (input == 0x04 || input == 'D'){ // "disable continous"-command? 206 | doContinously = 0; 207 | }else if (input == 0x05 || input == 'H'){ // "disable continous"-command? 208 | homeSensor(); 209 | }else if (input == 0x08){ // "set resolution"-command? 210 | char d; 211 | // wait for next byte 212 | while ((d = Serial.read()) == -1); 213 | 214 | next_steps = (short) (STEPS_PER_ROTATION * ((float)d / 360.0f)); 215 | 216 | }else if (input == 0x10){ // "set left motor speed" - command? 217 | // wait for 4 bytes 218 | while(Serial.available() < 4); 219 | 220 | // get the four bytes and convert them to a float 221 | char buffer[4]; 222 | buffer[3] = Serial.read(); 223 | buffer[2] = Serial.read(); 224 | buffer[1] = Serial.read(); 225 | buffer[0] = Serial.read(); 226 | 227 | float value = *((float*)&buffer); 228 | 229 | motor_left.speed_reference = (double) value; 230 | }else if (input == 0x11){ // "set right motor speed" - command? 231 | // wait for 4 bytes 232 | while(Serial.available() < 4); 233 | 234 | // get the four bytes and convert them to a float 235 | char buffer[4]; 236 | buffer[3] = Serial.read(); 237 | buffer[2] = Serial.read(); 238 | buffer[1] = Serial.read(); 239 | buffer[0] = Serial.read(); 240 | 241 | float value = *((float*)&buffer); 242 | motor_right.speed_reference = (double) value; 243 | } 244 | } 245 | 246 | // only do measurement if 247 | if(doOnce != 0){ 248 | 249 | // move the motor 250 | step_motor(next_steps); 251 | 252 | // increase the counter and "loop back" if we exceed the number of steps for one compleete sensor revolution 253 | step_counter += next_steps; 254 | 255 | // check if we have done a complete revolution 256 | if(step_counter > STEPS_PER_ROTATION){ 257 | step_counter -= STEPS_PER_ROTATION; 258 | 259 | // yes, send message (with odometry information) to indicate that 260 | sendData(-1, motor_left.odometry_counter, motor_right.odometry_counter); 261 | //sendData(-1, 0, 0); 262 | 263 | // reset odometry counters 264 | motor_left.odometry_counter = 0; 265 | motor_right.odometry_counter = 0; 266 | 267 | // this allows the code to either do repeated measurements (if doContinously = 1) or only do a measurement once (if doContinously = 0 and doOnce is set to 1 once) 268 | doOnce = doContinously; 269 | 270 | // if stopped, disable the motor 271 | if(doOnce == 0) 272 | digitalWrite(STEPPER_EN, HIGH); 273 | } 274 | 275 | 276 | // take reading 277 | while(!tfmini.available()); 278 | 279 | // take and send the actual measurements 280 | sendData(step_counter, tfmini.getDistance() * 10, 0);//tfmini.getStrength()); 281 | 282 | } else { 283 | delay(10); 284 | } 285 | } 286 | 287 | void sendData(short steps, short frontDistance, short backDistance){ 288 | Serial.write(0x55); // start byte 289 | Serial.write((steps >> 8) & 0xff); 290 | Serial.write((steps >> 0) & 0xff); 291 | Serial.write((frontDistance >> 8) & 0xff); 292 | Serial.write((frontDistance >> 0) & 0xff); 293 | Serial.write((backDistance >> 8) & 0xff); 294 | Serial.write((backDistance >> 0) & 0xff); 295 | //Serial.flush(); 296 | } 297 | 298 | 299 | 300 | void homeSensor(){ 301 | // enable stepper 302 | digitalWrite(STEPPER_EN, LOW); 303 | 304 | int smoothed = 0, smoothed16 = 0, newVal; 305 | 306 | // startup value 307 | smoothed = analogRead(STEPPER_SENSOR); 308 | smoothed16 = smoothed << 4; 309 | do{ 310 | // do some exponential filtering. See https://forum.arduino.cc/index.php?topic=445844.0 (last post) 311 | smoothed16 = smoothed16 - smoothed + analogRead(STEPPER_SENSOR); 312 | smoothed = smoothed16 >> 4; 313 | 314 | // step the motor 315 | step_motor(1); 316 | 317 | // get a new reading 318 | newVal = analogRead(STEPPER_SENSOR); 319 | 320 | //Serial.println(newVal); 321 | 322 | // continue while we see no "peak" in the sensor value 323 | }while(newVal - smoothed < 25); 324 | 325 | // disable stepper 326 | digitalWrite(STEPPER_EN, HIGH); 327 | 328 | // reset position step counter 329 | //step_counter = 0; 330 | } 331 | 332 | void step_motor(unsigned short steps){ 333 | for(unsigned short i = 0; i < steps; i++){ 334 | digitalWrite(STEPPER_STEP, HIGH); 335 | delayMicroseconds(800); 336 | digitalWrite(STEPPER_STEP, LOW); 337 | delayMicroseconds(800); 338 | } 339 | } 340 | 341 | 342 | void handle_motor(motor_t* motor, double h){ 343 | double speed = getMotorRotationSpeed(motor, h); 344 | 345 | // calculate error 346 | double e = motor->speed_reference - speed; 347 | 348 | // calculate control signal 349 | double u = calculate_pid(&motor->pid, e, h); 350 | 351 | // constrain control signal to within +-12 volts 352 | double saturated_u = constrain(u, -12.0, 12.0); 353 | 354 | //Serial.println(speed); 355 | 356 | // drive the motor with this signal 357 | actuate_motor(motor, saturated_u); 358 | 359 | } 360 | 361 | // does the PID calculation and returns the new control output 362 | double calculate_pid(PID_t* pid, double error, double h){ 363 | // proportional part 364 | pid->P = pid->Kp * error; 365 | 366 | // derivative part 367 | pid->D = pid->Tf / (pid->Tf + h) * pid->D + pid->Kd / (pid->Tf + h) * (error - pid->e_old); 368 | 369 | // calculate output 370 | double u = pid->P + pid->I + pid->D; 371 | 372 | // integral part 373 | pid->I += pid->Ki * h * error; 374 | 375 | // save error 376 | pid->e_old = error; 377 | 378 | // return control signal 379 | return u; 380 | } 381 | 382 | // motor function, input voltage in range -12 to 12 volts 383 | void actuate_motor(motor_t* motor, double u){ 384 | // cap u in the range -12 to 12 volts 385 | u = constrain(u, -12.0, 12.0); 386 | 387 | 388 | // theese small voltages will only make the motors whine anyway 389 | if( abs(u) < 0.6) 390 | u = 0; 391 | 392 | // convert voltage to pwm duty cycle 393 | u = 100.0 * (u / 12.0); 394 | 395 | // convert pwm duty cycle to raw value 396 | uint8_t PWM_VALUE = (uint8_t) abs(u * (double) 255 / 100.0 ); 397 | 398 | /* 399 | Serial.print(motor->pins.PIN_EN); 400 | Serial.print(":"); 401 | Serial.println(PWM_VALUE); 402 | */ 403 | 404 | analogWrite(motor->pins.PIN_EN, PWM_VALUE); 405 | if(u >= 0){ 406 | // forward 407 | digitalWrite(motor->pins.PIN_DIRA, HIGH); 408 | digitalWrite(motor->pins.PIN_DIRB, LOW); 409 | }else{ 410 | // backward 411 | digitalWrite(motor->pins.PIN_DIRA, LOW); 412 | digitalWrite(motor->pins.PIN_DIRB, HIGH); 413 | } 414 | } 415 | 416 | double getMotorRotationSpeed(motor_t* motor, double dt){ 417 | // read motor encoder 418 | motor->current_encoder_counter = motor->enc->read(); 419 | 420 | 421 | //Serial.println(motor->current_encoder_counter); 422 | 423 | // calculate difference in encoder counts since last time 424 | double position_delta = motor->current_encoder_counter - (double)motor->last_encoder_counter; 425 | 426 | // save current position 427 | motor->last_encoder_counter = motor->current_encoder_counter; 428 | 429 | // increase odometry counter 430 | motor->odometry_counter += position_delta; 431 | 432 | // calculate and return current speed in rad/s 433 | return (double) 2.0 * PI * position_delta / ENC_COUNTS_PER_REV / dt; 434 | 435 | } --------------------------------------------------------------------------------