├── .gitignore ├── README.md ├── PhobGCC ├── src │ ├── Phob1_1Teensy3_2.h │ ├── Phob1_1Teensy4_0.h │ └── Phob1_0Teensy3_2.h └── PhobGCC.ino └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore list for KiCAD Projects 2 | # Temporary files 3 | *.000 4 | *.bak 5 | *.bck 6 | *.kicad_pcb-bak 7 | *.sch-bak 8 | *~ 9 | *.kicad_prl 10 | *-backups* 11 | _autosave-* 12 | *.tmp 13 | *-save.pro 14 | *-save.kicad_pcb 15 | fp-info-cache 16 | # Netlist files (exported from Eeschema) 17 | *.net 18 | *.xml 19 | # Autorouter files (exported from Pcbnew) 20 | *.dsn 21 | *.ses 22 | # vim swap files 23 | *.swp 24 | # vim config file 25 | *.vimrc 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PhobGCC 2 | 3 | NOTE: This repository is not currently being mantained, has been moved to: https://github.com/PhobGCC 4 | 5 | *** 6 | 7 | This is the legacy repository for a gamecube controller motherboard using a teensy as the microcontroller. Aim is to make an accessible and consistent controller. Has the option of using hall effect sensors instead of potentiometers, notch calibration, and snapback filtering. 8 | 9 | If your interested in making one, join the project discord to ask questions and get the most up to date information: https://discord.gg/eNJ7xWMvxf 10 | 11 | Check out the wiki for some info about how the phobGCC works: https://github.com/Phobos132/PhobGCC/wiki 12 | 13 | 14 | Hall effect sensors: 15 | 16 | ![Prototypes](https://www.dropbox.com/s/fyltdef79c2z78y/Hall%20Sensors.png?raw=1) 17 | 18 | Board Version 1.1: 19 | 20 | ![1.1](https://www.dropbox.com/s/cgxgo3ve1nrf6j9/20220218_182602.jpg?raw=1) 21 | 22 | Initial prototypes: 23 | 24 | ![Prototypes](https://www.dropbox.com/s/q8ypkzmfeijdc5w/boards.jpg?raw=1) 25 | -------------------------------------------------------------------------------- /PhobGCC/src/Phob1_1Teensy3_2.h: -------------------------------------------------------------------------------- 1 | #ifndef BOARD_H 2 | #define BOARD_H 3 | 4 | #include 5 | #include 6 | 7 | //Hardware specific code for PhobGCC board revision 1.1 with a Teensy 3.2 8 | #define TEENSY3_2 9 | 10 | //defining which pin is what on the teensy 11 | const int _pinLa = 16; 12 | const int _pinRa = 23; 13 | const int _pinL = 12; 14 | const int _pinR = 3; 15 | const int _pinAx = 15; 16 | const int _pinAy = 14; 17 | //const int _pinCx = 21; 18 | //const int _pinCy = 22; 19 | const int _pinCx = 22; 20 | const int _pinCy = 21; 21 | const int _pinRX = 9; 22 | const int _pinTX = 10; 23 | const int _pinDr = 6; 24 | const int _pinDu = 18; 25 | const int _pinDl = 17; 26 | const int _pinDd = 11; 27 | const int _pinX = 1; 28 | const int _pinY = 2; 29 | const int _pinA = 4; 30 | const int _pinB = 20; 31 | const int _pinZ = 0; 32 | const int _pinS = 19; 33 | 34 | //don't #define USEADCSCALE 35 | 36 | void serialSetup() { 37 | Serial.begin(57600); 38 | Serial.println("This is the header for board revision 1.1 with a Teensy 3.2."); 39 | } 40 | 41 | void ADCSetup(ADC * adc, 42 | float &,/*ADCScale not used*/ 43 | float & /*ADCScaleFactor not used*/) { 44 | adc->adc0->setAveraging(1); 45 | adc->adc0->setResolution(12); 46 | adc->adc0->setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED); 47 | adc->adc0->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); 48 | 49 | } 50 | 51 | #endif // BOARD_H 52 | -------------------------------------------------------------------------------- /PhobGCC/src/Phob1_1Teensy4_0.h: -------------------------------------------------------------------------------- 1 | #ifndef BOARD_H 2 | #define BOARD_H 3 | 4 | #include 5 | #include 6 | 7 | //Hardware specific code for PhobGCC board revision 1.1 with a Teensy 4.0 8 | #define TEENSY4_0 9 | 10 | //defining which pin is what on the teensy 11 | const int _pinLa = 16; 12 | const int _pinRa = 23; 13 | const int _pinL = 12; 14 | const int _pinR = 3; 15 | const int _pinAx = 15; 16 | const int _pinAy = 14; 17 | //const int _pinCx = 21; 18 | //const int _pinCy = 22; 19 | const int _pinCx = 22; 20 | const int _pinCy = 21; 21 | const int _pinRX = 7; 22 | const int _pinTX = 8; 23 | const int _pinDr = 6; 24 | const int _pinDu = 18; 25 | const int _pinDl = 17; 26 | const int _pinDd = 11; 27 | const int _pinX = 1; 28 | const int _pinY = 2; 29 | const int _pinA = 4; 30 | const int _pinB = 20; 31 | const int _pinZ = 0; 32 | const int _pinS = 19; 33 | 34 | //don't #define USEADCSCALE 35 | 36 | void serialSetup() { 37 | Serial.begin(115200); 38 | Serial.println("This is the header for board revision 1.1 with a Teensy 4.0."); 39 | } 40 | 41 | void ADCSetup(ADC * adc, 42 | float &,/*ADCScale not used*/ 43 | float & /*ADCScaleFactor not used*/) { 44 | adc->adc0->setAveraging(1); 45 | adc->adc0->setResolution(12); 46 | adc->adc0->setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED); 47 | adc->adc0->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); 48 | 49 | } 50 | 51 | #endif // BOARD_H 52 | -------------------------------------------------------------------------------- /PhobGCC/src/Phob1_0Teensy3_2.h: -------------------------------------------------------------------------------- 1 | #ifndef BOARD_H 2 | #define BOARD_H 3 | 4 | #include 5 | #include 6 | 7 | //Hardware-specific code for PhobGCC board revision 1.0 with a Teensy 3.2 8 | #define TEENSY3_2 9 | 10 | //defining which pin is what on the teensy 11 | const int _pinLa = 16; 12 | const int _pinRa = 23; 13 | const int _pinL = 13; 14 | const int _pinR = 3; 15 | const int _pinAx = 15; 16 | const int _pinAy = 14; 17 | //const int _pinCx = 21; 18 | //const int _pinCy = 22; 19 | const int _pinCx = 22; 20 | const int _pinCy = 21; 21 | const int _pinRX = 9; 22 | const int _pinTX = 10; 23 | const int _pinDr = 7; 24 | const int _pinDu = 18; 25 | const int _pinDl = 17; 26 | const int _pinDd = 8; 27 | const int _pinX = 1; 28 | const int _pinY = 2; 29 | const int _pinA = 4; 30 | const int _pinB = 6; 31 | const int _pinZ = 0; 32 | const int _pinS = 19; 33 | 34 | #define USEADCSCALE 35 | 36 | void serialSetup() { 37 | Serial.begin(57600); 38 | Serial.println("This is the header for board revision 1.0 with a Teensy 3.2."); 39 | } 40 | 41 | void ADCSetup(ADC * adc, 42 | float & ADCScale, 43 | float & ADCScaleFactor) { 44 | adc->adc0->setAveraging(1); 45 | adc->adc0->setResolution(12); 46 | adc->adc0->setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED); 47 | adc->adc0->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); 48 | 49 | adc->adc1->setAveraging(32); 50 | adc->adc1->setResolution(16); 51 | adc->adc1->setConversionSpeed(ADC_CONVERSION_SPEED::MED_SPEED); 52 | adc->adc1->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_LOW_SPEED); 53 | 54 | VREF::start(); 55 | 56 | double refVoltage = 0; 57 | for (int i = 0; i < 512; i++) { 58 | int value = adc->adc1->analogRead(ADC_INTERNAL_SOURCE::VREF_OUT); 59 | double volts = value*3.3/(float)adc->adc1->getMaxValue(); 60 | refVoltage += volts; 61 | } 62 | refVoltage = refVoltage/512.0; 63 | 64 | ADCScale = 1.2/refVoltage; 65 | 66 | Serial.print("ADCScale: "); 67 | Serial.println(ADCScale); 68 | 69 | adc->adc1->setAveraging(1); 70 | adc->adc1->setResolution(12); 71 | adc->adc1->setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED); 72 | adc->adc1->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); 73 | 74 | ADCScaleFactor = 0.001*1.2*adc->adc1->getMaxValue()/3.3; 75 | Serial.print("ADCScaleFactor: "); 76 | Serial.println(ADCScaleFactor); 77 | } 78 | 79 | #endif // BOARD_H 80 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /PhobGCC/PhobGCC.ino: -------------------------------------------------------------------------------- 1 | //This software uses bits of code from GoodDoge's Dogebawx project, which was the initial starting point: https://github.com/DogeSSBM/DogeBawx 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "TeensyTimerTool.h" 12 | 13 | //Uncomment the appropriate include line for your hardware. 14 | //#include "src/Phob1_0Teensy3_2.h" 15 | //#include "src/Phob1_1Teensy3_2.h" 16 | //#include "src/Phob1_1Teensy4_0.h" 17 | 18 | #define BUILD_RELEASE 19 | //#define BUILD_DEV 20 | 21 | using namespace Eigen; 22 | 23 | TeensyTimerTool::OneShotTimer timer1; 24 | 25 | //defining control configuration 26 | int _pinZSwappable = _pinZ; 27 | int _pinXSwappable = _pinX; 28 | int _pinYSwappable = _pinY; 29 | int _jumpConfig = 0; 30 | int _lConfig = 0; 31 | int _rConfig = 0; 32 | int _triggerDefault = 0; 33 | int _lTrigger = 0; 34 | int _rTrigger = 1; 35 | bool _changeTrigger = true; 36 | int _cXOffset = 0; 37 | int _cYOffset = 0; 38 | int _cMax = 127; 39 | int _cMin = -127; 40 | int _LTriggerOffset = 49; 41 | int _RTriggerOffset = 49; 42 | int _triggerMin = 49; 43 | int _triggerMax = 255; 44 | bool _safeMode = true; 45 | 46 | ///// Values used for dealing with snapback in the Kalman Filter, a 6th power relationship between distance to center and ADC/acceleration variance is used, this was arrived at by trial and error 47 | 48 | float _velDampMin = 0.125; 49 | float _velDampMax = .5; 50 | 51 | // Values used for dealing with X/Y Smoothing in the CarVac Filter, for ledge-dashing 52 | // also used for C-stick snapback filtering 53 | 54 | float _smoothingMin = 0.0; 55 | float _smoothingMax = 0.9; 56 | 57 | //New snapback Kalman filter parameters. 58 | struct FilterGains { 59 | //What's the max stick distance from the center 60 | float maxStick; 61 | //filtered velocity terms 62 | //how fast the filtered velocity falls off in the absence of stick movement. 63 | //Probably don't touch this. 64 | float xVelDecay;//0.1 default for 1.2ms timesteps, larger for bigger timesteps 65 | float yVelDecay; 66 | //how much the current position disagreement impacts the filtered velocity. 67 | //Probably don't touch this. 68 | float xVelPosFactor;//0.01 default for 1.2ms timesteps, larger for bigger timesteps 69 | float yVelPosFactor; 70 | //how much to ignore filtered velocity when computing the new stick position. 71 | //DO CHANGE THIS 72 | //Higher gives shorter rise times and slower fall times (more pode, less snapback) 73 | float xVelDamp;//0.125 default for 1.2ms timesteps, smaller for bigger timesteps 74 | float yVelDamp; 75 | //speed and accel thresholds below which we try to follow the stick better 76 | //These may need tweaking according to how noisy the signal is 77 | //If it's noisier, we may need to add additional filtering 78 | //If the timesteps are *really small* then it may need to be increased to get 79 | // above the noise floor. Or some combination of filtering and playing with 80 | // the thresholds. 81 | float velThresh;//1 default for 1.2ms timesteps, larger for bigger timesteps 82 | float accelThresh;//5 default for 1.2ms timesteps, larger for bigger timesteps 83 | //This just applies a low-pass filter. 84 | //The purpose is to provide delay for single-axis ledgedashes. 85 | //Must be between 0 and 1. Larger = more smoothing and delay. 86 | float xSmoothing; 87 | float ySmoothing; 88 | //Same thing but for C-stick 89 | float cXSmoothing; 90 | float cYSmoothing; 91 | }; 92 | FilterGains _gains {//these values are actually timestep-compensated for in runKalman 93 | .maxStick = 100, 94 | .xVelDecay = 0.1, 95 | .yVelDecay = 0.1, 96 | .xVelPosFactor = 0.01, 97 | .yVelPosFactor = 0.01, 98 | .xVelDamp = 0.125, 99 | .yVelDamp = 0.125, 100 | .velThresh = 1.00, 101 | .accelThresh = 3.00, 102 | .xSmoothing = 0.0, 103 | .ySmoothing = 0.0, 104 | .cXSmoothing = 0.0, 105 | .cYSmoothing = 0.0 106 | }; 107 | FilterGains _g;//this gets filled by recomputeGains(); 108 | 109 | //////values used to determine how much large of a region will count as being "in a notch" 110 | 111 | const float _marginAngle = 1.50/100.0; //angle range(+/-) in radians that will be collapsed down to the ideal angle 112 | const float _tightAngle = 0.1/100.0;//angle range(+/-) in radians that the margin region will be collapsed down to, found that having a small value worked better for the transform than 0 113 | 114 | //////values used for calibration 115 | const int _noOfNotches = 16; 116 | const int _noOfCalibrationPoints = _noOfNotches * 2; 117 | const int _noOfAdjNotches = 12; 118 | float _ADCScale = 1; 119 | float _ADCScaleFactor = 1; 120 | const int _notCalibrating = -1; 121 | const float _maxStickAngle = 0.67195176201;//38.5 degrees; this is the max angular deflection of the stick. 122 | bool _calAStick = true; //determines which stick is being calibrated (if false then calibrate the c-stick) 123 | bool _advanceCal = false; 124 | bool _advanceCalPressed = false; 125 | bool _undoCal = false; 126 | bool _undoCalPressed = false; 127 | int _currentCalStep; //keeps track of which caliblration step is active, -1 means calibration is not running 128 | bool _notched = false; //keeps track of whether or not the controller has firefox notches 129 | const int _calibrationPoints = _noOfNotches+1; //number of calibration points for the c-stick and a-stick for a controller without notches 130 | float _cleanedPointsX[_noOfNotches+1]; //array to hold the x coordinates of the stick positions for calibration 131 | float _cleanedPointsY[_noOfNotches+1]; //array to hold the y coordinates of the stick positions for calibration 132 | float _notchPointsX[_noOfNotches+1]; //array to hold the x coordinates of the notches for calibration 133 | float _notchPointsY[_noOfNotches+1]; //array to hold the x coordinates of the notches for calibration 134 | // right notch 1 up right notch 2 up notch 3 up left notch 4 left notch 5 down left notch 6 down notch 7 down right notch 8 135 | // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 136 | const float _cDefaultCalPointsX[_noOfCalibrationPoints] = {0.507073712, 0.9026247224,0.5072693007,0.5001294236,0.5037118952,0.8146074226,0.5046028951,0.5066508636,0.5005339326,0.5065670067,0.5006805723,0.5056853599,0.5058308703,0.1989667596,0.5009560613,0.508400395, 0.507729394, 0.1003568119,0.5097473849,0.5074989796,0.5072406293,0.2014042034,0.5014653263,0.501119675, 0.502959011, 0.5032433665,0.5018446562,0.5085523857,0.5099732513,0.8100862401,0.5089320995,0.5052066109}; 137 | const float _cDefaultCalPointsY[_noOfCalibrationPoints] = {0.5006151799,0.5025356503,0.501470528, 0.5066983468,0.5008275958,0.8094667357,0.5008874968,0.5079207909,0.5071239815,0.9046004275,0.5010136589,0.5071086316,0.5058914031,0.8076523013,0.5078213507,0.5049117887,0.5075638281,0.5003774649,0.504562192, 0.50644895, 0.5074859854,0.1983865682,0.5074515232,0.5084323402,0.5015846608,0.1025902875,0.5043605453,0.5070589342,0.5073953693,0.2033337702,0.5005351734,0.5056548782}; 138 | const float _aDefaultCalPointsX[_noOfCalibrationPoints] = {0.3010610568,0.3603937084,0.3010903951,0.3000194135,0.3005567843,0.3471911134,0.3006904343,0.3009976295,0.3000800899,0.300985051, 0.3001020858,0.300852804, 0.3008746305,0.2548450139,0.3001434092,0.3012600593,0.3011594091,0.2400535218,0.3014621077,0.3011248469,0.3010860944,0.2552106305,0.3002197989,0.3001679513,0.3004438517,0.300486505, 0.3002766984,0.3012828579,0.3014959877,0.346512936, 0.3013398149,0.3007809916}; 139 | const float _aDefaultCalPointsY[_noOfCalibrationPoints] = {0.300092277, 0.3003803475,0.3002205792,0.301004752, 0.3001241394,0.3464200104,0.3001331245,0.3011881186,0.3010685972,0.3606900641,0.3001520488,0.3010662947,0.3008837105,0.3461478452,0.3011732026,0.3007367683,0.3011345742,0.3000566197,0.3006843288,0.3009673425,0.3011228978,0.2547579852,0.3011177285,0.301264851, 0.3002376991,0.2403885431,0.3006540818,0.3010588401,0.3011093054,0.2555000655,0.300080276, 0.3008482317}; 140 | // right up left down up right up left down left down right notch 1 notch 2 notch 3 notch 4 notch 5 notch 6 notch 7 notch 8 141 | const int _calOrder[_noOfCalibrationPoints] = {0, 1, 8, 9, 16, 17, 24, 25, 4, 5, 12, 13, 20, 21, 28, 29, 2, 3, 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31}; 142 | // right notch 1 up right notch 2 up notch 3 up left notch 4 left notch 5 down left notch 6 down notch 7 down right notch 8 143 | // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 144 | const float _notchAngleDefaults[_noOfNotches] = {0, M_PI/8.0, M_PI*2/8.0, M_PI*3/8.0, M_PI*4/8.0, M_PI*5/8.0, M_PI*6/8.0, M_PI*7/8.0, M_PI*8/8.0, M_PI*9/8.0, M_PI*10/8.0, M_PI*11/8.0, M_PI*12/8.0, M_PI*13/8.0, M_PI*14/8.0, M_PI*15/8.0}; 145 | //const float _notchRange[_noOfNotches] = {0, M_PI*1/16.0, M_PI/16.0, M_PI*1/16.0, 0, M_PI*1/16.0, M_PI/16.0, M_PI*1/16.0, 0, M_PI*1/16.0, M_PI/16.0, M_PI*1/16.0, 0, M_PI*1/16.0, M_PI/16.0, M_PI*1/16.0}; 146 | const float _notchAdjustStretchLimit = 0.3; 147 | float _aNotchAngles[_noOfNotches] = {0, M_PI/8.0, M_PI*2/8.0, M_PI*3/8.0, M_PI*4/8.0, M_PI*5/8.0, M_PI*6/8.0, M_PI*7/8.0, M_PI*8/8.0, M_PI*9/8.0, M_PI*10/8.0, M_PI*11/8.0, M_PI*12/8.0, M_PI*13/8.0, M_PI*14/8.0, M_PI*15/8.0}; 148 | float _measuredNotchAngles[_noOfNotches]; 149 | const int _notchStatusDefaults[_noOfNotches] = {3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1}; 150 | int _aNotchStatus[_noOfNotches] = {3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1}; 151 | int _cNotchStatus[_noOfNotches] = {3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1, 3, 1, 2, 1}; 152 | float _cNotchAngles[_noOfNotches]; 153 | // up right up left down left down right notch 1 notch 2 notch 3 notch 4 notch 5 notch 6 notch 7 notch 8 154 | const int _notchAdjOrder[_noOfAdjNotches] = {2, 6, 10, 14, 1, 3, 5, 7, 9, 11, 13, 15}; 155 | const int _cardinalNotch = 3; 156 | const int _secondaryNotch = 2; 157 | const int _tertiaryNotchActive = 1; 158 | const int _tertiaryNotchInactive = 0; 159 | const int _fitOrder = 3; //fit order used in the linearization step 160 | float _aFitCoeffsX[_fitOrder+1]; //coefficients for linearizing the X axis of the a-stick 161 | float _aFitCoeffsY[_fitOrder+1]; //coefficients for linearizing the Y axis of the a-stick 162 | float _cFitCoeffsX[_fitOrder+1]; //coefficients for linearizing the Y axis of the c-stick 163 | float _cFitCoeffsY[_fitOrder+1]; //coefficients for linearizing the Y axis of the c-stick 164 | float _aAffineCoeffs[_noOfNotches][6]; //affine transformation coefficients for all regions of the a-stick 165 | float _cAffineCoeffs[_noOfNotches][6]; //affine transformation coefficients for all regions of the c-stick 166 | float _aBoundaryAngles[_noOfNotches]; //angles at the boundaries between regions of the a-stick 167 | float _cBoundaryAngles[_noOfNotches]; //angles at the boundaries between regions of the c-stick 168 | float _tempCalPointsX[(_noOfNotches)*2]; //temporary storage for the x coordinate points collected during calibration before the are cleaned and put into _cleanedPointsX 169 | float _tempCalPointsY[(_noOfNotches)*2]; //temporary storage for the y coordinate points collected during calibration before the are cleaned and put into _cleanedPointsY 170 | 171 | //index values to store data into eeprom 172 | const int _bytesPerFloat = 4; 173 | const int _eepromAPointsX = 0; 174 | const int _eepromAPointsY = _eepromAPointsX+_noOfCalibrationPoints*_bytesPerFloat; 175 | const int _eepromCPointsX = _eepromAPointsY+_noOfCalibrationPoints*_bytesPerFloat; 176 | const int _eepromCPointsY = _eepromCPointsX+_noOfCalibrationPoints*_bytesPerFloat; 177 | const int _eepromxVelDamp = _eepromCPointsY+_noOfCalibrationPoints*_bytesPerFloat; 178 | const int _eepromyVelDamp = _eepromxVelDamp+_bytesPerFloat; 179 | const int _eepromJump = _eepromyVelDamp+_bytesPerFloat; 180 | const int _eepromANotchAngles = _eepromJump+_bytesPerFloat; 181 | const int _eepromCNotchAngles = _eepromANotchAngles+_noOfNotches*_bytesPerFloat; 182 | const int _eepromLToggle = _eepromCNotchAngles+_noOfNotches*_bytesPerFloat; 183 | const int _eepromRToggle = _eepromLToggle+_bytesPerFloat; 184 | const int _eepromcXOffset = _eepromRToggle+_bytesPerFloat; 185 | const int _eepromcYOffset = _eepromcXOffset+_bytesPerFloat; 186 | const int _eepromxSmoothing = _eepromcYOffset+_bytesPerFloat; 187 | const int _eepromySmoothing = _eepromxSmoothing+_bytesPerFloat; 188 | const int _eepromLOffset = _eepromySmoothing+_bytesPerFloat; 189 | const int _eepromROffset = _eepromLOffset+_bytesPerFloat; 190 | const int _eepromCxSmoothing = _eepromROffset+_bytesPerFloat; 191 | const int _eepromCySmoothing = _eepromCxSmoothing+_bytesPerFloat; 192 | 193 | Bounce bounceDr = Bounce(); 194 | Bounce bounceDu = Bounce(); 195 | Bounce bounceDl = Bounce(); 196 | Bounce bounceDd = Bounce(); 197 | 198 | ADC *adc = new ADC(); 199 | 200 | union Buttons{ 201 | uint8_t arr[10]; 202 | struct { 203 | 204 | // byte 0 205 | uint8_t A : 1; 206 | uint8_t B : 1; 207 | uint8_t X : 1; 208 | uint8_t Y : 1; 209 | uint8_t S : 1; 210 | uint8_t orig : 1; 211 | uint8_t errL : 1; 212 | uint8_t errS : 1; 213 | 214 | // byte 1 215 | uint8_t Dl : 1; 216 | uint8_t Dr : 1; 217 | uint8_t Dd : 1; 218 | uint8_t Du : 1; 219 | uint8_t Z : 1; 220 | uint8_t R : 1; 221 | uint8_t L : 1; 222 | uint8_t high : 1; 223 | 224 | //byte 2-7 225 | uint8_t Ax : 8; 226 | uint8_t Ay : 8; 227 | uint8_t Cx : 8; 228 | uint8_t Cy : 8; 229 | uint8_t La : 8; 230 | uint8_t Ra : 8; 231 | 232 | // magic byte 8 & 9 (only used in origin cmd) 233 | // have something to do with rumble motor status??? 234 | // ignore these, they are magic numbers needed 235 | // to make a cmd response work 236 | uint8_t magic1 : 8; 237 | uint8_t magic2 : 8; 238 | }; 239 | }btn; 240 | 241 | uint8_t hardwareL; 242 | uint8_t hardwareR; 243 | uint8_t hardwareZ; 244 | uint8_t hardwareX; 245 | uint8_t hardwareY; 246 | 247 | float _aStickX; 248 | float _posALastX; 249 | float _aStickY; 250 | float _posALastY; 251 | float _cStickX; 252 | float _cStickY; 253 | 254 | 255 | 256 | unsigned int _lastMicros; 257 | float _dT; 258 | bool _running = false; 259 | 260 | //The median filter can be either length 3, 4, or 5. 261 | #define MEDIANLEN 5 262 | //Or just comment this define to disable it entirely. 263 | //#define USEMEDIAN 264 | float _xPosList[MEDIANLEN];//for median filtering 265 | float _yPosList[MEDIANLEN];//for median filtering 266 | unsigned int _xMedianIndex; 267 | unsigned int _yMedianIndex; 268 | 269 | //new kalman filter state variables 270 | float _xPos;//input of kalman filter 271 | float _yPos;//input of kalman filter 272 | float _xPosFilt;//output of kalman filter 273 | float _yPosFilt;//output of kalman filter 274 | float _xVel; 275 | float _yVel; 276 | float _xVelFilt; 277 | float _yVelFilt; 278 | //simple low pass filter state variable for c-stick 279 | float _cXPos; 280 | float _cYPos; 281 | 282 | #ifdef TEENSY3_2 283 | #define CMD_LENGTH_SHORT 5 284 | #define CMD_LENGTH_LONG 13 285 | #define PROBE_LENGTH 12 286 | #define ORIGIN_LENGTH 40 287 | #define POLL_LENGTH 32 288 | 289 | ////Serial bitbanging settings 290 | const int _fastBaud = 1250000; 291 | //const int _slowBaud = 1000000; 292 | const int _slowBaud = 1000000; 293 | const int _fastDivider = (((F_CPU * 2) + ((_fastBaud) >> 1)) / (_fastBaud)); 294 | const int _slowDivider = (((F_CPU * 2) + ((_slowBaud) >> 1)) / (_slowBaud)); 295 | const int _fastBDH = (_fastDivider >> 13) & 0x1F; 296 | const int _slowBDH = (_slowDivider >> 13) & 0x1F; 297 | const int _fastBDL = (_fastDivider >> 5) & 0xFF; 298 | const int _slowBDL = (_slowDivider >> 5) & 0xFF; 299 | const int _fastC4 = _fastDivider & 0x1F; 300 | const int _slowC4 = _slowDivider & 0x1F; 301 | volatile int _writeQueue = 0; 302 | 303 | const char _probeResponse[PROBE_LENGTH] = { 304 | 0x08,0x08,0x0F,0xE8, 305 | 0x08,0x08,0x08,0x08, 306 | 0x08,0x08,0x08,0xEF}; 307 | volatile char _commResponse[ORIGIN_LENGTH] = { 308 | 0x08,0x08,0x08,0x08, 309 | 0x0F,0x08,0x08,0x08, 310 | 0xE8,0xEF,0xEF,0xEF, 311 | 0xE8,0xEF,0xEF,0xEF, 312 | 0xE8,0xEF,0xEF,0xEF, 313 | 0xE8,0xEF,0xEF,0xEF, 314 | 0x08,0xEF,0xEF,0x08, 315 | 0x08,0xEF,0xEF,0x08, 316 | 0x08,0x08,0x08,0x08, 317 | 0x08,0x08,0x08,0x08}; 318 | 319 | int cmd[CMD_LENGTH_LONG]; 320 | int cmdByte; 321 | volatile char _bitCount = 0; 322 | volatile bool _probe = false; 323 | volatile bool _pole = false; 324 | volatile int _commStatus = 0; 325 | static int _commIdle = 0; 326 | static int _commRead = 1; 327 | static int _commPoll = 2; 328 | static int _commWrite = 3; 329 | #endif // TEENSY3_2 330 | 331 | #ifdef TEENSY4_0 332 | ////Serial settings 333 | bool _writing = false; 334 | bool _waiting = false; 335 | int _bitQueue = 8; 336 | int _waitQueue = 0; 337 | int _writeQueue = 0; 338 | uint8_t _cmdByte = 0; 339 | const int _fastBaud = 2500000; 340 | const int _slowBaud = 2000000; 341 | const int _probeLength = 24; 342 | const int _originLength = 80; 343 | const int _pollLength = 64; 344 | static char _serialBuffer[128]; 345 | int _errorCount = 0; 346 | int _reportCount = 0; 347 | 348 | const char _probeResponse[_probeLength] = { 349 | 0,0,0,0, 1,0,0,1, 350 | 0,0,0,0, 0,0,0,0, 351 | 0,0,0,0, 0,0,1,1}; 352 | volatile char _commResponse[_originLength] = { 353 | 0,0,0,0,0,0,0,0, 354 | 0,0,0,0,0,0,0,0, 355 | 0,1,1,1,1,1,1,1, 356 | 0,1,1,1,1,1,1,1, 357 | 0,1,1,1,1,1,1,1, 358 | 0,1,1,1,1,1,1,1, 359 | 0,0,0,0,0,0,0,0, 360 | 0,0,0,0,0,0,0,0, 361 | 0,0,0,0,0,0,0,0, 362 | 0,0,0,0,0,0,0,0}; 363 | #endif // TEENSY4_0 364 | 365 | void setup() { 366 | serialSetup(); 367 | #ifdef BUILD_RELEASE 368 | Serial.println("Software version 0.20"); 369 | #endif 370 | #ifdef BUILD_DEV 371 | Serial.println("This is not a stable version"); 372 | #endif 373 | delay(1000); 374 | 375 | readEEPROM(); 376 | 377 | //set some of the unused values in the message response 378 | btn.errS = 0; 379 | btn.errL = 0; 380 | btn.orig = 0; 381 | btn.high = 1; 382 | 383 | _currentCalStep = _notCalibrating; 384 | 385 | for (int i = 0; i < MEDIANLEN; i++){ 386 | _xPosList[i] = 0; 387 | _yPosList[i] = 0; 388 | } 389 | _xMedianIndex = 0; 390 | _yMedianIndex = 0; 391 | 392 | _xPos = 0; 393 | _yPos = 0; 394 | _xPosFilt = 0; 395 | _yPosFilt = 0; 396 | _xVel = 0; 397 | _yVel = 0; 398 | _xVelFilt = 0; 399 | _yVelFilt = 0; 400 | _cXPos = 0; 401 | _cYPos = 0; 402 | 403 | _lastMicros = micros(); 404 | 405 | setPinModes(); 406 | 407 | ADCSetup(adc, _ADCScale, _ADCScaleFactor); 408 | 409 | #ifdef TEENSY4_0 410 | attachInterrupt(9, commInt, RISING); 411 | #endif // TEENSY4_0 412 | 413 | //_slowBaud = findFreq(); 414 | //serialFreq = 950000; 415 | //Serial.print("starting hw serial at freq:"); 416 | //Serial.println(_slowBaud); 417 | //start hardware serial 418 | #ifdef TEENSY4_0 419 | Serial2.addMemoryForRead(_serialBuffer,128); 420 | #endif // TEENSY4_0 421 | Serial2.begin(_slowBaud); 422 | //UART1_C2 &= ~UART_C2_RE; 423 | //attach the interrupt which will call the communicate function when the data line transitions from high to low 424 | 425 | #ifdef TEENSY3_2 426 | timer1.begin(communicate); 427 | //timer2.begin(checkCmd); 428 | //timer3.begin(writePole); 429 | digitalWriteFast(12,HIGH); 430 | //ARM_DEMCR |= ARM_DEMCR_TRCENA; 431 | //ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; 432 | attachInterrupt(_pinRX, bitCounter, FALLING); 433 | NVIC_SET_PRIORITY(IRQ_PORTC, 0); 434 | #endif // TEENSY3_2 435 | } 436 | 437 | void loop() { 438 | //check if we should be reporting values yet 439 | if(btn.B && !_running){ 440 | Serial.println("Starting to report values"); 441 | _running=true; 442 | } 443 | 444 | //read the controllers buttons 445 | readButtons(); 446 | 447 | //check to see if we are calibrating 448 | if(_currentCalStep >= 0){ 449 | if(_calAStick){ 450 | if(_currentCalStep >= _noOfCalibrationPoints){//adjust notch angles 451 | adjustNotch(_currentCalStep, _dT, hardwareY, hardwareX, btn.B, true, _measuredNotchAngles, _aNotchAngles, _aNotchStatus); 452 | if(hardwareY || hardwareX || (btn.B)){//only run this if the notch was adjusted 453 | //clean full cal points again, feeding updated angles in 454 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 455 | //linearize again 456 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 457 | //notchCalibrate again to update the affine transform 458 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 459 | } 460 | }else{//just show desired stick position 461 | displayNotch(_currentCalStep, true, _notchAngleDefaults); 462 | } 463 | readSticks(true,false,true); 464 | } 465 | else{ 466 | if(_currentCalStep >= _noOfCalibrationPoints){//adjust notch angles 467 | adjustNotch(_currentCalStep, _dT, hardwareY, hardwareX, btn.B, false, _measuredNotchAngles, _cNotchAngles, _cNotchStatus); 468 | if(hardwareY || hardwareX || (btn.B)){//only run this if the notch was adjusted 469 | //clean full cal points again, feeding updated angles in 470 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 471 | //linearize again 472 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 473 | //notchCalibrate again to update the affine transform 474 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 475 | } 476 | }else{//just show desired stick position 477 | displayNotch(_currentCalStep, false, _notchAngleDefaults); 478 | } 479 | readSticks(false,true,true); 480 | } 481 | } 482 | else{ 483 | //if not calibrating read the sticks normally 484 | readSticks(true,true,_running); 485 | } 486 | } 487 | 488 | #ifdef TEENSY4_0 489 | //commInt() will be called on every rising edge of a pulse that we receive 490 | //we will check if we have the expected amount of serial data yet, if we do we will do something with it, if we don't we will do nothing and wait for the next rising edge to check again 491 | void commInt() { 492 | //check to see if we have the expected amount of data yet 493 | if(Serial2.available() >= _bitQueue){ 494 | //check to see if we have been writing data, if have then we need to clear it and set the serial port back to low speed to be ready to receive the next command 495 | if(_writing){ 496 | //Set pin 13 (LED) low for debugging, if it flickers it means the teensy got stuck here somewhere 497 | digitalWriteFast(13,LOW); 498 | //wait for the stop bit to be read 499 | 500 | while(Serial2.available() <= _bitQueue){} 501 | //check to see if we just reset reportCount to 0, if we have then we will report the data we just sent over to the PC over serial 502 | if(_reportCount == 0){ 503 | char myBuffer[128]; 504 | for(int i = 0; i < _bitQueue+1; i++){ 505 | myBuffer[i] = (Serial2.read() > 0b11110000)+48; 506 | } 507 | //Serial.print("Sent: "); 508 | //Serial.write(myBuffer,_bitQueue+1); 509 | //Serial.println(); 510 | } 511 | 512 | //flush and clear the any remaining data just to be sure 513 | Serial2.flush(); 514 | Serial2.clear(); 515 | 516 | //turn the writing flag off, set the serial port to low speed, and set our expected bit queue to 8 to be ready to receive our next command 517 | _writing = false; 518 | Serial2.begin(2000000); 519 | _bitQueue = 8; 520 | } 521 | //if we are not writing, check to see if we were waiting for a poll command to finish 522 | //if we are, we need to clear the data and send our poll response 523 | else if(_waiting){ 524 | digitalWriteFast(13,LOW); 525 | //wait for the stop bit to be received 526 | while(Serial2.available() <= _bitQueue){} 527 | digitalWriteFast(13,HIGH); 528 | //check to see if we just reset reportCount to 0, if we have then we will report the remainder of the poll response to the PC over serial 529 | if(_reportCount == 0){ 530 | Serial.print("Poll: "); 531 | char myBuffer[128]; 532 | for(int i = 0; i < _bitQueue+1; i++){ 533 | myBuffer[i] = (Serial2.read() > 0b11110000)+48; 534 | } 535 | //Serial.write(myBuffer,_bitQueue+1); 536 | //Serial.println(); 537 | } 538 | 539 | //clear any remaining data 540 | Serial2.clear(); 541 | 542 | //clear any remaining data, set the waiting flag to false, and set the serial port to high speed to be ready to send our poll response 543 | Serial2.clear(); 544 | _waiting = false; 545 | Serial2.begin(2500000); 546 | 547 | //set the writing flag to true, set our expected bit queue to the poll response length -1 (to account for the stop bit) 548 | _writing = true; 549 | _bitQueue = _pollLength; 550 | 551 | //write the poll response 552 | for(int i = 0; i<_pollLength; i++){ 553 | if(_commResponse[i]){ 554 | //short low period = 1 555 | Serial2.write(0b11111100); 556 | } 557 | else{ 558 | //long low period = 0 559 | Serial2.write(0b11000000); 560 | } 561 | } 562 | //write stop bit to indicate end of response 563 | Serial2.write(0b11111100); 564 | } 565 | else{ 566 | //We are not writing a response or waiting for a poll response to finish, so we must have received the start of a new command 567 | //Set pin 13 (LED) low for debugging, if it flickers it means the teensy got stuck here somewhere 568 | digitalWriteFast(13,LOW); 569 | 570 | //increment the report count, will be used to only send a report every 64 commands to not overload the PC serial connection 571 | _reportCount++; 572 | if(_reportCount > 64){ 573 | _reportCount = 0; 574 | } 575 | 576 | //clear the command byte of previous data 577 | _cmdByte = 0; 578 | 579 | //write the new data from the serial buffer into the command byte 580 | for(int i = 0; i<8; i++){ 581 | _cmdByte = (_cmdByte<<1) | (Serial2.read() > 0b11110000); 582 | 583 | } 584 | 585 | //if we just reset reportCount, report the command we received and the number of strange commands we've seen so far over serial 586 | //if(_reportCount==0){ 587 | //Serial.print("Received: "); 588 | //Serial.println(_cmdByte,BIN); 589 | //Serial.print("Error Count:"); 590 | //Serial.println(_errorCount); 591 | //} 592 | 593 | //if the command byte is all 0s it is probe command, we will send a probe response 594 | if(_cmdByte == 0b00000000){ 595 | //wait for the stop bit to be received and clear it 596 | while(!Serial2.available()){} 597 | Serial2.clear(); 598 | 599 | //switch the hardware serial to high speed for sending the response, set the _writing flag to true, and set the expected bit queue length to the probe response length minus 1 (to account for the stop bit) 600 | Serial2.begin(2500000); 601 | _writing = true; 602 | _bitQueue = _probeLength; 603 | 604 | //write the probe response 605 | for(int i = 0; i<_probeLength; i++){ 606 | if(_probeResponse[i]){ 607 | //short low period = 1 608 | Serial2.write(0b11111100); 609 | } 610 | else{ 611 | //long low period = 0 612 | Serial2.write(0b11000000); 613 | } 614 | } 615 | //write stop bit to indicate end of response 616 | Serial2.write(0b11111100); 617 | } 618 | //if the command byte is 01000001 it is an origin command, we will send an origin response 619 | else if(_cmdByte == 0b01000001){ 620 | //wait for the stop bit to be received and clear it 621 | while(!Serial2.available()){} 622 | Serial2.clear(); 623 | 624 | //switch the hardware serial to high speed for sending the response, set the _writing flag to true, and set the expected bit queue length to the origin response length minus 1 (to account for the stop bit) 625 | Serial2.begin(2500000); 626 | _writing = true; 627 | _bitQueue = _originLength; 628 | 629 | //write the origin response 630 | for(int i = 0; i<_originLength; i++){ 631 | if(_commResponse[i]){ 632 | //short low period = 1 633 | Serial2.write(0b11111100); 634 | } 635 | else{ 636 | //long low period = 0 637 | Serial2.write(0b11000000); 638 | } 639 | } 640 | //write stop bit to indicate end of response 641 | Serial2.write(0b11111100); 642 | } 643 | 644 | //if the command byte is 01000000 it is an poll command, we need to wait for the poll command to finish then send our poll response 645 | //to do this we will set our expected bit queue to the remaining length of the poll command, and wait until it is finished 646 | else if(_cmdByte == 0b01000000){ 647 | _waiting = true; 648 | _bitQueue = 16; 649 | setPole(); 650 | } 651 | //if we got something else then something went wrong, print the command we got and increase the error count 652 | else{ 653 | Serial.print("error: "); 654 | Serial.println(_cmdByte,BIN); 655 | _errorCount ++; 656 | 657 | //we don't know for sure what state things are in, so clear, flush, and restart the serial port at low speed to be ready to receive a command 658 | Serial2.clear(); 659 | Serial2.flush(); 660 | Serial2.begin(2000000); 661 | //set our expected bit queue to 8, which will collect the first byte of any command we receive 662 | _bitQueue = 8; 663 | } 664 | } 665 | } 666 | //turn the LED back on to indicate we are not stuck 667 | digitalWriteFast(13,HIGH); 668 | } 669 | #endif // TEENSY4_0 670 | void readEEPROM(){ 671 | //get the jump setting 672 | EEPROM.get(_eepromJump, _jumpConfig); 673 | if(std::isnan(_jumpConfig)){ 674 | _jumpConfig = 0; 675 | } 676 | setJump(_jumpConfig); 677 | 678 | //get the L setting 679 | EEPROM.get(_eepromLToggle, _lConfig); 680 | if(std::isnan(_lConfig)) { 681 | _lConfig = _triggerDefault; 682 | } 683 | 684 | //get the R setting 685 | EEPROM.get(_eepromRToggle, _rConfig); 686 | if(std::isnan(_rConfig)) { 687 | _rConfig = _triggerDefault; 688 | } 689 | 690 | //get the C-stick X offset 691 | EEPROM.get(_eepromcXOffset, _cXOffset); 692 | if(std::isnan(_cXOffset)) { 693 | _cXOffset = 0; 694 | } 695 | if(_cXOffset > _cMax) { 696 | _cXOffset = _cMax; 697 | } else if(_cXOffset < _cMin) { 698 | _cXOffset = _cMin; 699 | } 700 | 701 | //get the C-stick Y offset 702 | EEPROM.get(_eepromcYOffset, _cYOffset); 703 | if(std::isnan(_cYOffset)) { 704 | _cYOffset = 0; 705 | } 706 | if(_cYOffset > _cMax) { 707 | _cYOffset = _cMax; 708 | } else if(_cYOffset < _cMin) { 709 | _cYOffset = _cMin; 710 | } 711 | 712 | //get the x-axis velocity dampening 713 | EEPROM.get(_eepromxVelDamp, _gains.xVelDamp); 714 | Serial.print("the xVelDamp value from eeprom is:"); 715 | Serial.println(_gains.xVelDamp); 716 | if(std::isnan(_gains.xVelDamp)){ 717 | _gains.xVelDamp = _velDampMin; 718 | Serial.print("the xVelDamp value was adjusted to:"); 719 | Serial.println(_gains.xVelDamp); 720 | } 721 | if (_gains.xVelDamp > _velDampMax) { 722 | _gains.xVelDamp = _velDampMax; 723 | } else if (_gains.xVelDamp < _velDampMin) { 724 | _gains.xVelDamp = _velDampMin; 725 | } 726 | 727 | //get the y-axis velocity dampening 728 | EEPROM.get(_eepromyVelDamp, _gains.yVelDamp); 729 | Serial.print("the yVelDamp value from eeprom is:"); 730 | Serial.println(_gains.yVelDamp); 731 | if(std::isnan(_gains.yVelDamp)){ 732 | _gains.yVelDamp = _velDampMin; 733 | Serial.print("the yVelDamp value was adjusted to:"); 734 | Serial.println(_gains.yVelDamp); 735 | } 736 | if (_gains.yVelDamp > _velDampMax) { 737 | _gains.yVelDamp = _velDampMax; 738 | } else if (_gains.yVelDamp < _velDampMin) { 739 | _gains.yVelDamp = _velDampMin; 740 | } 741 | 742 | //get the x-axis smoothing value 743 | EEPROM.get(_eepromxSmoothing, _gains.xSmoothing); 744 | Serial.print("the xSmoothing value from eeprom is:"); 745 | Serial.println(_gains.xSmoothing); 746 | if(std::isnan(_gains.xSmoothing)){ 747 | _gains.xSmoothing = _smoothingMin; 748 | Serial.print("the xSmoothing value was adjusted to:"); 749 | Serial.println(_gains.xSmoothing); 750 | } 751 | if(_gains.xSmoothing > _smoothingMax) { 752 | _gains.xSmoothing = _smoothingMax; 753 | } else if(_gains.xSmoothing < _smoothingMin) { 754 | _gains.xSmoothing = _smoothingMin; 755 | } 756 | 757 | //get the y-axis smoothing value 758 | EEPROM.get(_eepromySmoothing, _gains.ySmoothing); 759 | Serial.print("the ySmoothing value from eeprom is:"); 760 | Serial.println(_gains.ySmoothing); 761 | if(std::isnan(_gains.ySmoothing)){ 762 | _gains.ySmoothing = _smoothingMin; 763 | Serial.print("the ySmoothing value was adjusted to:"); 764 | Serial.println(_gains.ySmoothing); 765 | } 766 | if(_gains.ySmoothing > _smoothingMax) { 767 | _gains.ySmoothing = _smoothingMax; 768 | } else if(_gains.ySmoothing < _smoothingMin) { 769 | _gains.ySmoothing = _smoothingMin; 770 | } 771 | 772 | //get the c-stick x-axis smoothing value 773 | EEPROM.get(_eepromCxSmoothing, _gains.cXSmoothing); 774 | Serial.print("the cXSmoothing value from eeprom is:"); 775 | Serial.println(_gains.cXSmoothing); 776 | if(std::isnan(_gains.cXSmoothing)){ 777 | _gains.cXSmoothing = _smoothingMin; 778 | Serial.print("the cXSmoothing value was adjusted to:"); 779 | Serial.println(_gains.cXSmoothing); 780 | } 781 | if(_gains.cXSmoothing > _smoothingMax) { 782 | _gains.cXSmoothing = _smoothingMax; 783 | } else if(_gains.cXSmoothing < _smoothingMin) { 784 | _gains.cXSmoothing = _smoothingMin; 785 | } 786 | 787 | //get the c-stick y-axis smoothing value 788 | EEPROM.get(_eepromCySmoothing, _gains.cYSmoothing); 789 | Serial.print("the cYSmoothing value from eeprom is:"); 790 | Serial.println(_gains.cYSmoothing); 791 | if(std::isnan(_gains.cYSmoothing)){ 792 | _gains.cYSmoothing = _smoothingMin; 793 | Serial.print("the cYSmoothing value was adjusted to:"); 794 | Serial.println(_gains.cYSmoothing); 795 | } 796 | if(_gains.cYSmoothing > _smoothingMax) { 797 | _gains.cYSmoothing = _smoothingMax; 798 | } else if(_gains.cYSmoothing < _smoothingMin) { 799 | _gains.cYSmoothing = _smoothingMin; 800 | } 801 | 802 | //recompute the intermediate gains used directly by the kalman filter 803 | recomputeGains(); 804 | 805 | //get the L-trigger Offset value 806 | EEPROM.get(_eepromLOffset, _LTriggerOffset); 807 | if(std::isnan(_LTriggerOffset)){ 808 | _LTriggerOffset = _triggerMin; 809 | } 810 | if(_LTriggerOffset > _triggerMax) { 811 | _LTriggerOffset = _triggerMax; 812 | } else if(_LTriggerOffset < _triggerMin) { 813 | _LTriggerOffset = _triggerMin; 814 | } 815 | 816 | //get the R-trigger Offset value 817 | EEPROM.get(_eepromROffset, _RTriggerOffset); 818 | if(std::isnan(_RTriggerOffset)){ 819 | _RTriggerOffset = _triggerMin; 820 | } 821 | if(_RTriggerOffset > _triggerMax) { 822 | _RTriggerOffset = _triggerMax; 823 | } else if(_RTriggerOffset < _triggerMin) { 824 | _RTriggerOffset = _triggerMin; 825 | } 826 | 827 | 828 | //get the calibration points collected during the last A stick calibration 829 | EEPROM.get(_eepromAPointsX, _tempCalPointsX); 830 | EEPROM.get(_eepromAPointsY, _tempCalPointsY); 831 | EEPROM.get(_eepromANotchAngles, _aNotchAngles); 832 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 833 | Serial.println("calibration points cleaned"); 834 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 835 | Serial.println("A stick linearized"); 836 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 837 | //stickCal(_cleanedPointsX,_cleanedPointsY,_aNotchAngles,_aFitCoeffsX,_aFitCoeffsY,_aAffineCoeffs,_aBoundaryAngles); 838 | 839 | //get the calibration points collected during the last A stick calibration 840 | EEPROM.get(_eepromCPointsX, _tempCalPointsX); 841 | EEPROM.get(_eepromCPointsY, _tempCalPointsY); 842 | EEPROM.get(_eepromCNotchAngles, _cNotchAngles); 843 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 844 | Serial.println("calibration points cleaned"); 845 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 846 | Serial.println("C stick linearized"); 847 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 848 | //stickCal(_cleanedPointsX,_cleanedPointsY,_cNotchAngles,_cFitCoeffsX,_cFitCoeffsY,_cAffineCoeffs,_cBoundaryAngles); 849 | } 850 | void resetDefaults(){ 851 | Serial.println("RESETTING ALL DEFAULTS"); 852 | 853 | _jumpConfig = 0; 854 | setJump(_jumpConfig); 855 | EEPROM.put(_eepromJump,_jumpConfig); 856 | 857 | _lConfig = _triggerDefault; 858 | _rConfig = _triggerDefault; 859 | EEPROM.put(_eepromLToggle, _lConfig); 860 | EEPROM.put(_eepromRToggle, _rConfig); 861 | 862 | _cXOffset = 0; 863 | _cYOffset = 0; 864 | EEPROM.put(_eepromcXOffset, _cXOffset); 865 | EEPROM.put(_eepromcYOffset, _cYOffset); 866 | 867 | _gains.xVelDamp = _velDampMin; 868 | EEPROM.put(_eepromxVelDamp,_gains.xVelDamp); 869 | _gains.yVelDamp = _velDampMin; 870 | EEPROM.put(_eepromyVelDamp,_gains.yVelDamp); 871 | 872 | _gains.xSmoothing = _smoothingMin; 873 | EEPROM.put(_eepromxSmoothing, _gains.xSmoothing); 874 | _gains.ySmoothing = _smoothingMin; 875 | EEPROM.put(_eepromySmoothing, _gains.ySmoothing); 876 | 877 | _gains.cXSmoothing = _smoothingMin; 878 | EEPROM.put(_eepromCxSmoothing, _gains.cXSmoothing); 879 | _gains.cYSmoothing = _smoothingMin; 880 | EEPROM.put(_eepromCySmoothing, _gains.cYSmoothing); 881 | //recompute the intermediate gains used directly by the kalman filter 882 | recomputeGains(); 883 | 884 | _LTriggerOffset = _triggerMin; 885 | _RTriggerOffset = _triggerMin; 886 | EEPROM.put(_eepromLOffset, _LTriggerOffset); 887 | EEPROM.put(_eepromROffset, _RTriggerOffset); 888 | 889 | for(int i = 0; i < _noOfNotches; i++){ 890 | _aNotchAngles[i] = _notchAngleDefaults[i]; 891 | _cNotchAngles[i] = _notchAngleDefaults[i]; 892 | } 893 | EEPROM.put(_eepromANotchAngles,_aNotchAngles); 894 | EEPROM.put(_eepromCNotchAngles,_cNotchAngles); 895 | 896 | for(int i = 0; i < _noOfCalibrationPoints; i++){ 897 | _tempCalPointsX[i] = _aDefaultCalPointsX[i]; 898 | _tempCalPointsY[i] = _aDefaultCalPointsY[i]; 899 | } 900 | EEPROM.put(_eepromAPointsX,_tempCalPointsX); 901 | EEPROM.put(_eepromAPointsY,_tempCalPointsY); 902 | 903 | Serial.println("A calibration points stored in EEPROM"); 904 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 905 | Serial.println("A calibration points cleaned"); 906 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 907 | Serial.println("A stick linearized"); 908 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 909 | 910 | for(int i = 0; i < _noOfCalibrationPoints; i++){ 911 | _tempCalPointsX[i] = _cDefaultCalPointsX[i]; 912 | _tempCalPointsY[i] = _cDefaultCalPointsY[i]; 913 | } 914 | EEPROM.put(_eepromCPointsX,_tempCalPointsX); 915 | EEPROM.put(_eepromCPointsY,_tempCalPointsY); 916 | 917 | Serial.println("C calibration points stored in EEPROM"); 918 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 919 | Serial.println("C calibration points cleaned"); 920 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 921 | Serial.println("C stick linearized"); 922 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 923 | 924 | } 925 | void setPinModes(){ 926 | pinMode(_pinL,INPUT_PULLUP); 927 | pinMode(_pinR,INPUT_PULLUP); 928 | pinMode(_pinDr,INPUT_PULLUP); 929 | pinMode(_pinDu,INPUT_PULLUP); 930 | pinMode(_pinDl,INPUT_PULLUP); 931 | pinMode(_pinDd,INPUT_PULLUP); 932 | pinMode(_pinX,INPUT_PULLUP); 933 | pinMode(_pinY,INPUT_PULLUP); 934 | 935 | pinMode(_pinA,INPUT_PULLUP); 936 | pinMode(_pinB,INPUT_PULLUP); 937 | pinMode(_pinZ,INPUT_PULLUP); 938 | pinMode(_pinS,INPUT_PULLUP); 939 | #ifdef TEENSY4_0 940 | pinMode(9, INPUT_PULLUP); 941 | pinMode(13, OUTPUT); 942 | #endif // TEENSY4_0 943 | 944 | bounceDr.attach(_pinDr); 945 | bounceDr.interval(1000); 946 | bounceDu.attach(_pinDu); 947 | bounceDu.interval(1000); 948 | bounceDl.attach(_pinDl); 949 | bounceDl.interval(1000); 950 | bounceDd.attach(_pinDd); 951 | bounceDd.interval(1000); 952 | } 953 | void readButtons(){ 954 | btn.A = !digitalRead(_pinA); 955 | btn.B = !digitalRead(_pinB); 956 | btn.X = !digitalRead(_pinXSwappable); 957 | btn.Y = !digitalRead(_pinYSwappable); 958 | btn.Z = !digitalRead(_pinZSwappable); 959 | btn.S = !digitalRead(_pinS); 960 | btn.Du = !digitalRead(_pinDu); 961 | btn.Dd = !digitalRead(_pinDd); 962 | btn.Dl = !digitalRead(_pinDl); 963 | btn.Dr = !digitalRead(_pinDr); 964 | 965 | switch(_lConfig) { 966 | case 0: //Default Trigger state 967 | btn.L = !digitalRead(_pinL); 968 | break; 969 | case 1: //Digital Only Trigger state 970 | btn.L = !digitalRead(_pinL); 971 | break; 972 | case 2: //Analog Only Trigger state 973 | btn.L = (uint8_t) 0; 974 | break; 975 | /* 976 | case 3: //Trigger Plug Emulation state 977 | btn.L = !digitalRead(_pinL); 978 | break; 979 | case 4: //Digital => Analog Value state 980 | btn.L = (uint8_t) 0; 981 | break; 982 | */ 983 | default: 984 | btn.L = !digitalRead(_pinL); 985 | } 986 | 987 | switch(_rConfig) { 988 | case 0: //Default Trigger state 989 | btn.R = !digitalRead(_pinR); 990 | break; 991 | case 1: //Digital Only Trigger state 992 | btn.R = !digitalRead(_pinR); 993 | break; 994 | case 2: //Analog Only Trigger state 995 | btn.R = (uint8_t) 0; 996 | break; 997 | /* 998 | case 3: //Trigger Plug Emulation state 999 | btn.R = !digitalRead(_pinR); 1000 | break; 1001 | case 4: //Digital => Analog Value state 1002 | btn.R = (uint8_t) 0; 1003 | break; 1004 | */ 1005 | default: 1006 | btn.R = !digitalRead(_pinR); 1007 | } 1008 | 1009 | hardwareL = !digitalRead(_pinL); 1010 | hardwareR = !digitalRead(_pinR); 1011 | hardwareZ = !digitalRead(_pinZ); 1012 | hardwareX = !digitalRead(_pinX); 1013 | hardwareY = !digitalRead(_pinY); 1014 | 1015 | bounceDr.update(); 1016 | bounceDu.update(); 1017 | bounceDl.update(); 1018 | bounceDd.update(); 1019 | 1020 | 1021 | /* Current Commands List 1022 | * Safe Mode: AXY+Start 1023 | * Hard Reset: ABZ+Start 1024 | * Rumble Toggle: 1025 | * 1026 | * Calibration 1027 | * Analog Stick Calibration: AXY+L 1028 | * C-Stick Calibration: AXY+R 1029 | * Advance Calibration: L or R 1030 | * Undo Calibration: Z 1031 | * Skip to Notch Adjustment: Start 1032 | * Notch Adjustment CW/CCW: X/Y 1033 | * Notch Adjustment Reset: B 1034 | * 1035 | * Analog Stick Configuration: 1036 | * Increase/Decrease X-Axis Snapback Filtering: LX+Du/Dd 1037 | * Increase/Decrease Y-Axis Snapback Filtering: LY+Du/Dd 1038 | * Increase/Decrease X-Axis Delay: LA+Du/Dd 1039 | * Increase/Decrease Y-Axis Delay: LB+Du/Dd 1040 | * Show Filtering and Axis Delay: LStart+Dd 1041 | * 1042 | * C-Stick Configuration 1043 | * Increase/Decrease X-Axis Snapback Filtering: RX+Du/Dd 1044 | * Increase/Decrease Y-Axis Snapback Filtering: RY+Du/Dd 1045 | * Increase/Decrease X-Axis Offset: RA+Du/Dd 1046 | * Increase/Decrease Y-Axis Offset: RB+Du/Dd 1047 | * Show Filtering and Axis Offset: RStart+Dd 1048 | * 1049 | * Swap X with Z: XZ+Start 1050 | * Swap Y with Z: YZ+Start 1051 | * Reset Z-Jump: AXY+Z 1052 | * Toggle Analog Slider L: ZL+Start 1053 | * Toggle Analog Slider R: ZR+Start 1054 | * Increase/Decrease L-trigger Offset: ZL+Du/Dd 1055 | * Increase/Decrease R-Trigger Offset: ZR+Du/Dd 1056 | */ 1057 | 1058 | //check the dpad buttons to change the controller settings 1059 | if(!_safeMode && (_currentCalStep == -1)) { 1060 | if(btn.A && hardwareX && hardwareY && btn.S) { //Safe Mode Toggle 1061 | _safeMode = true; 1062 | freezeSticks(4000); 1063 | } else if (btn.A && btn.B && hardwareZ && btn.S) { //Hard Reset 1064 | resetDefaults(); 1065 | freezeSticks(2000); 1066 | } else if (btn.A && hardwareX && hardwareY && hardwareL) { //Analog Calibration 1067 | Serial.println("Calibrating the A stick"); 1068 | _calAStick = true; 1069 | _currentCalStep ++; 1070 | _advanceCal = true; 1071 | freezeSticks(2000); 1072 | } else if (btn.A && hardwareX && hardwareY && hardwareR) { //C-stick Calibration 1073 | Serial.println("Calibrating the C stick"); 1074 | _calAStick = false; 1075 | _currentCalStep ++; 1076 | _advanceCal = true; 1077 | freezeSticks(2000); 1078 | } else if(hardwareL && hardwareX && btn.Du) { //Increase Analog X-Axis Snapback Filtering 1079 | adjustSnapback(true, true, true); 1080 | } else if(hardwareL && hardwareX && btn.Dd) { //Decrease Analog X-Axis Snapback Filtering 1081 | adjustSnapback(true, true, false); 1082 | } else if(hardwareL && hardwareY && btn.Du) { //Increase Analog Y-Axis Snapback Filtering 1083 | adjustSnapback(true, false, true); 1084 | } else if(hardwareL && hardwareY && btn.Dd) { //Decrease Analog Y-Axis Snapback Filtering 1085 | adjustSnapback(true, false, false); 1086 | } else if(hardwareL && btn.A && btn.Du) { //Increase X-axis Delay 1087 | adjustSmoothing(true, true, true); 1088 | } else if(hardwareL && btn.A && btn.Dd) { //Decrease X-axis Delay 1089 | adjustSmoothing(true, true, false); 1090 | } else if(hardwareL && btn.B && btn.Du) { //Increase Y-axis Delay 1091 | adjustSmoothing(true, false, true); 1092 | } else if(hardwareL && btn.B && btn.Dd) { //Decrease Y-axis Delay 1093 | adjustSmoothing(true, false, false); 1094 | } else if(hardwareL && btn.S && btn.Dd) { //Show Current Analog Settings 1095 | showAstickSettings(); 1096 | } else if(hardwareR && hardwareX && btn.Du) { //Increase C-stick X-Axis Snapback Filtering 1097 | adjustCstickSmoothing(true, true, true); 1098 | } else if(hardwareR && hardwareX && btn.Dd) { //Decrease C-stick X-Axis Snapback Filtering 1099 | adjustCstickSmoothing(true, true, false); 1100 | } else if(hardwareR && hardwareY && btn.Du) { //Increase C-stick Y-Axis Snapback Filtering 1101 | adjustCstickSmoothing(true, false, true); 1102 | } else if(hardwareR && hardwareY && btn.Dd) { //Decrease C-stick Y-Axis Snapback Filtering 1103 | adjustCstickSmoothing(true, false, false); 1104 | } else if(hardwareR && btn.A && btn.Du) { //Increase C-stick X Offset 1105 | adjustCstickOffset(true, true, true); 1106 | } else if(hardwareR && btn.A && btn.Dd) { //Decrease C-stick X Offset 1107 | adjustCstickOffset(true, true, false); 1108 | } else if(hardwareR && btn.B && btn.Du) { //Increase C-stick Y Offset 1109 | adjustCstickOffset(true, false, true); 1110 | } else if(hardwareR && btn.B && btn.Dd) { //Decrease C-stick Y Offset 1111 | adjustCstickOffset(true, false, false); 1112 | } else if(hardwareR && btn.S && btn.Dd) { //Show Current C-stick SEttings 1113 | showCstickSettings(); 1114 | } else if(hardwareL && hardwareZ && btn.S) { //Toggle Analog L 1115 | nextTriggerState(_lConfig, true); 1116 | freezeSticks(2000); 1117 | } else if(hardwareR && hardwareZ && btn.S) { //Toggle Analog R 1118 | nextTriggerState(_rConfig, false); 1119 | freezeSticks(2000); 1120 | } else if(hardwareL && hardwareZ && btn.Du) { //Increase L-Trigger Offset 1121 | adjustTriggerOffset(true, true, true); 1122 | } else if(hardwareL && hardwareZ && btn.Dd) { //Decrease L-trigger Offset 1123 | adjustTriggerOffset(true, true, false); 1124 | } else if(hardwareR && hardwareZ && btn.Du) { //Increase R-trigger Offset 1125 | adjustTriggerOffset(true, false, true); 1126 | } else if(hardwareR && hardwareZ && btn.Dd) { //Decrease R-trigger Offset 1127 | adjustTriggerOffset(true, false, false); 1128 | } else if(hardwareX && hardwareZ && btn.S) { //Swap X and Z 1129 | readJumpConfig(true, false); 1130 | freezeSticks(2000); 1131 | } else if(hardwareY && hardwareZ && btn.S) { //Swap Y and Z 1132 | readJumpConfig(false, true); 1133 | freezeSticks(2000); 1134 | } else if(btn.A && hardwareX && hardwareY && hardwareZ) { // Reset X/Y/Z Config 1135 | readJumpConfig(false, false); 1136 | freezeSticks(2000); 1137 | } 1138 | } else if (_currentCalStep == -1) { //Safe Mode Disabled, Lock Settings 1139 | if(btn.A && hardwareX && hardwareY && btn.S) { //Safe Mode Toggle 1140 | _safeMode = false; 1141 | freezeSticks(2000); 1142 | } 1143 | if(hardwareL && hardwareR && btn.A && btn.S) { 1144 | btn.L = (uint8_t) (1); 1145 | btn.R = (uint8_t) (1); 1146 | btn.A = (uint8_t) (1); 1147 | btn.S = (uint8_t) (1); 1148 | } 1149 | } 1150 | 1151 | //Skip stick measurement and go to notch adjust using the start button while calibrating 1152 | if(btn.S && (_currentCalStep >= 0 && _currentCalStep < 32)){ 1153 | _currentCalStep = _noOfCalibrationPoints; 1154 | //Do the same thing we would have done at step 32 had we actually collected the points, but with stored tempCalPoints 1155 | if(!_calAStick){ 1156 | //get the calibration points collected during the last A stick calibration 1157 | EEPROM.get(_eepromCPointsX, _tempCalPointsX); 1158 | EEPROM.get(_eepromCPointsY, _tempCalPointsY); 1159 | EEPROM.get(_eepromCNotchAngles, _cNotchAngles); 1160 | //make temp temp cal points that are missing all tertiary notches so that we get a neutral grid 1161 | float tempCalPointsX[_noOfCalibrationPoints]; 1162 | float tempCalPointsY[_noOfCalibrationPoints]; 1163 | stripCalPoints(_tempCalPointsX, _tempCalPointsY, tempCalPointsX, tempCalPointsY); 1164 | //clean the stripped calibration points, use default angles 1165 | cleanCalPoints(tempCalPointsX, tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1166 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 1167 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 1168 | //apply the calibration to the original measured values including any tertiaries; we don't care about the angles 1169 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1170 | float transformedX[_noOfNotches+1]; 1171 | float transformedY[_noOfNotches+1]; 1172 | transformCalPoints(_cleanedPointsX, _cleanedPointsY, transformedX, transformedY, _cFitCoeffsX, _cFitCoeffsY, _cAffineCoeffs, _cBoundaryAngles); 1173 | //compute the angles for those notches into _measuredNotchAngles, using the default angles for the diagonals 1174 | computeStickAngles(transformedX, transformedY, _measuredNotchAngles); 1175 | //clean full cal points again, feeding those angles in 1176 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _measuredNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1177 | //clear unused notch angles 1178 | cleanNotches(_cNotchAngles, _measuredNotchAngles, _cNotchStatus); 1179 | //clean full cal points again again, feeding those measured angles in for missing tertiary notches 1180 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1181 | //linearize again 1182 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 1183 | //notchCalibrate again 1184 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 1185 | } else if(_calAStick){ 1186 | //get the calibration points collected during the last A stick calibration 1187 | EEPROM.get(_eepromAPointsX, _tempCalPointsX); 1188 | EEPROM.get(_eepromAPointsY, _tempCalPointsY); 1189 | EEPROM.get(_eepromANotchAngles, _aNotchAngles); 1190 | //make temp temp cal points that are missing all tertiary notches so that we get a neutral grid 1191 | float tempCalPointsX[_noOfCalibrationPoints]; 1192 | float tempCalPointsY[_noOfCalibrationPoints]; 1193 | stripCalPoints(_tempCalPointsX, _tempCalPointsY, tempCalPointsX, tempCalPointsY); 1194 | //clean the stripped calibration points, use default angles 1195 | cleanCalPoints(tempCalPointsX, tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1196 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 1197 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 1198 | //apply the calibration to the original measured values including any tertiaries; we don't care about the angles 1199 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1200 | float transformedX[_noOfNotches+1]; 1201 | float transformedY[_noOfNotches+1]; 1202 | transformCalPoints(_cleanedPointsX, _cleanedPointsY, transformedX, transformedY, _aFitCoeffsX, _aFitCoeffsY, _aAffineCoeffs, _aBoundaryAngles); 1203 | //compute the angles for those notches into _measuredNotchAngles, using the default angles for the diagonals 1204 | computeStickAngles(transformedX, transformedY, _measuredNotchAngles); 1205 | //clean full cal points again, feeding those angles in 1206 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _measuredNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1207 | //clear unused notch angles 1208 | cleanNotches(_aNotchAngles, _measuredNotchAngles, _aNotchStatus); 1209 | //clean full cal points again again, feeding those measured angles in for missing tertiary notches 1210 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1211 | //linearize again 1212 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 1213 | //notchCalibrate again 1214 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 1215 | } 1216 | } 1217 | //Undo Calibration using Z-button 1218 | if(hardwareZ && _undoCal && !_undoCalPressed) { 1219 | _undoCalPressed = true; 1220 | if(_currentCalStep % 2 == 0 && _currentCalStep < 32 && _currentCalStep != 0 ) { 1221 | _currentCalStep --; 1222 | _currentCalStep --; 1223 | } else if(_currentCalStep > 32) { 1224 | _currentCalStep --; 1225 | } 1226 | if(!_calAStick){ 1227 | int notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1228 | while((_currentCalStep >= _noOfCalibrationPoints) && (_cNotchStatus[notchIndex] == _tertiaryNotchInactive) && (_currentCalStep < _noOfCalibrationPoints + _noOfAdjNotches)){//this non-diagonal notch was not calibrated 1229 | //skip to the next valid notch 1230 | _currentCalStep--; 1231 | notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1232 | } 1233 | } else if(_calAStick){ 1234 | int notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1235 | while((_currentCalStep >= _noOfCalibrationPoints) && (_aNotchStatus[notchIndex] == _tertiaryNotchInactive) && (_currentCalStep < _noOfCalibrationPoints + _noOfAdjNotches)){//this non-diagonal notch was not calibrated 1236 | //skip to the next valid notch 1237 | _currentCalStep--; 1238 | notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1239 | } 1240 | } 1241 | } else if(!hardwareZ) { 1242 | _undoCalPressed = false; 1243 | } 1244 | 1245 | //Advance Calibration Using L or R triggers 1246 | if((hardwareL || hardwareR) && _advanceCal && !_advanceCalPressed){ 1247 | _advanceCalPressed = true; 1248 | if (!_calAStick){ 1249 | if(_currentCalStep < _noOfCalibrationPoints){//still collecting points 1250 | collectCalPoints(_calAStick, _currentCalStep,_tempCalPointsX,_tempCalPointsY); 1251 | } 1252 | _currentCalStep ++; 1253 | if(_currentCalStep >= 2 && _currentCalStep != _noOfNotches*2) {//don't undo at the beginning of collection or notch adjust 1254 | _undoCal = true; 1255 | } else { 1256 | _undoCal = false; 1257 | } 1258 | if(_currentCalStep == _noOfCalibrationPoints){//done collecting points 1259 | Serial.println("finished collecting the calibration points for the C stick"); 1260 | //make temp temp cal points that are missing all tertiary notches so that we get a neutral grid 1261 | float tempCalPointsX[_noOfCalibrationPoints]; 1262 | float tempCalPointsY[_noOfCalibrationPoints]; 1263 | stripCalPoints(_tempCalPointsX, _tempCalPointsY, tempCalPointsX, tempCalPointsY); 1264 | //clean the stripped calibration points, use default angles 1265 | cleanCalPoints(tempCalPointsX, tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1266 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 1267 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 1268 | //apply the calibration to the original measured values including any tertiaries; we don't care about the angles 1269 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1270 | float transformedX[_noOfNotches+1]; 1271 | float transformedY[_noOfNotches+1]; 1272 | transformCalPoints(_cleanedPointsX, _cleanedPointsY, transformedX, transformedY, _cFitCoeffsX, _cFitCoeffsY, _cAffineCoeffs, _cBoundaryAngles); 1273 | //compute the angles for those notches into _measuredNotchAngles, using the default angles for the diagonals 1274 | computeStickAngles(transformedX, transformedY, _measuredNotchAngles); 1275 | //clean full cal points again, feeding those angles in 1276 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _measuredNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1277 | //clear unused notch angles 1278 | cleanNotches(_cNotchAngles, _measuredNotchAngles, _cNotchStatus); 1279 | //clean full cal points again again, feeding those measured angles in for missing tertiary notches 1280 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1281 | //linearize again 1282 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 1283 | //notchCalibrate again 1284 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 1285 | } 1286 | int notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1287 | while((_currentCalStep >= _noOfCalibrationPoints) && (_cNotchStatus[notchIndex] == _tertiaryNotchInactive) && (_currentCalStep < _noOfCalibrationPoints + _noOfAdjNotches)){//this non-diagonal notch was not calibrated 1288 | //skip to the next valid notch 1289 | _currentCalStep++; 1290 | notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1291 | } 1292 | if(_currentCalStep >= _noOfCalibrationPoints + _noOfAdjNotches){//done adjusting notches 1293 | Serial.println("finished adjusting notches for the C stick"); 1294 | EEPROM.put(_eepromCPointsX,_tempCalPointsX); 1295 | EEPROM.put(_eepromCPointsY,_tempCalPointsY); 1296 | EEPROM.put(_eepromCNotchAngles,_cNotchAngles); 1297 | Serial.println("calibration points stored in EEPROM"); 1298 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _cNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _cNotchStatus); 1299 | Serial.println("calibration points cleaned"); 1300 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _cFitCoeffsX, _cFitCoeffsY); 1301 | Serial.println("C stick linearized"); 1302 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _cAffineCoeffs, _cBoundaryAngles); 1303 | _currentCalStep = -1; 1304 | _advanceCal = false; 1305 | } 1306 | } 1307 | else if (_calAStick){ 1308 | Serial.println("Current step:"); 1309 | Serial.println(_currentCalStep); 1310 | if(_currentCalStep < _noOfCalibrationPoints){//still collecting points 1311 | collectCalPoints(_calAStick, _currentCalStep,_tempCalPointsX,_tempCalPointsY); 1312 | } 1313 | _currentCalStep ++; 1314 | if(_currentCalStep >= 2 && _currentCalStep != _noOfCalibrationPoints) {//don't undo at the beginning of collection or notch adjust 1315 | _undoCal = true; 1316 | } else { 1317 | _undoCal = false; 1318 | } 1319 | if(_currentCalStep == _noOfCalibrationPoints){//done collecting points 1320 | //make temp temp cal points that are missing all tertiary notches so that we get a neutral grid 1321 | float tempCalPointsX[_noOfCalibrationPoints]; 1322 | float tempCalPointsY[_noOfCalibrationPoints]; 1323 | stripCalPoints(_tempCalPointsX, _tempCalPointsY, tempCalPointsX, tempCalPointsY); 1324 | //clean the stripped calibration points, use default angles 1325 | cleanCalPoints(tempCalPointsX, tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1326 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 1327 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 1328 | //apply the calibration to the original measured values including any tertiaries; we don't care about the angles 1329 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _notchAngleDefaults, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1330 | float transformedX[_noOfNotches+1]; 1331 | float transformedY[_noOfNotches+1]; 1332 | transformCalPoints(_cleanedPointsX, _cleanedPointsY, transformedX, transformedY, _aFitCoeffsX, _aFitCoeffsY, _aAffineCoeffs, _aBoundaryAngles); 1333 | //compute the angles for those notches into _measuredNotchAngles, using the default angles for the diagonals 1334 | computeStickAngles(transformedX, transformedY, _measuredNotchAngles); 1335 | //clean full cal points again, feeding those angles in 1336 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _measuredNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1337 | //clear unused notch angles 1338 | cleanNotches(_aNotchAngles, _measuredNotchAngles, _aNotchStatus); 1339 | //clean full cal points again again, feeding those measured angles in for missing tertiary notches 1340 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1341 | //linearize again 1342 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 1343 | //notchCalibrate again 1344 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 1345 | } 1346 | int notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1347 | while((_currentCalStep >= _noOfCalibrationPoints) && (_aNotchStatus[notchIndex] == _tertiaryNotchInactive) && (_currentCalStep < _noOfCalibrationPoints + _noOfAdjNotches)){//this non-diagonal notch was not calibrated 1348 | //skip to the next valid notch 1349 | _currentCalStep++; 1350 | notchIndex = _notchAdjOrder[min(_currentCalStep-_noOfCalibrationPoints, _noOfAdjNotches-1)];//limit this so it doesn't access outside the array bounds 1351 | } 1352 | if(_currentCalStep >= _noOfCalibrationPoints + _noOfAdjNotches){//done adjusting notches 1353 | Serial.println("finished adjusting notches for the A stick"); 1354 | EEPROM.put(_eepromAPointsX,_tempCalPointsX); 1355 | EEPROM.put(_eepromAPointsY,_tempCalPointsY); 1356 | EEPROM.put(_eepromANotchAngles,_aNotchAngles); 1357 | Serial.println("calibration points stored in EEPROM"); 1358 | cleanCalPoints(_tempCalPointsX, _tempCalPointsY, _aNotchAngles, _cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _aNotchStatus); 1359 | Serial.println("calibration points cleaned"); 1360 | linearizeCal(_cleanedPointsX, _cleanedPointsY, _cleanedPointsX, _cleanedPointsY, _aFitCoeffsX, _aFitCoeffsY); 1361 | Serial.println("A stick linearized"); 1362 | notchCalibrate(_cleanedPointsX, _cleanedPointsY, _notchPointsX, _notchPointsY, _noOfNotches, _aAffineCoeffs, _aBoundaryAngles); 1363 | _currentCalStep = -1; 1364 | _advanceCal = false; 1365 | } 1366 | } 1367 | } else if(!(hardwareL || hardwareR)) { 1368 | _advanceCalPressed = false; 1369 | } 1370 | } 1371 | void freezeSticks(int time) { 1372 | btn.Cx = (uint8_t) (255); 1373 | btn.Cy = (uint8_t) (255); 1374 | btn.Ax = (uint8_t) (255); 1375 | btn.Ay = (uint8_t) (255); 1376 | btn.La = (uint8_t) (255 + 60.0); 1377 | btn.Ra = (uint8_t) (255 + 60.0); 1378 | 1379 | btn.A = (uint8_t) 0; 1380 | btn.B = (uint8_t) 0; 1381 | btn.X = (uint8_t) 0; 1382 | btn.Y = (uint8_t) 0; 1383 | btn.L = (uint8_t) 0; 1384 | btn.R = (uint8_t) 0; 1385 | btn.Z = (uint8_t) 0; 1386 | btn.S = (uint8_t) 0; 1387 | 1388 | hardwareL = (uint8_t) 0; 1389 | hardwareR = (uint8_t) 0; 1390 | hardwareX = (uint8_t) 0; 1391 | hardwareY = (uint8_t) 0; 1392 | hardwareZ = (uint8_t) 0; 1393 | 1394 | int startTime = millis(); 1395 | int delta = 0; 1396 | while(delta < time){ 1397 | delta = millis() - startTime; 1398 | } 1399 | } 1400 | void adjustSnapback(bool _change, bool _xAxis, bool _increase){ 1401 | Serial.println("adjusting snapback filtering"); 1402 | if(_xAxis && _increase && _change){ 1403 | _gains.xVelDamp = _gains.xVelDamp*1.2599; 1404 | Serial.print("X filtering increased to:"); 1405 | Serial.println(_gains.xVelDamp); 1406 | } 1407 | else if(_xAxis && !_increase && _change){ 1408 | _gains.xVelDamp = _gains.xVelDamp*0.7937; 1409 | Serial.print("X filtering decreased to:"); 1410 | Serial.println(_gains.xVelDamp); 1411 | } 1412 | if(_gains.xVelDamp > _velDampMax){ 1413 | _gains.xVelDamp = _velDampMax; 1414 | } 1415 | else if(_gains.xVelDamp < _velDampMin){ 1416 | _gains.xVelDamp = _velDampMin; 1417 | } 1418 | 1419 | if(!_xAxis && _increase && _change){ 1420 | _gains.yVelDamp = _gains.yVelDamp*1.2599; 1421 | Serial.print("Y filtering increased to:"); 1422 | Serial.println(_gains.yVelDamp); 1423 | } 1424 | else if(!_xAxis && !_increase && _change){ 1425 | _gains.yVelDamp = _gains.yVelDamp*0.7937; 1426 | Serial.print("Y filtering decreased to:"); 1427 | Serial.println(_gains.yVelDamp); 1428 | } 1429 | if(_gains.yVelDamp >_velDampMax){ 1430 | _gains.yVelDamp = _velDampMax; 1431 | } 1432 | else if(_gains.yVelDamp < _velDampMin){ 1433 | _gains.yVelDamp = _velDampMin; 1434 | } 1435 | 1436 | //recompute the intermediate gains used directly by the kalman filter 1437 | recomputeGains(); 1438 | 1439 | float xVarDisplay = 3 * (log(_gains.xVelDamp / 0.125) / log(2)); 1440 | float yVarDisplay = 3 * (log(_gains.yVelDamp / 0.125) / log(2)); 1441 | 1442 | Serial.println("Var display results"); 1443 | Serial.println(xVarDisplay); 1444 | Serial.println(yVarDisplay); 1445 | 1446 | btn.Cx = (uint8_t) (xVarDisplay + 127.5); 1447 | btn.Cy = (uint8_t) (yVarDisplay + 127.5); 1448 | 1449 | //setPole(); 1450 | 1451 | int startTime = millis(); 1452 | int delta = 0; 1453 | while(delta < 2000){ 1454 | delta = millis() - startTime; 1455 | } 1456 | 1457 | EEPROM.put(_eepromxVelDamp,_gains.xVelDamp); 1458 | EEPROM.put(_eepromyVelDamp,_gains.yVelDamp); 1459 | } 1460 | void adjustSmoothing(bool _change, bool _xAxis, bool _increase) { 1461 | Serial.println("Adjusting Smoothing"); 1462 | if (_xAxis && _increase && _change) { 1463 | _gains.xSmoothing = _gains.xSmoothing + 0.1; 1464 | if(_gains.xSmoothing > _smoothingMax) { 1465 | _gains.xSmoothing = _smoothingMax; 1466 | } 1467 | EEPROM.put(_eepromxSmoothing, _gains.xSmoothing); 1468 | Serial.print("X Smoothing increased to:"); 1469 | Serial.println(_gains.xSmoothing); 1470 | } else if(_xAxis && !_increase && _change) { 1471 | _gains.xSmoothing = _gains.xSmoothing - 0.1; 1472 | if(_gains.xSmoothing < _smoothingMin) { 1473 | _gains.xSmoothing = _smoothingMin; 1474 | } 1475 | EEPROM.put(_eepromxSmoothing, _gains.xSmoothing); 1476 | Serial.print("X Smoothing decreased to:"); 1477 | Serial.println(_gains.xSmoothing); 1478 | } else if(!_xAxis && _increase && _change) { 1479 | _gains.ySmoothing = _gains.ySmoothing + 0.1; 1480 | if (_gains.ySmoothing > _smoothingMax) { 1481 | _gains.ySmoothing = _smoothingMax; 1482 | } 1483 | EEPROM.put(_eepromySmoothing, _gains.ySmoothing); 1484 | Serial.print("Y Smoothing increased to:"); 1485 | Serial.println(_gains.ySmoothing); 1486 | } else if(!_xAxis && !_increase && _change) { 1487 | _gains.ySmoothing = _gains.ySmoothing - 0.1; 1488 | if (_gains.ySmoothing < _smoothingMin) { 1489 | _gains.ySmoothing = _smoothingMin; 1490 | } 1491 | EEPROM.put(_eepromySmoothing, _gains.ySmoothing); 1492 | Serial.print("Y Smoothing decreased to:"); 1493 | Serial.println(_gains.ySmoothing); 1494 | } 1495 | 1496 | //recompute the intermediate gains used directly by the kalman filter 1497 | recomputeGains(); 1498 | 1499 | btn.Cx = (uint8_t) (127.5 + (_gains.xSmoothing * 10)); 1500 | btn.Cy = (uint8_t) (127.5 + (_gains.ySmoothing * 10)); 1501 | 1502 | int startTime = millis(); 1503 | int delta = 0; 1504 | while(delta < 2000){ 1505 | delta = millis() - startTime; 1506 | } 1507 | } 1508 | void showAstickSettings() { 1509 | //Snapback on A-stick 1510 | float xVarDisplay = 3 * (log(_gains.xVelDamp / 0.125) / log(2)); 1511 | float yVarDisplay = 3 * (log(_gains.yVelDamp / 0.125) / log(2)); 1512 | 1513 | btn.Ax = (uint8_t) (xVarDisplay + 127.5); 1514 | btn.Ay = (uint8_t) (yVarDisplay + 127.5); 1515 | 1516 | //Smoothing on C-stick 1517 | btn.Cx = (uint8_t) (127.5 + (_gains.xSmoothing * 10)); 1518 | btn.Cy = (uint8_t) (127.5 + (_gains.ySmoothing * 10)); 1519 | 1520 | int startTime = millis(); 1521 | int delta = 0; 1522 | while(delta < 2000){ 1523 | delta = millis() - startTime; 1524 | } 1525 | } 1526 | void adjustCstickSmoothing(bool _change, bool _xAxis, bool _increase) { 1527 | Serial.println("Adjusting C-Stick Smoothing"); 1528 | if (_xAxis && _increase && _change) { 1529 | _gains.cXSmoothing = _gains.cXSmoothing + 0.1; 1530 | if(_gains.cXSmoothing > _smoothingMax) { 1531 | _gains.cXSmoothing = _smoothingMax; 1532 | } 1533 | EEPROM.put(_eepromCxSmoothing, _gains.cXSmoothing); 1534 | Serial.print("C-Stick X Smoothing increased to:"); 1535 | Serial.println(_gains.cXSmoothing); 1536 | } else if(_xAxis && !_increase && _change) { 1537 | _gains.cXSmoothing = _gains.cXSmoothing - 0.1; 1538 | if(_gains.cXSmoothing < _smoothingMin) { 1539 | _gains.cXSmoothing = _smoothingMin; 1540 | } 1541 | EEPROM.put(_eepromCxSmoothing, _gains.cXSmoothing); 1542 | Serial.print("C-Stick X Smoothing decreased to:"); 1543 | Serial.println(_gains.cXSmoothing); 1544 | } else if(!_xAxis && _increase && _change) { 1545 | _gains.cYSmoothing = _gains.cYSmoothing + 0.1; 1546 | if (_gains.cYSmoothing > _smoothingMax) { 1547 | _gains.cYSmoothing = _smoothingMax; 1548 | } 1549 | EEPROM.put(_eepromCySmoothing, _gains.cYSmoothing); 1550 | Serial.print("C-Stick Y Smoothing increased to:"); 1551 | Serial.println(_gains.cYSmoothing); 1552 | } else if(!_xAxis && !_increase && _change) { 1553 | _gains.cYSmoothing = _gains.cYSmoothing - 0.1; 1554 | if (_gains.cYSmoothing < _smoothingMin) { 1555 | _gains.cYSmoothing = _smoothingMin; 1556 | } 1557 | EEPROM.put(_eepromCySmoothing, _gains.cYSmoothing); 1558 | Serial.print("C-Stick Y Smoothing decreased to:"); 1559 | Serial.println(_gains.cYSmoothing); 1560 | } 1561 | 1562 | //recompute the intermediate gains used directly by the kalman filter 1563 | recomputeGains(); 1564 | 1565 | btn.Cx = (uint8_t) (127.5 + (_gains.cXSmoothing * 10)); 1566 | btn.Cy = (uint8_t) (127.5 + (_gains.cYSmoothing * 10)); 1567 | 1568 | int startTime = millis(); 1569 | int delta = 0; 1570 | while(delta < 2000){ 1571 | delta = millis() - startTime; 1572 | } 1573 | } 1574 | void adjustCstickOffset(bool _change, bool _xAxis, bool _increase) { 1575 | Serial.println("Adjusting C-stick Offset"); 1576 | if(_xAxis && _increase && _change) { 1577 | _cXOffset++; 1578 | if(_cXOffset > _cMax) { 1579 | _cXOffset = _cMax; 1580 | } 1581 | EEPROM.put(_eepromcXOffset, _cXOffset); 1582 | Serial.print("X offset increased to:"); 1583 | Serial.println(_cXOffset); 1584 | } else if(_xAxis && !_increase && _change) { 1585 | _cXOffset--; 1586 | if(_cXOffset < _cMin) { 1587 | _cXOffset = _cMin; 1588 | } 1589 | EEPROM.put(_eepromcXOffset, _cXOffset); 1590 | Serial.print("X offset decreased to:"); 1591 | Serial.println(_cXOffset); 1592 | } else if(!_xAxis && _increase && _change) { 1593 | _cYOffset++; 1594 | if(_cYOffset > _cMax) { 1595 | _cYOffset = _cMax; 1596 | } 1597 | EEPROM.put(_eepromcYOffset, _cYOffset); 1598 | Serial.print("Y offset increased to:"); 1599 | Serial.println(_cYOffset); 1600 | } else if(!_xAxis && !_increase && _change) { 1601 | _cYOffset--; 1602 | if(_cYOffset < _cMin) { 1603 | _cYOffset = _cMin; 1604 | } 1605 | EEPROM.put(_eepromcYOffset, _cYOffset); 1606 | Serial.print("Y offset decreased to:"); 1607 | Serial.println(_cYOffset); 1608 | } 1609 | 1610 | btn.Cx = (uint8_t) (127.5 + _cXOffset); 1611 | btn.Cy = (uint8_t) (127.5 + _cYOffset); 1612 | 1613 | int startTime = millis(); 1614 | int delta = 0; 1615 | while(delta < 2000){ 1616 | delta = millis() - startTime; 1617 | } 1618 | } 1619 | void showCstickSettings() { 1620 | //Snapback/smoothing on A-stick 1621 | btn.Ax = (uint8_t) (127.5 + (_gains.cXSmoothing * 10)); 1622 | btn.Ay = (uint8_t) (127.5 + (_gains.cYSmoothing * 10)); 1623 | 1624 | //Smoothing on C-stick 1625 | btn.Cx = (uint8_t) (127.5 + _cXOffset); 1626 | btn.Cy = (uint8_t) (127.5 + _cYOffset); 1627 | 1628 | int startTime = millis(); 1629 | int delta = 0; 1630 | while(delta < 2000){ 1631 | delta = millis() - startTime; 1632 | } 1633 | } 1634 | void adjustTriggerOffset(bool _change, bool _lTrigger, bool _increase) { 1635 | if(_lTrigger && _increase && _change) { 1636 | _LTriggerOffset++; 1637 | if(_LTriggerOffset > _triggerMax) { 1638 | _LTriggerOffset = _triggerMax; 1639 | } 1640 | } else if(_lTrigger && !_increase && _change) { 1641 | _LTriggerOffset--; 1642 | if(_LTriggerOffset < _triggerMin) { 1643 | _LTriggerOffset = _triggerMin; 1644 | } 1645 | } else if(!_lTrigger && _increase && _change) { 1646 | _RTriggerOffset++; 1647 | if(_RTriggerOffset > _triggerMax) { 1648 | _RTriggerOffset = _triggerMax; 1649 | } 1650 | } else if(!_lTrigger && !_increase && _change) { 1651 | _RTriggerOffset--; 1652 | if(_RTriggerOffset < _triggerMin) { 1653 | _RTriggerOffset = _triggerMin; 1654 | } 1655 | } 1656 | 1657 | EEPROM.put(_eepromLOffset, _LTriggerOffset); 1658 | EEPROM.put(_eepromROffset, _RTriggerOffset); 1659 | 1660 | btn.Cx = (uint8_t) (127.5 + _LTriggerOffset); 1661 | btn.Cy = (uint8_t) (127.5 + _RTriggerOffset); 1662 | 1663 | int startTime = millis(); 1664 | int delta = 0; 1665 | while(delta < 2000){ 1666 | delta = millis() - startTime; 1667 | } 1668 | } 1669 | void readJumpConfig(bool _swapXZ, bool _swapYZ){ 1670 | Serial.print("setting jump to: "); 1671 | if(_swapXZ){ 1672 | _jumpConfig = 1; 1673 | Serial.println("X<->Z"); 1674 | } 1675 | else if(_swapYZ){ 1676 | _jumpConfig = 2; 1677 | Serial.println("Y<->Z"); 1678 | } 1679 | else{ 1680 | Serial.println("normal"); 1681 | _jumpConfig = 0; 1682 | } 1683 | EEPROM.put(_eepromJump,_jumpConfig); 1684 | setJump(_jumpConfig); 1685 | } 1686 | void setJump(int jumpConfig){ 1687 | switch(jumpConfig){ 1688 | case 1: 1689 | _pinZSwappable = _pinX; 1690 | _pinXSwappable = _pinZ; 1691 | _pinYSwappable = _pinY; 1692 | break; 1693 | case 2: 1694 | _pinZSwappable = _pinY; 1695 | _pinXSwappable = _pinX; 1696 | _pinYSwappable = _pinZ; 1697 | break; 1698 | default: 1699 | _pinZSwappable = _pinZ; 1700 | _pinXSwappable = _pinX; 1701 | _pinYSwappable = _pinY; 1702 | } 1703 | } 1704 | void nextTriggerState(int _currentConfig, bool _lTrigger) { 1705 | if(_lTrigger) { 1706 | if(_currentConfig >= 2/*4*/) { 1707 | _lConfig = 0; 1708 | } else { 1709 | _lConfig = _currentConfig + 1; 1710 | } 1711 | } else { 1712 | if(_currentConfig >= 2/*4*/) { 1713 | _rConfig = 0; 1714 | } else { 1715 | _rConfig = _currentConfig + 1; 1716 | } 1717 | } 1718 | EEPROM.put(_eepromLToggle, _lConfig); 1719 | EEPROM.put(_eepromRToggle, _rConfig); 1720 | } 1721 | void readSticks(int readA, int readC, int running){ 1722 | #ifdef USEADCSCALE 1723 | _ADCScale = _ADCScale*0.999 + _ADCScaleFactor/adc->adc1->analogRead(ADC_INTERNAL_SOURCE::VREF_OUT); 1724 | #endif 1725 | // otherwise _ADCScale is 1 1726 | 1727 | //read the analog stick, scale it down so that we don't get huge values when we linearize 1728 | //_aStickX = adc->adc0->analogRead(_pinAx)/4096.0*_ADCScale; 1729 | //_aStickY = adc->adc0->analogRead(_pinAy)/4096.0*_ADCScale; 1730 | 1731 | 1732 | //read the L and R sliders 1733 | switch(_lConfig) { 1734 | case 0: //Default Trigger state 1735 | btn.La = adc->adc0->analogRead(_pinLa)>>4; 1736 | break; 1737 | case 1: //Digital Only Trigger state 1738 | btn.La = (uint8_t) 0; 1739 | break; 1740 | case 2: //Analog Only Trigger state 1741 | btn.La = adc->adc0->analogRead(_pinLa)>>4; 1742 | break; 1743 | /* 1744 | case 3: //Trigger Plug Emulation state 1745 | btn.La = adc->adc0->analogRead(_pinLa)>>4; 1746 | if (btn.La > (((uint8_t) (_LTriggerOffset)) + 60.0)) { 1747 | btn.La = (((uint8_t) (_LTriggerOffset)) + 60.0); 1748 | } 1749 | break; 1750 | case 4: //Digital => Analog Value state 1751 | if(hardwareL) { 1752 | btn.La = (((uint8_t) (_LTriggerOffset)) + 60.0); 1753 | } else { 1754 | btn.La = (uint8_t) 0; 1755 | } 1756 | break; 1757 | */ 1758 | default: 1759 | btn.La = adc->adc0->analogRead(_pinLa)>>4; 1760 | } 1761 | 1762 | switch(_rConfig) { 1763 | case 0: //Default Trigger state 1764 | btn.Ra = adc->adc0->analogRead(_pinRa)>>4; 1765 | break; 1766 | case 1: //Digital Only Trigger state 1767 | btn.Ra = (uint8_t) 0; 1768 | break; 1769 | case 2: //Analog Only Trigger state 1770 | btn.Ra = adc->adc0->analogRead(_pinRa)>>4; 1771 | break; 1772 | /* 1773 | case 3: //Trigger Plug Emulation state 1774 | btn.Ra = adc->adc0->analogRead(_pinRa)>>4; 1775 | if (btn.Ra > (((uint8_t) (_RTriggerOffset)) + 60.0)) { 1776 | btn.Ra = (((uint8_t) (_RTriggerOffset)) + 60.0); 1777 | } 1778 | break; 1779 | case 4: //Digital => Analog Value state 1780 | if(hardwareR) { 1781 | btn.Ra = (((uint8_t) (_RTriggerOffset)) + 60.0); 1782 | } else { 1783 | btn.Ra = (uint8_t) 0; 1784 | } 1785 | break; 1786 | */ 1787 | default: 1788 | btn.Ra = adc->adc0->analogRead(_pinRa)>>4; 1789 | } 1790 | 1791 | //read the c stick, scale it down so that we don't get huge values when we linearize 1792 | //_cStickX = (_cStickX + adc->adc0->analogRead(_pinCx)/4096.0)*0.5; 1793 | //_cStickY = (_cStickY + adc->adc0->analogRead(_pinCy)/4096.0)*0.5; 1794 | 1795 | unsigned int adcCount = 0; 1796 | unsigned int aXSum = 0; 1797 | unsigned int aYSum = 0; 1798 | unsigned int cXSum = 0; 1799 | unsigned int cYSum = 0; 1800 | 1801 | do{ 1802 | adcCount++; 1803 | aXSum += adc->adc0->analogRead(_pinAx); 1804 | aYSum += adc->adc0->analogRead(_pinAy); 1805 | cXSum += adc->adc0->analogRead(_pinCx); 1806 | cYSum += adc->adc0->analogRead(_pinCy); 1807 | } 1808 | while((micros()-_lastMicros) < 1000); 1809 | 1810 | //Serial.println(adcCount); 1811 | _aStickX = aXSum/(float)adcCount/4096.0*_ADCScale; 1812 | _aStickY = aYSum/(float)adcCount/4096.0*_ADCScale; 1813 | _cStickX = (_cStickX + cXSum/(float)adcCount/4096.0)*0.5; 1814 | _cStickY = (_cStickY + cYSum/(float)adcCount/4096.0)*0.5; 1815 | 1816 | _dT = (micros() - _lastMicros)/1000.0; 1817 | _lastMicros = micros(); 1818 | //create the measurement value to be used in the kalman filter 1819 | float xZ; 1820 | float yZ; 1821 | 1822 | //linearize the analog stick inputs by multiplying by the coefficients found during calibration (3rd order fit) 1823 | xZ = linearize(_aStickX,_aFitCoeffsX); 1824 | yZ = linearize(_aStickY,_aFitCoeffsY); 1825 | 1826 | float posCx = linearize(_cStickX,_cFitCoeffsX); 1827 | float posCy = linearize(_cStickY,_cFitCoeffsY); 1828 | 1829 | 1830 | //Run the kalman filter to eliminate snapback 1831 | runKalman(xZ,yZ); 1832 | 1833 | //Run a simple low-pass filter on the C-stick 1834 | float oldCX = _cXPos; 1835 | float oldCY = _cYPos; 1836 | _cXPos = posCx; 1837 | _cYPos = posCy; 1838 | float xWeight1 = _g.cXSmoothing; 1839 | float xWeight2 = 1-xWeight1; 1840 | float yWeight1 = _g.cYSmoothing; 1841 | float yWeight2 = 1-yWeight1; 1842 | 1843 | _cXPos = xWeight1*_cXPos + xWeight2*oldCX; 1844 | _cYPos = yWeight1*_cYPos + yWeight2*oldCY; 1845 | 1846 | posCx = _cXPos; 1847 | posCy = _cYPos; 1848 | 1849 | float posAx = _xPosFilt; 1850 | float posAy = _yPosFilt; 1851 | 1852 | //Run a median filter to reduce noise 1853 | #ifdef USEMEDIAN 1854 | runMedian(posAx, _xPosList, _xMedianIndex); 1855 | runMedian(posAy, _yPosList, _yMedianIndex); 1856 | #endif 1857 | 1858 | notchRemap(posAx, posAy, &posAx, &posAy, _aAffineCoeffs, _aBoundaryAngles,_noOfNotches); 1859 | notchRemap(posCx, posCy, &posCx, &posCy, _cAffineCoeffs, _cBoundaryAngles,_noOfNotches); 1860 | 1861 | //Clamp values from -125 to +125 1862 | posAx = min(125, max(-125, posAx)); 1863 | posAy = min(125, max(-125, posAy)); 1864 | posCx = min(125, max(-125, posCx+_cXOffset)); 1865 | posCy = min(125, max(-125, posCy+_cYOffset)); 1866 | 1867 | float hystVal = 0.3; 1868 | //assign the remapped values to the button struct 1869 | if(_running){ 1870 | if(readA){ 1871 | float diffAx = (posAx+127.5)-btn.Ax; 1872 | if( (diffAx > (1.0 + hystVal)) || (diffAx < -hystVal) ){ 1873 | btn.Ax = (uint8_t) (posAx+127.5); 1874 | } 1875 | float diffAy = (posAy+127.5)-btn.Ay; 1876 | if( (diffAy > (1.0 + hystVal)) || (diffAy < -hystVal) ){ 1877 | btn.Ay = (uint8_t) (posAy+127.5); 1878 | } 1879 | } 1880 | if(readC){ 1881 | float diffCx = (posCx+127.5)-btn.Cx; 1882 | if( (diffCx > (1.0 + hystVal)) || (diffCx < -hystVal) ){ 1883 | btn.Cx = (uint8_t) (posCx+127.5); 1884 | } 1885 | float diffCy = (posCy+127.5)-btn.Cy; 1886 | if( (diffCy > (1.0 + hystVal)) || (diffCy < -hystVal) ){ 1887 | btn.Cy = (uint8_t) (posCy+127.5); 1888 | } 1889 | } 1890 | } 1891 | else 1892 | { 1893 | btn.Ax = (uint8_t) 127;//For some reason, this must be 127 and all other offsets need to be 127.5. 1894 | btn.Ay = (uint8_t) 127;//127 or 128 for everything would make sense (probably 128) but then the stick output 1895 | btn.Cx = (uint8_t) 127;//doesn't reach the cardinals when displaying the cal hints, even though the normal stick position output 1896 | btn.Cy = (uint8_t) 127;//does reach the cardinals. It's fucked up. Even worse, if this is 127.5, it doesn't zero properly on console. 1897 | } 1898 | 1899 | _posALastX = posAx; 1900 | _posALastY = posAy; 1901 | } 1902 | /******************* 1903 | notchRemap 1904 | Remaps the stick position using affine transforms generated from the notch positions 1905 | *******************/ 1906 | void notchRemap(float xIn, float yIn, float* xOut, float* yOut, float affineCoeffs[][6], float regionAngles[], int regions){ 1907 | //determine the angle between the x unit vector and the current position vector 1908 | float angle = atan2f(yIn,xIn); 1909 | 1910 | //unwrap the angle based on the first region boundary 1911 | if(angle < regionAngles[0]){ 1912 | angle += M_PI*2; 1913 | } 1914 | 1915 | //go through the region boundaries from lowest angle to highest, checking if the current position vector is in that region 1916 | //if the region is not found then it must be between the first and the last boundary, ie the last region 1917 | //we check GATE_REGIONS*2 because each notch has its own very small region we use to make notch values more consistent 1918 | //int region = regions*2-1; 1919 | int region = regions-1; 1920 | for(int i = 1; i < regions; i++){ 1921 | if(angle < regionAngles[i]){ 1922 | region = i-1; 1923 | break; 1924 | } 1925 | } 1926 | 1927 | //Apply the affine transformation using the coefficients found during calibration 1928 | *xOut = affineCoeffs[region][0]*xIn + affineCoeffs[region][1]*yIn + affineCoeffs[region][2]; 1929 | *yOut = affineCoeffs[region][3]*xIn + affineCoeffs[region][4]*yIn + affineCoeffs[region][5]; 1930 | 1931 | if((abs(*xOut)<5) && (abs(*yOut)>95)){ 1932 | *xOut = 0; 1933 | } 1934 | if((abs(*yOut)<5) && (abs(*xOut)>95)){ 1935 | *yOut = 0; 1936 | } 1937 | } 1938 | /******************* 1939 | setPole 1940 | takes the values that have been put into the button struct and translates them in the serial commands ready 1941 | to be sent to the gamecube/wii 1942 | *******************/ 1943 | void setPole(){ 1944 | for(int i = 0; i < 8; i++){ 1945 | //write all of the data in the button struct (taken from the dogebawx project, thanks to GoodDoge) 1946 | #ifdef TEENSY3_2 1947 | for(int j = 0; j < 4; j++){ 1948 | //this could probably be done better but we need to take 2 bits at a time to put into one serial byte 1949 | //for details on this read here: http://www.qwertymodo.com/hardware-projects/n64/n64-controller 1950 | int these2bits = (btn.arr[i]>>(6-j*2)) & 3; 1951 | switch(these2bits){ 1952 | case 0: 1953 | _commResponse[(i<<2)+j] = 0x08; 1954 | break; 1955 | case 1: 1956 | _commResponse[(i<<2)+j] = 0xE8; 1957 | break; 1958 | case 2: 1959 | _commResponse[(i<<2)+j] = 0x0F; 1960 | break; 1961 | case 3: 1962 | _commResponse[(i<<2)+j] = 0xEF; 1963 | break; 1964 | } 1965 | } 1966 | #endif // TEENSY3_2 1967 | #ifdef TEENSY4_0 1968 | for(int j = 0; j < 8; j++){ 1969 | _commResponse[i*8+j] = btn.arr[i]>>(7-j) & 1; 1970 | } 1971 | #endif // TEENSY4_0 1972 | } 1973 | } 1974 | 1975 | #ifdef TEENSY3_2 1976 | /******************* 1977 | communicate 1978 | try to communicate with the gamecube/wii 1979 | *******************/ 1980 | void bitCounter(){ 1981 | _bitCount ++; 1982 | //digitalWriteFast(12,!(_bitCount%2)); 1983 | if(_bitCount == 1){ 1984 | timer1.trigger((CMD_LENGTH_SHORT-1)*10); 1985 | _commStatus = _commRead; 1986 | } 1987 | } 1988 | void communicate(){ 1989 | //Serial.println(_commStatus,DEC); 1990 | if(_commStatus == _commRead){ 1991 | //digitalWriteFast(12,LOW); 1992 | //Serial.println(Serial2.available(),DEC); 1993 | while(Serial2.available() < (CMD_LENGTH_SHORT-1)){} 1994 | cmdByte = 0; 1995 | for(int i = 0; i < CMD_LENGTH_SHORT-1; i++){ 1996 | cmd[i] = Serial2.read(); 1997 | //Serial.println(cmd[i],BIN); 1998 | switch(cmd[i]){ 1999 | case 0x08: 2000 | cmdByte = (cmdByte<<2); 2001 | break; 2002 | case 0xE8: 2003 | cmdByte = (cmdByte<<2)+1; 2004 | break; 2005 | case 0xC8: 2006 | cmdByte = (cmdByte<<2)+1; 2007 | break; 2008 | case 0x0F: 2009 | cmdByte = (cmdByte<<2)+2; 2010 | break; 2011 | case 0x0E: 2012 | cmdByte = (cmdByte<<2)+2; 2013 | break; 2014 | case 0xEF: 2015 | cmdByte = (cmdByte<<2)+3; 2016 | break; 2017 | case 0xCF: 2018 | cmdByte = (cmdByte<<2)+3; 2019 | break; 2020 | case 0xEE: 2021 | cmdByte = (cmdByte<<2)+3; 2022 | break; 2023 | case 0xCE: 2024 | cmdByte = (cmdByte<<2)+3; 2025 | break; 2026 | default: 2027 | //got garbage data or a stop bit where it shouldn't be 2028 | Serial.println(cmd[i],BIN); 2029 | cmdByte = -1; 2030 | //Serial.println('o'); 2031 | } 2032 | if(cmdByte == -1){ 2033 | Serial.println('b'); 2034 | break; 2035 | } 2036 | } 2037 | 2038 | //Serial.println(cmdByte,HEX); 2039 | UART1_BDH = _fastBDH; 2040 | UART1_BDL = _fastBDL; 2041 | UART1_C4 = _fastC4; 2042 | UART1_C2 &= ~UART_C2_RE; 2043 | 2044 | switch(cmdByte){ 2045 | case 0x00: 2046 | timer1.trigger(PROBE_LENGTH*8); 2047 | for(int i = 0; i< PROBE_LENGTH; i++){ 2048 | Serial2.write(_probeResponse[i]); 2049 | } 2050 | Serial2.write(0xFF); 2051 | Serial.println("probe"); 2052 | _writeQueue = 9+(PROBE_LENGTH)*2+1; 2053 | _commStatus = _commWrite; 2054 | break; 2055 | case 0x41: 2056 | timer1.trigger(ORIGIN_LENGTH*8); 2057 | for(int i = 0; i< ORIGIN_LENGTH; i++){ 2058 | Serial2.write(_commResponse[i]); 2059 | } 2060 | Serial2.write(0xFF); 2061 | Serial.println("origin"); 2062 | _writeQueue = 9+(ORIGIN_LENGTH)*2+1; 2063 | _commStatus = _commWrite; 2064 | break; 2065 | case 0x40: 2066 | timer1.trigger(56); 2067 | _commStatus = _commPoll; 2068 | setPole(); 2069 | break; 2070 | default: 2071 | //got something strange, try waiting for a stop bit to syncronize 2072 | //resetFreq(); 2073 | digitalWriteFast(12,LOW); 2074 | Serial.println("error"); 2075 | Serial.println(_bitCount,DEC); 2076 | 2077 | UART1_BDH = _slowBDH; 2078 | UART1_BDL = _slowBDL; 2079 | UART1_C4 = _slowC4; 2080 | UART1_C2 |= UART_C2_RE; 2081 | Serial2.clear(); 2082 | 2083 | uint8_t thisbyte = 0; 2084 | while(thisbyte != 0xFF){ 2085 | while(!Serial2.available()); 2086 | thisbyte = Serial2.read(); 2087 | Serial.println(thisbyte,BIN); 2088 | } 2089 | //Serial2.clear(); 2090 | _commStatus = _commIdle; 2091 | _bitCount = 0; 2092 | _writeQueue = 0; 2093 | digitalWriteFast(12,HIGH); 2094 | } 2095 | //digitalWriteFast(12,HIGH); 2096 | } 2097 | else if(_commStatus == _commPoll){ 2098 | digitalWriteFast(12,LOW); 2099 | while(_bitCount<25){} 2100 | //Serial2.write((const char*)_commResponse,POLL_LENGTH); 2101 | for(int i = 0; i< POLL_LENGTH; i++){ 2102 | Serial2.write(_commResponse[i]); 2103 | } 2104 | Serial2.write(0xFF); 2105 | 2106 | timer1.trigger(135); 2107 | _writeQueue = 25+(POLL_LENGTH)*2+1; 2108 | _commStatus = _commWrite; 2109 | //digitalWriteFast(12,HIGH); 2110 | } 2111 | else if(_commStatus == _commWrite){ 2112 | //digitalWriteFast(12,LOW); 2113 | while(_writeQueue > _bitCount){} 2114 | 2115 | UART1_BDH = _slowBDH; 2116 | UART1_BDL = _slowBDL; 2117 | UART1_C4 = _slowC4; 2118 | UART1_C2 |= UART_C2_RE; 2119 | 2120 | _bitCount = 0; 2121 | _commStatus = _commIdle; 2122 | _writeQueue = 0; 2123 | digitalWriteFast(12,HIGH); 2124 | Serial2.clear(); 2125 | } 2126 | else{ 2127 | Serial.println('a'); 2128 | } 2129 | //Serial.println(_commStatus,DEC); 2130 | } 2131 | #endif // TEENSY3_2 2132 | /******************* 2133 | cleanCalPoints 2134 | take the x and y coordinates and notch angles collected during the calibration procedure, 2135 | and generate the cleaned (non-redundant) x and y stick coordinates and the corresponding x and y notch coordinates 2136 | *******************/ 2137 | void cleanCalPoints(const float calPointsX[], const float calPointsY[], const float notchAngles[], float cleanedPointsX[], float cleanedPointsY[], float notchPointsX[], float notchPointsY[], int notchStatus[]){ 2138 | 2139 | Serial.println("The raw calibration points (x,y) are:"); 2140 | for(int i = 0; i< _noOfCalibrationPoints; i++){ 2141 | Serial.print(calPointsX[i]); 2142 | Serial.print(","); 2143 | Serial.println(calPointsY[i]); 2144 | } 2145 | 2146 | Serial.println("The notch angles are:"); 2147 | for(int i = 0; i< _noOfNotches; i++){ 2148 | Serial.println(notchAngles[i]); 2149 | } 2150 | 2151 | notchPointsX[0] = 0; 2152 | notchPointsY[0] = 0; 2153 | cleanedPointsX[0] = 0; 2154 | cleanedPointsY[0] = 0; 2155 | 2156 | Serial.println("The notch points are:"); 2157 | for(int i = 0; i < _noOfNotches; i++){ 2158 | //add the origin values to the first x,y point 2159 | cleanedPointsX[0] += calPointsX[i*2]; 2160 | cleanedPointsY[0] += calPointsY[i*2]; 2161 | 2162 | //copy the cal point into the cleaned list 2163 | cleanedPointsX[i+1] = calPointsX[i*2+1]; 2164 | cleanedPointsY[i+1] = calPointsY[i*2+1]; 2165 | 2166 | //convert notch angles to x/y coords (weird since the stick moves spherically) 2167 | calcStickValues(notchAngles[i], notchPointsX+i+1, notchPointsY+i+1); 2168 | notchPointsX[i+1] = round(notchPointsX[i+1]); 2169 | notchPointsY[i+1] = round(notchPointsY[i+1]); 2170 | 2171 | Serial.print(notchPointsX[i+1]); 2172 | Serial.print(","); 2173 | Serial.println(notchPointsY[i+1]); 2174 | } 2175 | 2176 | //remove the largest and smallest two origin values to remove outliers 2177 | //first, find their indices 2178 | int smallestX = 0; 2179 | int smallX = 0; 2180 | int largeX = 0; 2181 | int largestX = 0; 2182 | int smallestY = 0; 2183 | int smallY = 0; 2184 | int largeY = 0; 2185 | int largestY = 0; 2186 | for (int i = 0; i < _noOfNotches; i++){ 2187 | if (calPointsX[i*2] < calPointsX[smallestX]){//if it's the new smallest 2188 | smallX = smallestX;//shuffle the old smallest to small 2189 | smallestX = i*2;//record the new smallest index 2190 | } else if (calPointsX[i*2] < calPointsX[smallX]){//if it's the new second-smallest 2191 | smallX = i*2;//record the new small index 2192 | } 2193 | if (calPointsX[i*2] > calPointsX[largestX]){//if it's the new largest 2194 | largeX = largestX;//shuffle the old largest to large 2195 | largestX = i*2;//record the new largest index 2196 | } else if (calPointsX[i*2] > calPointsX[largeX]){//if it's the new second-largest 2197 | largeX = i*2;//record the new large index 2198 | } 2199 | if (calPointsY[i*2] < calPointsY[smallestY]){ 2200 | smallY = smallestY; 2201 | smallestY = i*2; 2202 | } else if (calPointsY[i*2] < calPointsY[smallY]){ 2203 | smallY = i*2; 2204 | } 2205 | if (calPointsY[i*2] > calPointsY[largestY]){ 2206 | largeY = largestY; 2207 | largestY = i*2; 2208 | } else if (calPointsY[i*2] > calPointsY[largeY]){ 2209 | largeY = i*2; 2210 | } 2211 | } 2212 | //subtract the smallest and largest values 2213 | cleanedPointsX[0] -= calPointsX[smallestX]; 2214 | cleanedPointsX[0] -= calPointsX[smallX]; 2215 | cleanedPointsX[0] -= calPointsX[largeX]; 2216 | cleanedPointsX[0] -= calPointsX[largestX]; 2217 | cleanedPointsY[0] -= calPointsY[smallestY]; 2218 | cleanedPointsY[0] -= calPointsY[smallY]; 2219 | cleanedPointsY[0] -= calPointsY[largeY]; 2220 | cleanedPointsY[0] -= calPointsY[largestY]; 2221 | 2222 | //divide by the total number of calibration steps/2 to get the average origin value 2223 | //except it's minus 4 steps since we removed outliers 2224 | cleanedPointsX[0] = cleanedPointsX[0]/((float)_noOfNotches-4); 2225 | cleanedPointsY[0] = cleanedPointsY[0]/((float)_noOfNotches-4); 2226 | 2227 | for(int i = 0; i < _noOfNotches; i++){ 2228 | //calculate radius of cleaned point from center 2229 | float deltaX = cleanedPointsX[i+1] - cleanedPointsX[0]; 2230 | float deltaY = cleanedPointsY[i+1] - cleanedPointsY[0]; 2231 | float mag = sqrt(deltaX*deltaX + deltaY*deltaY); 2232 | 2233 | if(mag < 0.02){//if the cleaned point was at the center 2234 | //average the previous and next points (cardinal & diagonal) for some sanity 2235 | //note: this will likely bork if this happens to a cardinal or diagonal 2236 | int prevIndex = ((i-1+_noOfNotches) % _noOfNotches) + 1; 2237 | int nextIndex = ((i+1) % _noOfNotches) + 1; 2238 | 2239 | cleanedPointsX[i+1] = (cleanedPointsX[prevIndex] + cleanedPointsX[nextIndex])/2.0; 2240 | cleanedPointsY[i+1] = (cleanedPointsY[prevIndex] + cleanedPointsY[nextIndex])/2.0; 2241 | 2242 | notchPointsX[i+1] = (notchPointsX[prevIndex] + notchPointsX[nextIndex])/2.0; 2243 | notchPointsY[i+1] = (notchPointsY[prevIndex] + notchPointsY[nextIndex])/2.0; 2244 | 2245 | Serial.print("no input was found for notch: "); 2246 | Serial.println(i+1); 2247 | 2248 | //Mark that notch adjustment should be skipped for this 2249 | notchStatus[i] = _tertiaryNotchInactive; 2250 | }else{ 2251 | notchStatus[i] = _notchStatusDefaults[i]; 2252 | } 2253 | } 2254 | 2255 | Serial.println("The cleaned calibration points are:"); 2256 | for(int i = 0; i< (_noOfNotches+1); i++){ 2257 | Serial.print(cleanedPointsX[i]); 2258 | Serial.print(","); 2259 | Serial.println(cleanedPointsY[i]); 2260 | } 2261 | 2262 | Serial.println("The corresponding notch points are:"); 2263 | for(int i = 0; i< (_noOfNotches+1); i++){ 2264 | Serial.print(notchPointsX[i]); 2265 | Serial.print(","); 2266 | Serial.println(notchPointsY[i]); 2267 | } 2268 | 2269 | Serial.println("The notch statuses are:"); 2270 | for(int i = 0; i< (_noOfNotches); i++){ 2271 | Serial.println(notchStatus[i]); 2272 | } 2273 | } 2274 | //adjustNotch is used to adjust the angles of the notch. 2275 | //It is run after calibration points are collected. 2276 | //The notch adjustment is limited in order to control 2277 | //1. displacement of points (max 12 units out of +/- 100, for now) 2278 | //2. stretching of coordinates (max +/- 30%) 2279 | void adjustNotch(int currentStepIn, float loopDelta, bool CW, bool CCW, bool reset, bool calibratingAStick, float measuredNotchAngles[], float notchAngles[], int notchStatus[]){ 2280 | //This gets run after all the calibration points are collected 2281 | //So we subtract the number of calibration points and switch over to notch adjust order 2282 | const int notchIndex = _notchAdjOrder[currentStepIn-_noOfCalibrationPoints]; 2283 | 2284 | //display the desired value on the other stick 2285 | float x = 0; 2286 | float y = 0; 2287 | calcStickValues(measuredNotchAngles[notchIndex], &x, &y); 2288 | if(calibratingAStick){ 2289 | btn.Cx = (uint8_t) (x + 127.5); 2290 | btn.Cy = (uint8_t) (y + 127.5); 2291 | }else{ 2292 | btn.Ax = (uint8_t) (x + 127.5); 2293 | btn.Ay = (uint8_t) (y + 127.5); 2294 | } 2295 | 2296 | //do nothing if it's not a valid notch to calibrate 2297 | //it'll skip them anyway but just in case 2298 | if(notchStatus[notchIndex] == _tertiaryNotchInactive){ 2299 | return; 2300 | } 2301 | 2302 | //Adjust notch angle according to which button is pressed (do nothing for both buttons) 2303 | if(CW && !CCW){ 2304 | notchAngles[notchIndex] += loopDelta*0.000075; 2305 | }else if(CCW && !CW){ 2306 | notchAngles[notchIndex] -= loopDelta*0.000075; 2307 | }else if(reset){ 2308 | notchAngles[notchIndex] = measuredNotchAngles[notchIndex]; 2309 | }else{ 2310 | return; 2311 | } 2312 | 2313 | //Limit the notch adjustment 2314 | 2315 | //Start out with the limits being 12 units around the circle at the gate 2316 | /*this may be unnecessary in our case, because 12 units is also the 30% stretch limit 2317 | float lowerPosLimit = measuredNotchAngles[notchIndex] - 12/100.f; 2318 | float upperPosLimit = measuredNotchAngles[notchIndex] + 12/100.f; 2319 | if(upperPosLimit < lowerPosLimit){ 2320 | upperPosLimit += 2*M_PI; 2321 | } 2322 | */ 2323 | 2324 | //Now we need to determine the stretch/compression limit 2325 | //Figure out the previous and next notch angles. 2326 | //For most they're the adjacent notches. 2327 | int prevIndex = (notchIndex-1+_noOfNotches) % _noOfNotches; 2328 | int nextIndex = (notchIndex+1) % _noOfNotches; 2329 | //For diagonals, the cardinals are the index points. 2330 | if((notchIndex - 2) % 4 == 0){ 2331 | prevIndex = (notchIndex-2+_noOfNotches) % _noOfNotches; 2332 | nextIndex = (notchIndex+2) % _noOfNotches; 2333 | } 2334 | float prevAngle = notchAngles[prevIndex]; 2335 | float nextAngle = notchAngles[nextIndex]; 2336 | if(nextAngle < prevAngle){ 2337 | nextAngle += 2*M_PI; 2338 | } 2339 | float prevMeasAngle = measuredNotchAngles[prevIndex]; 2340 | float thisMeasAngle = measuredNotchAngles[notchIndex]; 2341 | float nextMeasAngle = measuredNotchAngles[nextIndex]; 2342 | if(nextMeasAngle < thisMeasAngle){ 2343 | nextMeasAngle += 2*M_PI; 2344 | } 2345 | float lowerStretchLimit = max(prevAngle + 0.7*(thisMeasAngle-prevMeasAngle), nextAngle - 1.3*(nextMeasAngle-thisMeasAngle)); 2346 | float upperStretchLimit = min(prevAngle + 1.3*(thisMeasAngle-prevMeasAngle), nextAngle - 0.7*(nextMeasAngle-thisMeasAngle)); 2347 | if(upperStretchLimit < lowerStretchLimit){ 2348 | upperStretchLimit += 2*M_PI; 2349 | } 2350 | 2351 | //Combine the limits 2352 | float lowerLimit = lowerStretchLimit;//max(lowerStretchLimit, lowerPosLimit); 2353 | float upperLimit = upperStretchLimit;//min(upperStretchLimit, upperPosLimit); 2354 | if(upperLimit < lowerLimit){ 2355 | upperLimit += 2*M_PI; 2356 | } 2357 | 2358 | //Apply the limits 2359 | notchAngles[notchIndex] = max(notchAngles[notchIndex], lowerLimit); 2360 | notchAngles[notchIndex] = min(notchAngles[notchIndex], upperLimit); 2361 | } 2362 | //displayNotch is used in lieu of adjustNotch when doing basic calibration 2363 | void displayNotch(const int currentStepIn, const bool calibratingAStick, const float notchAngles[]){ 2364 | int currentStep = _calOrder[currentStepIn]; 2365 | //display the desired value on the other stick 2366 | float x = 0; 2367 | float y = 0; 2368 | if(currentStep%2){ 2369 | const int notchIndex = currentStep/2; 2370 | calcStickValues(notchAngles[notchIndex], &x, &y); 2371 | } 2372 | if(calibratingAStick){ 2373 | btn.Cx = (uint8_t) (x + 127.5); 2374 | btn.Cy = (uint8_t) (y + 127.5); 2375 | }else{ 2376 | btn.Ax = (uint8_t) (x + 127.5); 2377 | btn.Ay = (uint8_t) (y + 127.5); 2378 | } 2379 | } 2380 | void collectCalPoints(bool aStick, int currentStepIn, float calPointsX[], float calPointsY[]){ 2381 | Serial.print("Collecting cal point for step: "); 2382 | Serial.println(currentStepIn); 2383 | const int currentStep = _calOrder[currentStepIn]; 2384 | 2385 | Serial.print("Cal point number: "); 2386 | Serial.println(currentStep); 2387 | float X; 2388 | float Y; 2389 | 2390 | for(int j = 0; j < MEDIANLEN; j++){ 2391 | X = 0; 2392 | Y = 0; 2393 | for(int i = 0; i < 128; i++){ 2394 | if(aStick){ 2395 | #ifdef USEADCSCALE 2396 | _ADCScale = _ADCScale*0.999 + _ADCScaleFactor/adc->adc1->analogRead(ADC_INTERNAL_SOURCE::VREF_OUT); 2397 | #endif 2398 | //otherwise _ADCScale is 1 2399 | X += adc->adc0->analogRead(_pinAx)/4096.0*_ADCScale; 2400 | Y += adc->adc0->analogRead(_pinAy)/4096.0*_ADCScale; 2401 | } 2402 | else{ 2403 | X += adc->adc0->analogRead(_pinCx)/4096.0; 2404 | Y += adc->adc0->analogRead(_pinCy)/4096.0; 2405 | } 2406 | } 2407 | X = X/128.0; 2408 | Y = Y/128.0; 2409 | 2410 | #ifdef USEMEDIAN 2411 | runMedian(X, _xPosList, _xMedianIndex); 2412 | runMedian(Y, _yPosList, _yMedianIndex); 2413 | #endif 2414 | } 2415 | 2416 | calPointsX[currentStep] = X; 2417 | calPointsY[currentStep] = Y; 2418 | 2419 | Serial.println("The collected coordinates are: "); 2420 | Serial.println(calPointsX[currentStep],8); 2421 | Serial.println(calPointsY[currentStep],8); 2422 | } 2423 | /******************* 2424 | linearizeCal 2425 | Generate a fit to linearize the stick response. 2426 | Inputs: 2427 | cleaned points X and Y, (must be 17 points for each of these, the first being the center, the others starting at 3 oclock and going around counterclockwise) 2428 | Outputs: 2429 | linearization fit coefficients for X and Y 2430 | *******************/ 2431 | void linearizeCal(float inX[],float inY[],float outX[], float outY[], float fitCoeffsX[], float fitCoeffsY[]){ 2432 | Serial.println("beginning linearization"); 2433 | 2434 | //do the curve fit first 2435 | //generate all the notched/not notched specific cstick values we will need 2436 | 2437 | double fitPointsX[5]; 2438 | double fitPointsY[5]; 2439 | 2440 | fitPointsX[0] = inX[8+1]; //right 2441 | fitPointsX[1] = (inX[6+1] + inX[10+1])/2.0; //right 45 deg 2442 | fitPointsX[2] = inX[0]; //center 2443 | fitPointsX[3] = (inX[2+1] + inX[14+1])/2.0; //left 45 deg 2444 | fitPointsX[4] = inX[0+1]; //left 2445 | 2446 | fitPointsY[0] = inY[12+1]; //down 2447 | fitPointsY[1] = (inY[10+1] + inY[14+1])/2.0;//down 45 deg 2448 | fitPointsY[2] = inY[0]; //center 2449 | fitPointsY[3] = (inY[6+1] + inY[2+1])/2.0; //up 45 deg 2450 | fitPointsY[4] = inY[4+1]; //up 2451 | 2452 | 2453 | //////determine the coefficients needed to linearize the stick 2454 | //create the expected output, what we want our curve to be fit too 2455 | //this is hard coded because it doesn't depend on the notch adjustments 2456 | // -100 -74.246 0 74.246 100, centered around 0-255 2457 | //It's not sin(45 deg) because it's a spherical motion, not planar. 2458 | double x_output[5] = {27.5,53.2537879754,127.5,201.7462120246,227.5}; 2459 | double y_output[5] = {27.5,53.2537879754,127.5,201.7462120246,227.5}; 2460 | 2461 | Serial.println("The fit input points are (x,y):"); 2462 | for(int i = 0; i < 5; i++){ 2463 | Serial.print(fitPointsX[i],8); 2464 | Serial.print(","); 2465 | Serial.println(fitPointsY[i],8); 2466 | } 2467 | 2468 | Serial.println("The corresponding fit output points are (x,y):"); 2469 | for(int i = 0; i < 5; i++){ 2470 | Serial.print(x_output[i]); 2471 | Serial.print(","); 2472 | Serial.println(y_output[i]); 2473 | } 2474 | 2475 | //perform the curve fit, order is 3 2476 | double tempCoeffsX[_fitOrder+1]; 2477 | double tempCoeffsY[_fitOrder+1]; 2478 | 2479 | fitCurve(_fitOrder, 5, fitPointsX, x_output, _fitOrder+1, tempCoeffsX); 2480 | fitCurve(_fitOrder, 5, fitPointsY, y_output, _fitOrder+1, tempCoeffsY); 2481 | 2482 | //write these coefficients to the array that was passed in, this is our first output 2483 | for(int i = 0; i < (_fitOrder+1); i++){ 2484 | fitCoeffsX[i] = tempCoeffsX[i]; 2485 | fitCoeffsY[i] = tempCoeffsY[i]; 2486 | } 2487 | 2488 | //we will now take out the offset, making the range -100 to 100 instead of 28 to 228 2489 | //calculate the offset 2490 | float xZeroError = linearize((float)fitPointsX[2],fitCoeffsX); 2491 | float yZeroError = linearize((float)fitPointsY[2],fitCoeffsY); 2492 | 2493 | //Adjust the fit's constant coefficient so that the stick zero position is 0 2494 | fitCoeffsX[3] = fitCoeffsX[3] - xZeroError; 2495 | fitCoeffsY[3] = fitCoeffsY[3] - yZeroError; 2496 | 2497 | Serial.println("The fit coefficients are are (x,y):"); 2498 | for(int i = 0; i < 4; i++){ 2499 | Serial.print(fitCoeffsX[i]); 2500 | Serial.print(","); 2501 | Serial.println(fitCoeffsY[i]); 2502 | } 2503 | 2504 | Serial.println("The linearized points are:"); 2505 | for(int i = 0; i <= _noOfNotches; i++){ 2506 | outX[i] = linearize(inX[i],fitCoeffsX); 2507 | outY[i] = linearize(inY[i],fitCoeffsY); 2508 | Serial.print(outX[i],8); 2509 | Serial.print(","); 2510 | Serial.println(outY[i],8); 2511 | } 2512 | 2513 | } 2514 | 2515 | void notchCalibrate(float xIn[], float yIn[], float xOut[], float yOut[], int regions, float allAffineCoeffs[][6], float regionAngles[]){ 2516 | for(int i = 1; i <= regions; i++){ 2517 | Serial.print("calibrating region: "); 2518 | Serial.println(i); 2519 | 2520 | MatrixXf pointsIn(3,3); 2521 | 2522 | MatrixXf pointsOut(3,3); 2523 | 2524 | if(i == (regions)){ 2525 | Serial.println("final region"); 2526 | pointsIn << xIn[0],xIn[i],xIn[1], 2527 | yIn[0],yIn[i],yIn[1], 2528 | 1,1,1; 2529 | pointsOut << xOut[0],xOut[i],xOut[1], 2530 | yOut[0],yOut[i],yOut[1], 2531 | 1,1,1; 2532 | } 2533 | else{ 2534 | pointsIn << xIn[0],xIn[i],xIn[i+1], 2535 | yIn[0],yIn[i],yIn[i+1], 2536 | 1,1,1; 2537 | pointsOut << xOut[0],xOut[i],xOut[i+1], 2538 | yOut[0],yOut[i],yOut[i+1], 2539 | 1,1,1; 2540 | } 2541 | 2542 | Serial.println("In points:"); 2543 | print_mtxf(pointsIn); 2544 | Serial.println("Out points:"); 2545 | print_mtxf(pointsOut); 2546 | 2547 | MatrixXf A(3,3); 2548 | 2549 | A = pointsOut*pointsIn.inverse(); 2550 | //A = pointsOut.colPivHouseholderQr().solve(pointsIn); 2551 | 2552 | 2553 | Serial.println("The transform matrix is:"); 2554 | print_mtxf(A); 2555 | 2556 | Serial.println("The affine transform coefficients for this region are:"); 2557 | 2558 | for(int j = 0; j <2;j++){ 2559 | for(int k = 0; k<3;k++){ 2560 | allAffineCoeffs[i-1][j*3+k] = A(j,k); 2561 | Serial.print(allAffineCoeffs[i-1][j*3+k]); 2562 | Serial.print(","); 2563 | } 2564 | } 2565 | 2566 | Serial.println(); 2567 | Serial.println("The angle defining this regions is:"); 2568 | regionAngles[i-1] = atan2f((yIn[i]-yIn[0]),(xIn[i]-xIn[0])); 2569 | //unwrap the angles so that the first has the smallest value 2570 | if(regionAngles[i-1] < regionAngles[0]){ 2571 | regionAngles[i-1] += M_PI*2; 2572 | } 2573 | Serial.println(regionAngles[i-1]); 2574 | } 2575 | } 2576 | float linearize(float point, float coefficients[]){ 2577 | return (coefficients[0]*(point*point*point) + coefficients[1]*(point*point) + coefficients[2]*point + coefficients[3]); 2578 | } 2579 | void runMedian(float &val, float valArray[MEDIANLEN], unsigned int &medianIndex){ 2580 | //takes the value, inserts it into the value array, and then 2581 | // writes the median back to the value 2582 | valArray[medianIndex] = val; 2583 | medianIndex = (medianIndex + 1) % MEDIANLEN; 2584 | 2585 | //We'll hardcode different sort versions according to how long the median is 2586 | //These are derived from RawTherapee's median.h. 2587 | #if MEDIANLEN == 3 2588 | val = max(min(valArray[0], valArray[1]), min(valArray[2], max(valArray[0], valArray[1]))); 2589 | #elif MEDIANLEN == 4 2590 | float maximin = max(min(valArray[0], valArray[1]), min(valArray[2], valArray[3])); 2591 | float minimax = min(max(valArray[0], valArray[1]), max(valArray[2], valArray[3])); 2592 | val = (maximin + minimax) / 2.0f; 2593 | #else //MEDIANLEN == 5 2594 | float tmpArray[MEDIANLEN]; 2595 | float tmp; 2596 | tmp = min(valArray[0], valArray[1]); 2597 | tmpArray[1] = max(valArray[0], valArray[1]); 2598 | tmpArray[0] = tmp; 2599 | tmp = min(valArray[3], valArray[4]); 2600 | tmpArray[4] = max(valArray[3], valArray[4]); 2601 | tmpArray[3] = max(tmpArray[0], tmp); 2602 | tmpArray[1] = min(tmpArray[1], tmpArray[4]); 2603 | tmp = min(tmpArray[1], valArray[2]); 2604 | tmpArray[2] = max(tmpArray[1], valArray[2]); 2605 | tmpArray[1] = tmp; 2606 | tmp = min(tmpArray[2], tmpArray[3]); 2607 | val = max(tmpArray[1], tmp); 2608 | #endif 2609 | } 2610 | void recomputeGains(){ 2611 | //Recompute the intermediate gains used directly by the kalman filter 2612 | //This happens according to the time between loop iterations. 2613 | //Before, this happened every iteration of runKalman, but now 2614 | //the event loop runs at a fixed 1000 Hz 2615 | //Even if it's not *exactly* 1000 Hz, it should be constant enough. 2616 | //Hopefully. 2617 | //So now, this should be called any time _gains gets changed. 2618 | const float timeFactor = 1.0 / 1.2; 2619 | const float timeDivisor = 1.2 / 1.0; 2620 | _g.maxStick = _gains.maxStick*_gains.maxStick;//we actually use the square 2621 | _g.xVelDecay = _gains.xVelDecay * timeFactor; 2622 | _g.yVelDecay = _gains.yVelDecay * timeFactor; 2623 | _g.xVelPosFactor = _gains.xVelPosFactor * timeFactor; 2624 | _g.yVelPosFactor = _gains.yVelPosFactor * timeFactor; 2625 | _g.xVelDamp = _gains.xVelDamp * timeDivisor; 2626 | _g.yVelDamp = _gains.yVelDamp * timeDivisor; 2627 | _g.velThresh = 1/(_gains.velThresh * timeFactor);//slight optimization by using the inverse 2628 | _g.accelThresh = 1/(_gains.accelThresh * timeFactor); 2629 | _g.velThresh = _g.velThresh*_g.velThresh;//square it because it's used squared 2630 | _g.accelThresh = _g.accelThresh*_g.accelThresh; 2631 | _g.xSmoothing = pow(1-_gains.xSmoothing, timeDivisor); 2632 | _g.ySmoothing = pow(1-_gains.ySmoothing, timeDivisor); 2633 | _g.cXSmoothing = pow(1-_gains.cXSmoothing, timeDivisor); 2634 | _g.cYSmoothing = pow(1-_gains.cYSmoothing, timeDivisor); 2635 | } 2636 | void runKalman(const float xZ,const float yZ){ 2637 | //Serial.println("Running Kalman"); 2638 | 2639 | //save previous values of state 2640 | //float _xPos;//input of kalman filter 2641 | //float _yPos;//input of kalman filter 2642 | const float oldXPos = _xPos; 2643 | const float oldYPos = _yPos; 2644 | //float _xPosFilt;//output of kalman filter 2645 | //float _yPosFilt;//output of kalman filter 2646 | const float oldXPosFilt = _xPosFilt; 2647 | const float oldYPosFilt = _yPosFilt; 2648 | //float _xVel; 2649 | //float _yVel; 2650 | const float oldXVel = _xVel; 2651 | const float oldYVel = _yVel; 2652 | //float _xVelFilt; 2653 | //float _yVelFilt; 2654 | const float oldXVelFilt = _xVelFilt; 2655 | const float oldYVelFilt = _yVelFilt; 2656 | 2657 | //compute new (more trivial) state 2658 | _xPos = xZ; 2659 | _yPos = yZ; 2660 | _xVel = _xPos - oldXPos; 2661 | _yVel = _yPos - oldYPos; 2662 | const float xVelSmooth = 0.5*(_xVel + oldXVel); 2663 | const float yVelSmooth = 0.5*(_yVel + oldYVel); 2664 | const float xAccel = _xVel - oldXVel; 2665 | const float yAccel = _yVel - oldYVel; 2666 | const float oldXPosDiff = oldXPos - oldXPosFilt; 2667 | const float oldYPosDiff = oldYPos - oldYPosFilt; 2668 | 2669 | //compute stick position exponents for weights 2670 | const float stickDistance2 = min(_g.maxStick, _xPos*_xPos + _yPos*_yPos)/_g.maxStick;//0-1 2671 | const float stickDistance6 = stickDistance2*stickDistance2*stickDistance2; 2672 | 2673 | //the current velocity weight for the filtered velocity is the stick r^2 2674 | const float xVelWeight1 = _g.xSmoothing*stickDistance2; 2675 | const float xVelWeight2 = 1-xVelWeight1; 2676 | const float yVelWeight1 = _g.ySmoothing*stickDistance2; 2677 | const float yVelWeight2 = 1-yVelWeight1; 2678 | 2679 | //modified velocity to feed into our kalman filter. 2680 | //We don't actually want an accurate model of the velocity, we want to suppress snapback without adding delay 2681 | //term 1: weight current velocity according to r^2 2682 | //term 2: the previous filtered velocity, weighted the opposite and also set to decay 2683 | //term 3: a corrective factor based on the disagreement between real and filtered position 2684 | _xVelFilt = xVelWeight1*_xVel + (1-_g.xVelDecay)*xVelWeight2*oldXVelFilt + _g.xVelPosFactor*oldXPosDiff; 2685 | _yVelFilt = yVelWeight1*_yVel + (1-_g.yVelDecay)*yVelWeight2*oldYVelFilt + _g.yVelPosFactor*oldYPosDiff; 2686 | 2687 | //the current position weight used for the filtered position is whatever is larger of 2688 | // a) 1 minus the sum of the squares of 2689 | // 1) the smoothed velocity divided by the velocity threshold 2690 | // 2) the acceleration divided by the accel threshold 2691 | // b) stick r^6 2692 | //When the stick is moving slowly, we want to weight it highly, in order to achieve 2693 | // quick control for inputs such as tilts. We lock out using both velocity and 2694 | // acceleration in order to rule out snapback. 2695 | //When the stick is near the rim, we also want instant response, and we know snapback 2696 | // doesn't reach the rim. 2697 | const float xPosWeightVelAcc = 1 - min(1, xVelSmooth*xVelSmooth*_g.velThresh + xAccel*xAccel*_g.accelThresh); 2698 | const float xPosWeight1 = _g.xSmoothing*max(xPosWeightVelAcc, stickDistance6); 2699 | const float xPosWeight2 = 1-xPosWeight1; 2700 | const float yPosWeightVelAcc = 1 - min(1, yVelSmooth*yVelSmooth*_g.velThresh + yAccel*yAccel*_g.accelThresh); 2701 | const float yPosWeight1 = _g.ySmoothing*max(yPosWeightVelAcc, stickDistance6); 2702 | const float yPosWeight2 = 1-yPosWeight1; 2703 | 2704 | //In calculating the filtered stick position, we have the following components 2705 | //term 1: current position, weighted according to the above weight 2706 | //term 2: a predicted position based on the filtered velocity and previous filtered position, 2707 | // with the filtered velocity damped, and the overall term weighted inverse of the previous term 2708 | //term 3: the integral error correction term 2709 | _xPosFilt = xPosWeight1*_xPos + 2710 | xPosWeight2*(oldXPosFilt + (1-_g.xVelDamp)*_xVelFilt); 2711 | _yPosFilt = yPosWeight1*_yPos + 2712 | yPosWeight2*(oldYPosFilt + (1-_g.yVelDamp)*_yVelFilt); 2713 | } 2714 | 2715 | 2716 | void print_mtxf(const Eigen::MatrixXf& X){ 2717 | int i, j, nrow, ncol; 2718 | nrow = X.rows(); 2719 | ncol = X.cols(); 2720 | Serial.print("nrow: "); Serial.println(nrow); 2721 | Serial.print("ncol: "); Serial.println(ncol); 2722 | Serial.println(); 2723 | for (i=0; i