├── CAD_V2 └── miniDogV2-002.stp ├── LICENSE ├── README.md ├── miniDogV2_004_Static ├── Filter.ino ├── Kinematics.ino ├── miniDogV2_004_Static.ino └── readangle.ino └── miniDogV2_011_Dynamic ├── Filter.ino ├── Kinematics.ino ├── miniDogV2_011_Dynamic.ino └── readangle.ino /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # miniDogV2 2 | miniDog V2 CAD and Code 3 | 4 | This relates to V2 of miniDog: https://www.youtube.com/playlist?list=PLpwJoq86vov_qzmadB1zj6aVKObH3WbeH 5 | -------------------------------------------------------------------------------- /miniDogV2_004_Static/Filter.ino: -------------------------------------------------------------------------------- 1 | // motion filter to filter motions and compliance 2 | 3 | float filter(float prevValue, float currentValue, int filter) { 4 | float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1); 5 | return lengthFiltered; 6 | } 7 | -------------------------------------------------------------------------------- /miniDogV2_004_Static/Kinematics.ino: -------------------------------------------------------------------------------- 1 | void kinematics (int leg, int mode, float x, float y, float z, float roll, float pitch, float yaw) { 2 | 3 | // *** TRANSLATION AXIS *** 4 | 5 | // moving the foot sideways on the end plane 6 | #define hipOffset 76.5 7 | float lengthY; 8 | float hipAngle1a; 9 | float hipAngle1b; 10 | float hipAngle1; 11 | float hipAngle1Degrees; 12 | float hipHyp; 13 | 14 | // moving the foot forwards or backwardes in the side plane 15 | float shoulderAngle2; 16 | float shoulderAngle2Degrees; 17 | float z2; 18 | 19 | // side plane of individual leg only 20 | #define shinLength 125 21 | #define thighLength 125 22 | float z3; 23 | float shoulderAngle1; 24 | float shoulderAngle1Degrees; 25 | float shoulderAngle1a; 26 | float shoulderAngle1b; 27 | float shoulderAngle1c; 28 | float shoulderAngle1d; 29 | float kneeAngle; 30 | float kneeAngleDegrees; 31 | 32 | // *** ROTATION AXIS 33 | 34 | // roll axis 35 | #define bodyWidth 126 // half the distance from the middle of the body to the hip pivot 36 | float legDiffRoll; // differnece in height for each leg 37 | float bodyDiffRoll; // how much shorter the 'virtual body' gets 38 | float footDisplacementRoll; // where the foot actually is 39 | float footDisplacementAngleRoll; // smaller angle 40 | float footWholeAngleRoll; // whole leg angle 41 | float hipRollAngle; // angle for hip when roll axis is in use 42 | float rollAngle; // angle in RADIANS that the body rolls 43 | float zz1a; // hypotenuse of final triangle 44 | float zz1; // new height for leg to pass onto the next bit of code 45 | float yy1; // new position for leg to move sideways 46 | 47 | // pitch axis 48 | 49 | #define bodyLength 147.5 // distance from centre of body to shoulder pivot 50 | float legDiffPitch; // differnece in height for each leg 51 | float bodyDiffPitch; // how much shorter the 'virtual body' gets 52 | float footDisplacementPitch; // where the foot actually is 53 | float footDisplacementAnglePitch; // smaller angle 54 | float footWholeAnglePitch; // whole leg angle 55 | float shoulderPitchAngle; // angle for hip when roll axis is in use 56 | float pitchAngle; // angle in RADIANS that the body rolls 57 | float zz2a; // hypotenuse of final triangle 58 | float zz2; // new height for the leg to pass onto the next bit of code 59 | float xx1; // new position to move the leg fowwards/backwards 60 | 61 | // yaw axis 62 | 63 | float yawAngle; // angle in RADIANs for rotation in yaw 64 | float existingAngle; // existing angle of leg from centre 65 | float radius; // radius of leg from centre of robot based on x and y from sticks 66 | float demandYaw; // demand yaw postion - existing yaw plus the stick yaw 67 | float xx3; // new X coordinate based on demand angle 68 | float yy3; // new Y coordinate based on demand angle 69 | 70 | // ****************************************************************************************************** 71 | // ***************************** KINEMATIC MODEL CALCS START HERE *************************************** 72 | // ****************************************************************************************************** 73 | 74 | // convert degrees to radians for the calcs 75 | yawAngle = (PI/180) * yaw; 76 | 77 | // put in offsets from robot's parameters so we can work out the radius of the foot from the robot's centre 78 | if (leg == 1) { // front left leg 79 | y = y - (bodyWidth+hipOffset); 80 | x = x - bodyLength; 81 | } 82 | else if (leg == 2) { // front right leg 83 | y = y + (bodyWidth+hipOffset); 84 | x = x - bodyLength; 85 | } 86 | else if (leg == 3) { // back left leg 87 | y = y - (bodyWidth+hipOffset); 88 | x = x + bodyLength; 89 | } 90 | else if (leg == 4) { // back left leg 91 | y = y + (bodyWidth+hipOffset); 92 | x = x + bodyLength; 93 | } 94 | 95 | //calc existing angle of leg from cetre 96 | existingAngle = atan(y/x); 97 | 98 | // calc radius from centre 99 | radius = y/sin(existingAngle); 100 | 101 | //calc demand yaw angle 102 | demandYaw = existingAngle + yawAngle; 103 | 104 | // calc new X and Y based on demand yaw angle 105 | xx3 = radius * cos(demandYaw); // calc new X and Y based on new yaw angle 106 | yy3 = radius * sin(demandYaw); 107 | 108 | // remove the offsets so we pivot around 0/0 x/y 109 | if (leg == 1) { // front left leg 110 | yy3 = yy3 + (bodyWidth+hipOffset); 111 | xx3 = xx3 + bodyLength; 112 | } 113 | else if (leg == 2) { // front right leg 114 | yy3 = yy3 - (bodyWidth+hipOffset); 115 | xx3 = xx3 + bodyLength; 116 | } 117 | else if (leg == 3) { // back left leg 118 | yy3 = yy3 + (bodyWidth+hipOffset); 119 | xx3 = xx3 - bodyLength; 120 | } 121 | else if (leg == 4) { // back left leg 122 | yy3 = yy3 - (bodyWidth+hipOffset); 123 | xx3 = xx3 - bodyLength; 124 | } 125 | 126 | // *** PITCH AXIS *** 127 | 128 | //turn around the pitch for front or back of the robot 129 | if (leg == 1 || leg == 2) { 130 | pitch = 0-pitch; 131 | } 132 | else if (leg == 3 || leg == 4) { 133 | pitch = 0+pitch; 134 | xx3 = xx3*-1; // switch over x for each end of the robot 135 | } 136 | 137 | // convert pitch to degrees 138 | pitchAngle = (PI/180) * pitch; 139 | 140 | //calc top triangle sides 141 | legDiffPitch = sin(pitchAngle) * bodyLength; 142 | bodyDiffPitch = cos(pitchAngle) * bodyLength; 143 | 144 | // calc actual height from the ground for each side 145 | legDiffPitch = z - legDiffPitch; 146 | 147 | // calc foot displacement 148 | footDisplacementPitch = ((bodyDiffPitch - bodyLength)*-1)+xx3; 149 | 150 | //calc smaller displacement angle 151 | footDisplacementAnglePitch = atan(footDisplacementPitch/legDiffPitch); 152 | 153 | //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle) 154 | zz2a = legDiffPitch/cos(footDisplacementAnglePitch); 155 | 156 | // calc the whole angle for the leg 157 | footWholeAnglePitch = footDisplacementAnglePitch + pitchAngle; 158 | 159 | //calc actual leg length - the new Z to pass on 160 | zz2 = cos(footWholeAnglePitch) * zz2a; 161 | 162 | //calc new Z to pass on 163 | xx1 = sin(footWholeAnglePitch) * zz2a; 164 | 165 | if (leg == 3 || leg == 4 ){ // switch back X for the back of the robot 166 | xx1 = xx1 *-1; 167 | } 168 | 169 | 170 | // *** ROLL AXIS *** 171 | 172 | //turn around roll angle for each side of the robot 173 | if (leg == 1 || leg == 3) { 174 | roll = 0+roll; 175 | yy3 = yy3*-1; 176 | } 177 | else if (leg == 2 || leg == 4) { 178 | roll = 0-roll; 179 | } 180 | 181 | // convert roll angle to radians 182 | rollAngle = (PI/180) * roll; //covert degrees from the stick to radians 183 | 184 | // calc the top triangle sides 185 | legDiffRoll = sin(rollAngle) * bodyWidth; 186 | bodyDiffRoll = cos(rollAngle) * bodyWidth; 187 | 188 | // calc actual height from the ground for each side 189 | legDiffRoll = zz2 - legDiffRoll; 190 | 191 | // calc foot displacement 192 | footDisplacementRoll = ((bodyDiffRoll - bodyWidth)*-1)-yy3; 193 | 194 | //calc smaller displacement angle 195 | footDisplacementAngleRoll = atan(footDisplacementRoll/legDiffRoll); 196 | 197 | //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle) 198 | zz1a = legDiffRoll/cos(footDisplacementAngleRoll); 199 | 200 | // calc the whole angle for the leg 201 | footWholeAngleRoll = footDisplacementAngleRoll + rollAngle; 202 | 203 | //calc actual leg length - the new Z to pass on 204 | zz1 = cos(footWholeAngleRoll) * zz1a; 205 | 206 | //calc new Y to pass on 207 | yy1 = sin(footWholeAngleRoll) * zz1a; 208 | 209 | // *** TRANSLATION AXIS *** 210 | 211 | // calculate the hip joint and new leg length based on how far the robot moves sideways 212 | hipAngle1 = atan(yy1/zz1); 213 | hipAngle1Degrees = ((hipAngle1 * (180/PI))); // convert to degrees and take off rest position 214 | z2 = zz1/cos(hipAngle1); 215 | 216 | // **************** 217 | 218 | // calculate the shoulder joint offset and new leg length based on now far the foot moves forward/backwards 219 | shoulderAngle2 = atan(xx1/z2); // calc how much extra to add to the shoulder joint 220 | shoulderAngle2Degrees = shoulderAngle2 * (180/PI); 221 | z3 = z2/cos(shoulderAngle2); // calc new leg length to feed to the next bit of code below 222 | 223 | // **************** 224 | 225 | // calculate leg length based on shin/thigh length and knee and shoulder angle 226 | shoulderAngle1a = sq(thighLength) + sq(z3) - sq(shinLength); 227 | shoulderAngle1b = 2 * thighLength * z3; 228 | shoulderAngle1c = shoulderAngle1a / shoulderAngle1b; 229 | shoulderAngle1 = acos(shoulderAngle1c); // radians 230 | kneeAngle = PI - (shoulderAngle1 *2); // radians 231 | 232 | //calc degrees from angles 233 | shoulderAngle1Degrees = shoulderAngle1 * (180/PI); // degrees 234 | kneeAngleDegrees = kneeAngle * (180/PI); // degrees 235 | 236 | 237 | // ****************************************************************************************************** 238 | // ***************** compliance / filtering / write out servo positoion below this point ***************** 239 | // ****************************************************************************************************** 240 | 241 | if (leg == 1) { // *front left leg* 242 | 243 | // convert degrees to servo microSeconds 244 | servo11PosTrack = (kneeAngleDegrees-90) * -25; // positive scaler 245 | servo7PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25) ; 246 | servo3PosTrack = (hipAngle1Degrees * -25); 247 | 248 | // no compliance mode 249 | if (mode == 0) { 250 | servo3Pos = servo3PosTrack; // front left hip 251 | servo7Pos = servo7PosTrack; // front left shoulder 252 | servo11Pos = servo11PosTrack; // front right knee 253 | } 254 | // compliance mode 255 | else if (mode == 1) { 256 | //*** front left hip *** 257 | if (hall2 > threshholdGlobal || hall2 < (threshholdGlobal*-1)) { 258 | servo3Pos = servo3Pos + (hall2 * multiplierHipsLeft); 259 | } 260 | else { // return to centre 261 | servo3Pos = servo3PosTrack; 262 | } 263 | //*** front left shoulder 264 | if (hall3 > threshholdGlobal || hall3 < (threshholdGlobal*-1)) { 265 | servo7Pos = servo7Pos + (hall3 * multiplierShouldersLeft); 266 | } 267 | else { // return to centre 268 | servo7Pos = servo7PosTrack; 269 | } 270 | // ***front left knee 271 | if (hall4 > threshholdGlobal || hall4 < (threshholdGlobal*-1)) { 272 | servo11Pos = servo11Pos + (hall4 * multiplierKneesLeft); 273 | } 274 | else { // return to centre 275 | servo11Pos = servo11PosTrack; 276 | } 277 | } 278 | // filter motions 279 | servo3PosFiltered = filter(servo3Pos, servo3PosFiltered, filterHipsLeft); 280 | servo3PosFiltered = constrain(servo3PosFiltered,-900,900); 281 | servo7PosFiltered = filter(servo7Pos, servo7PosFiltered, filterShouldersLeft); 282 | servo7PosFiltered = constrain(servo7PosFiltered,-900,900); 283 | servo11PosFiltered = filter(servo11Pos, servo11PosFiltered, filterKneesLeft); 284 | servo11PosFiltered = constrain(servo11PosFiltered,-900,900); 285 | // write out servo positions 286 | servo3.writeMicroseconds(servo3Offset + servo3PosFiltered); // front left hip 287 | servo7.writeMicroseconds(servo7Offset + servo7PosFiltered); // front left shoulder 288 | servo11.writeMicroseconds(servo11Offset + servo11PosFiltered); // front right knee 289 | } 290 | 291 | else if (leg == 2) { // *front right leg* 292 | 293 | // convert degrees to servo microSeconds 294 | servo12PosTrack = (kneeAngleDegrees-90) * 25; 295 | servo8PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25); 296 | servo4PosTrack = (hipAngle1Degrees * 25); 297 | 298 | // no compliance mode 299 | if (mode == 0) { 300 | servo4Pos = servo4PosTrack; // front right hip 301 | servo8Pos = servo8PosTrack; // front right shoulder 302 | servo12Pos = servo12PosTrack; // front right knee 303 | } 304 | // compliance mode 305 | if (mode == 1) { 306 | //*** front right hip *** 307 | if (hall1 > threshholdGlobal || hall1 < (threshholdGlobal*-1)) { 308 | servo4Pos = servo4Pos + (hall1 * multiplierHipsRight); 309 | } 310 | else { // return to centre 311 | servo4Pos = servo4PosTrack; 312 | } 313 | //*** front right shoulder 314 | if (hall6 > threshholdGlobal || hall6 < (threshholdGlobal*-1)) { 315 | servo8Pos = servo8Pos + (hall6 * multiplierShouldersRight); 316 | } 317 | else { // return to centre 318 | servo8Pos = servo8PosTrack; 319 | } 320 | // ***front right knee 321 | if (hall5 > threshholdGlobal || hall5 < (threshholdGlobal*-1)) { 322 | servo12Pos = servo12Pos + (hall5 * multiplierKneesRight); 323 | } 324 | else { // return to centre 325 | servo12Pos = servo12PosTrack; 326 | } 327 | } 328 | // filter motions 329 | servo4PosFiltered = filter(servo4Pos, servo4PosFiltered, filterHipsRight); 330 | servo4PosFiltered = constrain(servo4PosFiltered,-900,900); 331 | servo8PosFiltered = filter(servo8Pos, servo8PosFiltered, filterShouldersRight); 332 | servo8PosFiltered = constrain(servo8PosFiltered,-900,900); 333 | servo12PosFiltered = filter(servo12Pos, servo12PosFiltered, filterKneesRight); 334 | servo12PosFiltered = constrain(servo12PosFiltered,-900,900); 335 | // write out servo potitions 336 | servo4.writeMicroseconds(servo4Offset + servo4PosFiltered); // front right hip 337 | servo8.writeMicroseconds(servo8Offset + servo8PosFiltered); // front right shoulder 338 | servo12.writeMicroseconds(servo12Offset + servo12PosFiltered); // front right knee 339 | } 340 | 341 | else if (leg == 3) { // *back left leg* 342 | 343 | // convert degrees to servo microSeconds 344 | servo9PosTrack = (kneeAngleDegrees-90) * -25; 345 | servo5PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25); 346 | servo1PosTrack = (hipAngle1Degrees * -25); 347 | 348 | // no compliance mode 349 | if (mode == 0) { 350 | servo1Pos = servo1PosTrack; // back left hip 351 | servo5Pos = servo5PosTrack; // back left shoulder 352 | servo9Pos = servo9PosTrack; // back left knee 353 | } 354 | // compliance mode 355 | if (mode == 1) { 356 | //*** back left hip *** 357 | if (hall7 > threshholdGlobal || hall7 < (threshholdGlobal*-1)) { 358 | servo1Pos = servo1Pos + (hall7 * multiplierHipsLeft); 359 | } 360 | else { // return to centre 361 | servo1Pos = servo1PosTrack; 362 | } 363 | //*** back left shoulder 364 | if (hall12 > threshholdGlobal || hall12 < (threshholdGlobal*-1)) { 365 | servo5Pos = servo5Pos + (hall12 * multiplierShouldersLeft); 366 | } 367 | else { // return to centre 368 | servo5Pos = servo5PosTrack; 369 | } 370 | // ***back left knee 371 | if (hall11 > threshholdGlobal || hall11 < (threshholdGlobal*-1)) { 372 | servo9Pos = servo9Pos + (hall11 * multiplierKneesLeft); 373 | } 374 | else { // return to centre 375 | servo9Pos = servo9PosTrack; 376 | } 377 | } 378 | // filter motions 379 | servo1PosFiltered = filter(servo1Pos, servo1PosFiltered, filterHipsLeft); 380 | servo1PosFiltered = constrain(servo1PosFiltered,-900,900); 381 | servo5PosFiltered = filter(servo5Pos, servo5PosFiltered, filterShouldersLeft); 382 | servo5PosFiltered = constrain(servo5PosFiltered,-900,900); 383 | servo9PosFiltered = filter(servo9Pos, servo9PosFiltered, filterKneesLeft); 384 | servo9PosFiltered = constrain(servo9PosFiltered,-900,900); 385 | // write out servo positions 386 | servo1.writeMicroseconds(servo1Offset + servo1PosFiltered); // back left hip 387 | servo5.writeMicroseconds(servo5Offset + servo5PosFiltered); // back left shoulder 388 | servo9.writeMicroseconds(servo9Offset + servo9PosFiltered); // front left knee 389 | } 390 | 391 | else if (leg == 4) { // *back right leg* 392 | 393 | // convert degrees to servo microSeconds 394 | servo10PosTrack = (kneeAngleDegrees-90) * 25; 395 | servo6PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25); 396 | servo2PosTrack = (hipAngle1Degrees * 25); 397 | 398 | // no compliance mode 399 | if (mode == 0) { 400 | servo2Pos = servo2PosTrack; // back right hip 401 | servo6Pos = servo6PosTrack; // back right shoulder 402 | servo10Pos = servo10PosTrack; // back right knee 403 | } 404 | // compliance mode 405 | if (mode == 1) { 406 | //*** back right hip *** 407 | if (hall8 > threshholdGlobal || hall8 < (threshholdGlobal*-1)) { 408 | servo2Pos = servo2Pos + (hall8 * multiplierHipsRight); 409 | } 410 | else { // return to centre 411 | servo2Pos = servo2PosTrack; 412 | } 413 | //*** back right shoulder 414 | if (hall9 > threshholdGlobal || hall9 < (threshholdGlobal*-1)) { 415 | servo6Pos = servo6Pos + (hall9 * multiplierShouldersRight); 416 | } 417 | else { // return to centre 418 | servo6Pos = servo6PosTrack; 419 | } 420 | // ***back right knee 421 | if (hall10 > threshholdGlobal || hall10 < (threshholdGlobal*-1)) { 422 | servo10Pos = servo10Pos + (hall10 * multiplierKneesRight); 423 | } 424 | else { // return to centre 425 | servo10Pos = servo10PosTrack; 426 | } 427 | } 428 | // filter motions 429 | servo2PosFiltered = filter(servo2Pos, servo2PosFiltered, filterHipsRight); 430 | servo2PosFiltered = constrain(servo2PosFiltered,-900,900); 431 | servo6PosFiltered = filter(servo6Pos, servo6PosFiltered, filterShouldersRight); 432 | servo6PosFiltered = constrain(servo6PosFiltered,-900,900); 433 | servo10PosFiltered = filter(servo10Pos, servo10PosFiltered, filterKneesRight); 434 | servo10PosFiltered = constrain(servo10PosFiltered,-900,900); 435 | // write out servo positions 436 | servo2.writeMicroseconds(servo2Offset + servo2PosFiltered); // back right hip 437 | servo6.writeMicroseconds(servo6Offset + servo6PosFiltered); // back left shoulder 438 | servo10.writeMicroseconds(servo10Offset + servo10PosFiltered); // front left knee 439 | } 440 | 441 | 442 | 443 | } // end of kinematics 444 | -------------------------------------------------------------------------------- /miniDogV2_004_Static/miniDogV2_004_Static.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include //PID loop from http://playground.arduino.cc/Code/PIDLibrary 6 | 7 | double Pk1 = 10; 8 | double Ik1 = 30; 9 | double Dk1 = 0.1; 10 | 11 | double Setpoint1, Input1, Output1; // PID variables 12 | PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT); // PID Setup 13 | 14 | double Pk2 = 5; 15 | double Ik2 = 0; 16 | double Dk2 = 0; 17 | 18 | double Setpoint2, Input2, Output2; // PID variables 19 | PID PID2(&Input2, &Output2, &Setpoint2, Pk2, Ik2 , Dk2, DIRECT); // PID Setup 20 | 21 | double Pk3 = 5; 22 | double Ik3 = 0; 23 | double Dk3 = 0; 24 | 25 | double Setpoint3, Input3, Output3; // PID variables 26 | PID PID3(&Input3, &Output3, &Setpoint3, Pk3, Ik3 , Dk3, DIRECT); // PID Setup 27 | 28 | RF24 radio(27, 10); // CE, CSN 29 | const byte addresses[][6] = {"00001", "00002"}; 30 | 31 | //IMU stuff 32 | #include "I2Cdev.h" 33 | #include "MPU6050_6Axis_MotionApps20.h" 34 | #include "Wire.h" 35 | int requested_state; 36 | 37 | // class default I2C address is 0x68 38 | // specific I2C addresses may be passed as a parameter here 39 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 40 | // AD0 high = 0x69 41 | MPU6050 mpu; 42 | //MPU6050 mpu(0x69); // <-- use for AD0 high 43 | 44 | // MPU control/status vars 45 | bool dmpReady = false; // set true if DMP init was successful 46 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 47 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 48 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 49 | uint16_t fifoCount; // count of all bytes currently in FIFO 50 | uint8_t fifoBuffer[64]; // FIFO storage buffer 51 | 52 | // orientation/motion vars 53 | Quaternion q; // [w, x, y, z] quaternion container 54 | VectorInt16 aa; // [x, y, z] accel sensor measurements 55 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 56 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 57 | VectorFloat gravity; // [x, y, z] gravity vector 58 | float euler[3]; // [psi, theta, phi] Euler angle container 59 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 60 | 61 | float pitch; 62 | float roll; 63 | float rollFiltered; 64 | 65 | #define PI 3.1415926535897932384626433832795 66 | 67 | int IMUdataReady = 0; 68 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 69 | 70 | Servo servo1; 71 | Servo servo2; 72 | Servo servo3; 73 | Servo servo4; 74 | 75 | Servo servo5; 76 | Servo servo6; 77 | Servo servo7; 78 | Servo servo8; 79 | 80 | Servo servo9; 81 | Servo servo10; 82 | Servo servo11; 83 | Servo servo12; 84 | 85 | //**************remote control**************** 86 | struct RECEIVE_DATA_STRUCTURE{ 87 | //put your variable definitions here for the data you want to send 88 | //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO 89 | 90 | int16_t menuDown; 91 | int16_t Select; 92 | int16_t menuUp; 93 | int16_t toggleBottom; 94 | int16_t toggleTop; 95 | int16_t toggle1; 96 | int16_t toggle2; 97 | int16_t mode; 98 | int16_t RLR; 99 | int16_t RFB; 100 | int16_t RT; 101 | int16_t LLR; 102 | int16_t LFB; 103 | int16_t LT; 104 | int16_t checkit; 105 | int16_t checkit2; 106 | }; 107 | 108 | RECEIVE_DATA_STRUCTURE mydata_remote; 109 | 110 | int RLR; 111 | int RFB; 112 | int RT; 113 | int LLR; 114 | int LFB; 115 | int LT; 116 | int toggleTop; 117 | int toggleBottom; 118 | int toggle1; 119 | int toggle2; 120 | int Select; 121 | 122 | int x; 123 | int y; 124 | int z; 125 | 126 | int yaw; 127 | int r; 128 | int p; 129 | 130 | 131 | // timers 132 | 133 | unsigned long currentMillis; 134 | unsigned long previousMillis; 135 | unsigned long previousWalkMillis; 136 | unsigned long previousSafetyMillis; 137 | 138 | //multipier values for compliance 139 | 140 | float multiplierKneesRight = 2; 141 | float multiplierShouldersRight = 2; 142 | float multiplierHipsRight = 0; 143 | 144 | float multiplierKneesLeft = 2; 145 | float multiplierShouldersLeft = 2; 146 | float multiplierHipsLeft = 0; 147 | 148 | // filters 149 | 150 | int filterKneesRight = 15; 151 | int filterShouldersRight = 15; 152 | int filterHipsRight = 25; 153 | 154 | int filterKneesLeft = 15; 155 | int filterShouldersLeft = 15; 156 | int filterHipsLeft = 25; 157 | 158 | // global joint threshold value for hall effects 159 | int threshholdGlobal = 5; 160 | 161 | // values to write out to servos 162 | 163 | float servo1Pos; // initial value 164 | float servo2Pos; 165 | float servo3Pos; 166 | float servo4Pos; 167 | float servo5Pos; 168 | float servo6Pos; 169 | float servo7Pos; 170 | float servo8Pos; 171 | float servo9Pos; 172 | float servo10Pos; 173 | float servo11Pos; 174 | float servo12Pos; 175 | 176 | float servo1PosTrack; // ongoing tracking value 177 | float servo2PosTrack; 178 | float servo3PosTrack; 179 | float servo4PosTrack; 180 | float servo5PosTrack; 181 | float servo6PosTrack; 182 | float servo7PosTrack; 183 | float servo8PosTrack; 184 | float servo9PosTrack; 185 | float servo10PosTrack; 186 | float servo11PosTrack; 187 | float servo12PosTrack; 188 | 189 | float servo1PosFiltered; // filtered values 190 | float servo2PosFiltered; 191 | float servo3PosFiltered; 192 | float servo4PosFiltered; 193 | float servo5PosFiltered; 194 | float servo6PosFiltered; 195 | float servo7PosFiltered; 196 | float servo8PosFiltered; 197 | float servo9PosFiltered; 198 | float servo10PosFiltered; 199 | float servo11PosFiltered; 200 | float servo12PosFiltered; 201 | 202 | // servo offsets for default position - knees at 45', hips at 0', shoulders at 45' 203 | 204 | int servo1Offset = 1380; 205 | int servo2Offset = 1570; 206 | int servo3Offset = 1500; 207 | int servo4Offset = 1600; 208 | 209 | int servo5Offset = 1500; 210 | int servo6Offset = 1340; 211 | int servo7Offset = 1600; 212 | int servo8Offset = 1470; 213 | 214 | int servo9Offset = 1630; 215 | int servo10Offset = 1550; 216 | int servo11Offset = 1550; 217 | int servo12Offset = 1400; 218 | 219 | // hall effect sensors 220 | 221 | float hall1; 222 | float hall2; 223 | float hall3; 224 | float hall4; 225 | float hall5; 226 | float hall6; 227 | float hall7; 228 | float hall8; 229 | float hall9; 230 | float hall10; 231 | float hall11; 232 | float hall12; 233 | 234 | // offset values for auto-calibration at startup 235 | 236 | int hall1Offset; 237 | int hall2Offset; 238 | int hall3Offset; 239 | int hall4Offset; 240 | int hall5Offset; 241 | int hall6Offset; 242 | int hall7Offset; 243 | int hall8Offset; 244 | int hall9Offset; 245 | int hall10Offset; 246 | int hall11Offset; 247 | int hall12Offset; 248 | 249 | // modes 250 | 251 | int mode = 0; 252 | 253 | // walk test positions 254 | 255 | int initStart; 256 | int state; 257 | float rate; 258 | 259 | int targetLeg1x; 260 | int targetLeg1z; 261 | int prevLeg1x; 262 | int prevLeg1z; 263 | float currentLeg1x; 264 | float currentLeg1z; 265 | float stepDiffLeg1x; 266 | float stepDiffLeg1z; 267 | 268 | int targetLeg2x; 269 | int targetLeg2z; 270 | int prevLeg2x; 271 | int prevLeg2z; 272 | float currentLeg2x; 273 | float currentLeg2z; 274 | float stepDiffLeg2x; 275 | float stepDiffLeg2z; 276 | 277 | int targetLeg3x; 278 | int targetLeg3z; 279 | int prevLeg3x; 280 | int prevLeg3z; 281 | float currentLeg3x; 282 | float currentLeg3z; 283 | float stepDiffLeg3x; 284 | float stepDiffLeg3z; 285 | 286 | int targetLeg4x; 287 | int targetLeg4z; 288 | int prevLeg4x; 289 | int prevLeg4z; 290 | float currentLeg4x; 291 | float currentLeg4z; 292 | float stepDiffLeg4x; 293 | float stepDiffLeg4z; 294 | 295 | int targetLeg1y; 296 | int prevLeg1y; 297 | float currentLeg1y; 298 | float stepDiffLeg1y; 299 | 300 | int walkXPos1; 301 | int walkXPos2; 302 | int walkXPos3; 303 | int walkXPos4; 304 | int walkXPos5; 305 | int walkXPos6; 306 | int walkXPos7; 307 | int walkXPos8; 308 | 309 | int walkYPos1; 310 | int walkYPos2; 311 | int walkYPos3; 312 | int walkYPos4; 313 | int walkYPos5; 314 | int walkYPos6; 315 | int walkYPos7; 316 | int walkYPos8; 317 | 318 | int walkZPos1; 319 | int walkZPos2; 320 | int walkZPos3; 321 | int walkZPos4; 322 | int walkZPos5; 323 | int walkZPos6; 324 | int walkZPos7; 325 | int walkZPos8; 326 | 327 | void setup() { 328 | 329 | // PID stuff 330 | 331 | PID1.SetMode(AUTOMATIC); 332 | PID1.SetOutputLimits(-50, 50); 333 | PID1.SetSampleTime(10); 334 | 335 | PID2.SetMode(AUTOMATIC); 336 | PID2.SetOutputLimits(-50, 50); 337 | PID2.SetSampleTime(10); 338 | 339 | PID3.SetMode(AUTOMATIC); 340 | PID3.SetOutputLimits(-50, 50); 341 | PID3.SetSampleTime(10); 342 | 343 | // radio stuff 344 | 345 | radio.begin(); 346 | radio.openWritingPipe(addresses[0]); // 00002 347 | radio.openReadingPipe(1, addresses[1]); // 00001 348 | radio.setPALevel(RF24_PA_MIN); 349 | 350 | radio.startListening(); 351 | 352 | // join I2C bus (I2Cdev library doesn't do this automatically) 353 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 354 | Wire.begin(); 355 | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties 356 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 357 | Fastwire::setup(400, true); 358 | #endif 359 | 360 | // initialize device 361 | mpu.initialize(); 362 | devStatus = mpu.dmpInitialize(); 363 | 364 | // supply your own gyro offsets here, scaled for min sensitivity 365 | mpu.setXGyroOffset(53); 366 | mpu.setYGyroOffset(-5); 367 | mpu.setZGyroOffset(51); 368 | mpu.setXAccelOffset(-2230); 369 | mpu.setYAccelOffset(-698); 370 | mpu.setZAccelOffset(2035); 371 | 372 | // make sure it worked (returns 0 if so) 373 | if (devStatus == 0) { 374 | // turn on the DMP, now that it's ready 375 | mpu.setDMPEnabled(true); 376 | 377 | // enable Arduino interrupt detection 378 | attachInterrupt(26, dmpDataReady, RISING); 379 | mpuIntStatus = mpu.getIntStatus(); 380 | 381 | // get expected DMP packet size for later comparison 382 | packetSize = mpu.dmpGetFIFOPacketSize(); 383 | } else { 384 | // ERROR! 385 | // 1 = initial memory load failed 386 | // 2 = DMP configuration updates failed 387 | // (if it's going to break, usually the code will be 1) 388 | Serial.print(F("DMP Initialization failed (code ")); 389 | Serial.print(devStatus); 390 | Serial.println(F(")")); 391 | 392 | 393 | } 394 | 395 | Serial.begin(115200); 396 | 397 | pinMode(A6, INPUT); 398 | pinMode(A7, INPUT); 399 | pinMode(A8, INPUT); 400 | 401 | pinMode(A9, INPUT); 402 | pinMode(A14, INPUT); 403 | pinMode(A15, INPUT); 404 | 405 | pinMode(A0, INPUT); 406 | pinMode(A1, INPUT); 407 | pinMode(A2, INPUT); 408 | 409 | pinMode(A3, INPUT); 410 | pinMode(A12, INPUT); 411 | pinMode(A13, INPUT); 412 | 413 | // write default positions with delays 414 | 415 | servo1.attach(24); // hips 416 | servo2.attach(25); 417 | servo3.attach(5); 418 | servo4.attach(2); 419 | 420 | servo1.writeMicroseconds(servo1Offset); // back left hip 421 | servo2.writeMicroseconds(servo2Offset); // back right hip 422 | servo3.writeMicroseconds(servo3Offset); // front left hip 423 | servo4.writeMicroseconds(servo4Offset); // front right hip 424 | 425 | delay(1000); 426 | 427 | servo5.attach(9); // shoulders 428 | servo6.attach(29); 429 | servo7.attach(6); 430 | servo8.attach(1); 431 | 432 | servo5.writeMicroseconds(servo5Offset); // back left shoulder 433 | servo6.writeMicroseconds(servo6Offset); // back right shoulder 434 | servo7.writeMicroseconds(servo7Offset); // front left shoulder 435 | servo8.writeMicroseconds(servo8Offset); // front right shoulder 436 | 437 | delay(1000); 438 | 439 | servo9.attach(8); // knees 440 | servo10.attach(30); 441 | servo11.attach(7); 442 | servo12.attach(0); 443 | 444 | servo9.writeMicroseconds(servo9Offset); // back left knee 445 | servo10.writeMicroseconds(servo10Offset); // back right knee 446 | servo11.writeMicroseconds(servo11Offset); // front left knee 447 | servo12.writeMicroseconds(servo12Offset); // front right knee 448 | 449 | // read hall effects on start up 450 | 451 | hall2Offset = analogRead(A7); // front left hip | lower number is more force 452 | hall3Offset = analogRead(A8); // front left shoulder | higher number is more force. 453 | hall4Offset = analogRead(A9); // front left knee | lower number is more force 454 | 455 | hall1Offset = analogRead(A6); // front right hip | higher number is more foce 456 | hall6Offset = analogRead(A15); // front right shoulder | lower nuber is more force 457 | hall5Offset = analogRead(A14); // front right knee | higher number is more force 458 | 459 | // back 460 | // forces are from the ground up 461 | 462 | hall7Offset = analogRead(A0); // back left hip | higher number is more force 463 | hall12Offset = analogRead(A13); // back left shoulder | lower number is more force 464 | hall11Offset = analogRead(A12); // back left knee | higher number is more force 465 | 466 | hall8Offset = analogRead(A1); // back right hip | lower number is more force 467 | hall9Offset = analogRead(A2); // back right shoulder | higher number is more force 468 | hall10Offset = analogRead(A3); // back right knee | lower number is more force 469 | 470 | } 471 | 472 | void loop() { 473 | 474 | currentMillis = millis(); 475 | if (currentMillis - previousMillis >= 10) { // start timed event 476 | 477 | previousMillis = currentMillis; 478 | 479 | // receive radio data 480 | 481 | if (radio.available()) { 482 | radio.read(&mydata_remote, sizeof(RECEIVE_DATA_STRUCTURE)); 483 | previousSafetyMillis = currentMillis; 484 | } 485 | 486 | // check if remote has become disconnected 487 | 488 | if (currentMillis - previousSafetyMillis > 500) { 489 | Serial.println("*no remote data* "); 490 | RLR = 512; 491 | RFB = 512; 492 | RT = 512; 493 | LLR = 512; 494 | LFB = 512; 495 | LT = 512; 496 | toggleTop = 0; 497 | toggleBottom = 0; 498 | toggle1 = 0; 499 | toggle2 = 0; 500 | Select = 0; 501 | } 502 | 503 | // use values if the remote is connected ok 504 | 505 | else { 506 | RLR = mydata_remote.RLR; 507 | RFB = mydata_remote.RFB; 508 | RT = mydata_remote.RT; 509 | LLR = mydata_remote.LLR; 510 | LFB = mydata_remote.LFB; 511 | LT = mydata_remote.LT; 512 | toggleTop = mydata_remote.toggleTop; 513 | toggleBottom = mydata_remote.toggleBottom; 514 | toggle1 = mydata_remote.toggle1; 515 | toggle2 = mydata_remote.toggle2; 516 | Select = mydata_remote.Select; 517 | } 518 | 519 | // Zero values to swing around +/- 0 520 | 521 | RLR = RLR - 512; // get to +/- zero value 522 | RFB = RFB - 512; 523 | RT = RT - 512; 524 | LLR = LLR - 512; 525 | LFB = LFB - 512; 526 | LT = LT - 512; 527 | 528 | // Threshold remote data for slop in sticks 529 | 530 | if (RLR > 50) { 531 | RLR = RLR -50; 532 | } 533 | else if (RLR < -50) { 534 | RLR = RLR +50; 535 | } 536 | else { 537 | RLR = 0; 538 | } 539 | //******* 540 | if (RFB > 50) { 541 | RFB = RFB -50; 542 | } 543 | else if (RFB < -50) { 544 | RFB = RFB +50; 545 | } 546 | else { 547 | RFB = 0; 548 | } 549 | //****** 550 | if (RT > 50) { 551 | RT = RT -50; 552 | } 553 | else if (RT < -50) { 554 | RT = RT +50; 555 | } 556 | else { 557 | RT = 0; 558 | } 559 | //****** 560 | if (LLR > 50) { 561 | LLR = LLR -50; 562 | } 563 | else if (LLR < -50) { 564 | LLR = LLR +50; 565 | } 566 | else { 567 | LLR = 0; 568 | } 569 | //******* 570 | if (LFB > 50) { 571 | LFB = LFB -50; 572 | } 573 | else if (LFB < -50) { 574 | LFB = LFB +50; 575 | } 576 | else { 577 | LFB = 0; 578 | } 579 | //****** 580 | if (LT > 50) { 581 | LT = LT -50; 582 | } 583 | else if (LT < -50) { 584 | LT = LT +50; 585 | } 586 | else { 587 | LT = 0; 588 | } 589 | 590 | // check for IMU inpterrupt 591 | 592 | if (IMUdataReady == 1) { 593 | readAngles(); 594 | } 595 | 596 | roll = (ypr[1] * 180/M_PI) - 1.5; 597 | pitch = (ypr[2] * 180/M_PI) + 0.7; 598 | 599 | // modes - use serial 600 | 601 | if (Serial.available()) { // check for serial data 602 | char c = Serial.read(); 603 | 604 | if (c == 'a') { // no compliance 605 | mode = 0; 606 | } 607 | else if (c == 'b') { // compliance on 608 | mode = 1; 609 | } 610 | else if (c == 'z') { // calibrate hall sensors 611 | mode = 99; 612 | } 613 | } 614 | 615 | // mode - use remote 616 | 617 | if (toggleTop == 0) { // no compliance 618 | mode = 0; 619 | } 620 | 621 | else if (toggleTop == 1) { // compliance on 622 | mode = 1; 623 | } 624 | 625 | if (Select == 1) { // calibrate hall sensors 626 | mode = 99; 627 | } 628 | 629 | 630 | // manual offset calibration of hall effect sensors (when dog is on the ground)- re-reads offsets 631 | 632 | if (mode == 99) { 633 | hall2Offset = analogRead(A7); // front left hip | lower number is more force 634 | hall3Offset = analogRead(A8); // front left shoulder | higher number is more force. 635 | hall4Offset = analogRead(A9); // front left knee | lower number is more force 636 | 637 | hall1Offset = analogRead(A6); // front right hip | higher number is more foce 638 | hall6Offset = analogRead(A15); // front right shoulder | lower nuber is more force 639 | hall5Offset = analogRead(A14); // front right knee | higher number is more force 640 | 641 | // back 642 | // forces are from the ground up 643 | 644 | hall7Offset = analogRead(A0); // back left hip | higher number is more force 645 | hall12Offset = analogRead(A13); // back left shoulder | lower number is more force 646 | hall11Offset = analogRead(A12); // back left knee | higher number is more force 647 | 648 | hall8Offset = analogRead(A1); // back right hip | lower number is more force 649 | hall9Offset = analogRead(A2); // back right shoulder | higher number is more force 650 | hall10Offset = analogRead(A3); // back right knee | lower number is more force 651 | } 652 | 653 | 654 | // *** read hall effect sensors on every loop *** 655 | 656 | // front 657 | // forces are from the ground up 658 | 659 | hall2 = analogRead(A7) - hall2Offset; // front left hip | lower number is more force 660 | hall3 = analogRead(A8) - hall3Offset; // front left shoulder | higher number is more force. 661 | hall4 = analogRead(A9) - hall4Offset; // front left knee | lower number is more force 662 | 663 | hall1 = analogRead(A6) - hall1Offset; // front right hip | higher number is more foce 664 | hall6 = analogRead(A15) - hall6Offset; // front right shoulder | lower nuber is more force 665 | hall5 = analogRead(A14) - hall5Offset; // front right knee | higher number is more force 666 | 667 | // back 668 | // forces are from the ground up 669 | 670 | hall7 = analogRead(A0) - hall7Offset; // back left hip | higher number is more force 671 | hall12 = analogRead(A13) - hall12Offset; // back left shoulder | lower number is more force 672 | hall11 = analogRead(A12) - hall11Offset; // back left knee | higher number is more force 673 | 674 | hall8 = analogRead(A1) - hall8Offset; // back right hip | lower number is more force 675 | hall9 = analogRead(A2) - hall9Offset; // back right shoulder | higher number is more force 676 | hall10 = analogRead(A3) - hall10Offset; // back right knee | lower number is more force 677 | 678 | 679 | // **************** Kinematic Model DEMO **************************** 680 | 681 | if (toggleBottom == 0) { 682 | 683 | // convert sticks to measurements in mm or degrees 684 | 685 | z = map(RT, -460,460,80,274); // overall height of the robot | Higher number makes the leg longer 686 | z = constrain(z,130,216); 687 | 688 | x = map(RFB, -460,460,-60,60); // front/back | Higher number moves the foot forward 689 | x = constrain(x,-60,60); 690 | 691 | y = map(RLR, -460,460,-60,60); // side/side | Higher number moves the foot left 692 | y = constrain(y,-60,60); 693 | 694 | r = map(LLR, -460, 460, -30, 30); // ROLL - covert to degrees of rotation 695 | r = constrain(r,-20,20); 696 | 697 | p = (map(LFB, -460, 460, 30, -30))-1; // PITCH - covert to degrees of rotation (there is a weird offset on my joystick hence -1) 698 | p = constrain(p,-20,20); 699 | 700 | yaw = map(LT, -460, 460, -30, 30); // YAW - covert to degrees of rotation (there is a weird offset on my joystick hence -1) 701 | yaw = constrain(yaw,-20,20); 702 | 703 | // print control data for debug 704 | 705 | Serial.print(" MODE: "); 706 | Serial.print(mode); 707 | Serial.print(" z: "); 708 | Serial.print(z); 709 | Serial.print(" x: "); 710 | Serial.print(x); 711 | Serial.print(" y: "); 712 | Serial.print(y); 713 | Serial.print(" r: "); 714 | Serial.print(r); 715 | Serial.print(" p: "); 716 | Serial.print(p); 717 | Serial.print(" yaw: "); 718 | Serial.print(yaw); 719 | Serial.println(); 720 | 721 | // send data to kinematic model function, compliance engine, and eventually write out to servos 722 | 723 | kinematics (1, mode, x,y,z,r,p,yaw); // front left leg 724 | kinematics (2, mode, x,y,z,r,p,yaw); // front right leg 725 | kinematics (3, mode, x,y,z,r,p,yaw); // back left leg 726 | kinematics (4, mode, x,y,z,r,p,yaw); // back right leg 727 | 728 | initStart = 0; 729 | 730 | } 731 | 732 | // **************** Start of test walking mode **************************** 733 | // ******************** STATICALLY STABLE ********************************* 734 | 735 | else if (toggleBottom == 1) { // position legs for walk test 736 | 737 | 738 | // define walking positions 739 | 740 | walkXPos1 = -45; 741 | walkXPos2 = 0; 742 | walkXPos3 = 45; 743 | walkXPos4 = 30; 744 | walkXPos5 = 15; 745 | walkXPos6 = 0; 746 | walkXPos7 = -15; 747 | walkXPos8 = -30; 748 | 749 | walkZPos1 = 215; // leg down 750 | walkZPos2 = 150; // leg up 751 | walkZPos3 = 215; // leg down 752 | walkZPos4 = 215; // leg down 753 | walkZPos5 = 215; // leg down 754 | walkZPos6 = 215; // leg down 755 | walkZPos7 = 215; // leg down 756 | walkZPos8 = 215; // leg down 757 | 758 | walkYPos1 = -25; 759 | walkYPos2 = -25; 760 | walkYPos3 = -25; 761 | walkYPos4 = -25; 762 | walkYPos5 = 25; 763 | walkYPos6 = 25; 764 | walkYPos7 = 25; 765 | walkYPos8 = 25; 766 | 767 | if (initStart == 0) { 768 | currentLeg1x = walkXPos3; 769 | currentLeg1z = walkZPos3; // leg1 down 770 | 771 | currentLeg2x = walkXPos7; 772 | currentLeg2z = walkZPos7; // leg2 down 773 | 774 | currentLeg3x = walkXPos5; 775 | currentLeg3z = walkZPos5; // leg2 down 776 | 777 | currentLeg4x = walkXPos1; 778 | currentLeg4z = walkZPos1; // leg2 down 779 | 780 | currentLeg1y = walkYPos1; // lean 781 | 782 | initStart = 1; 783 | } 784 | 785 | if (toggle1 == 1) { // start state machine for walking 786 | 787 | //scale the rate so it ranges from 1 to 9 and it's a float 788 | 789 | rate = (float) (RFB*-1)/100; 790 | rate = 9-rate; 791 | 792 | if (state == 0) { 793 | targetLeg1x = walkXPos3; 794 | targetLeg1z = walkZPos3; 795 | targetLeg2x = walkXPos7; 796 | targetLeg2z = walkZPos7; 797 | targetLeg3x = walkXPos5; 798 | targetLeg3z = walkZPos5; 799 | targetLeg4x = walkXPos1; 800 | targetLeg4z = walkZPos1; 801 | targetLeg1y = walkYPos1; 802 | if (currentLeg1x >= targetLeg1x) { 803 | state = 1; 804 | prevLeg1x = targetLeg1x; 805 | prevLeg1z = targetLeg1z; 806 | prevLeg2x = targetLeg2x; 807 | prevLeg2z = targetLeg2z; 808 | prevLeg3x = targetLeg3x; 809 | prevLeg3z = targetLeg3z; 810 | prevLeg4x = targetLeg4x; 811 | prevLeg4z = targetLeg4z; 812 | prevLeg1y = targetLeg1y; 813 | // check we actually get there due to dividing errors 814 | currentLeg1x = targetLeg1x; 815 | currentLeg1z = targetLeg1z; 816 | currentLeg2x = targetLeg2x; 817 | currentLeg2z = targetLeg2z; 818 | currentLeg3x = targetLeg3x; 819 | currentLeg3z = targetLeg3z; 820 | currentLeg4x = targetLeg4x; 821 | currentLeg4z = targetLeg4z; 822 | currentLeg1y = targetLeg1y; 823 | } 824 | } 825 | 826 | else if (state == 1) { 827 | targetLeg1x = walkXPos4; 828 | targetLeg1z = walkZPos4; 829 | targetLeg2x = walkXPos8; 830 | targetLeg2z = walkZPos8; 831 | targetLeg3x = walkXPos6; 832 | targetLeg3z = walkZPos6; 833 | targetLeg4x = walkXPos2; 834 | targetLeg4z = walkZPos2; 835 | targetLeg1y = walkYPos2; 836 | if (currentLeg1x <= targetLeg1x) { 837 | state = 2; 838 | prevLeg1x = targetLeg1x; 839 | prevLeg1z = targetLeg1z; 840 | prevLeg2x = targetLeg2x; 841 | prevLeg2z = targetLeg2z; 842 | prevLeg3x = targetLeg3x; 843 | prevLeg3z = targetLeg3z; 844 | prevLeg4x = targetLeg4x; 845 | prevLeg4z = targetLeg4z; 846 | prevLeg1y = targetLeg1y; 847 | // check we actually get there due to dividing errors 848 | currentLeg1x = targetLeg1x; 849 | currentLeg1z = targetLeg1z; 850 | currentLeg2x = targetLeg2x; 851 | currentLeg2z = targetLeg2z; 852 | currentLeg3x = targetLeg3x; 853 | currentLeg3z = targetLeg3z; 854 | currentLeg4x = targetLeg4x; 855 | currentLeg4z = targetLeg4z; 856 | currentLeg1y = targetLeg1y; 857 | } 858 | } 859 | 860 | else if (state == 2) { 861 | targetLeg1x = walkXPos5; 862 | targetLeg1z = walkZPos5; 863 | targetLeg2x = walkXPos1; 864 | targetLeg2z = walkZPos1; 865 | targetLeg3x = walkXPos7; 866 | targetLeg3z = walkZPos7; 867 | targetLeg4x = walkXPos3; 868 | targetLeg4z = walkZPos3; 869 | targetLeg1y = walkYPos3; 870 | if (currentLeg1x <= targetLeg1x) { 871 | state = 3; 872 | prevLeg1x = targetLeg1x; 873 | prevLeg1z = targetLeg1z; 874 | prevLeg2x = targetLeg2x; 875 | prevLeg2z = targetLeg2z; 876 | prevLeg3x = targetLeg3x; 877 | prevLeg3z = targetLeg3z; 878 | prevLeg4x = targetLeg4x; 879 | prevLeg4z = targetLeg4z; 880 | prevLeg1y = targetLeg1y; 881 | // check we actually get there due to dividing errors 882 | currentLeg1x = targetLeg1x; 883 | currentLeg1z = targetLeg1z; 884 | currentLeg2x = targetLeg2x; 885 | currentLeg2z = targetLeg2z; 886 | currentLeg3x = targetLeg3x; 887 | currentLeg3z = targetLeg3z; 888 | currentLeg4x = targetLeg4x; 889 | currentLeg4z = targetLeg4z; 890 | currentLeg1y = targetLeg1y; 891 | } 892 | } 893 | 894 | else if (state == 3) { 895 | targetLeg1x = walkXPos6; 896 | targetLeg1z = walkZPos6; 897 | targetLeg2x = walkXPos2; 898 | targetLeg2z = walkZPos2; 899 | targetLeg3x = walkXPos8; 900 | targetLeg3z = walkZPos8; 901 | targetLeg4x = walkXPos4; 902 | targetLeg4z = walkZPos4; 903 | targetLeg1y = walkYPos4; 904 | if (currentLeg1x <= targetLeg1x) { 905 | state = 4; 906 | prevLeg1x = targetLeg1x; 907 | prevLeg1z = targetLeg1z; 908 | prevLeg2x = targetLeg2x; 909 | prevLeg2z = targetLeg2z; 910 | prevLeg3x = targetLeg3x; 911 | prevLeg3z = targetLeg3z; 912 | prevLeg4x = targetLeg4x; 913 | prevLeg4z = targetLeg4z; 914 | prevLeg1y = targetLeg1y; 915 | // check we actually get there due to dividing errors 916 | currentLeg1x = targetLeg1x; 917 | currentLeg1z = targetLeg1z; 918 | currentLeg2x = targetLeg2x; 919 | currentLeg2z = targetLeg2z; 920 | currentLeg3x = targetLeg3x; 921 | currentLeg3z = targetLeg3z; 922 | currentLeg4x = targetLeg4x; 923 | currentLeg4z = targetLeg4z; 924 | currentLeg1y = targetLeg1y; 925 | } 926 | } 927 | 928 | else if (state == 4) { 929 | targetLeg1x = walkXPos7; 930 | targetLeg1z = walkZPos7; 931 | targetLeg2x = walkXPos3; 932 | targetLeg2z = walkZPos3; 933 | targetLeg3x = walkXPos1; 934 | targetLeg3z = walkZPos1; 935 | targetLeg4x = walkXPos5; 936 | targetLeg4z = walkZPos5; 937 | targetLeg1y = walkYPos5; 938 | if (currentLeg1x <= targetLeg1x) { 939 | state = 5; 940 | prevLeg1x = targetLeg1x; 941 | prevLeg1z = targetLeg1z; 942 | prevLeg2x = targetLeg2x; 943 | prevLeg2z = targetLeg2z; 944 | prevLeg3x = targetLeg3x; 945 | prevLeg3z = targetLeg3z; 946 | prevLeg4x = targetLeg4x; 947 | prevLeg4z = targetLeg4z; 948 | prevLeg1y = targetLeg1y; 949 | // check we actually get there due to dividing errors 950 | currentLeg1x = targetLeg1x; 951 | currentLeg1z = targetLeg1z; 952 | currentLeg2x = targetLeg2x; 953 | currentLeg2z = targetLeg2z; 954 | currentLeg3x = targetLeg3x; 955 | currentLeg3z = targetLeg3z; 956 | currentLeg4x = targetLeg4x; 957 | currentLeg4z = targetLeg4z; 958 | currentLeg1y = targetLeg1y; 959 | } 960 | } 961 | 962 | else if (state == 5) { 963 | targetLeg1x = walkXPos8; 964 | targetLeg1z = walkZPos8; 965 | targetLeg2x = walkXPos4; 966 | targetLeg2z = walkZPos4; 967 | targetLeg3x = walkXPos2; 968 | targetLeg3z = walkZPos2; 969 | targetLeg4x = walkXPos6; 970 | targetLeg4z = walkZPos6; 971 | targetLeg1y = walkYPos6; 972 | if (currentLeg1x <= targetLeg1x) { 973 | state = 6; 974 | prevLeg1x = targetLeg1x; 975 | prevLeg1z = targetLeg1z; 976 | prevLeg2x = targetLeg2x; 977 | prevLeg2z = targetLeg2z; 978 | prevLeg3x = targetLeg3x; 979 | prevLeg3z = targetLeg3z; 980 | prevLeg4x = targetLeg4x; 981 | prevLeg4z = targetLeg4z; 982 | prevLeg1y = targetLeg1y; 983 | // check we actually get there due to dividing errors 984 | currentLeg1x = targetLeg1x; 985 | currentLeg1z = targetLeg1z; 986 | currentLeg2x = targetLeg2x; 987 | currentLeg2z = targetLeg2z; 988 | currentLeg3x = targetLeg3x; 989 | currentLeg3z = targetLeg3z; 990 | currentLeg4x = targetLeg4x; 991 | currentLeg4z = targetLeg4z; 992 | currentLeg1y = targetLeg1y; 993 | } 994 | } 995 | 996 | else if (state == 6) { 997 | targetLeg1x = walkXPos1; 998 | targetLeg1z = walkZPos1; 999 | targetLeg2x = walkXPos5; 1000 | targetLeg2z = walkZPos5; 1001 | targetLeg3x = walkXPos3; 1002 | targetLeg3z = walkZPos3; 1003 | targetLeg4x = walkXPos7; 1004 | targetLeg4z = walkZPos7; 1005 | targetLeg1y = walkYPos7; 1006 | if (currentLeg1x <= targetLeg1x) { 1007 | state = 7; 1008 | prevLeg1x = targetLeg1x; 1009 | prevLeg1z = targetLeg1z; 1010 | prevLeg2x = targetLeg2x; 1011 | prevLeg2z = targetLeg2z; 1012 | prevLeg3x = targetLeg3x; 1013 | prevLeg3z = targetLeg3z; 1014 | prevLeg4x = targetLeg4x; 1015 | prevLeg4z = targetLeg4z; 1016 | prevLeg1y = targetLeg1y; 1017 | // check we actually get there due to dividing errors 1018 | currentLeg1x = targetLeg1x; 1019 | currentLeg1z = targetLeg1z; 1020 | currentLeg2x = targetLeg2x; 1021 | currentLeg2z = targetLeg2z; 1022 | currentLeg3x = targetLeg3x; 1023 | currentLeg3z = targetLeg3z; 1024 | currentLeg4x = targetLeg4x; 1025 | currentLeg4z = targetLeg4z; 1026 | currentLeg1y = targetLeg1y; 1027 | } 1028 | } 1029 | 1030 | else if (state == 7) { 1031 | targetLeg1x = walkXPos2; 1032 | targetLeg1z = walkZPos2; 1033 | targetLeg2x = walkXPos6; 1034 | targetLeg2z = walkZPos6; 1035 | targetLeg3x = walkXPos4; 1036 | targetLeg3z = walkZPos4; 1037 | targetLeg4x = walkXPos8; 1038 | targetLeg4z = walkZPos8; 1039 | targetLeg1y = walkYPos8; 1040 | if (currentLeg1x >= targetLeg1x) { 1041 | state = 0; 1042 | prevLeg1x = targetLeg1x; 1043 | prevLeg1z = targetLeg1z; 1044 | prevLeg2x = targetLeg2x; 1045 | prevLeg2z = targetLeg2z; 1046 | prevLeg3x = targetLeg3x; 1047 | prevLeg3z = targetLeg3z; 1048 | prevLeg4x = targetLeg4x; 1049 | prevLeg4z = targetLeg4z; 1050 | prevLeg1y = targetLeg1y; 1051 | // check we actually get there due to dividing errors 1052 | currentLeg1x = targetLeg1x; 1053 | currentLeg1z = targetLeg1z; 1054 | currentLeg2x = targetLeg2x; 1055 | currentLeg2z = targetLeg2z; 1056 | currentLeg3x = targetLeg3x; 1057 | currentLeg3z = targetLeg3z; 1058 | currentLeg4x = targetLeg4x; 1059 | currentLeg4z = targetLeg4z; 1060 | currentLeg1y = targetLeg1y; 1061 | } 1062 | } 1063 | 1064 | stepDiffLeg1x = (targetLeg1x - prevLeg1x)/(5*rate); 1065 | stepDiffLeg1z = (targetLeg1z - prevLeg1z)/(5*rate); 1066 | currentLeg1x = currentLeg1x + stepDiffLeg1x; 1067 | currentLeg1z = currentLeg1z + stepDiffLeg1z; 1068 | 1069 | stepDiffLeg2x = (targetLeg2x - prevLeg2x)/(5*rate); 1070 | stepDiffLeg2z = (targetLeg2z - prevLeg2z)/(5*rate); 1071 | currentLeg2x = currentLeg2x + stepDiffLeg2x; 1072 | currentLeg2z = currentLeg2z + stepDiffLeg2z; 1073 | 1074 | stepDiffLeg3x = (targetLeg3x - prevLeg3x)/(5*rate); 1075 | stepDiffLeg3z = (targetLeg3z - prevLeg3z)/(5*rate); 1076 | currentLeg3x = currentLeg3x + stepDiffLeg3x; 1077 | currentLeg3z = currentLeg3z + stepDiffLeg3z; 1078 | 1079 | stepDiffLeg4x = (targetLeg4x - prevLeg4x)/(5*rate); 1080 | stepDiffLeg4z = (targetLeg4z - prevLeg4z)/(5*rate); 1081 | currentLeg4x = currentLeg4x + stepDiffLeg4x; 1082 | currentLeg4z = currentLeg4z + stepDiffLeg4z; 1083 | 1084 | stepDiffLeg1y = (targetLeg1y - prevLeg1y)/(5*rate); 1085 | currentLeg1y = currentLeg1y + stepDiffLeg1y; 1086 | 1087 | } // end of state machine for walk test 1088 | 1089 | // offsets to balance centre of gravity statically 1090 | 1091 | x = map(RFB, -460,460,-20,20); // front/back | Higher number moves the foot forward 1092 | x = constrain(x,-20,0); 1093 | 1094 | int offsetX = (offsetX - 25) - x; 1095 | int offsetY = offsetY + 25; 1096 | 1097 | Serial.print(pitch); 1098 | Serial.print(" , "); 1099 | Serial.print(roll); 1100 | Serial.print(" , "); 1101 | Serial.print(x); 1102 | Serial.print(" , "); 1103 | Serial.print(rate); 1104 | Serial.print(" , "); 1105 | Serial.print(targetLeg2x); 1106 | Serial.print(" , "); 1107 | Serial.print(currentLeg2x); 1108 | Serial.print(" , "); 1109 | Serial.print(targetLeg2z); 1110 | Serial.print(" , "); 1111 | Serial.print(currentLeg2z); 1112 | 1113 | Serial.println(); 1114 | 1115 | p = 1.5; // bodge to keep the nose up 1116 | 1117 | kinematics (1, mode, currentLeg1x+offsetX, currentLeg1y-offsetY, currentLeg1z, r, p, yaw); // front left leg 1118 | kinematics (2, mode, currentLeg2x+offsetX, currentLeg1y+offsetY, currentLeg2z, r, p, yaw); // front right leg 1119 | kinematics (3, mode, currentLeg3x+offsetX, currentLeg1y-offsetY, currentLeg3z, r, p, yaw); // back left leg 1120 | kinematics (4, mode, currentLeg4x+offsetX, currentLeg1y+offsetY, currentLeg4z, r, p, yaw); // back right leg 1121 | } 1122 | 1123 | 1124 | } // end of timed loop 1125 | 1126 | } // end of main loop 1127 | 1128 | 1129 | 1130 | 1131 | -------------------------------------------------------------------------------- /miniDogV2_004_Static/readangle.ino: -------------------------------------------------------------------------------- 1 | // IMU interrupt service routine 2 | 3 | void dmpDataReady() { 4 | IMUdataReady = 1; 5 | } 6 | 7 | 8 | // function that actually read the angle when the flag is set by the ISR 9 | 10 | void readAngles() { 11 | 12 | mpuIntStatus = mpu.getIntStatus(); 13 | fifoCount = mpu.getFIFOCount(); 14 | 15 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 16 | // reset so we can continue cleanly 17 | mpu.resetFIFO(); 18 | Serial.println(F("FIFO overflow!")); 19 | } 20 | 21 | else if (mpuIntStatus & 0x02) { 22 | // wait for correct available data length, should be a VERY short wait 23 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 24 | 25 | // read a packet from FIFO 26 | mpu.getFIFOBytes(fifoBuffer, packetSize); 27 | 28 | // track FIFO count here in case there is > 1 packet available 29 | // (this lets us immediately read more without waiting for an interrupt) 30 | fifoCount -= packetSize; 31 | 32 | mpu.dmpGetQuaternion(&q, fifoBuffer); 33 | mpu.dmpGetGravity(&gravity, &q); 34 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 35 | 36 | IMUdataReady = 0; 37 | } 38 | } 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /miniDogV2_011_Dynamic/Filter.ino: -------------------------------------------------------------------------------- 1 | // motion filter to filter motions and compliance 2 | 3 | float filter(float prevValue, float currentValue, int filter) { 4 | float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1); 5 | return lengthFiltered; 6 | } 7 | -------------------------------------------------------------------------------- /miniDogV2_011_Dynamic/Kinematics.ino: -------------------------------------------------------------------------------- 1 | void kinematics (int leg, int mode, float x, float y, float z, float roll, float pitch, float yaw) { 2 | 3 | // *** TRANSLATION AXIS *** 4 | 5 | // moving the foot sideways on the end plane 6 | #define hipOffset 76.5 7 | float lengthY; 8 | float hipAngle1a; 9 | float hipAngle1b; 10 | float hipAngle1; 11 | float hipAngle1Degrees; 12 | float hipHyp; 13 | 14 | // moving the foot forwards or backwardes in the side plane 15 | float shoulderAngle2; 16 | float shoulderAngle2Degrees; 17 | float z2; 18 | 19 | // side plane of individual leg only 20 | #define shinLength 125 21 | #define thighLength 125 22 | float z3; 23 | float shoulderAngle1; 24 | float shoulderAngle1Degrees; 25 | float shoulderAngle1a; 26 | float shoulderAngle1b; 27 | float shoulderAngle1c; 28 | float shoulderAngle1d; 29 | float kneeAngle; 30 | float kneeAngleDegrees; 31 | 32 | // *** ROTATION AXIS 33 | 34 | // roll axis 35 | #define bodyWidth 126 // half the distance from the middle of the body to the hip pivot 36 | float legDiffRoll; // differnece in height for each leg 37 | float bodyDiffRoll; // how much shorter the 'virtual body' gets 38 | float footDisplacementRoll; // where the foot actually is 39 | float footDisplacementAngleRoll; // smaller angle 40 | float footWholeAngleRoll; // whole leg angle 41 | float hipRollAngle; // angle for hip when roll axis is in use 42 | float rollAngle; // angle in RADIANS that the body rolls 43 | float zz1a; // hypotenuse of final triangle 44 | float zz1; // new height for leg to pass onto the next bit of code 45 | float yy1; // new position for leg to move sideways 46 | 47 | // pitch axis 48 | 49 | #define bodyLength 147.5 // distance from centre of body to shoulder pivot 50 | float legDiffPitch; // differnece in height for each leg 51 | float bodyDiffPitch; // how much shorter the 'virtual body' gets 52 | float footDisplacementPitch; // where the foot actually is 53 | float footDisplacementAnglePitch; // smaller angle 54 | float footWholeAnglePitch; // whole leg angle 55 | float shoulderPitchAngle; // angle for hip when roll axis is in use 56 | float pitchAngle; // angle in RADIANS that the body rolls 57 | float zz2a; // hypotenuse of final triangle 58 | float zz2; // new height for the leg to pass onto the next bit of code 59 | float xx1; // new position to move the leg fowwards/backwards 60 | 61 | // yaw axis 62 | 63 | float yawAngle; // angle in RADIANs for rotation in yaw 64 | float existingAngle; // existing angle of leg from centre 65 | float radius; // radius of leg from centre of robot based on x and y from sticks 66 | float demandYaw; // demand yaw postion - existing yaw plus the stick yaw 67 | float xx3; // new X coordinate based on demand angle 68 | float yy3; // new Y coordinate based on demand angle 69 | 70 | // ****************************************************************************************************** 71 | // ***************************** KINEMATIC MODEL CALCS START HERE *************************************** 72 | // ****************************************************************************************************** 73 | 74 | // convert degrees to radians for the calcs 75 | yawAngle = (PI/180) * yaw; 76 | 77 | // put in offsets from robot's parameters so we can work out the radius of the foot from the robot's centre 78 | if (leg == 1) { // front left leg 79 | y = y - (bodyWidth+hipOffset); 80 | x = x - bodyLength; 81 | } 82 | else if (leg == 2) { // front right leg 83 | y = y + (bodyWidth+hipOffset); 84 | x = x - bodyLength; 85 | } 86 | else if (leg == 3) { // back left leg 87 | y = y - (bodyWidth+hipOffset); 88 | x = x + bodyLength; 89 | } 90 | else if (leg == 4) { // back left leg 91 | y = y + (bodyWidth+hipOffset); 92 | x = x + bodyLength; 93 | } 94 | 95 | //calc existing angle of leg from cetre 96 | existingAngle = atan(y/x); 97 | 98 | // calc radius from centre 99 | radius = y/sin(existingAngle); 100 | 101 | //calc demand yaw angle 102 | demandYaw = existingAngle + yawAngle; 103 | 104 | // calc new X and Y based on demand yaw angle 105 | xx3 = radius * cos(demandYaw); // calc new X and Y based on new yaw angle 106 | yy3 = radius * sin(demandYaw); 107 | 108 | // remove the offsets so we pivot around 0/0 x/y 109 | if (leg == 1) { // front left leg 110 | yy3 = yy3 + (bodyWidth+hipOffset); 111 | xx3 = xx3 + bodyLength; 112 | } 113 | else if (leg == 2) { // front right leg 114 | yy3 = yy3 - (bodyWidth+hipOffset); 115 | xx3 = xx3 + bodyLength; 116 | } 117 | else if (leg == 3) { // back left leg 118 | yy3 = yy3 + (bodyWidth+hipOffset); 119 | xx3 = xx3 - bodyLength; 120 | } 121 | else if (leg == 4) { // back left leg 122 | yy3 = yy3 - (bodyWidth+hipOffset); 123 | xx3 = xx3 - bodyLength; 124 | } 125 | 126 | // *** PITCH AXIS *** 127 | 128 | //turn around the pitch for front or back of the robot 129 | if (leg == 1 || leg == 2) { 130 | pitch = 0-pitch; 131 | } 132 | else if (leg == 3 || leg == 4) { 133 | pitch = 0+pitch; 134 | xx3 = xx3*-1; // switch over x for each end of the robot 135 | } 136 | 137 | // convert pitch to degrees 138 | pitchAngle = (PI/180) * pitch; 139 | 140 | //calc top triangle sides 141 | legDiffPitch = sin(pitchAngle) * bodyLength; 142 | bodyDiffPitch = cos(pitchAngle) * bodyLength; 143 | 144 | // calc actual height from the ground for each side 145 | legDiffPitch = z - legDiffPitch; 146 | 147 | // calc foot displacement 148 | footDisplacementPitch = ((bodyDiffPitch - bodyLength)*-1)+xx3; 149 | 150 | //calc smaller displacement angle 151 | footDisplacementAnglePitch = atan(footDisplacementPitch/legDiffPitch); 152 | 153 | //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle) 154 | zz2a = legDiffPitch/cos(footDisplacementAnglePitch); 155 | 156 | // calc the whole angle for the leg 157 | footWholeAnglePitch = footDisplacementAnglePitch + pitchAngle; 158 | 159 | //calc actual leg length - the new Z to pass on 160 | zz2 = cos(footWholeAnglePitch) * zz2a; 161 | 162 | //calc new Z to pass on 163 | xx1 = sin(footWholeAnglePitch) * zz2a; 164 | 165 | if (leg == 3 || leg == 4 ){ // switch back X for the back of the robot 166 | xx1 = xx1 *-1; 167 | } 168 | 169 | 170 | // *** ROLL AXIS *** 171 | 172 | //turn around roll angle for each side of the robot 173 | if (leg == 1 || leg == 3) { 174 | roll = 0+roll; 175 | yy3 = yy3*-1; 176 | } 177 | else if (leg == 2 || leg == 4) { 178 | roll = 0-roll; 179 | } 180 | 181 | // convert roll angle to radians 182 | rollAngle = (PI/180) * roll; //covert degrees from the stick to radians 183 | 184 | // calc the top triangle sides 185 | legDiffRoll = sin(rollAngle) * bodyWidth; 186 | bodyDiffRoll = cos(rollAngle) * bodyWidth; 187 | 188 | // calc actual height from the ground for each side 189 | legDiffRoll = zz2 - legDiffRoll; 190 | 191 | // calc foot displacement 192 | footDisplacementRoll = ((bodyDiffRoll - bodyWidth)*-1)-yy3; 193 | 194 | //calc smaller displacement angle 195 | footDisplacementAngleRoll = atan(footDisplacementRoll/legDiffRoll); 196 | 197 | //calc distance from the ground at the displacement angle (the hypotenuse of the final triangle) 198 | zz1a = legDiffRoll/cos(footDisplacementAngleRoll); 199 | 200 | // calc the whole angle for the leg 201 | footWholeAngleRoll = footDisplacementAngleRoll + rollAngle; 202 | 203 | //calc actual leg length - the new Z to pass on 204 | zz1 = cos(footWholeAngleRoll) * zz1a; 205 | 206 | //calc new Y to pass on 207 | yy1 = sin(footWholeAngleRoll) * zz1a; 208 | 209 | // *** TRANSLATION AXIS *** 210 | 211 | // calculate the hip joint and new leg length based on how far the robot moves sideways 212 | hipAngle1 = atan(yy1/zz1); 213 | hipAngle1Degrees = ((hipAngle1 * (180/PI))); // convert to degrees and take off rest position 214 | z2 = zz1/cos(hipAngle1); 215 | 216 | // **************** 217 | 218 | // calculate the shoulder joint offset and new leg length based on now far the foot moves forward/backwards 219 | shoulderAngle2 = atan(xx1/z2); // calc how much extra to add to the shoulder joint 220 | shoulderAngle2Degrees = shoulderAngle2 * (180/PI); 221 | z3 = z2/cos(shoulderAngle2); // calc new leg length to feed to the next bit of code below 222 | 223 | // **************** 224 | 225 | // calculate leg length based on shin/thigh length and knee and shoulder angle 226 | shoulderAngle1a = sq(thighLength) + sq(z3) - sq(shinLength); 227 | shoulderAngle1b = 2 * thighLength * z3; 228 | shoulderAngle1c = shoulderAngle1a / shoulderAngle1b; 229 | shoulderAngle1 = acos(shoulderAngle1c); // radians 230 | kneeAngle = PI - (shoulderAngle1 *2); // radians 231 | 232 | //calc degrees from angles 233 | shoulderAngle1Degrees = shoulderAngle1 * (180/PI); // degrees 234 | kneeAngleDegrees = kneeAngle * (180/PI); // degrees 235 | 236 | 237 | // ****************************************************************************************************** 238 | // ***************** compliance / filtering / write out servo positoion below this point ***************** 239 | // ****************************************************************************************************** 240 | 241 | if (leg == 1) { // *front left leg* 242 | 243 | // convert degrees to servo microSeconds 244 | servo11PosTrack = (kneeAngleDegrees-90) * -25; // positive scaler 245 | servo7PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25) ; 246 | servo3PosTrack = (hipAngle1Degrees * -25); 247 | 248 | // no compliance mode 249 | if (mode == 0) { 250 | servo3Pos = servo3PosTrack; // front left hip 251 | servo7Pos = servo7PosTrack; // front left shoulder 252 | servo11Pos = servo11PosTrack; // front right knee 253 | } 254 | // compliance mode 255 | else if (mode == 1) { 256 | //*** front left hip *** 257 | if (hall2 > threshholdGlobal || hall2 < (threshholdGlobal*-1)) { 258 | servo3Pos = servo3Pos + (hall2 * multiplierHipsLeft); 259 | } 260 | else { // return to centre 261 | servo3Pos = servo3PosTrack; 262 | } 263 | //*** front left shoulder 264 | if (hall3 > threshholdGlobal || hall3 < (threshholdGlobal*-1)) { 265 | servo7Pos = servo7Pos + (hall3 * multiplierShouldersLeft); 266 | } 267 | else { // return to centre 268 | servo7Pos = servo7PosTrack; 269 | } 270 | // ***front left knee 271 | if (hall4 > threshholdGlobal || hall4 < (threshholdGlobal*-1)) { 272 | servo11Pos = servo11Pos + (hall4 * multiplierKneesLeft); 273 | } 274 | else { // return to centre 275 | servo11Pos = servo11PosTrack; 276 | } 277 | } 278 | // filter motions 279 | servo3PosFiltered = filter(servo3Pos, servo3PosFiltered, filterHipsLeft); 280 | servo3PosFiltered = constrain(servo3PosFiltered,-900,900); 281 | servo7PosFiltered = filter(servo7Pos, servo7PosFiltered, filterShouldersLeft); 282 | servo7PosFiltered = constrain(servo7PosFiltered,-900,900); 283 | servo11PosFiltered = filter(servo11Pos, servo11PosFiltered, filterKneesLeft); 284 | servo11PosFiltered = constrain(servo11PosFiltered,-900,900); 285 | // write out servo positions 286 | servo3.writeMicroseconds(servo3Offset + servo3PosFiltered); // front left hip 287 | servo7.writeMicroseconds(servo7Offset + servo7PosFiltered); // front left shoulder 288 | servo11.writeMicroseconds(servo11Offset + servo11PosFiltered); // front right knee 289 | } 290 | 291 | else if (leg == 2) { // *front right leg* 292 | 293 | // convert degrees to servo microSeconds 294 | servo12PosTrack = (kneeAngleDegrees-90) * 25; 295 | servo8PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25); 296 | servo4PosTrack = (hipAngle1Degrees * 25); 297 | 298 | // no compliance mode 299 | if (mode == 0) { 300 | servo4Pos = servo4PosTrack; // front right hip 301 | servo8Pos = servo8PosTrack; // front right shoulder 302 | servo12Pos = servo12PosTrack; // front right knee 303 | } 304 | // compliance mode 305 | if (mode == 1) { 306 | //*** front right hip *** 307 | if (hall1 > threshholdGlobal || hall1 < (threshholdGlobal*-1)) { 308 | servo4Pos = servo4Pos + (hall1 * multiplierHipsRight); 309 | } 310 | else { // return to centre 311 | servo4Pos = servo4PosTrack; 312 | } 313 | //*** front right shoulder 314 | if (hall6 > threshholdGlobal || hall6 < (threshholdGlobal*-1)) { 315 | servo8Pos = servo8Pos + (hall6 * multiplierShouldersRight); 316 | } 317 | else { // return to centre 318 | servo8Pos = servo8PosTrack; 319 | } 320 | // ***front right knee 321 | if (hall5 > threshholdGlobal || hall5 < (threshholdGlobal*-1)) { 322 | servo12Pos = servo12Pos + (hall5 * multiplierKneesRight); 323 | } 324 | else { // return to centre 325 | servo12Pos = servo12PosTrack; 326 | } 327 | } 328 | // filter motions 329 | servo4PosFiltered = filter(servo4Pos, servo4PosFiltered, filterHipsRight); 330 | servo4PosFiltered = constrain(servo4PosFiltered,-900,900); 331 | servo8PosFiltered = filter(servo8Pos, servo8PosFiltered, filterShouldersRight); 332 | servo8PosFiltered = constrain(servo8PosFiltered,-900,900); 333 | servo12PosFiltered = filter(servo12Pos, servo12PosFiltered, filterKneesRight); 334 | servo12PosFiltered = constrain(servo12PosFiltered,-900,900); 335 | // write out servo potitions 336 | servo4.writeMicroseconds(servo4Offset + servo4PosFiltered); // front right hip 337 | servo8.writeMicroseconds(servo8Offset + servo8PosFiltered); // front right shoulder 338 | servo12.writeMicroseconds(servo12Offset + servo12PosFiltered); // front right knee 339 | } 340 | 341 | else if (leg == 3) { // *back left leg* 342 | 343 | // convert degrees to servo microSeconds 344 | servo9PosTrack = (kneeAngleDegrees-90) * -25; 345 | servo5PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25); 346 | servo1PosTrack = (hipAngle1Degrees * -25); 347 | 348 | // no compliance mode 349 | if (mode == 0) { 350 | servo1Pos = servo1PosTrack; // back left hip 351 | servo5Pos = servo5PosTrack; // back left shoulder 352 | servo9Pos = servo9PosTrack; // back left knee 353 | } 354 | // compliance mode 355 | if (mode == 1) { 356 | //*** back left hip *** 357 | if (hall7 > threshholdGlobal || hall7 < (threshholdGlobal*-1)) { 358 | servo1Pos = servo1Pos + (hall7 * multiplierHipsLeft); 359 | } 360 | else { // return to centre 361 | servo1Pos = servo1PosTrack; 362 | } 363 | //*** back left shoulder 364 | if (hall12 > threshholdGlobal || hall12 < (threshholdGlobal*-1)) { 365 | servo5Pos = servo5Pos + (hall12 * multiplierShouldersLeft); 366 | } 367 | else { // return to centre 368 | servo5Pos = servo5PosTrack; 369 | } 370 | // ***back left knee 371 | if (hall11 > threshholdGlobal || hall11 < (threshholdGlobal*-1)) { 372 | servo9Pos = servo9Pos + (hall11 * multiplierKneesLeft); 373 | } 374 | else { // return to centre 375 | servo9Pos = servo9PosTrack; 376 | } 377 | } 378 | // filter motions 379 | servo1PosFiltered = filter(servo1Pos, servo1PosFiltered, filterHipsLeft); 380 | servo1PosFiltered = constrain(servo1PosFiltered,-900,900); 381 | servo5PosFiltered = filter(servo5Pos, servo5PosFiltered, filterShouldersLeft); 382 | servo5PosFiltered = constrain(servo5PosFiltered,-900,900); 383 | servo9PosFiltered = filter(servo9Pos, servo9PosFiltered, filterKneesLeft); 384 | servo9PosFiltered = constrain(servo9PosFiltered,-900,900); 385 | // write out servo positions 386 | servo1.writeMicroseconds(servo1Offset + servo1PosFiltered); // back left hip 387 | servo5.writeMicroseconds(servo5Offset + servo5PosFiltered); // back left shoulder 388 | servo9.writeMicroseconds(servo9Offset + servo9PosFiltered); // front left knee 389 | } 390 | 391 | else if (leg == 4) { // *back right leg* 392 | 393 | // convert degrees to servo microSeconds 394 | servo10PosTrack = (kneeAngleDegrees-90) * 25; 395 | servo6PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25); 396 | servo2PosTrack = (hipAngle1Degrees * 25); 397 | 398 | // no compliance mode 399 | if (mode == 0) { 400 | servo2Pos = servo2PosTrack; // back right hip 401 | servo6Pos = servo6PosTrack; // back right shoulder 402 | servo10Pos = servo10PosTrack; // back right knee 403 | } 404 | // compliance mode 405 | if (mode == 1) { 406 | //*** back right hip *** 407 | if (hall8 > threshholdGlobal || hall8 < (threshholdGlobal*-1)) { 408 | servo2Pos = servo2Pos + (hall8 * multiplierHipsRight); 409 | } 410 | else { // return to centre 411 | servo2Pos = servo2PosTrack; 412 | } 413 | //*** back right shoulder 414 | if (hall9 > threshholdGlobal || hall9 < (threshholdGlobal*-1)) { 415 | servo6Pos = servo6Pos + (hall9 * multiplierShouldersRight); 416 | } 417 | else { // return to centre 418 | servo6Pos = servo6PosTrack; 419 | } 420 | // ***back right knee 421 | if (hall10 > threshholdGlobal || hall10 < (threshholdGlobal*-1)) { 422 | servo10Pos = servo10Pos + (hall10 * multiplierKneesRight); 423 | } 424 | else { // return to centre 425 | servo10Pos = servo10PosTrack; 426 | } 427 | } 428 | // filter motions 429 | servo2PosFiltered = filter(servo2Pos, servo2PosFiltered, filterHipsRight); 430 | servo2PosFiltered = constrain(servo2PosFiltered,-900,900); 431 | servo6PosFiltered = filter(servo6Pos, servo6PosFiltered, filterShouldersRight); 432 | servo6PosFiltered = constrain(servo6PosFiltered,-900,900); 433 | servo10PosFiltered = filter(servo10Pos, servo10PosFiltered, filterKneesRight); 434 | servo10PosFiltered = constrain(servo10PosFiltered,-900,900); 435 | // write out servo positions 436 | servo2.writeMicroseconds(servo2Offset + servo2PosFiltered); // back right hip 437 | servo6.writeMicroseconds(servo6Offset + servo6PosFiltered); // back left shoulder 438 | servo10.writeMicroseconds(servo10Offset + servo10PosFiltered); // front left knee 439 | } 440 | 441 | 442 | 443 | } // end of kinematics 444 | -------------------------------------------------------------------------------- /miniDogV2_011_Dynamic/miniDogV2_011_Dynamic.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include //PID loop from http://playground.arduino.cc/Code/PIDLibrary 6 | 7 | double Pk1 = 2; 8 | double Ik1 = 0; 9 | double Dk1 = 0; 10 | 11 | double Setpoint1, Input1, Output1; // PID variables 12 | PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT); // PID Setup 13 | 14 | double Pk2 = 2; 15 | double Ik2 = 3; 16 | double Dk2 = 0; 17 | 18 | double Setpoint2, Input2, Output2; // PID variables 19 | PID PID2(&Input2, &Output2, &Setpoint2, Pk2, Ik2 , Dk2, DIRECT); // PID Setup 20 | 21 | double Pk3 = 2; 22 | double Ik3 = 3; 23 | double Dk3 = 0; 24 | 25 | double Setpoint3, Input3, Output3; // PID variables 26 | PID PID3(&Input3, &Output3, &Setpoint3, Pk3, Ik3 , Dk3, DIRECT); // PID Setup 27 | 28 | RF24 radio(27, 10); // CE, CSN 29 | const byte addresses[][6] = {"00001", "00002"}; 30 | 31 | //IMU stuff 32 | #include "I2Cdev.h" 33 | #include "MPU6050_6Axis_MotionApps20.h" 34 | #include "Wire.h" 35 | int requested_state; 36 | 37 | // class default I2C address is 0x68 38 | // specific I2C addresses may be passed as a parameter here 39 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 40 | // AD0 high = 0x69 41 | MPU6050 mpu; 42 | //MPU6050 mpu(0x69); // <-- use for AD0 high 43 | 44 | // MPU control/status vars 45 | bool dmpReady = false; // set true if DMP init was successful 46 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 47 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 48 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 49 | uint16_t fifoCount; // count of all bytes currently in FIFO 50 | uint8_t fifoBuffer[64]; // FIFO storage buffer 51 | 52 | // orientation/motion vars 53 | Quaternion q; // [w, x, y, z] quaternion container 54 | VectorInt16 aa; // [x, y, z] accel sensor measurements 55 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 56 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 57 | VectorFloat gravity; // [x, y, z] gravity vector 58 | float euler[3]; // [psi, theta, phi] Euler angle container 59 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 60 | 61 | float pitch; 62 | float roll; 63 | float rollFiltered; 64 | 65 | #define PI 3.1415926535897932384626433832795 66 | 67 | int IMUdataReady = 0; 68 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 69 | 70 | Servo servo1; 71 | Servo servo2; 72 | Servo servo3; 73 | Servo servo4; 74 | 75 | Servo servo5; 76 | Servo servo6; 77 | Servo servo7; 78 | Servo servo8; 79 | 80 | Servo servo9; 81 | Servo servo10; 82 | Servo servo11; 83 | Servo servo12; 84 | 85 | //**************remote control**************** 86 | struct RECEIVE_DATA_STRUCTURE{ 87 | //put your variable definitions here for the data you want to send 88 | //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO 89 | 90 | int16_t menuDown; 91 | int16_t Select; 92 | int16_t menuUp; 93 | int16_t toggleBottom; 94 | int16_t toggleTop; 95 | int16_t toggle1; 96 | int16_t toggle2; 97 | int16_t mode; 98 | int16_t RLR; 99 | int16_t RFB; 100 | int16_t RT; 101 | int16_t LLR; 102 | int16_t LFB; 103 | int16_t LT; 104 | int16_t checkit; 105 | int16_t checkit2; 106 | }; 107 | 108 | RECEIVE_DATA_STRUCTURE mydata_remote; 109 | 110 | int RLR; 111 | int RFB; 112 | int RT; 113 | int LLR; 114 | int LFB; 115 | int LT; 116 | int toggleTop; 117 | int toggleBottom; 118 | int toggle1; 119 | int toggle2; 120 | int Select; 121 | 122 | int x; 123 | int y; 124 | int z; 125 | 126 | int yaw; 127 | int r; 128 | int p; 129 | 130 | 131 | // timers 132 | 133 | unsigned long currentMillis; 134 | unsigned long previousMillis; 135 | unsigned long previousWalkMillis; 136 | unsigned long previousSafetyMillis; 137 | 138 | // There is a first order filter running on all motions, allbeit turned down a lot. It's also used for the reactivity of the compliance 139 | 140 | float multiplierKneesRight = 2; 141 | float multiplierShouldersRight = 2; 142 | float multiplierHipsRight = 0; 143 | 144 | float multiplierKneesLeft = 2; 145 | float multiplierShouldersLeft = 2; 146 | float multiplierHipsLeft = 0; 147 | 148 | // filters values 149 | 150 | int filterKneesRight = 15; 151 | int filterShouldersRight = 15; 152 | int filterHipsRight = 25; 153 | 154 | int filterKneesLeft = 15; 155 | int filterShouldersLeft = 15; 156 | int filterHipsLeft = 25; 157 | 158 | // global joint threshold value for hall effects 159 | int threshholdGlobal = 5; 160 | 161 | // values to write out to servos 162 | 163 | float servo1Pos; // initial value 164 | float servo2Pos; 165 | float servo3Pos; 166 | float servo4Pos; 167 | float servo5Pos; 168 | float servo6Pos; 169 | float servo7Pos; 170 | float servo8Pos; 171 | float servo9Pos; 172 | float servo10Pos; 173 | float servo11Pos; 174 | float servo12Pos; 175 | 176 | float servo1PosTrack; // ongoing tracking value 177 | float servo2PosTrack; 178 | float servo3PosTrack; 179 | float servo4PosTrack; 180 | float servo5PosTrack; 181 | float servo6PosTrack; 182 | float servo7PosTrack; 183 | float servo8PosTrack; 184 | float servo9PosTrack; 185 | float servo10PosTrack; 186 | float servo11PosTrack; 187 | float servo12PosTrack; 188 | 189 | float servo1PosFiltered; // filtered values 190 | float servo2PosFiltered; 191 | float servo3PosFiltered; 192 | float servo4PosFiltered; 193 | float servo5PosFiltered; 194 | float servo6PosFiltered; 195 | float servo7PosFiltered; 196 | float servo8PosFiltered; 197 | float servo9PosFiltered; 198 | float servo10PosFiltered; 199 | float servo11PosFiltered; 200 | float servo12PosFiltered; 201 | 202 | // ******************************************************************************* 203 | // servo offsets for default position - knees at 45', hips at 0', shoulders at 45' 204 | // ******************************************************************************* 205 | 206 | int servo1Offset = 1480; // these are really important to get as accurate as possible 207 | int servo2Offset = 1570; // they are used as home positions of all servos 208 | int servo3Offset = 1500; 209 | int servo4Offset = 1600; 210 | 211 | int servo5Offset = 1500; 212 | int servo6Offset = 1340; 213 | int servo7Offset = 1600; 214 | int servo8Offset = 1570; 215 | 216 | int servo9Offset = 1630; 217 | int servo10Offset = 1550; 218 | int servo11Offset = 1550; 219 | int servo12Offset = 1400; 220 | 221 | // ******************************************************************************* 222 | 223 | // hall effect sensors - not used in miniDog V2 but the coe is still included 224 | 225 | float hall1; 226 | float hall2; 227 | float hall3; 228 | float hall4; 229 | float hall5; 230 | float hall6; 231 | float hall7; 232 | float hall8; 233 | float hall9; 234 | float hall10; 235 | float hall11; 236 | float hall12; 237 | 238 | // offset values for auto-calibration at startup 239 | 240 | int hall1Offset; 241 | int hall2Offset; 242 | int hall3Offset; 243 | int hall4Offset; 244 | int hall5Offset; 245 | int hall6Offset; 246 | int hall7Offset; 247 | int hall8Offset; 248 | int hall9Offset; 249 | int hall10Offset; 250 | int hall11Offset; 251 | int hall12Offset; 252 | 253 | // modes 254 | 255 | int mode = 0; 256 | 257 | // walk test positions 258 | 259 | int initStart; 260 | int state; 261 | float rate; 262 | 263 | int targetLeg1x; 264 | int targetLeg1z; 265 | int prevLeg1x; 266 | int prevLeg1z; 267 | float currentLeg1x; 268 | float currentLeg1z; 269 | float stepDiffLeg1x; 270 | float stepDiffLeg1z; 271 | 272 | int targetLeg2x; 273 | int targetLeg2z; 274 | int prevLeg2x; 275 | int prevLeg2z; 276 | float currentLeg2x; 277 | float currentLeg2z; 278 | float stepDiffLeg2x; 279 | float stepDiffLeg2z; 280 | 281 | int targetLeg3x; 282 | int targetLeg3z; 283 | int prevLeg3x; 284 | int prevLeg3z; 285 | float currentLeg3x; 286 | float currentLeg3z; 287 | float stepDiffLeg3x; 288 | float stepDiffLeg3z; 289 | 290 | int targetLeg4x; 291 | int targetLeg4z; 292 | int prevLeg4x; 293 | int prevLeg4z; 294 | float currentLeg4x; 295 | float currentLeg4z; 296 | float stepDiffLeg4x; 297 | float stepDiffLeg4z; 298 | 299 | int targetLeg1y; 300 | int prevLeg1y; 301 | float currentLeg1y; 302 | float stepDiffLeg1y; 303 | 304 | int walkXPos1; 305 | int walkXPos2; 306 | int walkXPos3; 307 | int walkXPos4; 308 | int walkXPos5; 309 | int walkXPos6; 310 | int walkXPos7; 311 | int walkXPos8; 312 | 313 | int walkYPos1; 314 | int walkYPos2; 315 | int walkYPos3; 316 | int walkYPos4; 317 | int walkYPos5; 318 | int walkYPos6; 319 | int walkYPos7; 320 | int walkYPos8; 321 | 322 | int walkZPos1; 323 | int walkZPos2; 324 | int walkZPos3; 325 | int walkZPos4; 326 | int walkZPos5; 327 | int walkZPos6; 328 | int walkZPos7; 329 | int walkZPos8; 330 | 331 | void setup() { 332 | 333 | // PID stuff 334 | 335 | PID1.SetMode(AUTOMATIC); 336 | PID1.SetOutputLimits(-50, 50); 337 | PID1.SetSampleTime(10); 338 | 339 | PID2.SetMode(AUTOMATIC); 340 | PID2.SetOutputLimits(-50, 50); 341 | PID2.SetSampleTime(10); 342 | 343 | PID3.SetMode(AUTOMATIC); 344 | PID3.SetOutputLimits(-50, 50); 345 | PID3.SetSampleTime(10); 346 | 347 | // radio stuff 348 | 349 | radio.begin(); 350 | radio.openWritingPipe(addresses[0]); // 00002 351 | radio.openReadingPipe(1, addresses[1]); // 00001 352 | radio.setPALevel(RF24_PA_MIN); 353 | 354 | radio.startListening(); 355 | 356 | // join I2C bus (I2Cdev library doesn't do this automatically) 357 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 358 | Wire.begin(); 359 | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties 360 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 361 | Fastwire::setup(400, true); 362 | #endif 363 | 364 | // initialize device 365 | mpu.initialize(); 366 | devStatus = mpu.dmpInitialize(); 367 | 368 | // supply your own gyro offsets here, scaled for min sensitivity 369 | mpu.setXGyroOffset(53); 370 | mpu.setYGyroOffset(-5); 371 | mpu.setZGyroOffset(51); 372 | mpu.setXAccelOffset(-2230); 373 | mpu.setYAccelOffset(-698); 374 | mpu.setZAccelOffset(2035); 375 | 376 | // make sure it worked (returns 0 if so) 377 | if (devStatus == 0) { 378 | // turn on the DMP, now that it's ready 379 | mpu.setDMPEnabled(true); 380 | 381 | // enable Arduino interrupt detection 382 | attachInterrupt(26, dmpDataReady, RISING); 383 | mpuIntStatus = mpu.getIntStatus(); 384 | 385 | // get expected DMP packet size for later comparison 386 | packetSize = mpu.dmpGetFIFOPacketSize(); 387 | } else { 388 | // ERROR! 389 | // 1 = initial memory load failed 390 | // 2 = DMP configuration updates failed 391 | // (if it's going to break, usually the code will be 1) 392 | Serial.print(F("DMP Initialization failed (code ")); 393 | Serial.print(devStatus); 394 | Serial.println(F(")")); 395 | 396 | 397 | } 398 | 399 | Serial.begin(115200); 400 | 401 | pinMode(A6, INPUT); 402 | pinMode(A7, INPUT); 403 | pinMode(A8, INPUT); 404 | 405 | pinMode(A9, INPUT); 406 | pinMode(A14, INPUT); 407 | pinMode(A15, INPUT); 408 | 409 | pinMode(A0, INPUT); 410 | pinMode(A1, INPUT); 411 | pinMode(A2, INPUT); 412 | 413 | pinMode(A3, INPUT); 414 | pinMode(A12, INPUT); 415 | pinMode(A13, INPUT); 416 | 417 | // write default positions with delays 418 | 419 | servo1.attach(24); // hips 420 | servo2.attach(25); 421 | servo3.attach(5); 422 | servo4.attach(2); 423 | 424 | servo1.writeMicroseconds(servo1Offset); // back left hip 425 | servo2.writeMicroseconds(servo2Offset); // back right hip 426 | servo3.writeMicroseconds(servo3Offset); // front left hip 427 | servo4.writeMicroseconds(servo4Offset); // front right hip 428 | 429 | delay(1000); 430 | 431 | servo5.attach(9); // shoulders 432 | servo6.attach(29); 433 | servo7.attach(6); 434 | servo8.attach(1); 435 | 436 | servo5.writeMicroseconds(servo5Offset); // back left shoulder 437 | servo6.writeMicroseconds(servo6Offset); // back right shoulder 438 | servo7.writeMicroseconds(servo7Offset); // front left shoulder 439 | servo8.writeMicroseconds(servo8Offset); // front right shoulder 440 | 441 | delay(1000); 442 | 443 | servo9.attach(8); // knees 444 | servo10.attach(30); 445 | servo11.attach(7); 446 | servo12.attach(0); 447 | 448 | servo9.writeMicroseconds(servo9Offset); // back left knee 449 | servo10.writeMicroseconds(servo10Offset); // back right knee 450 | servo11.writeMicroseconds(servo11Offset); // front left knee 451 | servo12.writeMicroseconds(servo12Offset); // front right knee 452 | 453 | // read hall effects on start up 454 | 455 | hall2Offset = analogRead(A7); // front left hip | lower number is more force 456 | hall3Offset = analogRead(A8); // front left shoulder | higher number is more force. 457 | hall4Offset = analogRead(A9); // front left knee | lower number is more force 458 | 459 | hall1Offset = analogRead(A6); // front right hip | higher number is more foce 460 | hall6Offset = analogRead(A15); // front right shoulder | lower nuber is more force 461 | hall5Offset = analogRead(A14); // front right knee | higher number is more force 462 | 463 | // back 464 | // forces are from the ground up 465 | 466 | hall7Offset = analogRead(A0); // back left hip | higher number is more force 467 | hall12Offset = analogRead(A13); // back left shoulder | lower number is more force 468 | hall11Offset = analogRead(A12); // back left knee | higher number is more force 469 | 470 | hall8Offset = analogRead(A1); // back right hip | lower number is more force 471 | hall9Offset = analogRead(A2); // back right shoulder | higher number is more force 472 | hall10Offset = analogRead(A3); // back right knee | lower number is more force 473 | 474 | } 475 | 476 | void loop() { 477 | 478 | currentMillis = millis(); 479 | if (currentMillis - previousMillis >= 10) { // start timed event 480 | 481 | previousMillis = currentMillis; 482 | 483 | // receive radio data 484 | 485 | if (radio.available()) { 486 | radio.read(&mydata_remote, sizeof(RECEIVE_DATA_STRUCTURE)); 487 | previousSafetyMillis = currentMillis; 488 | } 489 | 490 | // check if remote has become disconnected 491 | 492 | if (currentMillis - previousSafetyMillis > 500) { 493 | Serial.println("*no remote data* "); 494 | RLR = 512; 495 | RFB = 512; 496 | RT = 512; 497 | LLR = 512; 498 | LFB = 512; 499 | LT = 512; 500 | toggleTop = 0; 501 | toggleBottom = 0; 502 | toggle1 = 0; 503 | toggle2 = 0; 504 | Select = 0; 505 | } 506 | 507 | // use values if the remote is connected ok 508 | 509 | else { 510 | RLR = mydata_remote.RLR; 511 | RFB = mydata_remote.RFB; 512 | RT = mydata_remote.RT; 513 | LLR = mydata_remote.LLR; 514 | LFB = mydata_remote.LFB; 515 | LT = mydata_remote.LT; 516 | toggleTop = mydata_remote.toggleTop; 517 | toggleBottom = mydata_remote.toggleBottom; 518 | toggle1 = mydata_remote.toggle1; 519 | toggle2 = mydata_remote.toggle2; 520 | Select = mydata_remote.Select; 521 | } 522 | 523 | // Zero values to swing around +/- 0 524 | 525 | RLR = RLR - 512; // get to +/- zero value 526 | RFB = RFB - 512; 527 | RT = RT - 512; 528 | LLR = LLR - 512; 529 | LFB = LFB - 512; 530 | LT = LT - 512; 531 | 532 | // Threshold remote data for slop in sticks 533 | 534 | if (RLR > 50) { 535 | RLR = RLR -50; 536 | } 537 | else if (RLR < -50) { 538 | RLR = RLR +50; 539 | } 540 | else { 541 | RLR = 0; 542 | } 543 | //******* 544 | if (RFB > 50) { 545 | RFB = RFB -50; 546 | } 547 | else if (RFB < -50) { 548 | RFB = RFB +50; 549 | } 550 | else { 551 | RFB = 0; 552 | } 553 | //****** 554 | if (RT > 50) { 555 | RT = RT -50; 556 | } 557 | else if (RT < -50) { 558 | RT = RT +50; 559 | } 560 | else { 561 | RT = 0; 562 | } 563 | //****** 564 | if (LLR > 50) { 565 | LLR = LLR -50; 566 | } 567 | else if (LLR < -50) { 568 | LLR = LLR +50; 569 | } 570 | else { 571 | LLR = 0; 572 | } 573 | //******* 574 | if (LFB > 50) { 575 | LFB = LFB -50; 576 | } 577 | else if (LFB < -50) { 578 | LFB = LFB +50; 579 | } 580 | else { 581 | LFB = 0; 582 | } 583 | //****** 584 | if (LT > 50) { 585 | LT = LT -50; 586 | } 587 | else if (LT < -50) { 588 | LT = LT +50; 589 | } 590 | else { 591 | LT = 0; 592 | } 593 | 594 | // check for IMU inpterrupt 595 | 596 | if (IMUdataReady == 1) { 597 | readAngles(); 598 | } 599 | 600 | roll = (ypr[1] * 180/M_PI) - 1.5; 601 | pitch = (ypr[2] * 180/M_PI) + 0.7; 602 | 603 | // modes - use serial 604 | 605 | if (Serial.available()) { // check for serial data 606 | char c = Serial.read(); 607 | 608 | if (c == 'a') { // no compliance 609 | mode = 0; 610 | } 611 | else if (c == 'b') { // compliance on 612 | mode = 1; 613 | } 614 | else if (c == 'z') { // calibrate hall sensors 615 | mode = 99; 616 | } 617 | } 618 | 619 | // mode - use remote 620 | 621 | if (toggleTop == 0) { // no compliance 622 | mode = 0; 623 | } 624 | 625 | else if (toggleTop == 1) { // compliance on 626 | mode = 1; 627 | } 628 | 629 | if (Select == 1) { // calibrate hall sensors 630 | mode = 99; 631 | } 632 | 633 | 634 | // manual offset calibration of hall effect sensors (when dog is on the ground)- re-reads offsets 635 | 636 | if (mode == 99) { 637 | hall2Offset = analogRead(A7); // front left hip | lower number is more force 638 | hall3Offset = analogRead(A8); // front left shoulder | higher number is more force. 639 | hall4Offset = analogRead(A9); // front left knee | lower number is more force 640 | 641 | hall1Offset = analogRead(A6); // front right hip | higher number is more foce 642 | hall6Offset = analogRead(A15); // front right shoulder | lower nuber is more force 643 | hall5Offset = analogRead(A14); // front right knee | higher number is more force 644 | 645 | // back 646 | // forces are from the ground up 647 | 648 | hall7Offset = analogRead(A0); // back left hip | higher number is more force 649 | hall12Offset = analogRead(A13); // back left shoulder | lower number is more force 650 | hall11Offset = analogRead(A12); // back left knee | higher number is more force 651 | 652 | hall8Offset = analogRead(A1); // back right hip | lower number is more force 653 | hall9Offset = analogRead(A2); // back right shoulder | higher number is more force 654 | hall10Offset = analogRead(A3); // back right knee | lower number is more force 655 | } 656 | 657 | 658 | // *** read hall effect sensors on every loop *** 659 | 660 | // front 661 | // forces are from the ground up 662 | 663 | hall2 = analogRead(A7) - hall2Offset; // front left hip | lower number is more force 664 | hall3 = analogRead(A8) - hall3Offset; // front left shoulder | higher number is more force. 665 | hall4 = analogRead(A9) - hall4Offset; // front left knee | lower number is more force 666 | 667 | hall1 = analogRead(A6) - hall1Offset; // front right hip | higher number is more foce 668 | hall6 = analogRead(A15) - hall6Offset; // front right shoulder | lower nuber is more force 669 | hall5 = analogRead(A14) - hall5Offset; // front right knee | higher number is more force 670 | 671 | // back 672 | // forces are from the ground up 673 | 674 | hall7 = analogRead(A0) - hall7Offset; // back left hip | higher number is more force 675 | hall12 = analogRead(A13) - hall12Offset; // back left shoulder | lower number is more force 676 | hall11 = analogRead(A12) - hall11Offset; // back left knee | higher number is more force 677 | 678 | hall8 = analogRead(A1) - hall8Offset; // back right hip | lower number is more force 679 | hall9 = analogRead(A2) - hall9Offset; // back right shoulder | higher number is more force 680 | hall10 = analogRead(A3) - hall10Offset; // back right knee | lower number is more force 681 | 682 | 683 | // **************** Kinematic Model DEMO **************************** 684 | 685 | if (toggleBottom == 0) { 686 | 687 | // convert sticks to measurements in mm or degrees 688 | 689 | z = map(RT, -460,460,80,274); // overall height of the robot | Higher number makes the leg longer 690 | z = constrain(z,130,216); 691 | 692 | x = map(RFB, -460,460,-60,60); // front/back | Higher number moves the foot forward 693 | x = constrain(x,-60,60); 694 | 695 | y = map(RLR, -460,460,-60,60); // side/side | Higher number moves the foot left 696 | y = constrain(y,-60,60); 697 | 698 | r = map(LLR, -460, 460, -30, 30); // ROLL - covert to degrees of rotation 699 | r = constrain(r,-20,20); 700 | 701 | p = (map(LFB, -460, 460, 30, -30))-1; // PITCH - covert to degrees of rotation (there is a weird offset on my joystick hence -1) 702 | p = constrain(p,-20,20); 703 | 704 | yaw = map(LT, -460, 460, -30, 30); // YAW - covert to degrees of rotation (there is a weird offset on my joystick hence -1) 705 | yaw = constrain(yaw,-20,20); 706 | 707 | // print control data for debug 708 | 709 | Serial.print(" MODE: "); 710 | Serial.print(mode); 711 | Serial.print(" z: "); 712 | Serial.print(z); 713 | Serial.print(" x: "); 714 | Serial.print(x); 715 | Serial.print(" y: "); 716 | Serial.print(y); 717 | Serial.print(" r: "); 718 | Serial.print(r); 719 | Serial.print(" p: "); 720 | Serial.print(p); 721 | Serial.print(" yaw: "); 722 | Serial.print(yaw); 723 | Serial.println(); 724 | 725 | // send data to kinematic model function, compliance engine, and eventually write out to servos 726 | 727 | kinematics (1, mode, x,y-20,z,r,p,yaw); // front left leg 728 | kinematics (2, mode, x,y+20,z,r,p,yaw); // front right leg 729 | kinematics (3, mode, x,y-20,z,r,p,yaw); // back left leg 730 | kinematics (4, mode, x,y+20,z,r,p,yaw); // back right leg 731 | 732 | initStart = 0; // set the flag so we know what to do at the start of the walk gait below 733 | state = 20; // make sure the walk always starts at the beginning 734 | 735 | } 736 | 737 | // ****************** Start of test walking mode ***************************** 738 | // ******************** DYNAMICALLY STABLE ********************************* 739 | 740 | else if (toggleBottom == 1) { // position legs for walking and define the positions for this gait 741 | 742 | // define walking positions 743 | 744 | walkXPos1 = -60; 745 | walkXPos2 = 40; // was 0 in the 'old triangle' 746 | walkXPos3 = 60; 747 | walkXPos4 = 40; 748 | walkXPos5 = 20; 749 | walkXPos6 = 0; 750 | walkXPos7 = -20; 751 | walkXPos8 = -40; 752 | 753 | walkZPos1 = 215; // leg down 754 | walkZPos2 = 150; // leg up 755 | walkZPos3 = 215; // leg down 756 | walkZPos4 = 215; // leg down 757 | walkZPos5 = 215; // leg down 758 | walkZPos6 = 215; // leg down 759 | walkZPos7 = 215; // leg down 760 | walkZPos8 = 215; // leg down 761 | 762 | // the first time we go to this mode we position the feet to start walking 763 | 764 | if (initStart == 0) { // first state of prepairing to walk 765 | targetLeg1x = walkXPos6; 766 | targetLeg1z = walkZPos6; 767 | targetLeg2x = walkXPos6; 768 | targetLeg2z = walkZPos6; 769 | targetLeg3x = walkXPos6; 770 | targetLeg3z = walkZPos6; 771 | targetLeg4x = walkXPos6; 772 | targetLeg4z = walkZPos6; 773 | 774 | currentLeg1x = targetLeg1x; 775 | currentLeg1z = targetLeg1z; 776 | currentLeg2x = targetLeg2x; 777 | currentLeg2z = targetLeg2z; 778 | currentLeg3x = targetLeg3x; 779 | currentLeg3z = targetLeg3z; 780 | currentLeg4x = targetLeg4x; 781 | currentLeg4z = targetLeg4z; 782 | 783 | initStart = 1; 784 | state = 20; 785 | 786 | } 787 | 788 | if (toggle1 == 1) { // start state machine for walking 789 | 790 | /* 791 | 792 | //scale the rate so it ranges from 1 to 9 and it's a float - test manual rate control 793 | rate = (float) (RFB*-1)/1000; 794 | rate = 4-rate; 795 | 796 | 797 | // IMU rate control - not used in the end 798 | Setpoint1 = 0; 799 | Input1 = (pitch*-1)-1; 800 | PID1.Compute(); 801 | rate = constrain(Output1,3.5,4); 802 | */ 803 | 804 | rate = 2.9; // hardcode rate 805 | 806 | // state machine is below for the steps throughout the leg motions 807 | 808 | if (state == 20) { // *********** THE TWO FIRST STATES ARE ONLY USED ONCE TO GET INTO THE RIGHT POSITION 809 | targetLeg1x = walkXPos7; 810 | targetLeg1z = walkZPos1; // legs are down 811 | targetLeg2x = walkXPos7; // legs are back 812 | targetLeg2z = walkZPos1; 813 | targetLeg3x = walkXPos7; 814 | targetLeg3z = walkZPos1; 815 | targetLeg4x = walkXPos7; 816 | targetLeg4z = walkZPos1; 817 | if (currentLeg1x >= targetLeg1x) { 818 | //state = 21; 819 | prevLeg1x = targetLeg1x; 820 | prevLeg1z = targetLeg1z; 821 | prevLeg2x = targetLeg2x; 822 | prevLeg2z = targetLeg2z; 823 | prevLeg3x = targetLeg3x; 824 | prevLeg3z = targetLeg3z; 825 | prevLeg4x = targetLeg4x; 826 | prevLeg4z = targetLeg4z; 827 | // check we actually get there due to dividing errors 828 | currentLeg1x = targetLeg1x; 829 | currentLeg1z = targetLeg1z; 830 | currentLeg2x = targetLeg2x; 831 | currentLeg2z = targetLeg2z; 832 | currentLeg3x = targetLeg3x; 833 | currentLeg3z = targetLeg3z; 834 | currentLeg4x = targetLeg4x; 835 | currentLeg4z = targetLeg4z; 836 | if (RFB < -50) { // check we are pushing the hoystick forward before starting to walk 837 | state = 21; 838 | } 839 | } 840 | } 841 | 842 | else if (state == 21) { // *********** THE TWO FIRST STATES ARE ONLY USED ONCE TO GET INTO THE RIGHT POSITION *************** 843 | targetLeg1x = walkXPos4; 844 | targetLeg1z = walkZPos2; 845 | targetLeg2x = walkXPos1; 846 | targetLeg2z = walkZPos6; 847 | targetLeg3x = walkXPos1; 848 | targetLeg3z = walkZPos6; 849 | targetLeg4x = walkXPos4; 850 | targetLeg4z = walkZPos2; 851 | if (currentLeg1x >= targetLeg1x) { 852 | state = 0; 853 | prevLeg1x = targetLeg1x; 854 | prevLeg1z = targetLeg1z; 855 | prevLeg2x = targetLeg2x; 856 | prevLeg2z = targetLeg2z; 857 | prevLeg3x = targetLeg3x; 858 | prevLeg3z = targetLeg3z; 859 | prevLeg4x = targetLeg4x; 860 | prevLeg4z = targetLeg4z; 861 | // check we actually get there due to dividing errors 862 | currentLeg1x = targetLeg1x; 863 | currentLeg1z = targetLeg1z; 864 | currentLeg2x = targetLeg2x; 865 | currentLeg2z = targetLeg2z; 866 | currentLeg3x = targetLeg3x; 867 | currentLeg3z = targetLeg3z; 868 | currentLeg4x = targetLeg4x; 869 | currentLeg4z = targetLeg4z; 870 | } 871 | } 872 | 873 | else if (state == 0) { // ************ FIRST STATE THAT LOOPS FOR ACTUAL WALKING ************* 874 | targetLeg1x = walkXPos3; 875 | targetLeg1z = walkZPos3; 876 | targetLeg2x = walkXPos7; 877 | targetLeg2z = walkZPos7; 878 | targetLeg3x = walkXPos7; 879 | targetLeg3z = walkZPos7; 880 | targetLeg4x = walkXPos3; 881 | targetLeg4z = walkZPos3; 882 | if (currentLeg1x >= targetLeg1x) { 883 | state = 1; 884 | prevLeg1x = targetLeg1x; 885 | prevLeg1z = targetLeg1z; 886 | prevLeg2x = targetLeg2x; 887 | prevLeg2z = targetLeg2z; 888 | prevLeg3x = targetLeg3x; 889 | prevLeg3z = targetLeg3z; 890 | prevLeg4x = targetLeg4x; 891 | prevLeg4z = targetLeg4z; 892 | // check we actually get there due to dividing errors 893 | currentLeg1x = targetLeg1x; 894 | currentLeg1z = targetLeg1z; 895 | currentLeg2x = targetLeg2x; 896 | currentLeg2z = targetLeg2z; 897 | currentLeg3x = targetLeg3x; 898 | currentLeg3z = targetLeg3z; 899 | currentLeg4x = targetLeg4x; 900 | currentLeg4z = targetLeg4z; 901 | } 902 | } 903 | 904 | else if (state == 1) { 905 | targetLeg1x = walkXPos4; 906 | targetLeg1z = walkZPos4; 907 | targetLeg2x = walkXPos8; 908 | targetLeg2z = walkZPos8; 909 | targetLeg3x = walkXPos8; 910 | targetLeg3z = walkZPos8; 911 | targetLeg4x = walkXPos4; 912 | targetLeg4z = walkZPos4; 913 | if (currentLeg1x <= targetLeg1x) { 914 | state = 2; 915 | prevLeg1x = targetLeg1x; 916 | prevLeg1z = targetLeg1z; 917 | prevLeg2x = targetLeg2x; 918 | prevLeg2z = targetLeg2z; 919 | prevLeg3x = targetLeg3x; 920 | prevLeg3z = targetLeg3z; 921 | prevLeg4x = targetLeg4x; 922 | prevLeg4z = targetLeg4z; 923 | // check we actually get there due to dividing errors 924 | currentLeg1x = targetLeg1x; 925 | currentLeg1z = targetLeg1z; 926 | currentLeg2x = targetLeg2x; 927 | currentLeg2z = targetLeg2z; 928 | currentLeg3x = targetLeg3x; 929 | currentLeg3z = targetLeg3z; 930 | currentLeg4x = targetLeg4x; 931 | currentLeg4z = targetLeg4z; 932 | } 933 | } 934 | 935 | else if (state == 2) { 936 | targetLeg1x = walkXPos5; 937 | targetLeg1z = walkZPos5; 938 | targetLeg2x = walkXPos1; 939 | targetLeg2z = walkZPos1; 940 | targetLeg3x = walkXPos1; 941 | targetLeg3z = walkZPos1; 942 | targetLeg4x = walkXPos5; 943 | targetLeg4z = walkZPos5; 944 | if (currentLeg1x <= targetLeg1x) { 945 | state = 3; 946 | prevLeg1x = targetLeg1x; 947 | prevLeg1z = targetLeg1z; 948 | prevLeg2x = targetLeg2x; 949 | prevLeg2z = targetLeg2z; 950 | prevLeg3x = targetLeg3x; 951 | prevLeg3z = targetLeg3z; 952 | prevLeg4x = targetLeg4x; 953 | prevLeg4z = targetLeg4z; 954 | // check we actually get there due to dividing errors 955 | currentLeg1x = targetLeg1x; 956 | currentLeg1z = targetLeg1z; 957 | currentLeg2x = targetLeg2x; 958 | currentLeg2z = targetLeg2z; 959 | currentLeg3x = targetLeg3x; 960 | currentLeg3z = targetLeg3z; 961 | currentLeg4x = targetLeg4x; 962 | currentLeg4z = targetLeg4z; 963 | } 964 | } 965 | 966 | else if (state == 3) { 967 | targetLeg1x = walkXPos6; 968 | targetLeg1z = walkZPos6; 969 | targetLeg2x = walkXPos2; 970 | targetLeg2z = walkZPos2; 971 | targetLeg3x = walkXPos2; 972 | targetLeg3z = walkZPos2; 973 | targetLeg4x = walkXPos6; 974 | targetLeg4z = walkZPos6; 975 | if (currentLeg1x <= targetLeg1x) { 976 | state = 4; 977 | prevLeg1x = targetLeg1x; 978 | prevLeg1z = targetLeg1z; 979 | prevLeg2x = targetLeg2x; 980 | prevLeg2z = targetLeg2z; 981 | prevLeg3x = targetLeg3x; 982 | prevLeg3z = targetLeg3z; 983 | prevLeg4x = targetLeg4x; 984 | prevLeg4z = targetLeg4z; 985 | // check we actually get there due to dividing errors 986 | currentLeg1x = targetLeg1x; 987 | currentLeg1z = targetLeg1z; 988 | currentLeg2x = targetLeg2x; 989 | currentLeg2z = targetLeg2z; 990 | currentLeg3x = targetLeg3x; 991 | currentLeg3z = targetLeg3z; 992 | currentLeg4x = targetLeg4x; 993 | currentLeg4z = targetLeg4z; 994 | } 995 | } 996 | 997 | else if (state == 4) { 998 | targetLeg1x = walkXPos7; 999 | targetLeg1z = walkZPos7; 1000 | targetLeg2x = walkXPos3; 1001 | targetLeg2z = walkZPos3; 1002 | targetLeg3x = walkXPos3; 1003 | targetLeg3z = walkZPos3; 1004 | targetLeg4x = walkXPos7; 1005 | targetLeg4z = walkZPos7; 1006 | if (currentLeg1x <= targetLeg1x) { 1007 | state = 5; 1008 | prevLeg1x = targetLeg1x; 1009 | prevLeg1z = targetLeg1z; 1010 | prevLeg2x = targetLeg2x; 1011 | prevLeg2z = targetLeg2z; 1012 | prevLeg3x = targetLeg3x; 1013 | prevLeg3z = targetLeg3z; 1014 | prevLeg4x = targetLeg4x; 1015 | prevLeg4z = targetLeg4z; 1016 | // check we actually get there due to dividing errors 1017 | currentLeg1x = targetLeg1x; 1018 | currentLeg1z = targetLeg1z; 1019 | currentLeg2x = targetLeg2x; 1020 | currentLeg2z = targetLeg2z; 1021 | currentLeg3x = targetLeg3x; 1022 | currentLeg3z = targetLeg3z; 1023 | currentLeg4x = targetLeg4x; 1024 | currentLeg4z = targetLeg4z; 1025 | } 1026 | } 1027 | 1028 | else if (state == 5) { 1029 | targetLeg1x = walkXPos8; 1030 | targetLeg1z = walkZPos8; 1031 | targetLeg2x = walkXPos4; 1032 | targetLeg2z = walkZPos4; 1033 | targetLeg3x = walkXPos4; 1034 | targetLeg3z = walkZPos4; 1035 | targetLeg4x = walkXPos8; 1036 | targetLeg4z = walkZPos8; 1037 | if (currentLeg1x <= targetLeg1x) { 1038 | state = 6; 1039 | prevLeg1x = targetLeg1x; 1040 | prevLeg1z = targetLeg1z; 1041 | prevLeg2x = targetLeg2x; 1042 | prevLeg2z = targetLeg2z; 1043 | prevLeg3x = targetLeg3x; 1044 | prevLeg3z = targetLeg3z; 1045 | prevLeg4x = targetLeg4x; 1046 | prevLeg4z = targetLeg4z; 1047 | // check we actually get there due to dividing errors 1048 | currentLeg1x = targetLeg1x; 1049 | currentLeg1z = targetLeg1z; 1050 | currentLeg2x = targetLeg2x; 1051 | currentLeg2z = targetLeg2z; 1052 | currentLeg3x = targetLeg3x; 1053 | currentLeg3z = targetLeg3z; 1054 | currentLeg4x = targetLeg4x; 1055 | currentLeg4z = targetLeg4z; 1056 | } 1057 | } 1058 | 1059 | else if (state == 6) { 1060 | targetLeg1x = walkXPos1; 1061 | targetLeg1z = walkZPos1; 1062 | targetLeg2x = walkXPos5; 1063 | targetLeg2z = walkZPos5; 1064 | targetLeg3x = walkXPos5; 1065 | targetLeg3z = walkZPos5; 1066 | targetLeg4x = walkXPos1; 1067 | targetLeg4z = walkZPos1; 1068 | if (currentLeg1x <= targetLeg1x) { 1069 | state = 7; 1070 | prevLeg1x = targetLeg1x; 1071 | prevLeg1z = targetLeg1z; 1072 | prevLeg2x = targetLeg2x; 1073 | prevLeg2z = targetLeg2z; 1074 | prevLeg3x = targetLeg3x; 1075 | prevLeg3z = targetLeg3z; 1076 | prevLeg4x = targetLeg4x; 1077 | prevLeg4z = targetLeg4z; 1078 | // check we actually get there due to dividing errors 1079 | currentLeg1x = targetLeg1x; 1080 | currentLeg1z = targetLeg1z; 1081 | currentLeg2x = targetLeg2x; 1082 | currentLeg2z = targetLeg2z; 1083 | currentLeg3x = targetLeg3x; 1084 | currentLeg3z = targetLeg3z; 1085 | currentLeg4x = targetLeg4x; 1086 | currentLeg4z = targetLeg4z; 1087 | } 1088 | } 1089 | 1090 | else if (state == 7) { 1091 | targetLeg1x = walkXPos2; 1092 | targetLeg1z = walkZPos2; 1093 | targetLeg2x = walkXPos6; 1094 | targetLeg2z = walkZPos6; 1095 | targetLeg3x = walkXPos6; 1096 | targetLeg3z = walkZPos6; 1097 | targetLeg4x = walkXPos2; 1098 | targetLeg4z = walkZPos2; 1099 | if (currentLeg1x >= targetLeg1x) { 1100 | //state = 0; 1101 | prevLeg1x = targetLeg1x; 1102 | prevLeg1z = targetLeg1z; 1103 | prevLeg2x = targetLeg2x; 1104 | prevLeg2z = targetLeg2z; 1105 | prevLeg3x = targetLeg3x; 1106 | prevLeg3z = targetLeg3z; 1107 | prevLeg4x = targetLeg4x; 1108 | prevLeg4z = targetLeg4z; 1109 | // check we actually get there due to dividing errors 1110 | currentLeg1x = targetLeg1x; 1111 | currentLeg1z = targetLeg1z; 1112 | currentLeg2x = targetLeg2x; 1113 | currentLeg2z = targetLeg2z; 1114 | currentLeg3x = targetLeg3x; 1115 | currentLeg3z = targetLeg3z; 1116 | currentLeg4x = targetLeg4x; 1117 | currentLeg4z = targetLeg4z; 1118 | 1119 | if (RFB < -50) { // carry on with the loop 1120 | state = 0; 1121 | } 1122 | else { 1123 | state = 30; 1124 | } 1125 | 1126 | if (state == 30) { // ********* FINAL STATE(S) IN WALKING SO THE LEGS COME BACK TOGETHER AGAIN 1127 | targetLeg1x = walkXPos6; 1128 | targetLeg1z = walkZPos6; 1129 | targetLeg2x = walkXPos6; 1130 | targetLeg2z = walkZPos6; 1131 | targetLeg3x = walkXPos6; 1132 | targetLeg3z = walkZPos6; 1133 | targetLeg4x = walkXPos6; 1134 | targetLeg4z = walkZPos6; 1135 | if (currentLeg1x >= targetLeg1x) { 1136 | state = 20; // go back to the start until we push the joystick again 1137 | prevLeg1x = targetLeg1x; 1138 | prevLeg1z = targetLeg1z; 1139 | prevLeg2x = targetLeg2x; 1140 | prevLeg2z = targetLeg2z; 1141 | prevLeg3x = targetLeg3x; 1142 | prevLeg3z = targetLeg3z; 1143 | prevLeg4x = targetLeg4x; 1144 | prevLeg4z = targetLeg4z; 1145 | // check we actually get there due to dividing errors 1146 | currentLeg1x = targetLeg1x; 1147 | currentLeg1z = targetLeg1z; 1148 | currentLeg2x = targetLeg2x; 1149 | currentLeg2z = targetLeg2z; 1150 | currentLeg3x = targetLeg3x; 1151 | currentLeg3z = targetLeg3z; 1152 | currentLeg4x = targetLeg4x; 1153 | currentLeg4z = targetLeg4z; 1154 | } 1155 | } 1156 | 1157 | } 1158 | } // end of current state 1159 | 1160 | // *** interpolation divison based on rate *** 1161 | 1162 | stepDiffLeg1x = (targetLeg1x - prevLeg1x)/(5*rate); 1163 | stepDiffLeg1z = (targetLeg1z - prevLeg1z)/(5*rate); 1164 | currentLeg1x = currentLeg1x + stepDiffLeg1x; 1165 | currentLeg1z = currentLeg1z + stepDiffLeg1z; 1166 | 1167 | stepDiffLeg2x = (targetLeg2x - prevLeg2x)/(5*rate); 1168 | stepDiffLeg2z = (targetLeg2z - prevLeg2z)/(5*rate); 1169 | currentLeg2x = currentLeg2x + stepDiffLeg2x; 1170 | currentLeg2z = currentLeg2z + stepDiffLeg2z; 1171 | 1172 | stepDiffLeg3x = (targetLeg3x - prevLeg3x)/(5*rate); 1173 | stepDiffLeg3z = (targetLeg3z - prevLeg3z)/(5*rate); 1174 | currentLeg3x = currentLeg3x + stepDiffLeg3x; 1175 | currentLeg3z = currentLeg3z + stepDiffLeg3z; 1176 | 1177 | stepDiffLeg4x = (targetLeg4x - prevLeg4x)/(5*rate); 1178 | stepDiffLeg4z = (targetLeg4z - prevLeg4z)/(5*rate); 1179 | currentLeg4x = currentLeg4x + stepDiffLeg4x; 1180 | currentLeg4z = currentLeg4z + stepDiffLeg4z; 1181 | 1182 | 1183 | } // end of state machine for walk test 1184 | 1185 | // offsets to balance centre of gravity statically 1186 | int offsetX = (offsetX - 5); // offset legs back/forward to hard coded balance 1187 | int offsetY = offsetY + 20; // move the legs closer together for better stability 1188 | 1189 | // offsets based on IMU below 1190 | 1191 | // vary X translation based on pitch 1192 | Setpoint2 = 0; 1193 | Input2 = pitch; 1194 | PID2.Compute(); 1195 | Output2 = constrain(Output2,-20,20); 1196 | offsetX = offsetX + Output2; 1197 | 1198 | /* 1199 | // vary X translation based on roll - not initially used. 1200 | Setpoint3 = 0; 1201 | Input3 = (roll*-1); 1202 | PID3.Compute(); 1203 | y = Output3; 1204 | y = constrain(y, -10, 10); 1205 | */ 1206 | 1207 | y = -3; // manual bodge to keep it on centre - there's quite a lot of issues getting the home positions of the servos aligned, so use this to bias it 1208 | 1209 | Serial.print(state); 1210 | Serial.print(" , "); 1211 | Serial.print(initStart); 1212 | Serial.print(" , "); 1213 | Serial.print(RFB); 1214 | 1215 | 1216 | Serial.println(); 1217 | 1218 | kinematics (1, mode, currentLeg1x+offsetX, 0-offsetY+y, currentLeg1z, r, p, yaw); // front left leg 1219 | kinematics (2, mode, currentLeg2x+offsetX, 0+offsetY+y, currentLeg2z, r, p, yaw); // front right leg 1220 | kinematics (3, mode, currentLeg3x+offsetX, 0-offsetY+y, currentLeg3z, r, p, yaw); // back left leg 1221 | kinematics (4, mode, currentLeg4x+offsetX, 0+offsetY+y, currentLeg4z, r, p, yaw); // back right leg 1222 | } 1223 | 1224 | } // end of timed loop 1225 | 1226 | } // end of main loop 1227 | 1228 | 1229 | 1230 | 1231 | -------------------------------------------------------------------------------- /miniDogV2_011_Dynamic/readangle.ino: -------------------------------------------------------------------------------- 1 | // IMU interrupt service routine 2 | 3 | void dmpDataReady() { 4 | IMUdataReady = 1; 5 | } 6 | 7 | 8 | // function that actually read the angle when the flag is set by the ISR 9 | 10 | void readAngles() { 11 | 12 | mpuIntStatus = mpu.getIntStatus(); 13 | fifoCount = mpu.getFIFOCount(); 14 | 15 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 16 | // reset so we can continue cleanly 17 | mpu.resetFIFO(); 18 | Serial.println(F("FIFO overflow!")); 19 | } 20 | 21 | else if (mpuIntStatus & 0x02) { 22 | // wait for correct available data length, should be a VERY short wait 23 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 24 | 25 | // read a packet from FIFO 26 | mpu.getFIFOBytes(fifoBuffer, packetSize); 27 | 28 | // track FIFO count here in case there is > 1 packet available 29 | // (this lets us immediately read more without waiting for an interrupt) 30 | fifoCount -= packetSize; 31 | 32 | mpu.dmpGetQuaternion(&q, fifoBuffer); 33 | mpu.dmpGetGravity(&gravity, &q); 34 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 35 | 36 | IMUdataReady = 0; 37 | } 38 | } 39 | 40 | 41 | 42 | --------------------------------------------------------------------------------