├── .gitignore ├── LICENSE ├── README.md ├── calibration ├── README.md ├── platformio.ini └── src │ └── firmware.ino ├── config ├── config.h ├── custom │ ├── beebo_config.h │ ├── beebo_m_config.h │ ├── dev_config.h │ ├── myrobot_config.h │ └── square_config.h └── lino_base_config.h ├── docs ├── advanced_setup.png ├── bts7960_connection.png ├── generic_1_in_connection.png ├── generic_2_in_connection.png ├── imu_connection.png ├── mecanum_wheels_orientation.png └── minimal_setup.png ├── firmware ├── include │ └── README ├── lib │ ├── battery │ │ ├── battery.cpp │ │ └── battery.h │ ├── encoder │ │ ├── encoder.cpp │ │ ├── encoder.h │ │ ├── examples │ │ │ ├── Basic │ │ │ │ └── Basic.pde │ │ │ ├── NoInterrupts │ │ │ │ └── NoInterrupts.pde │ │ │ ├── SpeedTest │ │ │ │ └── SpeedTest.pde │ │ │ └── TwoKnobs │ │ │ │ └── TwoKnobs.pde │ │ ├── keywords.txt │ │ └── utility │ │ │ ├── direct_pin_read.h │ │ │ ├── interrupt_config.h │ │ │ └── interrupt_pins.h │ ├── imu │ │ ├── AK09918.cpp │ │ ├── AK09918.h │ │ ├── AK8963.cpp │ │ ├── AK8963.h │ │ ├── MPU6050.cpp │ │ ├── MPU6050.h │ │ ├── MPU9150.cpp │ │ ├── MPU9150.h │ │ ├── MPU9250.cpp │ │ ├── MPU9250.h │ │ ├── QMC5883L.cpp │ │ ├── QMC5883L.h │ │ ├── QMI8658.cpp │ │ ├── QMI8658.h │ │ ├── QMI8658reg.h │ │ ├── default_imu.h │ │ ├── default_mag.h │ │ ├── helper_3dmath.h │ │ ├── imu.h │ │ ├── imu_interface.h │ │ ├── mag.h │ │ └── mag_interface.h │ ├── kinematics │ │ ├── kinematics.cpp │ │ └── kinematics.h │ ├── lidar │ │ ├── lidar.cpp │ │ └── lidar.h │ ├── motor │ │ ├── default_motor.h │ │ ├── motor.h │ │ └── motor_interface.h │ ├── odometry │ │ ├── odometry.cpp │ │ └── odometry.h │ ├── pid │ │ ├── pid.cpp │ │ └── pid.h │ ├── range │ │ ├── range.cpp │ │ └── range.h │ ├── syslog │ │ ├── syslog.cpp │ │ └── syslog.h │ └── wifi │ │ ├── ota.cpp │ │ ├── ota.h │ │ ├── wifis.cpp │ │ └── wifis.h ├── platformio.ini └── src │ └── firmware.ino ├── test_motors ├── platformio.ini └── src │ └── firmware.ino └── test_sensors ├── platformio.ini └── src └── firmware.ino /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .swp -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /calibration/README.md: -------------------------------------------------------------------------------- 1 | ## Calibration 2 | 3 | **The calibration utility does not work on esp32. Please use test_motors and test_sensors.** 4 | 5 | Before proceeding, **ensure that your robot is elevated and the wheels aren't touching the ground**. 6 | 5.1 7 | ### 1. Motor Check 8 | Go to calibration folder and upload the firmware: 9 | 10 | cd linorobot2_hardware/calibration 11 | pio run --target upload -e 12 | 13 | Available Teensy boards: 14 | - teensy31 15 | - teensy35 16 | - teensy36 17 | - teensy40 18 | - teensy41 19 | 20 | Some Linux machines might encounter a problem related to libusb. If so, install libusb-dev: 21 | 22 | sudo apt install libusb-dev 23 | sudo apt remove brltty (conflict with some UART adaptor) 24 | 25 | Start spinning the motors by running: 26 | 27 | screen /dev/ttyACM0 28 | 29 | On the terminal type `spin` and press the enter key. 30 | 31 | The wheels will spin one by one for 10 seconds from Motor1 to Motor4. Check if each wheel's direction is spinning **forward** and take note of the motors that are spinning in the opposite direction. Set MOTORX_INV constant in [lino_base_config.h](https://github.com/linorobot/linorobot2_hardware/blob/master/config/lino_base_config.h#L71-L74) to `true` to invert the motor's direction. Reupload the calibration firmware once you're done. Press `Ctrl` + `a` + `d` to exit the screen terminal. 32 | 33 | cd linorobot2_hardware/calibration 34 | pio run --target upload -e 35 | 36 | ### 2. Encoder Check 37 | 38 | Open your terminal and run: 39 | 40 | screen /dev/ttyACM0 41 | 42 | Type `sample` and press the enter key. Verify if all the wheels are spinning **forward**. Redo the previous step if there are motors still spinning in the opposite direction. 43 | 44 | You'll see a summary of the total encoder readings and counts per revolution after the motors have been sampled. If you see any negative number in the MOTOR ENCODER READINGS section, invert the encoder pin by setting `MOTORX_ENCODER_INV` in [lino_base_config.h](https://github.com/linorobot/linorobot2_hardware/blob/master/config/lino_base_config.h#L65-L68) to `true`. Reupload the calibration firmware to check if the encoder pins have been reconfigured properly: 45 | 46 | cd linorobot2_hardware/calibration 47 | pio run --target upload -e 48 | screen /dev/ttyACM0 49 | 50 | Type `sample` and press the enter key. Verify if all encoder values are now **positive**. Redo this step if you missed out any. 51 | 52 | ### 3. Counts Per Revolution 53 | 54 | On the previous instruction where you check the encoder reads for each motor, you'll see that there's also COUNTS PER REVOLUTION values printed on the screen. If you have defined `MOTOR_OPERATING_VOLTAGE` and `MOTOR_POWER_MEASURED_VOLTAGE`, you can assign these values to `COUNTS_PER_REVX` constants in [lino_base_config.h](https://github.com/linorobot/linorobot2_hardware/blob/master/config/lino_base_config.h#L55-L58) to have a more accurate model of the encoder. 55 | -------------------------------------------------------------------------------- /calibration/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | ; calibration simply runs the motors so no need for ros_distro or wifi transport [serial will be used by default] 12 | ; in addition, lib_extra_dirs is added for compiling calibration routine 13 | [platformio] 14 | extra_configs = 15 | ../firmware/platformio.ini 16 | [env] 17 | lib_extra_dirs = 18 | ../firmware/lib 19 | [env:myrobot] 20 | platform = espressif32 21 | board = esp32dev 22 | framework = arduino 23 | ;board_build.mcu = esp32 24 | board_build.f_flash = 80000000L 25 | board_build.flash_mode = qio 26 | upload_port = /dev/ttyUSB0 27 | upload_protocol = esptool 28 | ;board_microros_distro = humble 29 | ;board_microros_transport = wifi 30 | monitor_speed = 115200 31 | monitor_port = /dev/ttyUSB0 32 | monitor_dtr = 0 33 | monitor_rts = 0 34 | lib_deps = 35 | ${env.lib_deps} 36 | madhephaestus/ESP32Servo@^0.13.0 37 | madhephaestus/ESP32Encoder @ ^0.10.1 38 | build_flags = 39 | -I ../config 40 | -D USE_MYROBOT_CONFIG 41 | ;-D USE_WIFI_TRANSPORT 42 | -------------------------------------------------------------------------------- /calibration/src/firmware.ino: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "config.h" 27 | #include "motor.h" 28 | #include "pid.h" 29 | 30 | #define ENCODER_USE_INTERRUPTS 31 | #define ENCODER_OPTIMIZE_INTERRUPTS 32 | 33 | #include "encoder.h" 34 | #include "kinematics.h" 35 | 36 | #ifndef BAUDRATE 37 | #define BAUDRATE 115200 38 | #endif 39 | #define SAMPLE_TIME 20 // seconds 40 | 41 | Encoder motor1_encoder(MOTOR1_ENCODER_A, MOTOR1_ENCODER_B, COUNTS_PER_REV1, MOTOR1_ENCODER_INV); 42 | Encoder motor2_encoder(MOTOR2_ENCODER_A, MOTOR2_ENCODER_B, COUNTS_PER_REV2, MOTOR2_ENCODER_INV); 43 | Encoder motor3_encoder(MOTOR3_ENCODER_A, MOTOR3_ENCODER_B, COUNTS_PER_REV3, MOTOR3_ENCODER_INV); 44 | Encoder motor4_encoder(MOTOR4_ENCODER_A, MOTOR4_ENCODER_B, COUNTS_PER_REV4, MOTOR4_ENCODER_INV); 45 | 46 | Motor motor1_controller(PWM_FREQUENCY, PWM_BITS, MOTOR1_INV, MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); 47 | Motor motor2_controller(PWM_FREQUENCY, PWM_BITS, MOTOR2_INV, MOTOR2_PWM, MOTOR2_IN_A, MOTOR2_IN_B); 48 | Motor motor3_controller(PWM_FREQUENCY, PWM_BITS, MOTOR3_INV, MOTOR3_PWM, MOTOR3_IN_A, MOTOR3_IN_B); 49 | Motor motor4_controller(PWM_FREQUENCY, PWM_BITS, MOTOR4_INV, MOTOR4_PWM, MOTOR4_IN_A, MOTOR4_IN_B); 50 | 51 | PID motor1_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 52 | PID motor2_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 53 | PID motor3_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 54 | PID motor4_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 55 | 56 | Kinematics kinematics( 57 | Kinematics::LINO_BASE, 58 | MOTOR_MAX_RPM, 59 | MAX_RPM_RATIO, 60 | MOTOR_OPERATING_VOLTAGE, 61 | MOTOR_POWER_MAX_VOLTAGE, 62 | WHEEL_DIAMETER, 63 | LR_WHEELS_DISTANCE); 64 | 65 | long long int counts_per_rev[4]; 66 | int total_motors = 4; 67 | Motor *motors[4] = {&motor1_controller, &motor2_controller, &motor3_controller, &motor4_controller}; 68 | Encoder *encoders[4] = {&motor1_encoder, &motor2_encoder, &motor3_encoder, &motor4_encoder}; 69 | PID *pids[4] = {&motor1_pid, &motor2_pid, &motor3_pid, &motor4_pid}; 70 | String labels[4] = {"FRONT LEFT - M1: ", "FRONT RIGHT - M2: ", "REAR LEFT - M3: ", "REAR RIGHT - M4: "}; 71 | 72 | void setup() 73 | { 74 | #ifdef BOARD_INIT 75 | BOARD_INIT; 76 | #endif 77 | 78 | Serial.begin(BAUDRATE); 79 | while (!Serial) 80 | { 81 | } 82 | Serial.println("Sampling process will spin the motors at its maximum RPM."); 83 | Serial.println("Please ensure that the robot is ELEVATED and there are NO OBSTRUCTIONS to the wheels."); 84 | Serial.println(""); 85 | Serial.println("Type 'spin' and press enter to spin the motors."); 86 | Serial.println("Type 'sample' and press enter to spin the motors with motor summary."); 87 | Serial.println("Type 'ticks' and press enter to measure ticks per revolution of the motors."); 88 | Serial.println("Type 'test' and press enter to spin the using cmd_vel and observe PID in action."); 89 | Serial.println("Press enter to clear command."); 90 | Serial.println(""); 91 | } 92 | 93 | void loop() 94 | { 95 | static String cmd = ""; 96 | 97 | while (Serial.available()) 98 | { 99 | char character = Serial.read(); 100 | cmd.concat(character); 101 | Serial.print(character); 102 | delay(1); 103 | if (character == '\r' and cmd.equals("spin\r")) 104 | { 105 | cmd = ""; 106 | Serial.println("\r\n"); 107 | sampleMotors(0); 108 | } 109 | else if (character == '\r' and cmd.equals("sample\r")) 110 | { 111 | cmd = ""; 112 | Serial.println("\r\n"); 113 | sampleMotors(1); 114 | } 115 | else if (character == '\r' and cmd.equals("ticks\r")) 116 | { 117 | cmd = ""; 118 | Serial.println("\r\n"); 119 | testMotorForTicksPerRevolution(); 120 | } 121 | else if (character == '\r' and cmd.equals("test\r")) 122 | { 123 | cmd = ""; 124 | Serial.println("\r\n"); 125 | testMotorsWithCmdVel(); 126 | } 127 | else if (character == '\r') 128 | { 129 | Serial.println(""); 130 | cmd = ""; 131 | } 132 | } 133 | } 134 | void testMotorForTicksPerRevolution() 135 | { 136 | if (Kinematics::LINO_BASE == Kinematics::DIFFERENTIAL_DRIVE) 137 | { 138 | total_motors = 2; 139 | } 140 | int PWM_FOR_TEST = 200; 141 | unsigned long start_time = micros(); 142 | unsigned long last_status = micros(); 143 | bool show_incremental_tick_count = false; 144 | 145 | 146 | Serial.println("Please ensure that the robot is ELEVATED and there are NO OBSTRUCTIONS to the wheels."); 147 | Serial.println("ticks test will run each motor at slow speed one motor at a time."); 148 | Serial.println("count the number of revolutions visually and make a note of final tick count for each motor along with number of revolutions observed."); 149 | Serial.println("Number of ticks for each motor can be calculated with following formula:"); 150 | Serial.println("ticks per rev = final tick count/number of revolutions counted"); 151 | Serial.println("Press enter to continue to tick count test."); 152 | Serial.println(""); 153 | char character = Serial.read(); 154 | 155 | for (int i = 0; i < total_motors; i++) 156 | { 157 | Serial.print(labels[i]); 158 | while (true) 159 | { 160 | if (micros() - start_time >= SAMPLE_TIME * 1000000) 161 | { 162 | //Serial.println("STOP motor"); 163 | motors[i]->brake(); 164 | start_time = micros(); 165 | 166 | break; 167 | } 168 | 169 | 170 | // the required rpm is capped at -/+ MAX_RPM to prevent the PID from having too much error 171 | // the PWM value sent to the motor driver is the calculated PID based on required RPM vs measured RPM 172 | float current_tick_count = encoders[i]->read(); 173 | // set show_incremental_tick_count = true at beginning of function if you want to print incremental values 174 | // otherwise it will only print final total tick count for each motor 175 | if(show_incremental_tick_count){ 176 | Serial.print("current_tick_count:: "); 177 | Serial.println(current_tick_count); 178 | }else{ 179 | Serial.print("."); 180 | 181 | } 182 | // run the motor with low pwm so it rotates slowly and revolutions can be counted visually 183 | // If the motor is rotating too fast change value of PWM_FOR_TEST at beginning of this function 184 | int pwm = PWM_FOR_TEST; 185 | motors[i]->spin(pwm); 186 | delay(1000); 187 | 188 | } 189 | Serial.println(""); 190 | Serial.print("final_tick_count for "); 191 | Serial.print(labels[i]); 192 | Serial.print(" = "); 193 | float final_tick_count = encoders[i]->read(); 194 | Serial.println(final_tick_count); 195 | Serial.println("============="); 196 | 197 | } 198 | 199 | } 200 | void testMotorsWithCmdVel() 201 | { 202 | if (Kinematics::LINO_BASE == Kinematics::DIFFERENTIAL_DRIVE) 203 | { 204 | total_motors = 2; 205 | } 206 | geometry_msgs__msg__Twist twist_msg; 207 | // set test twist message with x=0.5 fwd full speed 208 | twist_msg.linear.x = 0.5; 209 | twist_msg.linear.y = 0.0; 210 | twist_msg.angular.z = 0.0; 211 | unsigned long start_time = micros(); 212 | unsigned long last_status = micros(); 213 | for (int i = 0; i < total_motors; i++) 214 | { 215 | while (true) 216 | { 217 | if (micros() - start_time >= SAMPLE_TIME * 1000000) 218 | { 219 | Serial.println("STOP motor"); 220 | motors[i]->brake(); 221 | Serial.println("============="); 222 | start_time = micros(); 223 | 224 | break; 225 | } 226 | 227 | // do all speed calculations 228 | // get the required rpm for each motor based on required velocities, and base used 229 | Kinematics::rpm req_rpm = kinematics.getRPM( 230 | twist_msg.linear.x, 231 | twist_msg.linear.y, 232 | twist_msg.angular.z); 233 | float req_rpm_val = 0; 234 | switch (i) 235 | { 236 | case 0: 237 | req_rpm_val = req_rpm.motor1; 238 | break; 239 | case 1: 240 | req_rpm_val = req_rpm.motor2; 241 | break; 242 | case 2: 243 | req_rpm_val = req_rpm.motor3; 244 | break; 245 | case 3: 246 | req_rpm_val = req_rpm.motor4; 247 | break; 248 | default: 249 | break; 250 | } 251 | float current_rpm = encoders[i]->getRPM(); 252 | 253 | // the required rpm is capped at -/+ MAX_RPM to prevent the PID from having too much error 254 | // the PWM value sent to the motor driver is the calculated PID based on required RPM vs measured RPM 255 | Serial.print("req_rpm:: "); 256 | Serial.print(req_rpm_val); 257 | Serial.print(" current_rpm:: "); 258 | Serial.print(current_rpm); 259 | int pwm = pids[i]->compute(req_rpm_val, current_rpm); 260 | Serial.print(" pwm:: "); 261 | Serial.println(pwm); 262 | motors[i]->spin(pwm); 263 | delay(100); 264 | 265 | } 266 | } 267 | } 268 | void sampleMotors(bool show_summary) 269 | { 270 | if (Kinematics::LINO_BASE == Kinematics::DIFFERENTIAL_DRIVE) 271 | { 272 | total_motors = 2; 273 | } 274 | 275 | float measured_voltage = constrain(MOTOR_POWER_MEASURED_VOLTAGE, 0, MOTOR_OPERATING_VOLTAGE); 276 | float scaled_max_rpm = ((measured_voltage / MOTOR_OPERATING_VOLTAGE) * MOTOR_MAX_RPM); 277 | float total_rev = scaled_max_rpm * (SAMPLE_TIME / 60.0); 278 | 279 | for (int i = 0; i < total_motors; i++) 280 | { 281 | Serial.print("SPINNING "); 282 | Serial.print(labels[i]); 283 | 284 | unsigned long start_time = micros(); 285 | unsigned long last_status = micros(); 286 | 287 | encoders[i]->write(0); 288 | while (true) 289 | { 290 | if (micros() - start_time >= SAMPLE_TIME * 1000000) 291 | { 292 | // Serial.println("STOP"); 293 | 294 | motors[i]->spin(0); 295 | Serial.println(""); 296 | break; 297 | } 298 | 299 | if (micros() - last_status >= 1000000) 300 | { 301 | last_status = micros(); 302 | Serial.print("."); 303 | } 304 | delay(1); // Fix: without this small delay the motors don't spin 305 | motors[i]->spin(PWM_MAX); 306 | } 307 | // Serial.println("before encoder read"); 308 | counts_per_rev[i] = encoders[i]->read() / total_rev; 309 | } 310 | if (show_summary) 311 | printSummary(); 312 | } 313 | 314 | void printSummary() 315 | { 316 | Serial.println("\r\n================MOTOR ENCODER READINGS================"); 317 | Serial.print(labels[0]); 318 | Serial.print(encoders[0]->read()); 319 | Serial.print(" "); 320 | 321 | Serial.print(labels[1]); 322 | Serial.println(encoders[1]->read()); 323 | 324 | Serial.print(labels[2]); 325 | Serial.print(encoders[2]->read()); 326 | Serial.print(" "); 327 | 328 | Serial.print(labels[3]); 329 | Serial.println(encoders[3]->read()); 330 | Serial.println(""); 331 | 332 | Serial.println("================COUNTS PER REVOLUTION================="); 333 | Serial.print(labels[0]); 334 | Serial.print(counts_per_rev[0]); 335 | Serial.print(" "); 336 | 337 | Serial.print(labels[1]); 338 | Serial.println(counts_per_rev[1]); 339 | 340 | Serial.print(labels[2]); 341 | Serial.print(counts_per_rev[2]); 342 | Serial.print(" "); 343 | 344 | Serial.print(labels[3]); 345 | Serial.println(counts_per_rev[3]); 346 | Serial.println(""); 347 | 348 | Serial.println("====================MAX VELOCITIES===================="); 349 | float max_rpm = kinematics.getMaxRPM(); 350 | 351 | Kinematics::velocities max_linear = kinematics.getVelocities(max_rpm, max_rpm, max_rpm, max_rpm); 352 | Kinematics::velocities max_angular = kinematics.getVelocities(-max_rpm, max_rpm, -max_rpm, max_rpm); 353 | 354 | Serial.print("Linear Velocity: +- "); 355 | Serial.print(max_linear.linear_x); 356 | Serial.println(" m/s"); 357 | 358 | Serial.print("Angular Velocity: +- "); 359 | Serial.print(max_angular.angular_z); 360 | Serial.println(" rad/s"); 361 | } 362 | -------------------------------------------------------------------------------- /config/config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | #ifndef CONFIG_H 15 | #define CONFIG_H 16 | 17 | #ifdef USE_BEEBO_CONFIG 18 | #include "custom/beebo_config.h" 19 | #endif 20 | 21 | #ifdef USE_BEEBO_M_CONFIG 22 | #include "custom/beebo_m_config.h" 23 | #endif 24 | 25 | #ifdef USE_SQUARE_CONFIG 26 | #include "custom/square_config.h" 27 | #endif 28 | 29 | #ifdef USE_DEV_CONFIG 30 | #include "custom/dev_config.h" 31 | #endif 32 | 33 | #ifdef USE_MYROBOT_CONFIG 34 | #include "custom/myrobot_config.h" 35 | #endif 36 | 37 | // this should be the last one 38 | #ifndef LINO_BASE 39 | #include "lino_base_config.h" 40 | #endif 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /config/custom/beebo_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef BEEBO_CONFIG_H 16 | #define BEEBO_CONFIG_H 17 | 18 | #define LED_PIN 13 19 | 20 | #define LINO_BASE SKID_STEER 21 | 22 | #define USE_GENERIC_1_IN_MOTOR_DRIVER 23 | #define USE_GY85_IMU 24 | 25 | #define K_P 0.6 26 | #define K_I 0.8 27 | #define K_D 0.5 28 | 29 | #define MOTOR_MAX_RPM 140 30 | #define MAX_RPM_RATIO 0.85 31 | #define MOTOR_OPERATING_VOLTAGE 24 32 | #define MOTOR_POWER_MAX_VOLTAGE 14.6 33 | #define MOTOR_POWER_MEASURED_VOLTAGE 14.6 34 | #define COUNTS_PER_REV1 144384 35 | #define COUNTS_PER_REV2 140777 36 | #define COUNTS_PER_REV3 148326 37 | #define COUNTS_PER_REV4 144495 38 | #define WHEEL_DIAMETER 0.152 39 | #define LR_WHEELS_DISTANCE 0.271 40 | #define PWM_BITS 10 41 | #define PWM_FREQUENCY 20000 42 | 43 | /// ENCODER PINS 44 | #define MOTOR1_ENCODER_A 14 45 | #define MOTOR1_ENCODER_B 15 46 | #define MOTOR1_ENCODER_INV false 47 | 48 | #define MOTOR2_ENCODER_A 11 49 | #define MOTOR2_ENCODER_B 12 50 | #define MOTOR2_ENCODER_INV false 51 | 52 | #define MOTOR3_ENCODER_A 17 53 | #define MOTOR3_ENCODER_B 16 54 | #define MOTOR3_ENCODER_INV true 55 | 56 | #define MOTOR4_ENCODER_A 9 57 | #define MOTOR4_ENCODER_B 10 58 | #define MOTOR4_ENCODER_INV false 59 | 60 | // Motor Pins 61 | #define MOTOR1_PWM 21 62 | #define MOTOR1_IN_A 20 63 | #define MOTOR1_IN_B -1 64 | #define MOTOR1_INV false 65 | 66 | #define MOTOR2_PWM 5 67 | #define MOTOR2_IN_A 6 68 | #define MOTOR2_IN_B -1 69 | #define MOTOR2_INV true 70 | 71 | #define MOTOR3_PWM 22 72 | #define MOTOR3_IN_A 23 73 | #define MOTOR3_IN_B -1 74 | #define MOTOR3_INV false 75 | 76 | #define MOTOR4_PWM 4 77 | #define MOTOR4_IN_A 3 78 | #define MOTOR4_IN_B -1 79 | #define MOTOR4_INV false 80 | 81 | #define PWM_MAX pow(2, PWM_BITS) - 1 82 | #define PWM_MIN -PWM_MAX 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /config/custom/beebo_m_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef BEEBO_M_CONFIG_H 16 | #define BEEBO_M_CONFIG_H 17 | 18 | #define LED_PIN 13 19 | 20 | #define LINO_BASE MECANUM 21 | 22 | #define USE_GENERIC_1_IN_MOTOR_DRIVER 23 | #define USE_GY85_IMU 24 | 25 | #define K_P 0.6 26 | #define K_I 0.8 27 | #define K_D 0.5 28 | 29 | #define MOTOR_MAX_RPM 140 30 | #define MAX_RPM_RATIO 0.85 31 | #define MOTOR_OPERATING_VOLTAGE 24 32 | #define MOTOR_POWER_MAX_VOLTAGE 14.6 33 | #define MOTOR_POWER_MEASURED_VOLTAGE 14.6 34 | #define COUNTS_PER_REV1 144384 35 | #define COUNTS_PER_REV2 140777 36 | #define COUNTS_PER_REV3 148326 37 | #define COUNTS_PER_REV4 144495 38 | #define WHEEL_DIAMETER 0.1 39 | #define LR_WHEELS_DISTANCE 0.31 40 | #define PWM_BITS 10 41 | #define PWM_FREQUENCY 20000 42 | 43 | /// ENCODER PINS 44 | #define MOTOR1_ENCODER_A 14 45 | #define MOTOR1_ENCODER_B 15 46 | #define MOTOR1_ENCODER_INV false 47 | 48 | #define MOTOR2_ENCODER_A 11 49 | #define MOTOR2_ENCODER_B 12 50 | #define MOTOR2_ENCODER_INV false 51 | 52 | #define MOTOR3_ENCODER_A 17 53 | #define MOTOR3_ENCODER_B 16 54 | #define MOTOR3_ENCODER_INV true 55 | 56 | #define MOTOR4_ENCODER_A 9 57 | #define MOTOR4_ENCODER_B 10 58 | #define MOTOR4_ENCODER_INV false 59 | 60 | // Motor Pins 61 | #define MOTOR1_PWM 21 62 | #define MOTOR1_IN_A 20 63 | #define MOTOR1_IN_B -1 64 | #define MOTOR1_INV false 65 | 66 | #define MOTOR2_PWM 5 67 | #define MOTOR2_IN_A 6 68 | #define MOTOR2_IN_B -1 69 | #define MOTOR2_INV true 70 | 71 | #define MOTOR3_PWM 22 72 | #define MOTOR3_IN_A 23 73 | #define MOTOR3_IN_B -1 74 | #define MOTOR3_INV false 75 | 76 | #define MOTOR4_PWM 4 77 | #define MOTOR4_IN_A 3 78 | #define MOTOR4_IN_B -1 79 | #define MOTOR4_INV false 80 | 81 | #define PWM_MAX pow(2, PWM_BITS) - 1 82 | #define PWM_MIN -PWM_MAX 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /config/custom/dev_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef DEV_CONFIG_H 16 | #define DEV_CONFIG_H 17 | 18 | #define LED_PIN 13 19 | 20 | #define LINO_BASE SKID_STEER 21 | 22 | #define USE_GENERIC_1_IN_MOTOR_DRIVER 23 | #define USE_FAKE_IMU 24 | 25 | #define K_P 0.6 26 | #define K_I 0.8 27 | #define K_D 0.5 28 | 29 | #define MOTOR_MAX_RPM 140 30 | #define MAX_RPM_RATIO 0.85 31 | #define MOTOR_OPERATING_VOLTAGE 24 32 | #define MOTOR_POWER_MAX_VOLTAGE 12 33 | #define MOTOR_POWER_MEASURED_VOLTAGE 12 34 | #define COUNTS_PER_REV1 144000 35 | #define COUNTS_PER_REV2 144000 36 | #define COUNTS_PER_REV3 144000 37 | #define COUNTS_PER_REV4 144000 38 | #define WHEEL_DIAMETER 0.152 39 | #define LR_WHEELS_DISTANCE 0.271 40 | #define PWM_BITS 8 41 | #define PWM_FREQUENCY 20000 42 | 43 | /// ENCODER PINS 44 | #define MOTOR1_ENCODER_A 14 45 | #define MOTOR1_ENCODER_B 15 46 | #define MOTOR1_ENCODER_INV false 47 | 48 | #define MOTOR2_ENCODER_A 11 49 | #define MOTOR2_ENCODER_B 12 50 | #define MOTOR2_ENCODER_INV false 51 | 52 | #define MOTOR3_ENCODER_A 17 53 | #define MOTOR3_ENCODER_B 16 54 | #define MOTOR3_ENCODER_INV true 55 | 56 | #define MOTOR4_ENCODER_A 9 57 | #define MOTOR4_ENCODER_B 10 58 | #define MOTOR4_ENCODER_INV false 59 | 60 | // Motor Pins 61 | #define MOTOR1_PWM 21 62 | #define MOTOR1_IN_A 20 63 | #define MOTOR1_IN_B -1 64 | #define MOTOR1_INV false 65 | 66 | #define MOTOR2_PWM 5 67 | #define MOTOR2_IN_A 6 68 | #define MOTOR2_IN_B -1 69 | #define MOTOR2_INV true 70 | 71 | #define MOTOR3_PWM 22 72 | #define MOTOR3_IN_A 23 73 | #define MOTOR3_IN_B -1 74 | #define MOTOR3_INV false 75 | 76 | #define MOTOR4_PWM 4 77 | #define MOTOR4_IN_A 3 78 | #define MOTOR4_IN_B -1 79 | #define MOTOR4_INV false 80 | 81 | #define PWM_MAX pow(2, PWM_BITS) - 1 82 | #define PWM_MIN -PWM_MAX 83 | 84 | #endif -------------------------------------------------------------------------------- /config/custom/myrobot_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef MYROBOT_CONFIG_H 16 | #define MYROBOT_CONFIG_H 17 | 18 | #define LED_PIN 2 19 | #define ESP32 20 | #define SOC_PCNT_SUPPORTED 21 | #define BAUDRATE 115200 22 | #define LINO_BASE DIFFERENTIAL_DRIVE 23 | 24 | #define USE_GENERIC_2_IN_MOTOR_DRIVER 25 | 26 | #define USE_MPU6050_IMU 27 | #define SDA_PIN 21 // specify I2C pins for esp32 28 | #define SCL_PIN 22 29 | 30 | // #define USE_LIDAR_UDP 31 | // #define LIDAR_RXD 17 // you may use any available input pin 32 | // // #define LIDAR_PWM 15 // do not use, the PWM control loop is not implememted yet 33 | // #define LIDAR_SERIAL 1 // uart number, 1 or 2 34 | // #define LIDAR_BAUDRATE 230400 // the Lidar serial buadrate 35 | // #define LIDAR_SERVER { 192, 168, 1, 145 } // eg your desktop IP addres 36 | // #define LIDAR_PORT 8889 // the UDP port on server 37 | 38 | 39 | #define K_P 0.6 40 | #define K_I 0.8 41 | #define K_D 0.5 42 | 43 | #define MOTOR_MAX_RPM 160 44 | #define MAX_RPM_RATIO 0.8 45 | #define MOTOR_OPERATING_VOLTAGE 6 46 | #define MOTOR_POWER_MAX_VOLTAGE 6 47 | #define MOTOR_POWER_MEASURED_VOLTAGE 8.2 48 | #define COUNTS_PER_REV1 975 //960 49 | #define COUNTS_PER_REV2 975 //960 50 | #define COUNTS_PER_REV3 240 51 | #define COUNTS_PER_REV4 240 52 | #define WHEEL_DIAMETER 0.0675 53 | #define LR_WHEELS_DISTANCE 0.126 54 | #define PWM_BITS 8 55 | #define PWM_FREQUENCY 8000 56 | 57 | // Fixed pin numbers for ESP32-WROOM-32D 38 PIN VERSION 58 | /// ENCODER PINS 59 | #define MOTOR1_ENCODER_A 5 60 | #define MOTOR1_ENCODER_B 17 61 | #define MOTOR1_ENCODER_INV false 62 | 63 | #define MOTOR2_ENCODER_A 19 64 | #define MOTOR2_ENCODER_B 18 65 | #define MOTOR2_ENCODER_INV false 66 | 67 | #define MOTOR3_ENCODER_A 17 68 | #define MOTOR3_ENCODER_B 16 69 | #define MOTOR3_ENCODER_INV true 70 | 71 | #define MOTOR4_ENCODER_A 1 72 | #define MOTOR4_ENCODER_B 3 73 | #define MOTOR4_ENCODER_INV false 74 | 75 | // Motor Pins 76 | #define MOTOR1_PWM 12 77 | #define MOTOR1_IN_A 14 78 | #define MOTOR1_IN_B 27 79 | #define MOTOR1_INV true 80 | 81 | #define MOTOR2_PWM 26 82 | #define MOTOR2_IN_A 25 83 | #define MOTOR2_IN_B 33 84 | #define MOTOR2_INV true 85 | 86 | #define MOTOR3_PWM 32 87 | #define MOTOR3_IN_A 33 88 | #define MOTOR3_IN_B -1 89 | #define MOTOR3_INV false 90 | 91 | #define MOTOR4_PWM 34 92 | #define MOTOR4_IN_A 35 93 | #define MOTOR4_IN_B -1 94 | #define MOTOR4_INV false 95 | 96 | #define PWM_MAX pow(2, PWM_BITS) - 1 97 | #define PWM_MIN -(pow(2, PWM_BITS) - 1) 98 | #define USE_WIFI_TRANSPORT 99 | #define AGENT_IP { 192, 168, 1, 100 } // eg IP of the desktop computer 100 | #define AGENT_PORT 8888 101 | // Enable WiFi with null terminated list of multiple APs SSID and password 102 | #define WIFI_AP_LIST {{"WIFI_SSID", "WIFI_PASSWORD"}, {NULL}} 103 | #define WIFI_MONITOR 2 // min. period to send wifi signal strength to syslog 104 | // #define USE_ARDUINO_OTA 105 | #define USE_SYSLOG 106 | #define SYSLOG_SERVER { 192, 168, 1, 100 } // eg IP of the desktop computer 107 | #define SYSLOG_PORT 514 108 | #define DEVICE_HOSTNAME "linorobot2" 109 | #define APP_NAME "hardware" 110 | #endif -------------------------------------------------------------------------------- /config/custom/square_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef SQUARE_CONFIG_H 16 | #define SQUARE_CONFIG_H 17 | 18 | #define LED_PIN 13 19 | 20 | #define LINO_BASE DIFFERENTIAL_DRIVE 21 | 22 | #define USE_GENERIC_2_IN_MOTOR_DRIVER 23 | #define USE_GY85_IMU 24 | 25 | #define K_P 20. 26 | #define K_I 0.8 27 | #define K_D 5. 28 | 29 | #define MOTOR_MAX_RPM 100 30 | #define MAX_RPM_RATIO 0.8 31 | #define MOTOR_OPERATING_VOLTAGE 12 32 | #define MOTOR_POWER_MAX_VOLTAGE 12 33 | #define MOTOR_POWER_MEASURED_VOLTAGE 11.67 34 | #define COUNTS_PER_REV1 2513 35 | #define COUNTS_PER_REV2 2580 36 | #define COUNTS_PER_REV3 2421 37 | #define COUNTS_PER_REV4 2501 38 | #define WHEEL_DIAMETER 0.09 39 | #define LR_WHEELS_DISTANCE 0.24 40 | #define PWM_BITS 10 41 | #define PWM_FREQUENCY 20000 42 | 43 | /// ENCODER PINS 44 | #define MOTOR1_ENCODER_A 14 45 | #define MOTOR1_ENCODER_B 15 46 | #define MOTOR1_ENCODER_INV false 47 | 48 | #define MOTOR2_ENCODER_A 11 49 | #define MOTOR2_ENCODER_B 12 50 | #define MOTOR2_ENCODER_INV false 51 | 52 | #define MOTOR3_ENCODER_A 17 53 | #define MOTOR3_ENCODER_B 16 54 | #define MOTOR3_ENCODER_INV true 55 | 56 | #define MOTOR4_ENCODER_A 9 57 | #define MOTOR4_ENCODER_B 10 58 | #define MOTOR4_ENCODER_INV false 59 | 60 | // Motor Pins 61 | #define MOTOR1_PWM 1 62 | #define MOTOR1_IN_A 20 63 | #define MOTOR1_IN_B 21 64 | #define MOTOR1_INV false 65 | 66 | #define MOTOR2_PWM 5 67 | #define MOTOR2_IN_A 6 68 | #define MOTOR2_IN_B 8 69 | #define MOTOR2_INV true 70 | 71 | #define MOTOR3_PWM 22 72 | #define MOTOR3_IN_A 23 73 | #define MOTOR3_IN_B 0 74 | #define MOTOR3_INV false 75 | 76 | #define MOTOR4_PWM 4 77 | #define MOTOR4_IN_A 3 78 | #define MOTOR4_IN_B 2 79 | #define MOTOR4_INV false 80 | 81 | #define PWM_MAX pow(2, PWM_BITS) - 1 82 | #define PWM_MIN -PWM_MAX 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /config/lino_base_config.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef LINO_BASE_CONFIG_H 16 | #define LINO_BASE_CONFIG_H 17 | 18 | #define LED_PIN 13 //used for debugging status 19 | 20 | //uncomment the base you're building 21 | #define LINO_BASE DIFFERENTIAL_DRIVE // 2WD and Tracked robot w/ 2 motors 22 | // #define LINO_BASE SKID_STEER // 4WD robot 23 | // #define LINO_BASE MECANUM // Mecanum drive robot 24 | 25 | //uncomment the motor driver you're using 26 | #define USE_GENERIC_2_IN_MOTOR_DRIVER // Motor drivers with 2 Direction Pins(INA, INB) and 1 PWM(ENABLE) pin ie. L298, L293, VNH5019 27 | // #define USE_GENERIC_1_IN_MOTOR_DRIVER // Motor drivers with 1 Direction Pin(INA) and 1 PWM(ENABLE) pin. 28 | // #define USE_BTS7960_MOTOR_DRIVER // BTS7970 Motor Driver 29 | // #define USE_ESC_MOTOR_DRIVER // Motor ESC for brushless motors 30 | 31 | //uncomment the IMU you're using 32 | #define USE_GY85_IMU 33 | // #define USE_MPU6050_IMU 34 | // #define USE_MPU9150_IMU 35 | // #define USE_MPU9250_IMU 36 | // #define USE_QMI8658_IMU 37 | // #define USE_HMC5883L_MAG 38 | // #define USE_AK8963_MAG 39 | // #define USE_AK8975_MAG 40 | // #define USE_AK09918_MAG 41 | // #define USE_QMC5883L_MAG 42 | // #define MAG_BIAS { 0, 0, 0 } 43 | 44 | #define K_P 0.6 // P constant 45 | #define K_I 0.8 // I constant 46 | #define K_D 0.5 // D constant 47 | 48 | /* 49 | ROBOT ORIENTATION 50 | FRONT 51 | MOTOR1 MOTOR2 (2WD/ACKERMANN) 52 | MOTOR3 MOTOR4 (4WD/MECANUM) 53 | BACK 54 | */ 55 | 56 | //define your robot' specs here 57 | #define MOTOR_MAX_RPM 140 // motor's max RPM 58 | #define MAX_RPM_RATIO 0.85 // max RPM allowed for each MAX_RPM_ALLOWED = MOTOR_MAX_RPM * MAX_RPM_RATIO 59 | #define MOTOR_OPERATING_VOLTAGE 24 // motor's operating voltage (used to calculate max RPM) 60 | #define MOTOR_POWER_MAX_VOLTAGE 12 // max voltage of the motor's power source (used to calculate max RPM) 61 | #define MOTOR_POWER_MEASURED_VOLTAGE 12 // current voltage reading of the power connected to the motor (used for calibration) 62 | #define COUNTS_PER_REV1 144000 // wheel1 encoder's no of ticks per rev 63 | #define COUNTS_PER_REV2 144000 // wheel2 encoder's no of ticks per rev 64 | #define COUNTS_PER_REV3 144000 // wheel3 encoder's no of ticks per rev 65 | #define COUNTS_PER_REV4 144000 // wheel4 encoder's no of ticks per rev 66 | #define WHEEL_DIAMETER 0.152 // wheel's diameter in meters 67 | #define LR_WHEELS_DISTANCE 0.271 // distance between left and right wheels 68 | #define PWM_BITS 10 // PWM Resolution of the microcontroller 69 | #define PWM_FREQUENCY 20000 // PWM Frequency 70 | 71 | // INVERT ENCODER COUNTS 72 | #define MOTOR1_ENCODER_INV false 73 | #define MOTOR2_ENCODER_INV false 74 | #define MOTOR3_ENCODER_INV false 75 | #define MOTOR4_ENCODER_INV false 76 | 77 | // INVERT MOTOR DIRECTIONS 78 | #define MOTOR1_INV false 79 | #define MOTOR2_INV false 80 | #define MOTOR3_INV false 81 | #define MOTOR4_INV false 82 | 83 | // ENCODER PINS 84 | #define MOTOR1_ENCODER_A 14 85 | #define MOTOR1_ENCODER_B 15 86 | 87 | #define MOTOR2_ENCODER_A 11 88 | #define MOTOR2_ENCODER_B 12 89 | 90 | #define MOTOR3_ENCODER_A 17 91 | #define MOTOR3_ENCODER_B 16 92 | 93 | #define MOTOR4_ENCODER_A 9 94 | #define MOTOR4_ENCODER_B 10 95 | 96 | // MOTOR PINS 97 | #ifdef USE_GENERIC_2_IN_MOTOR_DRIVER 98 | #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can swap it with pin no 1 instead. 99 | #define MOTOR1_IN_A 20 100 | #define MOTOR1_IN_B 1 101 | 102 | #define MOTOR2_PWM 5 103 | #define MOTOR2_IN_A 6 104 | #define MOTOR2_IN_B 8 105 | 106 | #define MOTOR3_PWM 22 107 | #define MOTOR3_IN_A 23 108 | #define MOTOR3_IN_B 0 109 | 110 | #define MOTOR4_PWM 4 111 | #define MOTOR4_IN_A 3 112 | #define MOTOR4_IN_B 2 113 | 114 | #define PWM_MAX pow(2, PWM_BITS) - 1 115 | #define PWM_MIN -PWM_MAX 116 | #endif 117 | 118 | #ifdef USE_GENERIC_1_IN_MOTOR_DRIVER 119 | #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead. 120 | #define MOTOR1_IN_A 20 121 | #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 122 | 123 | #define MOTOR2_PWM 5 124 | #define MOTOR2_IN_A 6 125 | #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 126 | 127 | #define MOTOR3_PWM 22 128 | #define MOTOR3_IN_A 23 129 | #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 130 | 131 | #define MOTOR4_PWM 4 132 | #define MOTOR4_IN_A 3 133 | #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 134 | 135 | #define PWM_MAX pow(2, PWM_BITS) - 1 136 | #define PWM_MIN -PWM_MAX 137 | #endif 138 | 139 | #ifdef USE_BTS7960_MOTOR_DRIVER 140 | #define MOTOR1_PWM -1 //DON'T TOUCH THIS! This is just a placeholder 141 | #define MOTOR1_IN_A 21 // Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead. 142 | #define MOTOR1_IN_B 20 // Pin no 20 is not a PWM pin on Teensy 4.x, you can use pin no 0 instead. 143 | 144 | #define MOTOR2_PWM -1 //DON'T TOUCH THIS! This is just a placeholder 145 | #define MOTOR2_IN_A 5 146 | #define MOTOR2_IN_B 6 147 | 148 | #define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder 149 | #define MOTOR3_IN_A 22 150 | #define MOTOR3_IN_B 23 151 | 152 | #define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder 153 | #define MOTOR4_IN_A 4 154 | #define MOTOR4_IN_B 3 155 | 156 | #define PWM_MAX pow(2, PWM_BITS) - 1 157 | #define PWM_MIN -PWM_MAX 158 | #endif 159 | 160 | #ifdef USE_ESC_MOTOR_DRIVER 161 | #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x. You can use pin no 1 instead. 162 | #define MOTOR1_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder 163 | #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 164 | 165 | #define MOTOR2_PWM 5 166 | #define MOTOR2_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder 167 | #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 168 | 169 | #define MOTOR3_PWM 22 170 | #define MOTOR3_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder 171 | #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 172 | 173 | #define MOTOR4_PWM 4 174 | #define MOTOR4_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder 175 | #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder 176 | 177 | #define PWM_MAX 400 178 | #define PWM_MIN -PWM_MAX 179 | #endif 180 | 181 | // #define USE_WIFI_TRANSPORT // use micro ros wifi transport 182 | #define AGENT_IP { 192, 168, 1, 100 } // eg IP of the desktop computer 183 | #define AGENT_PORT 8888 184 | // Enable WiFi with null terminated list of multiple APs SSID and password 185 | // #define WIFI_AP_LIST {{"WIFI_SSID", "WIFI_PASSWORD"}, {NULL}} 186 | // #define WIFI_MONITOR 2 // min. period to send wifi signal strength to syslog 187 | // #define USE_ARDUINO_OTA 188 | // #define USE_SYSLOG 189 | #define SYSLOG_SERVER { 192, 168, 1, 100 } // eg IP of the desktop computer 190 | #define SYSLOG_PORT 514 191 | #define DEVICE_HOSTNAME "linorobot2" 192 | #define APP_NAME "hardware" 193 | // #define USE_LIDAR_UDP // send lidar data to udp server 194 | #define LIDAR_RXD 4 195 | // #define LIDAR_PWM 5 196 | #define LIDAR_SERIAL 1 // uart number 197 | #define LIDAR_BAUDRATE 230400 198 | #define LIDAR_SERVER { 192, 168, 1, 100 } // eg IP of the desktop computer 199 | #define LIDAR_PORT 8889 200 | #define BAUDRATE 115200 201 | // #define SDA_PIN 44 // specify I2C pins 202 | // #define SCL_PIN 45 203 | #define NODE_NAME "linorobot_base_node" 204 | // #define TOPIC_PREFIX "myrobot/" 205 | 206 | // battery voltage ADC pin 207 | // #define BATTERY_PIN 33 208 | // 3.3V ref, 12 bits ADC, 33k + 10k voltage divider 209 | #define BATTERY_ADJUST(v) ((v) * (3.3 / 4096 * (33 + 10) / 10)) 210 | // #define USE_INA219 211 | // #define TRIG_PIN 31 // ultrasonic sensor HC-SR04 212 | // #define ECHO_PIN 32 213 | // #define USE_SHORT_BRAKE // for shorter stopping distance 214 | // #define WDT_TIMEOUT 30 // Sec 215 | // #define BOARD_INIT sleep(5) // wait to begin IMU calibration 216 | 217 | #ifdef USE_SYSLOG 218 | #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){ \ 219 | syslog(LOG_ERR, "%s RCCHECK failed %d", __FUNCTION__, temp_rc); \ 220 | return false; }} 221 | #endif 222 | 223 | #endif 224 | -------------------------------------------------------------------------------- /docs/advanced_setup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/advanced_setup.png -------------------------------------------------------------------------------- /docs/bts7960_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/bts7960_connection.png -------------------------------------------------------------------------------- /docs/generic_1_in_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/generic_1_in_connection.png -------------------------------------------------------------------------------- /docs/generic_2_in_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/generic_2_in_connection.png -------------------------------------------------------------------------------- /docs/imu_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/imu_connection.png -------------------------------------------------------------------------------- /docs/mecanum_wheels_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/mecanum_wheels_orientation.png -------------------------------------------------------------------------------- /docs/minimal_setup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robofoundry/linorobot2_hardware_hippo_esp32_fix/8baaa4a16d6364faf74177c62ab69ad08aa99720/docs/minimal_setup.png -------------------------------------------------------------------------------- /firmware/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /firmware/lib/battery/battery.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "config.h" 4 | 5 | #ifdef USE_INA219 6 | #include 7 | 8 | #define INA219_ADDRESS 0x42 9 | INA219_WE ina219 = INA219_WE(INA219_ADDRESS); 10 | 11 | float shuntVoltage_mV = 0.0; 12 | float loadVoltage_V = 0.0; 13 | float busVoltage_V = 0.0; 14 | float current_mA = 0.0; 15 | float power_mW = 0.0; 16 | bool ina219_overflow = false; 17 | 18 | void initBattery(){ 19 | if(!ina219.init()){ 20 | Serial.println("INA219 not connected!"); 21 | } 22 | ina219.setADCMode(BIT_MODE_9); 23 | ina219.setPGain(PG_320); 24 | ina219.setBusRange(BRNG_16); 25 | ina219.setShuntSizeInOhms(0.01); // used in INA219. 26 | } 27 | 28 | void InaDataUpdate(){ 29 | shuntVoltage_mV = ina219.getShuntVoltage_mV(); 30 | busVoltage_V = ina219.getBusVoltage_V(); 31 | current_mA = ina219.getCurrent_mA(); 32 | power_mW = ina219.getBusPower(); 33 | loadVoltage_V = busVoltage_V + (shuntVoltage_mV/1000); 34 | ina219_overflow = ina219.getOverflow(); 35 | } 36 | #else 37 | void initBattery() {}; 38 | #endif 39 | 40 | #ifdef BATTERY_PIN 41 | double readVoltage(int pin) { 42 | int reading = 0; 43 | int i; 44 | for (i = 0; i < 4; i++) // smoothing 45 | reading += analogRead(pin); 46 | reading /= i; 47 | return BATTERY_ADJUST(reading); 48 | } 49 | #endif 50 | 51 | sensor_msgs__msg__BatteryState battery_msg_; 52 | sensor_msgs__msg__BatteryState getBattery() 53 | { 54 | #ifdef BATTERY_PIN 55 | battery_msg_.voltage = readVoltage(BATTERY_PIN); 56 | #endif 57 | #ifdef USE_INA219 58 | // read voltage 59 | InaDataUpdate(); 60 | battery_msg_.voltage = loadVoltage_V; 61 | battery_msg_.current = -current_mA / 1000; // Amp, minu when discharge 62 | #endif 63 | return battery_msg_; 64 | } 65 | -------------------------------------------------------------------------------- /firmware/lib/battery/battery.h: -------------------------------------------------------------------------------- 1 | #ifndef BATTERY_H 2 | #define BATTERY_H 3 | 4 | #include 5 | sensor_msgs__msg__BatteryState getBattery(); 6 | void initBattery(); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /firmware/lib/encoder/encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "encoder.h" 3 | 4 | #if !defined(ESP32) && !defined(PICO) 5 | // Yes, all the code is in the header file, to provide the user 6 | // configure options with #define (before they include it), and 7 | // to facilitate some crafty optimizations! 8 | 9 | Encoder_internal_state_t * Encoder::interruptArgs[]; 10 | #endif 11 | 12 | -------------------------------------------------------------------------------- /firmware/lib/encoder/examples/Basic/Basic.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - Basic Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these two numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder myEnc(5, 6); 14 | // avoid using pins with LEDs attached 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | Serial.println("Basic Encoder Test:"); 19 | } 20 | 21 | long oldPosition = -999; 22 | 23 | void loop() { 24 | long newPosition = myEnc.read(); 25 | if (newPosition != oldPosition) { 26 | oldPosition = newPosition; 27 | Serial.println(newPosition); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /firmware/lib/encoder/examples/NoInterrupts/NoInterrupts.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - NoInterrupts Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | // If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including 8 | // Encoder, the library will never use interrupts. This is mainly 9 | // useful to reduce the size of the library when you are using it 10 | // with pins that do not support interrupts. Without interrupts, 11 | // your program must call the read() function rapidly, or risk 12 | // missing changes in position. 13 | #define ENCODER_DO_NOT_USE_INTERRUPTS 14 | #include 15 | 16 | // Beware of Serial.print() speed. Without interrupts, if you 17 | // transmit too much data with Serial.print() it can slow your 18 | // reading from Encoder. Arduino 1.0 has improved transmit code. 19 | // Using the fastest baud rate also helps. Teensy has USB packet 20 | // buffering. But all boards can experience problems if you print 21 | // too much and fill up buffers. 22 | 23 | // Change these two numbers to the pins connected to your encoder. 24 | // With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever 25 | // used, even if the pin has interrupt capability 26 | Encoder myEnc(5, 6); 27 | // avoid using pins with LEDs attached 28 | 29 | void setup() { 30 | Serial.begin(9600); 31 | Serial.println("Basic NoInterrupts Test:"); 32 | } 33 | 34 | long position = -999; 35 | 36 | void loop() { 37 | long newPos = myEnc.read(); 38 | if (newPos != position) { 39 | position = newPos; 40 | Serial.println(position); 41 | } 42 | // With any substantial delay added, Encoder can only track 43 | // very slow motion. You may uncomment this line to see 44 | // how badly a delay affects your encoder. 45 | //delay(50); 46 | } 47 | -------------------------------------------------------------------------------- /firmware/lib/encoder/examples/SpeedTest/SpeedTest.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - SpeedTest - for measuring maximum Encoder speed 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | 8 | // This SpeedTest example provides a simple way to verify how much 9 | // CPU time Encoder is consuming. Connect a DC voltmeter to the 10 | // output pin and measure the voltage while the encoder is stopped 11 | // or running at a very slow speed. Even though the pin is rapidly 12 | // pulsing, a DC voltmeter will show the average voltage. Due to 13 | // software timing, it will read a number much less than a steady 14 | // logic high, but this number will give you a baseline reading 15 | // for output with minimal interrupt overhead. Then increase the 16 | // encoder speed. The voltage will decrease as the processor spends 17 | // more time in Encoder's interrupt routines counting the pulses 18 | // and less time pulsing the output pin. When the voltage is 19 | // close to zero and will not decrease any farther, you have reached 20 | // the absolute speed limit. Or, if using a mechanical system where 21 | // you reach a speed limit imposed by your motors or other hardware, 22 | // the amount this voltage has decreased, compared to the baseline, 23 | // should give you a good approximation of the portion of available 24 | // CPU time Encoder is consuming at your maximum speed. 25 | 26 | // Encoder requires low latency interrupt response. Available CPU 27 | // time does NOT necessarily prove or guarantee correct performance. 28 | // If another library, like NewSoftSerial, is disabling interrupts 29 | // for lengthy periods of time, Encoder can be prevented from 30 | // properly counting the intput signals while interrupt are disabled. 31 | 32 | 33 | // This optional setting causes Encoder to use more optimized code, 34 | // but the downside is a conflict if any other part of your sketch 35 | // or any other library you're using requires attachInterrupt(). 36 | // It must be defined before Encoder.h is included. 37 | //#define ENCODER_OPTIMIZE_INTERRUPTS 38 | 39 | #include 40 | #include "pins_arduino.h" 41 | 42 | // Change these two numbers to the pins connected to your encoder 43 | // or shift register circuit which emulates a quadrature encoder 44 | // case 1: both pins are interrupts 45 | // case 2: only first pin used as interrupt 46 | Encoder myEnc(5, 6); 47 | 48 | // Connect a DC voltmeter to this pin. 49 | const int outputPin = 12; 50 | 51 | /* This simple circuit, using a Dual Flip-Flop chip, can emulate 52 | quadrature encoder signals. The clock can come from a fancy 53 | function generator or a cheap 555 timer chip. The clock 54 | frequency can be measured with another board running FreqCount 55 | http://www.pjrc.com/teensy/td_libs_FreqCount.html 56 | 57 | +5V 58 | | Quadrature Encoder Signal Emulator 59 | Clock | 60 | Input o----*-------------------------- ---------------------------o Output1 61 | | |14 | | 62 | | _______|_______ | | _______________ 63 | | | CD4013 | | | | CD4013 | 64 | | 5 | | 1 | | 9 | | 13 65 | ---------| D Q |-----|----*----| D Q |------o Output2 66 | | | | | | | | 67 | | | 3 | | | 11 | | 68 | | ----|> Clk | ---------|> Clk | 69 | | | | | | 70 | | 6 | | 8 | | 71 | | ----| S | ----| S | 72 | | | | | | | | 73 | | | 4 | _ | 2 | 10 | _ | 12 74 | | *----| R Q |--- *----| R Q |---- 75 | | | | | | | | | 76 | | | |_______________| | |_______________| | 77 | | | | | | 78 | | | | 7 | | 79 | | | | | | 80 | -------------------------------------------------------------- 81 | | | | 82 | | | | 83 | ----- ----- ----- 84 | --- --- --- 85 | - - - 86 | */ 87 | 88 | 89 | void setup() { 90 | pinMode(outputPin, OUTPUT); 91 | } 92 | 93 | #if defined(__AVR__) 94 | #define REGTYPE unsigned char 95 | #elif defined(__PIC32MX__) 96 | #define REGTYPE unsigned long 97 | #endif 98 | 99 | void loop() { 100 | volatile int count = 0; 101 | volatile REGTYPE *reg = portOutputRegister(digitalPinToPort(outputPin)); 102 | REGTYPE mask = digitalPinToBitMask(outputPin); 103 | 104 | while (1) { 105 | myEnc.read(); // Read the encoder while interrupts are enabled. 106 | noInterrupts(); 107 | *reg |= mask; // Pulse the pin high, while interrupts are disabled. 108 | count = count + 1; 109 | *reg &= ~mask; 110 | interrupts(); 111 | } 112 | } 113 | 114 | -------------------------------------------------------------------------------- /firmware/lib/encoder/examples/TwoKnobs/TwoKnobs.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - TwoKnobs Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these pin numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder knobLeft(5, 6); 14 | Encoder knobRight(7, 8); 15 | // avoid using pins with LEDs attached 16 | 17 | void setup() { 18 | Serial.begin(9600); 19 | Serial.println("TwoKnobs Encoder Test:"); 20 | } 21 | 22 | long positionLeft = -999; 23 | long positionRight = -999; 24 | 25 | void loop() { 26 | long newLeft, newRight; 27 | newLeft = knobLeft.read(); 28 | newRight = knobRight.read(); 29 | if (newLeft != positionLeft || newRight != positionRight) { 30 | Serial.print("Left = "); 31 | Serial.print(newLeft); 32 | Serial.print(", Right = "); 33 | Serial.print(newRight); 34 | Serial.println(); 35 | positionLeft = newLeft; 36 | positionRight = newRight; 37 | } 38 | // if a character is sent from the serial monitor, 39 | // reset both back to zero. 40 | if (Serial.available()) { 41 | Serial.read(); 42 | Serial.println("Reset both knobs to zero"); 43 | knobLeft.write(0); 44 | knobRight.write(0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /firmware/lib/encoder/keywords.txt: -------------------------------------------------------------------------------- 1 | ENCODER_USE_INTERRUPTS LITERAL1 2 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 3 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 4 | Encoder KEYWORD1 5 | -------------------------------------------------------------------------------- /firmware/lib/encoder/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(TEENSYDUINO) && (defined(KINETISK) || defined(KINETISL)) 12 | 13 | #define IO_REG_TYPE uint8_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__IMXRT1052__) || defined(__IMXRT1062__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portOutputRegister(pin)) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 24 | 25 | #elif defined(__SAM3X8E__) // || defined(ESP8266) 26 | 27 | #define IO_REG_TYPE uint32_t 28 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 29 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 30 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 31 | 32 | #elif defined(__PIC32MX__) 33 | 34 | #define IO_REG_TYPE uint32_t 35 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 36 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 37 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 38 | 39 | /* ESP8266 v2.0.0 Arduino workaround for bug https://github.com/esp8266/Arduino/issues/1110 */ 40 | #elif defined(ESP8266) 41 | 42 | #define IO_REG_TYPE uint32_t 43 | #define PIN_TO_BASEREG(pin) ((volatile uint32_t *)(0x60000000+(0x318))) 44 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 45 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 46 | 47 | /* ESP32 Arduino (https://github.com/espressif/arduino-esp32) */ 48 | #elif defined(ESP32) 49 | 50 | #define IO_REG_TYPE uint32_t 51 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 52 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 53 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 54 | 55 | #elif defined(__SAMD21G18A__) || defined(__SAMD21E18A__) 56 | 57 | #define IO_REG_TYPE uint32_t 58 | #define PIN_TO_BASEREG(pin) portModeRegister(digitalPinToPort(pin)) 59 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 60 | #define DIRECT_PIN_READ(base, mask) (((*((base)+8)) & (mask)) ? 1 : 0) 61 | 62 | #elif defined(__SAMD51__) 63 | 64 | #define IO_REG_TYPE uint32_t 65 | #define PIN_TO_BASEREG(pin) portInputRegister(digitalPinToPort(pin)) 66 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 67 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 68 | 69 | #elif defined(RBL_NRF51822) 70 | 71 | #define IO_REG_TYPE uint32_t 72 | #define PIN_TO_BASEREG(pin) (0) 73 | #define PIN_TO_BITMASK(pin) (pin) 74 | #define DIRECT_PIN_READ(base, pin) nrf_gpio_pin_read(pin) 75 | 76 | #elif defined(ARDUINO_ARCH_NRF52840) 77 | #define IO_REG_TYPE uint32_t 78 | #define PIN_TO_BASEREG(pin) (0) 79 | #define PIN_TO_BITMASK(pin) digitalPinToPinName(pin) 80 | #define DIRECT_PIN_READ(base, pin) nrf_gpio_pin_read(pin) 81 | 82 | #elif defined(__arc__) /* Arduino101/Genuino101 specifics */ 83 | 84 | #include "scss_registers.h" 85 | #include "portable.h" 86 | #include "avr/pgmspace.h" 87 | #define GPIO_ID(pin) (g_APinDescription[pin].ulGPIOId) 88 | #define GPIO_TYPE(pin) (g_APinDescription[pin].ulGPIOType) 89 | #define GPIO_BASE(pin) (g_APinDescription[pin].ulGPIOBase) 90 | #define EXT_PORT_OFFSET_SS 0x0A 91 | #define EXT_PORT_OFFSET_SOC 0x50 92 | #define PIN_TO_BASEREG(pin) ((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase) 93 | #define PIN_TO_BITMASK(pin) pin 94 | #define IO_REG_TYPE uint32_t 95 | static inline __attribute__((always_inline)) 96 | IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin) 97 | { 98 | IO_REG_TYPE ret; 99 | if (SS_GPIO == GPIO_TYPE(pin)) { 100 | ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS)); 101 | } else { 102 | ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC); 103 | } 104 | return ((ret >> GPIO_ID(pin)) & 0x01); 105 | } 106 | #define DIRECT_PIN_READ(base, pin) directRead(base, pin) 107 | 108 | #endif 109 | 110 | #endif -------------------------------------------------------------------------------- /firmware/lib/encoder/utility/interrupt_config.h: -------------------------------------------------------------------------------- 1 | #if defined(__AVR__) 2 | 3 | #include 4 | #include 5 | 6 | #define attachInterrupt(num, func, mode) enableInterrupt(num) 7 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 8 | #define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num)) 9 | #define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num)) 10 | #else 11 | #define SCRAMBLE_INT_ORDER(num) (num) 12 | #define DESCRAMBLE_INT_ORDER(num) (num) 13 | #endif 14 | 15 | static void enableInterrupt(uint8_t num) 16 | { 17 | switch (DESCRAMBLE_INT_ORDER(num)) { 18 | #if defined(EICRA) && defined(EIMSK) 19 | case 0: 20 | EICRA = (EICRA & 0xFC) | 0x01; 21 | EIMSK |= 0x01; 22 | return; 23 | case 1: 24 | EICRA = (EICRA & 0xF3) | 0x04; 25 | EIMSK |= 0x02; 26 | return; 27 | case 2: 28 | EICRA = (EICRA & 0xCF) | 0x10; 29 | EIMSK |= 0x04; 30 | return; 31 | case 3: 32 | EICRA = (EICRA & 0x3F) | 0x40; 33 | EIMSK |= 0x08; 34 | return; 35 | #elif defined(MCUCR) && defined(GICR) 36 | case 0: 37 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 38 | GICR |= (1 << INT0); 39 | return; 40 | case 1: 41 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 42 | GICR |= (1 << INT1); 43 | return; 44 | #elif defined(MCUCR) && defined(GIMSK) 45 | case 0: 46 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 47 | GIMSK |= (1 << INT0); 48 | return; 49 | case 1: 50 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 51 | GIMSK |= (1 << INT1); 52 | return; 53 | #endif 54 | #if defined(EICRB) && defined(EIMSK) 55 | case 4: 56 | EICRB = (EICRB & 0xFC) | 0x01; 57 | EIMSK |= 0x10; 58 | return; 59 | case 5: 60 | EICRB = (EICRB & 0xF3) | 0x04; 61 | EIMSK |= 0x20; 62 | return; 63 | case 6: 64 | EICRB = (EICRB & 0xCF) | 0x10; 65 | EIMSK |= 0x40; 66 | return; 67 | case 7: 68 | EICRB = (EICRB & 0x3F) | 0x40; 69 | EIMSK |= 0x80; 70 | return; 71 | #endif 72 | } 73 | } 74 | 75 | #elif defined(__PIC32MX__) 76 | 77 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 78 | #undef ENCODER_OPTIMIZE_INTERRUPTS 79 | #endif 80 | 81 | #else 82 | 83 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 84 | #undef ENCODER_OPTIMIZE_INTERRUPTS 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /firmware/lib/encoder/utility/interrupt_pins.h: -------------------------------------------------------------------------------- 1 | // interrupt pins for known boards 2 | 3 | // Teensy (and maybe others) define these automatically 4 | #if !defined(CORE_NUM_INTERRUPT) 5 | 6 | // Wiring boards 7 | #if defined(WIRING) 8 | #define CORE_NUM_INTERRUPT NUM_EXTERNAL_INTERRUPTS 9 | #if NUM_EXTERNAL_INTERRUPTS > 0 10 | #define CORE_INT0_PIN EI0 11 | #endif 12 | #if NUM_EXTERNAL_INTERRUPTS > 1 13 | #define CORE_INT1_PIN EI1 14 | #endif 15 | #if NUM_EXTERNAL_INTERRUPTS > 2 16 | #define CORE_INT2_PIN EI2 17 | #endif 18 | #if NUM_EXTERNAL_INTERRUPTS > 3 19 | #define CORE_INT3_PIN EI3 20 | #endif 21 | #if NUM_EXTERNAL_INTERRUPTS > 4 22 | #define CORE_INT4_PIN EI4 23 | #endif 24 | #if NUM_EXTERNAL_INTERRUPTS > 5 25 | #define CORE_INT5_PIN EI5 26 | #endif 27 | #if NUM_EXTERNAL_INTERRUPTS > 6 28 | #define CORE_INT6_PIN EI6 29 | #endif 30 | #if NUM_EXTERNAL_INTERRUPTS > 7 31 | #define CORE_INT7_PIN EI7 32 | #endif 33 | 34 | // Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc... 35 | #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328PB__) ||defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) 36 | #define CORE_NUM_INTERRUPT 2 37 | #define CORE_INT0_PIN 2 38 | #define CORE_INT1_PIN 3 39 | 40 | // Arduino Mega 41 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 42 | #define CORE_NUM_INTERRUPT 6 43 | #define CORE_INT0_PIN 2 44 | #define CORE_INT1_PIN 3 45 | #define CORE_INT2_PIN 21 46 | #define CORE_INT3_PIN 20 47 | #define CORE_INT4_PIN 19 48 | #define CORE_INT5_PIN 18 49 | 50 | // Arduino Nano Every, Uno R2 Wifi 51 | #elif defined(__AVR_ATmega4809__) 52 | #define CORE_NUM_INTERRUPT 22 53 | #define CORE_INT0_PIN 0 54 | #define CORE_INT1_PIN 1 55 | #define CORE_INT2_PIN 2 56 | #define CORE_INT3_PIN 3 57 | #define CORE_INT4_PIN 4 58 | #define CORE_INT5_PIN 5 59 | #define CORE_INT6_PIN 6 60 | #define CORE_INT7_PIN 7 61 | #define CORE_INT8_PIN 8 62 | #define CORE_INT9_PIN 9 63 | #define CORE_INT10_PIN 10 64 | #define CORE_INT11_PIN 11 65 | #define CORE_INT12_PIN 12 66 | #define CORE_INT13_PIN 13 67 | #define CORE_INT14_PIN 14 68 | #define CORE_INT15_PIN 15 69 | #define CORE_INT16_PIN 16 70 | #define CORE_INT17_PIN 17 71 | #define CORE_INT18_PIN 18 72 | #define CORE_INT19_PIN 19 73 | #define CORE_INT20_PIN 20 74 | #define CORE_INT21_PIN 21 75 | 76 | // Arduino Leonardo (untested) 77 | #elif defined(__AVR_ATmega32U4__) && !defined(CORE_TEENSY) 78 | #define CORE_NUM_INTERRUPT 5 79 | #define CORE_INT0_PIN 3 80 | #define CORE_INT1_PIN 2 81 | #define CORE_INT2_PIN 0 82 | #define CORE_INT3_PIN 1 83 | #define CORE_INT4_PIN 7 84 | 85 | // Sanguino (untested) and ATmega1284P 86 | #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega1284P__) 87 | #define CORE_NUM_INTERRUPT 3 88 | #define CORE_INT0_PIN 10 89 | #define CORE_INT1_PIN 11 90 | #define CORE_INT2_PIN 2 91 | 92 | // ATmega32u2 and ATmega32u16 based boards with HoodLoader2 93 | #elif defined(__AVR_ATmega32U2__) || defined(__AVR_ATmega16U2__) 94 | #define CORE_NUM_INTERRUPT 8 95 | #define CORE_INT0_PIN 8 96 | #define CORE_INT1_PIN 17 97 | #define CORE_INT2_PIN 13 98 | #define CORE_INT3_PIN 14 99 | #define CORE_INT4_PIN 15 100 | #define CORE_INT5_PIN 16 101 | #define CORE_INT6_PIN 19 102 | #define CORE_INT7_PIN 20 103 | 104 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 105 | #elif defined(__PIC32MX__) && defined(_BOARD_UNO_) 106 | #define CORE_NUM_INTERRUPT 5 107 | #define CORE_INT0_PIN 38 108 | #define CORE_INT1_PIN 2 109 | #define CORE_INT2_PIN 7 110 | #define CORE_INT3_PIN 8 111 | #define CORE_INT4_PIN 35 112 | 113 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 114 | #elif defined(__PIC32MX__) && defined(_BOARD_MEGA_) 115 | #define CORE_NUM_INTERRUPT 5 116 | #define CORE_INT0_PIN 3 117 | #define CORE_INT1_PIN 2 118 | #define CORE_INT2_PIN 7 119 | #define CORE_INT3_PIN 21 120 | #define CORE_INT4_PIN 20 121 | 122 | // http://hlt.media.mit.edu/?p=1229 123 | #elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 124 | #define CORE_NUM_INTERRUPT 1 125 | #define CORE_INT0_PIN 2 126 | 127 | // ATtiny44 ATtiny84 128 | #elif defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) 129 | #define CORE_NUM_INTERRUPT 1 130 | #define CORE_INT0_PIN 8 131 | 132 | // ATtiny441 ATtiny841 133 | #elif defined(__AVR_ATtiny441__) || defined(__AVR_ATtiny841__) 134 | #define CORE_NUM_INTERRUPT 1 135 | #define CORE_INT0_PIN 9 136 | 137 | //https://github.com/SpenceKonde/ATTinyCore/blob/master/avr/extras/ATtiny_x313.md 138 | #elif defined(__AVR_ATtinyX313__) 139 | #define CORE_NUM_INTERRUPT 2 140 | #define CORE_INT0_PIN 4 141 | #define CORE_INT1_PIN 5 142 | 143 | // Attiny167 same core as abobe 144 | #elif defined(__AVR_ATtiny167__) 145 | #define CORE_NUM_INTERRUPT 2 146 | #define CORE_INT0_PIN 14 147 | #define CORE_INT1_PIN 3 148 | 149 | 150 | // Arduino Due 151 | #elif defined(__SAM3X8E__) 152 | #define CORE_NUM_INTERRUPT 54 153 | #define CORE_INT0_PIN 0 154 | #define CORE_INT1_PIN 1 155 | #define CORE_INT2_PIN 2 156 | #define CORE_INT3_PIN 3 157 | #define CORE_INT4_PIN 4 158 | #define CORE_INT5_PIN 5 159 | #define CORE_INT6_PIN 6 160 | #define CORE_INT7_PIN 7 161 | #define CORE_INT8_PIN 8 162 | #define CORE_INT9_PIN 9 163 | #define CORE_INT10_PIN 10 164 | #define CORE_INT11_PIN 11 165 | #define CORE_INT12_PIN 12 166 | #define CORE_INT13_PIN 13 167 | #define CORE_INT14_PIN 14 168 | #define CORE_INT15_PIN 15 169 | #define CORE_INT16_PIN 16 170 | #define CORE_INT17_PIN 17 171 | #define CORE_INT18_PIN 18 172 | #define CORE_INT19_PIN 19 173 | #define CORE_INT20_PIN 20 174 | #define CORE_INT21_PIN 21 175 | #define CORE_INT22_PIN 22 176 | #define CORE_INT23_PIN 23 177 | #define CORE_INT24_PIN 24 178 | #define CORE_INT25_PIN 25 179 | #define CORE_INT26_PIN 26 180 | #define CORE_INT27_PIN 27 181 | #define CORE_INT28_PIN 28 182 | #define CORE_INT29_PIN 29 183 | #define CORE_INT30_PIN 30 184 | #define CORE_INT31_PIN 31 185 | #define CORE_INT32_PIN 32 186 | #define CORE_INT33_PIN 33 187 | #define CORE_INT34_PIN 34 188 | #define CORE_INT35_PIN 35 189 | #define CORE_INT36_PIN 36 190 | #define CORE_INT37_PIN 37 191 | #define CORE_INT38_PIN 38 192 | #define CORE_INT39_PIN 39 193 | #define CORE_INT40_PIN 40 194 | #define CORE_INT41_PIN 41 195 | #define CORE_INT42_PIN 42 196 | #define CORE_INT43_PIN 43 197 | #define CORE_INT44_PIN 44 198 | #define CORE_INT45_PIN 45 199 | #define CORE_INT46_PIN 46 200 | #define CORE_INT47_PIN 47 201 | #define CORE_INT48_PIN 48 202 | #define CORE_INT49_PIN 49 203 | #define CORE_INT50_PIN 50 204 | #define CORE_INT51_PIN 51 205 | #define CORE_INT52_PIN 52 206 | #define CORE_INT53_PIN 53 207 | 208 | // ESP8266 (https://github.com/esp8266/Arduino/) 209 | #elif defined(ESP8266) 210 | #define CORE_NUM_INTERRUPT EXTERNAL_NUM_INTERRUPTS 211 | #define CORE_INT0_PIN 0 212 | #define CORE_INT1_PIN 1 213 | #define CORE_INT2_PIN 2 214 | #define CORE_INT3_PIN 3 215 | #define CORE_INT4_PIN 4 216 | #define CORE_INT5_PIN 5 217 | // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 218 | // most esp8266 modules, so we should avoid adding interrupts to these pins. 219 | #define CORE_INT12_PIN 12 220 | #define CORE_INT13_PIN 13 221 | #define CORE_INT14_PIN 14 222 | #define CORE_INT15_PIN 15 223 | 224 | // ESP32 (https://github.com/espressif/arduino-esp32) 225 | #elif defined(ESP32) 226 | 227 | #define CORE_NUM_INTERRUPT 40 228 | #define CORE_INT0_PIN 0 229 | #define CORE_INT1_PIN 1 230 | #define CORE_INT2_PIN 2 231 | #define CORE_INT3_PIN 3 232 | #define CORE_INT4_PIN 4 233 | #define CORE_INT5_PIN 5 234 | // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 235 | // esp32, so we should avoid adding interrupts to these pins. 236 | #define CORE_INT12_PIN 12 237 | #define CORE_INT13_PIN 13 238 | #define CORE_INT14_PIN 14 239 | #define CORE_INT15_PIN 15 240 | #define CORE_INT16_PIN 16 241 | #define CORE_INT17_PIN 17 242 | #define CORE_INT18_PIN 18 243 | #define CORE_INT19_PIN 19 244 | #define CORE_INT21_PIN 21 245 | #define CORE_INT22_PIN 22 246 | #define CORE_INT23_PIN 23 247 | #define CORE_INT25_PIN 25 248 | #define CORE_INT26_PIN 26 249 | #define CORE_INT27_PIN 27 250 | #define CORE_INT32_PIN 32 251 | #define CORE_INT33_PIN 33 252 | #define CORE_INT34_PIN 34 253 | #define CORE_INT35_PIN 35 254 | #define CORE_INT36_PIN 36 255 | #define CORE_INT39_PIN 39 256 | 257 | 258 | // Arduino Zero - TODO: interrupts do not seem to work 259 | // please help, contribute a fix! 260 | #elif defined(__SAMD21G18A__) || defined(__SAMD21E18A__) 261 | #define CORE_NUM_INTERRUPT 31 262 | #define CORE_INT0_PIN 0 263 | #define CORE_INT1_PIN 1 264 | #define CORE_INT2_PIN 2 265 | #define CORE_INT3_PIN 3 266 | #define CORE_INT4_PIN 4 267 | #define CORE_INT5_PIN 5 268 | #define CORE_INT6_PIN 6 269 | #define CORE_INT7_PIN 7 270 | #define CORE_INT8_PIN 8 271 | #define CORE_INT9_PIN 9 272 | #define CORE_INT10_PIN 10 273 | #define CORE_INT11_PIN 11 274 | #define CORE_INT12_PIN 12 275 | #define CORE_INT13_PIN 13 276 | #define CORE_INT14_PIN 14 277 | #define CORE_INT15_PIN 15 278 | #define CORE_INT16_PIN 16 279 | #define CORE_INT17_PIN 17 280 | #define CORE_INT18_PIN 18 281 | #define CORE_INT19_PIN 19 282 | #define CORE_INT20_PIN 20 283 | #define CORE_INT21_PIN 21 284 | #define CORE_INT22_PIN 22 285 | #define CORE_INT23_PIN 23 286 | #define CORE_INT24_PIN 24 287 | #define CORE_INT25_PIN 25 288 | #define CORE_INT26_PIN 26 289 | #define CORE_INT27_PIN 27 290 | #define CORE_INT28_PIN 28 291 | #define CORE_INT29_PIN 29 292 | #define CORE_INT30_PIN 30 293 | 294 | #elif defined(__SAMD51__) 295 | #define CORE_NUM_INTERRUPT 26 296 | #define CORE_INT0_PIN 0 297 | #define CORE_INT1_PIN 1 298 | #define CORE_INT2_PIN 2 299 | #define CORE_INT3_PIN 3 300 | #define CORE_INT4_PIN 4 301 | #define CORE_INT5_PIN 5 302 | #define CORE_INT6_PIN 6 303 | #define CORE_INT7_PIN 7 304 | #define CORE_INT8_PIN 8 305 | #define CORE_INT9_PIN 9 306 | #define CORE_INT10_PIN 10 307 | #define CORE_INT11_PIN 11 308 | #define CORE_INT12_PIN 12 309 | #define CORE_INT13_PIN 13 310 | #define CORE_INT14_PIN 14 311 | #define CORE_INT15_PIN 15 312 | #define CORE_INT16_PIN 16 313 | #define CORE_INT17_PIN 17 314 | #define CORE_INT18_PIN 18 315 | #define CORE_INT19_PIN 19 316 | #define CORE_INT20_PIN 20 317 | #define CORE_INT21_PIN 21 318 | #define CORE_INT22_PIN 22 319 | #define CORE_INT23_PIN 23 320 | #define CORE_INT24_PIN 24 321 | #define CORE_INT25_PIN 25 322 | 323 | // Arduino 101 324 | #elif defined(__arc__) 325 | #define CORE_NUM_INTERRUPT 14 326 | #define CORE_INT2_PIN 2 327 | #define CORE_INT5_PIN 5 328 | #define CORE_INT7_PIN 7 329 | #define CORE_INT8_PIN 8 330 | #define CORE_INT10_PIN 10 331 | #define CORE_INT11_PIN 11 332 | #define CORE_INT12_PIN 12 333 | #define CORE_INT13_PIN 13 334 | 335 | // Arduino Nano 33 BLE 336 | #elif defined(ARDUINO_ARCH_NRF52840) 337 | #define CORE_NUM_INTERRUPT 22 338 | #define CORE_INT0_PIN 0 339 | #define CORE_INT1_PIN 1 340 | #define CORE_INT2_PIN 2 341 | #define CORE_INT3_PIN 3 342 | #define CORE_INT4_PIN 4 343 | #define CORE_INT5_PIN 5 344 | #define CORE_INT6_PIN 6 345 | #define CORE_INT7_PIN 7 346 | #define CORE_INT8_PIN 8 347 | #define CORE_INT9_PIN 9 348 | #define CORE_INT10_PIN 10 349 | #define CORE_INT11_PIN 11 350 | #define CORE_INT12_PIN 12 351 | #define CORE_INT13_PIN 13 352 | #define CORE_INT14_PIN A0 353 | #define CORE_INT15_PIN A1 354 | #define CORE_INT16_PIN A2 355 | #define CORE_INT17_PIN A3 356 | #define CORE_INT18_PIN A4 357 | #define CORE_INT19_PIN A5 358 | #define CORE_INT20_PIN A6 359 | #define CORE_INT21_PIN A7 360 | #endif 361 | #endif 362 | 363 | #if !defined(CORE_NUM_INTERRUPT) 364 | #error "Interrupts are unknown for this board, please add to this code" 365 | #endif 366 | #if CORE_NUM_INTERRUPT <= 0 367 | #error "Encoder requires interrupt pins, but this board does not have any :(" 368 | #error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge." 369 | #endif -------------------------------------------------------------------------------- /firmware/lib/imu/AK09918.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | AK09918.cpp 3 | A library for Grove - IMU 9DOF(ICM20600 + AK09918) 4 | 5 | Copyright (c) 2018 seeed technology inc. 6 | Website : www.seeed.cc 7 | Author : Jerry Yip 8 | Create Time: 2018-06 9 | Version : 0.1 10 | Change Log : 11 | 12 | The MIT License (MIT) 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | */ 32 | 33 | 34 | #include "AK09918.h" 35 | 36 | AK09918::AK09918() { 37 | _addr = AK09918_I2C_ADDR; 38 | } 39 | 40 | 41 | AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) { 42 | if (mode == AK09918_SELF_TEST) { 43 | mode = AK09918_POWER_DOWN; 44 | } 45 | _mode = mode; 46 | 47 | if (mode == AK09918_NORMAL) { 48 | return AK09918_ERR_OK; 49 | } else { 50 | return AK09918::switchMode(_mode); 51 | } 52 | } 53 | 54 | AK09918_err_type_t AK09918::isDataReady() { 55 | if (!I2Cdev::readByte(_addr, AK09918_ST1, _buffer)) { 56 | return AK09918_ERR_READ_FAILED; 57 | } else { 58 | if (_buffer[0] & AK09918_DRDY_BIT) { 59 | return AK09918_ERR_OK; 60 | } else { 61 | return AK09918_ERR_NOT_RDY; 62 | } 63 | } 64 | } 65 | 66 | AK09918_err_type_t AK09918::isDataSkip() { 67 | if (!I2Cdev::readByte(_addr, AK09918_ST1, _buffer)) { 68 | return AK09918_ERR_READ_FAILED; 69 | } else { 70 | if (_buffer[0] & AK09918_DOR_BIT) { 71 | return AK09918_ERR_DOR; 72 | } else { 73 | return AK09918_ERR_OK; 74 | } 75 | } 76 | } 77 | 78 | AK09918_err_type_t AK09918::getData(int32_t* axis_x, int32_t* axis_y, int32_t* axis_z) { 79 | AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z); 80 | (*axis_x) = (*axis_x) * 15 / 100; 81 | (*axis_y) = (*axis_y) * 15 / 100; 82 | (*axis_z) = (*axis_z) * 15 / 100; 83 | 84 | return err; 85 | } 86 | 87 | AK09918_err_type_t AK09918::getRawData(int32_t* axis_x, int32_t* axis_y, int32_t* axis_z) { 88 | if (_mode == AK09918_NORMAL) { 89 | AK09918::switchMode(AK09918_NORMAL); 90 | bool is_end = false; 91 | int count = 0; 92 | while (!is_end) { 93 | if (AK09918::_getRawMode() == 0x00) { 94 | is_end = true; 95 | } 96 | if (count >= 15) { 97 | return AK09918_ERR_TIMEOUT; 98 | } 99 | count ++; 100 | delay(1); 101 | } 102 | } 103 | 104 | 105 | if (!I2Cdev::readBytes(_addr, AK09918_HXL, 8, _buffer)) { 106 | return AK09918_ERR_READ_FAILED; 107 | } else { 108 | *axis_x = (int16_t)(_buffer[1] << 8 | _buffer[0]); 109 | *axis_y = (int16_t)(_buffer[3] << 8 | _buffer[2]); 110 | *axis_z = (int16_t)(_buffer[5] << 8 | _buffer[4]); 111 | if (_buffer[7] & AK09918_HOFL_BIT) { 112 | return AK09918_ERR_OVERFLOW; 113 | } 114 | return AK09918_ERR_OK; 115 | } 116 | } 117 | 118 | void AK09918::getHeading(int16_t* x, int16_t* y, int16_t* z) { 119 | I2Cdev::readBytes(_addr, AK09918_HXL, 8, _buffer); 120 | *x = (int16_t)(_buffer[1] << 8 | _buffer[0]); 121 | *y = (int16_t)(_buffer[3] << 8 | _buffer[2]); 122 | *z = (int16_t)(_buffer[5] << 8 | _buffer[4]); 123 | } 124 | 125 | AK09918_mode_type_t AK09918::getMode() { 126 | return _mode; 127 | } 128 | 129 | AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) { 130 | if (mode == AK09918_SELF_TEST) { 131 | return AK09918_ERR_WRITE_FAILED; 132 | } 133 | _mode = mode; 134 | if (!I2Cdev::writeByte(_addr, AK09918_CNTL2, mode)) { 135 | return AK09918_ERR_WRITE_FAILED; 136 | } 137 | return AK09918_ERR_OK; 138 | } 139 | 140 | // 1.Set Power-down mode. (MODE[4:0] bits = “00000”) 141 | // 2.Set Self-test mode. (MODE[4:0] bits = “10000”) 142 | // 3.Check Data Ready or not by polling DRDY bit of ST1 register. 143 | // 4.When Data Ready, proceed to the next step. Read measurement data. (HXL to HZH) 144 | AK09918_err_type_t AK09918::selfTest() { 145 | int32_t axis_x, axis_y, axis_z; 146 | bool is_end = false; 147 | AK09918_err_type_t err; 148 | if (!I2Cdev::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) { 149 | return AK09918_ERR_WRITE_FAILED; 150 | } 151 | delay(1); 152 | if (!I2Cdev::writeByte(_addr, AK09918_CNTL2, AK09918_SELF_TEST)) { 153 | return AK09918_ERR_WRITE_FAILED; 154 | } 155 | 156 | while (!is_end) { 157 | err = AK09918::isDataReady(); 158 | if (err == AK09918_ERR_OK) { 159 | is_end = true; 160 | } else if (err == AK09918_ERR_READ_FAILED) { 161 | return AK09918_ERR_READ_FAILED; 162 | } 163 | delay(1); 164 | } 165 | 166 | // read data and check 167 | if (!I2Cdev::readBytes(_addr, AK09918_HXL, 8, _buffer)) { 168 | return AK09918_ERR_READ_FAILED; 169 | } else { 170 | axis_x = (int32_t)((((int16_t)_buffer[1]) << 8) | _buffer[0]); 171 | axis_y = (int32_t)((((int16_t)_buffer[3]) << 8) | _buffer[2]); 172 | axis_z = (int32_t)((((int16_t)_buffer[5]) << 8) | _buffer[4]); 173 | 174 | // Serial.print("X:"); 175 | // Serial.print(axis_x); 176 | // Serial.print("\tY:"); 177 | // Serial.print(axis_y); 178 | // Serial.print("\tZ:"); 179 | // Serial.println(axis_z); 180 | 181 | if ((axis_x >= -200) && (axis_x <= 200) && (axis_y >= -200) && (axis_y <= 200) && \ 182 | (axis_z >= -1000) && (axis_z <= -150)) { 183 | return AK09918_ERR_OK; 184 | } else { 185 | return AK09918_ERR_SELFTEST_FAILED; 186 | } 187 | 188 | } 189 | } 190 | 191 | AK09918_err_type_t AK09918::reset() { 192 | if (!I2Cdev::writeByte(_addr, AK09918_CNTL3, AK09918_SRST_BIT)) { 193 | return AK09918_ERR_WRITE_FAILED; 194 | } 195 | return AK09918_ERR_OK; 196 | } 197 | 198 | String AK09918::strError(AK09918_err_type_t err) { 199 | String result; 200 | switch (err) { 201 | case AK09918_ERR_OK: 202 | result = "AK09918_ERR_OK: OK"; 203 | break; 204 | 205 | case AK09918_ERR_DOR: 206 | result = "AK09918_ERR_DOR: Data skipped"; 207 | break; 208 | 209 | case AK09918_ERR_NOT_RDY: 210 | result = "AK09918_ERR_NOT_RDY: Not ready"; 211 | break; 212 | 213 | case AK09918_ERR_TIMEOUT: 214 | result = "AK09918_ERR_TIMEOUT: Timeout"; 215 | break; 216 | 217 | case AK09918_ERR_SELFTEST_FAILED: 218 | result = "AK09918_ERR_SELFTEST_FAILED: Self test failed"; 219 | break; 220 | 221 | case AK09918_ERR_OVERFLOW: 222 | result = "AK09918_ERR_OVERFLOW: Sensor overflow"; 223 | break; 224 | 225 | case AK09918_ERR_WRITE_FAILED: 226 | result = "AK09918_ERR_WRITE_FAILED: Fail to write"; 227 | break; 228 | 229 | case AK09918_ERR_READ_FAILED: 230 | result = "AK09918_ERR_READ_FAILED: Fail to read"; 231 | break; 232 | 233 | default: 234 | result = "Unknown Error"; 235 | break; 236 | } 237 | return result; 238 | } 239 | 240 | uint16_t AK09918::getDeviceID() { 241 | I2Cdev::readBytes(_addr, AK09918_WIA1, 2, _buffer); 242 | return (((uint16_t)_buffer[0]) << 8) | _buffer[1]; 243 | } 244 | 245 | uint8_t AK09918::_getRawMode() { 246 | if (!I2Cdev::readByte(0x0c, AK09918_CNTL2, _buffer)) { 247 | return 0xFF; 248 | } else { 249 | return _buffer[0]; 250 | } 251 | } 252 | -------------------------------------------------------------------------------- /firmware/lib/imu/AK09918.h: -------------------------------------------------------------------------------- 1 | /* 2 | AK09918.h 3 | A library for Grove - IMU 9DOF(ICM20600 + AK09918) 4 | 5 | Copyright (c) 2018 seeed technology inc. 6 | Website : www.seeed.cc 7 | Author : Jerry Yip 8 | Create Time: 2018-06 9 | Version : 0.1 10 | Change Log : 11 | 12 | The MIT License (MIT) 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | */ 32 | 33 | 34 | 35 | 36 | #ifndef __IMU_9DOF_AK09918_H__ 37 | #define __IMU_9DOF_AK09918_H__ 38 | 39 | #include "I2Cdev.h" 40 | 41 | /*************************************************************** 42 | AK09918 I2C Register 43 | ***************************************************************/ 44 | #define AK09918_I2C_ADDR 0x0c // I2C address (Can't be changed) 45 | #define AK09918_WIA1 0x00 // Company ID 46 | #define AK09918_WIA2 0x01 // Device ID 47 | #define AK09918_RSV1 0x02 // Reserved 1 48 | #define AK09918_RSV2 0x03 // Reserved 2 49 | #define AK09918_ST1 0x10 // DataStatus 1 50 | #define AK09918_HXL 0x11 // X-axis data 51 | #define AK09918_HXH 0x12 52 | #define AK09918_HYL 0x13 // Y-axis data 53 | #define AK09918_HYH 0x14 54 | #define AK09918_HZL 0x15 // Z-axis data 55 | #define AK09918_HZH 0x16 56 | #define AK09918_TMPS 0x17 // Dummy 57 | #define AK09918_ST2 0x18 // Datastatus 2 58 | #define AK09918_CNTL1 0x30 // Dummy 59 | #define AK09918_CNTL2 0x31 // Control settings 60 | #define AK09918_CNTL3 0x32 // Control settings 61 | 62 | #define AK09918_SRST_BIT 0x01 // Soft Reset 63 | #define AK09918_HOFL_BIT 0x08 // Sensor Over Flow 64 | #define AK09918_DOR_BIT 0x02 // Data Over Run 65 | #define AK09918_DRDY_BIT 0x01 // Data Ready 66 | 67 | // #define AK09918_MEASURE_PERIOD 9 // Must not be changed 68 | // AK09918 has following seven operation modes: 69 | // (1) Power-down mode: AK09918 doesn't measure 70 | // (2) Single measurement mode: measure when you call any getData() function 71 | // (3) Continuous measurement mode 1: 10Hz, measure 10 times per second, 72 | // (4) Continuous measurement mode 2: 20Hz, measure 20 times per second, 73 | // (5) Continuous measurement mode 3: 50Hz, measure 50 times per second, 74 | // (6) Continuous measurement mode 4: 100Hz, measure 100 times per second, 75 | // (7) Self-test mode 76 | enum AK09918_mode_type_t { 77 | AK09918_POWER_DOWN = 0x00, 78 | AK09918_NORMAL = 0x01, 79 | AK09918_CONTINUOUS_10HZ = 0x02, 80 | AK09918_CONTINUOUS_20HZ = 0x04, 81 | AK09918_CONTINUOUS_50HZ = 0x06, 82 | AK09918_CONTINUOUS_100HZ = 0x08, 83 | AK09918_SELF_TEST = 0x10, // ignored by switchMode() and initialize(), call selfTest() to use this mode 84 | }; 85 | 86 | enum AK09918_err_type_t { 87 | AK09918_ERR_OK = 0, // ok 88 | AK09918_ERR_DOR = 1, // data skipped 89 | AK09918_ERR_NOT_RDY = 2, // not ready 90 | AK09918_ERR_TIMEOUT = 3, // read/write timeout 91 | AK09918_ERR_SELFTEST_FAILED = 4, // self test failed 92 | AK09918_ERR_OVERFLOW = 5, // sensor overflow, means |x|+|y|+|z| >= 4912uT 93 | AK09918_ERR_WRITE_FAILED = 6, // fail to write 94 | AK09918_ERR_READ_FAILED = 7, // fail to read 95 | 96 | }; 97 | 98 | class AK09918 { 99 | public: 100 | AK09918(); 101 | 102 | // default to AK09918_CONTINUOUS_100HZ mode 103 | AK09918_err_type_t initialize(AK09918_mode_type_t mode = AK09918_CONTINUOUS_100HZ); 104 | // At AK09918_CONTINUOUS_** mode, check if data is ready to read 105 | AK09918_err_type_t isDataReady(); 106 | // At AK09918_CONTINUOUS_** mode, check if data is skipped 107 | AK09918_err_type_t isDataSkip(); 108 | // Get magnet data in uT 109 | AK09918_err_type_t getData(int32_t* axis_x, int32_t* axis_y, int32_t* axis_z); 110 | // Get raw I2C magnet data 111 | AK09918_err_type_t getRawData(int32_t* axis_x, int32_t* axis_y, int32_t* axis_z); 112 | // getHeading in I2Cdevlib 113 | void getHeading(int16_t *x, int16_t *y, int16_t *z); 114 | 115 | // Return the working mode of AK09918 116 | AK09918_mode_type_t getMode(); 117 | // Switch the working mode of AK09918 118 | AK09918_err_type_t switchMode(AK09918_mode_type_t mode); 119 | // Start a self-test, if pass, return AK09918_ERR_OK 120 | AK09918_err_type_t selfTest(); 121 | // Reset AK09918 122 | AK09918_err_type_t reset(); 123 | // Get details of AK09918_err_type_t 124 | String strError(AK09918_err_type_t err); 125 | // Get device ID 126 | uint16_t getDeviceID(); 127 | 128 | 129 | 130 | private: 131 | uint8_t _getRawMode(); 132 | uint8_t _addr; 133 | AK09918_mode_type_t _mode; 134 | uint8_t _buffer[16]; 135 | 136 | }; 137 | 138 | 139 | #endif // __IMU_9DOF_AK09918_H__ 140 | -------------------------------------------------------------------------------- /firmware/lib/imu/AK8963.cpp: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - AK8963 I2C device class header file 2 | // Based on AKM AK8963 datasheet, 10/2013 3 | // 8/27/2011 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2016-01-02 - initial release based on AK8975 code 8 | // 9 | 10 | /* ============================================ 11 | I2Cdev device library code is placed under the MIT license 12 | Copyright (c) 2011 Jeff Rowberg 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | =============================================== 32 | */ 33 | 34 | #include "AK8963.h" 35 | 36 | /** Default constructor, uses default I2C address. 37 | * @see AK8963_DEFAULT_ADDRESS 38 | */ 39 | AK8963::AK8963() { 40 | devAddr = AK8963_DEFAULT_ADDRESS; 41 | } 42 | 43 | /** Specific address constructor. 44 | * @param address I2C address 45 | * @see AK8963_DEFAULT_ADDRESS 46 | * @see AK8963_ADDRESS_00 47 | */ 48 | AK8963::AK8963(uint8_t address) { 49 | devAddr = address; 50 | } 51 | 52 | /** Power on and prepare for general usage. 53 | * No specific pre-configuration is necessary for this device. 54 | */ 55 | void AK8963::initialize() { 56 | } 57 | 58 | /** Verify the I2C connection. 59 | * Make sure the device is connected and responds as expected. 60 | * @return True if connection is valid, false otherwise 61 | */ 62 | bool AK8963::testConnection() { 63 | if (I2Cdev::readByte(devAddr, AK8963_RA_WIA, buffer) == 1) { 64 | return (buffer[0] == 0x48); 65 | } 66 | return false; 67 | } 68 | 69 | // WIA register 70 | 71 | uint8_t AK8963::getDeviceID() { 72 | I2Cdev::readByte(devAddr, AK8963_RA_WIA, buffer); 73 | return buffer[0]; 74 | } 75 | 76 | // INFO register 77 | 78 | uint8_t AK8963::getInfo() { 79 | I2Cdev::readByte(devAddr, AK8963_RA_INFO, buffer); 80 | return buffer[0]; 81 | } 82 | 83 | // ST1 register 84 | 85 | bool AK8963::getDataReady() { 86 | I2Cdev::readBit(devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer); 87 | return buffer[0]; 88 | } 89 | 90 | bool AK8963::getDataOverrun() { 91 | I2Cdev::readBit(devAddr, AK8963_RA_ST1, AK8963_ST1_DOR_BIT, buffer); 92 | return buffer[0]; 93 | } 94 | 95 | // H* registers 96 | void AK8963::getHeading(int16_t *x, int16_t *y, int16_t *z) { 97 | I2Cdev::readBytes(devAddr, AK8963_RA_HXL, 6, buffer); 98 | *x = (((int16_t)buffer[1]) << 8) | buffer[0]; 99 | *y = (((int16_t)buffer[3]) << 8) | buffer[2]; 100 | *z = (((int16_t)buffer[5]) << 8) | buffer[4]; 101 | } 102 | int16_t AK8963::getHeadingX() { 103 | I2Cdev::readBytes(devAddr, AK8963_RA_HXL, 2, buffer); 104 | return (((int16_t)buffer[1]) << 8) | buffer[0]; 105 | } 106 | int16_t AK8963::getHeadingY() { 107 | I2Cdev::readBytes(devAddr, AK8963_RA_HYL, 2, buffer); 108 | return (((int16_t)buffer[1]) << 8) | buffer[0]; 109 | } 110 | int16_t AK8963::getHeadingZ() { 111 | I2Cdev::readBytes(devAddr, AK8963_RA_HZL, 2, buffer); 112 | return (((int16_t)buffer[1]) << 8) | buffer[0]; 113 | } 114 | 115 | // ST2 register 116 | bool AK8963::getOverflowStatus() { 117 | I2Cdev::readBit(devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer); 118 | return buffer[0]; 119 | } 120 | bool AK8963::getOutputBit() { 121 | I2Cdev::readBit(devAddr, AK8963_RA_ST2, AK8963_ST2_BITM_BIT, buffer); 122 | return buffer[0]; 123 | } 124 | 125 | // CNTL1 register 126 | uint8_t AK8963::getMode() { 127 | I2Cdev::readBits(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_MODE_BIT, AK8963_CNTL1_MODE_LENGTH, buffer); 128 | return buffer[0]; 129 | } 130 | void AK8963::setMode(uint8_t mode) { 131 | I2Cdev::writeBits(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_MODE_BIT, AK8963_CNTL1_MODE_LENGTH, mode); 132 | } 133 | uint8_t AK8963::getResolution() { 134 | I2Cdev::readBit(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_RES_BIT, buffer); 135 | return buffer[0]; 136 | } 137 | void AK8963::setResolution(uint8_t res) { 138 | I2Cdev::writeBit(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_RES_BIT, res); 139 | } 140 | 141 | // CNTL2 register 142 | void AK8963::reset() { 143 | I2Cdev::writeByte(devAddr, AK8963_RA_CNTL2, AK8963_CNTL2_RESET); 144 | } 145 | 146 | // ASTC register 147 | void AK8963::setSelfTest(bool enabled) { 148 | I2Cdev::writeBit(devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled); 149 | } 150 | 151 | // I2CDIS 152 | void AK8963::disableI2C() { 153 | I2Cdev::writeByte(devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_DISABLE); 154 | } 155 | 156 | // ASA* registers 157 | void AK8963::getAdjustment(int8_t *x, int8_t *y, int8_t *z) { 158 | I2Cdev::readBytes(devAddr, AK8963_RA_ASAX, 3, buffer); 159 | *x = buffer[0]; 160 | *y = buffer[1]; 161 | *z = buffer[2]; 162 | } 163 | void AK8963::setAdjustment(int8_t x, int8_t y, int8_t z) { 164 | buffer[0] = x; 165 | buffer[1] = y; 166 | buffer[2] = z; 167 | I2Cdev::writeBytes(devAddr, AK8963_RA_ASAX, 3, buffer); 168 | } 169 | uint8_t AK8963::getAdjustmentX() { 170 | I2Cdev::readByte(devAddr, AK8963_RA_ASAX, buffer); 171 | return buffer[0]; 172 | } 173 | void AK8963::setAdjustmentX(uint8_t x) { 174 | I2Cdev::writeByte(devAddr, AK8963_RA_ASAX, x); 175 | } 176 | uint8_t AK8963::getAdjustmentY() { 177 | I2Cdev::readByte(devAddr, AK8963_RA_ASAY, buffer); 178 | return buffer[0]; 179 | } 180 | void AK8963::setAdjustmentY(uint8_t y) { 181 | I2Cdev::writeByte(devAddr, AK8963_RA_ASAY, y); 182 | } 183 | uint8_t AK8963::getAdjustmentZ() { 184 | I2Cdev::readByte(devAddr, AK8963_RA_ASAZ, buffer); 185 | return buffer[0]; 186 | } 187 | void AK8963::setAdjustmentZ(uint8_t z) { 188 | I2Cdev::writeByte(devAddr, AK8963_RA_ASAZ, z); 189 | } 190 | -------------------------------------------------------------------------------- /firmware/lib/imu/AK8963.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - AK8963 I2C device class header file 2 | // Based on AKM AK8963 datasheet, 10/2013 3 | // 8/27/2011 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2016-01-02 - initial release based on AK8975 code 8 | // 9 | 10 | /* ============================================ 11 | I2Cdev device library code is placed under the MIT license 12 | Copyright (c) 2011 Jeff Rowberg 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | =============================================== 32 | */ 33 | 34 | #ifndef _AK8963_H_ 35 | #define _AK8963_H_ 36 | 37 | #include "I2Cdev.h" 38 | 39 | #define AK8963_ADDRESS_00 0x0C 40 | #define AK8963_ADDRESS_01 0x0D 41 | #define AK8963_ADDRESS_10 0x0E 42 | #define AK8963_ADDRESS_11 0x0F 43 | #define AK8963_DEFAULT_ADDRESS AK8963_ADDRESS_00 44 | 45 | #define AK8963_RA_WIA 0x00 46 | #define AK8963_RA_INFO 0x01 47 | #define AK8963_RA_ST1 0x02 48 | #define AK8963_RA_HXL 0x03 49 | #define AK8963_RA_HXH 0x04 50 | #define AK8963_RA_HYL 0x05 51 | #define AK8963_RA_HYH 0x06 52 | #define AK8963_RA_HZL 0x07 53 | #define AK8963_RA_HZH 0x08 54 | #define AK8963_RA_ST2 0x09 55 | #define AK8963_RA_CNTL1 0x0A 56 | #define AK8963_RA_CNTL2 0x0B 57 | #define AK8963_RA_ASTC 0x0C 58 | #define AK8963_RA_TS1 0x0D // SHIPMENT TEST, DO NOT USE 59 | #define AK8963_RA_TS2 0x0E // SHIPMENT TEST, DO NOT USE 60 | #define AK8963_RA_I2CDIS 0x0F 61 | #define AK8963_RA_ASAX 0x10 62 | #define AK8963_RA_ASAY 0x11 63 | #define AK8963_RA_ASAZ 0x12 64 | 65 | #define AK8963_ST1_DRDY_BIT 0 66 | #define AK8963_ST1_DOR_BIT 1 67 | 68 | #define AK8963_ST2_HOFL_BIT 3 69 | #define AK8963_ST2_BITM_BIT 4 70 | 71 | #define AK8963_CNTL1_MODE_BIT 3 72 | #define AK8963_CNTL1_MODE_LENGTH 4 73 | #define AK8963_CNTL1_RES_BIT 4 74 | 75 | #define AK8963_CNTL2_RESET 0x01 76 | 77 | #define AK8963_MODE_POWERDOWN 0x0 78 | #define AK8963_MODE_SINGLE 0x1 79 | #define AK8963_MODE_CONTINUOUS_8HZ 0x2 80 | #define AK8963_MODE_EXTERNAL 0x4 81 | #define AK8963_MODE_CONTINUOUS_100HZ 0x6 82 | #define AK8963_MODE_SELFTEST 0x8 83 | #define AK8963_MODE_FUSEROM 0xF 84 | 85 | #define AK8963_RES_14_BIT 0 86 | #define AK8963_RES_16_BIT 1 87 | 88 | #define AK8963_CNTL2_SRST_BIT 0 89 | 90 | #define AK8963_ASTC_SELF_BIT 6 91 | 92 | #define AK8963_I2CDIS_DISABLE 0x1B 93 | 94 | class AK8963 { 95 | public: 96 | AK8963(); 97 | AK8963(uint8_t address); 98 | 99 | void initialize(); 100 | bool testConnection(); 101 | 102 | // WIA register 103 | uint8_t getDeviceID(); 104 | 105 | // INFO register 106 | uint8_t getInfo(); 107 | 108 | // ST1 register 109 | bool getDataReady(); 110 | bool getDataOverrun(); 111 | 112 | // H* registers 113 | void getHeading(int16_t *x, int16_t *y, int16_t *z); 114 | int16_t getHeadingX(); 115 | int16_t getHeadingY(); 116 | int16_t getHeadingZ(); 117 | 118 | // ST2 register 119 | bool getOverflowStatus(); 120 | bool getOutputBit(); 121 | 122 | // CNTL1 register 123 | uint8_t getMode(); 124 | void setMode(uint8_t mode); 125 | uint8_t getResolution(); 126 | void setResolution(uint8_t resolution); 127 | void reset(); 128 | 129 | // ASTC register 130 | void setSelfTest(bool enabled); 131 | 132 | // I2CDIS 133 | void disableI2C(); // um, why...? 134 | 135 | // ASA* registers 136 | void getAdjustment(int8_t *x, int8_t *y, int8_t *z); 137 | void setAdjustment(int8_t x, int8_t y, int8_t z); 138 | uint8_t getAdjustmentX(); 139 | void setAdjustmentX(uint8_t x); 140 | uint8_t getAdjustmentY(); 141 | void setAdjustmentY(uint8_t y); 142 | uint8_t getAdjustmentZ(); 143 | void setAdjustmentZ(uint8_t z); 144 | 145 | private: 146 | uint8_t devAddr; 147 | uint8_t buffer[6]; 148 | uint8_t mode; 149 | }; 150 | 151 | #endif /* _AK8963_H_ */ 152 | -------------------------------------------------------------------------------- /firmware/lib/imu/QMC5883L.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "QMC5883L.h" 4 | 5 | /* 6 | * QMC5883L 7 | * http://wiki.epalsite.com/images/7/72/QMC5883L-Datasheet-1.0.pdf 8 | */ 9 | 10 | /* The default I2C address of this chip */ 11 | #define QMC5883L_ADDR 0x0D 12 | 13 | /* Register numbers */ 14 | #define QMC5883L_X_LSB 0 15 | #define QMC5883L_X_MSB 1 16 | #define QMC5883L_Y_LSB 2 17 | #define QMC5883L_Y_MSB 3 18 | #define QMC5883L_Z_LSB 4 19 | #define QMC5883L_Z_MSB 5 20 | #define QMC5883L_STATUS 6 21 | #define QMC5883L_TEMP_LSB 7 22 | #define QMC5883L_TEMP_MSB 8 23 | #define QMC5883L_CONFIG 9 24 | #define QMC5883L_CONFIG2 10 25 | #define QMC5883L_RESET 11 26 | #define QMC5883L_RESERVED 12 27 | #define QMC5883L_CHIP_ID 13 28 | 29 | /* Bit values for the STATUS register */ 30 | #define QMC5883L_STATUS_DRDY 1 31 | #define QMC5883L_STATUS_OVL 2 32 | #define QMC5883L_STATUS_DOR 4 33 | 34 | /* Oversampling values for the CONFIG register */ 35 | #define QMC5883L_CONFIG_OS512 0b00000000 36 | #define QMC5883L_CONFIG_OS256 0b01000000 37 | #define QMC5883L_CONFIG_OS128 0b10000000 38 | #define QMC5883L_CONFIG_OS64 0b11000000 39 | 40 | /* Range values for the CONFIG register */ 41 | #define QMC5883L_CONFIG_2GAUSS 0b00000000 42 | #define QMC5883L_CONFIG_8GAUSS 0b00010000 43 | 44 | /* Rate values for the CONFIG register */ 45 | #define QMC5883L_CONFIG_10HZ 0b00000000 46 | #define QMC5883L_CONFIG_50HZ 0b00000100 47 | #define QMC5883L_CONFIG_100HZ 0b00001000 48 | #define QMC5883L_CONFIG_200HZ 0b00001100 49 | 50 | /* Mode values for the CONFIG register */ 51 | #define QMC5883L_CONFIG_STANDBY 0b00000000 52 | #define QMC5883L_CONFIG_CONT 0b00000001 53 | 54 | /* Apparently M_PI isn't available in all environments. */ 55 | #ifndef M_PI 56 | #define M_PI 3.14159265358979323846264338327950288 57 | #endif 58 | 59 | static void write_register( int addr, int reg, int value ) 60 | { 61 | Wire.beginTransmission(addr); 62 | Wire.write(reg); 63 | Wire.write(value); 64 | Wire.endTransmission(); 65 | } 66 | 67 | static int read_register( int addr, int reg, int count ) 68 | { 69 | Wire.beginTransmission(addr); 70 | Wire.write(reg); 71 | Wire.endTransmission(); 72 | 73 | Wire.requestFrom(addr,count); 74 | int n = Wire.available(); 75 | if(n!=count) return 0; 76 | 77 | return n; 78 | } 79 | 80 | void QMC5883L::reconfig() 81 | { 82 | write_register(addr,QMC5883L_CONFIG,oversampling|range|rate|mode); 83 | } 84 | 85 | void QMC5883L::reset() 86 | { 87 | write_register(addr,QMC5883L_RESET,0x01); 88 | reconfig(); 89 | } 90 | 91 | void QMC5883L::setOversampling( int x ) 92 | { 93 | switch(x) { 94 | case 512: 95 | oversampling = QMC5883L_CONFIG_OS512; 96 | break; 97 | case 256: 98 | oversampling = QMC5883L_CONFIG_OS256; 99 | break; 100 | case 128: 101 | oversampling = QMC5883L_CONFIG_OS128; 102 | break; 103 | case 64: 104 | oversampling = QMC5883L_CONFIG_OS64; 105 | break; 106 | } 107 | reconfig(); 108 | } 109 | 110 | void QMC5883L::setRange( int x ) 111 | { 112 | switch(x) { 113 | case 2: 114 | range = QMC5883L_CONFIG_2GAUSS; 115 | break; 116 | case 8: 117 | range = QMC5883L_CONFIG_8GAUSS; 118 | break; 119 | } 120 | reconfig(); 121 | } 122 | 123 | void QMC5883L::setSamplingRate( int x ) 124 | { 125 | switch(x) { 126 | case 10: 127 | rate = QMC5883L_CONFIG_10HZ; 128 | break; 129 | case 50: 130 | rate = QMC5883L_CONFIG_50HZ; 131 | break; 132 | case 100: 133 | rate = QMC5883L_CONFIG_100HZ; 134 | break; 135 | case 200: 136 | rate = QMC5883L_CONFIG_200HZ; 137 | break; 138 | } 139 | reconfig(); 140 | } 141 | 142 | void QMC5883L::init() { 143 | /* This assumes the wire library has been initialized. */ 144 | addr = QMC5883L_ADDR; 145 | oversampling = QMC5883L_CONFIG_OS512; 146 | range = QMC5883L_CONFIG_2GAUSS; 147 | rate = QMC5883L_CONFIG_50HZ; 148 | mode = QMC5883L_CONFIG_CONT; 149 | reset(); 150 | } 151 | 152 | int QMC5883L::ready() 153 | { 154 | if(!read_register(addr,QMC5883L_STATUS,1)) return 0; 155 | uint8_t status = Wire.read(); 156 | return status & QMC5883L_STATUS_DRDY; 157 | } 158 | 159 | int QMC5883L::readRaw( int16_t *x, int16_t *y, int16_t *z, int16_t *t ) 160 | { 161 | while(!ready()) {} 162 | 163 | if(!read_register(addr,QMC5883L_X_LSB,6)) return 0; 164 | 165 | *x = Wire.read() | (Wire.read()<<8); 166 | *y = Wire.read() | (Wire.read()<<8); 167 | *z = Wire.read() | (Wire.read()<<8); 168 | 169 | return 1; 170 | } 171 | 172 | void QMC5883L::resetCalibration() { 173 | xhigh = yhigh = 0; 174 | xlow = ylow = 0; 175 | } 176 | 177 | int QMC5883L::readHeading() 178 | { 179 | int16_t x, y, z, t; 180 | 181 | if(!readRaw(&x,&y,&z,&t)) return 0; 182 | 183 | /* Update the observed boundaries of the measurements */ 184 | 185 | if(xxhigh) xhigh = x; 187 | if(yyhigh) yhigh = y; 189 | 190 | /* Bail out if not enough data is available. */ 191 | 192 | if( xlow==xhigh || ylow==yhigh ) return 0; 193 | 194 | /* Recenter the measurement by subtracting the average */ 195 | 196 | x -= (xhigh+xlow)/2; 197 | y -= (yhigh+ylow)/2; 198 | 199 | /* Rescale the measurement to the range observed. */ 200 | 201 | float fx = (float)x/(xhigh-xlow); 202 | float fy = (float)y/(yhigh-ylow); 203 | 204 | int heading = 180.0*atan2(fy,fx)/M_PI; 205 | if(heading<=0) heading += 360; 206 | 207 | return heading; 208 | } 209 | -------------------------------------------------------------------------------- /firmware/lib/imu/QMC5883L.h: -------------------------------------------------------------------------------- 1 | #ifndef QMC5883L_H 2 | #define QMC5883L_H 3 | 4 | class QMC5883L { 5 | public: 6 | void init(); 7 | void reset(); 8 | int ready(); 9 | void reconfig(); 10 | 11 | int readHeading(); 12 | int readRaw( int16_t *x, int16_t *y, int16_t *z, int16_t *t ); 13 | void getHeading(int16_t *x, int16_t *y, int16_t *z) { 14 | int16_t tt; 15 | readRaw(x, y, z, &tt); 16 | } 17 | 18 | void resetCalibration(); 19 | 20 | void setSamplingRate( int rate ); 21 | void setRange( int range ); 22 | void setOversampling( int ovl ); 23 | void initialize() { 24 | init(); 25 | setSamplingRate(200); 26 | } 27 | 28 | private: 29 | int16_t xhigh, xlow; 30 | int16_t yhigh, ylow; 31 | uint8_t addr; 32 | uint8_t mode; 33 | uint8_t rate; 34 | uint8_t range; 35 | uint8_t oversampling; 36 | }; 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /firmware/lib/imu/QMI8658.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: QMI8658 3 | * @Author: zjw 4 | * @Date: 2022-10-24 5 | * @LastEditTime: 2022-10-24 6 | * @LastEditors: zjw 7 | */ 8 | 9 | #ifndef _QMI8658_H_ 10 | #define _QMI8658_H_ 11 | 12 | #include 13 | #include "QMI8658reg.h" 14 | 15 | typedef struct 16 | { 17 | float roll; 18 | float pitch; 19 | float yaw ; 20 | } EulerAngles; 21 | 22 | class QMI8658 23 | { 24 | uint8_t last_status; // status of last I2C transmission 25 | uint8_t read_reg(uint8_t reg); 26 | 27 | void write_reg(uint8_t reg,uint8_t value); 28 | 29 | public: 30 | uint16_t readWord_reg(uint8_t reg); 31 | // bool init(void); 32 | bool GetEulerAngles(float *pitch,float *roll, float *yaw); 33 | 34 | void config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr, 35 | enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable); 36 | void config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr, 37 | enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable); 38 | 39 | void read_sensor_data(float acc[3], float gyro[3]); 40 | void read_acc(float acc[3]); 41 | void read_gyro(float gyro[3]); 42 | void read_xyz(float acc[3], float gyro[3]); 43 | void axis_convert(float data_a[3], float data_g[3], int layout); 44 | void config_reg(unsigned char low_power); 45 | void enableSensors(unsigned char enableFlags); 46 | unsigned char get_id(void); 47 | unsigned char begin(void); 48 | void dump_reg(void); 49 | void qmi8658_on_demand_cali(void); 50 | 51 | public: 52 | int16_t ax, ay, az, gx, gy, gz; 53 | float pith, roll, yaw; 54 | unsigned long now, lastTime = 0; 55 | float dt; //微分时间 56 | float agz = 0; //角度变量 57 | long gzo = 0; //陀螺仪偏移量 58 | }; 59 | 60 | /*---------------------------------------------------------------------------------------------- 61 | QMI8658C UI Sensor Configuration Settings and Output Data 62 | */ 63 | /// 64 | #define QMI8658_ADDR 0X6B //device address 65 | #define WHO_AM_I 0X00 //Device identifier 66 | #define CTRL1 0x02 //Serial Interface and Sensor Enable 67 | #define CTRL2 0x03 //Accelerometer Settings 68 | #define CTRL3 0x04 //Gyroscope Settings 69 | #define CTRL4 0X05 //Magnetometer Settings 70 | #define CTRL5 0X06 //Sensor Data Processing Settings 71 | #define CTRL7 0x08 //Enable Sensors and Configure Data Reads 72 | #define CTRL8 0X09 //Reserved – Special Settings 73 | 74 | /// 75 | #define AccX_L 0x35 76 | #define AccX_H 0x36 77 | #define AccY_L 0x37 78 | #define AccY_H 0x38 79 | #define AccZ_L 0x39 80 | #define AccZ_H 0x3A 81 | #define TEMP_L 0x33 82 | 83 | #define GyrX_L 0x3B 84 | #define GyrX_H 0x3C 85 | #define GyrY_L 0x3D 86 | #define GyrY_H 0x3E 87 | #define GyrZ_L 0x3F 88 | #define GyrZ_H 0x40 89 | // int16_t QMI8658C_readBytes(unsigned char tmp); 90 | //extern QMI8658C _QMI8658C; 91 | #endif 92 | -------------------------------------------------------------------------------- /firmware/lib/imu/QMI8658reg.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _QMI8658REG_H_ 3 | #define _QMI8658REG_H_ 4 | 5 | // #define QMI8658_USE_SPI 6 | //#define QMI8658_SYNC_SAMPLE_MODE 7 | //#define QMI8658_SOFT_SELFTEST 8 | //#define QMI8658_USE_CALI 9 | 10 | #define QMI8658_USE_FIFO 11 | //#define QMI8658_USE_AMD 12 | //#define QMI8658_USE_PEDOMETER 13 | 14 | 15 | #define QMI8658_SLAVE_ADDR_L 0x6a 16 | #define QMI8658_SLAVE_ADDR_H 0x6b 17 | 18 | #define QMI8658_DISABLE_ALL (0x0) 19 | #define QMI8658_ACC_ENABLE (0x1) 20 | #define QMI8658_GYR_ENABLE (0x2) 21 | #define QMI8658_ACCGYR_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE) 22 | 23 | #define QMI8658_STATUS1_CMD_DONE (0x01) 24 | #define QMI8658_STATUS1_WAKEUP_EVENT (0x04) 25 | 26 | #define QMI8658_CTRL8_DATAVALID_EN 0x40 // bit6:1 int1, 0 int2 27 | #define QMI8658_CTRL8_PEDOMETER_EN 0x10 28 | #define QMI8658_CTRL8_SIGMOTION_EN 0x08 29 | #define QMI8658_CTRL8_NOMOTION_EN 0x04 30 | #define QMI8658_CTRL8_ANYMOTION_EN 0x02 31 | #define QMI8658_CTRL8_TAP_EN 0x01 32 | 33 | #define QMI8658_INT1_ENABLE 0x08 34 | #define QMI8658_INT2_ENABLE 0x10 35 | 36 | #define QMI8658_DRDY_DISABLE 0x20 // ctrl7 37 | 38 | #define QMI8658_FIFO_MAP_INT1 0x04 // ctrl1 39 | #define QMI8658_FIFO_MAP_INT2 ~0x04 // ctrl1 40 | 41 | #define qmi8658_log printf 42 | 43 | enum Qmi8658Register 44 | { 45 | Qmi8658Register_WhoAmI = 0, 46 | Qmi8658Register_Revision, 47 | Qmi8658Register_Ctrl1, 48 | Qmi8658Register_Ctrl2, 49 | Qmi8658Register_Ctrl3, 50 | Qmi8658Register_Ctrl4, 51 | Qmi8658Register_Ctrl5, 52 | Qmi8658Register_Ctrl6, 53 | Qmi8658Register_Ctrl7, 54 | Qmi8658Register_Ctrl8, 55 | Qmi8658Register_Ctrl9, 56 | Qmi8658Register_Cal1_L = 11, 57 | Qmi8658Register_Cal1_H, 58 | Qmi8658Register_Cal2_L, 59 | Qmi8658Register_Cal2_H, 60 | Qmi8658Register_Cal3_L, 61 | Qmi8658Register_Cal3_H, 62 | Qmi8658Register_Cal4_L, 63 | Qmi8658Register_Cal4_H, 64 | Qmi8658Register_FifoWmkTh = 19, 65 | Qmi8658Register_FifoCtrl = 20, 66 | Qmi8658Register_FifoCount = 21, 67 | Qmi8658Register_FifoStatus = 22, 68 | Qmi8658Register_FifoData = 23, 69 | Qmi8658Register_StatusI2CM = 44, 70 | Qmi8658Register_StatusInt = 45, 71 | Qmi8658Register_Status0, 72 | Qmi8658Register_Status1, 73 | Qmi8658Register_Timestamp_L = 48, 74 | Qmi8658Register_Timestamp_M, 75 | Qmi8658Register_Timestamp_H, 76 | Qmi8658Register_Tempearture_L = 51, 77 | Qmi8658Register_Tempearture_H, 78 | Qmi8658Register_Ax_L = 53, 79 | Qmi8658Register_Ax_H, 80 | Qmi8658Register_Ay_L, 81 | Qmi8658Register_Ay_H, 82 | Qmi8658Register_Az_L, 83 | Qmi8658Register_Az_H, 84 | Qmi8658Register_Gx_L = 59, 85 | Qmi8658Register_Gx_H, 86 | Qmi8658Register_Gy_L, 87 | Qmi8658Register_Gy_H, 88 | Qmi8658Register_Gz_L, 89 | Qmi8658Register_Gz_H, 90 | Qmi8658Register_Mx_L = 65, 91 | Qmi8658Register_Mx_H, 92 | Qmi8658Register_My_L, 93 | Qmi8658Register_My_H, 94 | Qmi8658Register_Mz_L, 95 | Qmi8658Register_Mz_H, 96 | Qmi8658Register_firmware_id = 73, 97 | Qmi8658Register_uuid = 81, 98 | 99 | Qmi8658Register_Pedo_L = 90, 100 | Qmi8658Register_Pedo_M = 91, 101 | Qmi8658Register_Pedo_H = 92, 102 | 103 | Qmi8658Register_Reset = 96 104 | }; 105 | 106 | enum qmi8658_Ois_Register 107 | { 108 | qmi8658_OIS_Reg_Ctrl1 = 0x02, 109 | qmi8658_OIS_Reg_Ctrl2, 110 | qmi8658_OIS_Reg_Ctrl3, 111 | qmi8658_OIS_Reg_Ctrl5 = 0x06, 112 | qmi8658_OIS_Reg_Ctrl7 = 0x08, 113 | qmi8658_OIS_Reg_StatusInt = 0x2D, 114 | qmi8658_OIS_Reg_Status0 = 0x2E, 115 | qmi8658_OIS_Reg_Ax_L = 0x33, 116 | qmi8658_OIS_Reg_Ax_H, 117 | qmi8658_OIS_Reg_Ay_L, 118 | qmi8658_OIS_Reg_Ay_H, 119 | qmi8658_OIS_Reg_Az_L, 120 | qmi8658_OIS_Reg_Az_H, 121 | 122 | qmi8658_OIS_Reg_Gx_L = 0x3B, 123 | qmi8658_OIS_Reg_Gx_H, 124 | qmi8658_OIS_Reg_Gy_L, 125 | qmi8658_OIS_Reg_Gy_H, 126 | qmi8658_OIS_Reg_Gz_L, 127 | qmi8658_OIS_Reg_Gz_H, 128 | }; 129 | 130 | enum qmi8658_Ctrl9Command 131 | { 132 | qmi8658_Ctrl9_Cmd_NOP = 0X00, 133 | qmi8658_Ctrl9_Cmd_GyroBias = 0X01, 134 | qmi8658_Ctrl9_Cmd_Rqst_Sdi_Mod = 0X03, 135 | qmi8658_Ctrl9_Cmd_Rst_Fifo = 0X04, 136 | qmi8658_Ctrl9_Cmd_Req_Fifo = 0X05, 137 | qmi8658_Ctrl9_Cmd_I2CM_Write = 0X06, 138 | qmi8658_Ctrl9_Cmd_WoM_Setting = 0x08, 139 | qmi8658_Ctrl9_Cmd_AccelHostDeltaOffset = 0x09, 140 | qmi8658_Ctrl9_Cmd_GyroHostDeltaOffset = 0x0A, 141 | qmi8658_Ctrl9_Cmd_EnableExtReset = 0x0B, 142 | qmi8658_Ctrl9_Cmd_EnableTap = 0x0C, 143 | qmi8658_Ctrl9_Cmd_EnablePedometer = 0x0D, 144 | qmi8658_Ctrl9_Cmd_Motion = 0x0E, 145 | qmi8658_Ctrl9_Cmd_CopyUsid = 0x10, 146 | qmi8658_Ctrl9_Cmd_SetRpu = 0x11, 147 | qmi8658_Ctrl9_Cmd_On_Demand_Cali = 0xA2, 148 | qmi8658_Ctrl9_Cmd_Dbg_WoM_Data_Enable = 0xF8 149 | }; 150 | 151 | 152 | enum qmi8658_LpfConfig 153 | { 154 | Qmi8658Lpf_Disable, 155 | Qmi8658Lpf_Enable 156 | }; 157 | 158 | enum qmi8658_HpfConfig 159 | { 160 | Qmi8658Hpf_Disable, 161 | Qmi8658Hpf_Enable 162 | }; 163 | 164 | enum qmi8658_StConfig 165 | { 166 | Qmi8658St_Disable, 167 | Qmi8658St_Enable 168 | }; 169 | 170 | enum qmi8658_LpfMode 171 | { 172 | A_LSP_MODE_0 = 0x00<<1, 173 | A_LSP_MODE_1 = 0x01<<1, 174 | A_LSP_MODE_2 = 0x02<<1, 175 | A_LSP_MODE_3 = 0x03<<1, 176 | 177 | G_LSP_MODE_0 = 0x00<<5, 178 | G_LSP_MODE_1 = 0x01<<5, 179 | G_LSP_MODE_2 = 0x02<<5, 180 | G_LSP_MODE_3 = 0x03<<5 181 | }; 182 | 183 | enum qmi8658_AccRange 184 | { 185 | Qmi8658AccRange_2g = 0x00 << 4, 186 | Qmi8658AccRange_4g = 0x01 << 4, 187 | Qmi8658AccRange_8g = 0x02 << 4, 188 | Qmi8658AccRange_16g = 0x03 << 4 189 | }; 190 | 191 | 192 | enum qmi8658_AccOdr 193 | { 194 | Qmi8658AccOdr_8000Hz = 0x00, 195 | Qmi8658AccOdr_4000Hz = 0x01, 196 | Qmi8658AccOdr_2000Hz = 0x02, 197 | Qmi8658AccOdr_1000Hz = 0x03, 198 | Qmi8658AccOdr_500Hz = 0x04, 199 | Qmi8658AccOdr_250Hz = 0x05, 200 | Qmi8658AccOdr_125Hz = 0x06, 201 | Qmi8658AccOdr_62_5Hz = 0x07, 202 | Qmi8658AccOdr_31_25Hz = 0x08, 203 | Qmi8658AccOdr_LowPower_128Hz = 0x0c, 204 | Qmi8658AccOdr_LowPower_21Hz = 0x0d, 205 | Qmi8658AccOdr_LowPower_11Hz = 0x0e, 206 | Qmi8658AccOdr_LowPower_3Hz = 0x0f 207 | }; 208 | 209 | enum qmi8658_GyrRange 210 | { 211 | Qmi8658GyrRange_16dps = 0 << 4, 212 | Qmi8658GyrRange_32dps = 1 << 4, 213 | Qmi8658GyrRange_64dps = 2 << 4, 214 | Qmi8658GyrRange_128dps = 3 << 4, 215 | Qmi8658GyrRange_256dps = 4 << 4, 216 | Qmi8658GyrRange_512dps = 5 << 4, 217 | Qmi8658GyrRange_1024dps = 6 << 4, 218 | Qmi8658GyrRange_2048dps = 7 << 4 219 | }; 220 | 221 | /*! 222 | * \brief Gyroscope output rate configuration. 223 | */ 224 | enum qmi8658_GyrOdr 225 | { 226 | Qmi8658GyrOdr_8000Hz = 0x00, 227 | Qmi8658GyrOdr_4000Hz = 0x01, 228 | Qmi8658GyrOdr_2000Hz = 0x02, 229 | Qmi8658GyrOdr_1000Hz = 0x03, 230 | Qmi8658GyrOdr_500Hz = 0x04, 231 | Qmi8658GyrOdr_250Hz = 0x05, 232 | Qmi8658GyrOdr_125Hz = 0x06, 233 | Qmi8658GyrOdr_62_5Hz = 0x07, 234 | Qmi8658GyrOdr_31_25Hz = 0x08 235 | }; 236 | 237 | enum qmi8658_AccUnit 238 | { 239 | Qmi8658AccUnit_g, 240 | Qmi8658AccUnit_ms2 241 | }; 242 | 243 | enum qmi8658_GyrUnit 244 | { 245 | Qmi8658GyrUnit_dps, 246 | Qmi8658GyrUnit_rads 247 | }; 248 | 249 | enum qmi8658_FifoMode 250 | { 251 | qmi8658_Fifo_Bypass = 0, 252 | qmi8658_Fifo_Fifo = 1, 253 | qmi8658_Fifo_Stream = 2, 254 | qmi8658_Fifo_StreamToFifo = 3 255 | }; 256 | 257 | 258 | enum qmi8658_FifoWmkLevel 259 | { 260 | qmi8658_Fifo_WmkEmpty = (0 << 4), 261 | qmi8658_Fifo_WmkOneQuarter = (1 << 4), 262 | qmi8658_Fifo_WmkHalf = (2 << 4), 263 | qmi8658_Fifo_WmkThreeQuarters = (3 << 4) 264 | }; 265 | 266 | enum qmi8658_FifoSize 267 | { 268 | qmi8658_Fifo_16 = (0 << 2), 269 | qmi8658_Fifo_32 = (1 << 2), 270 | qmi8658_Fifo_64 = (2 << 2), 271 | qmi8658_Fifo_128 = (3 << 2) 272 | }; 273 | 274 | enum qmi8658_Interrupt 275 | { 276 | qmi8658_Int_none, 277 | qmi8658_Int1, 278 | qmi8658_Int2, 279 | 280 | qmi8658_Int_total 281 | }; 282 | 283 | enum qmi8658_InterruptState 284 | { 285 | Qmi8658State_high = (1 << 7), 286 | Qmi8658State_low = (0 << 7) 287 | }; 288 | 289 | #define QMI8658_CALI_DATA_NUM 200 290 | 291 | typedef struct qmi8658_cali 292 | { 293 | float acc_last[3]; 294 | float acc[3]; 295 | float acc_fix[3]; 296 | float acc_bias[3]; 297 | float acc_sum[3]; 298 | 299 | float gyr_last[3]; 300 | float gyr[3]; 301 | float gyr_fix[3]; 302 | float gyr_bias[3]; 303 | float gyr_sum[3]; 304 | 305 | unsigned char imu_static_flag; 306 | unsigned char acc_fix_flag; 307 | unsigned char gyr_fix_flag; 308 | char acc_fix_index; 309 | unsigned char gyr_fix_index; 310 | 311 | unsigned char acc_cali_flag; 312 | unsigned char gyr_cali_flag; 313 | unsigned short acc_cali_num; 314 | unsigned short gyr_cali_num; 315 | // unsigned char acc_avg_num; 316 | // unsigned char gyr_avg_num; 317 | } qmi8658_cali; 318 | 319 | typedef struct 320 | { 321 | unsigned char enSensors; 322 | enum qmi8658_AccRange accRange; 323 | enum qmi8658_AccOdr accOdr; 324 | enum qmi8658_GyrRange gyrRange; 325 | enum qmi8658_GyrOdr gyrOdr; 326 | unsigned char ctrl8_value; 327 | #if defined(QMI8658_USE_FIFO) 328 | unsigned char fifo_ctrl; 329 | #endif 330 | } qmi8658_config; 331 | 332 | typedef struct 333 | { 334 | unsigned char slave; 335 | qmi8658_config cfg; 336 | unsigned short ssvt_a; 337 | unsigned short ssvt_g; 338 | unsigned int timestamp; 339 | unsigned int step; 340 | float imu[6]; 341 | } qmi8658_state; 342 | 343 | 344 | #endif 345 | -------------------------------------------------------------------------------- /firmware/lib/imu/default_imu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef DEFAULT_IMU 16 | #define DEFAULT_IMU 17 | 18 | //include IMU base interface 19 | #include "imu_interface.h" 20 | 21 | //include sensor API headers 22 | #include "I2Cdev.h" 23 | #include "ADXL345.h" 24 | #include "ITG3200.h" 25 | #include "HMC5883L.h" 26 | #include "MPU6050.h" 27 | #include "MPU9150.h" 28 | #include "MPU9250.h" 29 | #include "QMI8658.h" 30 | 31 | class GY85IMU: public IMUInterface 32 | { 33 | private: 34 | //constants specific to the sensor 35 | const float accel_scale_ = 1 / 256.0; 36 | const float gyro_scale_ = 1 / 14.375; 37 | 38 | // driver objects to be used 39 | ADXL345 accelerometer_; 40 | ITG3200 gyroscope_; 41 | 42 | // returned vector for sensor reading 43 | geometry_msgs__msg__Vector3 accel_; 44 | geometry_msgs__msg__Vector3 gyro_; 45 | 46 | public: 47 | GY85IMU() 48 | { 49 | // accel_cov_ = 0.001; //you can overwrite the convariance values here 50 | // gyro_cov_ = 0.001; //you can overwrite the convariance values here 51 | } 52 | 53 | bool startSensor() override 54 | { 55 | // here you can override startSensor() function and use the sensor's driver API 56 | // to initialize and test the sensor's connection during boot time 57 | Wire.begin(); 58 | bool ret; 59 | accelerometer_.initialize(); 60 | ret = accelerometer_.testConnection(); 61 | if(!ret) 62 | return false; 63 | 64 | gyroscope_.initialize(); 65 | ret = gyroscope_.testConnection(); 66 | if(!ret) 67 | return false; 68 | 69 | return true; 70 | } 71 | 72 | geometry_msgs__msg__Vector3 readAccelerometer() override 73 | { 74 | // here you can override readAccelerometer function and use the sensor's driver API 75 | // to grab the data from accelerometer and return as a Vector3 object 76 | int16_t ax, ay, az; 77 | 78 | accelerometer_.getAcceleration(&ax, &ay, &az); 79 | 80 | accel_.x = ax * (double) accel_scale_ * g_to_accel_; 81 | accel_.y = ay * (double) accel_scale_ * g_to_accel_; 82 | accel_.z = az * (double) accel_scale_ * g_to_accel_; 83 | 84 | return accel_; 85 | } 86 | 87 | geometry_msgs__msg__Vector3 readGyroscope() override 88 | { 89 | // here you can override readAccelerometer function and use the sensor's driver API 90 | // to grab the data from gyroscope and return as a Vector3 object 91 | int16_t gx, gy, gz; 92 | 93 | gyroscope_.getRotation(&gx, &gy, &gz); 94 | 95 | gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD; 96 | gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD; 97 | gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD; 98 | 99 | return gyro_; 100 | } 101 | }; 102 | 103 | 104 | class MPU6050IMU: public IMUInterface 105 | { 106 | private: 107 | const float accel_scale_ = 1 / 16384.0; 108 | const float gyro_scale_ = 1 / 131.0; 109 | 110 | MPU6050 accelerometer_; 111 | MPU6050 gyroscope_; 112 | 113 | geometry_msgs__msg__Vector3 accel_; 114 | geometry_msgs__msg__Vector3 gyro_; 115 | 116 | public: 117 | MPU6050IMU() 118 | { 119 | } 120 | 121 | bool startSensor() override 122 | { 123 | Wire.begin(); 124 | bool ret; 125 | accelerometer_.initialize(); 126 | ret = accelerometer_.testConnection(); 127 | if(!ret) 128 | return false; 129 | 130 | gyroscope_.initialize(); 131 | ret = gyroscope_.testConnection(); 132 | if(!ret) 133 | return false; 134 | 135 | return true; 136 | } 137 | 138 | geometry_msgs__msg__Vector3 readAccelerometer() override 139 | { 140 | int16_t ax, ay, az; 141 | 142 | accelerometer_.getAcceleration(&ax, &ay, &az); 143 | 144 | accel_.x = ax * (double) accel_scale_ * g_to_accel_; 145 | accel_.y = ay * (double) accel_scale_ * g_to_accel_; 146 | accel_.z = az * (double) accel_scale_ * g_to_accel_; 147 | 148 | return accel_; 149 | } 150 | 151 | geometry_msgs__msg__Vector3 readGyroscope() override 152 | { 153 | int16_t gx, gy, gz; 154 | 155 | gyroscope_.getRotation(&gx, &gy, &gz); 156 | 157 | gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD; 158 | gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD; 159 | gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD; 160 | 161 | return gyro_; 162 | } 163 | }; 164 | 165 | class MPU9150IMU: public IMUInterface 166 | { 167 | private: 168 | const float accel_scale_ = 1 / 16384.0; 169 | const float gyro_scale_ = 1 / 131.0; 170 | 171 | MPU9150 accelerometer_; 172 | MPU9150 gyroscope_; 173 | 174 | geometry_msgs__msg__Vector3 accel_; 175 | geometry_msgs__msg__Vector3 gyro_; 176 | 177 | public: 178 | MPU9150IMU() 179 | { 180 | } 181 | 182 | bool startSensor() override 183 | { 184 | Wire.begin(); 185 | bool ret; 186 | accelerometer_.initialize(); 187 | ret = accelerometer_.testConnection(); 188 | if(!ret) 189 | return false; 190 | 191 | gyroscope_.initialize(); 192 | ret = gyroscope_.testConnection(); 193 | if(!ret) 194 | return false; 195 | 196 | return true; 197 | } 198 | 199 | geometry_msgs__msg__Vector3 readAccelerometer() override 200 | { 201 | int16_t ax, ay, az; 202 | 203 | accelerometer_.getAcceleration(&ax, &ay, &az); 204 | 205 | accel_.x = ax * (double) accel_scale_ * g_to_accel_; 206 | accel_.y = ay * (double) accel_scale_ * g_to_accel_; 207 | accel_.z = az * (double) accel_scale_ * g_to_accel_; 208 | 209 | return accel_; 210 | } 211 | 212 | geometry_msgs__msg__Vector3 readGyroscope() override 213 | { 214 | int16_t gx, gy, gz; 215 | 216 | gyroscope_.getRotation(&gx, &gy, &gz); 217 | 218 | gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD; 219 | gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD; 220 | gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD; 221 | 222 | return gyro_; 223 | } 224 | }; 225 | 226 | class MPU9250IMU: public IMUInterface 227 | { 228 | private: 229 | const float accel_scale_ = 1 / 16384.0; 230 | const float gyro_scale_ = 1 / 131.0; 231 | 232 | MPU9250 accelerometer_; 233 | MPU9250 gyroscope_; 234 | 235 | geometry_msgs__msg__Vector3 accel_; 236 | geometry_msgs__msg__Vector3 gyro_; 237 | 238 | public: 239 | MPU9250IMU() 240 | { 241 | } 242 | 243 | bool startSensor() override 244 | { 245 | Wire.begin(); 246 | bool ret; 247 | accelerometer_.initialize(); 248 | ret = accelerometer_.testConnection(); 249 | if(!ret) 250 | return false; 251 | 252 | gyroscope_.initialize(); 253 | ret = gyroscope_.testConnection(); 254 | if(!ret) 255 | return false; 256 | 257 | return true; 258 | } 259 | 260 | geometry_msgs__msg__Vector3 readAccelerometer() override 261 | { 262 | int16_t ax, ay, az; 263 | 264 | accelerometer_.getAcceleration(&ax, &ay, &az); 265 | 266 | accel_.x = ax * (double) accel_scale_ * g_to_accel_; 267 | accel_.y = ay * (double) accel_scale_ * g_to_accel_; 268 | accel_.z = az * (double) accel_scale_ * g_to_accel_; 269 | 270 | return accel_; 271 | } 272 | 273 | geometry_msgs__msg__Vector3 readGyroscope() override 274 | { 275 | int16_t gx, gy, gz; 276 | 277 | gyroscope_.getRotation(&gx, &gy, &gz); 278 | 279 | gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD; 280 | gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD; 281 | gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD; 282 | 283 | return gyro_; 284 | } 285 | }; 286 | 287 | class FakeIMU: public IMUInterface 288 | { 289 | private: 290 | geometry_msgs__msg__Vector3 accel_; 291 | geometry_msgs__msg__Vector3 gyro_; 292 | 293 | public: 294 | FakeIMU() 295 | { 296 | } 297 | 298 | bool startSensor() override 299 | { 300 | return true; 301 | } 302 | 303 | geometry_msgs__msg__Vector3 readAccelerometer() override 304 | { 305 | return accel_; 306 | } 307 | 308 | geometry_msgs__msg__Vector3 readGyroscope() override 309 | { 310 | return gyro_; 311 | } 312 | }; 313 | 314 | class QMI8658IMU: public IMUInterface 315 | { 316 | private: 317 | QMI8658 qmi8658_; 318 | 319 | geometry_msgs__msg__Vector3 accel_; 320 | geometry_msgs__msg__Vector3 gyro_; 321 | 322 | public: 323 | QMI8658IMU() 324 | { 325 | } 326 | 327 | bool startSensor() override 328 | { 329 | Wire.begin(); 330 | if (qmi8658_.begin() == 0){ 331 | // Serial.println("qmi8658_init fail"); 332 | return false; 333 | } 334 | return true; 335 | } 336 | 337 | geometry_msgs__msg__Vector3 readAccelerometer() override 338 | { 339 | float ac[3]; 340 | qmi8658_.read_acc(ac); 341 | accel_.x = ac[0]; 342 | accel_.y = ac[1]; 343 | accel_.z = ac[2]; 344 | return accel_; 345 | } 346 | 347 | geometry_msgs__msg__Vector3 readGyroscope() override 348 | { 349 | float gy[3]; 350 | qmi8658_.read_gyro(gy); 351 | gyro_.x = gy[0]; 352 | gyro_.y = gy[1]; 353 | gyro_.z = gy[2]; 354 | return gyro_; 355 | } 356 | }; 357 | 358 | #endif 359 | //ADXL345 https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf 360 | //HMC8553L https://cdn-shop.adafruit.com/datasheets/HMC5883L_3-Axis_Digital_Compass_IC.pdf 361 | //ITG320 https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf 362 | 363 | 364 | //MPU9150 https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf 365 | //MPU9250 https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf 366 | //MPU6050 https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf 367 | 368 | //http://www.sureshjoshi.com/embedded/invensense-imus-what-to-know/ 369 | //https://stackoverflow.com/questions/19161872/meaning-of-lsb-unit-and-unit-lsb 370 | -------------------------------------------------------------------------------- /firmware/lib/imu/default_mag.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 2 | // Copyright (c) 2023 Thomas Chou 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 | #ifndef DEFAULT_MAG 17 | #define DEFAULT_MAG 18 | 19 | //include MAG base interface 20 | #include "mag_interface.h" 21 | 22 | //include sensor API headers 23 | #include "I2Cdev.h" 24 | #include "HMC5883L.h" 25 | #include "AK8963.h" 26 | #include "AK8975.h" 27 | #include "AK09918.h" 28 | #include "QMC5883L.h" 29 | 30 | class HMC5883LMAG: public MAGInterface 31 | { 32 | private: 33 | //constants specific to the sensor 34 | 35 | // driver objects to be used 36 | HMC5883L magnetometer_; 37 | 38 | // returned vector for sensor reading 39 | geometry_msgs__msg__Vector3 mag_; 40 | 41 | public: 42 | HMC5883LMAG() 43 | { 44 | } 45 | 46 | bool startSensor() override 47 | { 48 | // here you can override startSensor() function and use the sensor's driver API 49 | // to initialize and test the sensor's connection during boot time 50 | Wire.begin(); 51 | bool ret; 52 | magnetometer_.initialize(); 53 | ret = magnetometer_.testConnection(); 54 | if (!ret) 55 | return false; 56 | 57 | return true; 58 | } 59 | 60 | geometry_msgs__msg__Vector3 readMagnetometer() override 61 | { 62 | // here you can override readMagnetometer function and use the sensor's driver API 63 | // to grab the data from magnetometer and return as a Vector3 object 64 | int16_t ax, ay, az; 65 | 66 | magnetometer_.getHeading(&ax, &ay, &az); 67 | 68 | mag_.x = ax; 69 | mag_.y = ay; 70 | mag_.z = az; 71 | 72 | return mag_; 73 | } 74 | }; 75 | 76 | class AK8963MAG: public MAGInterface 77 | { 78 | private: 79 | //constants specific to the sensor 80 | 81 | // driver objects to be used 82 | AK8963 magnetometer_; 83 | 84 | // returned vector for sensor reading 85 | geometry_msgs__msg__Vector3 mag_; 86 | 87 | public: 88 | AK8963MAG() 89 | { 90 | } 91 | 92 | bool startSensor() override 93 | { 94 | // here you can override startSensor() function and use the sensor's driver API 95 | // to initialize and test the sensor's connection during boot time 96 | Wire.begin(); 97 | bool ret; 98 | magnetometer_.initialize(); 99 | ret = magnetometer_.testConnection(); 100 | if (!ret) 101 | return false; 102 | 103 | return true; 104 | } 105 | 106 | geometry_msgs__msg__Vector3 readMagnetometer() override 107 | { 108 | // here you can override readMagnetometer function and use the sensor's driver API 109 | // to grab the data from magnetometer and return as a Vector3 object 110 | int16_t ax, ay, az; 111 | 112 | magnetometer_.getHeading(&ax, &ay, &az); 113 | 114 | mag_.x = ax; 115 | mag_.y = ay; 116 | mag_.z = az; 117 | 118 | return mag_; 119 | } 120 | }; 121 | 122 | class AK8975MAG: public MAGInterface 123 | { 124 | private: 125 | //constants specific to the sensor 126 | 127 | // driver objects to be used 128 | AK8975 magnetometer_; 129 | 130 | // returned vector for sensor reading 131 | geometry_msgs__msg__Vector3 mag_; 132 | 133 | public: 134 | AK8975MAG() 135 | { 136 | } 137 | 138 | bool startSensor() override 139 | { 140 | // here you can override startSensor() function and use the sensor's driver API 141 | // to initialize and test the sensor's connection during boot time 142 | Wire.begin(); 143 | bool ret; 144 | magnetometer_.initialize(); 145 | ret = magnetometer_.testConnection(); 146 | if (!ret) 147 | return false; 148 | 149 | return true; 150 | } 151 | 152 | geometry_msgs__msg__Vector3 readMagnetometer() override 153 | { 154 | // here you can override readMagnetometer function and use the sensor's driver API 155 | // to grab the data from magnetometer and return as a Vector3 object 156 | int16_t ax, ay, az; 157 | 158 | magnetometer_.getHeading(&ax, &ay, &az); 159 | 160 | mag_.x = ax; 161 | mag_.y = ay; 162 | mag_.z = az; 163 | 164 | return mag_; 165 | } 166 | }; 167 | 168 | class AK09918MAG: public MAGInterface 169 | { 170 | private: 171 | //constants specific to the sensor 172 | 173 | // driver objects to be used 174 | AK09918 magnetometer_; 175 | 176 | // returned vector for sensor reading 177 | geometry_msgs__msg__Vector3 mag_; 178 | 179 | public: 180 | AK09918MAG() 181 | { 182 | } 183 | 184 | bool startSensor() override 185 | { 186 | // here you can override startSensor() function and use the sensor's driver API 187 | // to initialize and test the sensor's connection during boot time 188 | Wire.begin(); 189 | bool ret; 190 | ret = magnetometer_.initialize(); 191 | if (ret) 192 | return false; 193 | 194 | return true; 195 | } 196 | 197 | geometry_msgs__msg__Vector3 readMagnetometer() override 198 | { 199 | // here you can override readMagnetometer function and use the sensor's driver API 200 | // to grab the data from magnetometer and return as a Vector3 object 201 | int16_t ax, ay, az; 202 | 203 | magnetometer_.getHeading(&ax, &ay, &az); 204 | 205 | mag_.x = ax; 206 | mag_.y = ay; 207 | mag_.z = az; 208 | 209 | return mag_; 210 | } 211 | }; 212 | 213 | class QMC5883LMAG: public MAGInterface 214 | { 215 | private: 216 | //constants specific to the sensor 217 | 218 | // driver objects to be used 219 | QMC5883L magnetometer_; 220 | 221 | // returned vector for sensor reading 222 | geometry_msgs__msg__Vector3 mag_; 223 | 224 | public: 225 | QMC5883LMAG() 226 | { 227 | } 228 | 229 | bool startSensor() override 230 | { 231 | // here you can override startSensor() function and use the sensor's driver API 232 | // to initialize and test the sensor's connection during boot time 233 | Wire.begin(); 234 | bool ret; 235 | magnetometer_.initialize(); 236 | 237 | return true; 238 | } 239 | 240 | geometry_msgs__msg__Vector3 readMagnetometer() override 241 | { 242 | // here you can override readMagnetometer function and use the sensor's driver API 243 | // to grab the data from magnetometer and return as a Vector3 object 244 | int16_t ax, ay, az; 245 | 246 | magnetometer_.getHeading(&ax, &ay, &az); 247 | mag_.x = ax; 248 | mag_.y = ay; 249 | mag_.z = az; 250 | 251 | return mag_; 252 | } 253 | }; 254 | 255 | class FakeMAG: public MAGInterface 256 | { 257 | private: 258 | geometry_msgs__msg__Vector3 mag_; 259 | 260 | public: 261 | FakeMAG() 262 | { 263 | } 264 | 265 | bool startSensor() override 266 | { 267 | return true; 268 | } 269 | 270 | geometry_msgs__msg__Vector3 readMagnetometer() override 271 | { 272 | return mag_; 273 | } 274 | }; 275 | #endif 276 | //ADXL345 https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf 277 | //HMC8553L https://cdn-shop.adafruit.com/datasheets/HMC5883L_3-Axis_Digital_Compass_IC.pdf 278 | //ITG320 https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf 279 | 280 | 281 | //MPU9150 https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf 282 | //MPU9250 https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf 283 | //MPU6050 https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf 284 | 285 | //http://www.sureshjoshi.com/embedded/invensense-imus-what-to-know/ 286 | //https://stackoverflow.com/questions/19161872/meaning-of-lsb-unit-and-unit-lsb 287 | -------------------------------------------------------------------------------- /firmware/lib/imu/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU9150 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ 217 | -------------------------------------------------------------------------------- /firmware/lib/imu/imu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef IMU_CONFIG_H 16 | #define IMU_CONFIG_H 17 | 18 | // include the header of your new driver here similar to default_imu.h 19 | #include "default_imu.h" 20 | 21 | // now you can create a config constant that you can use in lino_base_config.h 22 | #ifdef USE_GY85_IMU 23 | // pass your built in class to IMU macro 24 | #define IMU GY85IMU 25 | #endif 26 | 27 | #ifdef USE_MPU6050_IMU 28 | #define IMU MPU6050IMU 29 | #endif 30 | 31 | #ifdef USE_MPU9150_IMU 32 | #define IMU MPU9150IMU 33 | #endif 34 | 35 | #ifdef USE_MPU9250_IMU 36 | #define IMU MPU9250IMU 37 | #endif 38 | 39 | #ifdef USE_QMI8658_IMU 40 | #define IMU QMI8658IMU 41 | #endif 42 | 43 | #ifndef IMU 44 | #define USE_FAKE_IMU 45 | #define IMU FakeIMU 46 | #endif 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /firmware/lib/imu/imu_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef IMU_INTERFACE 16 | #define IMU_INTERFACE 17 | 18 | #include 19 | 20 | class IMUInterface 21 | { 22 | protected: 23 | sensor_msgs__msg__Imu imu_msg_; 24 | const float g_to_accel_ = 9.81; 25 | const float mgauss_to_utesla_ = 0.1; 26 | const float utesla_to_tesla_ = 0.000001; 27 | 28 | float accel_cov_ = 0.00001; 29 | float gyro_cov_ = 0.00001; 30 | float ori_cov_ = 0.00001; 31 | const int sample_size_ = 40; 32 | 33 | geometry_msgs__msg__Vector3 gyro_cal_; 34 | 35 | void calibrateGyro() 36 | { 37 | geometry_msgs__msg__Vector3 gyro; 38 | 39 | for(int i=0; i -0.01 && imu_msg_.angular_velocity.x < 0.01 ) 81 | imu_msg_.angular_velocity.x = 0; 82 | 83 | if(imu_msg_.angular_velocity.y > -0.01 && imu_msg_.angular_velocity.y < 0.01 ) 84 | imu_msg_.angular_velocity.y = 0; 85 | 86 | if(imu_msg_.angular_velocity.z > -0.01 && imu_msg_.angular_velocity.z < 0.01 ) 87 | imu_msg_.angular_velocity.z = 0; 88 | 89 | imu_msg_.angular_velocity_covariance[0] = gyro_cov_; 90 | imu_msg_.angular_velocity_covariance[4] = gyro_cov_; 91 | imu_msg_.angular_velocity_covariance[8] = gyro_cov_; 92 | 93 | imu_msg_.linear_acceleration = readAccelerometer(); 94 | imu_msg_.linear_acceleration_covariance[0] = accel_cov_; 95 | imu_msg_.linear_acceleration_covariance[4] = accel_cov_; 96 | imu_msg_.linear_acceleration_covariance[8] = accel_cov_; 97 | 98 | imu_msg_.orientation_covariance[0] = ori_cov_; 99 | imu_msg_.orientation_covariance[4] = ori_cov_; 100 | imu_msg_.orientation_covariance[8] = ori_cov_; 101 | 102 | return imu_msg_; 103 | } 104 | }; 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /firmware/lib/imu/mag.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 2 | // Copyright (c) 2023 Thomas Chou 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 | #ifndef MAG_CONFIG_H 17 | #define MAG_CONFIG_H 18 | 19 | // include the header of your new driver here similar to default_mag.h 20 | #include "default_mag.h" 21 | 22 | // now you can create a config constant that you can use in lino_base_config.h 23 | #ifdef USE_HMC5883L_MAG 24 | // pass your built in class to MAG macro 25 | #define MAG HMC5883LMAG 26 | #endif 27 | 28 | #ifdef USE_AK8963_MAG 29 | #define MAG AK8963MAG 30 | #endif 31 | 32 | #ifdef USE_AK8975_MAG 33 | #define MAG AK8975MAG 34 | #endif 35 | 36 | #ifdef USE_AK09918_MAG 37 | #define MAG AK09918MAG 38 | #endif 39 | 40 | #ifdef USE_QMC5883L_MAG 41 | #define MAG QMC5883LMAG 42 | #endif 43 | 44 | #ifndef MAG // use fake mag when there is no real mag 45 | #define USE_FAKE_MAG 46 | #define MAG FakeMAG 47 | #endif 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /firmware/lib/imu/mag_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 2 | // Copyright (c) 2023 Thomas Chou 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 | #ifndef MAG_INTERFACE 17 | #define MAG_INTERFACE 18 | 19 | #include 20 | 21 | class MAGInterface 22 | { 23 | protected: 24 | sensor_msgs__msg__MagneticField mag_msg_; 25 | float mag_cov_ = 0.00001; 26 | 27 | public: 28 | MAGInterface() 29 | { 30 | mag_msg_.header.frame_id = micro_ros_string_utilities_set(mag_msg_.header.frame_id, "imu_link"); 31 | } 32 | 33 | virtual geometry_msgs__msg__Vector3 readMagnetometer() = 0; 34 | virtual bool startSensor() = 0; 35 | 36 | bool init() 37 | { 38 | bool sensor_ok = startSensor(); 39 | return sensor_ok; 40 | } 41 | 42 | sensor_msgs__msg__MagneticField getData() 43 | { 44 | mag_msg_.magnetic_field = readMagnetometer(); 45 | mag_msg_.magnetic_field_covariance[0] = mag_cov_; 46 | mag_msg_.magnetic_field_covariance[4] = mag_cov_; 47 | mag_msg_.magnetic_field_covariance[8] = mag_cov_; 48 | 49 | return mag_msg_; 50 | } 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /firmware/lib/kinematics/kinematics.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #include "Arduino.h" 16 | #include "kinematics.h" 17 | 18 | Kinematics::Kinematics(base robot_base, int motor_max_rpm, float max_rpm_ratio, 19 | float motor_operating_voltage, float motor_power_max_voltage, 20 | float wheel_diameter, float wheels_y_distance): 21 | base_platform_(robot_base), 22 | wheels_y_distance_(wheels_y_distance), 23 | wheel_circumference_(PI * wheel_diameter), 24 | total_wheels_(getTotalWheels(robot_base)) 25 | { 26 | motor_power_max_voltage = constrain(motor_power_max_voltage, 0, motor_operating_voltage); 27 | max_rpm_ = ((motor_power_max_voltage / motor_operating_voltage) * motor_max_rpm) * max_rpm_ratio; 28 | } 29 | 30 | Kinematics::rpm Kinematics::calculateRPM(float linear_x, float linear_y, float angular_z) 31 | { 32 | 33 | float tangential_vel = angular_z * (wheels_y_distance_ / 2.0); 34 | 35 | //convert m/s to m/min 36 | float linear_vel_x_mins = linear_x * 60.0; 37 | float linear_vel_y_mins = linear_y * 60.0; 38 | //convert rad/s to rad/min 39 | float tangential_vel_mins = tangential_vel * 60.0; 40 | 41 | float x_rpm = linear_vel_x_mins / wheel_circumference_; 42 | float y_rpm = linear_vel_y_mins / wheel_circumference_; 43 | float tan_rpm = tangential_vel_mins / wheel_circumference_; 44 | 45 | float a_x_rpm = fabs(x_rpm); 46 | float a_y_rpm = fabs(y_rpm); 47 | float a_tan_rpm = fabs(tan_rpm); 48 | 49 | float xy_sum = a_x_rpm + a_y_rpm; 50 | float xtan_sum = a_x_rpm + a_tan_rpm; 51 | 52 | //calculate the scale value how much each target velocity 53 | //must be scaled down in such cases where the total required RPM 54 | //is more than the motor's max RPM 55 | //this is to ensure that the required motion is achieved just with slower speed 56 | if(xy_sum >= max_rpm_ && angular_z == 0) 57 | { 58 | float vel_scaler = max_rpm_ / xy_sum; 59 | 60 | x_rpm *= vel_scaler; 61 | y_rpm *= vel_scaler; 62 | } 63 | 64 | else if(xtan_sum >= max_rpm_ && linear_y == 0) 65 | { 66 | float vel_scaler = max_rpm_ / xtan_sum; 67 | 68 | x_rpm *= vel_scaler; 69 | tan_rpm *= vel_scaler; 70 | } 71 | 72 | Kinematics::rpm rpm; 73 | 74 | //calculate for the target motor RPM and direction 75 | //front-left motor 76 | rpm.motor1 = x_rpm - y_rpm - tan_rpm; 77 | rpm.motor1 = constrain(rpm.motor1, -max_rpm_, max_rpm_); 78 | 79 | //front-right motor 80 | rpm.motor2 = x_rpm + y_rpm + tan_rpm; 81 | rpm.motor2 = constrain(rpm.motor2, -max_rpm_, max_rpm_); 82 | 83 | //rear-left motor 84 | rpm.motor3 = x_rpm + y_rpm - tan_rpm; 85 | rpm.motor3 = constrain(rpm.motor3, -max_rpm_, max_rpm_); 86 | 87 | //rear-right motor 88 | rpm.motor4 = x_rpm - y_rpm + tan_rpm; 89 | rpm.motor4 = constrain(rpm.motor4, -max_rpm_, max_rpm_); 90 | 91 | return rpm; 92 | } 93 | 94 | Kinematics::rpm Kinematics::getRPM(float linear_x, float linear_y, float angular_z) 95 | { 96 | if(base_platform_ == DIFFERENTIAL_DRIVE || base_platform_ == SKID_STEER) 97 | { 98 | linear_y = 0; 99 | } 100 | 101 | return calculateRPM(linear_x, linear_y, angular_z);; 102 | } 103 | 104 | Kinematics::velocities Kinematics::getVelocities(float rpm1, float rpm2, float rpm3, float rpm4) 105 | { 106 | Kinematics::velocities vel; 107 | float average_rps_x; 108 | float average_rps_y; 109 | float average_rps_a; 110 | 111 | if(base_platform_ == DIFFERENTIAL_DRIVE) 112 | { 113 | rpm3 = 0.0; 114 | rpm4 = 0.0; 115 | } 116 | 117 | //convert average revolutions per minute to revolutions per second 118 | average_rps_x = ((float)(rpm1 + rpm2 + rpm3 + rpm4) / total_wheels_) / 60.0; // RPM 119 | vel.linear_x = average_rps_x * wheel_circumference_; // m/s 120 | 121 | //convert average revolutions per minute in y axis to revolutions per second 122 | average_rps_y = ((float)(-rpm1 + rpm2 + rpm3 - rpm4) / total_wheels_) / 60.0; // RPM 123 | if(base_platform_ == MECANUM) 124 | vel.linear_y = average_rps_y * wheel_circumference_; // m/s 125 | else 126 | vel.linear_y = 0; 127 | 128 | //convert average revolutions per minute to revolutions per second 129 | average_rps_a = ((float)(-rpm1 + rpm2 - rpm3 + rpm4) / total_wheels_) / 60.0; 130 | vel.angular_z = (average_rps_a * wheel_circumference_) / (wheels_y_distance_ / 2.0); // rad/s 131 | 132 | return vel; 133 | } 134 | 135 | int Kinematics::getTotalWheels(base robot_base) 136 | { 137 | switch(robot_base) 138 | { 139 | case DIFFERENTIAL_DRIVE: return 2; 140 | case SKID_STEER: return 4; 141 | case MECANUM: return 4; 142 | default: return 2; 143 | } 144 | } 145 | 146 | float Kinematics::getMaxRPM() 147 | { 148 | return max_rpm_; 149 | } -------------------------------------------------------------------------------- /firmware/lib/kinematics/kinematics.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef KINEMATICS_H 16 | #define KINEMATICS_H 17 | 18 | #include "Arduino.h" 19 | 20 | #define RPM_TO_RPS 1/60 21 | 22 | class Kinematics 23 | { 24 | public: 25 | enum base {DIFFERENTIAL_DRIVE, SKID_STEER, MECANUM}; 26 | 27 | base base_platform_; 28 | 29 | struct rpm 30 | { 31 | float motor1; 32 | float motor2; 33 | float motor3; 34 | float motor4; 35 | }; 36 | 37 | struct velocities 38 | { 39 | float linear_x; 40 | float linear_y; 41 | float angular_z; 42 | }; 43 | 44 | struct pwm 45 | { 46 | int motor1; 47 | int motor2; 48 | int motor3; 49 | int motor4; 50 | }; 51 | Kinematics(base robot_base, int motor_max_rpm, float max_rpm_ratio, 52 | float motor_operating_voltage, float motor_power_max_voltage, 53 | float wheel_diameter, float wheels_y_distance); 54 | velocities getVelocities(float rpm1, float rpm2, float rpm3, float rpm4); 55 | rpm getRPM(float linear_x, float linear_y, float angular_z); 56 | float getMaxRPM(); 57 | 58 | private: 59 | rpm calculateRPM(float linear_x, float linear_y, float angular_z); 60 | int getTotalWheels(base robot_base); 61 | 62 | float max_rpm_; 63 | float wheels_y_distance_; 64 | float pwm_res_; 65 | float wheel_circumference_; 66 | int total_wheels_; 67 | }; 68 | 69 | #endif -------------------------------------------------------------------------------- /firmware/lib/lidar/lidar.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | // 15 | // Push lidar data to a UDP server, which will push laser scan message 16 | // TODO: add PWM closed loop control the scan frequency 17 | // TODO: add TCP for reliable data packet sequence 18 | // 19 | #include 20 | #include "config.h" 21 | #include "syslog.h" 22 | 23 | #ifdef USE_LIDAR_UDP 24 | #include 25 | #include 26 | #define BUFSIZE 512 27 | 28 | HardwareSerial comm(LIDAR_SERIAL); 29 | WiFiUDP udp; 30 | uint8_t buf[BUFSIZE]; 31 | 32 | void rx_err_callback(hardwareSerial_error_t err) 33 | { 34 | syslog(LOG_INFO, "%s err %d", __FUNCTION__, err); 35 | } 36 | 37 | size_t len = 0; 38 | void rx_callback(void) 39 | { 40 | size_t cc = comm.read(buf + len, BUFSIZE - len); 41 | // syslog(LOG_INFO, "%s recv %d", __FUNCTION__, cc); 42 | // send larger UDP packet 43 | if ((len += cc) && (len > 128)) { 44 | udp.beginPacket(LIDAR_SERVER, LIDAR_PORT); 45 | udp.write(buf, len); 46 | udp.endPacket(); 47 | // syslog(LOG_INFO, "%s send %d", __FUNCTION__, len); 48 | len = 0; 49 | } 50 | } 51 | 52 | void initLidar(void) { 53 | pinMode(LIDAR_RXD, INPUT); 54 | #ifdef LIDAR_PWM 55 | pinMode(LIDAR_PWM, OUTPUT); 56 | analogWrite(LIDAR_PWM, 1 << PWM_BITS -1); 57 | #endif 58 | comm.setRxBufferSize(1024); 59 | comm.onReceiveError(rx_err_callback); 60 | comm.onReceive(rx_callback); 61 | comm.begin(LIDAR_BAUDRATE, SERIAL_8N1, LIDAR_RXD); 62 | }; 63 | #else 64 | void initLidar(void) {}; 65 | void pushLidar(void) {}; 66 | #endif 67 | -------------------------------------------------------------------------------- /firmware/lib/lidar/lidar.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | #ifndef LIDAR_H 15 | #define LIDAR_H 16 | 17 | void initLidar(void); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /firmware/lib/motor/default_motor.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef DEFAULT_MOTOR 16 | #define DEFAULT_MOTOR 17 | 18 | #include 19 | #include 20 | #include "config.h" 21 | #ifdef ESP32 22 | inline void analogWriteFrequency(uint8_t pin, double frequency) 23 | { 24 | analogWriteFrequency(frequency); 25 | } 26 | #elif defined(PICO) 27 | inline void analogWriteFrequency(double frequency) 28 | { 29 | analogWriteFreq(frequency); 30 | } 31 | inline void analogWriteFrequency(uint8_t pin, double frequency) 32 | { 33 | analogWriteFreq(frequency); 34 | } 35 | #endif 36 | 37 | #include "motor_interface.h" 38 | 39 | class Generic2: public MotorInterface 40 | { 41 | private: 42 | int in_a_pin_; 43 | int in_b_pin_; 44 | int pwm_pin_; 45 | 46 | protected: 47 | void forward(int pwm) override 48 | { 49 | if (in_a_pin_ < 0) return; 50 | digitalWrite(in_a_pin_, HIGH); 51 | digitalWrite(in_b_pin_, LOW); 52 | analogWrite(pwm_pin_, abs(pwm)); 53 | } 54 | 55 | void reverse(int pwm) override 56 | { 57 | if (in_a_pin_ < 0) return; 58 | digitalWrite(in_a_pin_, LOW); 59 | digitalWrite(in_b_pin_, HIGH); 60 | analogWrite(pwm_pin_, abs(pwm)); 61 | } 62 | 63 | public: 64 | Generic2(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int in_a_pin, int in_b_pin): 65 | MotorInterface(invert), 66 | in_a_pin_(in_a_pin), 67 | in_b_pin_(in_b_pin), 68 | pwm_pin_(pwm_pin) 69 | { 70 | if (in_a_pin_ < 0) return; 71 | pinMode(in_a_pin_, OUTPUT); 72 | pinMode(in_b_pin_, OUTPUT); 73 | pinMode(pwm_pin_, OUTPUT); 74 | 75 | if(pwm_frequency > 0) 76 | { 77 | analogWriteFrequency(pwm_pin_, pwm_frequency); 78 | } 79 | analogWriteResolution(pwm_bits); 80 | 81 | //ensure that the motor is in neutral state during bootup 82 | analogWrite(pwm_pin_, abs(0)); 83 | } 84 | 85 | void brake() override 86 | { 87 | if (in_a_pin_ < 0) return; 88 | analogWrite(pwm_pin_, 0); 89 | #ifdef USE_SHORT_BRAKE 90 | digitalWrite(in_a_pin_, HIGH); // short brake 91 | digitalWrite(in_b_pin_, HIGH); 92 | #endif 93 | } 94 | }; 95 | 96 | class Generic1: public MotorInterface 97 | { 98 | private: 99 | int in_pin_; 100 | int pwm_pin_; 101 | 102 | protected: 103 | void forward(int pwm) override 104 | { 105 | if (in_pin_ < 0) return; 106 | digitalWrite(in_pin_, HIGH); 107 | analogWrite(pwm_pin_, abs(pwm)); 108 | } 109 | 110 | void reverse(int pwm) override 111 | { 112 | if (in_pin_ < 0) return; 113 | digitalWrite(in_pin_, LOW); 114 | analogWrite(pwm_pin_, abs(pwm)); 115 | } 116 | 117 | public: 118 | Generic1(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int in_pin, int unused=-1): 119 | MotorInterface(invert), 120 | in_pin_(in_pin), 121 | pwm_pin_(pwm_pin) 122 | { 123 | if (in_pin_ < 0) return; 124 | pinMode(in_pin_, OUTPUT); 125 | pinMode(pwm_pin_, OUTPUT); 126 | 127 | if(pwm_frequency > 0) 128 | { 129 | analogWriteFrequency(pwm_pin_, pwm_frequency); 130 | } 131 | analogWriteResolution(pwm_bits); 132 | 133 | //ensure that the motor is in neutral state during bootup 134 | analogWrite(pwm_pin_, abs(0)); 135 | } 136 | 137 | void brake() override 138 | { 139 | if (in_pin_ < 0) return; 140 | analogWrite(pwm_pin_, 0); 141 | } 142 | }; 143 | 144 | class BTS7960: public MotorInterface 145 | { 146 | private: 147 | int in_a_pin_; 148 | int in_b_pin_; 149 | int pwm_max_; 150 | 151 | protected: 152 | void forward(int pwm) override 153 | { 154 | if (in_a_pin_ < 0) return; 155 | #ifdef USE_SHORT_BRAKE 156 | analogWrite(in_a_pin_, pwm_max_ - abs(pwm)); 157 | analogWrite(in_b_pin_, pwm_max_); // short brake 158 | #else 159 | analogWrite(in_a_pin_, 0); 160 | analogWrite(in_b_pin_, abs(pwm)); 161 | #endif 162 | } 163 | 164 | void reverse(int pwm) override 165 | { 166 | if (in_a_pin_ < 0) return; 167 | #ifdef USE_SHORT_BRAKE 168 | analogWrite(in_b_pin_, pwm_max_ - abs(pwm)); 169 | analogWrite(in_a_pin_, pwm_max_); // short brake 170 | #else 171 | analogWrite(in_b_pin_, 0); 172 | analogWrite(in_a_pin_, abs(pwm)); 173 | #endif 174 | } 175 | 176 | public: 177 | BTS7960(float pwm_frequency, int pwm_bits, bool invert, int unused, int in_a_pin, int in_b_pin): 178 | MotorInterface(invert), 179 | in_a_pin_(in_a_pin), 180 | in_b_pin_(in_b_pin) 181 | { 182 | if (in_a_pin_ < 0) return; 183 | pwm_max_ = (1 << pwm_bits) - 1; 184 | pinMode(in_a_pin_, OUTPUT); 185 | pinMode(in_b_pin_, OUTPUT); 186 | 187 | if(pwm_frequency > 0) 188 | { 189 | analogWriteFrequency(in_a_pin_, pwm_frequency); 190 | analogWriteFrequency(in_b_pin_, pwm_frequency); 191 | 192 | } 193 | analogWriteResolution(pwm_bits); 194 | 195 | //ensure that the motor is in neutral state during bootup 196 | analogWrite(in_a_pin_, 0); 197 | analogWrite(in_b_pin_, 0); 198 | } 199 | 200 | BTS7960(float pwm_frequency, int pwm_bits, bool invert, int in_a_pin, int in_b_pin): 201 | MotorInterface(invert), 202 | in_a_pin_(in_a_pin), 203 | in_b_pin_(in_b_pin) 204 | { 205 | if (in_a_pin_ < 0) return; 206 | pwm_max_ = (1 << pwm_bits) - 1; 207 | pinMode(in_a_pin_, OUTPUT); 208 | pinMode(in_b_pin_, OUTPUT); 209 | 210 | if(pwm_frequency > 0) 211 | { 212 | analogWriteFrequency(in_a_pin_, pwm_frequency); 213 | analogWriteFrequency(in_b_pin_, pwm_frequency); 214 | 215 | } 216 | analogWriteResolution(pwm_bits); 217 | 218 | //ensure that the motor is in neutral state during bootup 219 | analogWrite(in_a_pin_, 0); 220 | analogWrite(in_b_pin_, 0); 221 | } 222 | 223 | void brake() override 224 | { 225 | if (in_a_pin_ < 0) return; 226 | #ifdef USE_SHORT_BRAKE 227 | analogWrite(in_a_pin_, pwm_max_); 228 | analogWrite(in_b_pin_, pwm_max_); // short brake 229 | #else 230 | analogWrite(in_b_pin_, 0); 231 | analogWrite(in_a_pin_, 0); 232 | #endif 233 | } 234 | }; 235 | 236 | class ESC: public MotorInterface 237 | { 238 | private: 239 | Servo motor_; 240 | int pwm_pin_; 241 | 242 | protected: 243 | void forward(int pwm) override 244 | { 245 | if (pwm_pin_ < 0) return; 246 | motor_.writeMicroseconds(1500 + pwm); 247 | } 248 | 249 | void reverse(int pwm) override 250 | { 251 | if (pwm_pin_ < 0) return; 252 | motor_.writeMicroseconds(1500 + pwm); 253 | } 254 | 255 | public: 256 | ESC(float pwm_frequency, int pwm_bits, bool invert, int pwm_pin, int unused=-1, int unused2=-1): 257 | MotorInterface(invert), 258 | pwm_pin_(pwm_pin) 259 | { 260 | if (pwm_pin_ < 0) return; 261 | motor_.attach(pwm_pin); 262 | 263 | //ensure that the motor is in neutral state during bootup 264 | motor_.writeMicroseconds(1500); 265 | } 266 | 267 | void brake() override 268 | { 269 | if (pwm_pin_ < 0) return; 270 | motor_.writeMicroseconds(1500); 271 | } 272 | }; 273 | 274 | #endif 275 | -------------------------------------------------------------------------------- /firmware/lib/motor/motor.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef MOTOR_H 16 | #define MOTOR_H 17 | 18 | // include the header of your new driver here similar to default_motor.h 19 | #include "default_motor.h" 20 | 21 | // now you can create a config constant that you can use in lino_base_config.h 22 | #ifdef USE_GENERIC_2_IN_MOTOR_DRIVER 23 | // pass your built in class to Motor macro 24 | #define Motor Generic2 25 | #endif 26 | 27 | #ifdef USE_GENERIC_1_IN_MOTOR_DRIVER 28 | // pass your built in class to Motor macro 29 | #define Motor Generic1 30 | #endif 31 | 32 | #ifdef USE_BTS7960_MOTOR_DRIVER 33 | // pass your built in class to Motor macro 34 | #define Motor BTS7960 35 | #endif 36 | 37 | #ifdef USE_ESC_MOTOR_DRIVER 38 | // pass your built in class to Motor macro 39 | #define Motor ESC 40 | #endif 41 | 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /firmware/lib/motor/motor_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef MOTOR_INTERFACE 16 | #define MOTOR_INTERFACE 17 | 18 | class MotorInterface 19 | { 20 | bool invert_; 21 | protected: 22 | virtual void forward(int pwm) = 0; 23 | virtual void reverse(int pwm) = 0; 24 | 25 | public: 26 | MotorInterface(int invert): 27 | invert_(invert) 28 | { 29 | } 30 | 31 | virtual void brake() = 0; 32 | void spin(int pwm) 33 | { 34 | if(invert_) 35 | pwm *= -1; 36 | 37 | if(pwm > 0) 38 | forward(pwm); 39 | else if(pwm < 0) 40 | reverse(pwm); 41 | else 42 | brake(); 43 | } 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /firmware/lib/odometry/odometry.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #include "odometry.h" 16 | 17 | Odometry::Odometry(): 18 | x_pos_(0.0), 19 | y_pos_(0.0), 20 | heading_(0.0) 21 | { 22 | odom_msg_.header.frame_id = micro_ros_string_utilities_set(odom_msg_.header.frame_id, "odom"); 23 | odom_msg_.child_frame_id = micro_ros_string_utilities_set(odom_msg_.child_frame_id, "base_footprint"); 24 | } 25 | 26 | void Odometry::update(float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z) 27 | { 28 | float delta_heading = angular_vel_z * vel_dt; //radians 29 | float cos_h = cos(heading_); 30 | float sin_h = sin(heading_); 31 | float delta_x = (linear_vel_x * cos_h - linear_vel_y * sin_h) * vel_dt; //m 32 | float delta_y = (linear_vel_x * sin_h + linear_vel_y * cos_h) * vel_dt; //m 33 | 34 | //calculate current position of the robot 35 | x_pos_ += delta_x; 36 | y_pos_ += delta_y; 37 | heading_ += delta_heading; 38 | 39 | //calculate robot's heading in quaternion angle 40 | //ROS has a function to calculate yaw in quaternion angle 41 | float q[4]; 42 | euler_to_quat(0, 0, heading_, q); 43 | 44 | //robot's position in x,y, and z 45 | odom_msg_.pose.pose.position.x = x_pos_; 46 | odom_msg_.pose.pose.position.y = y_pos_; 47 | odom_msg_.pose.pose.position.z = 0.0; 48 | 49 | //robot's heading in quaternion 50 | odom_msg_.pose.pose.orientation.x = (double) q[1]; 51 | odom_msg_.pose.pose.orientation.y = (double) q[2]; 52 | odom_msg_.pose.pose.orientation.z = (double) q[3]; 53 | odom_msg_.pose.pose.orientation.w = (double) q[0]; 54 | 55 | odom_msg_.pose.covariance[0] = 0.001; 56 | odom_msg_.pose.covariance[7] = 0.001; 57 | odom_msg_.pose.covariance[35] = 0.001; 58 | 59 | //linear speed from encoders 60 | odom_msg_.twist.twist.linear.x = linear_vel_x; 61 | odom_msg_.twist.twist.linear.y = linear_vel_y; 62 | odom_msg_.twist.twist.linear.z = 0.0; 63 | 64 | //angular speed from encoders 65 | odom_msg_.twist.twist.angular.x = 0.0; 66 | odom_msg_.twist.twist.angular.y = 0.0; 67 | odom_msg_.twist.twist.angular.z = angular_vel_z; 68 | 69 | odom_msg_.twist.covariance[0] = 0.0001; 70 | odom_msg_.twist.covariance[7] = 0.0001; 71 | odom_msg_.twist.covariance[35] = 0.0001; 72 | } 73 | 74 | nav_msgs__msg__Odometry Odometry::getData() 75 | { 76 | return odom_msg_; 77 | } 78 | 79 | const void Odometry::euler_to_quat(float roll, float pitch, float yaw, float* q) 80 | { 81 | float cy = cos(yaw * 0.5); 82 | float sy = sin(yaw * 0.5); 83 | float cp = cos(pitch * 0.5); 84 | float sp = sin(pitch * 0.5); 85 | float cr = cos(roll * 0.5); 86 | float sr = sin(roll * 0.5); 87 | 88 | q[0] = cy * cp * cr + sy * sp * sr; 89 | q[1] = cy * cp * sr - sy * sp * cr; 90 | q[2] = sy * cp * sr + cy * sp * cr; 91 | q[3] = sy * cp * cr - cy * sp * sr; 92 | } -------------------------------------------------------------------------------- /firmware/lib/odometry/odometry.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef ODOMETRY_H 16 | #define ODOMETRY_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | class Odometry 24 | { 25 | public: 26 | Odometry(); 27 | void update(float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z); 28 | nav_msgs__msg__Odometry getData(); 29 | 30 | private: 31 | const void euler_to_quat(float x, float y, float z, float* q); 32 | 33 | nav_msgs__msg__Odometry odom_msg_; 34 | float x_pos_; 35 | float y_pos_; 36 | float heading_; 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /firmware/lib/pid/pid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #include "Arduino.h" 16 | #include "pid.h" 17 | 18 | PID::PID(float min_val, float max_val, float kp, float ki, float kd): 19 | min_val_(min_val), 20 | max_val_(max_val), 21 | kp_(kp), 22 | ki_(ki), 23 | kd_(kd) 24 | { 25 | } 26 | 27 | double PID::compute(float setpoint, float measured_value) 28 | { 29 | double error; 30 | double pid; 31 | 32 | //setpoint is constrained between min and max to prevent pid from having too much error 33 | error = setpoint - measured_value; 34 | integral_ += error; 35 | derivative_ = error - prev_error_; 36 | 37 | if(setpoint == 0 && error == 0) 38 | { 39 | integral_ = 0; 40 | derivative_ = 0; 41 | } 42 | 43 | pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_); 44 | prev_error_ = error; 45 | 46 | return constrain(pid, min_val_, max_val_); 47 | } 48 | 49 | void PID::updateConstants(float kp, float ki, float kd) 50 | { 51 | kp_ = kp; 52 | ki_ = ki; 53 | kd_ = kd; 54 | } 55 | -------------------------------------------------------------------------------- /firmware/lib/pid/pid.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | 15 | #ifndef PID_H 16 | #define PID_H 17 | 18 | #include "Arduino.h" 19 | 20 | class PID 21 | { 22 | public: 23 | PID(float min_val, float max_val, float kp, float ki, float kd); 24 | double compute(float setpoint, float measured_value); 25 | void updateConstants(float kp, float ki, float kd); 26 | 27 | private: 28 | float min_val_; 29 | float max_val_; 30 | float kp_; 31 | float ki_; 32 | float kd_; 33 | double integral_; 34 | double derivative_; 35 | double prev_error_; 36 | }; 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /firmware/lib/range/range.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "config.h" 4 | 5 | //define sound speed in m/uS 6 | #define SOUND_SPEED 0.00034 7 | #define TIMEOUT 5000 // uS 8 | #define FOV (15 * 0.0174533) // field of view rad 9 | #define MIN_RANGE 0.02 // 2cm 10 | #define MAX_RANGE (SOUND_SPEED * TIMEOUT / 2 * 0.95) 11 | // the 5000uS timeout limits range to roughly 0.8m 12 | 13 | sensor_msgs__msg__Range range_msg_; 14 | sensor_msgs__msg__Range getRange() 15 | { 16 | #ifdef TRIG_PIN // ultrasonic sensor HC-SR04 17 | unsigned long duration; 18 | // ping 19 | // Sets the trigPin on HIGH state for 10 micro seconds 20 | digitalWrite(TRIG_PIN, HIGH); 21 | delayMicroseconds(10); 22 | digitalWrite(TRIG_PIN, LOW); 23 | // Reads the echoPin, returns the sound wave travel time in microseconds 24 | duration = pulseIn(ECHO_PIN, HIGH, TIMEOUT); 25 | range_msg_.range = duration ? (duration * SOUND_SPEED / 2) : +INFINITY; 26 | range_msg_.field_of_view = FOV; 27 | range_msg_.min_range = MIN_RANGE; 28 | range_msg_.max_range = MAX_RANGE; 29 | #endif 30 | return range_msg_; 31 | } 32 | 33 | void initRange() 34 | { 35 | #ifdef TRIG_PIN // ultrasonic sensor HC-SR04 36 | pinMode(TRIG_PIN, OUTPUT); 37 | #endif 38 | } 39 | -------------------------------------------------------------------------------- /firmware/lib/range/range.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGE_H 2 | #define RANGE_H 3 | 4 | #include 5 | sensor_msgs__msg__Range getRange(); 6 | void initRange(); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /firmware/lib/syslog/syslog.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "config.h" 3 | 4 | #ifdef USE_SYSLOG 5 | #include 6 | #include 7 | WiFiUDP udpClient; 8 | Syslog syslogv(udpClient, SYSLOG_SERVER, SYSLOG_PORT, DEVICE_HOSTNAME, APP_NAME, LOG_KERN); 9 | void syslog(uint16_t priority, const char *fmt, ...) { 10 | va_list args; 11 | va_start(args, fmt); 12 | syslogv.vlogf(priority, fmt, args); 13 | va_end(args); 14 | }; 15 | #endif 16 | -------------------------------------------------------------------------------- /firmware/lib/syslog/syslog.h: -------------------------------------------------------------------------------- 1 | #ifndef SYSLOG_H_ 2 | #define SYSLOG_H_ 3 | 4 | #include 5 | #include "config.h" 6 | 7 | #ifdef USE_SYSLOG 8 | void syslog(uint16_t priority, const char *fmt, ...); 9 | #else 10 | #define syslog(...) 11 | #endif 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /firmware/lib/wifi/ota.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | #include 15 | #include "config.h" 16 | #include "syslog.h" 17 | 18 | #ifdef USE_ARDUINO_OTA 19 | #include 20 | 21 | void initOta(void) 22 | { 23 | // Port defaults to 3232 24 | // Port defaults to 8266 25 | // ArduinoOTA.setPort(8266); 26 | 27 | // Hostname defaults to esp3232-[MAC] 28 | // Hostname defaults to esp8266-[ChipID] 29 | // ArduinoOTA.setHostname("myesp8266"); 30 | 31 | // No authentication by default 32 | // ArduinoOTA.setPassword("admin"); 33 | 34 | // Password can be set with it's md5 value as well 35 | // MD5(admin) = 21232f297a57a5a743894a0e4a801fc3 36 | // ArduinoOTA.setPasswordHash("21232f297a57a5a743894a0e4a801fc3"); 37 | 38 | ArduinoOTA.onStart([]() { 39 | String type; 40 | if (ArduinoOTA.getCommand() == U_FLASH) { 41 | type = "sketch"; 42 | } else { // U_FS 43 | type = "filesystem"; 44 | } 45 | 46 | // NOTE: if updating FS this would be the place to unmount FS using FS.end() 47 | Serial.println("Start updating " + type); 48 | }); 49 | ArduinoOTA.onEnd([]() { 50 | Serial.println("\nEnd"); 51 | }); 52 | ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { 53 | Serial.printf("Progress: %u%%\r", (progress / (total / 100))); 54 | }); 55 | ArduinoOTA.onError([](ota_error_t error) { 56 | Serial.printf("Error[%u]: ", error); 57 | if (error == OTA_AUTH_ERROR) { 58 | Serial.println("Auth Failed"); 59 | } else if (error == OTA_BEGIN_ERROR) { 60 | Serial.println("Begin Failed"); 61 | } else if (error == OTA_CONNECT_ERROR) { 62 | Serial.println("Connect Failed"); 63 | } else if (error == OTA_RECEIVE_ERROR) { 64 | Serial.println("Receive Failed"); 65 | } else if (error == OTA_END_ERROR) { 66 | Serial.println("End Failed"); 67 | } 68 | }); 69 | ArduinoOTA.begin(); 70 | } 71 | 72 | void runOta(void) 73 | { 74 | ArduinoOTA.handle(); 75 | } 76 | 77 | #endif // USE_ARDUINO_OTA 78 | -------------------------------------------------------------------------------- /firmware/lib/wifi/ota.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | #ifndef OTA_H 15 | #define OTA_H 16 | 17 | #ifdef USE_ARDUINO_OTA 18 | void initOta(void); 19 | void runOta(void); 20 | #else 21 | #define initOta() 22 | #define runOta() 23 | #endif 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /firmware/lib/wifi/wifis.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | #include 15 | #include 16 | #include "config.h" 17 | #include "syslog.h" 18 | #include "wifis.h" 19 | 20 | #ifdef WIFI_AP_LIST 21 | #include 22 | #define EXECUTE_EVERY_N_MS(MS, X) do { \ 23 | static volatile unsigned long init = millis(); \ 24 | if (millis() - init > MS) { X; init = millis();} \ 25 | } while (0) 26 | 27 | const char *wifi_ap_list[][2] = WIFI_AP_LIST; 28 | WiFiMulti wifiMulti; 29 | 30 | void initWifis(void) 31 | { 32 | for (int i = 0; wifi_ap_list[i][0] != NULL; i++) { 33 | wifiMulti.addAP(wifi_ap_list[i][0], wifi_ap_list[i][1]); 34 | } 35 | while (wifiMulti.run() != WL_CONNECTED) { 36 | delay(500); 37 | } 38 | Serial.println("WIFI connected"); 39 | Serial.print("IP address: "); 40 | Serial.println(WiFi.localIP()); 41 | syslog(LOG_INFO, "%s ssid %s rssi %d ip %s", __FUNCTION__, WiFi.SSID(), WiFi.RSSI(), 42 | WiFi.localIP().toString().c_str()); 43 | } 44 | 45 | void runWifis(void) 46 | { 47 | static uint8_t dis_bssid[6]; // check previous disconnected bssid to avoid repeated disconnection 48 | uint8_t *bssid; 49 | uint8_t bssidv[6]; 50 | 51 | #ifdef WIFI_MONITOR 52 | EXECUTE_EVERY_N_MS(WIFI_MONITOR * 60 * 1000, syslog(LOG_INFO, "%s ssid %s rssi %d", \ 53 | __FUNCTION__, WiFi.SSID(), WiFi.RSSI())); 54 | #endif 55 | #ifdef PICO // WiFi.BSSID api is different 56 | // when wifi signal is too weak, disconnect current ap and scan for strongest signal 57 | EXECUTE_EVERY_N_MS(2000, (WiFi.RSSI() < LOW_RSSI && (bssid = WiFi.BSSID(bssidv), memcmp(dis_bssid, bssid, 6))) ? \ 58 | (memcpy(dis_bssid, bssid, 6), WiFi.disconnect()) : 0); 59 | #else 60 | // when wifi signal is too weak, disconnect current ap and scan for strongest signal 61 | EXECUTE_EVERY_N_MS(2000, (WiFi.RSSI() < LOW_RSSI && (bssid = WiFi.BSSID(), memcmp(dis_bssid, bssid, 6))) ? \ 62 | (memcpy(dis_bssid, bssid, 6), WiFi.disconnect()) : 0); 63 | #endif 64 | wifiMulti.run(); 65 | } 66 | #endif // WIFI_AP_LIST 67 | -------------------------------------------------------------------------------- /firmware/lib/wifi/wifis.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Thomas Chou 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. 14 | #ifndef WIFIS_H 15 | #define WIFIS_H 16 | 17 | #if defined(USE_WIFI_TRANSPORT) && !defined(WIFI_AP_LIST) // fixup old wifi config 18 | #define WIFI_AP_LIST {{WIFI_SSID, WIFI_PASSWORD}, {NULL}} 19 | #endif 20 | #ifndef LOW_RSSI 21 | #define LOW_RSSI -75 // when wifi signal is too low, disconnect current ap and scan for strongest signal 22 | #endif 23 | 24 | #ifdef WIFI_AP_LIST 25 | void initWifis(void); 26 | void runWifis(void); 27 | #else 28 | #define initWifis() 29 | #define runWifis() 30 | #endif 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /firmware/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | [env] 11 | platform = teensy 12 | framework = arduino 13 | upload_port = /dev/ttyACM0 14 | upload_protocol = teensy-cli 15 | board_microros_transport = serial # or wifi 16 | board_microros_distro = ${sysenv.ROS_DISTRO} 17 | lib_deps = https://github.com/micro-ROS/micro_ros_platformio 18 | jrowberg/I2Cdevlib-Core 19 | jrowberg/I2Cdevlib-ADXL345 20 | ; jrowberg/I2Cdevlib-AK8963 21 | jrowberg/I2Cdevlib-AK8975 22 | jrowberg/I2Cdevlib-HMC5843 23 | jrowberg/I2Cdevlib-HMC5883L 24 | jrowberg/I2Cdevlib-ITG3200 25 | ; jrowberg/I2Cdevlib-MPU6050 26 | ; jrowberg/I2Cdevlib-MPU9150 27 | ; jrowberg/I2Cdevlib-MPU9250 28 | https://github.com/wollewald/INA219_WE 29 | https://github.com/arcao/Syslog 30 | build_flags = -I ../config 31 | 32 | 33 | 34 | [env:myrobot] 35 | platform = espressif32 36 | board = esp32dev 37 | framework = arduino 38 | ;board_build.mcu = esp32 39 | board_build.f_flash = 80000000L 40 | board_build.flash_mode = qio 41 | upload_port = /dev/ttyUSB0 42 | upload_protocol = esptool 43 | board_microros_distro = humble 44 | board_microros_transport = wifi 45 | monitor_speed = 115200 46 | monitor_port = /dev/ttyUSB0 47 | monitor_dtr = 0 48 | monitor_rts = 0 49 | lib_deps = 50 | ${env.lib_deps} 51 | madhephaestus/ESP32Servo@^0.13.0 52 | madhephaestus/ESP32Encoder @ ^0.10.1 53 | build_flags = 54 | -I ../config 55 | -D USE_MYROBOT_CONFIG 56 | -D USE_WIFI_TRANSPORT -------------------------------------------------------------------------------- /test_motors/platformio.ini: -------------------------------------------------------------------------------- 1 | [platformio] 2 | extra_configs = 3 | ../firmware/platformio.ini 4 | 5 | [env] 6 | lib_extra_dirs = 7 | ../firmware/lib 8 | -------------------------------------------------------------------------------- /test_motors/src/firmware.ino: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 2 | // Copyright (c) 2023 Thomas Chou 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 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "config.h" 28 | #include "syslog.h" 29 | #include "motor.h" 30 | #include "kinematics.h" 31 | #include "pid.h" 32 | #include "odometry.h" 33 | #include "imu.h" 34 | #include "mag.h" 35 | #define ENCODER_USE_INTERRUPTS 36 | #define ENCODER_OPTIMIZE_INTERRUPTS 37 | #include "encoder.h" 38 | #include "battery.h" 39 | #include "range.h" 40 | #include "lidar.h" 41 | #include "wifis.h" 42 | #include "ota.h" 43 | 44 | #ifndef BAUDRATE 45 | #define BAUDRATE 115200 46 | #endif 47 | 48 | nav_msgs__msg__Odometry odom_msg; 49 | sensor_msgs__msg__Imu imu_msg; 50 | sensor_msgs__msg__MagneticField mag_msg; 51 | geometry_msgs__msg__Twist twist_msg; 52 | sensor_msgs__msg__BatteryState battery_msg; 53 | sensor_msgs__msg__Range range_msg; 54 | 55 | Encoder motor1_encoder(MOTOR1_ENCODER_A, MOTOR1_ENCODER_B, COUNTS_PER_REV1, MOTOR1_ENCODER_INV); 56 | Encoder motor2_encoder(MOTOR2_ENCODER_A, MOTOR2_ENCODER_B, COUNTS_PER_REV2, MOTOR2_ENCODER_INV); 57 | Encoder motor3_encoder(MOTOR3_ENCODER_A, MOTOR3_ENCODER_B, COUNTS_PER_REV3, MOTOR3_ENCODER_INV); 58 | Encoder motor4_encoder(MOTOR4_ENCODER_A, MOTOR4_ENCODER_B, COUNTS_PER_REV4, MOTOR4_ENCODER_INV); 59 | 60 | Motor motor1_controller(PWM_FREQUENCY, PWM_BITS, MOTOR1_INV, MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); 61 | Motor motor2_controller(PWM_FREQUENCY, PWM_BITS, MOTOR2_INV, MOTOR2_PWM, MOTOR2_IN_A, MOTOR2_IN_B); 62 | Motor motor3_controller(PWM_FREQUENCY, PWM_BITS, MOTOR3_INV, MOTOR3_PWM, MOTOR3_IN_A, MOTOR3_IN_B); 63 | Motor motor4_controller(PWM_FREQUENCY, PWM_BITS, MOTOR4_INV, MOTOR4_PWM, MOTOR4_IN_A, MOTOR4_IN_B); 64 | 65 | PID motor1_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 66 | PID motor2_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 67 | PID motor3_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 68 | PID motor4_pid(PWM_MIN, PWM_MAX, K_P, K_I, K_D); 69 | 70 | Kinematics kinematics( 71 | Kinematics::LINO_BASE, 72 | MOTOR_MAX_RPM, 73 | MAX_RPM_RATIO, 74 | MOTOR_OPERATING_VOLTAGE, 75 | MOTOR_POWER_MAX_VOLTAGE, 76 | WHEEL_DIAMETER, 77 | LR_WHEELS_DISTANCE 78 | ); 79 | 80 | Odometry odometry; 81 | IMU imu; 82 | MAG mag; 83 | unsigned total_motors = 4; 84 | 85 | void setup() 86 | { 87 | #ifdef BOARD_INIT // board specific setup 88 | BOARD_INIT; 89 | #endif 90 | 91 | Serial.begin(BAUDRATE); 92 | pinMode(LED_PIN, OUTPUT); 93 | #ifdef SDA_PIN // specify I2C pins 94 | #ifdef ESP32 95 | Wire.begin(SDA_PIN, SCL_PIN); 96 | #else // teensy 97 | Wire.setSDA(SDA_PIN); 98 | Wire.setSCL(SCL_PIN); 99 | #endif 100 | #endif 101 | initWifis(); 102 | initOta(); 103 | 104 | imu.init(); 105 | mag.init(); 106 | initBattery(); 107 | initRange(); 108 | 109 | if(Kinematics::LINO_BASE == Kinematics::DIFFERENTIAL_DRIVE) 110 | { 111 | total_motors = 2; 112 | } 113 | motor1_encoder.getRPM(); 114 | motor2_encoder.getRPM(); 115 | motor3_encoder.getRPM(); 116 | motor4_encoder.getRPM(); 117 | } 118 | 119 | void loop() { 120 | static unsigned tk = 0; // tick 121 | const unsigned run_time = 8; // run time of each motor 122 | const unsigned cycle = run_time * total_motors; 123 | unsigned current_motor = tk / run_time % total_motors; 124 | unsigned direction = tk / cycle % 2; // 0 forward, 1 reverse 125 | const int pwm_max = (1 << PWM_BITS) - 1; 126 | static float max_rpm, stopping; 127 | 128 | digitalWrite(LED_PIN, direction ? LOW : HIGH); 129 | motor1_controller.spin((current_motor == 0) ? (direction ? -pwm_max : pwm_max) : 0); 130 | motor2_controller.spin((current_motor == 1) ? (direction ? -pwm_max : pwm_max) : 0); 131 | if(total_motors > 2){ 132 | motor3_controller.spin((current_motor == 2) ? (direction ? -pwm_max : pwm_max) : 0); 133 | motor4_controller.spin((current_motor == 3) ? (direction ? -pwm_max : pwm_max) : 0); 134 | } 135 | delay(1000); 136 | float current_rpm1 = motor1_encoder.getRPM(); 137 | float current_rpm2 = motor2_encoder.getRPM(); 138 | float current_rpm3 = motor3_encoder.getRPM(); 139 | float current_rpm4 = motor4_encoder.getRPM(); 140 | if (current_motor == 0 && tk % run_time == run_time - 1) max_rpm = current_rpm1; 141 | if (current_motor == 1 && tk % run_time == 0) stopping = current_rpm1; 142 | if (current_motor == 1 && tk % run_time == run_time - 1) max_rpm = current_rpm2; 143 | if (total_motors == 2 && current_motor == 0 && tk % run_time == 0) stopping = current_rpm2; 144 | if (current_motor == 2 && tk % run_time == 0) stopping = current_rpm2; 145 | if (current_motor == 2 && tk % run_time == run_time - 1) max_rpm = current_rpm3; 146 | if (current_motor == 3 && tk % run_time == 0) stopping = current_rpm3; 147 | if (current_motor == 3 && tk % run_time == run_time - 1) max_rpm = current_rpm4; 148 | if (total_motors == 4 && current_motor == 0 && tk % run_time == 0) stopping = current_rpm4; 149 | if (tk && tk % run_time == 0) { 150 | Kinematics::velocities max_linear = kinematics.getVelocities(max_rpm, max_rpm, max_rpm, max_rpm); 151 | Kinematics::velocities max_angular = kinematics.getVelocities(-max_rpm, max_rpm,-max_rpm, max_rpm); 152 | printf("MOTOR%d SPEED %6.2f m/s %6.2f rad/s STOP %6.3f m\n", current_motor ? current_motor : total_motors, 153 | max_linear.linear_x, max_angular.angular_z, max_linear.linear_x * stopping / max_rpm); 154 | syslog(LOG_INFO, "MOTOR%d SPEED %6.2f m/s %6.2f rad/s STOP %6.3f m\n", current_motor ? current_motor : total_motors, 155 | max_linear.linear_x, max_angular.angular_z, max_linear.linear_x * stopping / max_rpm); 156 | } 157 | printf("MOTOR%d %s RPM %8.1f %8.1f %8.1f %8.1f\n", 158 | current_motor + 1, direction ? "REV" : "FWD", 159 | current_rpm1, current_rpm2, current_rpm3, current_rpm4); 160 | syslog(LOG_INFO, "MOTOR%d %s RPM %8.1f %8.1f %8.1f %8.1f\n", 161 | current_motor + 1, direction ? "REV" : "FWD", 162 | current_rpm1, current_rpm2, current_rpm3, current_rpm4); 163 | tk++; 164 | runWifis(); 165 | runOta(); 166 | } 167 | -------------------------------------------------------------------------------- /test_sensors/platformio.ini: -------------------------------------------------------------------------------- 1 | [platformio] 2 | extra_configs = 3 | ../firmware/platformio.ini 4 | 5 | [env] 6 | lib_extra_dirs = 7 | ../firmware/lib 8 | -------------------------------------------------------------------------------- /test_sensors/src/firmware.ino: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Juan Miguel Jimeno 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. 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "config.h" 27 | #include "syslog.h" 28 | #include "motor.h" 29 | #include "kinematics.h" 30 | #include "pid.h" 31 | #include "odometry.h" 32 | #include "imu.h" 33 | #include "mag.h" 34 | #define ENCODER_USE_INTERRUPTS 35 | #define ENCODER_OPTIMIZE_INTERRUPTS 36 | #include "encoder.h" 37 | #include "battery.h" 38 | #include "range.h" 39 | #include "lidar.h" 40 | #include "wifis.h" 41 | #include "ota.h" 42 | 43 | #ifndef BAUDRATE 44 | #define BAUDRATE 115200 45 | #endif 46 | 47 | nav_msgs__msg__Odometry odom_msg; 48 | sensor_msgs__msg__Imu imu_msg; 49 | sensor_msgs__msg__MagneticField mag_msg; 50 | geometry_msgs__msg__Twist twist_msg; 51 | sensor_msgs__msg__BatteryState battery_msg; 52 | sensor_msgs__msg__Range range_msg; 53 | 54 | Odometry odometry; 55 | IMU imu; 56 | MAG mag; 57 | 58 | void setup() 59 | { 60 | #ifdef BOARD_INIT // board specific setup 61 | BOARD_INIT; 62 | #endif 63 | 64 | Serial.begin(BAUDRATE); 65 | pinMode(LED_PIN, OUTPUT); 66 | #ifdef SDA_PIN // specify I2C pins 67 | #ifdef ESP32 68 | Wire.begin(SDA_PIN, SCL_PIN); 69 | #else // teensy 70 | Wire.setSDA(SDA_PIN); 71 | Wire.setSCL(SCL_PIN); 72 | #endif 73 | #endif 74 | initWifis(); 75 | initOta(); 76 | 77 | imu.init(); 78 | mag.init(); 79 | initBattery(); 80 | initRange(); 81 | } 82 | 83 | void loop() { 84 | delay(1000); 85 | imu_msg = imu.getData(); 86 | mag_msg = mag.getData(); 87 | #ifdef MAG_BIAS 88 | const float mag_bias[3] = MAG_BIAS; 89 | mag_msg.magnetic_field.x -= mag_bias[0]; 90 | mag_msg.magnetic_field.y -= mag_bias[1]; 91 | mag_msg.magnetic_field.z -= mag_bias[2]; 92 | #endif 93 | battery_msg = getBattery(); 94 | range_msg = getRange(); 95 | printf("ACC %5.2f %5.2f %5.2f GYR %5.2f %5.2f %5.2f MAG %5.2f %5.2f %5.2f\n" 96 | " BAT %5.2fV RANGE %5.2fm\n", 97 | imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z, 98 | imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.x, 99 | mag_msg.magnetic_field.x, mag_msg.magnetic_field.y, mag_msg.magnetic_field.z, 100 | battery_msg.voltage, range_msg.range); 101 | syslog(LOG_INFO, "ACC %5.2f %5.2f %5.2f GYR %5.2f %5.2f %5.2f MAG %5.2f %5.2f %5.2f\n" 102 | " BAT %5.2fV RANGE %5.2fm\n", 103 | imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z, 104 | imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.x, 105 | mag_msg.magnetic_field.x, mag_msg.magnetic_field.y, mag_msg.magnetic_field.z, 106 | battery_msg.voltage, range_msg.range); 107 | runWifis(); 108 | runOta(); 109 | } 110 | --------------------------------------------------------------------------------