Posted 20 October 2021
I have been struggling for years now with Wall-E2’s reluctance to turn smoothly, and I have been looking for ways to improve the situation. I have even thought of going back to the 3-wheel form-factor with two driven wheels and a 3rd caster wheel, just for the ability to make smooth angle adjustments. However, the 3-wheel layout has its own problems, like a tendency to get stuck when one or both drive wheel loses traction, and a tendency for the caster wheel axle to accumulate carpet fibers to the point where it won’t turn.
Recently I ran across a blog post called ‘4-wheeled robot design basics and challenges‘ that seemed to describe Wall-E2’s turning difficulties pretty accurately. In addition, this post went on to suggest how the 4-wheel form-factor could be modified to minimize the high side-slip friction exhibited by most 4-wheel drive robots. The author suggested that by reducing the front-back dimension and increasing the side-side dimension, the side-slip friction exhibited during turns could be minimized. The author showed (literally) that the problem with a traditional 4-wheel form factor is that the front and back wheel pairs have non-intersecting turn radii, as shown in the following animated GIF from the RonRobotics site:
If the form-factor is changed from the traditional ‘long and narrow’ style to a more ‘short and wide’ style, then the turning situation is more like the following animated GIF:
The author didn’t provide any numerical guidelines for how short and how wide, but the above animated GIF provides a pretty good idea.
I decided to see if I could use TinkerCad to model a form factor that might be more like the ‘short and wide’ form-factor. From previous work I already had most of the individual elements (wheels, motors, battery compartment, charger module, etc), so I did a ‘FrankenRobot’ copy as shown below:
The orange disk shown in the above image was intended to model the turning circle. I manually adjusted the form factor until the turning circle more-or-less matched the layout in the ‘design 3’ image from the RonRobitics site.
After playing with this for a while, I came up with the following ‘final’ design for my new Wall-E3 short/wide robot:
With the above form factor, there is plenty of room for the battery pack (4 x 18650 Li-ion) and the TPS51000 charging module. After arriving at the above layout, I started working fabrication details. At first I thought this was a perfect opportunity to use some Adafruit 20x20mm slotted extrusions and associated hardware I have had hanging around for years looking for a project. However, as I got into the design in TinkerCad, I realized I had a problem, as shown in the following screenshot:
The motors must be mounted either above or below the extrusion. This could be effected using the Adafruit ‘T’ plate, as shown above, but then the question becomes – “what to do about a bottom plate”? I could put in a second set of extrusions, but that would severely reduce ground clearance. I could maybe bend the bottom part of the ‘T’ plate to support a bottom plate, but bending a cast aluminum piece didn’t seem like a good idea. In addition, an even bigger problem was how to mount the top plate. The obvious solution would be to drill 4 holes in the top plate, and use the slotted channel threaded insert hardware to mount the plate. But, how does that work – the threaded insert pieces are made to slide freely in the channel until tightening the mounting screw causes them to bind to the sides of the channel – all fine and good, but that won’t work for a blind mounting arrangement; the inserts could be anywhere in (or out of) the channel – oops! I thought I might be able to find a way of fixing the inserts into place, but that didn’t seem like a good idea either. I could simply drill and tap 4 holes around the periphery of the extrusions, but then I’m using expensive extrusions just like flat plate – bummer!
Eventually I realized that a standard U-channel from McMaster Carr should do the trick. The motors can be mounted through the ‘bottom’ of the U, and the top/bottom plates can be mounted to the top/bottom of the U-channel. Much simpler, and because I can mount the motors low in the U-channel, I can maximize ground clearance – yay!
27 October 2021 Update:
The first try at McMaster-Carr U-channel wasn’t too successful – it was much thicker and heavier than I realized from the drawings, so I went back to their site and re-ordered a much lighter (and nicer) U-channel style. The following photo shows the difference.
I got 2ea 24″ lengths of the lighter U-channel, and as an experiment I tried cutting 45º bevels on both pieces to see how I might form right angles for the chassis. This worked out pretty nicely. In addition, I cut a 1/2″ section from the thicker U-channel, and then cut it in half to produce 2ea right-angle brackets for use in connecting two pieces of a right-angle corner. The following photos show the results:
I was very pleased with my first try at forming a right angle corner for my new chassis – it looks like using pieces of the thicker U-channel for connecting brackets will work. I don’t think I’ll need more than two screws per corner, as I will also have top & bottom plates that will add tremendously to the overall structure rigidity. Now I just have to get the dimensions correct so that my already-cut top/bottom plates will fit, and then do all the other stuff to produce a finished ‘Wall-E3’ chassis.
02 November 2021 Update
After the usual trials and tribulations, I think I have produced a decent 200 x 230 mm chassis, as shown in the following photos:
One minor ‘gotcha’ to using the thinner sidewall U-channel is that I managed to strip the threads on a couple of the threaded holes as I was installing the top plate. Fortunately I had some 3mm press-fit nuts left over from Wall-E2 and was able to convert the threaded hole to a press-fit nut installation without too much problem. Right now I have two holes converted to press-fit, but I suspect all sixteen holes will need to be treated at some point. So, back to McMaster-Carr where I ordered another 100 press-fit nuts, along with some additional 3x6mm flathead screws.
By the way, just as a plug for McMaster-Carr: I ordered and received a 90º countersink bit so I can use flat-head screws on the chassis, but when I received it I realized the tip of the bit wouldn’t fit into my 3mm holes – bummer! So, I contacted McMaster-Carr and asked if I could return it for a refund, and (well within the 30 minute reply guarantee) I got a note back that they would refund the cost of the order, but I didn’t have to return the item – wow! I have been a loyal MC customer for decades, and they continue to impress me with their responsiveness and speedy deliveries. The next time you have a need for hardware, think of McMaster-Carr.
The next step is to add the motor drivers and motors to the prototype setup to wring out any mechanical issues there. As can be seen in the following photo, the current Wall-E2 setup has the motors mounted through the side walls equidistant from top and bottom. However, I plan to mount the motors as close as possible to the lower edge of the U-channel, so as to achieve the largest possible terrain clearance.
To facilitate this I designed and created a ‘motor mounting template’ with motor axle location holes that fits inside the sidewall U-channel, as shown below:
The template has two holes per motor location to allow the template to be placed either way (side tabs toward the top plate, or side tabs toward bottom plate) and still have one set of holes in the proper vertical location.
06 November 2021 Update:
As described in the companion post to this one, I assembled a basic system on a blank aluminum plate as shown in the following photo:
After fabricating the aluminum chassis, I started transferring pieces from the prototype to the new chassis, as shown below
After transferring everything except the Teensy module and the 5V LDO regulator module, this is what the new chassis looks like:
Still lots of work, but it is now clear that everything fits comfortably into the chassis, including both motor drivers which are mounted on top of the chassis on the Wall-E2 robot. This will clear up lots of space on the top plate, so things should be a lot neater with Wall-E3.
07 November 2021 Update:
After getting the basic component layout established, I decided to see if I could get the robot to actually move forwards and backwards under computer control via the HC-05 Bluetooth link. So, I transferred all four wheels from Wall-E2 to Wall-E3, and temporarily hot-glued the Teensy 3.5 and its accompanying 5V LDO regulator/HC-05 module to the battery pack, as shown in the following photo:
And, after fixing the problem of one set of motors turning in the opposite direction as the other set, I was able to move Wall-E3 forwards and backwards utilizing the wireless link from my PC to the robot, as shown in the following short video:
The next thing I tried was some manually controlled ‘spin’ turns. To do this I added an overload to the function that does normal rate-controlled turns using an MPU6050 IMU sensor to make a turn through a defined angle span. The overloaded function simply starts a CW or CCW turn and waits for any key to stop the turn. Here’s a short video showing the result
As can be seen from the above video, Wall-E3’s turn behavior is MUCH smoother than that of Wall-E3, and is quite close to the behavior exhibited in the ‘wide’ form factor GIF that prompted the Wall-E3 redesign (repeated below for comparison):
With such good results from the above turns on hard-surface flooring, I decided to try my luck with my test carpet patch. Turns on carpet with Wall-E2 have always been a jerky procedure, with PID algorithm having to ramp motor current way up to overcome wheel side friction, and then way back down again when the wheels broke free. With Wall-E3, the turning behavior on carpet was indistinguishable from its behavior on hard flooring – both cases exhibited very smooth turning behavior around an almost constant turning axis, as shown in the following short video.
For comparison, here is Wall-E2 making a 180⁰ turn on carpet:
And here is the plot of motor speed vs time for the turn.
As can be seen from the above, the motor speed goes from max (255) to min (50) several times per second as the wheels alternately grip and then slide. In contrast, the Wall-E3 turns on both carpet and hard flooring were done with a constant half-speed (125) motor current, resulting in an angular rate of about 70 ⁰ /sec on both carpet and hard surface.
20 November 2021 Update:
Unfortunately, when I tried to add MPU6050 capability to Wall-E3, I went down a rabbit hole due to changes in Jeff Rowberg’s I2CDevlib library for the MPU6050 that have made it incompatible with Teensy 3.x targets (see this post for the gory details). After chasing Alice for way too long, I finally figured out that I had two choices. I could use an older (as in 2 years older) version of the i2cdevlib libraries with a Teensy, or I could use the current versions with an Arduino UNO or MEGA. Because at this point I already had an Arduino UNO running with a MPU6050 and the new libraries, I decided to continue on that track and see if I could tune the PID engine for smooth(er) turns with the new form factor. Here’s the physical setup:
Here’s the code:
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 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 |
/* Name: Arduino_MPU6050_6Axis_MotionApps612.ino Created: 11/20/2021 7:15:44 AM Author: FRANKNEWXPS15\Frank */ /* Name: T35_MPU6050_Demo.ino Created: 11/13/2021 8:52:56 AM Author: FRANKNEWXPS15\Frank */ #pragma region INCLUDES #include "MPU6050_6Axis_MotionApps612.h" #include "I2Cdev.h" //modified so it #includes i2c_t3.h #include "I2C_Anything.h" #include <PrintEx.h> //allows printf-style printout syntax #include <elapsedMillis.h> #pragma endregion INCLUDES //#define MPU6050_I2C_ADDR 0x68 #define MPU6050_I2C_ADDR 0x69 MPU6050 mpu(MPU6050_I2C_ADDR); StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing #pragma region MPU6050_SUPPORT uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector int GetPacketLoopCount = 0; int OuterGetPacketLoopCount = 0; //MPU6050 status flags bool bMPU6050Ready = true; bool dmpReady = false; // set true if DMP init was successful volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR bool bFirstTime = true; #pragma endregion MPU6050_SUPPORT #pragma region MISC_PINS const int I_RunPin = A0; const int I_TotPin = A1; const int ChgConnectPin = 4; const int BattV_Pin = A2; #pragma endregion MISC_PINS #pragma region MOTOR_PINS //11/04/21 Now using Pololu VNH5019 drivers for all 4 motors //const int InA_Left = 22; //const int InB_Left = 21; //const int Spd_Left = 23; // //const int InA_Right = 34; //const int InB_Right = 33; //const int Spd_Right = 35; //11/14/21 chg to UNO pinouts const int Spd_Left = 10; const int InA_Left = 11; const int InB_Left = 12; const int Spd_Right = 5; const int InA_Right = 6; const int InB_Right = 7; #pragma endregion Motor Pin Assignments #pragma region MOTOR_PARAMETERS //drive wheel speed parameters const int MOTOR_SPEED_FULL = 200; //range is 0-255 const int MOTOR_SPEED_MAX = 255; //range is 0-255 const int MOTOR_SPEED_HALF = 127; //range is 0-255 const int MOTOR_SPEED_QTR = 75; //added 09/25/20 const int MOTOR_SPEED_LOW = 50; //added 01/22/15 const int MOTOR_SPEED_OFF = 0; //range is 0-255 //drive wheel direction constants const boolean FWD_DIR = true; const boolean REV_DIR = !FWD_DIR; const bool TURNDIR_CCW = true; const bool TURNDIR_CW = false; //Motor direction variables boolean bLeftMotorDirIsFwd = true; boolean bRightMotorDirIsFwd = true; bool bIsForwardDir = true; //default is foward direction #pragma endregion Motor Parameters #pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS elapsedMillis MsecSinceLastTurnRateUpdate; const int MAX_PULSE_WIDTH_MSEC = 100; const int MIN_PULSE_WIDTH_MSEC = 0; const int MAX_SPIN_MOTOR_SPEED = 250; const int MIN_SPIN_MOTOR_SPEED = 50; //const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //const int TURN_RATE_UPDATE_INTERVAL_MSEC = 60; const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100; const int TURN_START_SPEED = MOTOR_SPEED_QTR; //added 11/14/21 double Prev_HdgDeg = 0; float tgt_deg; float timeout_sec; //05/31/21 latest & greatest PID values //double TurnRate_Kp = 2.5; //double TurnRate_Ki = 2.0; //double TurnRate_Kd = 0.1; //double TurnRate_Kp = 5; //almost perfect for 90deg/sec //double TurnRate_Kp = 3; //almost perfect for 90deg/sec //double TurnRate_Ki = 1; //double TurnRate_Kd = 0; //11/16/21 almost perfect for 45 deg/sec, 30mSec //double TurnRate_Kp = 1.8; //double TurnRate_Ki = 0.5; //double TurnRate_Kd = 0; //11/16/21 almost perfect for 90 deg/sec, 100mSec //double TurnRate_Kp = 2.0; //double TurnRate_Ki = 1.5; //double TurnRate_Kd = 0; //11/16/21 almost perfect for 45 deg/sec, 100mSec //11/17/21 and pretty good for 90 deg/sec too! double TurnRate_Kp = 1.8; double TurnRate_Ki = 1.0; double TurnRate_Kd = 0.4; double TurnRatePIDSetPointDPS, TurnRatePIDOutput; double TurnRatePIDInput = 15.f; //PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT); const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20 const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started #pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS elapsedMillis MsecSinceLastUpdate; void setup() { Serial.begin(115200); //Serial.begin(57600); delay(2000); //10/06/21 - just use fixed delay instead #pragma region MPU6050_INIT Serial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR); mpu.initialize(); // verify connection Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if successful) if (devStatus == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); Serial.printf("Just after MPU6050 Init\n"); Serial.printf("Calibrating...\n"); mpu.CalibrateGyro(); //using default value of 15 Serial.printf("Retrieving Calibration Values\n\n"); mpu.PrintActiveOffsets(); //loop heading display until stabilized mySerial.printf("\nMsec\tHdg\n"); UpdateIMUHdgValDeg(); Prev_HdgDeg = IMUHdgValDeg; delay(100); UpdateIMUHdgValDeg(); mySerial.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg); while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f) { mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg); Prev_HdgDeg = IMUHdgValDeg; delay(100); UpdateIMUHdgValDeg(); } mySerial.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", millis() / 1000.f, IMUHdgValDeg - Prev_HdgDeg); bMPU6050Ready = true; delay(1000); } else //MPU6050 Init failed for some reason { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.printf("DMP Initialization failed (code %d\n", devStatus); } #pragma endregion MPU6050_INIT #pragma region MOTOR_PIN_INIT pinMode(Spd_Left, OUTPUT); pinMode(InA_Left, OUTPUT); pinMode(InB_Left, OUTPUT); pinMode(Spd_Right, OUTPUT); pinMode(InA_Right, OUTPUT); pinMode(InB_Right, OUTPUT); #pragma endregion MOTOR_PIN_INIT #pragma region MISC_PINS_INIT pinMode(I_RunPin, INPUT); pinMode(I_TotPin, INPUT); pinMode(ChgConnectPin, INPUT_PULLUP); pinMode(BattV_Pin, INPUT); #pragma endregion MISC_PINS_INIT //mySerial.printf("Starting Turns Testing\n"); //delay(1000); //mySerial.printf("CCW 45deg at 30deg/sec\n"); //delay(1000); //SpinTurn(true, 45, 30); //mySerial.printf("CW 45deg at 30deg/sec\n"); //delay(1000); //SpinTurn(false, 45, 30); //mySerial.printf("CCW 90deg at 30deg/sec\n"); //delay(1000); //SpinTurn(true, 90, 30); //mySerial.printf("CW 90deg at 30deg/sec\n"); //delay(1000); //SpinTurn(false, 90, 30); mySerial.printf("Tests Complete. Enter 'C' for manual control\n"); //mySerial.printf("Msec\tHdg\n", millis(), IMUHdgValDeg); MsecSinceLastUpdate = 0; } void loop() { CheckForUserInput(); //if (MsecSinceLastUpdate >= 200) //{ // MsecSinceLastUpdate -= 200; // UpdateIMUHdgValDeg(); // mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg); //} } #pragma region HDG_BASED_TURN_SUPPORT float GetHdgMatchVal(float tgt_deg, float cur_deg) { //Purpose: Compute the match ratio between two compass headings //Inputs: // tgt_deg = float representing target heading in +/-180 range // IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range //Outputs: // returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range //Plan: // Step1: convert both inputs to 0-360 deg range // Step2: compute match ratio //Notes: // formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings //Step1: convert both inputs to 0-360 deg range float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg; float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg; //Step2: compute match ratio float match_ratio = 1 - abs(tgthdg - curhdg) / 180; //DEBUG!! //mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio); //DEBUG!! return abs(match_ratio); } float UpdateIMUHdgValDeg() { //Purpose: Get latest yaw (heading) value from IMU //Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE //Outputs: // returns true if successful, otherwise false // IMUHdgValDeg updated on success //Plan: //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet //Step2: read all available packets to get to latest data //Step3: update IMUHdgValDeg with latest value //Notes: // 10/08/19 changed return type to boolean // 10/08/19 no longer need mpuIntStatus // 10/21/19 completely rewritten to use Homer's algorithm // 05/05/20 changed return type to float vs bool. bool retval = false; int flag = mpu.GetCurrentFIFOPacket(fifoBuffer, packetSize); //08/01/21 now using library version if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow { // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //compute the yaw value IMUHdgValDeg = ypr[0] * 180 / M_PI; retval = true; } //return retval; return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience } bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC) { //Purpose: Make a numDeg CW or CCW 'spin' turn //Inputs: // b_ccw - True if turn is to be ccw, false otherwise // numDeg - angle to be swept out in the turn // ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg // IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR // degPerSec = float value denoting desired turn rate //Plan: // Step1: Get current heading as starting point // Step2: Disable TIMER5 interrupts // Step3: Compute new target value & timeout value // Step4: Run motors until target reached, using inline PID algorithm to control turn rate // Step5: Re-enable TIMER5 interrupts //Notes: // 06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function // 06/06/21 added re-try for 180.00 return from IMU - could be bad value // 06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values // 06/12/21 cleaned up & commented out debug code // 11/14/21 removed 'first time skip' block; added motor start before entering loop float tgt_deg; float timeout_sec; bool bDoneTurning = false; bool bTimedOut = false; bool bResult = true; //04/21/20 added so will be only one exit point //DEBUG!! mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec); //mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n", //TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC); //DEBUG!! //no need to continue if the IMU isn't available if (!dmpReady) { mySerial.printf("DMP Failure - returning FALSE\n"); return false; } //Step1: Get current heading as starting point //06/06/21 it is possible for IMU to return 180.00 on failure //so try again. If it really IS 180, then //it will eventually time out and go on //08/26/21 re-wrote using 3-value array to make sure initial heading is a steady value UpdateIMUHdgValDeg(); int retries = 0; if ((IMUHdgValDeg == 180.f || IMUHdgValDeg == 0.f) && retries < 5) { //DEBUG!! mySerial.printf("Got 180.00 or 0.00 exactly (%2.3f) from IMU - retrying %d...\n", IMUHdgValDeg, retries); //DEBUG!! UpdateIMUHdgValDeg(); retries++; delay(100); } //Step2: Compute new target value & timeout value timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter //05/17/20 limit timeout_sec to 1 sec or more timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec; //12/05/19 added #define back in to manage which direction increases yaw values #ifdef MPU6050_CCW_INCREASES_YAWVAL tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg; #else tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg; #endif // MPU6050_CCW_INCREASES_YAWVAL //correct for -180/180 transition if (tgt_deg < -180) { tgt_deg += 360; } //07/29/19 bugfix if (tgt_deg > 180) { tgt_deg -= 360; } //DEBUG!! mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n", IMUHdgValDeg, numDeg, tgt_deg, timeout_sec); //DEBUG!! float curHdgMatchVal = 0; //09/08/18 added to bolster end-of-turn detection float prevHdgMatchVal = 0; float matchSlope = 0; //Step3: Run motors until target reached, using PID algorithm to control turn rate Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop //11/14/21 start motors before entering loop. For CCW turn, left motors go backwards, right ones forward int startSpd = (degPersec <= 45) ? TURN_START_SPEED : 2 * TURN_START_SPEED; SetLeftMotorDirAndSpeed(!b_ccw, startSpd); SetRightMotorDirAndSpeed(b_ccw, startSpd); elapsedMillis sinceLastTimeCheck = 0; elapsedMillis sinceLastComputeTime = 0; double lastError = 0; double lastInput = 0; double lastIval = 0; double lastDerror = 0; bool bFirstIMUHdg = true; //DEBUG!! mySerial.printf("Msec\tHdg\tPrvHdg\tdHdg\tRate\ttgtDPS\terr\tKp*err\tIval\tKd*Derr\tspeed\tMatch\tSlope\n"); //DEBUG!! while (!bDoneTurning && !bTimedOut) { //11/06/20 now just loops between PID calcs CheckForUserInput(); if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC) { sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC; UpdateIMUHdgValDeg(); //update IMUHdgValDeg double dHdg = IMUHdgValDeg - Prev_HdgDeg; if (dHdg > 180) { dHdg -= 360; //mySerial.printf("dHdg > 180 - subtracting 360\n"); } else if (dHdg < -180) { dHdg += 360; //mySerial.printf("dHdg < -180 - adding 360\n"); } //watch for turn rates that are wildly off double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC); //if (rate > 3 * degPersec) //{ // //DEBUG!! // mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec); // //DEBUG!! // rate = degPersec; //} //06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet //11/14/21 don't need this now, as motor is started at least TURN_RATE_UPDATE_INTERVAL_MSEC prior to loop entry //if (bFirstIMUHdg) //{ // bFirstIMUHdg = false; // Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time //} //else //{ TurnRatePIDOutput = PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd); int speed = 0; (TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput; (TurnRatePIDOutput <= MOTOR_SPEED_LOW) ? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput; SetLeftMotorDirAndSpeed(!b_ccw, speed); SetRightMotorDirAndSpeed(b_ccw, speed); //check for nearly there and all the way there curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg); matchSlope = curHdgMatchVal - prevHdgMatchVal; //DEBUG!! //mySerial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%2.2f\t%2.2f\n", // millis(), // IMUHdgValDeg, // Prev_HdgDeg, // dHdg, // rate, // degPersec, // lastError, // TurnRate_Kp * lastError, // TurnRate_Ki * lastIval, // TurnRate_Kd * lastDerror, // speed, // curHdgMatchVal, // matchSlope); // mySerial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%2.2f\t%2.2f\n", millis(), IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec, lastError, TurnRate_Kp * lastError, lastIval, TurnRate_Kd * lastDerror, speed, curHdgMatchVal, matchSlope); //DEBUG!! Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time //look for full match bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL || (prevHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope <= -0.01)); //have to use < vs <= as slope == 0 at start //mySerial.printf("curHdgMatchVal = %2.2f, prevHdgMatchVal = %2.2f, matchslope = %2.2f, bDoneTurning = %d\n", // curHdgMatchVal, // prevHdgMatchVal, // matchSlope, // bDoneTurning); prevHdgMatchVal = curHdgMatchVal; //07/31/21 moved below bDoneTurning chk so can use prevHdgMatchVal vs curHdgMatchVal in slope check //} } // // bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000); // // if (bTimedOut) // { // //DEBUG!! // mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal); // //DEBUG!! // // bResult = false; // } // // if (bDoneTurning) // { // mySerial.printf("Completed turn with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal); // } } // //DEBUG!! mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg); //DEBUG!! // ////Step4: Re-enable TIMER5 interrupts // TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt // StopBothMotors(); delay(1000); //added 04/27/21 for debug return bResult; } //09/03/21 chg fcn type from void to double to eliminate double& output param double PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd) { //Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR //Inputs: // input = double denoting current input value (turn rate, speed, whatever) // setpoint = double denoting desired setpoint in same units as input // sampleTime = int denoting sample time to be used in calcs. // lastError = ref to double denoting error saved from prev calc // lastInput = ref to double denoting input saved from prev calc // Kp/Ki/Kd = doubles denoting PID values to be used for calcs // Output = ref to double denoting output from calc double error = setpoint - input; double dInput = (input - lastInput); //lastIval += (error); lastIval += (Ki * error); //11/16/21 rev to clamp integral term lastIval = (lastIval > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : lastIval; lastIval = (lastIval < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : lastIval; double dErr = (error - lastError); //mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr, // Kp,Ki,Kd); /*Compute PID Output*/ //11/16/21 rev to subtract differential term rather than add. //double output = Kp * error + Ki * lastIval + Kd * dErr; double output = Kp * error + lastIval - Kd * dErr; //11/16/21 now clamping output value as well as I term output = (output > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : output; output = (output < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : output; /*Remember some variables for next time*/ lastError = error; lastInput = input; lastDerror = dErr; return output; //added 09/03/21 } #pragma endregion HDG_BASED_TURN_SUPPORT #pragma region MOTOR SUPPORT //09/08/20 modified for DRV8871 motor driver void MoveReverse(int leftspeednum, int rightspeednum) { //Purpose: Move in reverse direction continuously - companion to MoveAhead() //ProvEnA_Pinnce: G. Frank Paynter 09/08/18 //Inputs: // leftspeednum = integer denoting speed (0=stop, 255 = full speed) // rightspeednum = integer denoting speed (0=stop, 255 = full speed) //Outputs: both drive Motors energized with the specified speed //Plan: // Step 1: Set reverse direction for both wheels // Step 2: Run both Motors at specified speeds //Notes: // 01/22/20 now using Adafruit DRV8871 drivers //Step 1: Set reverse direction and speed for both wheels SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum); SetRightMotorDirAndSpeed(REV_DIR, rightspeednum); } //09/08/20 modified for DRV8871 motor driver void MoveAhead(int leftspeednum, int rightspeednum) { //Purpose: Move ahead continuously //ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014 //Inputs: // leftspeednum = integer denoting speed (0=stop, 255 = full speed) // rightspeednum = integer denoting speed (0=stop, 255 = full speed) //Outputs: both drive Motors energized with the specified speed //Plan: // Step 1: Set forward direction for both wheels // Step 2: Run both Motors at specified speeds //Notes: // 01/22/20 now using Adafruit DRV8871 drivers //mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum); //Step 1: Set forward direction and speed for both wheels SetLeftMotorDirAndSpeed(true, leftspeednum); SetRightMotorDirAndSpeed(true, rightspeednum); } //09/08/10 modified for DRV8871 motor driver //11/04/21 modified for Pololu VNH5019 motor driver void StopLeftMotors() { digitalWrite(InA_Left, LOW); digitalWrite(InB_Left, LOW); analogWrite(Spd_Left, MOTOR_SPEED_OFF); } //11/04/21 modified for Pololu VNH5019 motor driver void StopRightMotors() { digitalWrite(InA_Right, LOW); digitalWrite(InB_Right, LOW); analogWrite(Spd_Right, MOTOR_SPEED_OFF); } //09/08/20 added bool bisFwd param for DRV8871 motor driver void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum) { //Purpose: Run both Motors at left/rightspeednum speeds //Inputs: // speednum = speed value (0 = OFF, 255 = full speed) //Outputs: Both Motors run for timesec seconds at speednum speed //Plan: // Step 1: Apply drive to both wheels //Notes: // 01/14/15 - added left/right speed offset for straightness compensation // 01/22/15 - added code to restrict fast/slow values // 01/24/15 - revised for continuous run - no timing // 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds() // 12/07/15 - chg args from &int to int //Step 1: Apply drive to both wheels //DEBUG!! //mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum); //DEBUG!! SetLeftMotorDirAndSpeed(bisFwd, leftspeednum); SetRightMotorDirAndSpeed(bisFwd, rightspeednum); } void RunBothMotorsBidirectional(int leftspeed, int rightspeed) { //Purpose: Accommodate the need for independent bidirectional wheel motion //Inputs: // leftspeed - integer denoting left wheel speed. Positive value is fwd, negative is rev // rightspeed - integer denoting right wheel speed. Positive value is fwd, negative is rev //Outputs: // left/right wheel motors direction and speed set as appropriate //Plan: // Step1: Set left wheel direction and speed // Step2: Set right wheel direction and speed //Step1: Set left wheel direction and speed //DEBUG!! //mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed); if (leftspeed < 0) { SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse } else { SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd } //Step2: Set right wheel direction and speed if (rightspeed < 0) { SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse } else { SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd } } //09/08/20 added bool bisFwd param for DRV8871 motor driver void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum) { //Purpose: Run both Motors for timesec seconds at speednum speed //Inputs: // timesec = time in seconds to run the Motors // speednum = speed value (0 = OFF, 255 = full speed) //Outputs: Both Motors run for timesec seconds at speednum speed //Plan: // Step 1: Apply drive to both wheels // Step 2: Delay timsec seconds // Step 3: Remove drive from both wheels. //Notes: // 01/14/15 - added left/right speed offset for straightness compensation // 01/22/15 - added code to restrict fast/slow values // 11/25/15 - rev to use motor driver class object // 09/08/20 added bool bisFwd param for DRV8871 motor driver RunBothMotors(bisFwd, leftspeednum, rightspeednum); //Step 2: Delay timsec seconds delay(timeMsec); //Step3: Stop motors added 04/12/21 StopBothMotors(); } //11/25/15 added for symmetry ;-). void StopBothMotors() { StopLeftMotors(); StopRightMotors(); } void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed) { speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; //mySerial.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n", // (bIsFwd == true) ? "true" : "false", speed); //11/04/21 fwd for right motors is CCW when looking at shaft #ifndef NO_MOTORS if (bIsFwd) { digitalWrite(InA_Left, LOW); digitalWrite(InB_Left, HIGH); analogWrite(Spd_Left, speed); //mySerial.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n", // (bIsFwd == true) ? "true" : "false", speed); } else { //mySerial.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n", //(bIsFwd == true) ? "true" : "false", speed); digitalWrite(InA_Left, HIGH); digitalWrite(InB_Left, LOW); analogWrite(Spd_Left, speed); } #endif // !NO_MOTORS } void SetRightMotorDirAndSpeed(bool bIsFwd, int speed) { speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; //mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n", // (bIsFwd == true) ? "true" : "false", speed); //11/04/21 fwd for right motors is CW when looking at shaft #ifndef NO_MOTORS if (bIsFwd) { //mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n", // (bIsFwd == true) ? "true" : "false", speed); digitalWrite(InA_Right, HIGH); digitalWrite(InB_Right, LOW); analogWrite(Spd_Right, speed); } else { //mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n", // (bIsFwd == true) ? "true" : "false", speed); digitalWrite(InA_Right, LOW); digitalWrite(InB_Right, HIGH); analogWrite(Spd_Right, speed); } #endif // !NO_MOTORS } #pragma endregion Motor Support Functions void CheckForUserInput() { const int bufflen = 80; char buff[bufflen]; memset(buff, 0, bufflen); float OffsetCm; float Kp, Ki, Kd; const char s[2] = ","; char* token; if (Serial.available() > 0) { // read the incoming byte: int incomingByte = Serial.read(); // say what you got: //Serial.print("I received: "); //Serial.println(incomingByte, HEX); //chg to HEX 02/18/20 //02/18/20 experiment with multiple commands switch (incomingByte) { case 0x43: //ASCII 'C' case 0x63: //ASCII 'c' #pragma region MANUALCONTROL //enter infinite loop for direct remote control Serial.println(F("ENTERING COMMAND MODE:")); Serial.println(F("0 = 180 deg CCW Turn")); Serial.println(F("1 = 180 deg CW Turn")); Serial.println(F("A = Back to Auto Mode")); Serial.println(F("S = Stop")); Serial.println(F("F = Forward")); Serial.println(F("R = Reverse")); Serial.println(F("")); Serial.println(F(" Faster")); Serial.println(F("\t8")); Serial.println(F("Left 4\t5 6 Right")); Serial.println(F("\t2")); Serial.println(F(" Slower")); StopBothMotors(); int speed = 0; bool bAutoMode = false; while (!bAutoMode) { if (Serial.available() > 0) { // read the incoming byte: int incomingByte = Serial.read(); //mySerial.printf("Got %x\n", incomingByte); switch (incomingByte) { //case 0x54: //ASCII 'T' //case 0x74: //ASCII 't' // Serial.println(F("Toggle TIMER5 Enable/Disable")); // if (TIMSK5 == 0) // { // Serial.println(F("Enable TIMER5")); // //TIMSK5 |= OCIE5A; // TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt // mySerial.printf("TIMSK5 = %x\n", TIMSK5); // } // else // { // Serial.println(F("Disable TIMER5")); // TIMSK5 = 0; // mySerial.printf("TIMSK5 = %x\n", TIMSK5); // } // break; case 0x30: //Dec '0' Serial.println(F("CCW 180 deg Turn")); //RollingTurn(true, bIsForwardDir, 180); //SpinTurn(true, 180, 45); SpinTurn(true, 180, 90); MoveAhead(speed, speed); break; case 0x31: //Dec '1' Serial.println(F("CW 180 deg Turn")); //RollingTurn(false, bIsForwardDir, 180); SpinTurn(false, 180, 45); //MoveAhead(speed, speed); break; case 0x34: //Turn left 10 deg Serial.println(F("CCW 10 deg Turn")); //MoveAhead(speed, speed); //RollingTurn(true, bIsForwardDir, 10); //SpinTurn(true, 10, 30); //SpinTurn(true, 30, 30); //added 05/03/20 if (bIsForwardDir) { MoveAhead(speed, speed); } else { MoveReverse(speed, speed); } break; case 0x36: //Turn right 10 deg' mySerial.print("CW 10 deg Turn\n"); MoveAhead(speed, speed); //RollingTurn(false, bIsForwardDir, 10); //SpinTurn(false, 10, 30); //SpinTurn(false, 45, 30); //added 05/03/20 if (bIsForwardDir) { MoveAhead(speed, speed); } else { //MoveReverse(speed, speed); } break; case 0x38: //Speed up speed += 50; speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed; mySerial.printf("Speeding up: speed now %d\n", speed); if (bIsForwardDir) { MoveAhead(speed, speed); } else { //MoveReverse(speed, speed); } break; case 0x32: //Slow down speed -= 50; speed = (speed < 0) ? 0 : speed; mySerial.printf("Slowing down: speed now %d\n", speed); if (bIsForwardDir) { MoveAhead(speed, speed); } else { //MoveReverse(speed, speed); } break; case 0x35: //05/07/20 changed to use '5' vs 'S' Serial.println(F("Stopping Motors!")); StopBothMotors(); speed = 0; break; case 0x41: //ASCII 'A' case 0x61: //ASCII 'a' StopBothMotors(); Serial.println(F("Re-entering AUTO mode")); bAutoMode = true; break; case 0x52: //ASCII 'R' case 0x72: //ASCII 'r' Serial.println(F("Setting both motors to reverse")); bIsForwardDir = false; //MoveReverse(speed, speed); break; case 0x46: //ASCII 'F' case 0x66: //ASCII 'f' Serial.println(F("Setting both motors to forward")); bIsForwardDir = true; MoveAhead(speed, speed); #pragma endregion MANUALCONTROL break; #pragma region WALL OFFSET TRACKING CASES //left side wall tracking case 0x4C: //ASCII 'L' case 0x6C: //ASCII 'l' StopBothMotors(); Serial.println(F("Setting Kp value")); //consume CR/LF chars while (Serial.available() > 0) { int byte = Serial.read(); mySerial.printf("consumed %d\n", byte); } while (!Serial.available()) { } Serial.readBytes(buff, bufflen); mySerial.printf("%s\n", buff); /* extract Kp */ token = strtok(buff, s); Kp = atof(token); /* extract Ki */ token = strtok(NULL, s); Ki = atof(token); /* extract Kd */ token = strtok(NULL, s); Kd = atof(token); /* extract Offset in cm */ token = strtok(NULL, s); OffsetCm = atof(token); mySerial.printf("Kp,Ki,Kd,OffsetCm = %2.2f, %2.2f, %2.2f, %2.2f\n", Kp, Ki, Kd, OffsetCm); //TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm); break; case 0x4D: //ASCII 'M' case 0x6D: //ASCII 'm' StopBothMotors(); Serial.println(F("Setting Kp value")); //consume CR/LF chars while (Serial.available() > 0) { int byte = Serial.read(); mySerial.printf("consumed %d\n", byte); } while (!Serial.available()) { } Serial.readBytes(buff, bufflen); mySerial.printf("%s\n", buff); /* extract Kp */ token = strtok(buff, s); Kp = atof(token); /* extract Ki */ token = strtok(NULL, s); Ki = atof(token); /* extract Kd */ token = strtok(NULL, s); Kd = atof(token); /* extract Offset in cm */ token = strtok(NULL, s); OffsetCm = atof(token); mySerial.printf("Kp,Ki,Kd,OffsetCm = %2.2f, %2.2f, %2.2f, %2.2f\n", Kp, Ki, Kd, OffsetCm); //TrackRightWallOffset(Kp, Ki, Kd, OffsetCm); break; #pragma endregion WALL OFFSET TRACKING CASES Default : Serial.println(F("In Default Case: Stopping Motors!")); MoveAhead(0, 0); while (true) { } } } } } } } |
After a number of iterations, I arrived at the following PID values:
1 2 3 4 5 |
//11/16/21 almost perfect for 45 deg/sec, 100mSec //11/17/21 and pretty good for 90 deg/sec too! double TurnRate_Kp = 1.8; double TurnRate_Ki = 1.0; double TurnRate_Kd = 0.4; |
Here are the Excel plots showing the turn performance:
At this point, I think I’m pretty much done with this post. The ‘rest of the story’ will continue in the companion ‘Replacing Mega 2560 With Teensy‘ post.
Stay Tuned,
Frank
Pingback: Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part VI | Paynter's Palace
Pingback: WallE3 Spin Turn, Revisited | Paynter's Palace