Components and supplies
5V Voltage regulator (generic)
MOSFET (generic)
Diode (generic)
Perfboard
180 Servo (generic)
6V Voltage regulator (generic)
Stock material
Nuts and Bolts
Rotary potentiometer (generic)
Arduino Mega 2560
Relay (generic)
Resistor 1k ohm
Ball Bearings (generic)
Lead screws and Nuts T8
DC motor (generic)
Tools and machines
General workshop supplies
Soldering iron (generic)
3D Printer (generic)
Lathe (generic)
Project description
Code
Project_ZERBERUS
c_cpp
Just leave out the wire part and adress the Servos directly I only did that because my Arduino Clone partly quit on me and I had another half broken one laying around.
1# include <PID_v1.h> 2#include <Wire.h> 3 4#define hipvl1 22 5#define hipvl2 23 6#define hipvr1 24 7#define hipvr2 25 8#define hiphl1 26 9#define hiphl2 27 10#define hiphr1 28 11#define hiphr2 29 12 13#define kneevl1 30 14#define kneevl2 31 15#define kneevr1 32 16#define kneevr2 33 17#define kneehl1 34 18#define kneehl2 35 19#define kneehr1 36 20#define kneehr2 37 21 22#define potkvl A0 23#define potkvr A1 24#define potkhl A2 25#define potkhr A3 26#define pothvl A4 27#define pothvr A5 28#define pothhl A6 29#define pothhr A7 30 31#define remote1 A8 32#define remote2 A9 33 34int neigung; 35int hiplean = 8; //allgemeiner Neigungswert fr Hfen 36 37int vl; 38int vr; 39int hl; 40int hr; 41 42double Kp=0.03, Ki=0.6, Kd=0.6; 43 44double kneevlset; 45double kneevrset; 46double kneehlset; 47double kneehrset; 48double hipvlset; 49double hiphlset; 50double hipvrset; 51double hiphrset; 52 53double kneevlinp; 54double kneevrinp; 55double kneehlinp; 56double kneehrinp; 57double hipvlinp; 58double hiphlinp; 59double hipvrinp; 60double hiphrinp; 61 62 63double kneevloutp; 64double kneevroutp; 65double kneehloutp; 66double kneehroutp; 67double hipvloutp; 68double hiphloutp; 69double hipvroutp; 70double hiphroutp; 71 72int kneevl_; 73int kneevr_; 74int kneehl_; 75int kneehr_; 76int hipvl_; 77int hipvr_; 78int hiphl_; 79int hiphr_; 80 81PID kneevlPID (&kneevlinp, &kneevloutp, &kneevlset, Kp, Ki, Kd, DIRECT); 82PID kneevrPID (&kneevrinp, &kneevroutp, &kneevrset, Kp, Ki, Kd, DIRECT); 83PID kneehlPID (&kneehlinp, &kneehloutp, &kneehlset, Kp, Ki, Kd, DIRECT); 84PID kneehrPID (&kneehrinp, &kneehroutp, &kneehrset, Kp, Ki, Kd, DIRECT); 85PID hipvlPID (&hipvlinp, &hipvloutp, &hipvlset, Kp, Ki, Kd, DIRECT); 86PID hipvrPID (&hipvrinp, &hipvroutp, &hipvrset, Kp, Ki, Kd, DIRECT); 87PID hiphlPID (&hiphlinp, &hiphloutp, &hiphlset, Kp, Ki, Kd, DIRECT); 88PID hiphrPID (&hiphrinp, &hiphroutp, &hiphrset, Kp, Ki, Kd, DIRECT); 89 90void setup() { 91 pinMode(hipvl1,OUTPUT); 92 pinMode(hipvl2,OUTPUT); 93 pinMode(hipvr1,OUTPUT); 94 pinMode(hipvr2,OUTPUT); 95 pinMode(hiphl1,OUTPUT); 96 pinMode(hiphl2,OUTPUT); 97 pinMode(hiphr1,OUTPUT); 98 pinMode(hiphr2,OUTPUT); 99 100 pinMode(kneevl1,OUTPUT); 101 pinMode(kneevl2,OUTPUT); 102 pinMode(kneevr1,OUTPUT); 103 pinMode(kneevr2,OUTPUT); 104 pinMode(kneehl1,OUTPUT); 105 pinMode(kneehl2,OUTPUT); 106 pinMode(kneehr1,OUTPUT); 107 pinMode(kneehr2,OUTPUT); 108 109 pinMode(potkvl,INPUT); 110 pinMode(potkvr,INPUT); 111 pinMode(potkhl,INPUT); 112 pinMode(potkhr,INPUT); 113 pinMode(pothvl,INPUT); 114 pinMode(pothvr,INPUT); 115 pinMode(pothhl,INPUT); 116 pinMode(pothhr,INPUT); 117 118 pinMode(remote1,INPUT); 119 pinMode(remote2,INPUT); 120 121 kneevlPID.SetMode(AUTOMATIC); 122 kneevlPID.SetOutputLimits(0,100); 123 kneevlPID.SetTunings(Kp, Ki, Kd); 124 kneevlPID.SetSampleTime(100); 125 126 kneevrPID.SetMode(AUTOMATIC); 127 kneevrPID.SetOutputLimits(0,100); 128 kneevrPID.SetTunings(Kp, Ki, Kd); 129 kneevrPID.SetSampleTime(100); 130 131 kneehlPID.SetMode(AUTOMATIC); 132 kneehlPID.SetOutputLimits(0,100); 133 kneehlPID.SetTunings(Kp, Ki, Kd); 134 kneehlPID.SetSampleTime(100); 135 136 kneehrPID.SetMode(AUTOMATIC); 137 kneehrPID.SetOutputLimits(0,100); 138 kneehrPID.SetTunings(Kp, Ki, Kd); 139 kneehrPID.SetSampleTime(100); 140 141 hipvlPID.SetMode(AUTOMATIC); 142 hipvlPID.SetOutputLimits(0,100); 143 hipvlPID.SetTunings(Kp, Ki, Kd); 144 hipvlPID.SetSampleTime(100); 145 146 hipvrPID.SetMode(AUTOMATIC); 147 hipvrPID.SetOutputLimits(0,100); 148 hipvrPID.SetTunings(Kp, Ki, Kd); 149 hipvrPID.SetSampleTime(100); 150 151 hiphlPID.SetMode(AUTOMATIC); 152 hiphlPID.SetOutputLimits(0,100); 153 hiphlPID.SetTunings(Kp, Ki, Kd); 154 hiphlPID.SetSampleTime(100); 155 156 hiphrPID.SetMode(AUTOMATIC); 157 hiphrPID.SetOutputLimits(0,100); 158 hiphrPID.SetTunings(Kp, Ki, Kd); 159 hiphrPID.SetSampleTime(100); 160 161 Serial.begin(9600); 162 Wire.begin(); 163 HOME(); 164} 165 166void loop() { 167 168} 169 170 void hipbend() { 171 172 173 174 int hipvl_ = analogRead(pothvl); 175 int hipvr_ = analogRead(pothvr); 176 int hiphl_ = analogRead(pothhl); 177 int hiphr_ = analogRead(pothhr); 178 179 int hipleanvl = hipvl_; // aktueller Wert der Hftgelenke 180 int hipleanvr = hipvr_; 181 int hipleanhl = hiphl_; 182 int hipleanhr = hiphr_; 183 184 vl = 1; 185 vr = 1; 186 hl = 1; 187 hr = 1; 188 189if (neigung == 1) 190 { 191 hipvl_ = hipvl_ - hiplean; //Sollwert fr die Hftneigung 192 hiphl_ = hiphl_ - hiplean; 193 hipvr_ = hipvr_ + hiplean; 194 hiphr_ = hiphr_ + hiplean; 195 } 196 197else if (neigung == 0) 198 { 199 hipvl_ = hipvl_ + hiplean; //Sollwert fr die Hftneigung 200 hiphl_ = hiphl_ + hiplean; 201 hipvr_ = hipvr_ - hiplean; 202 hiphr_ = hiphr_ - hiplean; 203 } 204 205 if (hipvl_ > hipleanvl) 206 { 207 hipvlout(); 208 //hipvl nach innen 209 } 210 211 else if (hipvl_ < hipleanvl) 212 { 213 hipvlin(); 214 //hipvl nach auen 215 } 216 217 else if (hipvl_ = hipleanvl) 218 { 219 220 } 221 222 if (hipvr_ > hipleanvr) 223 { 224 hipvrin(); 225 //hipvr nach innen 226 } 227 228 else if (hipvr_ < hipleanvr) 229 { 230 hipvrout(); 231 //hipvr nach auen 232 } 233 234 else if (hipvr_ = hipleanvr) 235 { 236 237 } 238 239 if (hiphl_ > hipleanhl) 240 { 241 hiphlout(); 242 //hiphl nach innen 243 } 244 245 else if (hiphl_ < hipleanhl) 246 { 247 hiphlin(); 248 //hiphl nach auen 249 } 250 251 else if (hiphl_ = hipleanhl) 252 { 253 254 } 255 256 257 if (hiphr_ > hipleanhr) 258 { 259 hiphrout(); 260 //hiphr nach innen 261 } 262 263 else if (hiphr_ < hipleanhr) 264 { 265 hiphrin(); 266 //hiphr nach auen 267 } 268 269 270 else if (hiphr_ = hipleanhr) 271 { 272 273 } 274 275if (neigung == 1) 276 { 277 while(vl==1 || vr==1 || hl==1 || hr==1) 278 { 279 if(analogRead(pothvl) <= hipvl_) 280 { 281 vl = 0; 282 hipvllow(); 283 //hipvl low 284 } 285 286 if(analogRead(pothvr) >= hipvr_) 287 { 288 vr = 0; 289 hipvrlow(); 290 //hipvr low 291 } 292 293 if(analogRead(pothhl) <= hiphl_) 294 { 295 hl = 0; 296 hiphllow(); 297 //hiphl low 298 } 299 300 if(analogRead(pothhr) >= hiphr_) 301 { 302 hr = 0; 303 hiphrlow(); 304 //hiphr low 305 } 306 delay(500); 307 } 308 309 } 310 311 if (neigung == 0) 312 { 313 while(vl==1 || vr==1 || hl==1 || hr==1) 314 { 315 if(analogRead(pothvl) >= hipvl_) 316 { 317 vl = 0; 318 hipvllow(); 319 //hipvl low 320 } 321 322 if(analogRead(pothvr) <= hipvr_) 323 { 324 vr = 0; 325 hipvrlow(); 326 //hipvr low 327 } 328 329 if(analogRead(pothhl) >= hiphl_) 330 { 331 hl = 0; 332 hiphllow(); 333 //hiphl low 334 } 335 336 if(analogRead(pothhr) <= hiphr_) 337 { 338 hr = 0; 339 hiphrlow(); 340 //hiphr low 341 } 342 delay(500); 343 } 344 345 } 346 347 } 348 349 350 void hipvlin () 351 { 352 digitalWrite(hipvl2,HIGH); 353 digitalWrite(hipvl1,LOW); 354 Serial.println("hipvlin"); 355 } 356 357 358 void hipvlout () 359 { 360 digitalWrite(hipvl1,HIGH); 361 digitalWrite(hipvl2,LOW); 362 Serial.println("hipvlout"); 363 } 364 365 void hipvrin () 366 { 367 digitalWrite(hipvr2,HIGH); 368 digitalWrite(hipvr1,LOW); 369 Serial.println("hipvrin"); 370 } 371 372 void hipvrout () 373 { 374 digitalWrite(hipvr1,HIGH); 375 digitalWrite(hipvr2,LOW); 376 Serial.println("hipvrout"); 377 } 378 379 void hiphlin () 380 { 381 digitalWrite(hiphl2,HIGH); 382 digitalWrite(hiphl1,LOW); 383 Serial.println("hiphlin"); 384 } 385 386 void hiphlout () 387 { 388 digitalWrite(hiphl1,HIGH); 389 digitalWrite(hiphl2,LOW); 390 Serial.println("hiphlout"); 391 } 392 393 void hiphrin () 394 { 395 digitalWrite(hiphr2,HIGH); 396 digitalWrite(hiphr1,LOW); 397 Serial.println("hiphrin"); 398 } 399 400 void hiphrout () 401 { 402 digitalWrite(hiphr1,HIGH); 403 digitalWrite(hiphr2,LOW); 404 Serial.println("hiphrout"); 405 } 406 407 void kneevlin () 408 { 409 digitalWrite(kneevl2,HIGH); 410 digitalWrite(kneevl1,LOW); 411 Serial.println("kneevlin"); 412 } 413 414 void kneevlout () 415 { 416 digitalWrite(kneevl1,HIGH); 417 digitalWrite(kneevl2,LOW); 418 Serial.println("kneevlout"); 419 } 420 421 422 void kneevrin () 423 { 424 digitalWrite(kneevr2,HIGH); 425 digitalWrite(kneevr1,LOW); 426 Serial.println("kneevrin"); 427 } 428 429 void kneevrout () 430 { 431 digitalWrite(kneevr1,HIGH); 432 digitalWrite(kneevr2,LOW); 433 Serial.println("kneevrout"); 434 } 435 436 void kneehlin () 437 { 438 digitalWrite(kneehl2,HIGH); 439 digitalWrite(kneehl1,LOW); 440 Serial.println("kneehlin"); 441 } 442 443 void kneehlout () 444 { 445 digitalWrite(kneehl1,HIGH); 446 digitalWrite(kneehl2,LOW); 447 Serial.println("kneehlout"); 448 } 449 450 void kneehrout () 451 { 452 digitalWrite(kneehr2,HIGH); 453 digitalWrite(kneehr1,LOW); 454 Serial.println("kneehrin"); 455 } 456 457 void kneehrin () 458 { 459 digitalWrite(kneehr1,HIGH); 460 digitalWrite(kneehr2,LOW); 461 Serial.println("kneehrout"); 462 } 463 464 void hipvllow() 465 { 466 digitalWrite(hipvl1,LOW); 467 digitalWrite(hipvl2,LOW); 468 Serial.println("hipvllow"); 469 } 470 471 472 void hipvrlow() 473 { 474 digitalWrite(hipvr1,LOW); 475 digitalWrite(hipvr2,LOW); 476 Serial.println("hipvrlow"); 477 } 478 479 void hiphllow() 480 { 481 digitalWrite(hiphl1,LOW); 482 digitalWrite(hiphl2,LOW); 483 Serial.println("hiphllow"); 484 } 485 486 void hiphrlow() 487 { 488 digitalWrite(hiphr1,LOW); 489 digitalWrite(hiphr2,LOW); 490 Serial.println("hiphrlow"); 491 } 492 493 void kneevllow() 494 { 495 digitalWrite(kneevl1,LOW); 496 digitalWrite(kneevl2,LOW); 497 Serial.println("kneevllow"); 498 } 499 500 void kneevrlow() 501 { 502 digitalWrite(kneevr1,LOW); 503 digitalWrite(kneevr2,LOW); 504 Serial.println("kneevrlow"); 505 } 506 507 void kneehllow() 508 { 509 digitalWrite(kneehl1,LOW); 510 digitalWrite(kneehl2,LOW); 511 Serial.println("kneehllow"); 512 } 513 514 void kneehrlow() 515 { 516 digitalWrite(kneehr1,LOW); 517 digitalWrite(kneehr2,LOW); 518 Serial.println("kneehrlow"); 519 } 520 521 522 void kneevl() 523 { 524kneevl_ = 0; 525while(kneevl_ == 0) 526 { 527 kneevlinp = map(analogRead(potkvl),235,453,0,10); 528 529 kneevlPID.Compute(); 530 531 if(map(analogRead(potkvl),235,453,0,10) > kneevlset) 532 { 533 kneevlout(); 534 delay(kneevloutp); 535 } 536 537 if(map(analogRead(potkvl),235,453,0,10) < kneevlset) 538 { 539 kneevlin(); 540 delay(kneevloutp); 541 } 542 543 544 if(map(analogRead(potkvl),235,453,0,10) == kneevlset) 545 { 546 kneevllow(); 547 kneevl_ = 1; 548 } 549 } 550 } 551 552//_______________________________________________________________________________________ 553 554 void kneevr() 555 { 556kneevr_ = 0; 557while(kneevr_ == 0) 558 { 559 kneevrinp = map(analogRead(potkvr),400,680,0,10); 560 561 kneevrPID.Compute(); 562 563 if(map(analogRead(potkvr),400,680,0,10) > kneevrset) 564 { 565 kneevrout(); 566 delay(kneevroutp); 567 } 568 569 if(map(analogRead(potkvr),400,680,0,10) < kneevrset) 570 { 571 kneevrin(); 572 delay(kneevroutp); 573 } 574 575 576 if(map(analogRead(potkvr),400,680,0,10) == kneevrset) 577 { 578 kneevrlow(); 579 kneevr_ = 1; 580 } 581 } 582 } 583 584//_______________________________________________________________________________________ 585 586 587 void kneehl() 588 { 589kneehl_ = 0; 590while(kneehl_ == 0) 591 { 592 kneehlinp = map(analogRead(potkhl),171,383,0,10); 593 594 kneehlPID.Compute(); 595 596 if(map(analogRead(potkhl),171,383,0,10) > kneehlset) 597 { 598 kneehlout(); 599 delay(kneehloutp); 600 } 601 602 if(map(analogRead(potkhl),171,383,0,10) < kneehlset) 603 { 604 kneehlin(); 605 delay(kneehloutp); 606 } 607 608 609 if(map(analogRead(potkhl),171,383,0,10) == kneehlset) 610 { 611 kneehllow(); 612 kneehl_ = 1; 613 } 614 } 615 } 616 617//_______________________________________________________________________________________ 618 619 620 void kneehr() 621 { 622kneehr_ = 0; 623while(kneehr_ == 0) 624 { 625 kneehrinp = map(analogRead(potkhr),418,671,0,10); 626 627 kneehrPID.Compute(); 628 629 if(map(analogRead(potkhr),418,671,0,10) > kneehrset) 630 { 631 kneehrout(); 632 delay(kneehroutp); 633 } 634 635 if(map(analogRead(potkhr),418,671,0,10) < kneehrset) 636 { 637 kneehrin(); 638 delay(kneehroutp); 639 } 640 641 642 if(map(analogRead(potkhr),418,671,0,10) == kneehrset) 643 { 644 kneehrlow(); 645 kneehr_ = 1; 646 } 647 } 648 } 649 650//_______________________________________________________________________________________ 651 652 653 void hipvl() 654 { 655hipvl_ = 0; 656while(hipvl_ == 0) 657 { 658 hipvlinp = map(analogRead(pothvl),95,247,0,10); 659 660 hipvlPID.Compute(); 661Serial.print(hipvlinp); 662 if(map(analogRead(pothvl),95,247,0,10) < hipvlset) 663 { 664 hipvlout(); 665 delay(hipvloutp); 666 } 667 668 else if(map(analogRead(pothvl),95,247,0,10) > hipvlset) 669 { 670 hipvlin(); 671 delay(hipvloutp); 672 } 673 674 675 else if(map(analogRead(pothvl),95,247,0,10) == hipvlset) 676 { 677 hipvllow(); 678 hipvl_ = 1; 679 } 680 } 681 } 682 683//_______________________________________________________________________________________ 684 685 686 void hipvr() 687 { 688hipvr_ = 0; 689while(hipvr_ == 0) 690{ 691 hipvrinp = map(analogRead(pothvr),825,967,0,10); 692 693 hipvrPID.Compute(); 694 695 if(map(analogRead(pothvr),825,967,0,10) < hipvrset) 696 { 697 hipvrout(); 698 delay(hipvroutp); 699 } 700 701 if(map(analogRead(pothvr),825,967,0,10) > hipvrset) 702 { 703 hipvrin(); 704 delay(hipvroutp); 705 } 706 707 708 if(map(analogRead(pothvr),825,967,0,10) == hipvrset) 709 { 710 hipvrlow(); 711 hipvr_ = 1; 712 } 713 } 714 } 715 716//_______________________________________________________________________________________ 717 718 719 void hiphl() 720 { 721hiphl_ = 0; 722while(hiphl_ == 0) 723 { 724 hiphlinp = map(analogRead(pothhl),800,932,0,10); 725 726 hiphlPID.Compute(); 727 728 if(map(analogRead(pothhl),800,932,0,10) < hiphlset) 729 { 730 hiphlout(); 731 delay(hiphloutp); 732 } 733 734 if(map(analogRead(pothhl),800,932,0,10) > hiphlset) 735 { 736 hiphlin(); 737 delay(hiphloutp); 738 } 739 740 741 if(map(analogRead(pothhl),800,932,0,10) == hiphlset) 742 { 743 hiphllow(); 744 hiphl_ = 1; 745 } 746 } 747 } 748 749//_______________________________________________________________________________________ 750 751 752 void hiphr() 753 { 754hiphr_ = 0; 755while(hiphr_ == 0) 756 { 757 hiphrinp = map(analogRead(pothhr),94,204,0,10); 758 759 hiphrPID.Compute(); 760 761 if(map(analogRead(pothhr),94,204,0,10) < hiphrset) 762 { 763 hiphrout(); 764 delay(hiphroutp); 765 } 766 767 if(map(analogRead(pothhr),94,204,0,10) > hiphrset) 768 { 769 hiphrin(); 770 delay(hiphroutp); 771 } 772 773 774 if(map(analogRead(pothhr),94,204,0,10) == hiphrset) 775 { 776 hiphrlow(); 777 hiphr_ = 1; 778 } 779 } 780 } 781 782//_______________________________________________________________________________________ 783 784void HOME() 785{ 786kneevlset = 6; 787kneevrset = 7; 788kneehlset = 6; 789kneehrset = 6; 790hipvlset = 9; 791hiphlset = 9; 792hipvrset = 9; 793hiphrset = 9; 794 795 796 hipvl(); 797 delay(500); 798 799 hiphl(); 800 delay(500); 801 802 hiphr(); 803 delay(500); 804 805 hipvr(); 806 delay(500); 807 808 kneevl(); 809 delay(500); 810 811 //kneehl(); 812 //delay(500); 813 814 kneehr(); 815 delay(500); 816 817 kneevr(); 818 delay(500); 819 820 821 822 int servovl = 80; 823 int servovr = 80; 824 int servohl = 110; 825 int servohr = 30; 826 827 Wire.beginTransmission(8); 828 Wire.write(servovl); 829 Wire.write(servovr); 830 Wire.write(servohl); 831 Wire.write(servohr); 832 Wire.endTransmission(); 833} 834 835 836
Project_ZERBERUS
c_cpp
Just leave out the wire part and adress the Servos directly I only did that because my Arduino Clone partly quit on me and I had another half broken one laying around.
1# include <PID_v1.h> 2#include <Wire.h> 3 4#define hipvl1 22 5#define hipvl2 23 6#define hipvr1 24 7#define hipvr2 25 8#define hiphl1 26 9#define hiphl2 27 10#define hiphr1 28 11#define hiphr2 29 12 13#define kneevl1 30 14#define kneevl2 31 15#define kneevr1 32 16#define kneevr2 33 17#define kneehl1 34 18#define kneehl2 35 19#define kneehr1 36 20#define kneehr2 37 21 22#define potkvl A0 23#define potkvr A1 24#define potkhl A2 25#define potkhr A3 26#define pothvl A4 27#define pothvr A5 28#define pothhl A6 29#define pothhr A7 30 31#define remote1 A8 32#define remote2 A9 33 34int neigung; 35int hiplean = 8; //allgemeiner Neigungswert fr Hfen 36 37int vl; 38int vr; 39int hl; 40int hr; 41 42double Kp=0.03, Ki=0.6, Kd=0.6; 43 44double kneevlset; 45double kneevrset; 46double kneehlset; 47double kneehrset; 48double hipvlset; 49double hiphlset; 50double hipvrset; 51double hiphrset; 52 53double kneevlinp; 54double kneevrinp; 55double kneehlinp; 56double kneehrinp; 57double hipvlinp; 58double hiphlinp; 59double hipvrinp; 60double hiphrinp; 61 62 63double kneevloutp; 64double kneevroutp; 65double kneehloutp; 66double kneehroutp; 67double hipvloutp; 68double hiphloutp; 69double hipvroutp; 70double hiphroutp; 71 72int kneevl_; 73int kneevr_; 74int kneehl_; 75int kneehr_; 76int hipvl_; 77int hipvr_; 78int hiphl_; 79int hiphr_; 80 81PID kneevlPID (&kneevlinp, &kneevloutp, &kneevlset, Kp, Ki, Kd, DIRECT); 82PID kneevrPID (&kneevrinp, &kneevroutp, &kneevrset, Kp, Ki, Kd, DIRECT); 83PID kneehlPID (&kneehlinp, &kneehloutp, &kneehlset, Kp, Ki, Kd, DIRECT); 84PID kneehrPID (&kneehrinp, &kneehroutp, &kneehrset, Kp, Ki, Kd, DIRECT); 85PID hipvlPID (&hipvlinp, &hipvloutp, &hipvlset, Kp, Ki, Kd, DIRECT); 86PID hipvrPID (&hipvrinp, &hipvroutp, &hipvrset, Kp, Ki, Kd, DIRECT); 87PID hiphlPID (&hiphlinp, &hiphloutp, &hiphlset, Kp, Ki, Kd, DIRECT); 88PID hiphrPID (&hiphrinp, &hiphroutp, &hiphrset, Kp, Ki, Kd, DIRECT); 89 90void setup() { 91 pinMode(hipvl1,OUTPUT); 92 pinMode(hipvl2,OUTPUT); 93 pinMode(hipvr1,OUTPUT); 94 pinMode(hipvr2,OUTPUT); 95 pinMode(hiphl1,OUTPUT); 96 pinMode(hiphl2,OUTPUT); 97 pinMode(hiphr1,OUTPUT); 98 pinMode(hiphr2,OUTPUT); 99 100 pinMode(kneevl1,OUTPUT); 101 pinMode(kneevl2,OUTPUT); 102 pinMode(kneevr1,OUTPUT); 103 pinMode(kneevr2,OUTPUT); 104 pinMode(kneehl1,OUTPUT); 105 pinMode(kneehl2,OUTPUT); 106 pinMode(kneehr1,OUTPUT); 107 pinMode(kneehr2,OUTPUT); 108 109 pinMode(potkvl,INPUT); 110 pinMode(potkvr,INPUT); 111 pinMode(potkhl,INPUT); 112 pinMode(potkhr,INPUT); 113 pinMode(pothvl,INPUT); 114 pinMode(pothvr,INPUT); 115 pinMode(pothhl,INPUT); 116 pinMode(pothhr,INPUT); 117 118 pinMode(remote1,INPUT); 119 pinMode(remote2,INPUT); 120 121 kneevlPID.SetMode(AUTOMATIC); 122 kneevlPID.SetOutputLimits(0,100); 123 kneevlPID.SetTunings(Kp, Ki, Kd); 124 kneevlPID.SetSampleTime(100); 125 126 kneevrPID.SetMode(AUTOMATIC); 127 kneevrPID.SetOutputLimits(0,100); 128 kneevrPID.SetTunings(Kp, Ki, Kd); 129 kneevrPID.SetSampleTime(100); 130 131 kneehlPID.SetMode(AUTOMATIC); 132 kneehlPID.SetOutputLimits(0,100); 133 kneehlPID.SetTunings(Kp, Ki, Kd); 134 kneehlPID.SetSampleTime(100); 135 136 kneehrPID.SetMode(AUTOMATIC); 137 kneehrPID.SetOutputLimits(0,100); 138 kneehrPID.SetTunings(Kp, Ki, Kd); 139 kneehrPID.SetSampleTime(100); 140 141 hipvlPID.SetMode(AUTOMATIC); 142 hipvlPID.SetOutputLimits(0,100); 143 hipvlPID.SetTunings(Kp, Ki, Kd); 144 hipvlPID.SetSampleTime(100); 145 146 hipvrPID.SetMode(AUTOMATIC); 147 hipvrPID.SetOutputLimits(0,100); 148 hipvrPID.SetTunings(Kp, Ki, Kd); 149 hipvrPID.SetSampleTime(100); 150 151 hiphlPID.SetMode(AUTOMATIC); 152 hiphlPID.SetOutputLimits(0,100); 153 hiphlPID.SetTunings(Kp, Ki, Kd); 154 hiphlPID.SetSampleTime(100); 155 156 hiphrPID.SetMode(AUTOMATIC); 157 hiphrPID.SetOutputLimits(0,100); 158 hiphrPID.SetTunings(Kp, Ki, Kd); 159 hiphrPID.SetSampleTime(100); 160 161 Serial.begin(9600); 162 Wire.begin(); 163 HOME(); 164} 165 166void loop() { 167 168} 169 170 void hipbend() { 171 172 173 174 int hipvl_ = analogRead(pothvl); 175 int hipvr_ = analogRead(pothvr); 176 int hiphl_ = analogRead(pothhl); 177 int hiphr_ = analogRead(pothhr); 178 179 int hipleanvl = hipvl_; // aktueller Wert der Hftgelenke 180 int hipleanvr = hipvr_; 181 int hipleanhl = hiphl_; 182 int hipleanhr = hiphr_; 183 184 vl = 1; 185 vr = 1; 186 hl = 1; 187 hr = 1; 188 189if (neigung == 1) 190 { 191 hipvl_ = hipvl_ - hiplean; //Sollwert fr die Hftneigung 192 hiphl_ = hiphl_ - hiplean; 193 hipvr_ = hipvr_ + hiplean; 194 hiphr_ = hiphr_ + hiplean; 195 } 196 197else if (neigung == 0) 198 { 199 hipvl_ = hipvl_ + hiplean; //Sollwert fr die Hftneigung 200 hiphl_ = hiphl_ + hiplean; 201 hipvr_ = hipvr_ - hiplean; 202 hiphr_ = hiphr_ - hiplean; 203 } 204 205 if (hipvl_ > hipleanvl) 206 { 207 hipvlout(); 208 //hipvl nach innen 209 } 210 211 else if (hipvl_ < hipleanvl) 212 { 213 hipvlin(); 214 //hipvl nach auen 215 } 216 217 else if (hipvl_ = hipleanvl) 218 { 219 220 } 221 222 if (hipvr_ > hipleanvr) 223 { 224 hipvrin(); 225 //hipvr nach innen 226 } 227 228 else if (hipvr_ < hipleanvr) 229 { 230 hipvrout(); 231 //hipvr nach auen 232 } 233 234 else if (hipvr_ = hipleanvr) 235 { 236 237 } 238 239 if (hiphl_ > hipleanhl) 240 { 241 hiphlout(); 242 //hiphl nach innen 243 } 244 245 else if (hiphl_ < hipleanhl) 246 { 247 hiphlin(); 248 //hiphl nach auen 249 } 250 251 else if (hiphl_ = hipleanhl) 252 { 253 254 } 255 256 257 if (hiphr_ > hipleanhr) 258 { 259 hiphrout(); 260 //hiphr nach innen 261 } 262 263 else if (hiphr_ < hipleanhr) 264 { 265 hiphrin(); 266 //hiphr nach auen 267 } 268 269 270 else if (hiphr_ = hipleanhr) 271 { 272 273 } 274 275if (neigung == 1) 276 { 277 while(vl==1 || vr==1 || hl==1 || hr==1) 278 { 279 if(analogRead(pothvl) <= hipvl_) 280 { 281 vl = 0; 282 hipvllow(); 283 //hipvl low 284 } 285 286 if(analogRead(pothvr) >= hipvr_) 287 { 288 vr = 0; 289 hipvrlow(); 290 //hipvr low 291 } 292 293 if(analogRead(pothhl) <= hiphl_) 294 { 295 hl = 0; 296 hiphllow(); 297 //hiphl low 298 } 299 300 if(analogRead(pothhr) >= hiphr_) 301 { 302 hr = 0; 303 hiphrlow(); 304 //hiphr low 305 } 306 delay(500); 307 } 308 309 } 310 311 if (neigung == 0) 312 { 313 while(vl==1 || vr==1 || hl==1 || hr==1) 314 { 315 if(analogRead(pothvl) >= hipvl_) 316 { 317 vl = 0; 318 hipvllow(); 319 //hipvl low 320 } 321 322 if(analogRead(pothvr) <= hipvr_) 323 { 324 vr = 0; 325 hipvrlow(); 326 //hipvr low 327 } 328 329 if(analogRead(pothhl) >= hiphl_) 330 { 331 hl = 0; 332 hiphllow(); 333 //hiphl low 334 } 335 336 if(analogRead(pothhr) <= hiphr_) 337 { 338 hr = 0; 339 hiphrlow(); 340 //hiphr low 341 } 342 delay(500); 343 } 344 345 } 346 347 } 348 349 350 void hipvlin () 351 { 352 digitalWrite(hipvl2,HIGH); 353 digitalWrite(hipvl1,LOW); 354 Serial.println("hipvlin"); 355 } 356 357 358 void hipvlout () 359 { 360 digitalWrite(hipvl1,HIGH); 361 digitalWrite(hipvl2,LOW); 362 Serial.println("hipvlout"); 363 } 364 365 void hipvrin () 366 { 367 digitalWrite(hipvr2,HIGH); 368 digitalWrite(hipvr1,LOW); 369 Serial.println("hipvrin"); 370 } 371 372 void hipvrout () 373 { 374 digitalWrite(hipvr1,HIGH); 375 digitalWrite(hipvr2,LOW); 376 Serial.println("hipvrout"); 377 } 378 379 void hiphlin () 380 { 381 digitalWrite(hiphl2,HIGH); 382 digitalWrite(hiphl1,LOW); 383 Serial.println("hiphlin"); 384 } 385 386 void hiphlout () 387 { 388 digitalWrite(hiphl1,HIGH); 389 digitalWrite(hiphl2,LOW); 390 Serial.println("hiphlout"); 391 } 392 393 void hiphrin () 394 { 395 digitalWrite(hiphr2,HIGH); 396 digitalWrite(hiphr1,LOW); 397 Serial.println("hiphrin"); 398 } 399 400 void hiphrout () 401 { 402 digitalWrite(hiphr1,HIGH); 403 digitalWrite(hiphr2,LOW); 404 Serial.println("hiphrout"); 405 } 406 407 void kneevlin () 408 { 409 digitalWrite(kneevl2,HIGH); 410 digitalWrite(kneevl1,LOW); 411 Serial.println("kneevlin"); 412 } 413 414 void kneevlout () 415 { 416 digitalWrite(kneevl1,HIGH); 417 digitalWrite(kneevl2,LOW); 418 Serial.println("kneevlout"); 419 } 420 421 422 void kneevrin () 423 { 424 digitalWrite(kneevr2,HIGH); 425 digitalWrite(kneevr1,LOW); 426 Serial.println("kneevrin"); 427 } 428 429 void kneevrout () 430 { 431 digitalWrite(kneevr1,HIGH); 432 digitalWrite(kneevr2,LOW); 433 Serial.println("kneevrout"); 434 } 435 436 void kneehlin () 437 { 438 digitalWrite(kneehl2,HIGH); 439 digitalWrite(kneehl1,LOW); 440 Serial.println("kneehlin"); 441 } 442 443 void kneehlout () 444 { 445 digitalWrite(kneehl1,HIGH); 446 digitalWrite(kneehl2,LOW); 447 Serial.println("kneehlout"); 448 } 449 450 void kneehrout () 451 { 452 digitalWrite(kneehr2,HIGH); 453 digitalWrite(kneehr1,LOW); 454 Serial.println("kneehrin"); 455 } 456 457 void kneehrin () 458 { 459 digitalWrite(kneehr1,HIGH); 460 digitalWrite(kneehr2,LOW); 461 Serial.println("kneehrout"); 462 } 463 464 void hipvllow() 465 { 466 digitalWrite(hipvl1,LOW); 467 digitalWrite(hipvl2,LOW); 468 Serial.println("hipvllow"); 469 } 470 471 472 void hipvrlow() 473 { 474 digitalWrite(hipvr1,LOW); 475 digitalWrite(hipvr2,LOW); 476 Serial.println("hipvrlow"); 477 } 478 479 void hiphllow() 480 { 481 digitalWrite(hiphl1,LOW); 482 digitalWrite(hiphl2,LOW); 483 Serial.println("hiphllow"); 484 } 485 486 void hiphrlow() 487 { 488 digitalWrite(hiphr1,LOW); 489 digitalWrite(hiphr2,LOW); 490 Serial.println("hiphrlow"); 491 } 492 493 void kneevllow() 494 { 495 digitalWrite(kneevl1,LOW); 496 digitalWrite(kneevl2,LOW); 497 Serial.println("kneevllow"); 498 } 499 500 void kneevrlow() 501 { 502 digitalWrite(kneevr1,LOW); 503 digitalWrite(kneevr2,LOW); 504 Serial.println("kneevrlow"); 505 } 506 507 void kneehllow() 508 { 509 digitalWrite(kneehl1,LOW); 510 digitalWrite(kneehl2,LOW); 511 Serial.println("kneehllow"); 512 } 513 514 void kneehrlow() 515 { 516 digitalWrite(kneehr1,LOW); 517 digitalWrite(kneehr2,LOW); 518 Serial.println("kneehrlow"); 519 } 520 521 522 void kneevl() 523 { 524kneevl_ = 0; 525while(kneevl_ == 0) 526 { 527 kneevlinp = map(analogRead(potkvl),235,453,0,10); 528 529 kneevlPID.Compute(); 530 531 if(map(analogRead(potkvl),235,453,0,10) > kneevlset) 532 { 533 kneevlout(); 534 delay(kneevloutp); 535 } 536 537 if(map(analogRead(potkvl),235,453,0,10) < kneevlset) 538 { 539 kneevlin(); 540 delay(kneevloutp); 541 } 542 543 544 if(map(analogRead(potkvl),235,453,0,10) == kneevlset) 545 { 546 kneevllow(); 547 kneevl_ = 1; 548 } 549 } 550 } 551 552//_______________________________________________________________________________________ 553 554 void kneevr() 555 { 556kneevr_ = 0; 557while(kneevr_ == 0) 558 { 559 kneevrinp = map(analogRead(potkvr),400,680,0,10); 560 561 kneevrPID.Compute(); 562 563 if(map(analogRead(potkvr),400,680,0,10) > kneevrset) 564 { 565 kneevrout(); 566 delay(kneevroutp); 567 } 568 569 if(map(analogRead(potkvr),400,680,0,10) < kneevrset) 570 { 571 kneevrin(); 572 delay(kneevroutp); 573 } 574 575 576 if(map(analogRead(potkvr),400,680,0,10) == kneevrset) 577 { 578 kneevrlow(); 579 kneevr_ = 1; 580 } 581 } 582 } 583 584//_______________________________________________________________________________________ 585 586 587 void kneehl() 588 { 589kneehl_ = 0; 590while(kneehl_ == 0) 591 { 592 kneehlinp = map(analogRead(potkhl),171,383,0,10); 593 594 kneehlPID.Compute(); 595 596 if(map(analogRead(potkhl),171,383,0,10) > kneehlset) 597 { 598 kneehlout(); 599 delay(kneehloutp); 600 } 601 602 if(map(analogRead(potkhl),171,383,0,10) < kneehlset) 603 { 604 kneehlin(); 605 delay(kneehloutp); 606 } 607 608 609 if(map(analogRead(potkhl),171,383,0,10) == kneehlset) 610 { 611 kneehllow(); 612 kneehl_ = 1; 613 } 614 } 615 } 616 617//_______________________________________________________________________________________ 618 619 620 void kneehr() 621 { 622kneehr_ = 0; 623while(kneehr_ == 0) 624 { 625 kneehrinp = map(analogRead(potkhr),418,671,0,10); 626 627 kneehrPID.Compute(); 628 629 if(map(analogRead(potkhr),418,671,0,10) > kneehrset) 630 { 631 kneehrout(); 632 delay(kneehroutp); 633 } 634 635 if(map(analogRead(potkhr),418,671,0,10) < kneehrset) 636 { 637 kneehrin(); 638 delay(kneehroutp); 639 } 640 641 642 if(map(analogRead(potkhr),418,671,0,10) == kneehrset) 643 { 644 kneehrlow(); 645 kneehr_ = 1; 646 } 647 } 648 } 649 650//_______________________________________________________________________________________ 651 652 653 void hipvl() 654 { 655hipvl_ = 0; 656while(hipvl_ == 0) 657 { 658 hipvlinp = map(analogRead(pothvl),95,247,0,10); 659 660 hipvlPID.Compute(); 661Serial.print(hipvlinp); 662 if(map(analogRead(pothvl),95,247,0,10) < hipvlset) 663 { 664 hipvlout(); 665 delay(hipvloutp); 666 } 667 668 else if(map(analogRead(pothvl),95,247,0,10) > hipvlset) 669 { 670 hipvlin(); 671 delay(hipvloutp); 672 } 673 674 675 else if(map(analogRead(pothvl),95,247,0,10) == hipvlset) 676 { 677 hipvllow(); 678 hipvl_ = 1; 679 } 680 } 681 } 682 683//_______________________________________________________________________________________ 684 685 686 void hipvr() 687 { 688hipvr_ = 0; 689while(hipvr_ == 0) 690{ 691 hipvrinp = map(analogRead(pothvr),825,967,0,10); 692 693 hipvrPID.Compute(); 694 695 if(map(analogRead(pothvr),825,967,0,10) < hipvrset) 696 { 697 hipvrout(); 698 delay(hipvroutp); 699 } 700 701 if(map(analogRead(pothvr),825,967,0,10) > hipvrset) 702 { 703 hipvrin(); 704 delay(hipvroutp); 705 } 706 707 708 if(map(analogRead(pothvr),825,967,0,10) == hipvrset) 709 { 710 hipvrlow(); 711 hipvr_ = 1; 712 } 713 } 714 } 715 716//_______________________________________________________________________________________ 717 718 719 void hiphl() 720 { 721hiphl_ = 0; 722while(hiphl_ == 0) 723 { 724 hiphlinp = map(analogRead(pothhl),800,932,0,10); 725 726 hiphlPID.Compute(); 727 728 if(map(analogRead(pothhl),800,932,0,10) < hiphlset) 729 { 730 hiphlout(); 731 delay(hiphloutp); 732 } 733 734 if(map(analogRead(pothhl),800,932,0,10) > hiphlset) 735 { 736 hiphlin(); 737 delay(hiphloutp); 738 } 739 740 741 if(map(analogRead(pothhl),800,932,0,10) == hiphlset) 742 { 743 hiphllow(); 744 hiphl_ = 1; 745 } 746 } 747 } 748 749//_______________________________________________________________________________________ 750 751 752 void hiphr() 753 { 754hiphr_ = 0; 755while(hiphr_ == 0) 756 { 757 hiphrinp = map(analogRead(pothhr),94,204,0,10); 758 759 hiphrPID.Compute(); 760 761 if(map(analogRead(pothhr),94,204,0,10) < hiphrset) 762 { 763 hiphrout(); 764 delay(hiphroutp); 765 } 766 767 if(map(analogRead(pothhr),94,204,0,10) > hiphrset) 768 { 769 hiphrin(); 770 delay(hiphroutp); 771 } 772 773 774 if(map(analogRead(pothhr),94,204,0,10) == hiphrset) 775 { 776 hiphrlow(); 777 hiphr_ = 1; 778 } 779 } 780 } 781 782//_______________________________________________________________________________________ 783 784void HOME() 785{ 786kneevlset = 6; 787kneevrset = 7; 788kneehlset = 6; 789kneehrset = 6; 790hipvlset = 9; 791hiphlset = 9; 792hipvrset = 9; 793hiphrset = 9; 794 795 796 hipvl(); 797 delay(500); 798 799 hiphl(); 800 delay(500); 801 802 hiphr(); 803 delay(500); 804 805 hipvr(); 806 delay(500); 807 808 kneevl(); 809 delay(500); 810 811 //kneehl(); 812 //delay(500); 813 814 kneehr(); 815 delay(500); 816 817 kneevr(); 818 delay(500); 819 820 821 822 int servovl = 80; 823 int servovr = 80; 824 int servohl = 110; 825 int servohr = 30; 826 827 Wire.beginTransmission(8); 828 Wire.write(servovl); 829 Wire.write(servovr); 830 Wire.write(servohl); 831 Wire.write(servohr); 832 Wire.endTransmission(); 833} 834 835 836
Downloadable files
Concept of my motor controller
Concept of my motor controller
Documentation
Hip Joint
Again only a rough concept of what you could do.
Hip Joint
Hip Joint Stabilizer
Hip Joint Stabilizer
Hip Joint
Again only a rough concept of what you could do.
Hip Joint
Hip Joint Stabilizer
Hip Joint Stabilizer
Comments
Only logged in users can leave comments
AaronSilas
0 Followers
•0 Projects
Table of contents
Intro
12
0