(22-10-2010)
With the CM5 firmware ready, its now time to make a little program on the roboard, to test the servos.
This program will test the delay that the servos take moving between several angles, for example between 0º and 300º, or 0º to 100º, etc.
We will also run these tests on the same servos, in several conditions, with different loads, different supply voltages, different maximum torques, and both at the same time.
We will also test the torques with different voltages .
The idea is to profile the servos as best we can so that they can accurately be emulated in EZPysics.
(27-10-2010)
At this point, we wrote a little program that sends the servos to a start point, and then to an end point, and repeats the process for 10 times.
During this process, the program saves to a file, every servomotor positions and make a timestamp with uSeconds precision, each time it receives a new packet from Roboard containing the servos data.
We reached the following results so far:
1) With 10 servos we get and average of 283 packets per second coming from the CM5
2) Some servos are slower than others.
3) around 1 second to go from 0 to 1023 which means 300º/sec at 12V in accordance with AX12 specs
4) the worst delay between any two servos was around 21º for completion of the 300º. This translates to around 7% error.
5) The old AX12 servos seem to be more problematic and some of the physical connectors seem to be faulty maybe due to so much messing around.
After this we just took the file from the roboard, using winscp, and imported it to Exel, elaborating a graphic, to easily visualize, the behaviour of the servos in time.
In the picture below you can see that some AX12 servos are faster than others. The servos are told to move from position 0 to position 1023 and then back. During the transition, the CM5 continuously captures the servo positions (at around 280 times per second) and sends those positions to the RoBoard, which in turn stores them in CSV file format which Excel then transforms into nice graphs.
The top graph shows the difference between the maximum and minimum values among the 10 servos sampled at 280/sec. The bottom graph shows the absolute positions of the fastest and slowest servo.
As you can see, the RoBoard waits for all the servos to reach 0 before sending them off to 1023, then waiting for all of them to reach that position and so on.
See the full article to view the C code running on the RoBoard Linux
Links to related files:
1) Source Code for RoBoard and CM5
2) Excel Spreadsheet used for the above pretty pictures
3) CSV Output of the position samples as below
The output of the code below looks like this (first line has the servo IDs, right column has microsecond timestamp)
1 2 6 8 9 10 11 13 15 18 - 0 4 0 0 0 0 0 0 0 1 25334065 0 4 0 0 0 0 1 0 0 2 25338071 0 4 0 0 0 1 1 1 0 3 25342080 1 4 0 0 0 1 2 2 0 3 25347000 3 4 1 0 0 1 4 4 0 4 25351007 4 4 2 0 0 2 4 4 0 4 25355026 5 4 2 0 0 2 4 4 0 4 25359031 5 4 2 0 1 2 4 6 1 4 25363040 6 4 3 2 1 3 5 6 2 4 25367045 6 4 3 3 1 3 5 6 2 5 25371055 5 4 3 4 2 3 5 6 2 4 25375063 6 5 3 4 2 3 5 6 4 5 25379065 6 4 3 4 2 3 5 6 4 4 25383074 6 4 4 5 2 4 5 6 6 4 25387082 6 4 3 5 3 3 5 6 6 4 25392006 6 4 4 5 3 3 5 6 6 4 25396016 6 4 3 5 3 3 5 6 6 4 25400021 5 3 3 5 3 3 5 4 6 4 25404023 4 3 3 5 3 3 4 4 6 3 25408037 4 2 4 5 3 3 4 3 4 3 25412046 4 1 3 5 3 3 4 4 6 3 25416049 3 1 3 5 3 3 3 3 6 3 25420056 3 1 3 4 4 3 2 3 6 3 25424065 3 1 3 4 3 3 2 3 6 3 25428075 2 1 3 4 3 3 2 2 6 3 25432085 1 1 3 3 3 2 2 2 4 2 25436993 1 1 3 3 4 2 2 1 4 2 25441003 1 1 3 3 3 2 2 2 3 2 25445016 0 1 4 3 3 2 2 1 2 2 25449022 1 1 3 3 4 2 2 1 2 2 25453029 0 1 3 3 3 2 2 1 2 2 25457032 0 1 2 3 3 2 2 1 2 2 25461044 0 1 2 2 3 2 2 1 2 2 25465051 0 1 2 2 2 2 2 1 2 2 25469058 1 1 2 1 2 2 2 1 2 2 25473066 0 1 2 1 1 2 2 1 2 2 25477082 0 1 2 0 1 2 2 1 2 2 25481995 1 1 2 0 1 2 2 1 2 2 25484079 1 1 2 0 2 2 1 1 2 2 25489008 1 1 1 1 2 2 2 1 2 2 25493017 1 1 2 1 2 2 2 2 2 3 25497027 2 3 2 3 3 4 3 4 2 4 25501036 4 4 4 3 5 6 7 7 6 5 25505051 6 5 8 6 7 7 8 8 7 8 25509058 9 9 10 7 9 11 9 9 9 10 25513062 11 11 10 10 11 12 12 12 12 13 25517069 13 12 12 13 13 13 13 13 14 16 25521993 16 14 16 16 15 16 16 17 17 20 25526002 19 20 19 18 20 19 21 20 20 22 25530007 23 22 22 22 22 21 23 23 22 25 25534015 27 25 26 24 27 26 28 25 26 28 25538028 29 30 29 29 31 29 30 30 31 32 25542043 32 33 35 33 36 32 36 34 34 35 25546046 37 36 38 38 40 35 39 38 39 38 25550054 40 41 42 41 43 39 45 42 42 41 25554064 44 45 45 45 49 45 47 45 46 46 25558077 49 48 51 51 52 47 52 51 51 50 25562990 52 53 54 54 58 51 57 54 55 54 25567000 57 58 60 58 60 56 61 58 58 60 25571006 62 61 63 61 65 60 65 63 63 63 25575026 66 67 68 68 68 64 70 67 66 68 25579031 70 70 73 74 74 69 74 71 71 72 25583040 76 76 77 77 79 75 79 75 75 75 25587050 82 80 84 85 85 79 84 79 81 81 25592026 89 84 87 90 88 85 89 84 85 85 25596054 93 89 92 95 93 89 92 88 88 89 25600068 96 93 97 99 99 93 97 95 94 94 25604998 100 98 103 105 105 99 102 98 100 97 25609047 106 104 107 110 110 104 108 103 104 105 25613995 110 109 112 114 114 108 111 110 108 109 25618025 116 113 115 118 118 115 117 116 115 115 25622063 120 120 121 124 125 118 121 119 120 122 25627011 125 125 125 129 129 122 125 124 125 127 25631027 132 128 131 134 136 128 130 130 129 132 25635051 134 134 134 139 141 135 138 135 135 136 25640011 142 139 141 145 146 139 142 138 139 142 25644050 145 145 146 149 150 144 148 142 143 146 25648075 150 149 152 154 154 149 152 150 148 151 25652994 156 155 156 160 161 154 159 153 153 157 25657050 160 160 162 166 164 159 164 158 159 162 25661999 166 166 165 172 169 165 171 163 162 168 25666018 171 171 172 176 175 169 175 169 169 174 25670025 177 177 175 181 180 175 182 174 175 178 25674072 183 181 181 188 186 179 185 179 180 185 25679024 187 188 186 191 190 184 190 184 185 190 25683050 191 193 191 197 195 188 197 188 191 194 25687071 197 198 197 204 202 196 202 195 196 199 25692034 202 203 201 211 208 202 207 200 200 206 25696071 207 208 207 213 213 207 212 205 204 210 25701000 211 214 213 220 219 214 219 212 210 216 25705059 217 222 219 224 222 218 227 216 215 221 25710000 221 227 223 230 227 224 229 222 220 228 25714026 228 231 230 234 233 230 238 227 227 233 25718982 231 239 234 240 239 236 244 234 233 239 25723034 237 243 239 244 244 241 249 239 238 243 25727057 242 248 245 251 250 249 254 244 245 250 25732018 247 257 251 255 256 254 259 249 251 253 25736049 252 262 256 259 263 260 264 256 256 261 25740987 256 266 262 264 269 266 272 263 263 266 25745040 263 273 266 270 273 270 276 268 267 273 25749981 270 277 273 275 279 276 282 273 273 279 25753999 274 284 278 280 284 280 287 279 278 285 25758026 280 289 285 286 290 286 292 284 283 291 25762985 287 296 290 292 295 292 298 288 287 296 25767017 292 302 294 297 301 296 303 296 294 301 25771029 296 306 300 303 304 301 308 298 298 304 25775047 301 311 304 306 309 306 312 304 302 309 25779056 307 317 309 312 315 310 317 309 308 314 25783062 312 321 313 316 319 315 323 315 311 319 25787066 315 326 317 323 322 320 328 319 318 326 25792017 321 333 323 328 329 328 336 324 323 330 25796067 326 337 328 334 334 333 341 330 328 337 25800999 331 343 333 339 338 337 347 336 333 342 25805037 336 349 339 345 345 344 352 341 339 348 25810007 344 355 344 349 349 348 356 348 344 352 25814030 349 362 348 353 355 353 361 353 348 356 25818070 355 364 355 361 362 361 367 359 354 363 25823028 362 372 359 365 368 366 372 364 360 366 25827051 366 375 363 369 374 373 381 371 364 372 25832032 373 383 370 376 378 377 386 376 371 376 25836977 379 389 375 381 382 383 391 382 373 382 25841017 387 396 381 387 391 391 398 391 381 390 25845990 391 404 385 393 394 395 402 396 384 394 25850014 395 410 391 397 399 401 410 400 391 401 25855120 401 416 396 403 405 407 415 407 393 407 25859030 406 421 402 408 410 414 423 413 398 414 25863988 412 427 408 415 418 418 427 418 403 418 25868035 418 433 415 420 422 425 435 424 408 424 25872998 422 439 421 425 430 430 440 429 414 429 25877049 430 445 427 430 435 438 447 435 418 435 25882005 435 451 432 434 442 446 451 444 425 440 25886046 442 459 437 441 448 451 459 449 430 447 25891002 448 465 444 446 455 459 466 455 437 453 25895058 456 469 449 452 459 463 473 461 444 461 25900013 461 476 455 456 464 471 481 468 447 465 25904055 468 481 460 463 472 475 484 473 453 471 25909012 473 489 467 469 477 482 490 480 459 478 25913050 479 496 471 474 484 489 498 485 465 484 25918014 486 500 479 479 489 494 504 490 471 490 25922049 490 507 485 486 495 505 511 497 477 495 25926996 497 513 490 491 503 510 517 503 483 504 25931065 502 521 496 497 509 514 521 509 490 511 25935978 509 525 501 501 515 520 529 515 496 515 25939991 514 531 508 507 521 525 534 522 501 521 25944001 520 536 512 511 528 530 540 529 505 529 25948011 525 543 518 516 533 537 545 535 512 533 25952018 530 549 523 521 536 543 553 541 516 537 25956023 538 555 529 528 543 548 558 546 522 544 25960029 542 561 533 531 550 554 564 550 528 552 25964040 550 567 539 537 555 560 571 558 533 557 25968049 553 573 544 542 560 565 576 561 539 564 25972061 560 578 551 546 566 571 582 568 544 568 25976975 565 582 555 553 573 578 587 572 550 574 25980983 571 588 561 556 578 581 593 577 554 579 25984993 578 593 568 563 584 587 597 586 561 585 25988998 582 599 571 569 590 593 603 591 565 589 25993005 589 605 576 575 597 599 607 596 571 595 25997024 594 611 582 578 602 603 614 601 576 600 26001053 599 615 586 585 607 608 618 607 582 606 26005040 607 622 593 590 611 615 625 613 587 611 26009989 612 628 599 597 617 622 631 619 594 618 26014041 619 632 605 603 622 627 637 624 600 622 26018972 623 641 612 608 626 633 642 632 605 629 26023015 631 646 617 614 632 640 649 636 612 635 26027984 635 651 622 619 636 645 653 641 618 640 26031999 641 657 627 626 643 652 659 649 622 644 26036044 647 661 634 633 649 655 666 655 628 652 26041014 650 667 640 636 655 661 670 661 633 655 26045039 655 674 644 642 663 667 679 663 639 661 26049997 663 680 650 646 667 671 683 670 644 667 26054040 667 684 655 653 672 680 689 675 647 673 26058990 673 689 661 656 679 684 694 682 653 679 26063048 679 695 666 662 684 690 702 686 659 685 26067993 683 701 670 668 690 696 705 691 664 691 26072044 689 706 675 673 694 701 711 699 669 696 26076998 693 711 679 679 698 707 717 703 674 702 26081048 698 718 686 684 706 712 722 708 682 708 26086003 704 722 692 690 711 717 730 713 685 713 26090043 708 729 697 695 717 724 735 720 690 718 26095005 715 733 704 701 723 728 741 726 696 724 26099050 720 741 708 706 731 734 746 731 700 727 26103996 725 746 716 712 734 740 753 737 706 735 26108055 731 752 720 715 740 747 759 743 711 740 26113001 735 759 724 720 745 751 765 747 719 746 26117057 741 764 730 727 750 758 771 753 724 752 26122012 747 771 735 731 757 766 776 758 728 757 26126057 753 775 742 739 760 770 781 764 735 763 26131037 760 782 746 744 767 778 789 772 739 770 26135053 764 787 753 748 771 784 795 777 745 775 26140002 771 791 759 754 775 790 801 783 750 781 26144969 774 799 763 761 785 798 807 788 756 787 26149007 780 803 770 767 791 802 811 795 761 792 26153966 783 810 776 771 797 809 817 801 768 800 26158009 790 814 780 777 803 813 822 808 774 804 26162975 796 818 787 780 807 819 828 813 778 811 26167018 801 825 791 786 814 826 834 819 785 815 26171969 809 832 798 790 820 830 840 824 790 823 26176023 813 836 803 797 825 836 847 830 797 828 26180976 821 840 808 802 831 842 851 837 802 833 26185024 824 845 813 809 837 849 859 842 808 839 26189985 830 851 820 814 842 854 862 848 815 845 26194046 834 858 825 819 848 861 870 855 821 852 26198990 840 863 830 823 854 865 874 859 827 858 26203043 846 871 836 829 861 871 881 865 833 865 26208011 852 876 842 835 866 875 887 873 839 868 26212966 859 881 847 843 870 883 892 877 844 874 26217031 863 887 851 847 875 888 897 882 849 879 26221986 869 892 858 853 882 895 904 889 852 884 26226054 874 897 863 858 888 899 909 894 859 891 26231049 880 903 868 865 894 907 916 900 864 899 26235974 884 908 876 870 901 910 919 905 870 904 26240022 890 914 880 877 907 917 927 911 875 910 26245000 895 920 887 881 911 922 933 914 883 914 26249035 902 926 892 888 916 928 938 922 887 922 26254012 908 931 898 893 921 934 943 928 894 926 26258962 913 938 901 899 927 940 950 933 899 934 26263035 920 943 908 904 932 945 956 939 906 938 26268004 924 948 912 910 939 952 960 945 909 944 26272966 931 955 918 916 945 958 967 951 915 949 26277040 936 962 921 921 950 962 973 957 921 955 26282007 943 965 928 928 955 969 981 964 925 959 26286972 949 973 936 932 962 976 987 970 932 966 26291046 954 978 939 940 966 981 992 977 938 971 26295998 960 985 946 945 972 988 997 982 944 980 26300050 965 989 949 952 977 994 1005 989 949 983 26305012 971 995 957 957 983 999 1010 994 955 990 26309971 978 1001 962 964 989 1006 1015 1000 960 996 26314032 981 1008 969 970 994 1013 1020 1007 968 1004 26318995 989 1012 972 977 1001 1019 1023 1011 972 1009 26323034 994 1017 979 980 1008 1023 1023 1018 977 1015 26327996 1002 1023 984 988 1013 1023 1023 1022 983 1019 26332044 1005 1023 990 994 1016 1023 1023 1023 990 1023 26336978 1011 1023 994 998 1021 1023 1023 1023 995 1023 26341007 1016 1023 1000 1004 1023 1023 1023 1023 999 1023 26344998 1020 1023 1005 1008 1023 1023 1023 1023 1006 1023 26349005 1023 1023 1010 1014 1023 1023 1023 1023 1011 1023 26353018 1023 1023 1015 1019 1023 1023 1023 1023 1017 1023 26357998 1023 1023 1019 1022 1023 1023 1023 1023 1020 1023 26362018 1023 1023 1016 1023 1023 1023 1023 1023 1023 1023 26366031 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26370036 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26374960 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26378970 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26382974 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26386984 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26391015 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26395003 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26399005 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26403013 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26407021 1023 1023 1023 1023 1023 1023 1023 1023 1023 1023 26411054 1023 1023 1023 1023 1023 1023 1022 1023 1023 1023 26415041 1023 1023 1023 1023 1023 1023 1019 1023 1023 1023 26419955 1023 1020 1022 1023 1021 1019 1014 1022 1023 1020 26423976 1023 1017 1019 1021 1016 1015 1012 1020 1021 1018 26428002 1022 1012 1016 1019 1014 1011 1008 1015 1019 1013 26432030 1019 1010 1012 1014 1011 1006 1003 1011 1015 1009 26436988 1015 1004 1009 1011 1007 1001 1000 1008 1012 1005 26441033 1011 1000 1005 1006 1000 996 996 1005 1008 1002 26445037 1009 996 1002 1002 994 993 990 1000 1004 997 26449972 1002 990 995 997 991 987 986 994 999 991 26454021 998 987 993 994 987 983 981 989 994 987 26458954 994 983 988 988 982 978 976 986 990 982 26462980 988 978 981 983 978 975 970 980 985 977 26467038 983 974 979 979 973 969 966 975 982 972 26471975 979 968 973 973 968 963 960 970 975 967 26475995 974 964 969 969 964 959 956 964 972 963 26480018 969 957 962 965 960 955 950 959 965 956 26484985 964 953 959 958 955 950 947 955 961 953 26489017 960 948 953 955 950 944 941 951 957 949 26493043 954 944 949 949 943 940 937 945 952 945 26497973 949 939 943 943 939 934 931 939 946 939 26502034 944 932 937 938 934 928 927 934 939 934 26506968 941 929 933 932 927 923 922 932 936 931 26511002 934 924 929 928 924 918 917 925 932 925 26514997 930 917 921 923 919 915 909 920 925 918 26519966 923 913 919 918 915 909 906 915 922 913 26523993 919 908 913 913 908 905 901 911 918 909 26528022 914 904 909 908 907 899 897 906 913 903 26532041 908 897 903 903 901 893 891 899 907 898 26537016 903 892 900 898 894 889 886 895 902 892 26541061 896 887 893 893 887 883 880 890 898 885 26545964 892 882 889 889 884 879 875 886 892 879 26550027 885 878 883 883 875 872 870 880 887 874 26554971 883 872 879 877 872 867 865 875 882 870 26558992 878 867 873 871 868 863 860 869 875 866 26562999 871 861 867 866 863 858 853 864 871 861 26567966 866 855 861 862 857 853 848 858 865 855 26572009 861 849 857 856 852 846 843 852 861 849 26576023 856 845 851 851 847 840 838 848 856 844 26580031 851 839 848 846 841 836 834 843 851 839 26584953 845 834 842 840 836 831 828 838 848 834 26588969 840 828 837 835 832 825 822 831 842 827 26593038 834 824 830 829 826 820 818 826 836 822 26597978 830 821 826 824 821 814 812 822 833 817 26601990 825 814 820 820 816 810 808 817 826 813 26605992 821 810 815 815 811 805 805 813 822 807 26610009 813 804 810 810 806 799 798 806 813 802 26614950 809 799 805 803 799 793 792 799 809 795 26619011 803 792 800 799 793 790 787 795 803 791 26623032 798 788 796 793 789 782 781 788 798 784 26627964 790 781 789 788 782 776 775 783 792 778 26632029 786 775 783 785 775 770 769 776 786 772 26636968 780 773 778 779 770 766 765 772 782 768 26641006 775 764 771 774 767 758 758 765 773 760 26645961 770 759 765 769 758 752 753 758 768 755 26649990 764 755 760 763 753 748 746 753 761 749 26654039 760 748 754 757 749 740 741 747 756 742 26659005 752 743 748 752 743 734 735 744 750 737 26663960 748 733 743 746 739 728 730 737 745 731 26668030 741 729 736 740 733 724 722 732 739 727 26672983 735 723 730 735 728 717 717 725 735 719 26677949 730 719 724 729 722 713 712 720 727 715 26682000 724 712 720 723 717 707 709 714 723 709 26686963 717 707 714 718 710 703 702 708 716 705 26691042 713 701 708 712 705 696 696 703 711 698 26695987 707 696 704 709 698 690 690 697 705 694 26700033 703 689 697 703 692 685 684 691 700 686 26704996 697 686 691 698 689 680 679 686 694 681 26709957 691 680 685 693 681 671 674 682 689 675 26714017 687 673 678 689 679 666 667 675 684 668 26718979 682 665 674 681 672 661 663 670 680 662 26723947 674 661 670 676 667 657 656 662 674 656 26727994 671 654 663 670 660 650 651 659 668 651 26732965 664 651 657 665 655 645 645 651 662 646 26737007 659 644 653 659 649 640 639 649 658 642 26741981 653 639 648 654 644 633 635 640 651 634 26746024 648 632 643 647 638 626 629 635 647 629 26751001 642 626 636 643 632 620 622 630 641 622 26755029 638 620 630 637 627 614 617 622 635 618 26759977 632 613 624 633 622 609 611 618 632 613 26764032 625 607 617 626 617 603 605 612 625 606 26768995 620 600 613 621 613 598 600 607 620 600 26773956 612 597 606 615 608 592 594 599 613 592 26778010 608 589 601 608 599 587 588 594 609 586 26782979 599 583 593 604 595 580 582 585 601 581 26787023 594 577 587 596 588 573 578 579 596 576 26791977 587 572 583 592 584 567 572 573 588 568 26796025 580 566 576 586 576 561 564 568 583 564 26800996 575 558 571 578 569 556 557 561 578 559 26805023 568 553 563 573 565 550 552 556 571 552 26809976 561 545 559 567 557 544 545 549 565 545 26814023 555 538 553 562 552 537 539 545 560 537 26818960 549 534 545 556 546 530 532 537 553 532 26823014 542 525 540 550 541 525 526 532 549 526 26827971 538 521 534 544 533 519 520 526 543 520 26832013 531 513 529 537 530 511 513 519 537 513 26836973 524 508 522 534 522 506 509 510 532 507 26841023 517 502 515 528 517 499 501 503 525 501 26845959 511 493 509 521 509 493 495 498 519 492 26850003 507 488 505 516 504 484 488 490 514 487 26855055 498 481 498 510 497 479 483 486 505 479 26859029 492 476 490 506 490 473 477 480 501 473 26863991 487 468 486 499 484 466 471 475 494 467 26868950 480 463 477 493 478 461 463 467 488 462 26873011 474 455 472 487 472 454 457 461 481 455 26877975 468 448 467 482 465 447 449 455 473 448 26882026 461 442 459 476 459 440 445 448 468 441 26886991 455 438 454 470 455 434 438 443 462 437 26891959 448 430 449 464 449 427 434 435 457 431 26896010 441 424 441 459 442 424 426 429 450 425 26900996 435 417 436 454 435 416 422 424 445 421 26905027 427 411 430 449 430 411 415 416 439 414 26909998 422 405 424 442 422 405 409 411 434 410 26914959 416 399 416 437 419 400 401 407 427 402 26919010 409 391 410 432 411 393 398 400 423 396 26923982 405 386 404 426 405 386 390 395 415 389 26928951 400 378 398 420 399 380 384 388 412 382 26933004 393 372 392 416 394 374 378 381 405 377 26937972 389 365 388 409 389 368 372 373 402 372 26942017 382 362 381 406 385 362 366 370 396 366 26946988 376 355 376 399 380 358 361 362 391 362 26951937 371 350 370 394 374 352 356 358 385 357 26956002 364 344 364 387 368 346 352 350 381 353 26960984 357 337 359 382 362 342 347 344 375 348 26965020 352 333 354 378 356 336 342 340 371 341 26969965 344 326 349 370 352 330 335 336 365 336 26974022 339 322 342 366 346 324 329 330 361 329 26978971 332 315 338 361 338 318 322 323 354 325 26983948 327 310 332 354 336 312 317 319 350 318 26987990 321 304 327 349 329 308 311 311 344 313 26992977 317 298 320 345 323 302 307 308 339 308 26997025 310 291 314 337 319 298 300 299 332 303 27001996 306 285 309 333 315 292 297 296 328 296 27006947 299 280 305 327 308 287 290 290 321 291 27011937 294 273 301 322 303 281 285 284 317 284 27015992 287 266 293 316 298 275 279 279 310 278 27020987 283 259 288 310 293 270 273 273 306 273 27025023 277 254 283 305 288 266 266 268 300 265 27029982 269 248 276 297 283 260 261 262 294 258 27034977 264 242 270 292 276 253 256 256 288 253 27039959 256 236 263 287 269 246 251 248 283 247 27044012 252 230 256 281 264 240 246 243 276 241 27049011 246 224 252 274 256 234 238 236 272 236 27053983 241 217 244 269 250 228 233 232 266 229 27058942 ----------------------------------------------------------------------------------------
#include <stdio.h> #include <string.h> #include <stdlib.h> #include <fcntl.h> #include <sys/types.h> #include <time.h> #include <linux/serial.h> #include <sys/ioctl.h> #include <sys/time.h> #include "dynamixel.h" #include <termios.h> #include <unistd.h> int fdFTDI=-1; typedef struct SERVOSDATA { unsigned char ID; //1st byte unsigned int Pos;//2nd byte unsigned char Error; }ServosDataStructure_Type; typedef struct SERVOAVERAGE { unsigned char ID; float Pos; }ServosAverageStructure_Type; #define AverageCalcNumberOfPositions 20 #define MaxServos 20 ServosDataStructure_Type ServosInputData[MaxServos]; ServosDataStructure_Type ServosOutputData[MaxServos]; ServosDataStructure_Type ServosEndPos[MaxServos]; unsigned long int ServosTimeCounter[MaxServos]; unsigned char New_buffer[150]; unsigned char New_buffer_count=0; unsigned char New_buffer_ready=0; unsigned char u8CurrentInputArray=0; unsigned char u8CurrentOutputArray=0; unsigned char Readed=0; unsigned char CRC_Calc(unsigned char *Array, unsigned char length); int usb_open(char *dev_name, float baudrate); unsigned char NumberOfServos=0; //unsigned int NumberOfBytes=0; //unsigned int NumberOfGoodPackets=0; unsigned int NumberOfPackesPerSec=0; unsigned int PacksCounter=0; unsigned int GoodPacks=0; //unsigned int NumberOfBadPackets=0; //unsigned int NumberOfFailedHeaders=0; #define TOTALWRITE 500 FILE *fsFile; #define UseFile 1 #define stiff 8 long time1 = 0; long time2 = 0; void nonblock(int state) { struct termios ttystate; //get the terminal state tcgetattr(STDIN_FILENO, &ttystate); if (state==1) { //turn off canonical mode ttystate.c_lflag &= ~ICANON; //minimum of number input read. ttystate.c_cc[VMIN] = 1; } else if (state==0) { //turn on canonical mode ttystate.c_lflag |= ICANON; } //set the terminal attributes. tcsetattr(STDIN_FILENO, TCSANOW, &ttystate); } int main(int ac, unsigned char **args) /*FOLD00*/ { char FileName[100]; unsigned int CompCounter=0; unsigned int state=0; unsigned char u8TestServoNumber=0; unsigned char pause=0; char KeyBoardChar=0; int KeyboardCount=0; int deviceNumber=0; float portSpeed=0; int temporary=0; char stringPort[20]; struct termios termios_p; unsigned int j=0xffff; unsigned int i=0; unsigned long TimeCalcule=0; time_t TimeCounterStartSec=0; time_t TimeCounterEndSec=0; suseconds_t TimeCounterStartuSec=0; suseconds_t TimeCounterEnduSec=0; struct timeval detail_time; unsigned char fileHeaderPlaced=0; //unsigned char *broadcast1=&New_buffer[11]; /******************************************************************************\ |* Variables to configure the file name, the servo to test(0 to all servos) *| |* the number of repetitions, etc *| \******************************************************************************/ #define BEGIN_ANGLE 0x0001 #define END_ANGLE 0x03FF #define NUMBER_TESTS 20 memcpy(FileName,"testingTimes.txt",17);// file name u8TestServoNumber=0;//test all servos deviceNumber=(atoi(args[1])); portSpeed= atoi(args[2]); sprintf(stringPort,"/dev/ttyUSB%d",deviceNumber); printf("Opening Port %s at %f Baud\n\r", stringPort , portSpeed); fdFTDI = usb_open( stringPort , portSpeed ); /****************************************************************************** fdFTDI = open(stringPort,O_NONBLOCK|O_RDWR|O_NOCTTY); if(fdFTDI==-1) { //shit hapened puts("Error Opening"); } puts("Port Open\n\rApply Settings"); termios_p.c_iflag=IGNPAR; termios_p.c_oflag=0; termios_p.c_cflag= B115200|CREAD|CS8|CLOCAL; termios_p.c_lflag=0; termios_p.c_cc[VINTR] = 0; // Ctrl-c termios_p.c_cc[VQUIT] = 0; // Ctrl-\ termios_p.c_cc[VERASE] = 0; // del termios_p.c_cc[VKILL] = 0; // @ termios_p.c_cc[VEOF] = 0; // Ctrl-d termios_p.c_cc[VTIME] = 0; // inter-character timer unused termios_p.c_cc[VMIN] = 0; // blocking read until 1 character arrives termios_p.c_cc[VSWTC] = 0; // '' termios_p.c_cc[VSTART] = 0; // Ctrl-q termios_p.c_cc[VSTOP] = 0; // Ctrl-s termios_p.c_cc[VSUSP] = 0; // Ctrl-z termios_p.c_cc[VEOL] = 0; // '' termios_p.c_cc[VREPRINT] = 0; // Ctrl-r termios_p.c_cc[VDISCARD] = 0; // Ctrl-u termios_p.c_cc[VWERASE] = 0; // Ctrl-w termios_p.c_cc[VLNEXT] = 0; // Ctrl-v termios_p.c_cc[VEOL2] = 0; // '' tcflush(fdFTDI, TCIFLUSH); if(tcsetattr(fdFTDI,TCSANOW, &termios_p)==-1) { puts("Error setting configurations"); return 0; } /******************************************************************/ if(fdFTDI<0) // open port USB0 at 1 to 1Mbps 3 to 500Kbps { puts("Error Opening Port"); return 0; } puts("Port Open."); time1 = time(0); nonblock(1); puts("Press 's' to start."); j=0xFFFF; while(1) { if((j==0xffff) || (pause==2)) { read_servos(); } else if (read_servos() == 1) { gettimeofday(&detail_time,NULL); TimeCounterEndSec = (detail_time.tv_sec); TimeCounterEnduSec = (detail_time.tv_usec); //printf("time: %ld\n",TimeCounterEnd-TimeCounterStart); switch (state) { case 0: if(fileHeaderPlaced==0) { gettimeofday(&detail_time,NULL); TimeCounterStartSec=TimeCounterEndSec = (detail_time.tv_sec); TimeCounterStartuSec=TimeCounterEnduSec = (detail_time.tv_usec); for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { fprintf(fsFile,"%d;",ServosInputData[CompCounter].ID); } fprintf(fsFile,"\n"); fileHeaderPlaced=1; New_buffer_prepare(); } for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { ServosOutputData[CompCounter].ID=ServosInputData[CompCounter].ID; ServosOutputData[CompCounter].Pos=BEGIN_ANGLE; //ServosEndPos[CompCounter].Pos=1024; ServosTimeCounter[CompCounter]=0; } if(New_buffer_ready>0) break; New_buffer_ready=1; //write_servos(); i=0; state = 1; printf("\rTest number: %d\n",j+1); case 1: for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { if((ServosInputData[CompCounter].Pos == BEGIN_ANGLE) && (ServosTimeCounter[CompCounter]==0) ) { i++; ServosTimeCounter[CompCounter]=1; } if(i==NumberOfServos) { for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { ServosOutputData[CompCounter].ID=ServosInputData[CompCounter].ID; ServosOutputData[CompCounter].Pos=END_ANGLE; //ServosEndPos[CompCounter].Pos=1024; ServosTimeCounter[CompCounter]=0; } if(New_buffer_ready==1) break; New_buffer_ready=1; //write_servos(); //puts("\nreset position"); state=2; i=0; } } break; case 2: for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { if(ServosInputData[CompCounter].Pos == END_ANGLE && ServosTimeCounter[CompCounter]==0) { //printf("i: %d ID:%d\n",i,ServosInputData[CompCounter].ID); //printf("time: %ld\n",TimeCounterEnd-TimeCounterStart); //ServosTimeCounter[CompCounter]=(TimeCounterEnd-TimeCounterStart); i++; } if(i==NumberOfServos) { state=3; } } break; case 3: //puts("never here"); /*for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { //printf("servo: %d",ServosTimeCounter[CompCounter]); fprintf(fsFile,"%d;",ServosTimeCounter[CompCounter]); } fprintf(fsFile,"\n");*/ state =0; if(j==(NUMBER_TESTS-1)) { fclose(fsFile); puts("\rend"); j=0xffff; //exit(1); } else { j++; if(pause==1) pause=2; } break; } for(CompCounter=0;CompCounter<NumberOfServos;CompCounter++) { //printf("servo: %d",ServosTimeCounter[CompCounter]); fprintf(fsFile,"%d;",ServosInputData[CompCounter].Pos); } TimeCalcule = ((TimeCounterEndSec-TimeCounterStartSec)*1000000); TimeCalcule+= (TimeCounterEnduSec-TimeCounterStartuSec); fprintf(fsFile,"%d\n",TimeCalcule); } ioctl(0, FIONREAD, &KeyboardCount); if(KeyboardCount>0) { if (read (0, &KeyBoardChar, 1) > 0) //if(KeyboardCount>0) { //puts("dumass"); //KeyBoardChar=getchar(); if(KeyBoardChar=='x') { //fclose(fsFile); puts("\rexit"); exit(1); } else if(KeyBoardChar=='g') { printf("\rGoodPacks/s %d\n",NumberOfPackesPerSec); } else if(KeyBoardChar=='p') { if((j<0xFFFF) && (pause==0)) { puts("\rpause"); pause=1; } else { puts("\rNo pausing to do."); } } else if(KeyBoardChar=='b' ) { if(j==0xFFFF) { puts("\rAlready stoped"); } else { fprintf(fsFile,"break\n"); fclose(fsFile); puts("\rbreak"); j=0xffff; } } else if(KeyBoardChar=='s') { if(pause==2) { puts("\rContinuing..."); pause=0; } else if(pause==0) { if(j==0xFFFF) { puts("\r "); fsFile = fopen(FileName,"a+"); if(fsFile==NULL) { puts("\rError Opening Log File"); return; } fprintf(fsFile,"Start\n"); puts("\rStarted."); fileHeaderPlaced=0; state=0; j=0; } else { puts("\rAlready running..."); } } } else { puts("\rUnknown Command"); } /*else if(KeyBoardChar=='r') { j=0; }*/ KeyBoardChar=0; } } write_servos(); time2=time(0); if(time2>time1) { NumberOfPackesPerSec=PacksCounter; PacksCounter=0; time1=time(0); } //write_terminal(); } } New_buffer_prepare() { unsigned short i; i = 7 + NumberOfServos*3 + 1; New_buffer[0] = 0xFA; New_buffer[1] = 0xFA; New_buffer[2] = i>>8; // msb New_buffer[3] = i&0xff; // lsb New_buffer[4] = 0xff; New_buffer[5] = 0xff; New_buffer[6] = 0xfe; New_buffer[7] = NumberOfServos*3+4; // sync_write packet length New_buffer[8] = 0x83; // broadcast SYNC_WRITE New_buffer[9] = 0x1E; // start table position of "goal position" New_buffer[10] = 2; // data to be written New_buffer_count= (11 + (3*NumberOfServos) + 1 + 1); } int write_terminal(void) { //do nothing return 0; } write_servos() { static unsigned char buf[512]; static short buf_index = -2; static short buf_count; int i; unsigned int j; // superpacket: msbLen lsbLen [ ff ff fe buflen 83 start count id0 data0 id1 data1... checksum ] [ ff ff fe buflen 83 start.... ] .. superpacketChecksum if (New_buffer_ready==1) { for(i=0;i<NumberOfServos;i++) { j = (unsigned short)ServosOutputData[i].Pos; New_buffer[i*3+0+11] = ServosOutputData[i].ID; New_buffer[i*3+1+11] = j&0xff; New_buffer[i*3+2+11] = j>>8; } for(j=0,i=6;i<New_buffer_count-2;i++) //checksum { j += New_buffer[i]; } New_buffer[New_buffer_count-2] = ((0xffffffff-j)& 0xff); for(j=0,i=2;i<New_buffer_count-1;i++) // superpacketChecksum { j += New_buffer[i]; } New_buffer[New_buffer_count-1] = ((0xffffffff-j)& 0xff); /* if(New_buffer_ready == 0) New_buffer_ready = 1; */ /* printf("\nwr: "); for(i=0;i<New_buffer_count; i++) printf("%02x ",New_buffer[i]); printf("\n"); */ bcopy (New_buffer, buf, New_buffer_count); // try also buf=new_buffer New_buffer_ready = 2; buf_index = 0; buf_count = New_buffer_count; New_buffer_ready=2; } if(New_buffer_ready == 2) { switch (write(fdFTDI, &buf[buf_index], 1)) { case 1: //printf("%02x ",buf[buf_index]); if (buf_index == (buf_count-1)) { buf_index = -1; New_buffer_ready=0; //puts("\n"); } else { buf_index++; } break; case 0: // still writing previous byte break; case -1: // some error (or maybe still writing previous byte we dont know) break; default:; } } else { //do nothing } } int read_servos(void) { unsigned char ReadC=0; static unsigned char cPackLength=0; static int BytesCounter=0; unsigned char CalculatedCRC=0; //static int NumberOfPackesPerSec=0; unsigned char GotCRC=0; static unsigned char FoundFF=0; static unsigned char ServosCounter=0; static unsigned char u8InputArray[150]; //printf("in\n"); if(read(fdFTDI,&ReadC,1)<1) { return -1; } //NumberOfBytes++; //printf("%x",ReadC); if(FoundFF<2)// looking for the header { if(ReadC==0xFF) { FoundFF++; if(FoundFF == 2) { //printf("rd: FFFF"); } } else // if no header byte found, is a byte to trash { if(FoundFF==1) { // NumberOfFailedHeaders+=2; //FailedBytes+=2; FoundFF=0; } else { //NumberOfFailedHeaders++; //FailedBytes++; } } } else if(FoundFF==2) { cPackLength=ReadC; u8InputArray[0]=ReadC; if(NumberOfServos>0) { if(cPackLength!=(NumberOfServos*3)) { FoundFF=0; NumberOfServos=0; //printf("LenERR\n\rfs"); } else { FoundFF=4; BytesCounter=0; ServosCounter=0; } } else { FoundFF=3; BytesCounter=0; NumberOfServos=(cPackLength/3); New_buffer_prepare(); ServosCounter=0; } } else if(FoundFF==3 || FoundFF ==4) { //printf(" %d",ReadC); if(BytesCounter==(3*NumberOfServos)) { //Finish read all pack //puts("comp CRC"); GotCRC=ReadC; u8InputArray[BytesCounter+1]=ReadC; CalculatedCRC=CRC_Calc(&u8InputArray[1],(3*NumberOfServos)); //printf("\nCalc %d Got: %d",CalculatedCRC,GotCRC); if(GotCRC!=CalculatedCRC) { //NumberOfBadPackets++; //puts("PacketError"); FoundFF=0; BytesCounter=0; return -1; } else { //good packet found //puts("goodPack"); PacksCounter++; /* printf("\nrd: FF FF"); for(BytesCounter=0;BytesCounter< (NumberOfServos*3+2);BytesCounter++) { printf(" %02x", u8InputArray[BytesCounter]); } */ //printf(" GoodPacks/s %d\r",NumberOfPackesPerSec); // NumberOfGoodPackets++; //NumberOfServos=(cPackLength/4); FoundFF=0; BytesCounter=0; // now lets build the array return 1; } } else if(BytesCounter<(3*NumberOfServos)) { if((BytesCounter%3)==0 && BytesCounter>0) { ServosCounter++; } if((BytesCounter%3)==0) { ServosInputData[ServosCounter].ID=ReadC; } else if((BytesCounter%3)==1) { ServosInputData[ServosCounter].Pos=ReadC; } else if((BytesCounter%3)==2) { ServosInputData[ServosCounter].Pos+=(ReadC<<8); } u8InputArray[BytesCounter+1]=ReadC; BytesCounter++; } else { //printf("PackLen: %d Bytes %d ",cPackLength,BytesCounter); FoundFF=0; BytesCounter=0; } } return 0; } unsigned char CRC_Calc(unsigned char *Array, unsigned char length) /*FOLD00*/ { unsigned int CRCResult=0; unsigned int CounterCRC=0; while(CounterCRC<length) { CRCResult+=Array[CounterCRC++]; } return ((0xFFFFFFFF-CRCResult)&0xFF); } int fd; int usb_open(char *dev_name, float baudrate) /*FOLD00*/ { struct termios newtio; struct serial_struct serinfo; //int fd; memset(&newtio, 0, sizeof(newtio)); if((fd = open(dev_name, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) { fprintf(stderr, "device open error: %s\n", dev_name); return (-1); } newtio.c_cflag = B38400|CS8|CLOCAL|CREAD; newtio.c_iflag = IGNPAR; newtio.c_oflag = 0; newtio.c_lflag = 0; newtio.c_cc[VTIME] = 0; newtio.c_cc[VMIN] = 0; tcflush(fd, TCIFLUSH); tcsetattr(fd, TCSANOW, &newtio); if(ioctl(fd, TIOCGSERIAL, &serinfo) < 0) { fprintf(stderr, "Cannot get %s serial info\n", dev_name); return (-1); } serinfo.flags &= ~ASYNC_SPD_MASK; serinfo.flags |= ASYNC_SPD_CUST; serinfo.custom_divisor = serinfo.baud_base / baudrate; if(ioctl(fd, TIOCSSERIAL, &serinfo) < 0) { fprintf(stderr, "Cannot set %s serial info\n", dev_name); return (-1); } return (fd); } int usb_close() /*FOLD00*/ { close(fd); }
Actuatedcharacter's Blog 
Leave a Reply