├── 1Wire_Addr_finder_ds18b20.ino ├── 4-20ma └── README.md ├── LICENSE ├── README.md ├── i2c ├── master_3bytes.ino └── slave_3bytes.ino ├── imu ├── I2C.ino ├── ITGMPU │ └── GY521 │ │ ├── 2steppermotor │ │ ├── I2C.ino │ │ ├── Kalman.h │ │ ├── L298N_R3_2xstepper_v2.ino │ │ └── README.md │ │ ├── I2C.ino │ │ ├── Kalman.h │ │ ├── R3_L298N_stepper_v3.png │ │ ├── README.md │ │ └── stepper_L298N_gyro_control_knob.ino ├── Kalman.h ├── Kalman_Arduino_R3_MPU6050_2xmotor_stepper.ino ├── R3_MPU6050_2xstepper.ino ├── R3_MPU6050_2xstepper_v2.ino ├── linkit_web_imu_stepper.ino ├── mpu6050_stepper_v5.ino └── yun_www_imu_2xstepper.ino ├── read_own_vcc └── slave_clock └── test1.ino /1Wire_Addr_finder_ds18b20.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | OneWire ds(3); // Connect your 1-wire device to pin 3 4 | 5 | void setup(void) { 6 | Serial.begin(9600); 7 | 8 | discoverOneWireDevices(); 9 | } 10 | 11 | void discoverOneWireDevices(void) { 12 | byte i; 13 | byte present = 0; 14 | byte data[12]; 15 | byte addr[8]; 16 | 17 | Serial.print("Looking for 1-Wire devices...\n\r"); 18 | while(ds.search(addr)) { 19 | Serial.print("\n\rFound \'1-Wire\' device with address:\n\r"); 20 | for( i = 0; i < 8; i++) { 21 | Serial.print("0x"); 22 | if (addr[i] < 16) { 23 | Serial.print('0'); 24 | } 25 | Serial.print(addr[i], HEX); 26 | if (i < 7) { 27 | Serial.print(", "); 28 | } 29 | } 30 | if ( OneWire::crc8( addr, 7) != addr[7]) { 31 | Serial.print("CRC is not valid!\n"); 32 | return; 33 | } 34 | } 35 | Serial.print("\n\r\n\rThat's it.\r\n"); 36 | ds.reset_search(); 37 | return; 38 | } 39 | 40 | void loop(void) { 41 | 42 | } 43 | -------------------------------------------------------------------------------- /4-20ma/README.md: -------------------------------------------------------------------------------- 1 | ##4-20mA Sketch 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # arduino-stuff 2 | Misc sketch for Arduino boards 3 | -------------------------------------------------------------------------------- /i2c/master_3bytes.ino: -------------------------------------------------------------------------------- 1 | // Wire Master Reader 2 | #include 3 | int table[]={0,0,0}; 4 | int led1=51; 5 | int led2=52; 6 | int led3=53; 7 | 8 | void setup() 9 | { 10 | Wire.begin(); // join i2c bus (address optional for master) 11 | Serial.begin(9600); // start serial for output 12 | pinMode(led1,OUTPUT); 13 | pinMode(led2,OUTPUT); 14 | pinMode(led3,OUTPUT); 15 | } 16 | 17 | void loop() 18 | { 19 | Wire.requestFrom(2, 3); // request 3 bytes from slave device #2 20 | 21 | for(int i=0;i<3;i++) 22 | { 23 | int c = Wire.read(); // receive a byte as character 24 | 25 | Serial.print(c); 26 | table[i]=c; 27 | Serial.print('\t'); 28 | 29 | } 30 | Serial.print('\n'); 31 | Serial.print(table[0]); 32 | Serial.print('\t'); 33 | Serial.print(table[1]); 34 | Serial.print('\t'); 35 | Serial.print(table[2]); 36 | Serial.print('\n'); 37 | 38 | if(table[0]>150) 39 | { 40 | digitalWrite(led1,HIGH); 41 | }else{ 42 | digitalWrite(led1,LOW); 43 | } 44 | 45 | if(table[1]>150) 46 | { 47 | digitalWrite(led2,HIGH); 48 | }else{ 49 | digitalWrite(led2,LOW); 50 | } 51 | if(table[2]==1) 52 | { 53 | digitalWrite(led3,HIGH); 54 | }else{ 55 | digitalWrite(led3,LOW); 56 | } 57 | delay(500); 58 | } 59 | -------------------------------------------------------------------------------- /i2c/slave_3bytes.ino: -------------------------------------------------------------------------------- 1 | // Wire Slave Sender 2 | 3 | #include 4 | int table[]={0,0,0}; 5 | int lightSensor1=A0; 6 | int lightSensor2=A1; 7 | int button=8; 8 | 9 | void setup() 10 | { 11 | Serial.begin(9600); 12 | pinMode(lightSensor1,INPUT); 13 | pinMode(lightSensor2,INPUT); 14 | pinMode(button,INPUT); 15 | Wire.begin(2); // join i2c bus with address #2 16 | Wire.onRequest(requestEvent); // register event 17 | } 18 | 19 | void loop() 20 | { 21 | table[0]=map(analogRead(lightSensor1),0,1023,0,255); 22 | table[1]=map(analogRead(lightSensor2),0,1023,0,255); 23 | if(digitalRead(button)==HIGH) 24 | { 25 | table[2]=1; 26 | }else{ 27 | table[2]=0; 28 | } 29 | Serial.print(table[0]); 30 | Serial.print('\t'); 31 | Serial.print(table[1]); 32 | Serial.print('\t'); 33 | Serial.print(table[2]); 34 | Serial.print('\n'); 35 | delay(100); 36 | } 37 | 38 | // function that executes whenever data is requested by master 39 | // this function is registered as an event, see setup() 40 | void requestEvent() 41 | { 42 | // for(int i=0;i<3;i++) 43 | // { 44 | uint8_t Buffer[3]; 45 | Buffer[0]=table[0]; 46 | Buffer[1]=table[1]; 47 | Buffer[2]=table[2]; 48 | 49 | Wire.write(Buffer,3); 50 | 51 | } 52 | -------------------------------------------------------------------------------- /imu/I2C.ino: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB 19 | const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication 20 | 21 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { 22 | return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success 23 | } 24 | 25 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { 26 | Wire.beginTransmission(IMUAddress); 27 | Wire.write(registerAddress); 28 | Wire.write(data, length); 29 | uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success 30 | if (rcode) { 31 | Serial.print(F("i2cWrite failed: ")); 32 | Serial.println(rcode); 33 | } 34 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 35 | } 36 | 37 | uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { 38 | uint32_t timeOutTimer; 39 | Wire.beginTransmission(IMUAddress); 40 | Wire.write(registerAddress); 41 | uint8_t rcode = Wire.endTransmission(false); // Don't release the bus 42 | if (rcode) { 43 | Serial.print(F("i2cRead failed: ")); 44 | Serial.println(rcode); 45 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 46 | } 47 | Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading 48 | for (uint8_t i = 0; i < nbytes; i++) { 49 | if (Wire.available()) 50 | data[i] = Wire.read(); 51 | else { 52 | timeOutTimer = micros(); 53 | while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); 54 | if (Wire.available()) 55 | data[i] = Wire.read(); 56 | else { 57 | Serial.println(F("i2cRead timeout")); 58 | return 5; // This error value is not already taken by endTransmission 59 | } 60 | } 61 | } 62 | return 0; // Success 63 | } 64 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/2steppermotor/I2C.ino: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB 19 | const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication 20 | 21 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { 22 | return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success 23 | } 24 | 25 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { 26 | Wire.beginTransmission(IMUAddress); 27 | Wire.write(registerAddress); 28 | Wire.write(data, length); 29 | uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success 30 | if (rcode) { 31 | Serial.print(F("i2cWrite failed: ")); 32 | Serial.println(rcode); 33 | } 34 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 35 | } 36 | 37 | uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { 38 | uint32_t timeOutTimer; 39 | Wire.beginTransmission(IMUAddress); 40 | Wire.write(registerAddress); 41 | uint8_t rcode = Wire.endTransmission(false); // Don't release the bus 42 | if (rcode) { 43 | Serial.print(F("i2cRead failed: ")); 44 | Serial.println(rcode); 45 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 46 | } 47 | Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading 48 | for (uint8_t i = 0; i < nbytes; i++) { 49 | if (Wire.available()) 50 | data[i] = Wire.read(); 51 | else { 52 | timeOutTimer = micros(); 53 | while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); 54 | if (Wire.available()) 55 | data[i] = Wire.read(); 56 | else { 57 | Serial.println(F("i2cRead timeout")); 58 | return 5; // This error value is not already taken by endTransmission 59 | } 60 | } 61 | } 62 | return 0; // Success 63 | } 64 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/2steppermotor/Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h 19 | #define _Kalman_h 20 | 21 | class Kalman { 22 | public: 23 | Kalman() { 24 | /* We will set the variables like so, these can also be tuned by the user */ 25 | Q_angle = 0.001; 26 | Q_bias = 0.003; 27 | R_measure = 0.03; 28 | 29 | angle = 0; // Reset the angle 30 | bias = 0; // Reset bias 31 | 32 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 33 | P[0][1] = 0; 34 | P[1][0] = 0; 35 | P[1][1] = 0; 36 | }; 37 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 38 | double getAngle(double newAngle, double newRate, double dt) { 39 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 40 | // Modified by Kristian Lauszus 41 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 42 | 43 | // Discrete Kalman filter time update equations - Time Update ("Predict") 44 | // Update xhat - Project the state ahead 45 | /* Step 1 */ 46 | rate = newRate - bias; 47 | angle += dt * rate; 48 | 49 | // Update estimation error covariance - Project the error covariance ahead 50 | /* Step 2 */ 51 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 52 | P[0][1] -= dt * P[1][1]; 53 | P[1][0] -= dt * P[1][1]; 54 | P[1][1] += Q_bias * dt; 55 | 56 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 57 | // Calculate Kalman gain - Compute the Kalman gain 58 | /* Step 4 */ 59 | S = P[0][0] + R_measure; 60 | /* Step 5 */ 61 | K[0] = P[0][0] / S; 62 | K[1] = P[1][0] / S; 63 | 64 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 65 | /* Step 3 */ 66 | y = newAngle - angle; 67 | /* Step 6 */ 68 | angle += K[0] * y; 69 | bias += K[1] * y; 70 | 71 | // Calculate estimation error covariance - Update the error covariance 72 | /* Step 7 */ 73 | P[0][0] -= K[0] * P[0][0]; 74 | P[0][1] -= K[0] * P[0][1]; 75 | P[1][0] -= K[1] * P[0][0]; 76 | P[1][1] -= K[1] * P[0][1]; 77 | 78 | return angle; 79 | }; 80 | void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 81 | double getRate() { return rate; }; // Return the unbiased rate 82 | 83 | /* These are used to tune the Kalman filter */ 84 | void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 85 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 86 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 87 | 88 | double getQangle() { return Q_angle; }; 89 | double getQbias() { return Q_bias; }; 90 | double getRmeasure() { return R_measure; }; 91 | 92 | private: 93 | /* Kalman filter variables */ 94 | double Q_angle; // Process noise variance for the accelerometer 95 | double Q_bias; // Process noise variance for the gyro bias 96 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 97 | 98 | double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 99 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 100 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 101 | 102 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 103 | double K[2]; // Kalman gain - This is a 2x1 vector 104 | double y; // Angle difference 105 | double S; // Estimate error 106 | }; 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/2steppermotor/L298N_R3_2xstepper_v2.ino: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | This software may be distributed and modified under the terms of the GNU 3 | General Public License version 2 (GPL2) as published by the Free Software 4 | Foundation and appearing in the file GPL2.TXT included in the packaging of 5 | this file. Please note that GPL2 Section 2[b] requires that all works based 6 | on this software must also be made publicly available under the terms of 7 | the GPL2 ("Copyleft"). 8 | Contact information 9 | ------------------- 10 | Kristian Lauszus, TKJ Electronics 11 | Web : http://www.tkjelectronics.com 12 | e-mail : kristianl@tkjelectronics.com 13 | */ 14 | 15 | #include 16 | #include 17 | #include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter 18 | 19 | #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf 20 | 21 | Kalman kalmanX; // Create the Kalman instances 22 | Kalman kalmanY; 23 | 24 | /* IMU Data */ 25 | double accX, accY, accZ; 26 | double gyroX, gyroY, gyroZ; 27 | int16_t tempRaw; 28 | 29 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 30 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 31 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 32 | 33 | uint32_t timer; 34 | uint8_t i2cData[14]; // Buffer for I2C data 35 | 36 | Stepper stepper2(100, 8,9,10,11); 37 | Stepper stepper1(100, 7,6,5,4); 38 | 39 | int cantidad = 1; 40 | 41 | int previousx = 0; 42 | int previousy = 0; 43 | 44 | void setup() { 45 | 46 | stepper2.setSpeed(200); 47 | stepper1.setSpeed(200); 48 | 49 | Serial.begin(115200); 50 | Wire.begin(); 51 | TWBR = ((F_CPU /400000L) - 16) / 2; // Set I2C frequency to 400kHz 52 | 53 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 54 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 55 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 56 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 57 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 58 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 59 | 60 | while (i2cRead(0x75, i2cData, 1)); 61 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 62 | Serial.print(F("Error reading sensor")); 63 | while (1); 64 | } 65 | 66 | delay(100); // Wait for sensor to stabilize 67 | 68 | /* Set kalman and gyro starting angle */ 69 | while (i2cRead(0x3B, i2cData, 6)); 70 | accX = (i2cData[0] << 8) | i2cData[1]; 71 | accY = (i2cData[2] << 8) | i2cData[3]; 72 | accZ = (i2cData[4] << 8) | i2cData[5]; 73 | 74 | // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 75 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 76 | // It is then converted from radians to degrees 77 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 78 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 79 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 80 | #else // Eq. 28 and 29 81 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 82 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 83 | #endif 84 | 85 | kalmanX.setAngle(roll); // Set starting angle 86 | kalmanY.setAngle(pitch); 87 | gyroXangle = roll; 88 | gyroYangle = pitch; 89 | compAngleX = roll; 90 | compAngleY = pitch; 91 | 92 | timer = micros(); 93 | } 94 | 95 | void loop() { 96 | /* Update all the values */ 97 | while (i2cRead(0x3B, i2cData, 14)); 98 | accX = ((i2cData[0] << 8) | i2cData[1]); 99 | accY = ((i2cData[2] << 8) | i2cData[3]); 100 | accZ = ((i2cData[4] << 8) | i2cData[5]); 101 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 102 | gyroX = (i2cData[8] << 8) | i2cData[9]; 103 | gyroY = (i2cData[10] << 8) | i2cData[11]; 104 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 105 | 106 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 107 | timer = micros(); 108 | 109 | // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 110 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 111 | // It is then converted from radians to degrees 112 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 113 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 114 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 115 | #else // Eq. 28 and 29 116 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 117 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 118 | #endif 119 | 120 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 121 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 122 | 123 | #ifdef RESTRICT_PITCH 124 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 125 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 126 | kalmanX.setAngle(roll); 127 | compAngleX = roll; 128 | kalAngleX = roll; 129 | gyroXangle = roll; 130 | } else 131 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 132 | 133 | if (abs(kalAngleX) > 90) 134 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 135 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 136 | #else 137 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 138 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 139 | kalmanY.setAngle(pitch); 140 | compAngleY = pitch; 141 | kalAngleY = pitch; 142 | gyroYangle = pitch; 143 | } else 144 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 145 | 146 | if (abs(kalAngleY) > 90) 147 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 148 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 149 | #endif 150 | 151 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 152 | gyroYangle += gyroYrate * dt; 153 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 154 | //gyroYangle += kalmanY.getRate() * dt; 155 | 156 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 157 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 158 | 159 | // Reset the gyro angle when it has drifted too much 160 | if (gyroXangle < -180 || gyroXangle > 180) 161 | gyroXangle = kalAngleX; 162 | if (gyroYangle < -180 || gyroYangle > 180) 163 | gyroYangle = kalAngleY; 164 | 165 | /* Print Data */ 166 | #if 0//et to 1 to activate 167 | Serial.print(accX); Serial.print("\t"); 168 | Serial.print(accY); Serial.print("\t"); 169 | //Serial.print(accZ); Serial.print("\t"); 170 | //rial.print(gyroX); Serial.print("\t"); 171 | //erial.print(gyroY); Serial.print("\t"); 172 | //Serial.print(gyroZ); Serial.print("\t"); 173 | Serial.print("\t"); 174 | #endif 175 | 176 | //Serial.print(roll); Serial.print("\t"); 177 | //Serial.print(gyroXangle); Serial.print("\t"); 178 | //Serial.print(compAngleX); Serial.print("\t"); 179 | Serial.print(kalAngleX); Serial.print("\t"); 180 | 181 | Serial.print("\t"); 182 | 183 | //Serial.print(pitch); Serial.print("\t"); 184 | //Serial.print(gyroYangle); Serial.print("\t"); 185 | //Serial.print(compAngleY); Serial.print("\t"); 186 | Serial.print(kalAngleY); Serial.print("\t"); 187 | 188 | #if 0 // Set to 1 to print the temperature 189 | Serial.print("\t"); 190 | double temperature = (double)tempRaw / 340.0 + 36.53; 191 | Serial.print(temperature); Serial.print("\t"); 192 | #endif 193 | 194 | Serial.print("\r\n"); 195 | //delay(2); 196 | 197 | int valuex = kalAngleX; 198 | stepper2.step(valuex - previousx); 199 | previousx = valuex; 200 | 201 | int valuey = kalAngleY; 202 | stepper1.step(valuey - previousy); 203 | previousy = valuey; 204 | 205 | 206 | 207 | } 208 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/2steppermotor/README.md: -------------------------------------------------------------------------------- 1 | Materials 2 | 3 | 2 x L298N Driver 4 | 2 x Stepper motor 4 wire 5 | 1 X Arduino R3 6 | 1 x Gyroscope/Accelerometer ITG GY-521 MPU6050 7 | 8 | ------------------------- 9 | Software 10 | 11 | I2C.ino and Kalman.h MUST be in the same directory that sketch 12 | 13 | ------------------------- 14 | 15 | Pinout 16 | 17 | ITG Gy-521 to Arduino 18 | 19 | ITG pin SCL ---> Pin5 Arduino 20 | ITG pin SDA ---> Pin4 Arduino 21 | ITG VCC ----> 5v Arduino 22 | ITG GND ---> GND Arduino 23 | 24 | 25 | 1st L298N to Arduino 26 | 27 | L298N pin IN1 ---> Pin 11 Arduino 28 | L298N pin IN2 ---> Pin 10 Arduino 29 | L298N pin IN3 ---> Pin 9 Arduino 30 | L298N pin IN4 ---> Pin 8 Arduino 31 | 32 | L298N Pin GND ---> Pin GND Arduino (please don't forget this connection, otherwise the motor it doesn't work) 33 | 34 | 2nd L298N to Arduino 35 | 36 | L298N pin IN1 ---> Pin 4 Arduino 37 | L298N pin IN2 ---> Pin 5 Arduino 38 | L298N pin IN3 ---> Pin 6 Arduino 39 | L298N pin IN4 ---> Pin 7 Arduino 40 | 41 | L298N Pin GND ---> Pin GND Arduino (please don't forget this connection, otherwise the motor it doesn't work) 42 | 43 | 44 | L298N to Motor 45 | 46 | First you will need figure out what are the coils, take a multimeter and change to ohmmeter, 47 | then determine which pairs are group A and group B 48 | 49 | then 50 | 51 | L298N Out1/out2 ---> to Group A motor 52 | L298N Out3/Out4 ---> to Group B motor 53 | 54 | ------------------------------------------- 55 | 56 | LN982N and Power supply 57 | 58 | You will need to connect L298N to external power source 5vdc or 12Vdc , it depends of motor voltage. 59 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/I2C.ino: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB 19 | const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication 20 | 21 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) { 22 | return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success 23 | } 24 | 25 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) { 26 | Wire.beginTransmission(IMUAddress); 27 | Wire.write(registerAddress); 28 | Wire.write(data, length); 29 | uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success 30 | if (rcode) { 31 | Serial.print(F("i2cWrite failed: ")); 32 | Serial.println(rcode); 33 | } 34 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 35 | } 36 | 37 | uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) { 38 | uint32_t timeOutTimer; 39 | Wire.beginTransmission(IMUAddress); 40 | Wire.write(registerAddress); 41 | uint8_t rcode = Wire.endTransmission(false); // Don't release the bus 42 | if (rcode) { 43 | Serial.print(F("i2cRead failed: ")); 44 | Serial.println(rcode); 45 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission 46 | } 47 | Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading 48 | for (uint8_t i = 0; i < nbytes; i++) { 49 | if (Wire.available()) 50 | data[i] = Wire.read(); 51 | else { 52 | timeOutTimer = micros(); 53 | while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available()); 54 | if (Wire.available()) 55 | data[i] = Wire.read(); 56 | else { 57 | Serial.println(F("i2cRead timeout")); 58 | return 5; // This error value is not already taken by endTransmission 59 | } 60 | } 61 | } 62 | return 0; // Success 63 | } 64 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h 19 | #define _Kalman_h 20 | 21 | class Kalman { 22 | public: 23 | Kalman() { 24 | /* We will set the variables like so, these can also be tuned by the user */ 25 | Q_angle = 0.001; 26 | Q_bias = 0.003; 27 | R_measure = 0.03; 28 | 29 | angle = 0; // Reset the angle 30 | bias = 0; // Reset bias 31 | 32 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 33 | P[0][1] = 0; 34 | P[1][0] = 0; 35 | P[1][1] = 0; 36 | }; 37 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 38 | double getAngle(double newAngle, double newRate, double dt) { 39 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 40 | // Modified by Kristian Lauszus 41 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 42 | 43 | // Discrete Kalman filter time update equations - Time Update ("Predict") 44 | // Update xhat - Project the state ahead 45 | /* Step 1 */ 46 | rate = newRate - bias; 47 | angle += dt * rate; 48 | 49 | // Update estimation error covariance - Project the error covariance ahead 50 | /* Step 2 */ 51 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 52 | P[0][1] -= dt * P[1][1]; 53 | P[1][0] -= dt * P[1][1]; 54 | P[1][1] += Q_bias * dt; 55 | 56 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 57 | // Calculate Kalman gain - Compute the Kalman gain 58 | /* Step 4 */ 59 | S = P[0][0] + R_measure; 60 | /* Step 5 */ 61 | K[0] = P[0][0] / S; 62 | K[1] = P[1][0] / S; 63 | 64 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 65 | /* Step 3 */ 66 | y = newAngle - angle; 67 | /* Step 6 */ 68 | angle += K[0] * y; 69 | bias += K[1] * y; 70 | 71 | // Calculate estimation error covariance - Update the error covariance 72 | /* Step 7 */ 73 | P[0][0] -= K[0] * P[0][0]; 74 | P[0][1] -= K[0] * P[0][1]; 75 | P[1][0] -= K[1] * P[0][0]; 76 | P[1][1] -= K[1] * P[0][1]; 77 | 78 | return angle; 79 | }; 80 | void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 81 | double getRate() { return rate; }; // Return the unbiased rate 82 | 83 | /* These are used to tune the Kalman filter */ 84 | void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 85 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 86 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 87 | 88 | double getQangle() { return Q_angle; }; 89 | double getQbias() { return Q_bias; }; 90 | double getRmeasure() { return R_measure; }; 91 | 92 | private: 93 | /* Kalman filter variables */ 94 | double Q_angle; // Process noise variance for the accelerometer 95 | double Q_bias; // Process noise variance for the gyro bias 96 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 97 | 98 | double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 99 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 100 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 101 | 102 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 103 | double K[2]; // Kalman gain - This is a 2x1 vector 104 | double y; // Angle difference 105 | double S; // Estimate error 106 | }; 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/R3_L298N_stepper_v3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pumanzor/arduino-stuff/4646d1201833b48baf1e406029819bc84144f4fa/imu/ITGMPU/GY521/R3_L298N_stepper_v3.png -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/README.md: -------------------------------------------------------------------------------- 1 | ![alt tag](https://raw.githubusercontent.com/pumanzor/arduino-stuff/master/imu/ITGMPU/GY521/R3_L298N_stepper_v3.png) 2 | 3 | Materials 4 | 5 | 1 x L298N Driver 6 | 1 x Stepper motor 4 wire 7 | 1 X Arduino R3 8 | 1 x Gyroscope/Accelerometer ITG GY-521 MPU6050 9 | 10 | ------------------------- 11 | Software 12 | 13 | I2C.ino and Kalman.h MUST be in the same directory that sketch 14 | 15 | ------------------------- 16 | 17 | Pinout 18 | 19 | ITG Gy-521 to Arduino 20 | 21 | ITG pin SCL ---> Pin5 Arduino 22 | ITG pin SDA ---> Pin4 Arduino 23 | ITG VCC ----> 5v Arduino 24 | ITG GND ---> GND Arduino 25 | 26 | 27 | L298N to Arduino 28 | 29 | L298N pin IN1 ---> Pin 11 Arduino 30 | L298N pin IN2 ---> Pin 10 Arduino 31 | L298N pin IN3 ---> Pin 9 Arduino 32 | L298N pin IN4 ---> Pin 8 Arduino 33 | 34 | L298N Pin GND ---> Pin GND Arduino (please don't forget this connection, otherwise the motor it doesn't work) 35 | 36 | 37 | L298N to Motor 38 | 39 | First you will need figure out what are the coils, take a multimeter and change to ohmmeter, 40 | then determine which pairs are group A and group B 41 | 42 | then 43 | 44 | L298N Out1/out2 ---> to Group A motor 45 | L298N Out3/Out4 ---> to Group B motor 46 | 47 | ------------------------------------------- 48 | 49 | LN982N and Power supply 50 | 51 | You will need to connect L298N to external power source 5vdc or 12Vdc , it depends of motor voltage. 52 | -------------------------------------------------------------------------------- /imu/ITGMPU/GY521/stepper_L298N_gyro_control_knob.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "Kalman.h" 4 | 5 | #define RESTRICT_PITCH 6 | 7 | Kalman kalmanX; 8 | Kalman kalmanY; 9 | 10 | 11 | double accX, accY, accZ; 12 | double gyroX, gyroY, gyroZ; 13 | int16_t tempRaw; 14 | 15 | double gyroXangle, gyroYangle; 16 | double compAngleX, compAngleY; 17 | double kalAngleX, kalAngleY; 18 | 19 | uint32_t timer; 20 | uint8_t i2cData[14]; // Buffer for I2C data 21 | 22 | Stepper stepper2(400, 11,10,9,8); 23 | int cantidad = 1; 24 | 25 | int previous = 0; 26 | void setup() { 27 | 28 | stepper2.setSpeed(100); 29 | Serial.begin(115200); 30 | Wire.begin(); 31 | TWBR = ((F_CPU /400000L) - 16) / 2; // Set I2C frequency to 400kHz 32 | 33 | i2cData[0] = 7; 34 | i2cData[1] = 0x00; 35 | i2cData[2] = 0x00; 36 | i2cData[3] = 0x00; 37 | while (i2cWrite(0x19, i2cData, 4, false)); 38 | while (i2cWrite(0x6B, 0x01, true)); 39 | 40 | while (i2cRead(0x75, i2cData, 1)); 41 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 42 | Serial.print(F("Error reading sensor")); 43 | while (1); 44 | } 45 | 46 | delay(100); 47 | 48 | while (i2cRead(0x3B, i2cData, 6)); 49 | accX = (i2cData[0] << 8) | i2cData[1]; 50 | accY = (i2cData[2] << 8) | i2cData[3]; 51 | accZ = (i2cData[4] << 8) | i2cData[5]; 52 | 53 | 54 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 55 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 56 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 57 | #else // Eq. 28 and 29 58 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 59 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 60 | #endif 61 | 62 | kalmanX.setAngle(roll); // Set starting angle 63 | kalmanY.setAngle(pitch); 64 | gyroXangle = roll; 65 | gyroYangle = pitch; 66 | compAngleX = roll; 67 | compAngleY = pitch; 68 | 69 | timer = micros(); 70 | } 71 | 72 | void loop() { 73 | 74 | while (i2cRead(0x3B, i2cData, 14)); 75 | accX = ((i2cData[0] << 8) | i2cData[1]); 76 | accY = ((i2cData[2] << 8) | i2cData[3]); 77 | accZ = ((i2cData[4] << 8) | i2cData[5]); 78 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 79 | gyroX = (i2cData[8] << 8) | i2cData[9]; 80 | gyroY = (i2cData[10] << 8) | i2cData[11]; 81 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 82 | 83 | double dt = (double)(micros() - timer) / 1000000; 84 | timer = micros(); 85 | 86 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 87 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 88 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 89 | #else // Eq. 28 and 29 90 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 91 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 92 | #endif 93 | 94 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 95 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 96 | 97 | #ifdef RESTRICT_PITCH 98 | 99 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 100 | kalmanX.setAngle(roll); 101 | compAngleX = roll; 102 | kalAngleX = roll; 103 | gyroXangle = roll; 104 | } else 105 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); 106 | 107 | if (abs(kalAngleX) > 90) 108 | gyroYrate = -gyroYrate; 109 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 110 | #else 111 | 112 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 113 | kalmanY.setAngle(pitch); 114 | compAngleY = pitch; 115 | kalAngleY = pitch; 116 | gyroYangle = pitch; 117 | } else 118 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 119 | 120 | if (abs(kalAngleY) > 90) 121 | gyroXrate = -gyroXrate; 122 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); 123 | #endif 124 | 125 | gyroXangle += gyroXrate * dt; 126 | gyroYangle += gyroYrate * dt; 127 | 128 | 129 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; 130 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 131 | 132 | 133 | if (gyroXangle < -180 || gyroXangle > 180) 134 | gyroXangle = kalAngleX; 135 | if (gyroYangle < -180 || gyroYangle > 180) 136 | gyroYangle = kalAngleY; 137 | 138 | 139 | #if 0 // Set to 1 to activate 140 | Serial.print(accX); Serial.print("\t"); 141 | Serial.print(accY); Serial.print("\t"); 142 | Serial.print(accZ); Serial.print("\t"); 143 | 144 | Serial.print(gyroX); Serial.print("\t"); 145 | Serial.print(gyroY); Serial.print("\t"); 146 | Serial.print(gyroZ); Serial.print("\t"); 147 | 148 | Serial.print("\t"); 149 | #endif 150 | 151 | 152 | Serial.print(kalAngleX); Serial.print("\t"); 153 | 154 | Serial.print("\t"); 155 | 156 | Serial.print(kalAngleY); Serial.print("\t"); 157 | 158 | #if 0 159 | Serial.print("\t"); 160 | 161 | double temperature = (double)tempRaw / 340.0 + 36.53; 162 | Serial.print(temperature); Serial.print("\t"); 163 | #endif 164 | 165 | Serial.print("\r\n"); 166 | 167 | 168 | int valuex = kalAngleX; 169 | 170 | 171 | stepper2.step(valuex - previous); 172 | 173 | 174 | previous = valuex; 175 | 176 | 177 | 178 | } 179 | -------------------------------------------------------------------------------- /imu/Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h 19 | #define _Kalman_h 20 | 21 | class Kalman { 22 | public: 23 | Kalman() { 24 | /* We will set the variables like so, these can also be tuned by the user */ 25 | Q_angle = 0.001; 26 | Q_bias = 0.003; 27 | R_measure = 0.03; 28 | 29 | angle = 0; // Reset the angle 30 | bias = 0; // Reset bias 31 | 32 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_appl 33 | ication.2C_technical 34 | P[0][1] = 0; 35 | P[1][0] = 0; 36 | P[1][1] = 0; 37 | }; 38 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 39 | double getAngle(double newAngle, double newRate, double dt) { 40 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 41 | // Modified by Kristian Lauszus 42 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 43 | 44 | // Discrete Kalman filter time update equations - Time Update ("Predict") 45 | // Update xhat - Project the state ahead 46 | /* Step 1 */ 47 | rate = newRate - bias; 48 | angle += dt * rate; 49 | 50 | // Update estimation error covariance - Project the error covariance ahead 51 | /* Step 2 */ 52 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 53 | 54 | ...back 1 page 55 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 56 | 57 | This software may be distributed and modified under the terms of the GNU 58 | General Public License version 2 (GPL2) as published by the Free Software 59 | Foundation and appearing in the file GPL2.TXT included in the packaging of 60 | this file. Please note that GPL2 Section 2[b] requires that all works based 61 | on this software must also be made publicly available under the terms of 62 | the GPL2 ("Copyleft"). 63 | 64 | Contact information 65 | ------------------- 66 | 67 | Kristian Lauszus, TKJ Electronics 68 | Web : http://www.tkjelectronics.com 69 | e-mail : kristianl@tkjelectronics.com 70 | */ 71 | 72 | #ifndef _Kalman_h 73 | #define _Kalman_h 74 | 75 | class Kalman { 76 | public: 77 | Kalman() { 78 | /* We will set the variables like so, these can also be tuned by the user */ 79 | Q_angle = 0.001; 80 | Q_bias = 0.003; 81 | R_measure = 0.03; 82 | 83 | angle = 0; // Reset the angle 84 | bias = 0; // Reset bias 85 | 86 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_appl 87 | ication.2C_technical 88 | P[0][1] = 0; 89 | P[1][0] = 0; 90 | P[1][1] = 0; 91 | }; 92 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 93 | double getAngle(double newAngle, double newRate, double dt) { 94 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 95 | // Modified by Kristian Lauszus 96 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 97 | 98 | // Discrete Kalman filter time update equations - Time Update ("Predict") 99 | // Update xhat - Project the state ahead 100 | /* Step 1 */ 101 | rate = newRate - bias; 102 | angle += dt * rate; 103 | 104 | // Update estimation error covariance - Project the error covariance ahead 105 | /* Step 2 */ 106 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 107 | P[0][1] -= dt * P[1][1]; 108 | P[1][0] -= dt * P[1][1]; 109 | P[1][1] += Q_bias * dt; 110 | 111 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 112 | // Calculate Kalman gain - Compute the Kalman gain 113 | /* Step 4 */ 114 | S = P[0][0] + R_measure; 115 | /* Step 5 */ 116 | K[0] = P[0][0] / S; 117 | K[1] = P[1][0] / S; 118 | 119 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 120 | /* Step 3 */ 121 | y = newAngle - angle; 122 | /* Step 6 */ 123 | angle += K[0] * y; 124 | bias += K[1] * y; 125 | 126 | // Calculate estimation error covariance - Update the error covariance 127 | /* Step 7 */ 128 | P[0][0] -= K[0] * P[0][0]; 129 | P[0][1] -= K[0] * P[0][1]; 130 | P[1][0] -= K[1] * P[0][0]; 131 | P[1][1] -= K[1] * P[0][1]; 132 | 133 | return angle; 134 | }; 135 | void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 136 | double getRate() { return rate; }; // Return the unbiased rate 137 | 138 | /* These are used to tune the Kalman filter */ 139 | void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 140 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 141 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 142 | 143 | double getQangle() { return Q_angle; }; 144 | double getQbias() { return Q_bias; }; 145 | double getRmeasure() { return R_measure; }; 146 | 147 | private: 148 | /* Kalman filter variables */ 149 | double Q_angle; // Process noise variance for the accelerometer 150 | double Q_bias; // Process noise variance for the gyro bias 151 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 152 | 153 | double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 154 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 155 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 156 | 157 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 158 | double K[2]; // Kalman gain - This is a 2x1 vector 159 | double y; // Angle difference 160 | double S; // Estimate error 161 | }; 162 | 163 | #endif 164 | -------------------------------------------------------------------------------- /imu/Kalman_Arduino_R3_MPU6050_2xmotor_stepper.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /////kalman--------------------------------------------- 4 | #include 5 | #include "Kalman.h" 6 | #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - 7 | Kalman kalmanX; // Create the Kalman instances 8 | Kalman kalmanY; 9 | 10 | /* IMU Data */ 11 | double accX, accY, accZ; 12 | double gyroX, gyroY, gyroZ; 13 | int16_t tempRaw; 14 | 15 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 16 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 17 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 18 | 19 | uint32_t timer; 20 | uint8_t i2cData[14]; // Buffer for I2C data 21 | //////Kalman------------------------------------------------- 22 | 23 | 24 | AccelStepper stepper1(1, 9, 8); 25 | 26 | AccelStepper stepper2(1, 7, 6); 27 | int pos = 2000; 28 | int cantidad = 7; 29 | int addr = 0; 30 | 31 | 32 | // Define our three input button pins 33 | #define LEFT_PIN 4 34 | #define STOP_PIN 3 35 | #define RIGHT_PIN 5 36 | #define CONTROL_PIN 11 37 | 38 | #define SPEED_PIN 0 39 | 40 | // Define our maximum and minimum speed in steps per second (scale pot to these) 41 | #define MAX_SPEED 1500 42 | #define MIN_SPEED 0.1 43 | 44 | 45 | void setup() { 46 | 47 | 48 | // The only AccelStepper value we have to set here is the max speeed, which is higher than we'll ever go 49 | stepper1.setMaxSpeed(10000.0); 50 | stepper2.setMaxSpeed(10000.0); 51 | 52 | stepper1.setAcceleration(500); 53 | stepper2.setAcceleration(500); 54 | 55 | // Set up the three button inputs, with pullups 56 | pinMode(LEFT_PIN, INPUT_PULLUP); 57 | pinMode(STOP_PIN, INPUT_PULLUP); 58 | pinMode(RIGHT_PIN, INPUT_PULLUP); 59 | pinMode(CONTROL_PIN, INPUT_PULLUP); 60 | 61 | ////////kalmen--------------------------------------------------------------------- 62 | Serial.begin(115200); 63 | Wire.begin(); 64 | //TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 65 | TWBR = ((F_CPU / 100000L) - 16) / 2; // Set I2C frequency to 100kHz 66 | 67 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 68 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 69 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 70 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 71 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 72 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 73 | 74 | while (i2cRead(0x75, i2cData, 1)); 75 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 76 | Serial.print(F("Error reading sensor")); 77 | while (1); 78 | } 79 | 80 | delay(100); // Wait for sensor to stabilize 81 | 82 | /* Set kalman and gyro starting angle */ 83 | while (i2cRead(0x3B, i2cData, 6)); 84 | accX = (i2cData[0] << 8) | i2cData[1]; 85 | accY = (i2cData[2] << 8) | i2cData[3]; 86 | accZ = (i2cData[4] << 8) | i2cData[5]; 87 | 88 | 89 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 90 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 91 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 92 | #else // Eq. 28 and 29 93 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 94 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 95 | #endif 96 | 97 | kalmanX.setAngle(roll); // Set starting angle 98 | kalmanY.setAngle(pitch); 99 | gyroXangle = roll; 100 | gyroYangle = pitch; 101 | compAngleX = roll; 102 | compAngleY = pitch; 103 | 104 | timer = micros(); 105 | //////////////////kalman----------------------------------------- 106 | } 107 | 108 | void loop() { 109 | 110 | 111 | ////////////kalman-------------------------------------------------- 112 | 113 | /* Update all the values */ 114 | while (i2cRead(0x3B, i2cData, 14)); 115 | accX = ((i2cData[0] << 8) | i2cData[1]); 116 | accY = ((i2cData[2] << 8) | i2cData[3]); 117 | accZ = ((i2cData[4] << 8) | i2cData[5]); 118 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 119 | gyroX = (i2cData[8] << 8) | i2cData[9]; 120 | gyroY = (i2cData[10] << 8) | i2cData[11]; 121 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 122 | 123 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 124 | timer = micros(); 125 | 126 | 127 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 128 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 129 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 130 | #else // Eq. 28 and 29 131 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 132 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 133 | #endif 134 | 135 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 136 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 137 | 138 | #ifdef RESTRICT_PITCH 139 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 140 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 141 | kalmanX.setAngle(roll); 142 | compAngleX = roll; 143 | kalAngleX = roll; 144 | gyroXangle = roll; 145 | } else 146 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 147 | 148 | if (abs(kalAngleX) > 90) 149 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 150 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 151 | #else 152 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 153 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 154 | kalmanY.setAngle(pitch); 155 | compAngleY = pitch; 156 | kalAngleY = pitch; 157 | gyroYangle = pitch; 158 | } else 159 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 160 | 161 | if (abs(kalAngleY) > 90) 162 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 163 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 164 | #endif 165 | 166 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 167 | gyroYangle += gyroYrate * dt; 168 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 169 | //gyroYangle += kalmanY.getRate() * dt; 170 | 171 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 172 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 173 | 174 | // Reset the gyro angle when it has drifted too much 175 | if (gyroXangle < -180 || gyroXangle > 180) 176 | gyroXangle = kalAngleX; 177 | if (gyroYangle < -180 || gyroYangle > 180) 178 | gyroYangle = kalAngleY; 179 | 180 | /* Print Data */ 181 | #if 1// Set to 1 to activate 182 | //Serial.print(accX); Serial.print("\t"); 183 | //Serial.print(accY); Serial.print("\t"); 184 | //Serial.print(accZ); Serial.print("\t"); 185 | 186 | //Serial.print(gyroX); Serial.print("\t"); 187 | //Serial.print(gyroY); Serial.print("\t"); 188 | //Serial.print(gyroZ); Serial.print("\t"); 189 | 190 | Serial.print("\t"); 191 | #endif 192 | 193 | //Serial.print(roll); Serial.print("\t"); 194 | //Serial.print(gyroXangle); Serial.print("\t"); 195 | //Serial.print(compAngleX); Serial.print("\t"); 196 | Serial.print(kalAngleX); Serial.print("\t"); 197 | 198 | Serial.print("\t"); 199 | 200 | //Serial.print(pitch); Serial.print("\t"); 201 | //Serial.print(gyroYangle); Serial.print("\t"); 202 | //Serial.print(compAngleY); Serial.print("\t"); 203 | Serial.print(kalAngleY); Serial.print("\t"); 204 | 205 | #if 0 // Set to 1 to print the temperature 206 | Serial.print("\t"); 207 | 208 | double temperature = (double)tempRaw / 340.0 + 36.53; 209 | Serial.print(temperature); Serial.print("\t"); 210 | #endif 211 | 212 | Serial.print("\r\n"); 213 | 214 | 215 | ///////////////////-kalman----------------------------------------- 216 | 217 | 218 | //if (digitalRead(CONTROL_PIN) == LOW) 219 | 220 | 221 | //{ 222 | 223 | int valuey = kalAngleX; 224 | int valuex = kalAngleY; 225 | 226 | 227 | if (valuey >= -89 && valuey <= -85 ) 228 | 229 | {stepper2.moveTo(-87 * cantidad); 230 | stepper2.run();} 231 | 232 | else if (valuey >= -84 && valuey <= -80 ) 233 | 234 | {stepper2.moveTo(-83 * cantidad); 235 | stepper2.run();} 236 | 237 | else if (valuey >= -79 && valuey <= -75 ) 238 | 239 | {stepper2.moveTo(-77 * cantidad); 240 | stepper2.run();} 241 | 242 | else if (valuey >= -74 && valuey <= -70 ) 243 | 244 | {stepper2.moveTo(-73 * cantidad); 245 | stepper2.run();} 246 | 247 | else if (valuey >= -69 && valuey <= -65 ) 248 | 249 | {stepper2.moveTo(-67 * cantidad); 250 | stepper2.run();} 251 | 252 | else if (valuey >= -64 && valuey <= -60 ) 253 | 254 | {stepper2.moveTo(-63 * cantidad); 255 | stepper2.run();} 256 | 257 | else if (valuey >= -59 && valuey <= -55 ) 258 | 259 | {stepper2.moveTo(-67 * cantidad); 260 | stepper2.run();} 261 | 262 | else if (valuey >= -54 && valuey <= -50 ) 263 | 264 | {stepper2.moveTo(-53 * cantidad); 265 | stepper2.run();} 266 | 267 | else if (valuey >= -49 && valuey <= -45 ) 268 | 269 | {stepper2.moveTo(-47 * cantidad); 270 | stepper2.run();} 271 | 272 | else if (valuey >= -44 && valuey <= -40 ) 273 | 274 | {stepper2.moveTo(-43 * cantidad); 275 | stepper2.run();} 276 | 277 | else if (valuey >= -39 && valuey <= -35 ) 278 | 279 | {stepper2.moveTo(-37 * cantidad); 280 | stepper2.run();} 281 | 282 | else if (valuey >= -34 && valuey <= -30 ) 283 | 284 | {stepper2.moveTo(-33 * cantidad); 285 | stepper2.run();} 286 | 287 | else if (valuey >= -29 && valuey <= -25 ) 288 | 289 | {stepper2.moveTo(-27 * cantidad); 290 | stepper2.run();} 291 | 292 | else if (valuey >= -24 && valuey <= -20 ) 293 | 294 | {stepper2.moveTo(-23 * cantidad); 295 | stepper2.run();} 296 | 297 | else if (valuey >= -19 && valuey <= -15 ) 298 | 299 | {stepper2.moveTo(-17 * cantidad); 300 | stepper2.run();} 301 | 302 | else if (valuey >= -14 && valuey <= -10 ) 303 | 304 | {stepper2.moveTo(-13 * cantidad); 305 | stepper2.run();} 306 | 307 | else if (valuey >= -9 && valuey <= -5 ) 308 | 309 | {stepper2.moveTo(-7 * cantidad); 310 | stepper2.run();} 311 | 312 | else if (valuey >= -4 && valuey <= -1 ) 313 | 314 | {stepper2.moveTo(-3 * cantidad); 315 | stepper2.run();} 316 | 317 | else if (valuey >= 1 && valuey <= 5 ) 318 | 319 | {stepper2.moveTo(3 * cantidad); 320 | stepper2.run();} 321 | 322 | else if (valuey >= 6 && valuey <= 10 ) 323 | 324 | {stepper2.moveTo(8 * cantidad); 325 | stepper2.run();} 326 | 327 | else if (valuey >= 11 && valuey <= 15 ) 328 | 329 | {stepper2.moveTo(13 * cantidad); 330 | stepper2.run();} 331 | 332 | else if (valuey >= 16 && valuey <= 20 ) 333 | 334 | {stepper2.moveTo(18 * cantidad); 335 | stepper2.run();} 336 | 337 | else if (valuey >= 21 && valuey <= 25 ) 338 | 339 | {stepper2.moveTo(23 * cantidad); 340 | stepper2.run();} 341 | 342 | else if (valuey >= 26 && valuey <= 30 ) 343 | 344 | {stepper2.moveTo(28 * cantidad); 345 | stepper2.run();} 346 | 347 | else if (valuey >= 31 && valuey <= 35 ) 348 | 349 | {stepper2.moveTo(33 * cantidad); 350 | stepper2.run();} 351 | 352 | else if (valuey >= 36 && valuey <= 40 ) 353 | 354 | {stepper2.moveTo(38 * cantidad); 355 | stepper2.run();} 356 | 357 | else if (valuey >= 41 && valuey <= 45 ) 358 | 359 | {stepper2.moveTo(43 * cantidad); 360 | stepper2.run();} 361 | 362 | else if (valuey >= 46 && valuey <= 50 ) 363 | 364 | {stepper2.moveTo(48 * cantidad); 365 | stepper2.run();} 366 | 367 | else if (valuey >= 51 && valuey <= 55 ) 368 | 369 | {stepper2.moveTo(53 * cantidad); 370 | stepper2.run();} 371 | 372 | else if (valuey >= 56 && valuey <= 60 ) 373 | 374 | {stepper2.moveTo(58 * cantidad); 375 | stepper2.run();} 376 | 377 | else if (valuey >= 61 && valuey <= 65 ) 378 | 379 | {stepper2.moveTo(63 * cantidad); 380 | stepper2.run();} 381 | 382 | else if (valuey >= 66 && valuey <= 70 ) 383 | 384 | {stepper2.moveTo(68 * cantidad); 385 | stepper2.run();} 386 | 387 | else if (valuey >= 71 && valuey <= 85 ) 388 | 389 | {stepper2.moveTo(73 * cantidad); 390 | stepper2.run();} 391 | 392 | 393 | 394 | else if (valuey >= 86 && valuey <= 89 ) 395 | 396 | {stepper2.moveTo(88 * cantidad); 397 | stepper2.run();} 398 | 399 | ////////////////////////////// 400 | ///////////////////////////// 401 | ///////////////////////////// 402 | 403 | 404 | else {} 405 | 406 | 407 | if (valuex >= -89 && valuex <= -85 ) 408 | 409 | {stepper1.moveTo(-87 * cantidad); 410 | stepper1.run();} 411 | 412 | else if (valuex >= -84 && valuex <= -80 ) 413 | 414 | {stepper1.moveTo(-83 * cantidad); 415 | stepper1.run();} 416 | 417 | else if (valuex >= -79 && valuex <= -75 ) 418 | 419 | {stepper1.moveTo(-77 * cantidad); 420 | stepper1.run();} 421 | 422 | else if (valuex >= -74 && valuex <= -70 ) 423 | 424 | {stepper1.moveTo(-73 * cantidad); 425 | stepper1.run();} 426 | 427 | else if (valuex >= -69 && valuex <= -65 ) 428 | 429 | {stepper1.moveTo(-67 * cantidad); 430 | stepper1.run();} 431 | 432 | else if (valuex >= -64 && valuex <= -60 ) 433 | 434 | {stepper1.moveTo(-63 * cantidad); 435 | stepper1.run();} 436 | 437 | else if (valuex >= -59 && valuex <= -55 ) 438 | 439 | {stepper1.moveTo(-67 * cantidad); 440 | stepper1.run();} 441 | 442 | else if (valuex >= -54 && valuex <= -50 ) 443 | 444 | {stepper1.moveTo(-53 * cantidad); 445 | stepper1.run();} 446 | 447 | else if (valuex >= -49 && valuex <= -45 ) 448 | 449 | {stepper1.moveTo(-47 * cantidad); 450 | stepper1.run();} 451 | 452 | else if (valuex >= -44 && valuex <= -40 ) 453 | 454 | {stepper1.moveTo(-43 * cantidad); 455 | stepper1.run();} 456 | 457 | else if (valuex >= -39 && valuex <= -35 ) 458 | 459 | {stepper1.moveTo(-37 * cantidad); 460 | stepper1.run();} 461 | 462 | else if (valuex >= -34 && valuex <= -30 ) 463 | 464 | {stepper1.moveTo(-33 * cantidad); 465 | stepper1.run();} 466 | 467 | else if (valuex >= -29 && valuex <= -25 ) 468 | 469 | {stepper1.moveTo(-27 * cantidad); 470 | stepper1.run();} 471 | 472 | else if (valuex >= -24 && valuex <= -20 ) 473 | 474 | {stepper1.moveTo(-23 * cantidad); 475 | stepper1.run();} 476 | 477 | else if (valuex >= -19 && valuex <= -15 ) 478 | 479 | {stepper1.moveTo(-17 * cantidad); 480 | stepper1.run();} 481 | 482 | else if (valuex >= -14 && valuex <= -10 ) 483 | 484 | {stepper1.moveTo(-13 * cantidad); 485 | stepper1.run();} 486 | 487 | else if (valuex >= -9 && valuex <= -5 ) 488 | 489 | {stepper1.moveTo(-7 * cantidad); 490 | stepper1.run();} 491 | 492 | else if (valuex >= -4 && valuex <= -1 ) 493 | 494 | {stepper1.moveTo(-3 * cantidad); 495 | stepper1.run();} 496 | 497 | else if (valuex >= 1 && valuex <= 5 ) 498 | 499 | {stepper1.moveTo(3 * cantidad); 500 | stepper1.run();} 501 | 502 | else if (valuex >= 6 && valuex <= 10 ) 503 | 504 | {stepper1.moveTo(8 * cantidad); 505 | stepper1.run();} 506 | 507 | else if (valuex >= 11 && valuex <= 15 ) 508 | 509 | {stepper1.moveTo(13 * cantidad); 510 | stepper1.run();} 511 | 512 | else if (valuex >= 16 && valuex <= 20 ) 513 | 514 | {stepper1.moveTo(18 * cantidad); 515 | stepper1.run();} 516 | 517 | else if (valuex >= 21 && valuex <= 25 ) 518 | 519 | {stepper1.moveTo(23 * cantidad); 520 | stepper1.run();} 521 | 522 | else if (valuex >= 26 && valuex <= 30 ) 523 | 524 | {stepper1.moveTo(28 * cantidad); 525 | stepper1.run();} 526 | 527 | else if (valuex >= 31 && valuex <= 35 ) 528 | 529 | {stepper1.moveTo(33 * cantidad); 530 | stepper1.run();} 531 | 532 | else if (valuex >= 36 && valuex <= 40 ) 533 | 534 | {stepper1.moveTo(38 * cantidad); 535 | stepper1.run();} 536 | 537 | else if (valuex >= 41 && valuex <= 45 ) 538 | 539 | {stepper1.moveTo(43 * cantidad); 540 | stepper1.run();} 541 | 542 | else if (valuex >= 46 && valuex <= 50 ) 543 | 544 | {stepper1.moveTo(48 * cantidad); 545 | stepper1.run();} 546 | 547 | else if (valuex >= 51 && valuex <= 55 ) 548 | 549 | {stepper1.moveTo(53 * cantidad); 550 | stepper1.run();} 551 | 552 | else if (valuex >= 56 && valuex <= 60 ) 553 | 554 | {stepper1.moveTo(58 * cantidad); 555 | stepper1.run();} 556 | 557 | else if (valuex >= 61 && valuex <= 65 ) 558 | 559 | {stepper1.moveTo(63 * cantidad); 560 | stepper1.run();} 561 | 562 | else if (valuex >= 66 && valuex <= 70 ) 563 | 564 | {stepper1.moveTo(68 * cantidad); 565 | stepper1.run();} 566 | 567 | else if (valuex >= 71 && valuex <= 85 ) 568 | 569 | {stepper1.moveTo(73 * cantidad); 570 | stepper1.run();} 571 | 572 | 573 | 574 | else if (valuex >= 86 && valuex <= 89 ) 575 | 576 | {stepper1.moveTo(88 * cantidad); 577 | stepper1.run();} 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | else {} 588 | 589 | //////_--- 590 | 591 | 592 | //} 593 | 594 | 595 | /*else 596 | 597 | 598 | { 599 | 600 | if (digitalRead(LEFT_PIN) == 0) { 601 | 602 | stepper2.moveTo(pos); 603 | stepper2.run(); 604 | } 605 | 606 | else if (digitalRead(RIGHT_PIN) == 0) { 607 | 608 | stepper2.moveTo(-pos); 609 | stepper2.run(); 610 | } 611 | 612 | else if (digitalRead(STOP_PIN) == 0) { 613 | 614 | stepper2.moveTo(0); 615 | stepper2.run(); 616 | } 617 | 618 | 619 | 620 | /////////////web server 621 | 622 | /////////// 623 | 624 | }*/ 625 | 626 | } 627 | -------------------------------------------------------------------------------- /imu/R3_MPU6050_2xstepper.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "Kalman.h" 4 | 5 | 6 | #define RESTRICT_PITCH 7 | 8 | Kalman kalmanX; 9 | Kalman kalmanY; 10 | 11 | ///////////stepeer 12 | 13 | AccelStepper stepper1(1, 9, 8); 14 | 15 | AccelStepper stepper2(1, 7, 6); 16 | int pos = 2000; 17 | //int cantidad = 7; 18 | int addr = 0; 19 | 20 | 21 | // Define our three input button pins 22 | #define LEFT_PIN 4 23 | #define STOP_PIN 3 24 | #define RIGHT_PIN 5 25 | #define CONTROL_PIN 11 26 | 27 | #define SPEED_PIN 0 28 | 29 | int velocidad = 1000; 30 | int cantidad = 6; 31 | int aceleracion = 500; 32 | 33 | 34 | 35 | /* IMU Data */ 36 | double accX, accY, accZ; 37 | double gyroX, gyroY, gyroZ; 38 | int16_t tempRaw; 39 | 40 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 41 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 42 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 43 | 44 | uint32_t timer; 45 | uint8_t i2cData[14]; // Buffer for I2C data 46 | 47 | // TODO: Make calibration routine 48 | 49 | void setup() { 50 | pinMode(LEFT_PIN, INPUT_PULLUP); 51 | pinMode(STOP_PIN, INPUT_PULLUP); 52 | pinMode(RIGHT_PIN, INPUT_PULLUP); 53 | pinMode(CONTROL_PIN, INPUT_PULLUP); 54 | 55 | 56 | 57 | Serial.begin(115200); 58 | Wire.begin(); 59 | 60 | 61 | 62 | 63 | TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 64 | 65 | stepper1.setMaxSpeed(3000); 66 | stepper2.setMaxSpeed(3000); 67 | 68 | stepper1.setAcceleration(500); 69 | stepper2.setAcceleration(500); 70 | 71 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 72 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 73 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 74 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 75 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 76 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 77 | 78 | while (i2cRead(0x75, i2cData, 1)); 79 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 80 | Serial.print(F("Error reading sensor")); 81 | while (1); 82 | } 83 | 84 | delay(100); // Wait for sensor to stabilize 85 | 86 | /* Set kalman and gyro starting angle */ 87 | while (i2cRead(0x3B, i2cData, 6)); 88 | accX = (i2cData[0] << 8) | i2cData[1]; 89 | accY = (i2cData[2] << 8) | i2cData[3]; 90 | accZ = (i2cData[4] << 8) | i2cData[5]; 91 | 92 | 93 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 94 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 95 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 96 | #else // Eq. 28 and 29 97 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 98 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 99 | #endif 100 | 101 | kalmanX.setAngle(roll); // Set starting angle 102 | kalmanY.setAngle(pitch); 103 | gyroXangle = roll; 104 | gyroYangle = pitch; 105 | compAngleX = roll; 106 | compAngleY = pitch; 107 | 108 | 109 | } 110 | 111 | 112 | /////////////////7 113 | 114 | ///////////////7 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | void loop() { 123 | 124 | 125 | ////// 126 | 127 | ///// 128 | 129 | 130 | 131 | 132 | /* Update all the values */ 133 | while (i2cRead(0x3B, i2cData, 14)); 134 | accX = ((i2cData[0] << 8) | i2cData[1]); 135 | accY = ((i2cData[2] << 8) | i2cData[3]); 136 | accZ = ((i2cData[4] << 8) | i2cData[5]); 137 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 138 | gyroX = (i2cData[8] << 8) | i2cData[9]; 139 | gyroY = (i2cData[10] << 8) | i2cData[11]; 140 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 141 | 142 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 143 | timer = micros(); 144 | 145 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 146 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 147 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 148 | #else // Eq. 28 and 29 149 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 150 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 151 | #endif 152 | 153 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 154 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 155 | 156 | #ifdef RESTRICT_PITCH 157 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 158 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 159 | kalmanX.setAngle(roll); 160 | compAngleX = roll; 161 | kalAngleX = roll; 162 | gyroXangle = roll; 163 | } else 164 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 165 | 166 | if (abs(kalAngleX) > 90) 167 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 168 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 169 | #else 170 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 171 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 172 | kalmanY.setAngle(pitch); 173 | compAngleY = pitch; 174 | kalAngleY = pitch; 175 | gyroYangle = pitch; 176 | } else 177 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 178 | 179 | if (abs(kalAngleY) > 90) 180 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 181 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 182 | #endif 183 | 184 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 185 | gyroYangle += gyroYrate * dt; 186 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 187 | //gyroYangle += kalmanY.getRate() * dt; 188 | 189 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 190 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 191 | 192 | // Reset the gyro angle when it has drifted too much 193 | if (gyroXangle < -180 || gyroXangle > 180) 194 | gyroXangle = kalAngleX; 195 | if (gyroYangle < -180 || gyroYangle > 180) 196 | gyroYangle = kalAngleY; 197 | 198 | /* Print Data */ 199 | #if 0 // Set to 1 to activate 200 | Serial.print(accX); Serial.print("\t"); 201 | Serial.print(accY); Serial.print("\t"); 202 | Serial.print(accZ); Serial.print("\t"); 203 | 204 | Serial.print(gyroX); Serial.print("\t"); 205 | Serial.print(gyroY); Serial.print("\t"); 206 | Serial.print(gyroZ); Serial.print("\t"); 207 | 208 | Serial.print("\t"); 209 | #endif 210 | 211 | //Serial.print(roll); Serial.print("\t"); 212 | //Serial.print(gyroXangle); Serial.print("\t"); 213 | //Serial.print(compAngleX); Serial.print("\t"); 214 | Serial.print(kalAngleX); Serial.print("\t"); 215 | 216 | Serial.print("\t"); 217 | 218 | //Serial.print(pitch); Serial.print("\t"); 219 | //Serial.print(gyroYangle); Serial.print("\t"); 220 | //Serial.print(compAngleY); Serial.print("\t"); 221 | Serial.print(kalAngleY); Serial.print("\t"); 222 | 223 | #if 0 // Set to 1 to print the temperature 224 | Serial.print("\t"); 225 | 226 | double temperature = (double)tempRaw / 340.0 + 36.53; 227 | Serial.print(temperature); Serial.print("\t"); 228 | #endif 229 | 230 | Serial.print("\r\n"); 231 | delay(2); 232 | 233 | 234 | //////////////////7 235 | 236 | 237 | if (digitalRead(CONTROL_PIN) == LOW) 238 | 239 | 240 | { 241 | 242 | int valuey = kalAngleX; 243 | int valuex = kalAngleY; 244 | 245 | 246 | if (valuey >= -89 && valuey <= -85 ) 247 | 248 | {stepper2.moveTo(-87 * cantidad); 249 | stepper2.run();} 250 | 251 | else if (valuey >= -84 && valuey <= -80 ) 252 | 253 | {stepper2.moveTo(-83 * cantidad); 254 | stepper2.run();} 255 | 256 | else if (valuey >= -79 && valuey <= -75 ) 257 | 258 | {stepper2.moveTo(-77 * cantidad); 259 | stepper2.run();} 260 | 261 | else if (valuey >= -74 && valuey <= -70 ) 262 | 263 | {stepper2.moveTo(-73 * cantidad); 264 | stepper2.run();} 265 | 266 | else if (valuey >= -69 && valuey <= -65 ) 267 | 268 | {stepper2.moveTo(-67 * cantidad); 269 | stepper2.run();} 270 | 271 | else if (valuey >= -64 && valuey <= -60 ) 272 | 273 | {stepper2.moveTo(-63 * cantidad); 274 | stepper2.run();} 275 | 276 | else if (valuey >= -59 && valuey <= -55 ) 277 | 278 | {stepper2.moveTo(-67 * cantidad); 279 | stepper2.run();} 280 | 281 | else if (valuey >= -54 && valuey <= -50 ) 282 | 283 | {stepper2.moveTo(-53 * cantidad); 284 | stepper2.run();} 285 | 286 | else if (valuey >= -49 && valuey <= -45 ) 287 | 288 | {stepper2.moveTo(-47 * cantidad); 289 | stepper2.run();} 290 | 291 | else if (valuey >= -44 && valuey <= -40 ) 292 | 293 | {stepper2.moveTo(-43 * cantidad); 294 | stepper2.run();} 295 | 296 | else if (valuey >= -39 && valuey <= -35 ) 297 | 298 | {stepper2.moveTo(-37 * cantidad); 299 | stepper2.run();} 300 | 301 | else if (valuey >= -34 && valuey <= -30 ) 302 | 303 | {stepper2.moveTo(-33 * cantidad); 304 | stepper2.run();} 305 | 306 | else if (valuey >= -29 && valuey <= -25 ) 307 | 308 | {stepper2.moveTo(-27 * cantidad); 309 | stepper2.run();} 310 | 311 | else if (valuey >= -24 && valuey <= -20 ) 312 | 313 | {stepper2.moveTo(-23 * cantidad); 314 | stepper2.run();} 315 | 316 | else if (valuey >= -19 && valuey <= -15 ) 317 | 318 | {stepper2.moveTo(-17 * cantidad); 319 | stepper2.run();} 320 | 321 | else if (valuey >= -14 && valuey <= -10 ) 322 | 323 | {stepper2.moveTo(-13 * cantidad); 324 | stepper2.run();} 325 | 326 | else if (valuey >= -9 && valuey <= -5 ) 327 | 328 | {stepper2.moveTo(-7 * cantidad); 329 | stepper2.run();} 330 | 331 | else if (valuey >= -4 && valuey <= -1 ) 332 | 333 | {stepper2.moveTo(-3 * cantidad); 334 | stepper2.run();} 335 | 336 | else if (valuey >= 1 && valuey <= 5 ) 337 | 338 | {stepper2.moveTo(3 * cantidad); 339 | stepper2.run();} 340 | 341 | else if (valuey == 0) 342 | 343 | {stepper2.moveTo(0 * cantidad); 344 | stepper2.run();} 345 | 346 | 347 | 348 | else if (valuey >= 6 && valuey <= 10 ) 349 | 350 | {stepper2.moveTo(8 * cantidad); 351 | stepper2.run();} 352 | 353 | else if (valuey >= 11 && valuey <= 15 ) 354 | 355 | {stepper2.moveTo(13 * cantidad); 356 | stepper2.run();} 357 | 358 | else if (valuey >= 16 && valuey <= 20 ) 359 | 360 | {stepper2.moveTo(18 * cantidad); 361 | stepper2.run();} 362 | 363 | else if (valuey >= 21 && valuey <= 25 ) 364 | 365 | {stepper2.moveTo(23 * cantidad); 366 | stepper2.run();} 367 | 368 | else if (valuey >= 26 && valuey <= 30 ) 369 | 370 | {stepper2.moveTo(28 * cantidad); 371 | stepper2.run();} 372 | 373 | else if (valuey >= 31 && valuey <= 35 ) 374 | 375 | {stepper2.moveTo(33 * cantidad); 376 | stepper2.run();} 377 | 378 | else if (valuey >= 36 && valuey <= 40 ) 379 | 380 | {stepper2.moveTo(38 * cantidad); 381 | stepper2.run();} 382 | 383 | else if (valuey >= 41 && valuey <= 45 ) 384 | 385 | {stepper2.moveTo(43 * cantidad); 386 | stepper2.run();} 387 | 388 | else if (valuey >= 46 && valuey <= 50 ) 389 | 390 | {stepper2.moveTo(48 * cantidad); 391 | stepper2.run();} 392 | 393 | else if (valuey >= 51 && valuey <= 55 ) 394 | 395 | {stepper2.moveTo(53 * cantidad); 396 | stepper2.run();} 397 | 398 | else if (valuey >= 56 && valuey <= 60 ) 399 | 400 | {stepper2.moveTo(58 * cantidad); 401 | stepper2.run();} 402 | 403 | else if (valuey >= 61 && valuey <= 65 ) 404 | 405 | {stepper2.moveTo(63 * cantidad); 406 | stepper2.run();} 407 | 408 | else if (valuey >= 66 && valuey <= 70 ) 409 | 410 | {stepper2.moveTo(68 * cantidad); 411 | stepper2.run();} 412 | 413 | else if (valuey >= 71 && valuey <= 85 ) 414 | 415 | {stepper2.moveTo(73 * cantidad); 416 | stepper2.run();} 417 | 418 | 419 | 420 | else if (valuey >= 86 && valuey <= 89 ) 421 | 422 | {stepper2.moveTo(88 * cantidad); 423 | stepper2.run();} 424 | 425 | ////////////////////////////// 426 | ///////////////////////////// 427 | ///////////////////////////// 428 | 429 | 430 | else {} 431 | 432 | 433 | if (valuex >= -89 && valuex <= -85 ) 434 | 435 | {stepper1.moveTo(-87 * cantidad); 436 | stepper1.run();} 437 | 438 | else if (valuex >= -84 && valuex <= -80 ) 439 | 440 | {stepper1.moveTo(-83 * cantidad); 441 | stepper1.run();} 442 | 443 | else if (valuex >= -79 && valuex <= -75 ) 444 | 445 | {stepper1.moveTo(-77 * cantidad); 446 | stepper1.run();} 447 | 448 | else if (valuex >= -74 && valuex <= -70 ) 449 | 450 | {stepper1.moveTo(-73 * cantidad); 451 | stepper1.run();} 452 | 453 | else if (valuex >= -69 && valuex <= -65 ) 454 | 455 | {stepper1.moveTo(-67 * cantidad); 456 | stepper1.run();} 457 | 458 | else if (valuex >= -64 && valuex <= -60 ) 459 | 460 | {stepper1.moveTo(-63 * cantidad); 461 | stepper1.run();} 462 | 463 | else if (valuex >= -59 && valuex <= -55 ) 464 | 465 | {stepper1.moveTo(-67 * cantidad); 466 | stepper1.run();} 467 | 468 | else if (valuex >= -54 && valuex <= -50 ) 469 | 470 | {stepper1.moveTo(-53 * cantidad); 471 | stepper1.run();} 472 | 473 | else if (valuex >= -49 && valuex <= -45 ) 474 | 475 | {stepper1.moveTo(-47 * cantidad); 476 | stepper1.run();} 477 | 478 | else if (valuex >= -44 && valuex <= -40 ) 479 | 480 | {stepper1.moveTo(-43 * cantidad); 481 | stepper1.run();} 482 | 483 | else if (valuex >= -39 && valuex <= -35 ) 484 | 485 | {stepper1.moveTo(-37 * cantidad); 486 | stepper1.run();} 487 | 488 | else if (valuex >= -34 && valuex <= -30 ) 489 | 490 | {stepper1.moveTo(-33 * cantidad); 491 | stepper1.run();} 492 | 493 | else if (valuex >= -29 && valuex <= -25 ) 494 | 495 | {stepper1.moveTo(-27 * cantidad); 496 | stepper1.run();} 497 | 498 | else if (valuex >= -24 && valuex <= -20 ) 499 | 500 | {stepper1.moveTo(-23 * cantidad); 501 | stepper1.run();} 502 | 503 | else if (valuex >= -19 && valuex <= -15 ) 504 | 505 | {stepper1.moveTo(-17 * cantidad); 506 | stepper1.run();} 507 | 508 | else if (valuex >= -14 && valuex <= -10 ) 509 | 510 | {stepper1.moveTo(-13 * cantidad); 511 | stepper1.run();} 512 | 513 | else if (valuex >= -9 && valuex <= -5 ) 514 | 515 | {stepper1.moveTo(-7 * cantidad); 516 | stepper1.run();} 517 | 518 | else if (valuex >= -4 && valuex <= -1 ) 519 | 520 | {stepper1.moveTo(-3 * cantidad); 521 | stepper1.run();} 522 | 523 | 524 | else if (valuex == 0 ) 525 | 526 | {stepper1.moveTo(0 * cantidad); 527 | stepper1.run();} 528 | 529 | 530 | else if (valuex >= 1 && valuex <= 5 ) 531 | 532 | {stepper1.moveTo(3 * cantidad); 533 | stepper1.run();} 534 | 535 | else if (valuex >= 6 && valuex <= 10 ) 536 | 537 | {stepper1.moveTo(8 * cantidad); 538 | stepper1.run();} 539 | 540 | else if (valuex >= 11 && valuex <= 15 ) 541 | 542 | {stepper1.moveTo(13 * cantidad); 543 | stepper1.run();} 544 | 545 | else if (valuex >= 16 && valuex <= 20 ) 546 | 547 | {stepper1.moveTo(18 * cantidad); 548 | stepper1.run();} 549 | 550 | else if (valuex >= 21 && valuex <= 25 ) 551 | 552 | {stepper1.moveTo(23 * cantidad); 553 | stepper1.run();} 554 | 555 | else if (valuex >= 26 && valuex <= 30 ) 556 | 557 | {stepper1.moveTo(28 * cantidad); 558 | stepper1.run();} 559 | 560 | else if (valuex >= 31 && valuex <= 35 ) 561 | 562 | {stepper1.moveTo(33 * cantidad); 563 | stepper1.run();} 564 | 565 | else if (valuex >= 36 && valuex <= 40 ) 566 | 567 | {stepper1.moveTo(38 * cantidad); 568 | stepper1.run();} 569 | 570 | else if (valuex >= 41 && valuex <= 45 ) 571 | 572 | {stepper1.moveTo(43 * cantidad); 573 | stepper1.run();} 574 | 575 | else if (valuex >= 46 && valuex <= 50 ) 576 | 577 | {stepper1.moveTo(48 * cantidad); 578 | stepper1.run();} 579 | 580 | else if (valuex >= 51 && valuex <= 55 ) 581 | 582 | {stepper1.moveTo(53 * cantidad); 583 | stepper1.run();} 584 | 585 | else if (valuex >= 56 && valuex <= 60 ) 586 | 587 | {stepper1.moveTo(58 * cantidad); 588 | stepper1.run();} 589 | 590 | else if (valuex >= 61 && valuex <= 65 ) 591 | 592 | {stepper1.moveTo(63 * cantidad); 593 | stepper1.run();} 594 | 595 | else if (valuex >= 66 && valuex <= 70 ) 596 | 597 | {stepper1.moveTo(68 * cantidad); 598 | stepper1.run();} 599 | 600 | else if (valuex >= 71 && valuex <= 85 ) 601 | 602 | {stepper1.moveTo(73 * cantidad); 603 | stepper1.run();} 604 | 605 | 606 | 607 | else if (valuex >= 86 && valuex <= 89 ) 608 | 609 | {stepper1.moveTo(88 * cantidad); 610 | stepper1.run();} 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | else {} 621 | 622 | //////_--- 623 | 624 | 625 | } 626 | 627 | 628 | else 629 | 630 | 631 | { 632 | 633 | if (digitalRead(LEFT_PIN) == 0) { 634 | 635 | stepper2.moveTo(pos); 636 | stepper2.run(); 637 | } 638 | 639 | else if (digitalRead(RIGHT_PIN) == 0) { 640 | 641 | stepper2.moveTo(-pos); 642 | stepper2.run(); 643 | } 644 | 645 | else if (digitalRead(STOP_PIN) == 0) { 646 | 647 | stepper2.moveTo(0); 648 | stepper2.run(); 649 | } 650 | 651 | 652 | 653 | } 654 | 655 | 656 | 657 | 658 | 659 | } 660 | -------------------------------------------------------------------------------- /imu/R3_MPU6050_2xstepper_v2.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "Kalman.h" 4 | 5 | 6 | #define RESTRICT_PITCH 7 | 8 | Kalman kalmanX; 9 | Kalman kalmanY; 10 | 11 | ///////////stepeer 12 | 13 | AccelStepper stepper1(1, 9, 8); 14 | 15 | AccelStepper stepper2(1, 7, 6); 16 | int pos = 200; 17 | //int cantidad = 7; 18 | int addr = 0; 19 | 20 | 21 | // Define our three input button pins 22 | #define LEFT_PIN 4 23 | #define CENTER_PIN 3 24 | #define RIGHT_PIN 5 25 | #define CONTROL_PIN 11 26 | 27 | #define LEFT_PIN2 2 28 | #define CENTER_PIN2 10 29 | #define RIGHT_PIN2 12 30 | 31 | 32 | 33 | #define SPEED_PIN 0 34 | 35 | int velocidad = 1000; 36 | int cantidad = 6; 37 | int aceleracion = 500; 38 | 39 | 40 | 41 | /* IMU Data */ 42 | double accX, accY, accZ; 43 | double gyroX, gyroY, gyroZ; 44 | int16_t tempRaw; 45 | 46 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 47 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 48 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 49 | 50 | uint32_t timer; 51 | uint8_t i2cData[14]; // Buffer for I2C data 52 | 53 | // TODO: Make calibration routine 54 | 55 | void setup() { 56 | pinMode(LEFT_PIN, INPUT_PULLUP); 57 | pinMode(CENTER_PIN, INPUT_PULLUP); 58 | pinMode(RIGHT_PIN, INPUT_PULLUP); 59 | pinMode(CONTROL_PIN, INPUT_PULLUP); 60 | 61 | pinMode(LEFT_PIN2, INPUT_PULLUP); 62 | pinMode(CENTER_PIN2, INPUT_PULLUP); 63 | pinMode(RIGHT_PIN2, INPUT_PULLUP); 64 | 65 | 66 | Serial.begin(115200); 67 | Wire.begin(); 68 | 69 | 70 | 71 | 72 | TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 73 | 74 | stepper1.setMaxSpeed(3000); 75 | stepper2.setMaxSpeed(3000); 76 | 77 | stepper1.setAcceleration(500); 78 | stepper2.setAcceleration(500); 79 | 80 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 81 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 82 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 83 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 84 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 85 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 86 | 87 | while (i2cRead(0x75, i2cData, 1)); 88 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 89 | Serial.print(F("Error reading sensor")); 90 | while (1); 91 | } 92 | 93 | delay(100); // Wait for sensor to stabilize 94 | 95 | /* Set kalman and gyro starting angle */ 96 | while (i2cRead(0x3B, i2cData, 6)); 97 | accX = (i2cData[0] << 8) | i2cData[1]; 98 | accY = (i2cData[2] << 8) | i2cData[3]; 99 | accZ = (i2cData[4] << 8) | i2cData[5]; 100 | 101 | 102 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 103 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 104 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 105 | #else // Eq. 28 and 29 106 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 107 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 108 | #endif 109 | 110 | kalmanX.setAngle(roll); // Set starting angle 111 | kalmanY.setAngle(pitch); 112 | gyroXangle = roll; 113 | gyroYangle = pitch; 114 | compAngleX = roll; 115 | compAngleY = pitch; 116 | 117 | 118 | } 119 | 120 | 121 | /////////////////7 122 | 123 | ///////////////7 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | void loop() { 132 | 133 | 134 | ////// 135 | 136 | ///// 137 | 138 | 139 | 140 | 141 | /* Update all the values */ 142 | while (i2cRead(0x3B, i2cData, 14)); 143 | accX = ((i2cData[0] << 8) | i2cData[1]); 144 | accY = ((i2cData[2] << 8) | i2cData[3]); 145 | accZ = ((i2cData[4] << 8) | i2cData[5]); 146 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 147 | gyroX = (i2cData[8] << 8) | i2cData[9]; 148 | gyroY = (i2cData[10] << 8) | i2cData[11]; 149 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 150 | 151 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 152 | timer = micros(); 153 | 154 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 155 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 156 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 157 | #else // Eq. 28 and 29 158 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 159 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 160 | #endif 161 | 162 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 163 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 164 | 165 | #ifdef RESTRICT_PITCH 166 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 167 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 168 | kalmanX.setAngle(roll); 169 | compAngleX = roll; 170 | kalAngleX = roll; 171 | gyroXangle = roll; 172 | } else 173 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 174 | 175 | if (abs(kalAngleX) > 90) 176 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 177 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 178 | #else 179 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 180 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 181 | kalmanY.setAngle(pitch); 182 | compAngleY = pitch; 183 | kalAngleY = pitch; 184 | gyroYangle = pitch; 185 | } else 186 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 187 | 188 | if (abs(kalAngleY) > 90) 189 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 190 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 191 | #endif 192 | 193 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 194 | gyroYangle += gyroYrate * dt; 195 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 196 | //gyroYangle += kalmanY.getRate() * dt; 197 | 198 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 199 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 200 | 201 | // Reset the gyro angle when it has drifted too much 202 | if (gyroXangle < -180 || gyroXangle > 180) 203 | gyroXangle = kalAngleX; 204 | if (gyroYangle < -180 || gyroYangle > 180) 205 | gyroYangle = kalAngleY; 206 | 207 | /* Print Data */ 208 | #if 0 // Set to 1 to activate 209 | Serial.print(accX); Serial.print("\t"); 210 | Serial.print(accY); Serial.print("\t"); 211 | Serial.print(accZ); Serial.print("\t"); 212 | 213 | Serial.print(gyroX); Serial.print("\t"); 214 | Serial.print(gyroY); Serial.print("\t"); 215 | Serial.print(gyroZ); Serial.print("\t"); 216 | 217 | Serial.print("\t"); 218 | #endif 219 | 220 | //Serial.print(roll); Serial.print("\t"); 221 | //Serial.print(gyroXangle); Serial.print("\t"); 222 | //Serial.print(compAngleX); Serial.print("\t"); 223 | Serial.print(kalAngleX); Serial.print("\t"); 224 | 225 | Serial.print("\t"); 226 | 227 | //Serial.print(pitch); Serial.print("\t"); 228 | //Serial.print(gyroYangle); Serial.print("\t"); 229 | //Serial.print(compAngleY); Serial.print("\t"); 230 | Serial.print(kalAngleY); Serial.print("\t"); 231 | 232 | #if 0 // Set to 1 to print the temperature 233 | Serial.print("\t"); 234 | 235 | double temperature = (double)tempRaw / 340.0 + 36.53; 236 | Serial.print(temperature); Serial.print("\t"); 237 | #endif 238 | 239 | Serial.print("\r\n"); 240 | delay(2); 241 | 242 | 243 | //////////////////7 244 | 245 | 246 | if (digitalRead(CONTROL_PIN) == LOW) 247 | 248 | 249 | { 250 | 251 | int valuey = kalAngleX; 252 | int valuex = kalAngleY; 253 | 254 | 255 | if (valuey >= -89 && valuey <= -85 ) 256 | 257 | {stepper2.moveTo(-87 * cantidad); 258 | stepper2.run();} 259 | 260 | else if (valuey >= -84 && valuey <= -80 ) 261 | 262 | {stepper2.moveTo(-83 * cantidad); 263 | stepper2.run();} 264 | 265 | else if (valuey >= -79 && valuey <= -75 ) 266 | 267 | {stepper2.moveTo(-77 * cantidad); 268 | stepper2.run();} 269 | 270 | else if (valuey >= -74 && valuey <= -70 ) 271 | 272 | {stepper2.moveTo(-73 * cantidad); 273 | stepper2.run();} 274 | 275 | else if (valuey >= -69 && valuey <= -65 ) 276 | 277 | {stepper2.moveTo(-67 * cantidad); 278 | stepper2.run();} 279 | 280 | else if (valuey >= -64 && valuey <= -60 ) 281 | 282 | {stepper2.moveTo(-63 * cantidad); 283 | stepper2.run();} 284 | 285 | else if (valuey >= -59 && valuey <= -55 ) 286 | 287 | {stepper2.moveTo(-67 * cantidad); 288 | stepper2.run();} 289 | 290 | else if (valuey >= -54 && valuey <= -50 ) 291 | 292 | {stepper2.moveTo(-53 * cantidad); 293 | stepper2.run();} 294 | 295 | else if (valuey >= -49 && valuey <= -45 ) 296 | 297 | {stepper2.moveTo(-47 * cantidad); 298 | stepper2.run();} 299 | 300 | else if (valuey >= -44 && valuey <= -40 ) 301 | 302 | {stepper2.moveTo(-43 * cantidad); 303 | stepper2.run();} 304 | 305 | else if (valuey >= -39 && valuey <= -35 ) 306 | 307 | {stepper2.moveTo(-37 * cantidad); 308 | stepper2.run();} 309 | 310 | else if (valuey >= -34 && valuey <= -30 ) 311 | 312 | {stepper2.moveTo(-33 * cantidad); 313 | stepper2.run();} 314 | 315 | else if (valuey >= -29 && valuey <= -25 ) 316 | 317 | {stepper2.moveTo(-27 * cantidad); 318 | stepper2.run();} 319 | 320 | else if (valuey >= -24 && valuey <= -20 ) 321 | 322 | {stepper2.moveTo(-23 * cantidad); 323 | stepper2.run();} 324 | 325 | else if (valuey >= -19 && valuey <= -15 ) 326 | 327 | {stepper2.moveTo(-17 * cantidad); 328 | stepper2.run();} 329 | 330 | else if (valuey >= -14 && valuey <= -10 ) 331 | 332 | {stepper2.moveTo(-13 * cantidad); 333 | stepper2.run();} 334 | 335 | else if (valuey >= -9 && valuey <= -5 ) 336 | 337 | {stepper2.moveTo(-7 * cantidad); 338 | stepper2.run();} 339 | 340 | else if (valuey >= -4 && valuey <= -1 ) 341 | 342 | {stepper2.moveTo(-3 * cantidad); 343 | stepper2.run();} 344 | 345 | else if (valuey >= 1 && valuey <= 5 ) 346 | 347 | {stepper2.moveTo(3 * cantidad); 348 | stepper2.run();} 349 | 350 | else if (valuey == 0) 351 | 352 | {stepper2.moveTo(0 * cantidad); 353 | stepper2.run();} 354 | 355 | 356 | 357 | else if (valuey >= 6 && valuey <= 10 ) 358 | 359 | {stepper2.moveTo(8 * cantidad); 360 | stepper2.run();} 361 | 362 | else if (valuey >= 11 && valuey <= 15 ) 363 | 364 | {stepper2.moveTo(13 * cantidad); 365 | stepper2.run();} 366 | 367 | else if (valuey >= 16 && valuey <= 20 ) 368 | 369 | {stepper2.moveTo(18 * cantidad); 370 | stepper2.run();} 371 | 372 | else if (valuey >= 21 && valuey <= 25 ) 373 | 374 | {stepper2.moveTo(23 * cantidad); 375 | stepper2.run();} 376 | 377 | else if (valuey >= 26 && valuey <= 30 ) 378 | 379 | {stepper2.moveTo(28 * cantidad); 380 | stepper2.run();} 381 | 382 | else if (valuey >= 31 && valuey <= 35 ) 383 | 384 | {stepper2.moveTo(33 * cantidad); 385 | stepper2.run();} 386 | 387 | else if (valuey >= 36 && valuey <= 40 ) 388 | 389 | {stepper2.moveTo(38 * cantidad); 390 | stepper2.run();} 391 | 392 | else if (valuey >= 41 && valuey <= 45 ) 393 | 394 | {stepper2.moveTo(43 * cantidad); 395 | stepper2.run();} 396 | 397 | else if (valuey >= 46 && valuey <= 50 ) 398 | 399 | {stepper2.moveTo(48 * cantidad); 400 | stepper2.run();} 401 | 402 | else if (valuey >= 51 && valuey <= 55 ) 403 | 404 | {stepper2.moveTo(53 * cantidad); 405 | stepper2.run();} 406 | 407 | else if (valuey >= 56 && valuey <= 60 ) 408 | 409 | {stepper2.moveTo(58 * cantidad); 410 | stepper2.run();} 411 | 412 | else if (valuey >= 61 && valuey <= 65 ) 413 | 414 | {stepper2.moveTo(63 * cantidad); 415 | stepper2.run();} 416 | 417 | else if (valuey >= 66 && valuey <= 70 ) 418 | 419 | {stepper2.moveTo(68 * cantidad); 420 | stepper2.run();} 421 | 422 | else if (valuey >= 71 && valuey <= 85 ) 423 | 424 | {stepper2.moveTo(73 * cantidad); 425 | stepper2.run();} 426 | 427 | 428 | 429 | else if (valuey >= 86 && valuey <= 89 ) 430 | 431 | {stepper2.moveTo(88 * cantidad); 432 | stepper2.run();} 433 | 434 | ////////////////////////////// 435 | ///////////////////////////// 436 | ///////////////////////////// 437 | 438 | 439 | else {} 440 | 441 | 442 | if (valuex >= -89 && valuex <= -85 ) 443 | 444 | {stepper1.moveTo(-87 * cantidad); 445 | stepper1.run();} 446 | 447 | else if (valuex >= -84 && valuex <= -80 ) 448 | 449 | {stepper1.moveTo(-83 * cantidad); 450 | stepper1.run();} 451 | 452 | else if (valuex >= -79 && valuex <= -75 ) 453 | 454 | {stepper1.moveTo(-77 * cantidad); 455 | stepper1.run();} 456 | 457 | else if (valuex >= -74 && valuex <= -70 ) 458 | 459 | {stepper1.moveTo(-73 * cantidad); 460 | stepper1.run();} 461 | 462 | else if (valuex >= -69 && valuex <= -65 ) 463 | 464 | {stepper1.moveTo(-67 * cantidad); 465 | stepper1.run();} 466 | 467 | else if (valuex >= -64 && valuex <= -60 ) 468 | 469 | {stepper1.moveTo(-63 * cantidad); 470 | stepper1.run();} 471 | 472 | else if (valuex >= -59 && valuex <= -55 ) 473 | 474 | {stepper1.moveTo(-67 * cantidad); 475 | stepper1.run();} 476 | 477 | else if (valuex >= -54 && valuex <= -50 ) 478 | 479 | {stepper1.moveTo(-53 * cantidad); 480 | stepper1.run();} 481 | 482 | else if (valuex >= -49 && valuex <= -45 ) 483 | 484 | {stepper1.moveTo(-47 * cantidad); 485 | stepper1.run();} 486 | 487 | else if (valuex >= -44 && valuex <= -40 ) 488 | 489 | {stepper1.moveTo(-43 * cantidad); 490 | stepper1.run();} 491 | 492 | else if (valuex >= -39 && valuex <= -35 ) 493 | 494 | {stepper1.moveTo(-37 * cantidad); 495 | stepper1.run();} 496 | 497 | else if (valuex >= -34 && valuex <= -30 ) 498 | 499 | {stepper1.moveTo(-33 * cantidad); 500 | stepper1.run();} 501 | 502 | else if (valuex >= -29 && valuex <= -25 ) 503 | 504 | {stepper1.moveTo(-27 * cantidad); 505 | stepper1.run();} 506 | 507 | else if (valuex >= -24 && valuex <= -20 ) 508 | 509 | {stepper1.moveTo(-23 * cantidad); 510 | stepper1.run();} 511 | 512 | else if (valuex >= -19 && valuex <= -15 ) 513 | 514 | {stepper1.moveTo(-17 * cantidad); 515 | stepper1.run();} 516 | 517 | else if (valuex >= -14 && valuex <= -10 ) 518 | 519 | {stepper1.moveTo(-13 * cantidad); 520 | stepper1.run();} 521 | 522 | else if (valuex >= -9 && valuex <= -5 ) 523 | 524 | {stepper1.moveTo(-7 * cantidad); 525 | stepper1.run();} 526 | 527 | else if (valuex >= -4 && valuex <= -1 ) 528 | 529 | {stepper1.moveTo(-3 * cantidad); 530 | stepper1.run();} 531 | 532 | 533 | else if (valuex == 0 ) 534 | 535 | {stepper1.moveTo(0 * cantidad); 536 | stepper1.run();} 537 | 538 | 539 | else if (valuex >= 1 && valuex <= 5 ) 540 | 541 | {stepper1.moveTo(3 * cantidad); 542 | stepper1.run();} 543 | 544 | else if (valuex >= 6 && valuex <= 10 ) 545 | 546 | {stepper1.moveTo(8 * cantidad); 547 | stepper1.run();} 548 | 549 | else if (valuex >= 11 && valuex <= 15 ) 550 | 551 | {stepper1.moveTo(13 * cantidad); 552 | stepper1.run();} 553 | 554 | else if (valuex >= 16 && valuex <= 20 ) 555 | 556 | {stepper1.moveTo(18 * cantidad); 557 | stepper1.run();} 558 | 559 | else if (valuex >= 21 && valuex <= 25 ) 560 | 561 | {stepper1.moveTo(23 * cantidad); 562 | stepper1.run();} 563 | 564 | else if (valuex >= 26 && valuex <= 30 ) 565 | 566 | {stepper1.moveTo(28 * cantidad); 567 | stepper1.run();} 568 | 569 | else if (valuex >= 31 && valuex <= 35 ) 570 | 571 | {stepper1.moveTo(33 * cantidad); 572 | stepper1.run();} 573 | 574 | else if (valuex >= 36 && valuex <= 40 ) 575 | 576 | {stepper1.moveTo(38 * cantidad); 577 | stepper1.run();} 578 | 579 | else if (valuex >= 41 && valuex <= 45 ) 580 | 581 | {stepper1.moveTo(43 * cantidad); 582 | stepper1.run();} 583 | 584 | else if (valuex >= 46 && valuex <= 50 ) 585 | 586 | {stepper1.moveTo(48 * cantidad); 587 | stepper1.run();} 588 | 589 | else if (valuex >= 51 && valuex <= 55 ) 590 | 591 | {stepper1.moveTo(53 * cantidad); 592 | stepper1.run();} 593 | 594 | else if (valuex >= 56 && valuex <= 60 ) 595 | 596 | {stepper1.moveTo(58 * cantidad); 597 | stepper1.run();} 598 | 599 | else if (valuex >= 61 && valuex <= 65 ) 600 | 601 | {stepper1.moveTo(63 * cantidad); 602 | stepper1.run();} 603 | 604 | else if (valuex >= 66 && valuex <= 70 ) 605 | 606 | {stepper1.moveTo(68 * cantidad); 607 | stepper1.run();} 608 | 609 | else if (valuex >= 71 && valuex <= 85 ) 610 | 611 | {stepper1.moveTo(73 * cantidad); 612 | stepper1.run();} 613 | 614 | 615 | 616 | else if (valuex >= 86 && valuex <= 89 ) 617 | 618 | {stepper1.moveTo(88 * cantidad); 619 | stepper1.run();} 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | else {} 630 | 631 | //////_--- 632 | 633 | 634 | } 635 | 636 | 637 | else 638 | 639 | 640 | { 641 | 642 | if (digitalRead(LEFT_PIN) == 0) { 643 | 644 | stepper2.moveTo(pos); 645 | stepper2.run(); 646 | } 647 | 648 | else if (digitalRead(RIGHT_PIN) == 0) { 649 | 650 | stepper2.moveTo(-pos); 651 | stepper2.run(); 652 | } 653 | 654 | else if (digitalRead(CENTER_PIN) == 0) { 655 | 656 | stepper2.moveTo(0); 657 | stepper2.run(); 658 | } 659 | 660 | 661 | else if (digitalRead(LEFT_PIN2) == 0) { 662 | 663 | stepper1.moveTo(pos); 664 | stepper1.run(); 665 | } 666 | 667 | else if (digitalRead(RIGHT_PIN2) == 0) { 668 | 669 | stepper1.moveTo(-pos); 670 | stepper1.run(); 671 | } 672 | 673 | else if (digitalRead(CENTER_PIN2) == 0) { 674 | 675 | stepper1.moveTo(0); 676 | stepper1.run(); 677 | } 678 | 679 | 680 | 681 | } 682 | 683 | 684 | 685 | 686 | 687 | } 688 | -------------------------------------------------------------------------------- /imu/linkit_web_imu_stepper.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | const char* host = "192.168.100.1"; 7 | /////kalman--------------------------------------------- 8 | #include 9 | #include "Kalman.h" 10 | #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - 11 | Kalman kalmanX; // Create the Kalman instances 12 | Kalman kalmanY; 13 | 14 | /* IMU Data */ 15 | double accX, accY, accZ; 16 | double gyroX, gyroY, gyroZ; 17 | int16_t tempRaw; 18 | 19 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 20 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 21 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 22 | 23 | uint32_t timer; 24 | uint8_t i2cData[14]; // Buffer for I2C data 25 | //////Kalman------------------------------------------------- 26 | 27 | 28 | AccelStepper stepper1(1, 9, 8); 29 | 30 | AccelStepper stepper2(1, 7, 6); 31 | int pos = 2000; 32 | int cantidad = 7; 33 | int addr = 0; 34 | 35 | 36 | // Define our three input button pins 37 | #define LEFT_PIN 4 38 | #define STOP_PIN 3 39 | #define RIGHT_PIN 5 40 | #define CONTROL_PIN 11 41 | 42 | #define SPEED_PIN 0 43 | 44 | // Define our maximum and minimum speed in steps per second (scale pot to these) 45 | #define MAX_SPEED 1500 46 | #define MIN_SPEED 0.1 47 | 48 | 49 | 50 | int acl = 1000; 51 | 52 | void setup() { 53 | 54 | 55 | HttpClient client; 56 | 57 | stepper1.setMaxSpeed(10000.0); 58 | stepper2.setMaxSpeed(10000.0); 59 | 60 | if (client) { 61 | 62 | //Bridge.begin(); 63 | 64 | // Create and make a HTTP request: 65 | String cmd = "http://"; 66 | cmd += host; 67 | cmd += "/vent.txt"; 68 | client.get(cmd); 69 | // Read incoming bytes from the server 70 | while (client.available()) { 71 | int c = client.read(); 72 | if (c == 500) {cantidad=3;} 73 | else if (c == 1500) { cantidad=7;} 74 | else if (c == 2000) { cantidad=15;} 75 | 76 | 77 | } 78 | delay(60000); 79 | } 80 | 81 | //Bridge.begin(); 82 | 83 | // The only AccelStepper value we have to set here is the max speeed, which is higher than we'll ever go 84 | 85 | 86 | stepper1.setAcceleration(acl); 87 | stepper2.setAcceleration(acl); 88 | 89 | // Set up the three button inputs, with pullups 90 | pinMode(LEFT_PIN, INPUT_PULLUP); 91 | pinMode(STOP_PIN, INPUT_PULLUP); 92 | pinMode(RIGHT_PIN, INPUT_PULLUP); 93 | pinMode(CONTROL_PIN, INPUT_PULLUP); 94 | 95 | ////////kalmen--------------------------------------------------------------------- 96 | /////////////////////////////////////////////// Serial.begin(115200); 97 | Wire.begin(); 98 | TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 99 | //TWBR = ((F_CPU / 100000L) - 16) / 2; // Set I2C frequency to 100kHz 100 | 101 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 102 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 103 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 104 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 105 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 106 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 107 | 108 | while (i2cRead(0x75, i2cData, 1)); 109 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 110 | Serial.print(F("Error reading sensor")); 111 | while (1); 112 | } 113 | 114 | delay(100); // Wait for sensor to stabilize 115 | 116 | /* Set kalman and gyro starting angle */ 117 | while (i2cRead(0x3B, i2cData, 6)); 118 | accX = (i2cData[0] << 8) | i2cData[1]; 119 | accY = (i2cData[2] << 8) | i2cData[3]; 120 | accZ = (i2cData[4] << 8) | i2cData[5]; 121 | 122 | 123 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 124 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 125 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 126 | #else // Eq. 28 and 29 127 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 128 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 129 | #endif 130 | 131 | kalmanX.setAngle(roll); // Set starting angle 132 | kalmanY.setAngle(pitch); 133 | gyroXangle = roll; 134 | gyroYangle = pitch; 135 | compAngleX = roll; 136 | compAngleY = pitch; 137 | 138 | timer = micros(); 139 | //////////////////kalman----------------------------------------- 140 | } 141 | 142 | 143 | 144 | 145 | 146 | void loop() { 147 | 148 | 149 | ////////////kalman-------------------------------------------------- 150 | 151 | /* Update all the values */ 152 | while (i2cRead(0x3B, i2cData, 14)); 153 | accX = ((i2cData[0] << 8) | i2cData[1]); 154 | accY = ((i2cData[2] << 8) | i2cData[3]); 155 | accZ = ((i2cData[4] << 8) | i2cData[5]); 156 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 157 | gyroX = (i2cData[8] << 8) | i2cData[9]; 158 | gyroY = (i2cData[10] << 8) | i2cData[11]; 159 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 160 | 161 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 162 | timer = micros(); 163 | 164 | 165 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 166 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 167 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 168 | #else // Eq. 28 and 29 169 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 170 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 171 | #endif 172 | 173 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 174 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 175 | 176 | #ifdef RESTRICT_PITCH 177 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 178 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 179 | kalmanX.setAngle(roll); 180 | compAngleX = roll; 181 | kalAngleX = roll; 182 | gyroXangle = roll; 183 | } else 184 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 185 | 186 | if (abs(kalAngleX) > 90) 187 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 188 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 189 | #else 190 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 191 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 192 | kalmanY.setAngle(pitch); 193 | compAngleY = pitch; 194 | kalAngleY = pitch; 195 | gyroYangle = pitch; 196 | } else 197 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 198 | 199 | if (abs(kalAngleY) > 90) 200 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 201 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 202 | #endif 203 | 204 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 205 | gyroYangle += gyroYrate * dt; 206 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 207 | //gyroYangle += kalmanY.getRate() * dt; 208 | 209 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 210 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 211 | 212 | // Reset the gyro angle when it has drifted too much 213 | if (gyroXangle < -180 || gyroXangle > 180) 214 | gyroXangle = kalAngleX; 215 | if (gyroYangle < -180 || gyroYangle > 180) 216 | gyroYangle = kalAngleY; 217 | 218 | /* Print Data */ 219 | #if 0// Set to 1 to activate 220 | //Serial.print(accX); Serial.print("\t"); 221 | //Serial.print(accY); Serial.print("\t"); 222 | //Serial.print(accZ); Serial.print("\t"); 223 | 224 | //Serial.print(gyroX); Serial.print("\t"); 225 | //Serial.print(gyroY); Serial.print("\t"); 226 | //Serial.print(gyroZ); Serial.print("\t"); 227 | 228 | Serial.print("\t"); 229 | #endif 230 | 231 | //Serial.print(roll); Serial.print("\t"); 232 | //Serial.print(gyroXangle); Serial.print("\t"); 233 | //Serial.print(compAngleX); Serial.print("\t"); 234 | Serial.print(kalAngleX); Serial.print("\t"); 235 | 236 | Serial.print("\t"); 237 | 238 | //Serial.print(pitch); Serial.print("\t"); 239 | //Serial.print(gyroYangle); Serial.print("\t"); 240 | //Serial.print(compAngleY); Serial.print("\t"); 241 | Serial.print(kalAngleY); Serial.print("\t"); 242 | 243 | #if 0 // Set to 1 to print the temperature 244 | Serial.print("\t"); 245 | 246 | double temperature = (double)tempRaw / 340.0 + 36.53; 247 | Serial.print(temperature); Serial.print("\t"); 248 | #endif 249 | 250 | Serial.print("\r\n"); 251 | 252 | 253 | ///////////////////-kalman----------------------------------------- 254 | 255 | 256 | //if (digitalRead(CONTROL_PIN) == LOW) 257 | 258 | 259 | //{ 260 | 261 | int valuey = kalAngleX; 262 | int valuex = kalAngleY; 263 | 264 | 265 | if (valuey >= -89 && valuey <= -85 ) 266 | 267 | {stepper2.moveTo(-87 * cantidad); 268 | stepper2.run();} 269 | 270 | else if (valuey >= -84 && valuey <= -80 ) 271 | 272 | {stepper2.moveTo(-83 * cantidad); 273 | stepper2.run();} 274 | 275 | else if (valuey >= -79 && valuey <= -75 ) 276 | 277 | {stepper2.moveTo(-77 * cantidad); 278 | stepper2.run();} 279 | 280 | else if (valuey >= -74 && valuey <= -70 ) 281 | 282 | {stepper2.moveTo(-73 * cantidad); 283 | stepper2.run();} 284 | 285 | else if (valuey >= -69 && valuey <= -65 ) 286 | 287 | {stepper2.moveTo(-67 * cantidad); 288 | stepper2.run();} 289 | 290 | else if (valuey >= -64 && valuey <= -60 ) 291 | 292 | {stepper2.moveTo(-63 * cantidad); 293 | stepper2.run();} 294 | 295 | else if (valuey >= -59 && valuey <= -55 ) 296 | 297 | {stepper2.moveTo(-67 * cantidad); 298 | stepper2.run();} 299 | 300 | else if (valuey >= -54 && valuey <= -50 ) 301 | 302 | {stepper2.moveTo(-53 * cantidad); 303 | stepper2.run();} 304 | 305 | else if (valuey >= -49 && valuey <= -45 ) 306 | 307 | {stepper2.moveTo(-47 * cantidad); 308 | stepper2.run();} 309 | 310 | else if (valuey >= -44 && valuey <= -40 ) 311 | 312 | {stepper2.moveTo(-43 * cantidad); 313 | stepper2.run();} 314 | 315 | else if (valuey >= -39 && valuey <= -35 ) 316 | 317 | {stepper2.moveTo(-37 * cantidad); 318 | stepper2.run();} 319 | 320 | else if (valuey >= -34 && valuey <= -30 ) 321 | 322 | {stepper2.moveTo(-33 * cantidad); 323 | stepper2.run();} 324 | 325 | else if (valuey >= -29 && valuey <= -25 ) 326 | 327 | {stepper2.moveTo(-27 * cantidad); 328 | stepper2.run();} 329 | 330 | else if (valuey >= -24 && valuey <= -20 ) 331 | 332 | {stepper2.moveTo(-23 * cantidad); 333 | stepper2.run();} 334 | 335 | else if (valuey >= -19 && valuey <= -15 ) 336 | 337 | {stepper2.moveTo(-17 * cantidad); 338 | stepper2.run();} 339 | 340 | else if (valuey >= -14 && valuey <= -10 ) 341 | 342 | {stepper2.moveTo(-13 * cantidad); 343 | stepper2.run();} 344 | 345 | else if (valuey >= -9 && valuey <= -5 ) 346 | 347 | {stepper2.moveTo(-7 * cantidad); 348 | stepper2.run();} 349 | 350 | else if (valuey >= -4 && valuey <= -1 ) 351 | 352 | {stepper2.moveTo(-3 * cantidad); 353 | stepper2.run();} 354 | 355 | else if (valuey >= 1 && valuey <= 5 ) 356 | 357 | {stepper2.moveTo(3 * cantidad); 358 | stepper2.run();} 359 | 360 | else if (valuey >= 6 && valuey <= 10 ) 361 | 362 | {stepper2.moveTo(8 * cantidad); 363 | stepper2.run();} 364 | 365 | else if (valuey >= 11 && valuey <= 15 ) 366 | 367 | {stepper2.moveTo(13 * cantidad); 368 | stepper2.run();} 369 | 370 | else if (valuey >= 16 && valuey <= 20 ) 371 | 372 | {stepper2.moveTo(18 * cantidad); 373 | stepper2.run();} 374 | 375 | else if (valuey >= 21 && valuey <= 25 ) 376 | 377 | {stepper2.moveTo(23 * cantidad); 378 | stepper2.run();} 379 | 380 | else if (valuey >= 26 && valuey <= 30 ) 381 | 382 | {stepper2.moveTo(28 * cantidad); 383 | stepper2.run();} 384 | 385 | else if (valuey >= 31 && valuey <= 35 ) 386 | 387 | {stepper2.moveTo(33 * cantidad); 388 | stepper2.run();} 389 | 390 | else if (valuey >= 36 && valuey <= 40 ) 391 | 392 | {stepper2.moveTo(38 * cantidad); 393 | stepper2.run();} 394 | 395 | else if (valuey >= 41 && valuey <= 45 ) 396 | 397 | {stepper2.moveTo(43 * cantidad); 398 | stepper2.run();} 399 | 400 | else if (valuey >= 46 && valuey <= 50 ) 401 | 402 | {stepper2.moveTo(48 * cantidad); 403 | stepper2.run();} 404 | 405 | else if (valuey >= 51 && valuey <= 55 ) 406 | 407 | {stepper2.moveTo(53 * cantidad); 408 | stepper2.run();} 409 | 410 | else if (valuey >= 56 && valuey <= 60 ) 411 | 412 | {stepper2.moveTo(58 * cantidad); 413 | stepper2.run();} 414 | 415 | else if (valuey >= 61 && valuey <= 65 ) 416 | 417 | {stepper2.moveTo(63 * cantidad); 418 | stepper2.run();} 419 | 420 | else if (valuey >= 66 && valuey <= 70 ) 421 | 422 | {stepper2.moveTo(68 * cantidad); 423 | stepper2.run();} 424 | 425 | else if (valuey >= 71 && valuey <= 85 ) 426 | 427 | {stepper2.moveTo(73 * cantidad); 428 | stepper2.run();} 429 | 430 | 431 | 432 | else if (valuey >= 86 && valuey <= 89 ) 433 | 434 | {stepper2.moveTo(88 * cantidad); 435 | stepper2.run();} 436 | 437 | ////////////////////////////// 438 | ///////////////////////////// 439 | ///////////////////////////// 440 | 441 | 442 | else {} 443 | 444 | 445 | if (valuex >= -89 && valuex <= -85 ) 446 | 447 | {stepper1.moveTo(-87 * cantidad); 448 | stepper1.run();} 449 | 450 | else if (valuex >= -84 && valuex <= -80 ) 451 | 452 | {stepper1.moveTo(-83 * cantidad); 453 | stepper1.run();} 454 | 455 | else if (valuex >= -79 && valuex <= -75 ) 456 | 457 | {stepper1.moveTo(-77 * cantidad); 458 | stepper1.run();} 459 | 460 | else if (valuex >= -74 && valuex <= -70 ) 461 | 462 | {stepper1.moveTo(-73 * cantidad); 463 | stepper1.run();} 464 | 465 | else if (valuex >= -69 && valuex <= -65 ) 466 | 467 | {stepper1.moveTo(-67 * cantidad); 468 | stepper1.run();} 469 | 470 | else if (valuex >= -64 && valuex <= -60 ) 471 | 472 | {stepper1.moveTo(-63 * cantidad); 473 | stepper1.run();} 474 | 475 | else if (valuex >= -59 && valuex <= -55 ) 476 | 477 | {stepper1.moveTo(-67 * cantidad); 478 | stepper1.run();} 479 | 480 | else if (valuex >= -54 && valuex <= -50 ) 481 | 482 | {stepper1.moveTo(-53 * cantidad); 483 | stepper1.run();} 484 | 485 | else if (valuex >= -49 && valuex <= -45 ) 486 | 487 | {stepper1.moveTo(-47 * cantidad); 488 | stepper1.run();} 489 | 490 | else if (valuex >= -44 && valuex <= -40 ) 491 | 492 | {stepper1.moveTo(-43 * cantidad); 493 | stepper1.run();} 494 | 495 | else if (valuex >= -39 && valuex <= -35 ) 496 | 497 | {stepper1.moveTo(-37 * cantidad); 498 | stepper1.run();} 499 | 500 | else if (valuex >= -34 && valuex <= -30 ) 501 | 502 | {stepper1.moveTo(-33 * cantidad); 503 | stepper1.run();} 504 | 505 | else if (valuex >= -29 && valuex <= -25 ) 506 | 507 | {stepper1.moveTo(-27 * cantidad); 508 | stepper1.run();} 509 | 510 | else if (valuex >= -24 && valuex <= -20 ) 511 | 512 | {stepper1.moveTo(-23 * cantidad); 513 | stepper1.run();} 514 | 515 | else if (valuex >= -19 && valuex <= -15 ) 516 | 517 | {stepper1.moveTo(-17 * cantidad); 518 | stepper1.run();} 519 | 520 | else if (valuex >= -14 && valuex <= -10 ) 521 | 522 | {stepper1.moveTo(-13 * cantidad); 523 | stepper1.run();} 524 | 525 | else if (valuex >= -9 && valuex <= -5 ) 526 | 527 | {stepper1.moveTo(-7 * cantidad); 528 | stepper1.run();} 529 | 530 | else if (valuex >= -4 && valuex <= -1 ) 531 | 532 | {stepper1.moveTo(-3 * cantidad); 533 | stepper1.run();} 534 | 535 | else if (valuex >= 1 && valuex <= 5 ) 536 | 537 | {stepper1.moveTo(3 * cantidad); 538 | stepper1.run();} 539 | 540 | else if (valuex >= 6 && valuex <= 10 ) 541 | 542 | {stepper1.moveTo(8 * cantidad); 543 | stepper1.run();} 544 | 545 | else if (valuex >= 11 && valuex <= 15 ) 546 | 547 | {stepper1.moveTo(13 * cantidad); 548 | stepper1.run();} 549 | 550 | else if (valuex >= 16 && valuex <= 20 ) 551 | 552 | {stepper1.moveTo(18 * cantidad); 553 | stepper1.run();} 554 | 555 | else if (valuex >= 21 && valuex <= 25 ) 556 | 557 | {stepper1.moveTo(23 * cantidad); 558 | stepper1.run();} 559 | 560 | else if (valuex >= 26 && valuex <= 30 ) 561 | 562 | {stepper1.moveTo(28 * cantidad); 563 | stepper1.run();} 564 | 565 | else if (valuex >= 31 && valuex <= 35 ) 566 | 567 | {stepper1.moveTo(33 * cantidad); 568 | stepper1.run();} 569 | 570 | else if (valuex >= 36 && valuex <= 40 ) 571 | 572 | {stepper1.moveTo(38 * cantidad); 573 | stepper1.run();} 574 | 575 | else if (valuex >= 41 && valuex <= 45 ) 576 | 577 | {stepper1.moveTo(43 * cantidad); 578 | stepper1.run();} 579 | 580 | else if (valuex >= 46 && valuex <= 50 ) 581 | 582 | {stepper1.moveTo(48 * cantidad); 583 | stepper1.run();} 584 | 585 | else if (valuex >= 51 && valuex <= 55 ) 586 | 587 | {stepper1.moveTo(53 * cantidad); 588 | stepper1.run();} 589 | 590 | else if (valuex >= 56 && valuex <= 60 ) 591 | 592 | {stepper1.moveTo(58 * cantidad); 593 | stepper1.run();} 594 | 595 | else if (valuex >= 61 && valuex <= 65 ) 596 | 597 | {stepper1.moveTo(63 * cantidad); 598 | stepper1.run();} 599 | 600 | else if (valuex >= 66 && valuex <= 70 ) 601 | 602 | {stepper1.moveTo(68 * cantidad); 603 | stepper1.run();} 604 | 605 | else if (valuex >= 71 && valuex <= 85 ) 606 | 607 | {stepper1.moveTo(73 * cantidad); 608 | stepper1.run();} 609 | 610 | 611 | 612 | else if (valuex >= 86 && valuex <= 89 ) 613 | 614 | {stepper1.moveTo(88 * cantidad); 615 | stepper1.run();} 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | else {} 626 | 627 | //////_--- 628 | 629 | 630 | //} 631 | 632 | 633 | /*else 634 | 635 | 636 | { 637 | 638 | if (digitalRead(LEFT_PIN) == 0) { 639 | 640 | stepper2.moveTo(pos); 641 | stepper2.run(); 642 | } 643 | 644 | else if (digitalRead(RIGHT_PIN) == 0) { 645 | 646 | stepper2.moveTo(-pos); 647 | stepper2.run(); 648 | } 649 | 650 | else if (digitalRead(STOP_PIN) == 0) { 651 | 652 | stepper2.moveTo(0); 653 | stepper2.run(); 654 | } 655 | 656 | 657 | 658 | /////////////web server 659 | 660 | /////////// 661 | 662 | }*/ 663 | 664 | } 665 | -------------------------------------------------------------------------------- /imu/mpu6050_stepper_v5.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | /////kalman--------------------------------------------- 5 | #include 6 | #include "Kalman.h" 7 | #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - 8 | Kalman kalmanX; // Create the Kalman instances 9 | Kalman kalmanY; 10 | 11 | /* IMU Data */ 12 | double accX, accY, accZ; 13 | double gyroX, gyroY, gyroZ; 14 | int16_t tempRaw; 15 | 16 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 17 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 18 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 19 | 20 | uint32_t timer; 21 | uint8_t i2cData[14]; // Buffer for I2C data 22 | //////Kalman------------------------------------------------- 23 | 24 | 25 | AccelStepper stepper1(1, 9, 8); 26 | 27 | AccelStepper stepper2(1, 7, 6); 28 | int pos = 2000; 29 | int cantidad = 60; 30 | 31 | // Define our three input button pins 32 | #define LEFT_PIN 4 33 | #define STOP_PIN 3 34 | #define RIGHT_PIN 2 35 | 36 | #define SPEED_PIN 0 37 | 38 | // Define our maximum and minimum speed in steps per second (scale pot to these) 39 | #define MAX_SPEED 1500 40 | #define MIN_SPEED 0.1 41 | 42 | 43 | void setup() { 44 | 45 | // The only AccelStepper value we have to set here is the max speeed, which is higher than we'll ever go 46 | stepper1.setMaxSpeed(10000.0); 47 | stepper2.setMaxSpeed(10000.0); 48 | 49 | stepper1.setAcceleration(1000); 50 | stepper2.setAcceleration(1000); 51 | 52 | // Set up the three button inputs, with pullups 53 | pinMode(LEFT_PIN, INPUT_PULLUP); 54 | pinMode(STOP_PIN, INPUT_PULLUP); 55 | pinMode(RIGHT_PIN, INPUT_PULLUP); 56 | 57 | ////////kalmen--------------------------------------------------------------------- 58 | Serial.begin(115200); 59 | Wire.begin(); 60 | TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz 61 | 62 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 63 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 64 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 65 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 66 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 67 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 68 | 69 | while (i2cRead(0x75, i2cData, 1)); 70 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 71 | Serial.print(F("Error reading sensor")); 72 | while (1); 73 | } 74 | 75 | delay(100); // Wait for sensor to stabilize 76 | 77 | /* Set kalman and gyro starting angle */ 78 | while (i2cRead(0x3B, i2cData, 6)); 79 | accX = (i2cData[0] << 8) | i2cData[1]; 80 | accY = (i2cData[2] << 8) | i2cData[3]; 81 | accZ = (i2cData[4] << 8) | i2cData[5]; 82 | 83 | 84 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 85 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 86 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 87 | #else // Eq. 28 and 29 88 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 89 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 90 | #endif 91 | 92 | kalmanX.setAngle(roll); // Set starting angle 93 | kalmanY.setAngle(pitch); 94 | gyroXangle = roll; 95 | gyroYangle = pitch; 96 | compAngleX = roll; 97 | compAngleY = pitch; 98 | 99 | timer = micros(); 100 | //////////////////kalman----------------------------------------- 101 | } 102 | 103 | void loop() { 104 | 105 | 106 | static char sign = 0; 107 | 108 | 109 | ////////////kalman-------------------------------------------------- 110 | 111 | /* Update all the values */ 112 | while (i2cRead(0x3B, i2cData, 14)); 113 | accX = ((i2cData[0] << 8) | i2cData[1]); 114 | accY = ((i2cData[2] << 8) | i2cData[3]); 115 | accZ = ((i2cData[4] << 8) | i2cData[5]); 116 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 117 | gyroX = (i2cData[8] << 8) | i2cData[9]; 118 | gyroY = (i2cData[10] << 8) | i2cData[11]; 119 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 120 | 121 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 122 | timer = micros(); 123 | 124 | // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 125 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 126 | // It is then converted from radians to degrees 127 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 128 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 129 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 130 | #else // Eq. 28 and 29 131 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 132 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 133 | #endif 134 | 135 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 136 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 137 | 138 | #ifdef RESTRICT_PITCH 139 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 140 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 141 | kalmanX.setAngle(roll); 142 | compAngleX = roll; 143 | kalAngleX = roll; 144 | gyroXangle = roll; 145 | } else 146 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 147 | 148 | if (abs(kalAngleX) > 90) 149 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 150 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 151 | #else 152 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 153 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 154 | kalmanY.setAngle(pitch); 155 | compAngleY = pitch; 156 | kalAngleY = pitch; 157 | gyroYangle = pitch; 158 | } else 159 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 160 | 161 | if (abs(kalAngleY) > 90) 162 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 163 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 164 | #endif 165 | 166 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 167 | gyroYangle += gyroYrate * dt; 168 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 169 | //gyroYangle += kalmanY.getRate() * dt; 170 | 171 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 172 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 173 | 174 | // Reset the gyro angle when it has drifted too much 175 | if (gyroXangle < -180 || gyroXangle > 180) 176 | gyroXangle = kalAngleX; 177 | if (gyroYangle < -180 || gyroYangle > 180) 178 | gyroYangle = kalAngleY; 179 | 180 | /* Print Data */ 181 | #if 1// Set to 1 to activate 182 | //Serial.print(accX); Serial.print("\t"); 183 | //Serial.print(accY); Serial.print("\t"); 184 | //Serial.print(accZ); Serial.print("\t"); 185 | 186 | //Serial.print(gyroX); Serial.print("\t"); 187 | //Serial.print(gyroY); Serial.print("\t"); 188 | //Serial.print(gyroZ); Serial.print("\t"); 189 | 190 | Serial.print("\t"); 191 | #endif 192 | 193 | //Serial.print(roll); Serial.print("\t"); 194 | //Serial.print(gyroXangle); Serial.print("\t"); 195 | //Serial.print(compAngleX); Serial.print("\t"); 196 | Serial.print(kalAngleX); Serial.print("\t"); 197 | 198 | Serial.print("\t"); 199 | 200 | //Serial.print(pitch); Serial.print("\t"); 201 | //Serial.print(gyroYangle); Serial.print("\t"); 202 | //Serial.print(compAngleY); Serial.print("\t"); 203 | Serial.print(kalAngleY); Serial.print("\t"); 204 | 205 | #if 0 // Set to 1 to print the temperature 206 | Serial.print("\t"); 207 | 208 | double temperature = (double)tempRaw / 340.0 + 36.53; 209 | Serial.print(temperature); Serial.print("\t"); 210 | #endif 211 | 212 | Serial.print("\r\n"); 213 | 214 | ////////////////////-kalman----------------------------------------- 215 | 216 | 217 | if (digitalRead(LEFT_PIN) == 0) { 218 | sign = 1; 219 | stepper2.moveTo(pos); 220 | stepper2.run(); 221 | } 222 | 223 | else if (digitalRead(RIGHT_PIN) == 0) { 224 | sign = -1; 225 | stepper2.moveTo(-pos); 226 | stepper2.run(); 227 | } 228 | 229 | else if (digitalRead(STOP_PIN) == 0) { 230 | sign = 0; 231 | stepper2.moveTo(0); 232 | stepper2.run(); 233 | } 234 | 235 | 236 | 237 | else { 238 | 239 | 240 | 241 | /////----- 242 | int valuey = kalAngleY; 243 | 244 | if (valuey > -89 && valuey < -80 ) 245 | 246 | {stepper2.moveTo(-85 * cantidad); 247 | stepper2.run();} 248 | 249 | 250 | else if (valuey > -79 && valuey < -70 ) 251 | 252 | {stepper2.moveTo(-75 * cantidad); 253 | stepper2.run();} 254 | 255 | else if (valuey > -69 && valuey < -60 ) 256 | 257 | {stepper2.moveTo(-65 * cantidad); 258 | stepper2.run();} 259 | 260 | else if (valuey > -59 && valuey < -50 ) 261 | 262 | {stepper2.moveTo(-55 * cantidad); 263 | stepper2.run();} 264 | 265 | else if (valuey > -49 && valuey < -40 ) 266 | 267 | {stepper2.moveTo(-45 * cantidad); 268 | stepper2.run();} 269 | 270 | else if (valuey > -39 && valuey < -30 ) 271 | 272 | {stepper2.moveTo(-35 * cantidad); 273 | stepper2.run();} 274 | 275 | else if (valuey > -29 && valuey < -20 ) 276 | 277 | {stepper2.moveTo(-25 * cantidad); 278 | stepper2.run();} 279 | 280 | else if (valuey > -19 && valuey < -10 ) 281 | 282 | {stepper2.moveTo(-15 * cantidad); 283 | stepper2.run();} 284 | 285 | else if (valuey > -9 && valuey < 0 ) 286 | 287 | {stepper2.moveTo(-5 * cantidad); 288 | stepper2.run();} 289 | 290 | else if (valuey > 1 && valuey < 10 ) 291 | 292 | {stepper2.moveTo(5 * cantidad); 293 | stepper2.run();} 294 | 295 | else if (valuey > 11 && valuey < 20 ) 296 | 297 | {stepper2.moveTo(15 * cantidad); 298 | stepper2.run();} 299 | 300 | else if (valuey > 21 && valuey < 30 ) 301 | 302 | {stepper2.moveTo(25 * cantidad); 303 | stepper2.run();} 304 | 305 | 306 | else if (valuey > 31 && valuey < 40 ) 307 | 308 | {stepper2.moveTo(35 * cantidad); 309 | stepper2.run();} 310 | 311 | 312 | else if (valuey > 41 && valuey < 50 ) 313 | 314 | {stepper2.moveTo(45 * cantidad); 315 | stepper2.run();} 316 | 317 | 318 | else if (valuey > 51 && valuey < 60 ) 319 | 320 | {stepper2.moveTo(55 * cantidad); 321 | stepper2.run();} 322 | 323 | else if (valuey > 61 && valuey < 70 ) 324 | 325 | {stepper2.moveTo(65 * cantidad); 326 | stepper2.run();} 327 | 328 | else if (valuey > 71 && valuey < 80 ) 329 | 330 | {stepper2.moveTo(75 * cantidad); 331 | stepper2.run();} 332 | 333 | else if (valuey > 81 && valuey < 89 ) 334 | 335 | {stepper2.moveTo(85 * cantidad); 336 | stepper2.run();} 337 | 338 | //// 339 | 340 | int valuex = kalAngleX; 341 | 342 | if (valuex > -89 && valuex < -80 ) 343 | 344 | {stepper2.moveTo(-85 * cantidad); 345 | stepper2.run();} 346 | 347 | 348 | else if (valuex > -79 && valuex < -70 ) 349 | 350 | {stepper2.moveTo(-75 * cantidad); 351 | stepper2.run();} 352 | 353 | else if (valuex > -69 && valuex < -60 ) 354 | 355 | {stepper2.moveTo(-65 * cantidad); 356 | stepper2.run();} 357 | 358 | else if (valuex > -59 && valuex < -50 ) 359 | 360 | {stepper2.moveTo(-55 * cantidad); 361 | stepper2.run();} 362 | 363 | else if (valuex > -49 && valuex < -40 ) 364 | 365 | {stepper2.moveTo(-45 * cantidad); 366 | stepper2.run();} 367 | 368 | else if (valuex > -39 && valuex < -30 ) 369 | 370 | {stepper2.moveTo(-35 * cantidad); 371 | stepper2.run();} 372 | 373 | else if (valuex > -29 && valuex < -20 ) 374 | 375 | {stepper2.moveTo(-25 * cantidad); 376 | stepper2.run();} 377 | 378 | else if (valuex > -11 && valuex < -10 ) 379 | 380 | {stepper2.moveTo(-15 * cantidad); 381 | stepper2.run();} 382 | 383 | else if (valuex > -9 && valuex < 0 ) 384 | 385 | {stepper2.moveTo(-5 * cantidad); 386 | stepper2.run();} 387 | 388 | else if (valuex > 1 && valuex < 10 ) 389 | 390 | {stepper2.moveTo(5 * cantidad); 391 | stepper2.run();} 392 | 393 | else if (valuex > 11 && valuex < 20 ) 394 | 395 | {stepper2.moveTo(15 * cantidad); 396 | stepper2.run();} 397 | 398 | else if (valuex > 21 && valuex < 30 ) 399 | 400 | {stepper2.moveTo(25 * cantidad); 401 | stepper2.run();} 402 | 403 | 404 | else if (valuex > 31 && valuex < 40 ) 405 | 406 | {stepper2.moveTo(35 * cantidad); 407 | stepper2.run();} 408 | 409 | 410 | else if (valuex > 41 && valuex < 50 ) 411 | 412 | {stepper2.moveTo(45 * cantidad); 413 | stepper2.run();} 414 | 415 | 416 | else if (valuex > 51 && valuex < 60 ) 417 | 418 | {stepper2.moveTo(55 * cantidad); 419 | stepper2.run();} 420 | 421 | else if (valuex > 61 && valuex < 70 ) 422 | 423 | {stepper2.moveTo(65 * cantidad); 424 | stepper2.run();} 425 | 426 | else if (valuex > 71 && valuex < 80 ) 427 | 428 | {stepper2.moveTo(75 * cantidad); 429 | stepper2.run();} 430 | 431 | else if (valuex > 81 && valuex < 89 ) 432 | 433 | {stepper2.moveTo(85 * cantidad); 434 | stepper2.run();} 435 | 436 | 437 | //////_--- 438 | 439 | 440 | 441 | } 442 | 443 | 444 | 445 | 446 | 447 | } 448 | -------------------------------------------------------------------------------- /imu/yun_www_imu_2xstepper.ino: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | #include "Kalman.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | ////web server 13 | YunServer server; 14 | String msg; 15 | 16 | char datos; 17 | //int velocidad; 18 | //int aceleracion; 19 | 20 | 21 | #define RESTRICT_PITCH 22 | 23 | Kalman kalmanX; 24 | Kalman kalmanY; 25 | 26 | ///////////stepeer 27 | 28 | AccelStepper stepper1(1, 9, 8); 29 | 30 | AccelStepper stepper2(1, 7, 6); 31 | int pos = 2000; 32 | //int cantidad = 7; 33 | int addr = 0; 34 | 35 | 36 | // Define our three input button pins 37 | #define LEFT_PIN 4 38 | #define STOP_PIN 3 39 | #define RIGHT_PIN 5 40 | #define CONTROL_PIN 11 41 | 42 | #define SPEED_PIN 0 43 | 44 | int velocidad = 1000; 45 | int cantidad = 7; 46 | int aceleracion = 500; 47 | 48 | 49 | 50 | /* IMU Data */ 51 | double accX, accY, accZ; 52 | double gyroX, gyroY, gyroZ; 53 | int16_t tempRaw; 54 | 55 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only 56 | double compAngleX, compAngleY; // Calculated angle using a complementary filter 57 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter 58 | 59 | uint32_t timer; 60 | uint8_t i2cData[14]; // Buffer for I2C data 61 | 62 | // TODO: Make calibration routine 63 | 64 | void setup() { 65 | pinMode(LEFT_PIN, INPUT_PULLUP); 66 | pinMode(STOP_PIN, INPUT_PULLUP); 67 | pinMode(RIGHT_PIN, INPUT_PULLUP); 68 | pinMode(CONTROL_PIN, INPUT_PULLUP); 69 | 70 | 71 | 72 | Serial.begin(115200); 73 | Wire.begin(); 74 | 75 | Bridge.begin(); 76 | server.listenOnLocalhost(); 77 | server.begin(); 78 | //while (!Serial); 79 | 80 | 81 | //while (CONTROL_PIN == LOW); 82 | //runREAD(); 83 | 84 | 85 | 86 | //TWBR = ((F_CPU / 100000L) - 16) / 2; // Set I2C frequency to 400kHz 87 | 88 | stepper1.setMaxSpeed(1000); 89 | stepper2.setMaxSpeed(1000); 90 | 91 | stepper1.setAcceleration(500); 92 | stepper2.setAcceleration(500); 93 | 94 | i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz 95 | i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling 96 | i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s 97 | i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g 98 | while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once 99 | while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode 100 | 101 | while (i2cRead(0x75, i2cData, 1)); 102 | if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register 103 | Serial.print(F("Error reading sensor")); 104 | while (1); 105 | } 106 | 107 | delay(100); // Wait for sensor to stabilize 108 | 109 | /* Set kalman and gyro starting angle */ 110 | while (i2cRead(0x3B, i2cData, 6)); 111 | accX = (i2cData[0] << 8) | i2cData[1]; 112 | accY = (i2cData[2] << 8) | i2cData[3]; 113 | accZ = (i2cData[4] << 8) | i2cData[5]; 114 | 115 | // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 116 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 117 | // It is then converted from radians to degrees 118 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 119 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 120 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 121 | #else // Eq. 28 and 29 122 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 123 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 124 | #endif 125 | 126 | kalmanX.setAngle(roll); // Set starting angle 127 | kalmanY.setAngle(pitch); 128 | gyroXangle = roll; 129 | gyroYangle = pitch; 130 | compAngleX = roll; 131 | compAngleY = pitch; 132 | 133 | timer = micros(); 134 | } 135 | 136 | 137 | /////////////////7 138 | void runREAD() { 139 | Process p; 140 | p.begin("cat"); 141 | p.addParameter("/www/param4.txt"); 142 | p.run(); 143 | 144 | while (p.available()>0) { 145 | datos = p.read(); 146 | cantidad = datos - 0; 147 | 148 | } 149 | Serial.flush(); 150 | 151 | 152 | } 153 | ///////////////7 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | void loop() { 162 | 163 | 164 | ////// 165 | 166 | YunClient client = server.accept(); //check new clients 167 | 168 | if(client) { 169 | String command = client.readStringUntil('/'); //read the incoming data 170 | if (command == "msg") { 171 | msg = client.readStringUntil('/'); // read the incoming data 172 | File dataFile = FileSystem.open("/www/param4.txt",FILE_WRITE); 173 | dataFile.print(msg); 174 | dataFile.close(); 175 | 176 | } 177 | client.stop(); 178 | } 179 | ///// 180 | 181 | 182 | 183 | 184 | /* Update all the values */ 185 | while (i2cRead(0x3B, i2cData, 14)); 186 | accX = ((i2cData[0] << 8) | i2cData[1]); 187 | accY = ((i2cData[2] << 8) | i2cData[3]); 188 | accZ = ((i2cData[4] << 8) | i2cData[5]); 189 | tempRaw = (i2cData[6] << 8) | i2cData[7]; 190 | gyroX = (i2cData[8] << 8) | i2cData[9]; 191 | gyroY = (i2cData[10] << 8) | i2cData[11]; 192 | gyroZ = (i2cData[12] << 8) | i2cData[13]; 193 | 194 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time 195 | timer = micros(); 196 | 197 | // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26 198 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 199 | // It is then converted from radians to degrees 200 | #ifdef RESTRICT_PITCH // Eq. 25 and 26 201 | double roll = atan2(accY, accZ) * RAD_TO_DEG; 202 | double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; 203 | #else // Eq. 28 and 29 204 | double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; 205 | double pitch = atan2(-accX, accZ) * RAD_TO_DEG; 206 | #endif 207 | 208 | double gyroXrate = gyroX / 131.0; // Convert to deg/s 209 | double gyroYrate = gyroY / 131.0; // Convert to deg/s 210 | 211 | #ifdef RESTRICT_PITCH 212 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 213 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { 214 | kalmanX.setAngle(roll); 215 | compAngleX = roll; 216 | kalAngleX = roll; 217 | gyroXangle = roll; 218 | } else 219 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 220 | 221 | if (abs(kalAngleX) > 90) 222 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading 223 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); 224 | #else 225 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees 226 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { 227 | kalmanY.setAngle(pitch); 228 | compAngleY = pitch; 229 | kalAngleY = pitch; 230 | gyroYangle = pitch; 231 | } else 232 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter 233 | 234 | if (abs(kalAngleY) > 90) 235 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading 236 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter 237 | #endif 238 | 239 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter 240 | gyroYangle += gyroYrate * dt; 241 | //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate 242 | //gyroYangle += kalmanY.getRate() * dt; 243 | 244 | compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter 245 | compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; 246 | 247 | // Reset the gyro angle when it has drifted too much 248 | if (gyroXangle < -180 || gyroXangle > 180) 249 | gyroXangle = kalAngleX; 250 | if (gyroYangle < -180 || gyroYangle > 180) 251 | gyroYangle = kalAngleY; 252 | 253 | /* Print Data */ 254 | #if 0 // Set to 1 to activate 255 | Serial.print(accX); Serial.print("\t"); 256 | Serial.print(accY); Serial.print("\t"); 257 | Serial.print(accZ); Serial.print("\t"); 258 | 259 | Serial.print(gyroX); Serial.print("\t"); 260 | Serial.print(gyroY); Serial.print("\t"); 261 | Serial.print(gyroZ); Serial.print("\t"); 262 | 263 | Serial.print("\t"); 264 | #endif 265 | 266 | //Serial.print(roll); Serial.print("\t"); 267 | //Serial.print(gyroXangle); Serial.print("\t"); 268 | //Serial.print(compAngleX); Serial.print("\t"); 269 | Serial.print(kalAngleX); Serial.print("\t"); 270 | 271 | Serial.print("\t"); 272 | 273 | //Serial.print(pitch); Serial.print("\t"); 274 | //Serial.print(gyroYangle); Serial.print("\t"); 275 | //Serial.print(compAngleY); Serial.print("\t"); 276 | Serial.print(kalAngleY); Serial.print("\t"); 277 | 278 | #if 0 // Set to 1 to print the temperature 279 | Serial.print("\t"); 280 | 281 | double temperature = (double)tempRaw / 340.0 + 36.53; 282 | Serial.print(temperature); Serial.print("\t"); 283 | #endif 284 | 285 | Serial.print("\r\n"); 286 | delay(2); 287 | 288 | 289 | //////////////////7 290 | 291 | 292 | if (digitalRead(CONTROL_PIN) == LOW) 293 | 294 | 295 | { 296 | 297 | int valuey = kalAngleX; 298 | int valuex = kalAngleY; 299 | 300 | 301 | if (valuey >= -89 && valuey <= -85 ) 302 | 303 | {stepper2.moveTo(-87 * cantidad); 304 | stepper2.run();} 305 | 306 | else if (valuey >= -84 && valuey <= -80 ) 307 | 308 | {stepper2.moveTo(-83 * cantidad); 309 | stepper2.run();} 310 | 311 | else if (valuey >= -79 && valuey <= -75 ) 312 | 313 | {stepper2.moveTo(-77 * cantidad); 314 | stepper2.run();} 315 | 316 | else if (valuey >= -74 && valuey <= -70 ) 317 | 318 | {stepper2.moveTo(-73 * cantidad); 319 | stepper2.run();} 320 | 321 | else if (valuey >= -69 && valuey <= -65 ) 322 | 323 | {stepper2.moveTo(-67 * cantidad); 324 | stepper2.run();} 325 | 326 | else if (valuey >= -64 && valuey <= -60 ) 327 | 328 | {stepper2.moveTo(-63 * cantidad); 329 | stepper2.run();} 330 | 331 | else if (valuey >= -59 && valuey <= -55 ) 332 | 333 | {stepper2.moveTo(-67 * cantidad); 334 | stepper2.run();} 335 | 336 | else if (valuey >= -54 && valuey <= -50 ) 337 | 338 | {stepper2.moveTo(-53 * cantidad); 339 | stepper2.run();} 340 | 341 | else if (valuey >= -49 && valuey <= -45 ) 342 | 343 | {stepper2.moveTo(-47 * cantidad); 344 | stepper2.run();} 345 | 346 | else if (valuey >= -44 && valuey <= -40 ) 347 | 348 | {stepper2.moveTo(-43 * cantidad); 349 | stepper2.run();} 350 | 351 | else if (valuey >= -39 && valuey <= -35 ) 352 | 353 | {stepper2.moveTo(-37 * cantidad); 354 | stepper2.run();} 355 | 356 | else if (valuey >= -34 && valuey <= -30 ) 357 | 358 | {stepper2.moveTo(-33 * cantidad); 359 | stepper2.run();} 360 | 361 | else if (valuey >= -29 && valuey <= -25 ) 362 | 363 | {stepper2.moveTo(-27 * cantidad); 364 | stepper2.run();} 365 | 366 | else if (valuey >= -24 && valuey <= -20 ) 367 | 368 | {stepper2.moveTo(-23 * cantidad); 369 | stepper2.run();} 370 | 371 | else if (valuey >= -19 && valuey <= -15 ) 372 | 373 | {stepper2.moveTo(-17 * cantidad); 374 | stepper2.run();} 375 | 376 | else if (valuey >= -14 && valuey <= -10 ) 377 | 378 | {stepper2.moveTo(-13 * cantidad); 379 | stepper2.run();} 380 | 381 | else if (valuey >= -9 && valuey <= -5 ) 382 | 383 | {stepper2.moveTo(-7 * cantidad); 384 | stepper2.run();} 385 | 386 | else if (valuey >= -4 && valuey <= -1 ) 387 | 388 | {stepper2.moveTo(-3 * cantidad); 389 | stepper2.run();} 390 | 391 | else if (valuey >= 1 && valuey <= 5 ) 392 | 393 | {stepper2.moveTo(3 * cantidad); 394 | stepper2.run();} 395 | 396 | else if (valuey >= 6 && valuey <= 10 ) 397 | 398 | {stepper2.moveTo(8 * cantidad); 399 | stepper2.run();} 400 | 401 | else if (valuey >= 11 && valuey <= 15 ) 402 | 403 | {stepper2.moveTo(13 * cantidad); 404 | stepper2.run();} 405 | 406 | else if (valuey >= 16 && valuey <= 20 ) 407 | 408 | {stepper2.moveTo(18 * cantidad); 409 | stepper2.run();} 410 | 411 | else if (valuey >= 21 && valuey <= 25 ) 412 | 413 | {stepper2.moveTo(23 * cantidad); 414 | stepper2.run();} 415 | 416 | else if (valuey >= 26 && valuey <= 30 ) 417 | 418 | {stepper2.moveTo(28 * cantidad); 419 | stepper2.run();} 420 | 421 | else if (valuey >= 31 && valuey <= 35 ) 422 | 423 | {stepper2.moveTo(33 * cantidad); 424 | stepper2.run();} 425 | 426 | else if (valuey >= 36 && valuey <= 40 ) 427 | 428 | {stepper2.moveTo(38 * cantidad); 429 | stepper2.run();} 430 | 431 | else if (valuey >= 41 && valuey <= 45 ) 432 | 433 | {stepper2.moveTo(43 * cantidad); 434 | stepper2.run();} 435 | 436 | else if (valuey >= 46 && valuey <= 50 ) 437 | 438 | {stepper2.moveTo(48 * cantidad); 439 | stepper2.run();} 440 | 441 | else if (valuey >= 51 && valuey <= 55 ) 442 | 443 | {stepper2.moveTo(53 * cantidad); 444 | stepper2.run();} 445 | 446 | else if (valuey >= 56 && valuey <= 60 ) 447 | 448 | {stepper2.moveTo(58 * cantidad); 449 | stepper2.run();} 450 | 451 | else if (valuey >= 61 && valuey <= 65 ) 452 | 453 | {stepper2.moveTo(63 * cantidad); 454 | stepper2.run();} 455 | 456 | else if (valuey >= 66 && valuey <= 70 ) 457 | 458 | {stepper2.moveTo(68 * cantidad); 459 | stepper2.run();} 460 | 461 | else if (valuey >= 71 && valuey <= 85 ) 462 | 463 | {stepper2.moveTo(73 * cantidad); 464 | stepper2.run();} 465 | 466 | 467 | 468 | else if (valuey >= 86 && valuey <= 89 ) 469 | 470 | {stepper2.moveTo(88 * cantidad); 471 | stepper2.run();} 472 | 473 | ////////////////////////////// 474 | ///////////////////////////// 475 | ///////////////////////////// 476 | 477 | 478 | else {} 479 | 480 | 481 | if (valuex >= -89 && valuex <= -85 ) 482 | 483 | {stepper1.moveTo(-87 * cantidad); 484 | stepper1.run();} 485 | 486 | else if (valuex >= -84 && valuex <= -80 ) 487 | 488 | {stepper1.moveTo(-83 * cantidad); 489 | stepper1.run();} 490 | 491 | else if (valuex >= -79 && valuex <= -75 ) 492 | 493 | {stepper1.moveTo(-77 * cantidad); 494 | stepper1.run();} 495 | 496 | else if (valuex >= -74 && valuex <= -70 ) 497 | 498 | {stepper1.moveTo(-73 * cantidad); 499 | stepper1.run();} 500 | 501 | else if (valuex >= -69 && valuex <= -65 ) 502 | 503 | {stepper1.moveTo(-67 * cantidad); 504 | stepper1.run();} 505 | 506 | else if (valuex >= -64 && valuex <= -60 ) 507 | 508 | {stepper1.moveTo(-63 * cantidad); 509 | stepper1.run();} 510 | 511 | else if (valuex >= -59 && valuex <= -55 ) 512 | 513 | {stepper1.moveTo(-67 * cantidad); 514 | stepper1.run();} 515 | 516 | else if (valuex >= -54 && valuex <= -50 ) 517 | 518 | {stepper1.moveTo(-53 * cantidad); 519 | stepper1.run();} 520 | 521 | else if (valuex >= -49 && valuex <= -45 ) 522 | 523 | {stepper1.moveTo(-47 * cantidad); 524 | stepper1.run();} 525 | 526 | else if (valuex >= -44 && valuex <= -40 ) 527 | 528 | {stepper1.moveTo(-43 * cantidad); 529 | stepper1.run();} 530 | 531 | else if (valuex >= -39 && valuex <= -35 ) 532 | 533 | {stepper1.moveTo(-37 * cantidad); 534 | stepper1.run();} 535 | 536 | else if (valuex >= -34 && valuex <= -30 ) 537 | 538 | {stepper1.moveTo(-33 * cantidad); 539 | stepper1.run();} 540 | 541 | else if (valuex >= -29 && valuex <= -25 ) 542 | 543 | {stepper1.moveTo(-27 * cantidad); 544 | stepper1.run();} 545 | 546 | else if (valuex >= -24 && valuex <= -20 ) 547 | 548 | {stepper1.moveTo(-23 * cantidad); 549 | stepper1.run();} 550 | 551 | else if (valuex >= -19 && valuex <= -15 ) 552 | 553 | {stepper1.moveTo(-17 * cantidad); 554 | stepper1.run();} 555 | 556 | else if (valuex >= -14 && valuex <= -10 ) 557 | 558 | {stepper1.moveTo(-13 * cantidad); 559 | stepper1.run();} 560 | 561 | else if (valuex >= -9 && valuex <= -5 ) 562 | 563 | {stepper1.moveTo(-7 * cantidad); 564 | stepper1.run();} 565 | 566 | else if (valuex >= -4 && valuex <= -1 ) 567 | 568 | {stepper1.moveTo(-3 * cantidad); 569 | stepper1.run();} 570 | 571 | else if (valuex >= 1 && valuex <= 5 ) 572 | 573 | {stepper1.moveTo(3 * cantidad); 574 | stepper1.run();} 575 | 576 | else if (valuex >= 6 && valuex <= 10 ) 577 | 578 | {stepper1.moveTo(8 * cantidad); 579 | stepper1.run();} 580 | 581 | else if (valuex >= 11 && valuex <= 15 ) 582 | 583 | {stepper1.moveTo(13 * cantidad); 584 | stepper1.run();} 585 | 586 | else if (valuex >= 16 && valuex <= 20 ) 587 | 588 | {stepper1.moveTo(18 * cantidad); 589 | stepper1.run();} 590 | 591 | else if (valuex >= 21 && valuex <= 25 ) 592 | 593 | {stepper1.moveTo(23 * cantidad); 594 | stepper1.run();} 595 | 596 | else if (valuex >= 26 && valuex <= 30 ) 597 | 598 | {stepper1.moveTo(28 * cantidad); 599 | stepper1.run();} 600 | 601 | else if (valuex >= 31 && valuex <= 35 ) 602 | 603 | {stepper1.moveTo(33 * cantidad); 604 | stepper1.run();} 605 | 606 | else if (valuex >= 36 && valuex <= 40 ) 607 | 608 | {stepper1.moveTo(38 * cantidad); 609 | stepper1.run();} 610 | 611 | else if (valuex >= 41 && valuex <= 45 ) 612 | 613 | {stepper1.moveTo(43 * cantidad); 614 | stepper1.run();} 615 | 616 | else if (valuex >= 46 && valuex <= 50 ) 617 | 618 | {stepper1.moveTo(48 * cantidad); 619 | stepper1.run();} 620 | 621 | else if (valuex >= 51 && valuex <= 55 ) 622 | 623 | {stepper1.moveTo(53 * cantidad); 624 | stepper1.run();} 625 | 626 | else if (valuex >= 56 && valuex <= 60 ) 627 | 628 | {stepper1.moveTo(58 * cantidad); 629 | stepper1.run();} 630 | 631 | else if (valuex >= 61 && valuex <= 65 ) 632 | 633 | {stepper1.moveTo(63 * cantidad); 634 | stepper1.run();} 635 | 636 | else if (valuex >= 66 && valuex <= 70 ) 637 | 638 | {stepper1.moveTo(68 * cantidad); 639 | stepper1.run();} 640 | 641 | else if (valuex >= 71 && valuex <= 85 ) 642 | 643 | {stepper1.moveTo(73 * cantidad); 644 | stepper1.run();} 645 | 646 | 647 | 648 | else if (valuex >= 86 && valuex <= 89 ) 649 | 650 | {stepper1.moveTo(88 * cantidad); 651 | stepper1.run();} 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | else {} 662 | 663 | //////_--- 664 | 665 | 666 | } 667 | 668 | 669 | else 670 | 671 | 672 | { 673 | 674 | if (digitalRead(LEFT_PIN) == 0) { 675 | 676 | stepper2.moveTo(pos); 677 | stepper2.run(); 678 | } 679 | 680 | else if (digitalRead(RIGHT_PIN) == 0) { 681 | 682 | stepper2.moveTo(-pos); 683 | stepper2.run(); 684 | } 685 | 686 | else if (digitalRead(STOP_PIN) == 0) { 687 | 688 | stepper2.moveTo(0); 689 | stepper2.run(); 690 | } 691 | 692 | 693 | 694 | } 695 | 696 | 697 | 698 | 699 | 700 | } 701 | -------------------------------------------------------------------------------- /read_own_vcc: -------------------------------------------------------------------------------- 1 | It turns out the Arduino 168 and 328 can measure their own voltage rail. 2 | 3 | Code 4 | 5 | Copy, paste into Arduino and see what it returns. This works on an Arduino 168 or 328. 6 | 7 | ``` long readVcc() { long result; // Read 1.1V reference against AVcc ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); delay(2); // Wait for Vref to settle ADCSRA |= _BV(ADSC); // Convert while (bit_is_set(ADCSRA,ADSC)); result = ADCL; result |= ADCH<<8; result = 1126400L / result; // Back-calculate AVcc in mV return result; } 8 | 9 | void setup() { Serial.begin(9600); } 10 | 11 | void loop() { Serial.println( readVcc(), DEC ); delay(1000); } ``` 12 | 13 | The voltage is returned in millivolts. So 5000 is 5V, 3300 is 3.3V. 14 | 15 | Note the following: * This works on Arduinos with a 328 or 168 only. It looks like the same trick might be possible on the Arduino Mega - experiments are ongoing, and will be reported here. 16 | -------------------------------------------------------------------------------- /slave_clock/test1.ino: -------------------------------------------------------------------------------- 1 | 2 | void setup() { 3 | // initialize the digital pin as an output. 4 | // Pin 13 has an LED connected on most Arduino boards: 5 | Serial.begin(9600); 6 | pinMode(13, OUTPUT); 7 | pinMode(12, INPUT); 8 | pinMode(2, OUTPUT); // relay for +/- 24V on pin#2 9 | } 10 | 11 | 12 | void loop() { 13 | while (digitalRead(12)){ // check the switch or jumper on pin#12 14 | 15 | /* tick 1 minute backwards every 1 second */ 16 | 17 | digitalWrite(13, HIGH); // set the LED on 18 | digitalWrite(2, HIGH); // tick 19 | delay(1000); // wait for a second 20 | digitalWrite(13, LOW); // set the LED off 21 | digitalWrite(2, LOW); // tick 22 | delay(1000); // wait for a second 23 | } 24 | 25 | /* Something COOLER */ 26 | 27 | float something = millis()/60000.0; 28 | int value = 128.0 + 128 * sin( something * 2.0 * PI ); 29 | // analogWrite(Pin,value); 30 | 31 | digitalWrite(13, HIGH); // set the LED on 32 | digitalWrite(2, HIGH); // tick 33 | delay(250+(value*10)); // sine wave delay 34 | Serial.println(250+(value*10)); 35 | digitalWrite(13, LOW); // set the LED off 36 | digitalWrite(2, LOW); // tick 37 | delay(250+(value*10)); // sine wave delay 38 | 39 | } 40 | --------------------------------------------------------------------------------