├── CAD └── AT-AT_03.zip ├── Code └── 004 │ ├── 004.ino │ ├── Interpolation.ino │ ├── kinematics.ino │ └── thresholdSticks.ino ├── LICENSE └── README.md /CAD/AT-AT_03.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XRobots/ATAT/54db284c522bd2c2240e01b2d8faa60923af284a/CAD/AT-AT_03.zip -------------------------------------------------------------------------------- /Code/004/004.ino: -------------------------------------------------------------------------------- 1 | #include 2 | IBusBM IBus; // IBus object 3 | 4 | // ramp lib 5 | #include 6 | 7 | #include 8 | 9 | Servo servo1; 10 | Servo servo2; 11 | Servo servo3; 12 | 13 | Servo servo4; 14 | Servo servo5; 15 | Servo servo6; 16 | 17 | Servo servo7; 18 | Servo servo8; 19 | Servo servo9; 20 | 21 | Servo servo10; 22 | Servo servo11; 23 | Servo servo12; 24 | 25 | int ch1; 26 | int ch2; 27 | int ch3; 28 | int ch4; 29 | int ch5; 30 | int ch6; 31 | int ch7; 32 | int ch8; 33 | int ch9; 34 | int ch10; 35 | 36 | float RFB; 37 | float RFBa; 38 | float RFBFiltered; 39 | float RLR; 40 | float RLRa; 41 | float RLRFiltered; 42 | float RT; 43 | float RTa; 44 | float RTFiltered; 45 | 46 | float LFB; 47 | float LFBa; 48 | float LFBFiltered; 49 | float LLR; 50 | float LLRa; 51 | float LLRFiltered; 52 | float LT; 53 | float LTa; 54 | float LTFiltered; 55 | 56 | float pos1; 57 | float pos2; 58 | float pos3; 59 | float pos4; 60 | float pos5; 61 | float pos6; 62 | float pos7; 63 | float pos8; 64 | float pos9; 65 | float pos10; 66 | float pos11; 67 | float pos12; 68 | 69 | float pos1Offset; 70 | float pos2Offset; 71 | float pos3Offset; 72 | float pos4Offset; 73 | float pos5Offset; 74 | float pos6Offset; 75 | float pos7Offset; 76 | float pos8Offset; 77 | float pos9Offset; 78 | float pos10Offset; 79 | float pos11Offset; 80 | float pos12Offset; 81 | 82 | float pos1Filtered; 83 | float pos2Filtered; 84 | float pos3Filtered; 85 | float pos4Filtered; 86 | float pos5Filtered; 87 | float pos6Filtered; 88 | float pos7Filtered; 89 | float pos8Filtered; 90 | 91 | float x; 92 | float z; 93 | float za; 94 | float xa; 95 | float y; 96 | 97 | float leg1z; 98 | float leg2z; 99 | float leg3z; 100 | float leg4z; 101 | float leg1x; 102 | float leg2x; 103 | float leg3x; 104 | float leg4x; 105 | int leg1y; 106 | int leg2y; 107 | int leg3y; 108 | int leg4y; 109 | 110 | float stride; 111 | int stride2; 112 | 113 | unsigned long currentMillis; 114 | long previousMillis = 0; // set up timers 115 | long interval = 10; // time constant for timer 116 | 117 | long previousInterpMillis = 0; // set up timers for interpolation 118 | int interpFlag = 0; 119 | 120 | int stepFlag = 0; 121 | long previousStepMillis = 0; // timers for walking states 122 | int stepStartFlag = 0; 123 | int duration; 124 | int timer1; 125 | 126 | int duration2; 127 | int timer2; 128 | 129 | 130 | int stepFlag2 = 0; 131 | long previousStepMillis2 = 0; // timers for walking states 132 | 133 | float walkXPos1; 134 | float walkXPos2; 135 | float walkXPos3; 136 | float walkXPos4; 137 | float walkXPos5; 138 | float walkXPos6; 139 | float walkXPos7; 140 | float walkXPos8; 141 | 142 | float walkYPos1; 143 | float walkYPos2; 144 | float walkYPos3; 145 | float walkYPos4; 146 | 147 | float walkZPos1; 148 | float walkZPos2; 149 | float walkZPos3; 150 | float walkZPos4; 151 | float walkZPos5; 152 | float walkZPos6; 153 | float walkZPos7; 154 | float walkZPos8; 155 | 156 | 157 | class Interpolation { 158 | public: 159 | rampInt myRamp; 160 | int interpolationFlag = 0; 161 | int savedValue; 162 | 163 | int go(int input, int duration) { 164 | 165 | if (input != savedValue) { // check for new data 166 | interpolationFlag = 0; 167 | } 168 | savedValue = input; // bookmark the old value 169 | 170 | if (interpolationFlag == 0) { // only do it once until the flag is reset 171 | myRamp.go(input, duration, LINEAR, ONCEFORWARD); // start interpolation (value to go to, duration) 172 | interpolationFlag = 1; 173 | } 174 | 175 | int output = myRamp.update(); 176 | return output; 177 | } 178 | }; // end of class 179 | 180 | Interpolation interpFRX; // interpolation objects 181 | Interpolation interpFRY; 182 | Interpolation interpFRZ; 183 | 184 | Interpolation interpFLX; // interpolation objects 185 | Interpolation interpFLY; 186 | Interpolation interpFLZ; 187 | 188 | Interpolation interpBRX; // interpolation objects 189 | Interpolation interpBRY; 190 | Interpolation interpBRZ; 191 | 192 | Interpolation interpBLX; // interpolation objects 193 | Interpolation interpBLY; 194 | Interpolation interpBLZ; 195 | 196 | void setup() { 197 | 198 | Serial.begin(115200); 199 | 200 | IBus.begin(Serial3, IBUSBM_NOTIMER); // change to Serial1 or Serial2 port when required 201 | 202 | pinMode(8, INPUT_PULLUP); 203 | pinMode(9, INPUT_PULLUP); 204 | 205 | servo1.attach(22); 206 | servo2.attach(24); 207 | servo3.attach(26); 208 | servo4.attach(28); 209 | servo5.attach(30); 210 | servo6.attach(32); 211 | 212 | servo7.attach(34); 213 | servo8.attach(36); 214 | servo9.attach(38); 215 | servo10.attach(40); 216 | servo11.attach(42); 217 | servo12.attach(44); 218 | 219 | // offsets for procesise home positions 220 | 221 | pos1Offset = 100; 222 | pos2Offset = -80; 223 | pos3Offset = 150; 224 | pos4Offset = -100; 225 | pos5Offset = 0; 226 | pos6Offset = 0; 227 | pos7Offset = 60; 228 | pos8Offset = 0; 229 | pos9Offset = 50; 230 | pos10Offset = -50; 231 | pos11Offset = -50; 232 | pos12Offset = 0; 233 | 234 | } 235 | 236 | void loop() { 237 | 238 | currentMillis = millis(); 239 | if (currentMillis - previousMillis >= 10) { // start timed event 240 | previousMillis = currentMillis; 241 | 242 | IBus.loop(); 243 | 244 | ch1 = IBus.readChannel(0); // get latest value for servo channel 0 245 | ch2 = IBus.readChannel(1); // get latest value for servo channel 1 246 | ch3 = IBus.readChannel(2); // get latest value for servo channel 3 247 | ch4 = IBus.readChannel(3); // get latest value for servo channel 4 248 | ch5 = IBus.readChannel(4); // get latest value for servo channel 5 249 | ch6 = IBus.readChannel(5); // get latest value for servo channel 6 250 | ch7 = IBus.readChannel(6); // get latest value for servo channel 7 251 | ch8 = IBus.readChannel(7); // get latest value for servo channel 8 252 | ch9 = IBus.readChannel(8); // get latest value for servo channel 9 253 | ch10 = IBus.readChannel(9); // get latest value for servo channel 10 254 | 255 | /* 256 | Serial.print(ch1); 257 | Serial.print(" , "); 258 | Serial.print(ch2); 259 | Serial.print(" , "); 260 | Serial.print(ch3); 261 | Serial.print(" , "); 262 | Serial.print(ch4); 263 | Serial.print(" , "); 264 | Serial.print(ch5); 265 | Serial.print(" , "); 266 | Serial.print(ch6); 267 | Serial.print(" , "); 268 | Serial.print(ch7); 269 | Serial.print(" , "); 270 | Serial.print(ch8); 271 | Serial.print(" , "); 272 | Serial.print(ch9); 273 | Serial.print(" , "); 274 | Serial.print(ch10); 275 | Serial.println(); 276 | */ 277 | 278 | LFB = ch1; 279 | RFB = ch4; 280 | RLR = ch2; 281 | RT = ch6; 282 | LLR = ch3; 283 | LT = ch5; 284 | 285 | // threshold sticks 286 | 287 | RFBa = thresholdStick(RFB); 288 | RLRa = thresholdStick(RLR); 289 | RTa = thresholdStick(RT); 290 | LFBa = thresholdStick(LFB); 291 | LLRa = thresholdStick(LLR); 292 | LTa = thresholdStick(LT); 293 | 294 | // filter sticks 295 | 296 | RFBFiltered = filter(RFBa, RFBFiltered,20); 297 | RLRFiltered = filter(RLRa, RLRFiltered,20); 298 | RTFiltered = filter(RTa, RTFiltered,20); 299 | LFBFiltered = filter(LFBa, LFBFiltered,20); 300 | LLRFiltered = filter(LLRa, LLRFiltered,20); 301 | LTFiltered = filter(LTa, LTFiltered,20); 302 | 303 | if (ch7 < 1300) { // kinematics test mode 304 | za = map(LTFiltered,-255,255,240,295); 305 | xa = map(RFBFiltered,-255,255,60,-60); 306 | 307 | z = (float) za/1000; 308 | x = (float) xa/1000; 309 | 310 | //Serial.println(x); 311 | 312 | kinematics(1, x, y ,z ,0, 0); 313 | kinematics(2, x, y ,z ,0, 0); 314 | kinematics(3, x, y ,z ,0, 0); 315 | kinematics(4, x, y ,z ,0, 0); 316 | } 317 | 318 | else if (ch7 > 1400 && ch7 < 1700) { // walking mode 319 | 320 | timer1 = 400; 321 | duration = 400; 322 | 323 | // define walking positions 324 | 325 | stride = RFBFiltered/-230; // sclae stick for stride length. 326 | 327 | walkXPos1 = stride * 0.05; 328 | walkXPos2 = stride * 0; 329 | walkXPos3 = stride * -0.05; 330 | walkXPos4 = stride * -0.025; 331 | walkXPos5 = stride * -0.0125; 332 | walkXPos6 = stride * 0; 333 | walkXPos7 = stride * 0.0125; 334 | walkXPos8 = stride * 0.022; 335 | 336 | walkZPos1 = 0.28; // leg down 337 | walkZPos2 = 0.25; // leg up 338 | walkZPos3 = 0.28; // leg down 339 | walkZPos4 = 0.28; // leg down 340 | walkZPos5 = 0.28; // leg down 341 | walkZPos6 = 0.28; // leg down 342 | walkZPos7 = 0.28; // leg down 343 | walkZPos8 = 0.28; // leg down 344 | 345 | if (stepFlag == 0 && currentMillis - previousStepMillis > timer1) { 346 | previousStepMillis = currentMillis; 347 | stepFlag = 1; 348 | leg1x = walkXPos3; 349 | leg1z = walkZPos3; 350 | leg2x = walkXPos7; 351 | leg2z = walkZPos7; 352 | leg3x = walkXPos5; 353 | leg3z = walkZPos5; 354 | leg4x = walkXPos1; 355 | leg4z = walkZPos1; 356 | } 357 | else if (stepFlag == 1 && currentMillis - previousStepMillis > timer1) { 358 | previousStepMillis = currentMillis; 359 | stepFlag = 2; 360 | leg1x = walkXPos4; 361 | leg1z = walkZPos4; 362 | leg2x = walkXPos8; 363 | leg2z = walkZPos8; 364 | leg3x = walkXPos6; 365 | leg3z = walkZPos6; 366 | leg4x = walkXPos2; 367 | leg4z = walkZPos2; 368 | } 369 | else if (stepFlag == 2 && currentMillis - previousStepMillis > timer1) { 370 | previousStepMillis = currentMillis; 371 | stepFlag = 3; 372 | leg1x = walkXPos5; 373 | leg1z = walkZPos5; 374 | leg2x = walkXPos1; 375 | leg2z = walkZPos1; 376 | leg3x = walkXPos7; 377 | leg3z = walkZPos7; 378 | leg4x = walkXPos3; 379 | leg4z = walkZPos3; 380 | } 381 | else if (stepFlag == 3 && currentMillis - previousStepMillis > timer1) { 382 | previousStepMillis = currentMillis; 383 | stepFlag = 4; 384 | leg1x = walkXPos6; 385 | leg1z = walkZPos6; 386 | leg2x = walkXPos2; 387 | leg2z = walkZPos2; 388 | leg3x = walkXPos8; 389 | leg3z = walkZPos8; 390 | leg4x = walkXPos4; 391 | leg4z = walkZPos4; 392 | } 393 | else if (stepFlag == 4 && currentMillis - previousStepMillis > timer1) { 394 | previousStepMillis = currentMillis; 395 | stepFlag = 5; 396 | leg1x = walkXPos7; 397 | leg1z = walkZPos7; 398 | leg2x = walkXPos3; 399 | leg2z = walkZPos3; 400 | leg3x = walkXPos1; 401 | leg3z = walkZPos1; 402 | leg4x = walkXPos5; 403 | leg4z = walkZPos5; 404 | } 405 | else if (stepFlag == 5 && currentMillis - previousStepMillis > timer1) { 406 | previousStepMillis = currentMillis; 407 | stepFlag = 6; 408 | leg1x = walkXPos8; 409 | leg1z = walkZPos8; 410 | leg2x = walkXPos4; 411 | leg2z = walkZPos4; 412 | leg3x = walkXPos2; 413 | leg3z = walkZPos2; 414 | leg4x = walkXPos6; 415 | leg4z = walkZPos6; 416 | } 417 | else if (stepFlag == 6 && currentMillis - previousStepMillis > timer1) { 418 | previousStepMillis = currentMillis; 419 | stepFlag = 7; 420 | leg1x = walkXPos1; 421 | leg1z = walkZPos1; 422 | leg2x = walkXPos5; 423 | leg2z = walkZPos5; 424 | leg3x = walkXPos3; 425 | leg3z = walkZPos3; 426 | leg4x = walkXPos7; 427 | leg4z = walkZPos7; 428 | } 429 | else if (stepFlag == 7 && currentMillis - previousStepMillis > timer1) { 430 | previousStepMillis = currentMillis; 431 | stepFlag = 0; 432 | leg1x = walkXPos2; 433 | leg1z = walkZPos2; 434 | leg2x = walkXPos6; 435 | leg2z = walkZPos6; 436 | leg3x = walkXPos4; 437 | leg3z = walkZPos4; 438 | leg4x = walkXPos8; 439 | leg4z = walkZPos8; 440 | } 441 | 442 | kinematics(1, leg1x, 0 ,leg1z ,1, duration); 443 | kinematics(2, leg2x, 0 ,leg2z ,1, duration); 444 | kinematics(3, leg3x, 0 ,leg3z ,1, duration); 445 | kinematics(4, leg4x, 0 ,leg4z ,1, duration); 446 | 447 | } 448 | 449 | else if (ch7 > 1700) { 450 | 451 | timer2 = 400; 452 | duration2 = 400; 453 | 454 | stride2 = LTFiltered*-1; // sclae stick for stride length. 455 | 456 | walkZPos1 = 0.28; // leg down 457 | walkZPos2 = 0.25; // leg up 458 | 459 | if (stepFlag2 == 0 && currentMillis - previousStepMillis2 > timer2) { 460 | previousStepMillis2 = currentMillis; 461 | stepFlag2 = 1; 462 | leg1z = walkZPos1; 463 | leg2z = walkZPos2; 464 | leg3z = walkZPos2; 465 | leg4z = walkZPos1; 466 | } 467 | if (stepFlag2 == 1 && currentMillis - previousStepMillis2 > timer2) { 468 | previousStepMillis2 = currentMillis; 469 | stepFlag2 = 2; 470 | leg1y = stride2; 471 | leg2y = stride2*-1; 472 | leg3y = stride2*-1; 473 | leg4y = stride2; 474 | } 475 | else if (stepFlag2 == 2 && currentMillis - previousStepMillis2 > timer2) { 476 | previousStepMillis2 = currentMillis; 477 | stepFlag2 = 3; 478 | leg1z = walkZPos1; 479 | leg2z = walkZPos1; 480 | leg3z = walkZPos1; 481 | leg4z = walkZPos1; 482 | } 483 | else if (stepFlag2 == 3 && currentMillis - previousStepMillis2 > timer2) { 484 | previousStepMillis2 = currentMillis; 485 | stepFlag2 = 4; 486 | leg1z = walkZPos2; 487 | leg2z = walkZPos1; 488 | leg3z = walkZPos1; 489 | leg4z = walkZPos2; 490 | } 491 | else if (stepFlag2 == 4 && currentMillis - previousStepMillis2 > timer2) { 492 | previousStepMillis2 = currentMillis; 493 | stepFlag2 = 5; 494 | leg1y = stride2*-1; 495 | leg2y = stride2; 496 | leg3y = stride2; 497 | leg4y = stride2*-1; 498 | } 499 | else if (stepFlag2 == 5 && currentMillis - previousStepMillis2 > timer2) { 500 | previousStepMillis2 = currentMillis; 501 | stepFlag2 = 0; 502 | leg1z = walkZPos1; 503 | leg2z = walkZPos1; 504 | leg3z = walkZPos1; 505 | leg4z = walkZPos1; 506 | } 507 | 508 | 509 | 510 | //Serial.println(leg1y); 511 | 512 | leg1x = 0; 513 | kinematics(1, leg1x, leg1y ,leg1z ,1, duration2); 514 | kinematics(2, leg2x, leg2y ,leg2z ,1, duration2); 515 | kinematics(3, leg3x, leg3y ,leg3z ,1, duration2); 516 | kinematics(4, leg4x, leg4y ,leg4z ,1, duration2); 517 | 518 | } 519 | 520 | 521 | 522 | 523 | 524 | } // end of timed loop 525 | 526 | } 527 | 528 | 529 | 530 | -------------------------------------------------------------------------------- /Code/004/Interpolation.ino: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /Code/004/kinematics.ino: -------------------------------------------------------------------------------- 1 | void kinematics (int leg, float xIn, int yIn, float zIn, int interOn, int dur) { 2 | 3 | #define length1 0.152 4 | float angle1; 5 | float angle1a; 6 | float angle1b; 7 | float angle1c; 8 | float angle2; 9 | float angle2Degrees; 10 | float angle2aDegrees; 11 | float angle1Degrees; 12 | float z3; 13 | float shoulderAngle2; 14 | float shoulderAngle2Degrees; 15 | 16 | float shoulderAngleServo; 17 | float shoulderAngleServo2; 18 | float kneeAngleServo; 19 | 20 | float zOut; 21 | float xOut; 22 | float yOut; 23 | 24 | 25 | // ** INTERPOLATION ** 26 | // use Interpolated values if Interpolation is on 27 | if (interOn == 1) { 28 | 29 | zIn = zIn * 1000; // refactor because the ramp calss doesn't seem to like floats 30 | xIn = xIn * 1000; 31 | 32 | if (leg == 1) { // front right 33 | z = interpFRZ.go(zIn,dur); 34 | x = interpFRX.go(xIn,dur); 35 | y = interpFRY.go(yIn,dur); 36 | } 37 | 38 | else if (leg == 2) { // front left 39 | z = interpFLZ.go(zIn,dur); 40 | x = interpFLX.go(xIn,dur); 41 | y = interpFLY.go(yIn,dur); 42 | } 43 | 44 | else if (leg == 4) { // back right 45 | z = interpBRZ.go(zIn,dur); 46 | x = interpBRX.go(xIn,dur); 47 | y = interpBRY.go(yIn,dur); 48 | } 49 | 50 | else if (leg == 3) { // back left 51 | z = interpBLZ.go(zIn,dur); 52 | x = interpBLX.go(xIn,dur); 53 | y = interpBLY.go(yIn,dur); 54 | } 55 | 56 | 57 | z = z/1000; // refactor again 58 | x = x/1000; 59 | 60 | z = constrain(z,0.25,0.29); 61 | x = constrain(x,-0.4,0.4); 62 | } 63 | 64 | 65 | 66 | // calculate the shoulder joint offset and new leg length based on now far the foot moves forward/backwards 67 | shoulderAngle2 = atan(x/z); // calc how much extra to add to the shoulder joint 68 | z3 = z/cos(shoulderAngle2); // calc new leg length to feed to the next bit of code below 69 | 70 | // calculate leg length based on shin/thigh length and knee and shoulder angle 71 | angle1a = sq(length1) + sq(z3) - sq(length1); 72 | angle1b = 2 * length1 * z3; 73 | angle1c = angle1a / angle1b; 74 | angle1 = acos(angle1c); // hip angle RADIANS 75 | angle2 = PI - (angle1 * 2); // knee angle RADIANS 76 | 77 | 78 | //calc degrees from angles 79 | angle2Degrees = angle2 * (180/PI); // knee angle DEGREES 80 | angle2aDegrees = angle2Degrees / 2; // half knee angle for each servo DEGREES 81 | shoulderAngle2Degrees = shoulderAngle2 * (180/PI); // front/back shoulder offset DEGREES 82 | 83 | shoulderAngleServo = (angle2aDegrees-68)*50; // remove defualt angle offset. Multiple degrees to get servo microseconds. 84 | kneeAngleServo = (angle2aDegrees-68)*50; 85 | shoulderAngleServo2 = shoulderAngle2Degrees*50; 86 | 87 | int factor = 20; 88 | 89 | if(leg == 1) { 90 | pos1 = 1500 - shoulderAngleServo - shoulderAngleServo2; 91 | pos2 = 1500 + kneeAngleServo - shoulderAngleServo2; 92 | pos1Filtered = filter(pos1, pos1Filtered, factor); 93 | pos2Filtered = filter(pos2, pos2Filtered, factor); 94 | servo1.writeMicroseconds(pos1Filtered + pos1Offset); 95 | servo2.writeMicroseconds(pos2Filtered + pos2Offset); 96 | pos9 = 1500 - y; 97 | servo9.writeMicroseconds(pos9 + pos9Offset); 98 | } 99 | else if (leg == 2) { 100 | pos5 = 1500 + shoulderAngleServo + shoulderAngleServo2; 101 | pos6 = 1500 - kneeAngleServo + shoulderAngleServo2; 102 | pos5Filtered = filter(pos5, pos5Filtered, factor); 103 | pos6Filtered = filter(pos6, pos6Filtered, factor); 104 | servo5.writeMicroseconds(pos5Filtered + pos5Offset); 105 | servo6.writeMicroseconds(pos6Filtered + pos6Offset); 106 | pos11 = 1500 - y; 107 | servo11.writeMicroseconds(pos11 + pos11Offset); 108 | } 109 | else if (leg == 3) { 110 | pos3 = 1500 - shoulderAngleServo - shoulderAngleServo2; 111 | pos4 = 1500 + kneeAngleServo - shoulderAngleServo2; 112 | pos3Filtered = filter(pos3, pos3Filtered, factor); 113 | pos4Filtered = filter(pos4, pos4Filtered, factor); 114 | servo3.writeMicroseconds(pos3Filtered + pos3Offset); 115 | servo4.writeMicroseconds(pos4Filtered + pos4Offset); 116 | pos10 = 1500 - y; 117 | servo10.writeMicroseconds(pos10 + pos10Offset); 118 | } 119 | else if (leg == 4) { 120 | pos7 = 1500 + shoulderAngleServo + shoulderAngleServo2; 121 | pos8 = 1500 - kneeAngleServo + shoulderAngleServo2; 122 | pos7Filtered = filter(pos7, pos7Filtered, factor); 123 | pos8Filtered = filter(pos8, pos8Filtered, factor); 124 | servo7.writeMicroseconds(pos7Filtered + pos7Offset); 125 | servo8.writeMicroseconds(pos8Filtered + pos8Offset); 126 | pos12 = 1500 - y; 127 | servo12.writeMicroseconds(pos12 + pos12Offset); 128 | } 129 | 130 | 131 | 132 | 133 | 134 | 135 | } 136 | 137 | 138 | -------------------------------------------------------------------------------- /Code/004/thresholdSticks.ino: -------------------------------------------------------------------------------- 1 | int thresholdStick (int pos) { 2 | 3 | // get zero centre position 4 | pos = pos - 1500; 5 | 6 | // threshold value for control sticks 7 | if (pos > 130) { 8 | pos = pos -130; 9 | } 10 | else if (pos < -130) { 11 | pos = pos +130; 12 | } 13 | else { 14 | pos = 0; 15 | } 16 | 17 | pos = map(pos, -370,370,-255,255); 18 | pos = constrain(pos,-255,255); 19 | 20 | return pos; 21 | } 22 | 23 | 24 | 25 | // motion filter to filter motions and compliance 26 | 27 | float filter(float prevValue, float currentValue, int filter) { 28 | float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1); 29 | return lengthFiltered; 30 | } 31 | 32 | 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 James Bruton 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ATAT 2 | 3 | https://youtu.be/AAgPmHINRJA 4 | --------------------------------------------------------------------------------