├── LICENSE ├── README.md ├── extras ├── AccessPoint2Client ├── EddieStartup.service ├── README.md └── stopService └── src ├── Kalman.h ├── build ├── encoder.h ├── identity.h ├── imu ├── imu.c └── imu.h ├── main.c ├── motordriver ├── MotorDriver.c ├── MotorDriver.h └── MotorDriver_mraa.c ├── pid.c ├── pid.h └── udp.h /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EddieBalance 2 | Eddie the Balance Bot is a self blanacing robot based on the Intel Edison and Sparkfun Blocks. 3 | 4 | This project now reflects my EddiePlus design which incorporates encoders and some new body stylings. The new 3D models and details including a builder's guide are available on thingiverse: www.thingiverse.com/thing:694969 5 | 6 | EddieBalance is currently running the 05-15 Yocto image from Intel and requires the installation of libmraa (instructions to come in due time). 7 | 8 | EddieBalance listens for data on two ports; one for gaining control and the other for sending commands. And Eddie returns data to the last IP received on the response port. 9 | 10 | * Control port: UDP 4240 11 | * Command port: UDP 4242 12 | * Response port: UDP 4243 13 | 14 | To remote control Eddie you must gain control with a "bind" process that involves: 15 | 16 | * Sending "DISCOVER" to 255.255.255.255 on port 4240 - All Eddie's on the network will respond with "DISCOVER: [unique name]". 17 | 18 | * Using the IP/name from the response send "BIND" to [ip address] on port 4240 - Eddie will respond with "BIND: OK". 19 | 20 | * Now you can send command packets (currently listed in main.c) to UDP port 4242. 21 | 22 | The bind process was a necessary evil to allow multiple Eddie robots on the same WiFi network. I've been unable to get the Edison's AccessPoint mode to work with the GPIO issue in motordriver.c and Bluetooth control is next on my list of features. 23 | 24 | The build details for the original Eddie still remain on thingiverse (but I highly recommend building a balance bot with encoders): www.thingiverse.com/thing:655423 25 | 26 | If you need help building your Eddie please contact me through thingiverse and I'll get an email back to you. 27 | -------------------------------------------------------------------------------- /extras/AccessPoint2Client: -------------------------------------------------------------------------------- 1 | systemctl stop hostapd 2 | systemctl disable hostapd 3 | systemctl enable wpa_supplicant 4 | systemctl start wpa_supplicant 5 | wpa_cli reconfigure 6 | wpa_cli select_network wlan0 7 | udhcpc -i wlan0 -------------------------------------------------------------------------------- /extras/EddieStartup.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=EddieStartup 3 | After=network-online.target 4 | Requires=network-online.target 5 | 6 | [Service] 7 | ExecStart=/home/root/EddieBalance/src/./EddieBalance 8 | Restart=always 9 | RestartSec=10s 10 | Environment=NODE_ENV=production 11 | 12 | [Install] 13 | WantedBy=multi-user.target 14 | -------------------------------------------------------------------------------- /extras/README.md: -------------------------------------------------------------------------------- 1 | # EddieBalance 2 | Eddie the Balance Bot 3 | 4 | Extras Directory 5 | 6 | EddieStartup.service is a basic service example to have Eddie automatically run at startup. To use this you must ensure the file location in the script is correct for your system and follow these instructions: 7 | 8 | Copy: EddieStartup.service to /lib/systemd/system/ 9 | 10 | Execute: systemctl daemon-reload 11 | 12 | Execute: systemctl enable EddieStartup.service 13 | 14 | When you boot the latest build of EddieBalance will run automatically. 15 | 16 | To debug or rebuild you can use the StopService script. 17 | -------------------------------------------------------------------------------- /extras/stopService: -------------------------------------------------------------------------------- 1 | systemctl stop EddieStartup.service 2 | -------------------------------------------------------------------------------- /src/Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h 19 | #define _Kalman_h 20 | 21 | /* Kalman filter variables */ 22 | double Q_kalmanangle; // Process noise variance for the accelerometer 23 | double Q_bias; // Process noise variance for the gyro bias 24 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 25 | 26 | double kalmanangle; // The kalmanangle calculated by the Kalman filter - part of the 2x1 state vector 27 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 28 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getkalmanangle to update the rate 29 | 30 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 31 | double K[2]; // Kalman gain - This is a 2x1 vector 32 | double y; // kalmanangle difference 33 | double S; // Estimate error 34 | 35 | void InitKalman() 36 | { 37 | /* We will set the variables like so, these can also be tuned by the user */ 38 | Q_kalmanangle = 0.001; 39 | Q_bias = 0.003; 40 | R_measure = 0.03; 41 | 42 | kalmanangle = 0; // Reset the kalmanangle 43 | bias = 0; // Reset bias 44 | 45 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting kalmanangle (use setkalmanangle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 46 | P[0][1] = 0; 47 | P[1][0] = 0; 48 | P[1][1] = 0; 49 | } 50 | // The kalmanangle should be in degrees and the rate should be in degrees per second and the delta time in seconds 51 | double getkalmanangle(double newkalmanangle, double newRate, double dt) 52 | { 53 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 54 | // Modified by Kristian Lauszus 55 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 56 | 57 | // Discrete Kalman filter time update equations - Time Update ("Predict") 58 | // Update xhat - Project the state ahead 59 | /* Step 1 */ 60 | rate = newRate - bias; 61 | kalmanangle += dt * rate; 62 | 63 | // Update estimation error covariance - Project the error covariance ahead 64 | /* Step 2 */ 65 | P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_kalmanangle); 66 | P[0][1] -= dt * P[1][1]; 67 | P[1][0] -= dt * P[1][1]; 68 | P[1][1] += Q_bias * dt; 69 | 70 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 71 | // Calculate Kalman gain - Compute the Kalman gain 72 | /* Step 4 */ 73 | S = P[0][0] + R_measure; 74 | /* Step 5 */ 75 | K[0] = P[0][0] / S; 76 | K[1] = P[1][0] / S; 77 | 78 | // Calculate kalmanangle and bias - Update estimate with measurement zk (newkalmanangle) 79 | /* Step 3 */ 80 | y = newkalmanangle - kalmanangle; 81 | /* Step 6 */ 82 | kalmanangle += K[0] * y; 83 | bias += K[1] * y; 84 | 85 | // Calculate estimation error covariance - Update the error covariance 86 | /* Step 7 */ 87 | P[0][0] -= K[0] * P[0][0]; 88 | P[0][1] -= K[0] * P[0][1]; 89 | P[1][0] -= K[1] * P[0][0]; 90 | P[1][1] -= K[1] * P[0][1]; 91 | 92 | return kalmanangle; 93 | }; 94 | void setkalmanangle(double newkalmanangle) { kalmanangle = newkalmanangle; }; // Used to set kalmanangle, this should be set as the starting kalmanangle 95 | double getRate() { return rate; }; // Return the unbiased rate 96 | 97 | /* These are used to tune the Kalman filter */ 98 | void setQkalmanangle(double newQ_kalmanangle) { Q_kalmanangle = newQ_kalmanangle; }; 99 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 100 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 101 | 102 | double getQkalmanangle() { return Q_kalmanangle; }; 103 | double getQbias() { return Q_bias; }; 104 | double getRmeasure() { return R_measure; }; 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /src/build: -------------------------------------------------------------------------------- 1 | gcc main.c pid.c imu/imu.c motordriver/MotorDriver.c -o EddieBalance -lmraa -lm -lpthread 2 | -------------------------------------------------------------------------------- /src/encoder.h: -------------------------------------------------------------------------------- 1 | #ifndef ENCODER_H 2 | #define ENCODER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "mraa.h" 11 | 12 | //left encoder: 183 46 13 | //right encoder: 45 44 14 | 15 | static volatile double position[ 2 ]; 16 | mraa_gpio_context encoderx[ 4 ]; 17 | 18 | int lastpins[ 4 ]; 19 | void EncoderInterruptA( void * args ) 20 | { 21 | int currentpins[ 2 ]; 22 | 23 | int change = 0; 24 | currentpins[ 0 ] = mraa_gpio_read( encoderx[ 0 ] ); 25 | currentpins[ 1 ] = mraa_gpio_read( encoderx[ 1 ] ); 26 | 27 | if( currentpins[ 0 ] != lastpins[ 0 ] ) 28 | { 29 | if( currentpins[ 0 ] > lastpins[ 0 ] ) 30 | { 31 | if( currentpins[ 1 ] ) 32 | { 33 | --change; 34 | } 35 | else 36 | { 37 | ++change; 38 | } 39 | } 40 | else 41 | { 42 | if( currentpins[ 1 ] ) 43 | { 44 | ++change; 45 | } 46 | else 47 | { 48 | --change; 49 | } 50 | } 51 | } 52 | else if( currentpins[ 1 ] != lastpins[ 1 ] ) 53 | { 54 | if( currentpins[ 1 ] > lastpins[ 1 ] ) 55 | { 56 | if( currentpins[ 0 ] ) 57 | { 58 | ++change; 59 | } 60 | else 61 | { 62 | --change; 63 | } 64 | } 65 | else 66 | { 67 | if( currentpins[ 0 ] ) 68 | { 69 | --change; 70 | } 71 | else 72 | { 73 | ++change; 74 | } 75 | } 76 | } 77 | 78 | position[ 0 ] += change; 79 | 80 | lastpins[ 0 ] = currentpins[ 0 ]; 81 | lastpins[ 1 ] = currentpins[ 1 ]; 82 | } 83 | 84 | void EncoderInterruptB( void * args ) 85 | { 86 | int currentpins[ 2 ]; 87 | 88 | int change = 0; 89 | currentpins[ 0 ] = mraa_gpio_read( encoderx[ 2 ] ); 90 | currentpins[ 1 ] = mraa_gpio_read( encoderx[ 3 ] ); 91 | 92 | if( currentpins[ 0 ] != lastpins[ 2 ] ) 93 | { 94 | if( currentpins[ 0 ] > lastpins[ 2 ] ) 95 | { 96 | if( currentpins[ 1 ] ) 97 | { 98 | ++change; 99 | } 100 | else 101 | { 102 | --change; 103 | } 104 | } 105 | else 106 | { 107 | if( currentpins[ 1 ] ) 108 | { 109 | --change; 110 | } 111 | else 112 | { 113 | ++change; 114 | } 115 | } 116 | } 117 | else if( currentpins[ 1 ] != lastpins[ 3 ] ) 118 | { 119 | if( currentpins[ 1 ] > lastpins[ 3 ] ) 120 | { 121 | if( currentpins[ 0 ] ) 122 | { 123 | --change; 124 | } 125 | else 126 | { 127 | ++change; 128 | } 129 | } 130 | else 131 | { 132 | if( currentpins[ 0 ] ) 133 | { 134 | ++change; 135 | } 136 | else 137 | { 138 | --change; 139 | } 140 | } 141 | } 142 | 143 | position[ 1 ] += change; 144 | lastpins[ 2 ] = currentpins[ 0 ]; 145 | lastpins[ 3 ] = currentpins[ 1 ]; 146 | } 147 | 148 | void ResetEncoders() 149 | { 150 | position[ 0 ] = position[ 1 ] = 0; 151 | } 152 | 153 | double GetEncoder( ) 154 | { 155 | return (position[ 0 ] + position[ 1 ]) / 2; 156 | } 157 | 158 | void GetEncoders( double * temp ) 159 | { 160 | temp[0] = position[ 0 ]; 161 | temp[1] = position[ 1 ]; 162 | } 163 | 164 | void GetEncoderChange( double * temp ) 165 | { 166 | temp[0] = position[ 0 ]; 167 | temp[1] = position[ 1 ]; 168 | position[ 0 ] = position[ 1 ] = 0; 169 | } 170 | 171 | void EncoderAddPos2( double distance1, double distance2 ) 172 | { 173 | position[0] += distance1; 174 | position[1] += distance2; 175 | } 176 | 177 | void EncoderAddPos( double distance ) 178 | { 179 | position[0] += distance; 180 | position[1] += distance; 181 | } 182 | 183 | void initEncoders( int a, int b, int c, int d ) 184 | { 185 | mraa_init(); 186 | position[0] = position[1] = 0; 187 | encoderx[ 0 ] = mraa_gpio_init_raw( a ); 188 | encoderx[ 1 ] = mraa_gpio_init_raw( b ); 189 | encoderx[ 2 ] = mraa_gpio_init_raw( c ); 190 | encoderx[ 3 ] = mraa_gpio_init_raw( d ); 191 | 192 | mraa_gpio_dir( encoderx[ 0 ], MRAA_GPIO_IN ); 193 | mraa_gpio_isr( encoderx[ 0 ], MRAA_GPIO_EDGE_BOTH, &EncoderInterruptA, NULL ); 194 | mraa_gpio_dir( encoderx[ 1 ], MRAA_GPIO_IN); 195 | mraa_gpio_isr( encoderx[ 1 ], MRAA_GPIO_EDGE_BOTH, &EncoderInterruptA, NULL ); 196 | mraa_gpio_dir( encoderx[ 2 ], MRAA_GPIO_IN ); 197 | mraa_gpio_isr( encoderx[ 2 ], MRAA_GPIO_EDGE_BOTH, &EncoderInterruptB, NULL ); 198 | mraa_gpio_dir( encoderx[ 3 ], MRAA_GPIO_IN); 199 | mraa_gpio_isr( encoderx[ 3 ], MRAA_GPIO_EDGE_BOTH, &EncoderInterruptB, NULL ); 200 | 201 | } 202 | 203 | void CloseEncoder() 204 | { 205 | mraa_gpio_close( encoderx[ 0 ] ); 206 | mraa_gpio_close( encoderx[ 1 ] ); 207 | mraa_gpio_close( encoderx[ 2 ] ); 208 | mraa_gpio_close( encoderx[ 3 ] ); 209 | } 210 | 211 | #endif -------------------------------------------------------------------------------- /src/identity.h: -------------------------------------------------------------------------------- 1 | #ifndef IDENTITY_H 2 | #define IDENTITY_H 3 | 4 | #include 5 | 6 | char thisEddieName[64]; 7 | 8 | int initName(); 9 | void setName( char * p_name ); 10 | void initIdentity(); 11 | unsigned short checksum( const char * key, int len ); 12 | 13 | int initName() 14 | { 15 | char responseBuff[64] = {0}; 16 | int pipe = open( "/etc/EddieBalance.conf", O_RDONLY ); 17 | if ( pipe == -1 ) 18 | { 19 | //DEBUG: printf("Eddie::initName: Open file for reading failed.\r\n"); 20 | return 0; 21 | } 22 | if ( read( pipe, thisEddieName, sizeof(thisEddieName) ) ) 23 | { 24 | //DEBUG: printf( "Eddie::initName: set name to: %s\r\n", thisEddieName ); 25 | return 1; 26 | } 27 | return 0; 28 | } 29 | 30 | void setName( char * p_name ) 31 | { 32 | int pipe = open( "/etc/EddieBalance.conf", O_WRONLY | O_CREAT ); 33 | if ( pipe == -1 ) 34 | { 35 | //DEBUG: printf("Eddie::setName: Open file for writing failed.\r\n"); 36 | return; 37 | } 38 | if ( write( pipe, p_name, sizeof(thisEddieName) ) ) 39 | { 40 | strcpy( thisEddieName, p_name ); 41 | } 42 | } 43 | 44 | void initIdentity() 45 | { 46 | if ( !initName() ) //If initName fails no name has been saved. Generate a unique ID to append to default name. 47 | { 48 | int thisEddieID; 49 | char responseBuff[256] = {0}; 50 | FILE * pipe = popen( "ls /dev/disk/by-uuid\n", "r" ); 51 | if (pipe == NULL ) 52 | { 53 | printf("Eddie::initIdentity: Invoking command failed.\r\n"); 54 | } 55 | while( fgets( responseBuff + strlen(responseBuff), sizeof(responseBuff) - strlen(responseBuff), pipe ) != NULL ); 56 | pclose( pipe ); 57 | 58 | thisEddieID = checksum( responseBuff, strlen( responseBuff ) ); 59 | sprintf( thisEddieName, "EddieBalance[%04x]", thisEddieID ); 60 | } 61 | } 62 | 63 | unsigned short checksum( const char * key, int len ) 64 | { 65 | unsigned short crc = 0xFFFF; 66 | int i, j; 67 | 68 | for( i = 0; i < len; ++i ) 69 | { 70 | char data = key[i]; 71 | crc = crc ^ ( data << 8 ); 72 | for ( j = 0; j < 8; ++j ) 73 | { 74 | if ( ( crc & 0x8000 ) != 0 ) 75 | { 76 | crc = ( crc << 1 ) ^ 0x1021; 77 | } 78 | else 79 | { 80 | crc <<= 1; 81 | } 82 | } 83 | } 84 | return crc; 85 | } 86 | 87 | #endif //--IDENTITY_H 88 | -------------------------------------------------------------------------------- /src/imu/imu.c: -------------------------------------------------------------------------------- 1 | #include "imu.h" 2 | #include "mraa.h" 3 | #include "math.h" 4 | 5 | #define MAX_BUFFER_LENGTH 512 6 | #define GYRO_I2C_ADDR 0x6B 7 | #define XM_I2C_ADDR 0x1D 8 | 9 | uint8_t rx_tx_buf[MAX_BUFFER_LENGTH]; 10 | mraa_i2c_context i2c; 11 | 12 | float temp; 13 | float mx, my, mz; 14 | float ax, ay, az; 15 | float gx, gy, gz; 16 | 17 | float i2cHeading, i2cPitch, i2cRoll; 18 | 19 | void sendi2c(unsigned int address, unsigned int reg, unsigned char tosend) 20 | { 21 | mraa_i2c_address(i2c, address); 22 | rx_tx_buf[0] = reg; 23 | rx_tx_buf[1] = tosend; 24 | mraa_i2c_write(i2c, rx_tx_buf, 2); 25 | } 26 | 27 | char readi2c(int address, int reg, int count) 28 | { 29 | int i = 0; 30 | mraa_i2c_address(i2c, address); 31 | for (i = 0; i < count; i++) 32 | { 33 | rx_tx_buf[i] = mraa_i2c_read_byte_data(i2c, reg + i); 34 | } 35 | if(count == 1)return rx_tx_buf[0]; 36 | return 0; 37 | } 38 | 39 | void readGyro() 40 | { 41 | readi2c(GYRO_I2C_ADDR, OUT_X_L_G, 6); // Read 6 bytes, beginning at OUT_X_L_G 42 | float tgx = (float)((short)(rx_tx_buf[1] << 8) | rx_tx_buf[0]) * (500.0 /*dps*/ / 32768.0); 43 | float tgy = (float)((short)(rx_tx_buf[3] << 8) | rx_tx_buf[2]) * (500.0 /*dps*/ / 32768.0); 44 | float tgz = (float)((short)(rx_tx_buf[5] << 8) | rx_tx_buf[4]) * (500.0 /*dps*/ / 32768.0); 45 | 46 | gx = tgz; 47 | gy = tgy; 48 | gz = tgx; 49 | } 50 | 51 | void readAccel() 52 | { 53 | readi2c(XM_I2C_ADDR, OUT_X_L_A, 6); // Read 6 bytes, beginning at OUT_X_L_G 54 | float tax = (float)((short)(rx_tx_buf[1] << 8) | rx_tx_buf[0]) * 0.00006103515625; // Store x-axis values into gx 55 | float tay = (float)((short)(rx_tx_buf[3] << 8) | rx_tx_buf[2]) * 0.00006103515625; // Store y-axis values into gy 56 | float taz = (float)((short)(rx_tx_buf[5] << 8) | rx_tx_buf[4]) * 0.00006103515625; // Store z-axis values into gz 57 | 58 | ax = taz; 59 | ay = tay; 60 | az = tax; 61 | 62 | readi2c(XM_I2C_ADDR, OUT_TEMP_L_XM, 2); 63 | temp = (float)((short)(rx_tx_buf[1] << 8) | rx_tx_buf[0]); 64 | } 65 | 66 | void readMag() 67 | { 68 | readi2c(XM_I2C_ADDR, OUT_X_L_M, 6); // Read 6 bytes, beginning at OUT_X_L_G 69 | float tmx = (float)((short)(rx_tx_buf[1] << 8) | rx_tx_buf[0]) * 0.00006103515625; // Store x-axis values into gx 70 | float tmy = (float)((short)(rx_tx_buf[3] << 8) | rx_tx_buf[2]) * 0.00006103515625; // Store y-axis values into gy 71 | float tmz = (float)((short)(rx_tx_buf[5] << 8) | rx_tx_buf[4]) * 0.00006103515625; // Store z-axis values into gz 72 | 73 | mx = tmz; 74 | my = tmy; 75 | mz = tmx; 76 | } 77 | 78 | void readSensors() 79 | { 80 | readGyro(); 81 | readAccel(); 82 | readMag(); 83 | } 84 | 85 | void getOrientation() 86 | { 87 | readSensors(); 88 | 89 | float const PI_F = 3.14159265F; 90 | 91 | // i2cRoll: Rotation around the X-axis. -180 <= i2cRoll <= 180 92 | // a positive i2cRoll angle is defined to be a clockwise rotation about the positive X-axis 93 | // y 94 | // i2cRoll = atan2(---) 95 | // z 96 | // where: y, z are returned value from accelerometer sensor 97 | i2cRoll = (float)atan2(ay, az); 98 | 99 | // i2cPitch: Rotation around the Y-axis. -180 <= i2cRoll <= 180 100 | // a positive i2cPitch angle is defined to be a clockwise rotation about the positive Y-axis 101 | // -x 102 | // i2cPitch = atan(-------------------------------) 103 | // y * sin(i2cRoll) + z * cos(i2cRoll) 104 | // where: x, y, z are returned value from accelerometer sensor 105 | if (ay * sin(i2cRoll) + az * cos(i2cRoll) == 0) i2cPitch = ax > 0 ? (PI_F / 2) : (-PI_F / 2); 106 | else i2cPitch = (float)atan(-ax / (ay * sin(i2cRoll) + az * cos(i2cRoll))); 107 | 108 | // i2cHeading: Rotation around the Z-axis. -180 <= i2cRoll <= 180 109 | // a positive i2cHeading angle is defined to be a clockwise rotation about the positive Z-axis 110 | // z * sin(i2cRoll) - y * cos(i2cRoll) 111 | // i2cHeading = atan2(--------------------------------------------------------------------------) 112 | // x * cos(i2cPitch) + y * sin(i2cPitch) * sin(i2cRoll) + z * sin(i2cPitch) * cos(i2cRoll)) 113 | // where: x, y, z are returned value from magnetometer sensor 114 | // i2cHeading = (float)atan2(mz * sin(i2cRoll) - my * cos(i2cRoll), mx * cos(i2cPitch) + my * sin(i2cPitch) * sin(i2cRoll) + mz * sin(i2cPitch) * cos(i2cRoll)); 115 | 116 | // Convert angular data to degree 117 | i2cRoll = - i2cRoll * 180.0 / PI_F; 118 | i2cPitch = i2cPitch * 180.0 / PI_F; 119 | i2cHeading = - i2cHeading * 180.0 / PI_F; 120 | 121 | } 122 | 123 | void imuinit() 124 | { 125 | mraa_init(); 126 | i2c = mraa_i2c_init(1); 127 | 128 | sendi2c( GYRO_I2C_ADDR, FIFO_CTRL_REG_G, 0 ); 129 | sendi2c( GYRO_I2C_ADDR, CTRL_REG1_G, 0x0F ); //Normal mode, enable all axes //0xFF ); //??unknown config?? 130 | sendi2c( GYRO_I2C_ADDR, CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency 131 | sendi2c( GYRO_I2C_ADDR, CTRL_REG4_G, 0x10 ); // Set scale to 500 dps 132 | sendi2c( GYRO_I2C_ADDR, CTRL_REG5_G, 0x00 ); // FIFO Disabled, HPF Disabled 133 | 134 | sendi2c( XM_I2C_ADDR, FIFO_CTRL_REG, 0 ); 135 | sendi2c( XM_I2C_ADDR, CTRL_REG1_XM, 0xFF ); 136 | sendi2c( XM_I2C_ADDR, CTRL_REG2_XM, 0x00); //Set scale +/-2g 137 | sendi2c( XM_I2C_ADDR, CTRL_REG4_XM, 0x30 ); 138 | 139 | sendi2c( XM_I2C_ADDR, CTRL_REG5_XM, 0x94); 140 | sendi2c( XM_I2C_ADDR, CTRL_REG6_XM, 0x00); 141 | sendi2c( XM_I2C_ADDR, CTRL_REG7_XM, 0x00); 142 | /* 143 | return; 144 | 145 | while(1) 146 | { 147 | readGyro(); 148 | readAccel(); 149 | readMag(); 150 | 151 | printf("gx:%6.2f gy:%6.2f gz:%6.2f ax:%6.2f ay:%6.2f az:%6.2f mx:%6.2f my:%6.2f mz:%6.2f temp:%0.0f\n",gx,gy,gz,ax,ay,az,mx,my,mz,temp); 152 | usleep(20000); 153 | } 154 | */ 155 | } 156 | -------------------------------------------------------------------------------- /src/imu/imu.h: -------------------------------------------------------------------------------- 1 | #ifndef I2CTESTH 2 | #define I2CTESTH 3 | 4 | //////////////////////////// 5 | // LSM9DS0 Gyro Registers // 6 | //////////////////////////// 7 | #define WHO_AM_I_G 0x0F 8 | #define CTRL_REG1_G 0x20 9 | #define CTRL_REG2_G 0x21 10 | #define CTRL_REG3_G 0x22 11 | #define CTRL_REG4_G 0x23 12 | #define CTRL_REG5_G 0x24 13 | #define REFERENCE_G 0x25 14 | #define STATUS_REG_G 0x27 15 | #define OUT_X_L_G 0x28 16 | #define OUT_X_H_G 0x29 17 | #define OUT_Y_L_G 0x2A 18 | #define OUT_Y_H_G 0x2B 19 | #define OUT_Z_L_G 0x2C 20 | #define OUT_Z_H_G 0x2D 21 | #define FIFO_CTRL_REG_G 0x2E 22 | #define FIFO_SRC_REG_G 0x2F 23 | #define INT1_CFG_G 0x30 24 | #define INT1_SRC_G 0x31 25 | #define INT1_THS_XH_G 0x32 26 | #define INT1_THS_XL_G 0x33 27 | #define INT1_THS_YH_G 0x34 28 | #define INT1_THS_YL_G 0x35 29 | #define INT1_THS_ZH_G 0x36 30 | #define INT1_THS_ZL_G 0x37 31 | #define INT1_DURATION_G 0x38 32 | 33 | ////////////////////////////////////////// 34 | // LSM9DS0 Accel/Magneto (XM) Registers // 35 | ////////////////////////////////////////// 36 | #define OUT_TEMP_L_XM 0x05 37 | #define OUT_TEMP_H_XM 0x06 38 | #define STATUS_REG_M 0x07 39 | #define OUT_X_L_M 0x08 40 | #define OUT_X_H_M 0x09 41 | #define OUT_Y_L_M 0x0A 42 | #define OUT_Y_H_M 0x0B 43 | #define OUT_Z_L_M 0x0C 44 | #define OUT_Z_H_M 0x0D 45 | #define WHO_AM_I_XM 0x0F 46 | #define INT_CTRL_REG_M 0x12 47 | #define INT_SRC_REG_M 0x13 48 | #define INT_THS_L_M 0x14 49 | #define INT_THS_H_M 0x15 50 | #define OFFSET_X_L_M 0x16 51 | #define OFFSET_X_H_M 0x17 52 | #define OFFSET_Y_L_M 0x18 53 | #define OFFSET_Y_H_M 0x19 54 | #define OFFSET_Z_L_M 0x1A 55 | #define OFFSET_Z_H_M 0x1B 56 | #define REFERENCE_X 0x1C 57 | #define REFERENCE_Y 0x1D 58 | #define REFERENCE_Z 0x1E 59 | #define CTRL_REG0_XM 0x1F 60 | #define CTRL_REG1_XM 0x20 61 | #define CTRL_REG2_XM 0x21 62 | #define CTRL_REG3_XM 0x22 63 | #define CTRL_REG4_XM 0x23 64 | #define CTRL_REG5_XM 0x24 65 | #define CTRL_REG6_XM 0x25 66 | #define CTRL_REG7_XM 0x26 67 | #define STATUS_REG_A 0x27 68 | #define OUT_X_L_A 0x28 69 | #define OUT_X_H_A 0x29 70 | #define OUT_Y_L_A 0x2A 71 | #define OUT_Y_H_A 0x2B 72 | #define OUT_Z_L_A 0x2C 73 | #define OUT_Z_H_A 0x2D 74 | #define FIFO_CTRL_REG 0x2E 75 | #define FIFO_SRC_REG 0x2F 76 | #define INT_GEN_1_REG 0x30 77 | #define INT_GEN_1_SRC 0x31 78 | #define INT_GEN_1_THS 0x32 79 | #define INT_GEN_1_DURATION 0x33 80 | #define INT_GEN_2_REG 0x34 81 | #define INT_GEN_2_SRC 0x35 82 | #define INT_GEN_2_THS 0x36 83 | #define INT_GEN_2_DURATION 0x37 84 | #define CLICK_CFG 0x38 85 | #define CLICK_SRC 0x39 86 | #define CLICK_THS 0x3A 87 | #define TIME_LIMIT 0x3B 88 | #define TIME_LATENCY 0x3C 89 | #define TIME_WINDOW 0x3D 90 | #define ACT_THS 0x3E 91 | #define ACT_DUR 0x3F 92 | 93 | extern float temp; 94 | extern float mx,my,mz; 95 | extern float ax,ay,az; 96 | extern float gx,gy,gz; 97 | extern float i2cHeading,i2cPitch,i2cRoll; 98 | 99 | void imuinit(); 100 | void readSensors(); 101 | void getOrientation(); 102 | 103 | #endif 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /src/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | Eddie the balance bot. Copyright (C) 2015 Renee L. Glinski. All rights reserved. 3 | 4 | This software may be distributed and modified under the terms of the GNU 5 | General Public License version 2 (GPL2) as published by the Free Software 6 | Foundation and appearing in the file LICENSE included in the packaging of 7 | this file. Please note that GPL2 Section 2[b] requires that all works based 8 | on this software must also be made publicly available under the terms of 9 | the GPL2 ("Copyleft"). 10 | */ 11 | 12 | #include 13 | #include 14 | #include //print function 15 | #include 16 | #include 17 | #include 18 | 19 | #include "mraa.h" 20 | 21 | #include "encoder.h" 22 | #include "identity.h" 23 | #include "imu/imu.h" 24 | #include "Kalman.h" 25 | #include "motordriver/MotorDriver.h" 26 | #include "pid.h" 27 | #include "udp.h" 28 | 29 | //#define DISABLE_MOTORS 30 | 31 | static double last_gy_ms; 32 | static double last_PID_ms; 33 | double current_milliseconds() 34 | { 35 | struct timeval c_time; 36 | gettimeofday(&c_time, NULL); 37 | double milliseconds = c_time.tv_sec * 1000 + c_time.tv_usec / 1000; 38 | return milliseconds; 39 | } 40 | 41 | enum 42 | { 43 | CONSOLE = 0, 44 | UDP //Will send all print() calls to UDP port 4243, all printf() will still go to console (:) 45 | }; 46 | int outputto = UDP; //Change to fit current need. 47 | 48 | int Running = 1; 49 | int inFalloverState = 0; //Used to flag when Eddie has fallen over and disables motors 50 | int inRunAwayState = 0; 51 | int inSteadyState = 0; //Used to flag how long Eddie is being held upright in a steady state and will enable motors 52 | int StreamData = 0; 53 | 54 | PID_t pitchPID[2]; //PID Controllers for pitch angle 55 | float pitchPIDoutput[2] = {0}; 56 | float pidP_P_GAIN, pidP_I_GAIN, pidP_D_GAIN, pidP_I_LIMIT, pidP_EMA_SAMPLES; 57 | 58 | PID_t speedPID[2]; //PID Controllers for wheel speed 59 | float speedPIDoutput[2] = {0}; 60 | float pidS_P_GAIN, pidS_I_GAIN, pidS_D_GAIN, pidS_I_LIMIT, pidS_EMA_SAMPLES; 61 | 62 | float filteredPitch; 63 | float filteredRoll; 64 | 65 | float driveTrim = 0; 66 | float turnTrim = 0; 67 | double smoothedDriveTrim = 0; 68 | 69 | /* print() function used to handle data output 70 | * Current implementation will check output mode and direct data accordingly 71 | */ 72 | int print(const char *format, ...) 73 | { 74 | static char buffer[2048]; 75 | 76 | va_list arg; 77 | int len; 78 | 79 | va_start(arg, format); 80 | len = vsprintf(buffer, format, arg); 81 | va_end(arg); 82 | 83 | if( len <= 0 ) return len; 84 | 85 | switch( outputto ) 86 | { 87 | case CONSOLE: 88 | printf("%s", buffer); 89 | break; 90 | case UDP: 91 | UDPBindSend(buffer, len); 92 | break; 93 | } 94 | 95 | return len; 96 | } 97 | 98 | // Define the exit signal handler 99 | void signal_callback_handler(int signum) 100 | { 101 | print("Exiting program; Caught signal %d\r\n", signum); 102 | Running = 0; 103 | } 104 | 105 | /* Incoming UDP Control Packet handler */ 106 | void UDP_Control_Handler( char * p_udpin ) 107 | { 108 | //DEBUG: printf( "UDP Control Packet Received: %s\r\n", p_udpin ); 109 | 110 | char response[128] = {0}; 111 | 112 | if ( memcmp( p_udpin, "DISCOVER", 8 ) == 0 ) 113 | { 114 | sprintf( response, "DISCOVER: %s", thisEddieName ); 115 | } 116 | else if ( strncmp( p_udpin, "SETNAME", 7 ) == 0 ) 117 | { 118 | setName( &p_udpin[7] ); 119 | sprintf( response, "SETNAME: %s", thisEddieName ); 120 | } 121 | else if ( memcmp( p_udpin, "BIND", 4 ) == 0 ) 122 | { 123 | setCommandBindAddress(); 124 | sprintf( response, "BIND: OK" ); 125 | } 126 | 127 | UDPCtrlSend( response ); 128 | } 129 | 130 | /* Incoming UDP Command Packet handler: 131 | * 132 | * DRIVE[value] = + is Forwards, - is Reverse, 0.0 is IDLE 133 | * TURN[value] = + is Right, - is Left, 0.0 is STRAIGHT 134 | * 135 | * SETPIDS = Changes all PIDs for speed and pitch controllers 136 | * GETPIDS = Returns all PIDs for speed and pitch controllers via UDP 137 | * 138 | * PIDP[P,I,D][value] = adjust pitch PIDs 139 | * SPID[P,I,D][value] = adjust speed PIDs 140 | * 141 | * KALQA[value] = adjust Kalman Q Angle 142 | * KALQB[value] = adjust Kalman Q Bias 143 | * KALR[value] = adjust Kalman R Measure 144 | * 145 | * STOPUDP = Will stop Eddie from sending UDP to current recipient 146 | * 147 | * STREAM[0,1] = Enable/Disable Live Data Stream 148 | * 149 | */ 150 | void UDP_Command_Handler( char * p_udpin ) 151 | { 152 | /* DRIVE commands */ 153 | if( memcmp( p_udpin, "DRIVE", 5 ) == 0 ) 154 | { 155 | driveTrim = atof( &p_udpin[5] ); 156 | } 157 | /* TURN commands */ 158 | else if( memcmp( p_udpin, "TURN", 4 ) == 0 ) 159 | { 160 | turnTrim = atof( &p_udpin[4] ); 161 | } 162 | /* Get/Set all PID quick commands*/ 163 | else if ( strncmp( p_udpin, "SETPIDS:", 8 ) == 0 ) 164 | { 165 | char * pch; 166 | 167 | pch = strtok ( &p_udpin[8], "," ); 168 | pidP_P_GAIN = atof(pch); 169 | 170 | pch = strtok (NULL, ","); 171 | pidP_I_GAIN = atof(pch); 172 | 173 | pch = strtok (NULL, ","); 174 | pidP_D_GAIN = atof(pch); 175 | 176 | pch = strtok (NULL, ","); 177 | pidS_P_GAIN = atof(pch); 178 | 179 | pch = strtok (NULL, ","); 180 | pidS_I_GAIN = atof(pch); 181 | 182 | pch = strtok (NULL, ","); 183 | pidS_D_GAIN = atof(pch); 184 | } 185 | else if ( strncmp( p_udpin, "GETPIDS", 7 ) == 0 ) 186 | { 187 | print( "CURRENTPIDS:%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f\r\n", pidP_P_GAIN, pidP_I_GAIN, pidP_D_GAIN, pidS_P_GAIN, pidS_I_GAIN, pidS_D_GAIN ); 188 | } 189 | /* Individual Pitch PID Controller commands */ 190 | else if ( strncmp( p_udpin, "PPIDP", 5 ) == 0 ) 191 | { 192 | float newGain = 0; 193 | newGain = atof( &p_udpin[5] ); 194 | print( "New Pitch PID P Gain Received: Changing %0.3f to %0.3f\r\n", pidP_P_GAIN, newGain ); 195 | pidP_P_GAIN = newGain; 196 | } 197 | else if ( strncmp( p_udpin, "PPIDI", 5 ) == 0 ) 198 | { 199 | float newGain = 0; 200 | newGain = atof( &p_udpin[5] ); 201 | print( "New Pitch PID I Gain Received: Changing %0.3f to %0.3f\r\n", pidP_I_GAIN, newGain ); 202 | pidP_I_GAIN = newGain; 203 | } 204 | else if ( strncmp( p_udpin, "PPIDD", 5 ) == 0 ) 205 | { 206 | float newGain = 0; 207 | newGain = atof( &p_udpin[5] ); 208 | print( "New Pitch PID D Gain Received: Changing %0.3f to %0.3f\r\n", pidP_D_GAIN, newGain ); 209 | pidP_D_GAIN = newGain; 210 | } 211 | /* Individual Speed PID Controller commands*/ 212 | else if ( strncmp( p_udpin, "SPIDP", 5 ) == 0 ) 213 | { 214 | float newGain = 0; 215 | newGain = atof( &p_udpin[5] ); 216 | print( "New Speed PID P Gain Received: Changing %0.3f to %0.3f\r\n", pidS_P_GAIN, newGain ); 217 | pidS_P_GAIN = newGain; 218 | } 219 | else if ( strncmp( p_udpin, "SPIDI", 5 ) == 0 ) 220 | { 221 | float newGain = 0; 222 | newGain = atof( &p_udpin[5] ); 223 | print( "New Speed PID I Gain Received: Changing %0.3f to %0.3f\r\n", pidS_I_GAIN, newGain ); 224 | pidS_I_GAIN = newGain; 225 | } 226 | else if ( strncmp( p_udpin, "SPIDD", 5 ) == 0 ) 227 | { 228 | float newGain = 0; 229 | newGain = atof( &p_udpin[5] ); 230 | print( "New Speed PID D Gain Received: Changing %0.3f to %0.3f\r\n", pidS_D_GAIN, newGain ); 231 | pidS_D_GAIN = newGain; 232 | } 233 | /* Pitch Kalman filter tuning commands */ 234 | else if ( strncmp( p_udpin, "KALQA", 4 ) == 0 ) 235 | { 236 | float newGain = 0; 237 | newGain = atof( &p_udpin[4] ); 238 | print( "Setting Kalman Q Angle to: %0.4f\r\n", newGain ); 239 | setQkalmanangle( newGain ); 240 | } 241 | else if ( strncmp( p_udpin, "KALQB", 4 ) == 0 ) 242 | { 243 | float newGain = 0; 244 | newGain = atof( &p_udpin[4] ); 245 | print( "Setting Kalman Q Bias to: %0.4f\r\n", newGain ); 246 | setQbias( newGain ); 247 | } 248 | else if ( strncmp( p_udpin, "KALR", 4 ) == 0 ) 249 | { 250 | float newGain = 0; 251 | newGain = atof( &p_udpin[4] ); 252 | print( "Setting Kalman R Measure to: %0.4f\r\n", newGain ); 253 | setRmeasure( newGain ); 254 | } 255 | /* UDP Hangup command */ 256 | else if ( strncmp( p_udpin, "STOPUDP", 7 ) == 0 ) 257 | { 258 | UDPCloseTX(); 259 | } 260 | /* Enable/Disable live data stream */ 261 | else if ( strncmp( p_udpin, "STREAM1", 7 ) == 0 ) 262 | { 263 | StreamData = 1; 264 | } 265 | else if ( strncmp( p_udpin, "STREAM0", 7 ) == 0 ) 266 | { 267 | StreamData = 0; 268 | } 269 | } 270 | 271 | int main(int argc, char **argv) 272 | { 273 | //Register signal and signal handler 274 | signal(SIGINT, signal_callback_handler); 275 | 276 | //Init UDP with callbacks and pointer to run status 277 | initUDP( &UDP_Command_Handler, &UDP_Control_Handler, &Running ); 278 | 279 | print("Eddie starting...\r\n"); 280 | 281 | initIdentity(); 282 | 283 | double EncoderPos[2] = {0}; 284 | 285 | initEncoders( 183, 46, 45, 44 ); 286 | print("Encoders activated.\r\n"); 287 | 288 | imuinit(); 289 | print("IMU Started.\r\n"); 290 | 291 | float kalmanAngle; 292 | InitKalman(); 293 | 294 | #ifndef DISABLE_MOTORS 295 | print( "Starting motor driver (and resetting wireless) please be patient..\r\n" ); 296 | if ( motor_driver_enable() < 1 ) 297 | { 298 | print("Startup Failed; Error starting motor driver.\r\n"); 299 | motor_driver_disable(); 300 | return -1; 301 | } 302 | print("Motor Driver Started.\r\n"); 303 | #endif 304 | 305 | print("Eddie is starting the UDP network thread..\r\n"); 306 | pthread_create( &udplistenerThread, NULL, &udplistener_Thread, NULL ); 307 | 308 | print( "Eddie is Starting PID controllers\r\n" ); 309 | /*Set default PID values and init pitchPID controllers*/ 310 | pidP_P_GAIN = PIDP_P_GAIN; pidP_I_GAIN = PIDP_I_GAIN; pidP_D_GAIN = PIDP_D_GAIN; pidP_I_LIMIT = PIDP_I_LIMIT; pidP_EMA_SAMPLES = PIDP_EMA_SAMPLES; 311 | PIDinit( &pitchPID[0], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES ); 312 | PIDinit( &pitchPID[1], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES ); 313 | 314 | /*Set default values and init speedPID controllers*/ 315 | pidS_P_GAIN = PIDS_P_GAIN; pidS_I_GAIN = PIDS_I_GAIN; pidS_D_GAIN = PIDS_D_GAIN; pidS_I_LIMIT = PIDS_I_LIMIT; pidS_EMA_SAMPLES = PIDS_EMA_SAMPLES; 316 | PIDinit( &speedPID[0], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES ); 317 | PIDinit( &speedPID[1], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES ); 318 | 319 | //Get estimate of starting angle and specify complementary filter and kalman filter start angles 320 | getOrientation(); 321 | kalmanAngle = filteredPitch = i2cPitch; 322 | setkalmanangle( filteredPitch ); 323 | filteredRoll = i2cRoll; 324 | 325 | print( "Eddie startup complete. Hold me upright to begin\r\n" ); 326 | 327 | double gy_scale = 0.01; 328 | last_PID_ms = last_gy_ms = current_milliseconds(); 329 | 330 | while(Running) 331 | { 332 | GetEncoders( EncoderPos ); 333 | 334 | if( fabs(GetEncoder()) > 2000 && !inRunAwayState ) 335 | { 336 | print( "Help! I'm running and not moving.\r\n"); 337 | ResetEncoders(); 338 | inRunAwayState = 1; 339 | } 340 | 341 | /*Read IMU and calculate rough angle estimates*/ 342 | getOrientation(); 343 | 344 | /*Calculate time since last IMU reading and determine gyro scale (dt)*/ 345 | gy_scale = ( current_milliseconds() - last_gy_ms ) / 1000.0f; 346 | 347 | last_gy_ms = current_milliseconds(); 348 | 349 | /*Complementary filters to smooth rough pitch and roll estimates*/ 350 | filteredPitch = 0.995 * ( filteredPitch + ( gy * gy_scale ) ) + ( 0.005 * i2cPitch ); 351 | filteredRoll = 0.98 * ( filteredRoll + ( gx * gy_scale ) ) + ( 0.02 * i2cRoll ); 352 | 353 | /*Kalman filter for most accurate pitch estimates*/ 354 | kalmanAngle = -getkalmanangle(filteredPitch, gy, gy_scale /*dt*/); 355 | 356 | /* Monitor angles to determine if Eddie has fallen too far... or if Eddie has been returned upright*/ 357 | if ( ( inRunAwayState || ( fabs( kalmanAngle ) > 50 || fabs( filteredRoll ) > 45 ) ) && !inFalloverState ) 358 | { 359 | #ifndef DISABLE_MOTORS 360 | motor_driver_standby(1); 361 | #endif 362 | inFalloverState = 1; 363 | print( "Help! I've fallen over and I can't get up =)\r\n"); 364 | } 365 | else if ( fabs( kalmanAngle ) < 10 && inFalloverState && fabs( filteredRoll ) < 20 ) 366 | { 367 | if ( ++inSteadyState == 100 ) 368 | { 369 | inRunAwayState = 0; 370 | inSteadyState = 0; 371 | #ifndef DISABLE_MOTORS 372 | motor_driver_standby(0); 373 | #endif 374 | inFalloverState = 0; 375 | print( "Thank you!\r\n" ); 376 | } 377 | } 378 | else 379 | { 380 | inSteadyState = 0; 381 | } 382 | 383 | if ( !inFalloverState ) 384 | { 385 | /* Drive operations */ 386 | smoothedDriveTrim = ( 0.99 * smoothedDriveTrim ) + ( 0.01 * driveTrim ); 387 | if( smoothedDriveTrim != 0 ) 388 | { 389 | EncoderAddPos(smoothedDriveTrim); //Alter encoder position to generate movement 390 | } 391 | 392 | /* Turn operations */ 393 | if( turnTrim != 0 ) 394 | { 395 | EncoderAddPos2( turnTrim, -turnTrim ); //Alter encoder positions to turn 396 | } 397 | 398 | double timenow = current_milliseconds(); 399 | 400 | speedPIDoutput[0] = PIDUpdate( 0, EncoderPos[0], timenow - last_PID_ms, &speedPID[0] );//Wheel Speed PIDs 401 | speedPIDoutput[1] = PIDUpdate( 0, EncoderPos[1], timenow - last_PID_ms, &speedPID[1] );//Wheel Speed PIDs 402 | pitchPIDoutput[0] = PIDUpdate( speedPIDoutput[0], kalmanAngle, timenow - last_PID_ms, &pitchPID[0] );//Pitch Angle PIDs 403 | pitchPIDoutput[1] = PIDUpdate( speedPIDoutput[1], kalmanAngle, timenow - last_PID_ms, &pitchPID[1] );//Pitch Angle PIDs 404 | 405 | last_PID_ms = timenow; 406 | 407 | //Limit PID output to +/-100 to match 100% motor throttle 408 | if ( pitchPIDoutput[0] > 100.0 ) pitchPIDoutput[0] = 100.0; 409 | if ( pitchPIDoutput[1] > 100.0 ) pitchPIDoutput[1] = 100.0; 410 | if ( pitchPIDoutput[0] < -100.0 ) pitchPIDoutput[0] = -100.0; 411 | if ( pitchPIDoutput[1] < -100.0 ) pitchPIDoutput[1] = -100.0; 412 | 413 | } 414 | else //We are inFalloverState 415 | { 416 | ResetEncoders(); 417 | pitchPID[0].accumulatedError = 0; 418 | pitchPID[1].accumulatedError = 0; 419 | speedPID[0].accumulatedError = 0; 420 | speedPID[1].accumulatedError = 0; 421 | driveTrim = 0; 422 | turnTrim = 0; 423 | } 424 | 425 | #ifndef DISABLE_MOTORS 426 | set_motor_speed_right( pitchPIDoutput[0] ); 427 | set_motor_speed_left( pitchPIDoutput[1] ); 428 | #endif 429 | 430 | if ( (!inFalloverState || outputto == UDP) && StreamData ) 431 | { 432 | print( "PIDout: %0.2f,%0.2f\tcompPitch: %6.2f kalPitch: %6.2f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\r\n", 433 | speedPIDoutput[0], 434 | pitchPIDoutput[0], 435 | filteredPitch, 436 | kalmanAngle, 437 | pitchPID[0].error, 438 | pitchPID[0].accumulatedError, 439 | pitchPID[0].differentialError, 440 | speedPID[0].error, 441 | speedPID[0].accumulatedError, 442 | speedPID[0].differentialError 443 | ); 444 | } 445 | 446 | } //--while(Running) 447 | 448 | print( "Eddie is cleaning up...\r\n" ); 449 | 450 | CloseEncoder(); 451 | 452 | pthread_join(udplistenerThread, NULL); 453 | print( "UDP Thread Joined..\r\n" ); 454 | 455 | #ifndef DISABLE_MOTORS 456 | motor_driver_disable(); 457 | print( "Motor Driver Disabled..\r\n" ); 458 | #endif 459 | 460 | print( "Eddie cleanup complete. Good Bye!\r\n" ); 461 | return 0; 462 | } 463 | -------------------------------------------------------------------------------- /src/motordriver/MotorDriver.c: -------------------------------------------------------------------------------- 1 | #include "MotorDriver.h" 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | int pwm0, pwm1, ain1, ain2, bin1, bin2; 9 | 10 | //Datasheet spec is 100kHz Maximum PWM switching frequency 11 | #define PWM_PERIOD 1000000 //nano seconds for 1kHz 12 | 13 | char c_system_command[128] = {0}; 14 | 15 | int motor_driver_enable() 16 | { 17 | //Setup GPIO Pins 18 | system( "echo 48 > /sys/class/gpio/export" ); 19 | system( "echo 47 > /sys/class/gpio/export" ); 20 | system( "echo 15 > /sys/class/gpio/export" ); 21 | system( "echo 14 > /sys/class/gpio/export" ); 22 | system( "echo 49 > /sys/class/gpio/export" ); 23 | system( "echo out > /sys/class/gpio/gpio48/direction" ); 24 | system( "echo out > /sys/class/gpio/gpio47/direction" ); 25 | system( "echo out > /sys/class/gpio/gpio15/direction" ); 26 | system( "echo out > /sys/class/gpio/gpio14/direction" ); 27 | system( "echo out > /sys/class/gpio/gpio49/direction" ); 28 | 29 | //Setup PWM0 (Left Motor) 30 | system( "echo mode1 > /sys/kernel/debug/gpio_debug/gpio12/current_pinmux" ); 31 | system( "echo 0 > /sys/class/pwm/pwmchip0/export" ); 32 | system( "echo 1000000 > /sys/class/pwm/pwmchip0/pwm0/period" ); //TODO: Consider using PWM_PERIOD 33 | system( "echo 1 > /sys/class/pwm/pwmchip0/pwm0/enable" ); 34 | //Setup PWM1 (Right Motor) 35 | system( "echo mode1 > /sys/kernel/debug/gpio_debug/gpio13/current_pinmux" ); 36 | system( "echo 1 > /sys/class/pwm/pwmchip0/export" ); 37 | system( "echo 1000000 > /sys/class/pwm/pwmchip0/pwm1/period" ); //TODO: Consider using PWM_PERIOD 38 | system( "echo 1 > /sys/class/pwm/pwmchip0/pwm1/enable" ); 39 | 40 | //Take H Bridge out of standby mode 41 | system( "echo 1 > /sys/class/gpio/gpio49/value" ); 42 | 43 | return 1; 44 | } 45 | 46 | void motor_driver_standby( char p_option ) 47 | { 48 | if ( p_option > 0 ) 49 | { 50 | system( "echo 0 > /sys/class/gpio/gpio49/value" ); //Enter Standby 51 | } 52 | else 53 | { 54 | system( "echo 1 > /sys/class/gpio/gpio49/value" ); //Exit Standby 55 | } 56 | } 57 | 58 | void motor_driver_disable() 59 | { 60 | motor_driver_standby(1); 61 | 62 | system( "echo 48 > /sys/class/gpio/unexport" ); 63 | system( "echo 47 > /sys/class/gpio/unexport" ); 64 | system( "echo 15 > /sys/class/gpio/unexport" ); 65 | system( "echo 14 > /sys/class/gpio/unexport" ); 66 | system( "echo 49 > /sys/class/gpio/unexport" ); 67 | 68 | system( "echo 0 > /sys/class/pwm/pwmchip0/pwm0/enable" ); 69 | system( "echo 0 > /sys/class/pwm/pwmchip0/pwm1/enable" ); 70 | system( "echo 0 > /sys/class/pwm/pwmchip0/unexport" ); 71 | system( "echo 1 > /sys/class/pwm/pwmchip0/unexport" ); 72 | } 73 | 74 | void set_motor_direction_left ( char p_direction ) 75 | { 76 | ain1 = open("/sys/class/gpio/gpio48/value", O_RDWR); 77 | ain2 = open("/sys/class/gpio/gpio47/value", O_RDWR); 78 | 79 | if ( p_direction == FORWARD ) //Left motor CCW 80 | { 81 | write(ain1, "0", sizeof(char)); //AIN1 Low 82 | write(ain2, "1", sizeof(char)); //AIN2 High 83 | } 84 | else if ( p_direction == REVERSE ) //Left motor CW 85 | { 86 | write(ain1, "1", sizeof(char)); //AIN1 High 87 | write(ain2, "0", sizeof(char)); //AIN2 Low 88 | } 89 | 90 | close(ain1); 91 | close(ain2); 92 | } 93 | void set_motor_direction_right( char p_direction ) 94 | { 95 | bin1 = open("/sys/class/gpio/gpio15/value", O_RDWR); 96 | bin2 = open("/sys/class/gpio/gpio14/value", O_RDWR); 97 | 98 | if ( p_direction == FORWARD ) //Right motor CW 99 | { 100 | write(bin1, "1", sizeof(char)); //BIN1 High 101 | write(bin2, "0", sizeof(char)); //BIN2 Low 102 | } 103 | else if ( p_direction == REVERSE ) //Right motor CCW 104 | { 105 | write(bin1, "0", sizeof(char)); //BIN1 Low 106 | write(bin2, "1", sizeof(char)); //BIN2 High 107 | } 108 | 109 | close(bin1); 110 | close(bin2); 111 | } 112 | 113 | void set_motor_speed_left ( float p_speed ) 114 | { 115 | if ( p_speed < 0.0f ) 116 | { 117 | if ( p_speed < -100.0f ) p_speed = -100.0f; 118 | set_motor_direction_left( REVERSE ); 119 | } 120 | else //if ( p_speed > 0.0f ) 121 | { 122 | if ( p_speed > 100.0f ) p_speed = 100.0f; 123 | set_motor_direction_left( FORWARD ); 124 | } 125 | 126 | pwm0 = open("/sys/class/pwm/pwmchip0/pwm0/duty_cycle", O_RDWR); 127 | int length = sprintf(c_system_command, "%d", (int)(PWM_PERIOD * fabs(p_speed/100))); 128 | write(pwm0, c_system_command, length * sizeof(char)); 129 | close(pwm0); 130 | } 131 | void set_motor_speed_right( float p_speed ) 132 | { 133 | if ( p_speed < 0.0f ) 134 | { 135 | if ( p_speed < -100.0f ) p_speed = -100.0f; 136 | set_motor_direction_right( REVERSE ); 137 | } 138 | else //if ( p_speed > 0.0f ) 139 | { 140 | if ( p_speed > 100.0f ) p_speed = 100.0f; 141 | set_motor_direction_right( FORWARD ); 142 | } 143 | 144 | pwm1 = open("/sys/class/pwm/pwmchip0/pwm1/duty_cycle", O_RDWR); 145 | int length = sprintf(c_system_command, "%d", (int)(PWM_PERIOD * fabs(p_speed/100))); 146 | write(pwm1, c_system_command, length * sizeof(char)); 147 | close(pwm1); 148 | } 149 | -------------------------------------------------------------------------------- /src/motordriver/MotorDriver.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTORDRIVER 2 | #define MOTORDRIVER 3 | 4 | /* 5 | #define PWM0 "GP12" //Left Motor 6 | #define PWM1 "GP13" 7 | #define AIN1 "GP48" //Left Motor 8 | #define AIN2 "GP47" //Left Motor 9 | #define BIN1 "GP15" 10 | #define BIN2 "GP14" 11 | #define STBY "GP49" //Low Standby Endable 12 | */ 13 | 14 | //CCW Drive = IN1 Low , IN2 High, PWM speed 15 | //CW Drive = IN1 High, IN2 Low , PWM speed 16 | //PWM Low will enable breaking 17 | 18 | //http://www.emutexlabs.com/project/215-intel-edison-gpio-pin-multiplexing-guide 19 | //https://www.sparkfun.com/datasheets/Robotics/TB6612FNG.pdf 20 | //https://www.sparkfun.com/products/13043 21 | 22 | #define STOP 2 23 | #define FORWARD 1 24 | #define REVERSE 0 25 | 26 | int motor_driver_enable(); 27 | void motor_driver_disable(); 28 | 29 | void set_motor_direction_left ( char p_direction ); 30 | void set_motor_direction_right( char p_direction ); 31 | 32 | void set_motor_speed_left ( float p_speed ); 33 | void set_motor_speed_right( float p_speed ); 34 | 35 | #endif //MOTORDRIVER 36 | -------------------------------------------------------------------------------- /src/motordriver/MotorDriver_mraa.c: -------------------------------------------------------------------------------- 1 | #include "MotorDriver.h" 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "mraa.h" 9 | 10 | mraa_gpio_context gpio_ain1; 11 | mraa_gpio_context gpio_ain2; 12 | mraa_gpio_context gpio_bin1; 13 | mraa_gpio_context gpio_bin2; 14 | mraa_gpio_context gpio_stby; 15 | mraa_pwm_context pwm0; 16 | mraa_pwm_context pwm1; 17 | 18 | 19 | //Datasheet spec is 100kHz Maximum PWM switching frequency 20 | #define PWM_PERIOD 1000000 //nano seconds for 1kHz 21 | 22 | char c_system_command[128] = {0}; 23 | 24 | int motor_driver_enable() 25 | { 26 | /* MRAA GPIO Initialization..*/ 27 | gpio_ain1 = mraa_gpio_init_raw(48); 28 | gpio_ain2 = mraa_gpio_init_raw(47); 29 | gpio_bin1 = mraa_gpio_init_raw(15); 30 | gpio_bin2 = mraa_gpio_init_raw(14); 31 | gpio_stby = mraa_gpio_init_raw(49); 32 | 33 | mraa_gpio_dir(gpio_ain1, MRAA_GPIO_OUT); 34 | mraa_gpio_dir(gpio_ain2, MRAA_GPIO_OUT); 35 | mraa_gpio_dir(gpio_bin1, MRAA_GPIO_OUT); 36 | mraa_gpio_dir(gpio_bin2, MRAA_GPIO_OUT); 37 | mraa_gpio_dir(gpio_stby, MRAA_GPIO_OUT); 38 | 39 | 40 | /* MRAA PWM Initialization.. DOES NOT WORK.. Will return -1 with latest mraa library */ 41 | pwm0 = mraa_pwm_init(12); 42 | pwm1 = mraa_pwm_init(13); 43 | 44 | if ( pwm0 == NULL ) 45 | { 46 | printf("PWM0 was NULL must be some MRAA bug cause I can't fix it :(\r\n"); 47 | return -1; 48 | } 49 | if ( pwm1 == NULL ) 50 | { 51 | printf("PWM1 was NULL\r\n"); 52 | return -1; 53 | } 54 | printf("PWM Init OK\r\n"); 55 | mraa_pwm_period_us(pwm0, 200); 56 | mraa_pwm_period_us(pwm1, 200); 57 | printf("PWM Period OK\r\n"); 58 | mraa_pwm_enable(pwm0, 1); 59 | mraa_pwm_enable(pwm1, 1); 60 | printf("PWM Enabed\r\n"); 61 | 62 | //Take H Bridge out of standby mode 63 | mraa_gpio_write(gpio_stby, 1); 64 | 65 | return 1; 66 | } 67 | 68 | void motor_driver_disable() 69 | { 70 | //Put H Bridge into standby mode 71 | mraa_gpio_write(gpio_stby, 0); 72 | 73 | //Cleaning up GPIO stuff 74 | mraa_gpio_close(gpio_ain1); 75 | mraa_gpio_close(gpio_ain2); 76 | mraa_gpio_close(gpio_bin1); 77 | mraa_gpio_close(gpio_bin2); 78 | mraa_gpio_close(gpio_stby); 79 | 80 | if ( pwm0 != NULL ) mraa_pwm_close(pwm0); 81 | if ( pwm1 != NULL ) mraa_pwm_close(pwm1); 82 | 83 | } 84 | 85 | void motor_driver_standby( char p_option ) 86 | { 87 | if ( p_option > 0 ) 88 | { 89 | mraa_gpio_write(gpio_stby, 0); //Enter Standby 90 | } 91 | else 92 | { 93 | mraa_gpio_write(gpio_stby, 1); //Exit Standby 94 | } 95 | } 96 | 97 | void set_motor_direction_left ( char p_direction ) 98 | { 99 | if ( p_direction == FORWARD ) //Left motor CCW 100 | { 101 | mraa_gpio_write(gpio_ain1, 0); //AIN1 Low 102 | mraa_gpio_write(gpio_ain2, 1); //AIN2 High 103 | } 104 | else if ( p_direction == REVERSE ) //Left motor CW 105 | { 106 | mraa_gpio_write(gpio_ain1, 1); //AIN1 High 107 | mraa_gpio_write(gpio_ain2, 0); //AIN2 Low 108 | } 109 | } 110 | void set_motor_direction_right( char p_direction ) 111 | { 112 | if ( p_direction == FORWARD ) //Right motor CW 113 | { 114 | mraa_gpio_write(gpio_bin1, 1); //BIN1 High 115 | mraa_gpio_write(gpio_bin2, 0); //BIN2 Low 116 | } 117 | else if ( p_direction == REVERSE ) //Right motor CCW 118 | { 119 | mraa_gpio_write(gpio_bin1, 0); //BIN1 Low 120 | mraa_gpio_write(gpio_bin2, 1); //BIN2 High 121 | } 122 | } 123 | 124 | void set_motor_speed_left ( float p_speed ) 125 | { 126 | if ( p_speed < 0.0f ) 127 | { 128 | if ( p_speed < -100.0f ) p_speed = -100.0f; 129 | set_motor_direction_left( REVERSE ); 130 | } 131 | else //if ( p_speed > 0.0f ) 132 | { 133 | if ( p_speed > 100.0f ) p_speed = 100.0f; 134 | set_motor_direction_left( FORWARD ); 135 | } 136 | 137 | /* TODO: Use MRAA to set PWM duty_cycle 138 | pwm0 = open("/sys/class/pwm/pwmchip0/pwm0/duty_cycle", O_RDWR); 139 | int length = sprintf(c_system_command, "%d", (int)(PWM_PERIOD * fabs(p_speed/100))); 140 | write(pwm0, c_system_command, length * sizeof(char)); 141 | close(pwm0); 142 | */ 143 | } 144 | void set_motor_speed_right( float p_speed ) 145 | { 146 | if ( p_speed < 0.0f ) 147 | { 148 | if ( p_speed < -100.0f ) p_speed = -100.0f; 149 | set_motor_direction_right( REVERSE ); 150 | } 151 | else //if ( p_speed > 0.0f ) 152 | { 153 | if ( p_speed > 100.0f ) p_speed = 100.0f; 154 | set_motor_direction_right( FORWARD ); 155 | } 156 | 157 | /* TODO: Use MRAA to set PWM duty_cycle 158 | pwm1 = open("/sys/class/pwm/pwmchip0/pwm1/duty_cycle", O_RDWR); 159 | int length = sprintf(c_system_command, "%d", (int)(PWM_PERIOD * fabs(p_speed/100))); 160 | write(pwm1, c_system_command, length * sizeof(char)); 161 | close(pwm1); 162 | */ 163 | } 164 | -------------------------------------------------------------------------------- /src/pid.c: -------------------------------------------------------------------------------- 1 | /* 2 | * pid.c 3 | * 4 | * Created on: Jul 26, 2013 5 | * Author: Renee 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | #include "pid.h" 12 | 13 | void PIDinit(PID_t * pid, float *pgain, float *igain, float *dgain, float *ilimit, float *numsamples) 14 | { 15 | pid->processGain = pgain; 16 | pid->integralTime = igain; 17 | pid->derivateTime = dgain; 18 | 19 | pid->error = 0; 20 | pid->accumulatedError = 0; 21 | pid->differentialError = 0; 22 | pid->lastFeedbackReading = 0; 23 | 24 | pid->iLimit = ilimit; 25 | pid->EMAnumberSamples = numsamples; 26 | } 27 | 28 | double 29 | exponentialMovingAverage( const double value, const double previousEMA, const double alpha ) 30 | { 31 | double EMA = previousEMA; 32 | EMA += alpha * (value - previousEMA); 33 | return EMA; 34 | } 35 | 36 | void 37 | calculateP( const double setpoint, const double actual_position, PID_t* pPID ) 38 | { 39 | pPID->error = setpoint - actual_position; 40 | } 41 | 42 | void 43 | calculateI( const double setpoint, const double dTmilliseconds, PID_t* pPID ) 44 | { 45 | pPID->accumulatedError += pPID->error * dTmilliseconds / (*pPID->integralTime); 46 | 47 | if(pPID->accumulatedError > (*pPID->iLimit)) 48 | { 49 | pPID->accumulatedError = (*pPID->iLimit); 50 | } 51 | else if(pPID->accumulatedError < -(*pPID->iLimit)) 52 | { 53 | pPID->accumulatedError = -(*pPID->iLimit); 54 | } 55 | } 56 | 57 | void 58 | calculateD( const double actual_position, const double dTmilliseconds, PID_t* pPID ) 59 | { 60 | double currentDifferentialError = -1 * (*pPID->derivateTime) * ((actual_position - pPID->lastFeedbackReading) / dTmilliseconds); 61 | pPID->lastFeedbackReading = actual_position; 62 | 63 | if( *pPID->EMAnumberSamples > 0) 64 | { 65 | pPID->differentialError = exponentialMovingAverage( currentDifferentialError, pPID->differentialError, ( 1.0 / *pPID->EMAnumberSamples ) ); 66 | } 67 | else 68 | { 69 | pPID->differentialError = currentDifferentialError; 70 | } 71 | } 72 | 73 | double 74 | PIDUpdate( double setpoint, double actual_position, double dTmilliseconds, PID_t* pPID ) 75 | { 76 | double controllerOutput = 0; 77 | 78 | calculateP(setpoint, actual_position, pPID); 79 | 80 | if ( *pPID->integralTime == 0 ) pPID->accumulatedError = 0; 81 | else calculateI(setpoint, dTmilliseconds, pPID); 82 | 83 | if ( *pPID->derivateTime == 0 ) pPID->differentialError = 0; 84 | else calculateD( actual_position, dTmilliseconds, pPID ); 85 | 86 | controllerOutput = ( *pPID->processGain ) * ( pPID->error + pPID->accumulatedError + pPID->differentialError ); 87 | return controllerOutput; 88 | } 89 | -------------------------------------------------------------------------------- /src/pid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * pid.h 3 | * 4 | * Created on: Jul 26, 2013 5 | * Author: Renee 6 | */ 7 | #ifndef PID_H_ 8 | #define PID_H_ 9 | 10 | //Speed PID Configuration 11 | #define PIDS_P_GAIN 0.02f 12 | #define PIDS_I_GAIN 800.0f 13 | #define PIDS_D_GAIN 340.0f 14 | #define PIDS_EMA_SAMPLES 10.0f 15 | #define PIDS_I_LIMIT 450.0 //Ilimit is before process gain 16 | 17 | //Pitch PID Configuration 18 | #define PIDP_P_GAIN 6.5f 19 | #define PIDP_I_GAIN 600.0f 20 | #define PIDP_D_GAIN 30.0f 21 | #define PIDP_EMA_SAMPLES 2.0f 22 | #define PIDP_I_LIMIT 10.0 //Ilimit is before process gain 23 | 24 | typedef struct 25 | { 26 | float *processGain; 27 | float *integralTime; 28 | float *derivateTime; 29 | 30 | double error; 31 | double accumulatedError; 32 | double differentialError; 33 | double lastFeedbackReading; 34 | 35 | float *iLimit; 36 | 37 | float *EMAnumberSamples; //Determines the EMAalpha; 38 | } PID_t; 39 | 40 | double PIDUpdate( double setpoint, double actual_position, double dTmilliseconds, PID_t* pPID); 41 | void PIDinit(PID_t * pid, float *pgain, float *igain, float *dgain, float *ilimit, float *numsamples); 42 | 43 | #endif /*PID_H_*/ 44 | -------------------------------------------------------------------------------- /src/udp.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_H 2 | #define UDP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define MAXMESSAGESIZE 64 14 | 15 | #define UDP_COMMAND_PORT 4242 //UDP Port for receiving command packets 16 | #define UDP_CONTROL_PORT 4240 //UDP Port for receiving control packets 17 | #define UDP_RESPOND_PORT 4243 //UDP Port for returning data to user 18 | 19 | int * isRunning; 20 | 21 | // Define the exit signal handler 22 | void udp_signal_handler(int signum) 23 | { 24 | (*isRunning) = 0; 25 | } 26 | 27 | int tx_command_socketfd = -1; //Socket descriptor used to send data to bound client 28 | int tx_control_socketfd = -1; //Socket descriptor used to response to control packets 29 | int rx_command_socketfd; //Socket descriptor used to receive commands from user 30 | int rx_control_socketfd; //Socket descriptor used to receive control data from user 31 | 32 | struct sockaddr_in tx_cmd_addrin; 33 | struct sockaddr_in tx_ctrl_addrin; 34 | struct sockaddr_in rx_cmd_addrin; 35 | struct sockaddr_in rx_ctrl_addrin; 36 | 37 | pthread_t udplistenerThread; 38 | 39 | void (*commandFunctionPtr)(char *); 40 | void (*controlFunctionPtr)(char *); 41 | 42 | void UDPCloseTX(); 43 | void UDPCloseCtrlTX(); 44 | void initUDPCmdSend( char * sendtoIP, unsigned short sendtoPort ); 45 | 46 | void initUDP( void * p_cmdFuncPtr, void * p_ctrlFuncPtr, int * p_running ) 47 | { 48 | commandFunctionPtr = p_cmdFuncPtr; 49 | controlFunctionPtr = p_ctrlFuncPtr; 50 | isRunning = p_running; 51 | } 52 | 53 | char lastRXAddress[16] = {0}; //The last address a UDP message was received from 54 | char commandBindAddress[16] = {0}; //This is the address we are bound to receive commands from 55 | int isBoundToClient = 0; 56 | 57 | void setCommandBindAddress() 58 | { 59 | //Set the bind address to the last address received from 60 | strcpy( commandBindAddress, lastRXAddress ); 61 | 62 | //Init the TX command socket with the new bind address 63 | initUDPCmdSend( commandBindAddress, UDP_RESPOND_PORT ); 64 | 65 | isBoundToClient = 1; 66 | } 67 | 68 | void initUDPCtrlSend( char * sendtoIP, unsigned short sendtoPort ) 69 | { 70 | tx_control_socketfd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); // Create socket for sending 71 | memset( &tx_ctrl_addrin, 0, sizeof( tx_ctrl_addrin ) ); // Zero out structure 72 | tx_ctrl_addrin.sin_family = AF_INET; // Internet address family 73 | tx_ctrl_addrin.sin_addr.s_addr = inet_addr( sendtoIP ); // Destination IP address 74 | tx_ctrl_addrin.sin_port = htons(sendtoPort); // Destination port 75 | } 76 | 77 | void initUDPCmdSend( char * sendtoIP, unsigned short sendtoPort ) 78 | { 79 | tx_command_socketfd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); // Create socket for sending 80 | memset( &tx_cmd_addrin, 0, sizeof( tx_cmd_addrin ) ); // Zero out structure 81 | tx_cmd_addrin.sin_family = AF_INET; // Internet address family 82 | tx_cmd_addrin.sin_addr.s_addr = inet_addr( sendtoIP ); // Destination IP address 83 | tx_cmd_addrin.sin_port = htons(sendtoPort); // Destination port 84 | } 85 | 86 | void UDPBindSend( char * data, int len ) 87 | { 88 | if ( tx_command_socketfd >= 0 && isBoundToClient ) 89 | { 90 | sendto( tx_command_socketfd, data, len, 0, ( struct sockaddr * )&tx_cmd_addrin, sizeof( tx_cmd_addrin ) ); 91 | } 92 | } 93 | 94 | void UDPCtrlSend( char * data ) 95 | { 96 | if(tx_control_socketfd >= 0) 97 | { 98 | sendto( tx_control_socketfd, data, strlen(data), 0, ( struct sockaddr * )&tx_ctrl_addrin, sizeof( tx_ctrl_addrin ) ); 99 | } 100 | } 101 | 102 | void initListener( unsigned short udpListenPort, int * p_socket, struct sockaddr_in * p_addr ) 103 | { 104 | *p_socket = socket( AF_INET, SOCK_DGRAM, 0 ); 105 | bzero( p_addr, sizeof( *p_addr ) ); 106 | (*p_addr).sin_family = AF_INET; 107 | (*p_addr).sin_addr.s_addr = htonl( INADDR_ANY ); 108 | (*p_addr).sin_port = htons( udpListenPort ); 109 | bind( *p_socket, (struct sockaddr *)p_addr, sizeof( *p_addr ) ); 110 | } 111 | 112 | int checkUDPReady( char * udpBuffer, int * p_socket ) 113 | { 114 | int bytesAv = 0; 115 | 116 | /* If there is data to be read on the socket bring it in and capture the source IP */ 117 | if( ioctl( *p_socket, FIONREAD, &bytesAv ) > 0 || bytesAv > 0 ) 118 | { 119 | int udpMsgLen = 0; 120 | struct sockaddr_in rx_from_addr; 121 | socklen_t len = sizeof( rx_from_addr ); 122 | //Receive UDP data 123 | udpMsgLen = recvfrom( *p_socket, udpBuffer, MAXMESSAGESIZE, 0, ( struct sockaddr * )&rx_from_addr, &len ); 124 | udpBuffer[ udpMsgLen ] = 0; //Null terminate UDP RX string 125 | 126 | //Get address from this received packet 127 | char thisRXaddress[16] = {0}; 128 | sprintf( thisRXaddress, "%s", inet_ntoa( rx_from_addr.sin_addr ) ); 129 | 130 | //If this RX address does not match the last RX address && is a control packet... 131 | if ( p_socket == &rx_control_socketfd && memcmp( lastRXAddress, thisRXaddress, sizeof(lastRXAddress) ) != 0 ) 132 | { 133 | UDPCloseCtrlTX(); //...close the control TX socket 134 | initUDPCtrlSend( thisRXaddress, UDP_RESPOND_PORT ); //and re-open with the address we need to respond to 135 | } 136 | 137 | //Store the last RX address 138 | strcpy( lastRXAddress, thisRXaddress ); 139 | 140 | return 1; 141 | } 142 | 143 | return 0; 144 | } 145 | 146 | void UDPCloseTX() 147 | { 148 | close( tx_command_socketfd ); 149 | tx_command_socketfd = -1; 150 | } 151 | void UDPCloseCtrlTX() 152 | { 153 | close( tx_control_socketfd ); 154 | tx_control_socketfd = -1; 155 | } 156 | 157 | void* udplistener_Thread( void * arg ) 158 | { 159 | signal( SIGINT, udp_signal_handler ); 160 | signal( SIGHUP, udp_signal_handler ); 161 | signal( SIGTERM, udp_signal_handler ); 162 | prctl(PR_SET_NAME, "updlistener", 0, 0, 0); 163 | 164 | initListener( UDP_COMMAND_PORT, &rx_command_socketfd, &rx_cmd_addrin ); 165 | initListener( UDP_CONTROL_PORT, &rx_control_socketfd, &rx_ctrl_addrin ); 166 | 167 | char incomingUDP[ MAXMESSAGESIZE + 1 ]; 168 | 169 | while( (*isRunning) ) 170 | { 171 | usleep( 20000 ); //Give this thread a break between iterations to keep CPU usage down 172 | 173 | /* Check for UDP data on Control port */ 174 | while( checkUDPReady( incomingUDP, &rx_control_socketfd ) ) 175 | { 176 | (*controlFunctionPtr)(incomingUDP); 177 | bzero( incomingUDP, sizeof( incomingUDP ) ); 178 | } 179 | 180 | /* Check for UDP data on Command port */ 181 | while( checkUDPReady( incomingUDP, &rx_command_socketfd ) ) 182 | { 183 | if ( isBoundToClient && memcmp( lastRXAddress, commandBindAddress, sizeof( lastRXAddress ) ) == 0 ) 184 | { 185 | (*commandFunctionPtr)(incomingUDP); 186 | } 187 | bzero( incomingUDP, sizeof( incomingUDP ) ); 188 | } 189 | } 190 | 191 | return NULL; 192 | } 193 | 194 | #endif 195 | --------------------------------------------------------------------------------