Posted 06 October 2019
In previous posts I have described my efforts to integrate heading-based wall tracking into my two-wheel and four-wheel robots. I installed a MPU6050 module into Wall-E2, my primary four-wheel robot, some time ago but was never able to make heading-based turns work for one reason or another. In conjunction with some other experiments, I installed an MPU6050 module on my two-wheel robot so that I could investigate the issues with heading-based turns and heading-based wall tracking with a simpler hardware configuration.
With the two-wheel robot I was able to demonstrate successful heading-based wall tracking, but I was unable to port the capability to my four-wheel configuration. Not only that, but for some reason I started having problems getting reliable yaw/heading values from my two-wheel robot configuration. This post describes the steps I took to troubleshoot the problem, ultimately arriving at a stable polling-only (no interrupt line required) yaw/heading value retrieval algorithm suitable for both the two-wheel and four-wheel robot configurations.
Back to Basics:
As I always do when faced with a complex problem with conflicting results, I decided to simplify the problem as much as possible. In this case that meant reducing the hardware configuration to just a MPU6050 module and an Arduino Mega controller, as shown below:
Arduino Mega and MPU6050
On the software side, I started with the simplest possible Arduino sketch – Jeff Rowberg’s ‘MPU6050_DMP6.ino’ example, included in his latest I2CDevLib library and described in this post.
After getting everything running properly in this very basic configuration using an interrupt-driven algorithm, I moved on to working with the polling-driven arrangement, to confirm that polling was a viable strategy. To do this I modified the hardware to disconnect the interrupt line from the MPU6050 to the controller board, and modified the software as described in this post to use a polling arrangement vs interrupts.
After confirming that this simple example worked properly and seemed stable, it was time to work my way back into the two-wheel robot hardware and software configuration (again!). To do this I started with the basic controller/MPU6050 only hardware configuration, but running my two-wheel robot software program, modified to eliminate everything but the ‘RollingTurn()’ function that uses heading information from the MPU6050 to initiate and terminate turns. After some false starts and blind alleys, I finally arrived at a stable software configuration demonstrating consistent heading-based turn performance using polling only – no interrupts! the code is shown below:
|
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 |
/* Name: TwoWheelRobot.ino Created: 6/3/2019 9:31:46 AM Author: FRANKWIN10\Frank Notes: 11/05/19 changed to MPU6050 DMP Version 6.12 */ #pragma region INCLUDES //#include <Adafruit_INA219.h> #include <elapsedMillis.h> #include <NewPing.h> //added 01/15/15 #include <PrintEx.h> //allows printf-style printout syntax //#include "MPU6050_6Axis_MotionApps20.h" //06/23/18 modified for 20Hz interrupt freq #include "MPU6050_6Axis_MotionApps_V6_12.h" //changed to this version 10/05/19 #include "I2Cdev.h" //02/19/19: this includes SBWire.h #pragma endregion INCLUDES StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing elapsedMillis SinceLastDistanceCheck; elapsedMillis sinceLastTimeCheck; //used for rolling turn timeout elapsedMillis sinceLastHdgCheck; //rolling turn heading check timer #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_LOW = 50; //added 01/22/15 const int MOTOR_SPEED_OFF = 0; //range is 0-255 const int MOTOR_SPEED_ADJ_FACTOR = 0; //chg to 40 at 5:55pm const int LEFT_SPEED_COMP_VAL_FWD = 0; //left speed compensation value const int RIGHT_SPEED_COMP_VAL_FWD = 5; //right speed compensation value const int LEFT_SPEED_COMP_VAL_REV = 0; //left speed compensation value const int RIGHT_SPEED_COMP_VAL_REV = 0; //right speed compensation value //drive wheel direction constants const boolean FWD_DIR = true; const boolean REV_DIR = !FWD_DIR; //Motor direction variables boolean bLeftMotorDirIsFwd = true; boolean bRightMotorDirIsFwd = true; const float ROLLING_TURN_MAX_SEC_PER_DEG = 1 / 15.0; //used to limit time in rolling turns const int OFFSIDE_MOTOR_SPEED = MOTOR_SPEED_LOW; const int DRIVESIDE_MOTOR_SPEED_HIGH = MOTOR_SPEED_MAX; const int DRIVESIDE_MOTOR_SPEED_LOW = MOTOR_SPEED_FULL; const float HDG_NEAR_MATCH_VAL = 0.9; //slow the turn down here const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started #pragma endregion Motor Parameters #pragma region DEFINES #define NO_MOTORS //#define NO_LIDAR //#define NO_PINGS //#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR) //#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop #pragma endregion Program Defines #pragma region PROGRAM_CONSTANTS const int MIN_PING_INTERVAL_MSEC = 200; const int CURRENT_AVERAGE_NUMBER = 10; //added 07/09/19 const int MAX_AD_VALUE = 1023; const float ADC_REF_VOLTS = 3.3; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF const float VOLTAGE_TO_CURRENT_RATIO = 0.75f; //Volts/Amp rev 07/09/19. #pragma endregion Program Constants #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) #pragma region MOTOR_PINS //Left Motor int enA = 2; int in1 = 3; int in2 = 4; //Right Motor int in3 = 5; int in4 = 6; int enB = 7; #pragma endregion Motor Pin Assignments #pragma region SENSOR_PINS const int RightSensorTrigPin = 52; const int LeftSensorTrigPin = 53; const int LeftSensorEchoPin = LeftSensorTrigPin; const int RightSensorEchoPin = RightSensorTrigPin; const int TOT_CURR_PIN = A8; //LIDAR MODE pin (continuous mode) const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18 #pragma endregion Sensor Pin Assigns #pragma region DIST_HDG_MEASUREMENT_SUPPORT //misc LIDAR and Ping sensor parameters const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling const int MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors //distance and heading running average const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg const int LR_PING_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ const int HDG_RUNNING_AVG_ARRAY_SIZE = 20; //1 second byte aLeftDist[LR_PING_DIST_ARRAY_SIZE]; byte aRightDist[LR_PING_DIST_ARRAY_SIZE]; #pragma endregion Distance Measurement Support //MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0) MPU6050 mpu(0x68); //07/28/19 use default addr for GY-521 on 2-motor robot //added 01/16/15 for NewPing support NewPing LeftPing(LeftSensorTrigPin, LeftSensorEchoPin, MAX_LR_DISTANCE_CM / 2); NewPing RightPing(RightSensorTrigPin, RightSensorEchoPin, MAX_LR_DISTANCE_CM / 2); #pragma region LOOP_VARS int leftspeednum = MOTOR_SPEED_HALF; int rightspeednum = MOTOR_SPEED_HALF; elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec int m_FinalLeftSpeed = 0; int m_FinalRightSpeed = 0; float shuntvoltage = 0; float busvoltage = 0; float current_mA = 0; float loadvoltage = 0; float power_mW = 0; float TotAmps = 0.f; float SecSinceStart = 0.f; float StartSec = 0; int m_LeftDistCm = 0; int m_RightDistCm = 0; #pragma endregion Loop Variables #pragma region MPU6050_SUPPORT //uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU c/o 10/08/19 not used for polling uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t DMPpacketSize; // 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 //RTC/FRAM/MPU6050 status flags bool RTC_Avail = true; bool bRTCLostPower = false; //added 10/17/18 bool bFRAMReady = true; bool bMPU6050Ready = true; bool dmpReady = false; // set true if DMP init was successful volatile float global_yawval = 0; //updated by GetIMUHeadingDeg() const int MAX_FIFO_WAIT_LOOP_COUNT = 1000; #pragma endregion MPU6050 Support #pragma region WALL_FOLLOW_SUPPORT #define TURNDIR_CCW false #define TURNDIR_CW true const int DESIRED_WALL_OFFSET_DIST_CM = 30; const int WALL_APPR_ERR_WIN_MULTFACT = 2; //added 08/12/19 const int WALL_TRK_ERR_WIN_MULTFACT = 1; //added 08/12/19 const int VERY_FAR_AWAY_CM = 5; const int FAR_AWAY_CM = 2; const int CLOSE_CM = 1; const int VERY_FAR_AWAY_TURN_DEG = 20; const int FAR_AWAY_TURN_DEG = 10; const int CLOSE_TURN_DEG = 5; const int NAV_UPDATE_DELAY_MSEC = 1000; const int TURN_INCREMENT_DEG = 10; const int U_TURN_SEC = 8; int m_PrevHdgCutDeg = 0; bool m_PrevHdgCutDir = TURNDIR_CW; //CW int m_PrevLeftDistCm = 0; int m_PrevRightDistCm = 0; elapsedMillis sinceLastHdgCutUpdateMsec; //added 10/15/18 to replace lastmillisec int m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //used to increase close in response #pragma endregion Wall Following Support //08/03/19 experiment with using a timer interrupt for distance checking while in a turn const int TIMER3_OUT_PIN = 31; void setup() { Serial.begin(115200); Serial.println("Two Wheel Robot"); // join I2C bus (I2Cdev library doesn't do this automatically) Wire.begin(); Wire.setTwiMaxLoops(15000); //08/23 needed for SBWire library Wire.setClock(100000); //07/25/19 slow down I2C clock for better noise immunity //motor control PWM pins //set up left motor pins as outputs pinMode(enA, OUTPUT); pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); //set up right motor pins as outputs pinMode(in3, OUTPUT); pinMode(in4, OUTPUT); pinMode(enB, OUTPUT); pinMode(TOT_CURR_PIN, INPUT);//07/09/19 now connected to 'Total Current' 1NA619 charge current sensor digitalWrite(TOT_CURR_PIN, LOW); //turn off the internal pullup resistor analogReference(EXTERNAL); //07/09/18 now using external 3.3V ref analogRead(0); //dummy read to let the ADC circuitry settle down // configure LED for output pinMode(LED_PIN, OUTPUT); // initialize MPU6050 added 09/03/18 Serial.println(F("Initializing MPU6050 ...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); 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 DMPpacketSize = mpu.dmpGetFIFOPacketSize(); bMPU6050Ready = true; StartSec = millis() / 1000.f; mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", StartSec); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); bMPU6050Ready = false; } mySerial.printf("Sec\tHdg\tLdist\tRdist\tErrDeg\tPrevDeg\tTurnDir\tTurnDeg\n"); //column headers m_LeftDistCm = GetLeftDistCm(); m_RightDistCm = GetRightDistCm(); RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL); SinceLastDistanceCheck = 0; sinceLastTimeCheck = 0; //used for rolling turn timeout sinceLastHdgCheck = 0; //rolling turn heading check timer mySerial.printf("Starting test turns...\n"); delay(1000); mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount()); mySerial.printf("Turning right 30 deg...\n"); RollingTurn(true, true, 30); delay(1000); mySerial.printf("Turning left 30 deg...\n"); mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount()); mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount()); RollingTurn(false, true, 30); sinceLastNavUpdateMsec = 0; } void loop() { // if programming failed, don't try to do anything if (dmpReady && mpu.dmpPacketAvailable()) { //global_yawval = GetIMUHeadingDeg(); //retrieve the most current yaw value from IMU GetIMUHeadingDeg(); //updates global_yawval if successful // blink LED to indicate activity bool blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } //if (SecSinceStart - StartSec > U_TURN_SEC) //{ // mySerial.printf("Turning Around!\n"); // RollingTurn(TURNDIR_CCW,true,180); // RunBothMotorsMsec(U_TURN_SEC*1000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL); // StopBothMotors(); // while (1); //} if (sinceLastNavUpdateMsec >= MIN_PING_INTERVAL_MSEC) { sinceLastNavUpdateMsec -= MIN_PING_INTERVAL_MSEC; int loc_leftdist = GetLeftDistCm(); int loc_rightdist = GetRightDistCm(); int loc_disterror = 0; int turndeg = 0; bool turndirection = false; UpdateDistArrays(loc_leftdist, loc_rightdist); ////decide what to do //if (loc_leftdist < loc_rightdist) //{ // //left wall is closer, so CCW moves twd wall, CW moves away // loc_disterror = loc_leftdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away' // turndirection = loc_disterror > 0 ? TURNDIR_CCW : TURNDIR_CW; // if (abs(loc_disterror) > VERY_FAR_AWAY_CM) //use 20-deg cut // { // turndeg = VERY_FAR_AWAY_TURN_DEG; // } // else if (abs(loc_disterror) > FAR_AWAY_CM) //use 10-deg cut // { // turndeg = FAR_AWAY_TURN_DEG; // } // else // { // mySerial.printf("Close to target distance - what do I do now?\n"); // RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL); // } //} //else //right wall is closer or equal... //{ // //right wall is closer, so CW moves twd wall, CCW moves away // loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away' // turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW; // if (abs(loc_disterror) > m_TrkErrWinMult * VERY_FAR_AWAY_CM) //use 20-deg cut // { // turndeg = VERY_FAR_AWAY_TURN_DEG; // } // else if (abs(loc_disterror) > m_TrkErrWinMult * FAR_AWAY_CM) //use 10-deg cut // { // turndeg = FAR_AWAY_TURN_DEG; // } // else if (abs(loc_disterror) > m_TrkErrWinMult * CLOSE_CM) //use 5-deg cut // { // turndeg = CLOSE_TURN_DEG; // m_TrkErrWinMult = WALL_TRK_ERR_WIN_MULTFACT; // } // else //must be close to desired offset distance. Reduce error window size // { // //mySerial.printf("Close to target distance - setting turndeg to zero\n"); // turndeg = 0; // m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach window sizes // } //} //if (turndeg == 0) //{ // //robot is close to target - take out any remaining cut // //mySerial.printf("turndeg = 0; Removing Entire Cut\n"); // if (m_PrevHdgCutDeg > 0) // { // RollingTurn(!turndirection, true, m_PrevHdgCutDeg); // m_PrevHdgCutDeg = 0; // } //} //else if (turndeg > m_PrevHdgCutDeg) //{ // //cut not cutting it; increase it by 10 deg // //mySerial.printf("turndeg > m_PrevHdgCutDeg; Increasing Cut\n"); // turndeg = TURN_INCREMENT_DEG; // RollingTurn(turndirection, true, turndeg); // m_PrevHdgCutDeg += TURN_INCREMENT_DEG; //} //else if (turndeg < m_PrevHdgCutDeg)// turndeg > 0 && < m_PrevHdgCutDeg) //{ // if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true)) // { // //take out some of the cut // //mySerial.printf(" turndeg > 0 && < m_PrevHdgCutDeg: Reducing Cut\n"); // turndeg = TURN_INCREMENT_DEG; // RollingTurn(!turndirection, true, turndeg); // m_PrevHdgCutDeg -= TURN_INCREMENT_DEG; // } //} //else if (turndeg == m_PrevHdgCutDeg) //this case takes more work //{ // if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true)) // { // //do nothing - things are going smoothly // } // else // { // //cut has been maxed out, but dist & tgt not converging // //increase cut in same direction as before // //mySerial.printf("not converging - increasing cut\n"); // turndeg = TURN_INCREMENT_DEG; // RollingTurn(turndirection, true, turndeg); // m_PrevHdgCutDeg += TURN_INCREMENT_DEG; // m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach sensitivity // } //} ////regardless of what happens above, continue to run the motors straight ahead //RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL); //07/09/19 if GetIMUHeadingDeg() succeeds, do the rest TotAmps = GetTotalAmps(); SecSinceStart = millis() / 1000.f; //hopefully this will be a float result mySerial.printf("%2.2f\t%3.2f\t%d\t%d\t%d\t%d\t%s\t%d\n", SecSinceStart, global_yawval,loc_leftdist,loc_rightdist, loc_disterror,m_PrevHdgCutDeg, turndirection == TURNDIR_CCW ? "CCW":"CW", turndeg); } } #pragma region MOTOR_SUPPORT_ROUTINES void RunBothMotors(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: //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 ////DEBUG!! // mySerial.printf("In RunBothMotors(%d,%d\n", leftspeednum, rightspeednum); ////DEBUG!! SetLeftMotorSpeed(leftspeednum); SetRightMotorSpeed(rightspeednum); } //12/26/14 added for independent motor speed void RunBothMotorsMsec(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 RunBothMotors(leftspeednum, rightspeednum); //Step 2: Delay timsec seconds delay(timeMsec); StopBothMotors(); } //11/25/15 added for symmetry ;-). void StopBothMotors() { //added analogWrite calls to ensure speed set to zero analogWrite(enA, MOTOR_SPEED_OFF); analogWrite(enB, MOTOR_SPEED_OFF); digitalWrite(in1, LOW); digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, LOW); } //01/24/15 removed time from MoveAhead sig, added MoveAheadMsec void MoveAhead(int leftspeednum, int rightspeednum) { //Purpose: Move ahead continuously //Provenance: 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 //Step 1: Set forward direction for both wheels SetLeftMotorDir(true); SetRightMotorDir(true); //Step 2: Run both myMotors for timsec seconds RunBothMotors(leftspeednum, rightspeednum); } void MoveReverse(int leftspeednum, int rightspeednum) { //Purpose: Move in reverse direction continuously - companion to MoveAhead() //Provenance: 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 //Step 1: Set reverse direction for both wheels SetLeftMotorDir(false); SetRightMotorDir(false); //Step 2: Run both myMotors for timsec seconds //01/24/15 - removed timing - now continuous run RunBothMotors(leftspeednum, rightspeednum); } void SetLeftMotorDir(boolean bIsFwdDir) { if (bIsFwdDir) { digitalWrite(in1, LOW); digitalWrite(in2, HIGH); } else //must be REV { digitalWrite(in1, HIGH); digitalWrite(in2, LOW); } bLeftMotorDirIsFwd = bIsFwdDir; } void SetRightMotorDir(boolean bIsFwdDir) { if (bIsFwdDir) { digitalWrite(in3, LOW); digitalWrite(in4, HIGH); } else //must be REV { digitalWrite(in3, HIGH); digitalWrite(in4, LOW); } bRightMotorDirIsFwd = bIsFwdDir; } //11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd void SetLeftMotorSpeed(int speed) { if (bLeftMotorDirIsFwd) { speed = speed + LEFT_SPEED_COMP_VAL_FWD; //check limits speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; } else //must be REV { speed = speed + LEFT_SPEED_COMP_VAL_REV; //check limits speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; } #ifndef NO_MOTORS //Serial.print("Left Speed = "); Serial.println(speed); analogWrite(enA, speed); #endif //02/13/16 added for 'pause' debug m_FinalLeftSpeed = speed; } //11/25/15 added to utilize new private member variable bRightMotorDirIsFwd void SetRightMotorSpeed(int speed) { if (bRightMotorDirIsFwd) { speed = speed + LEFT_SPEED_COMP_VAL_FWD; //check limits speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; } else //must be REV { speed = speed + LEFT_SPEED_COMP_VAL_REV; //check limits speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX; speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF; } //02/13/16 added for 'pause' debug m_FinalLeftSpeed = speed; #ifndef NO_MOTORS //Serial.print("Right Speed = "); Serial.println(speed); analogWrite(enB, speed); #endif } #pragma endregion Motor Support Routines bool GetIMUHeadingDeg() { //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 // global_yawval 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 global_yawval with latest value //Notes: // 10/08/19 changed return type to boolean // 10/08/19 no longer need mpuIntStatus //mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit fifoCount = mpu.getFIFOCount();// get current FIFO count // check for overflow (this should never happen unless our code is too inefficient) //if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) if (fifoCount >= 1024) { //if the FIFO count reset so we can continue cleanly mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount()); mpu.resetFIFO(); // now have to wait for DMP data ready again int wait_count = 0; while (!mpu.dmpPacketAvailable() && wait_count < 1000) { delay(1); wait_count++; } //if the wait loop timed out, punt if (wait_count >= 1000) { mySerial.printf("wait loop timed out wait_count = %d",wait_count); return false; } } // read all available packets from FIFO while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()! { mpu.getFIFOBytes(fifoBuffer, DMPpacketSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= DMPpacketSize; } // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //compute the yaw value global_yawval = ypr[0] * 180 / M_PI; return true; } float GetTotalAmps() { //Purpose: Get total current in amps //Inputs: // Voltage on TOT_CURR_PIN is approximately Ichg*2 Amps // VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio //Outputs: // returns total robot current (chg current plus running current) //Notes: // 02/28/18 chg name from GetBattChgAmps() to GetTotalAmps() int ITotAnalogReading = GetAverageAnalogReading(TOT_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023 float ITotVolts = ((float)ITotAnalogReading / (float)MAX_AD_VALUE)* ADC_REF_VOLTS; float ITotAmps = ITotVolts * VOLTAGE_TO_CURRENT_RATIO; return ITotAmps; } int GetAverageAnalogReading(int pin, int numavgs) { long totreads = 0; for (int i = 0; i < numavgs; i++) { totreads += analogRead(pin); } return (int)((float)totreads / (float)numavgs); //truncation ok } //01/10/18 reverted to regular ping(). median distance function takes too long int GetLeftDistCm() { //Serial.print("LeftPing\t"); Serial.println(millis()); //Notes: // 04/30/17 added limit detection/correction int timeUsec = LeftPing.ping(); //Serial.print("Left Ping Usec = "); Serial.println(timeUsec); int distCm = LeftPing.convert_cm(timeUsec); //04/30/17 added limit detection/correction distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM; return distCm; } //01/10/18 reverted to regular ping(). median distance function takes too long int GetRightDistCm() { //Serial.print("RightPing\t"); Serial.println(millis()); //Notes: // 04/30/17 added limit detection/correction int timeUsec = RightPing.ping(); //Serial.print("Right Ping Usec = "); Serial.println(timeUsec); int distCm = RightPing.convert_cm(timeUsec); //04/30/17 added limit detection/correction distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM; return distCm; } bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg) { //Purpose: Make a numDeg forwards or backwards CW turn //Inputs: // bIsFwd - True if turn is to be forward, 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 // global_yawval = IMU heading value updated by GetIMUHeadingDeg() //Plan: // Step1: Get current heading as starting point // Step2: Compute new target value // Step3: Set motor speeds based on fwd/backwds flag // Step4: Run motors until target reached //Notes: // 08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt // 08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this // 09/08/18 revised end-of-turn detection to include slope calc. Threshold alone is too fragile // 09/11/18 revised to handle both turn directions // 04/29/19 uncommented // 07/31/19 rev for better initial heading capture // 07/31/19 rev return type to bool // 07/31/19 rev to eliminate timed loop - now runs as fast as possible // 07/31/19 rev to not stop motors at end - now calling pgm is resp for this // 10/06/19 rev for new IMU polling setup float tgt_deg; float timeout_sec; bool bDoneTurning = false; bool bTimedOut = false; //DEBUG!! mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg); //DEBUG!! //Step1: Get current heading as starting point //if (!GetIMUHeadingDeg()) //{ // mySerial.printf("RollingTurn(%d,%d,%d): Initial Heading Capture Failed!", // bIsCCW, bIsFwd, numDeg); // return false; //} //no need to continue if the IMU isn't available if (!dmpReady) { return false; } //Wait for packet availability (times out after one second) ... int wait_count = 0; while (!mpu.dmpPacketAvailable() && wait_count < 1000) { delay(1); wait_count++; } //if the wait loop timed out, punt if (wait_count >= 1000) { return false; } //if we get to here, the IMU is OK and at least one packet is available //global_yawval = GetIMUHeadingDeg(); GetIMUHeadingDeg(); //updates global_yawval if successful //Step2: Compute new target value & timeout value //timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG; timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg; //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", global_yawval, numDeg, tgt_deg, timeout_sec); //DEBUG!! //Step3: Start motors SetRightMotorDir(bIsFwd); SetLeftMotorDir(bIsFwd); int rightspeed; int leftspeed; if (!bIsCCW) //2-motor robot is backwards from 4-motor one { rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED; leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH; } else { rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH; leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED; } SetLeftMotorSpeed(leftspeed); SetRightMotorSpeed(rightspeed); sinceLastTimeCheck = 0; //needed for watchdog timer //mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n"); bool bFirstPass = true; //for 'slowdown' print control float curHdgMatchVal = 0; //09/08/18 added to bolster end-of-turn detection float prevHdgMatchVal = 0; float matchSlope = 0; //Step4: Run motors until target reached mpu.resetFIFO(); while (!bDoneTurning && !bTimedOut) { //GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop if (mpu.dmpPacketAvailable()) { //global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail GetIMUHeadingDeg(); //updates global_yawval if successful //DEBUG!! mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount); //DEBUG!! } //07/31/19 now running as fast as we can get valid hdgs from IMU //check for nearly there and all the way there curHdgMatchVal = GetHdgMatchVal(tgt_deg, global_yawval); matchSlope = curHdgMatchVal - prevHdgMatchVal; if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL) { if (bFirstPass) { prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs //mySerial.printf("Slowing down at %4.2f deg\n", global_yawval); bFirstPass = false; } } //look for full match //bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL // || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < 0)); //have to use < vs <= as slope == 0 at start bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000); //DEBUG!! if (bDoneTurning) { mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n", global_yawval, tgt_deg, curHdgMatchVal, matchSlope); } //DEBUG!! if (bTimedOut) { mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", global_yawval, tgt_deg, curHdgMatchVal); } mpu.resetFIFO(); } return true; } 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 // global_yawval = 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); } bool isConverging(int tgtdist, bool bIsRightSide) { //Purpose: Determine if act dist & tgt dist are converging //Inputs: // tgtdist = it representing target distance // bIsRightSide = boolean denoting side under consideration. True = right, False = left // aRightDist[], aLeftDist[] = int arrays containing the LR_PING_DIST_ARRAY_SIZE previous ping measurements //Plan: // Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE // Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent) // if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent) // else, return FALSE (non-convergent) //Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE int slope = 0; int last_dist = 0; int first_dist = 0; if (bIsRightSide) { first_dist = aRightDist[0]; last_dist = aRightDist[LR_PING_DIST_ARRAY_SIZE - 1]; slope = last_dist - first_dist; //don't need to divide } else { first_dist = aLeftDist[0]; last_dist = aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1]; slope = last_dist - first_dist; //don't need to divide } ////DEBUG!! // mySerial.printf("IsConverging: firstd/lastd = %d/%d, slope = %d\ttgtdist = %d\n", first_dist,last_dist, slope, tgtdist); ////DEBUG!! //Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent) // if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent) // else, return FALSE (non-convergent) if (slope > 0 && last_dist <= tgtdist) return true; if (slope < 0 && last_dist >= tgtdist) return true; else return false; } void UpdateDistArrays(int leftdist, int rightdist) { for (size_t i = 0; i < LR_PING_DIST_ARRAY_SIZE-1; i++) { aLeftDist[i] = aLeftDist[i + 1]; aRightDist[i] = aRightDist[i + 1]; } aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1] = leftdist; aRightDist[LR_PING_DIST_ARRAY_SIZE - 1] = rightdist; } |
In the above code, the only relevant functions are the ‘GetIMUHeading()’ and ‘RollingTurn()’ functions, as shown below:
|
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 |
bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg) { //Purpose: Make a numDeg forwards or backwards CW turn //Inputs: // bIsFwd - True if turn is to be forward, 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 // global_yawval = IMU heading value updated by GetIMUHeadingDeg() //Plan: // Step1: Get current heading as starting point // Step2: Compute new target value // Step3: Set motor speeds based on fwd/backwds flag // Step4: Run motors until target reached //Notes: // 08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt // 08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this // 09/08/18 revised end-of-turn detection to include slope calc. Threshold alone is too fragile // 09/11/18 revised to handle both turn directions // 04/29/19 uncommented // 07/31/19 rev for better initial heading capture // 07/31/19 rev return type to bool // 07/31/19 rev to eliminate timed loop - now runs as fast as possible // 07/31/19 rev to not stop motors at end - now calling pgm is resp for this // 10/06/19 rev for new IMU polling setup float tgt_deg; float timeout_sec; bool bDoneTurning = false; bool bTimedOut = false; //DEBUG!! mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg); //DEBUG!! //no need to continue if the IMU isn't available if (!dmpReady) { return false; } //Wait for packet availability (times out after one second) ... int wait_count = 0; while (!mpu.dmpPacketAvailable() && wait_count < 1000) { delay(1); wait_count++; } //if the wait loop timed out, punt if (wait_count >= 1000) { return false; } //if we get to here, the IMU is OK and at least one packet is available GetIMUHeadingDeg(); //updates global_yawval if successful //Step2: Compute new target value & timeout value //timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG; timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant for testing tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg; //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", global_yawval, numDeg, tgt_deg, timeout_sec); //DEBUG!! //Step3: Start motors SetRightMotorDir(bIsFwd); SetLeftMotorDir(bIsFwd); int rightspeed; int leftspeed; if (!bIsCCW) //2-motor robot is backwards from 4-motor one { rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED; leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH; } else { rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH; leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED; } SetLeftMotorSpeed(leftspeed); SetRightMotorSpeed(rightspeed); sinceLastTimeCheck = 0; //needed for watchdog timer //mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n"); bool bFirstPass = true; //for 'slowdown' print control float curHdgMatchVal = 0; //09/08/18 added to bolster end-of-turn detection float prevHdgMatchVal = 0; float matchSlope = 0; //Step4: Run motors until target reached while (!bDoneTurning && !bTimedOut) { //GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop if (mpu.dmpPacketAvailable()) { //global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail GetIMUHeadingDeg(); //updates global_yawval if successful //DEBUG!! mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount); //DEBUG!! } //07/31/19 now running as fast as we can get valid hdgs from IMU //check for nearly there and all the way there curHdgMatchVal = GetHdgMatchVal(tgt_deg, global_yawval); matchSlope = curHdgMatchVal - prevHdgMatchVal; if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL) { if (bFirstPass) { prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs //mySerial.printf("Slowing down at %4.2f deg\n", global_yawval); bFirstPass = false; } } //look for full match //bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL // || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < 0)); //have to use < vs <= as slope == 0 at start bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000); //DEBUG!! if (bDoneTurning) { mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n", global_yawval, tgt_deg, curHdgMatchVal, matchSlope); } //DEBUG!! if (bTimedOut) { //DEBUG!! mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", global_yawval, tgt_deg, curHdgMatchVal); return false; //DEBUG!! } } return true; } |
|
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 |
bool GetIMUHeadingDeg() { //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 // global_yawval 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 global_yawval with latest value //Notes: // 10/08/19 changed return type to boolean // 10/08/19 no longer need mpuIntStatus //mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit fifoCount = mpu.getFIFOCount();// get current FIFO count // check for overflow (this should never happen unless our code is too inefficient) //if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) if (fifoCount >= 1024) { //if the FIFO count reset so we can continue cleanly mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount()); mpu.resetFIFO(); // now have to wait for DMP data ready again int wait_count = 0; while (!mpu.dmpPacketAvailable() && wait_count < 1000) { delay(1); wait_count++; } //if the wait loop timed out, punt if (wait_count >= 1000) { mySerial.printf("wait loop timed out wait_count = %d",wait_count); return false; } } // read all available packets from FIFO while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()! { mpu.getFIFOBytes(fifoBuffer, DMPpacketSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= DMPpacketSize; } // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //compute the yaw value global_yawval = ypr[0] * 180 / M_PI; return true; } |
When the ‘RollingTurn()’ function is called, it waits for mpu.dmpPacketAvailable() to return TRUE, and then it calls GetIMUHeadingDeg(), which updates a global variable (subtly named ‘global_yawval’. This value is then used to determine turn completion.
GetIMUHeadingDeg() reads bytes from the FIFO and computes a yaw value using the retreived quaternion data.
After getting everything going to my satisfaction, I added code to setup() for a 30-degree turn to the right followed by a 30-degree turn to the left, followed by an infinite loop of yaw value readouts. The output from one test run is shown below.
|
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 |
Opening port Port open Two Wheel Robot Initializing MPU6050 ... Testing device connections... MPU6050 connection successful Initializing DMP... Enabling DMP... DMP ready! Waiting for MPU6050 drift rate to settle... MPU6050 Ready at 5.22 Sec Sec Hdg Ldist Rdist ErrDeg PrevDeg TurnDir TurnDeg Starting test turns... FIFO count = 0 Turning right 30 deg... In RollingTurn(CW, FWD,30.00) Init hdg = 2.13 deg, Turn = 30.00 deg, tgt = 32.13 deg, timeout = 20000.00 sec 6269 2.14 0 6280 2.16 0 6289 2.18 0 6299 2.20 0 6308 2.22 0 6319 2.23 0 6329 2.25 0 6339 2.27 0 6348 2.29 0 6359 2.32 0 6369 2.34 0 6379 2.35 0 6389 2.37 0 6398 2.39 0 6409 2.41 0 6418 2.42 0 6428 2.44 0 6439 2.46 0 6449 2.48 0 6459 2.51 0 6468 2.52 0 6478 2.54 0 6489 2.56 0 6499 2.58 0 6509 2.60 0 6518 2.61 0 6529 2.63 0 6539 2.66 0 6549 2.68 0 6558 2.69 0 6568 2.71 0 6578 2.73 0 6589 2.75 0 6599 2.77 0 6608 2.79 0 6619 2.80 0 6628 2.83 0 6638 2.85 0 6648 2.87 0 6659 2.89 0 6668 2.91 0 6678 2.92 0 6688 2.94 0 6699 2.96 0 6709 2.98 0 6719 3.00 0 6728 3.02 0 6737 3.05 0 6748 3.07 0 6758 3.10 0 6768 3.13 0 6778 3.15 0 6788 3.19 0 6798 3.25 0 6808 3.31 0 6818 3.39 0 6829 3.50 0 6838 3.71 0 6848 4.02 0 6857 4.40 0 6867 4.75 0 6878 5.06 0 6888 5.38 0 6898 5.69 0 6907 6.02 0 6918 6.36 0 6928 6.70 0 6938 7.00 0 6948 7.28 0 6958 7.58 0 6968 7.89 0 6977 8.22 0 6987 8.53 0 6998 8.82 0 7008 9.13 0 7017 9.44 0 7027 9.72 0 7037 9.96 0 7048 10.20 0 7058 10.43 0 7067 10.65 0 7077 10.88 0 7088 11.12 0 7098 11.33 0 7107 11.54 0 7117 11.78 0 7128 12.04 0 7138 12.25 0 7147 12.47 0 7157 12.70 0 7168 12.92 0 7177 13.13 0 7187 13.35 0 7197 13.56 0 7207 13.76 0 7217 13.99 0 7227 14.28 0 7237 14.59 0 7247 14.93 0 7257 15.26 0 7267 15.57 0 7277 15.87 0 7287 16.15 0 7297 16.39 0 7308 16.62 0 7317 16.86 0 7327 17.10 0 7336 17.33 0 7347 17.59 0 7357 17.90 0 7366 18.23 0 7377 18.55 0 7387 18.86 0 7397 19.17 0 7407 19.48 0 7417 19.77 0 7427 20.00 0 7437 20.22 0 7447 20.45 0 7456 20.68 0 7467 20.91 0 7477 21.15 0 7487 21.40 0 7496 21.65 0 7506 21.90 0 7517 22.16 0 7527 22.41 0 7536 22.65 0 7546 22.85 0 7557 23.04 0 7566 23.22 0 7576 23.43 0 7586 23.68 0 7597 23.97 0 7606 24.23 0 7617 24.46 0 7626 24.73 0 7636 25.05 0 7647 25.36 0 7657 25.63 0 7666 25.87 0 7676 26.13 0 7687 26.44 0 7697 26.78 0 7706 27.08 0 7716 27.39 0 7727 27.71 0 7736 28.03 0 7746 28.32 0 7756 28.58 0 7767 28.84 0 7776 29.03 0 7786 29.24 0 7796 29.47 0 7805 29.73 0 7816 29.99 0 7826 30.24 0 7836 30.48 0 found match with yaw = 30.48, tgt = 32.13, match = 0.991, and slope = 0.090 Turning left 30 deg... FIFO count = 1024 FIFO count = 0 In RollingTurn(CCW, FWD,30.00) Init hdg = 41.58 deg, Turn = 30.00 deg, tgt = 11.58 deg, timeout = 20000.00 sec 8864 41.59 0 8873 41.59 0 8885 41.54 0 8894 41.55 0 8903 41.57 0 8913 41.58 0 8925 41.58 0 8934 41.44 0 8943 41.26 0 8953 41.06 0 8964 40.78 0 8974 40.49 0 8984 40.23 0 8993 39.98 0 9004 39.72 0 9014 39.43 0 9023 39.14 0 9033 38.84 0 9044 38.53 0 9054 38.22 0 9064 37.91 0 9073 37.62 0 9083 37.42 0 9094 37.32 0 9103 37.28 0 9113 37.30 0 9124 37.32 0 9134 37.33 0 9143 37.33 0 9153 37.21 0 9163 37.08 0 9174 36.96 0 9183 36.80 0 9194 36.65 0 9203 36.51 0 9213 36.38 0 9223 36.26 0 9234 36.13 0 9243 36.01 0 9253 35.85 0 9264 35.66 0 9274 35.46 0 9283 35.25 0 9293 35.04 0 9304 34.85 0 9314 34.68 0 9323 34.53 0 9333 34.42 0 9344 34.31 0 9354 34.20 0 9363 34.09 0 9373 33.97 0 9383 33.84 0 9393 33.69 0 9403 33.52 0 9413 33.35 0 9423 33.18 0 9433 33.03 0 9443 32.88 0 9453 32.73 0 9463 32.59 0 9473 32.44 0 9483 32.26 0 9493 32.07 0 9503 31.85 0 9512 31.63 0 9523 31.42 0 9533 31.25 0 9543 31.11 0 9552 30.99 0 9563 30.91 0 9573 30.84 0 9583 30.75 0 9592 30.66 0 9603 30.55 0 9613 30.44 0 9623 30.32 0 9632 30.22 0 9643 30.10 0 9653 29.98 0 9663 29.86 0 9672 29.70 0 9682 29.54 0 9693 29.37 0 9703 29.18 0 9712 29.01 0 9722 28.82 0 9733 28.64 0 9742 28.46 0 9752 28.30 0 9762 28.13 0 9773 27.95 0 9782 27.77 0 9792 27.59 0 9802 27.39 0 9811 27.19 0 9822 26.99 0 9832 26.80 0 9842 26.62 0 9851 26.45 0 9863 26.26 0 9872 26.06 0 9882 25.85 0 9892 25.62 0 9903 25.38 0 9912 25.13 0 9922 24.87 0 9932 24.62 0 9942 24.36 0 9952 24.09 0 9962 23.85 0 9972 23.62 0 9981 23.39 0 9992 23.12 0 10002 22.84 0 10011 22.56 0 10021 22.29 0 10032 22.03 0 10042 21.76 0 10052 21.54 0 10061 21.35 0 10072 21.17 0 10082 20.97 0 10091 20.78 0 10102 20.61 0 10112 20.45 0 10122 20.28 0 10132 20.13 0 10141 19.95 0 10151 19.81 0 10162 19.68 0 10171 19.56 0 10182 19.40 0 10191 19.24 0 10202 19.06 0 10212 18.90 0 10221 18.73 0 10231 18.56 0 10242 18.39 0 10251 18.22 0 10262 18.01 0 10271 17.82 0 10281 17.65 0 10292 17.47 0 10301 17.29 0 10311 17.14 0 10321 17.02 0 10331 16.90 0 10342 16.77 0 10351 16.64 0 10361 16.48 0 10372 16.33 0 10381 16.18 0 10391 16.01 0 10401 15.84 0 10412 15.64 0 10421 15.42 0 10431 15.22 0 10441 15.01 0 10451 14.81 0 10461 14.64 0 10471 14.49 0 10481 14.36 0 10491 14.24 0 10501 14.11 0 10511 13.97 0 10521 13.81 0 10530 13.64 0 10541 13.50 0 10551 13.36 0 found match with yaw = 13.36, tgt = 11.58, match = 0.990, and slope = 0.090 10.78 10.13 200 200 0 0 CCW 0 10.98 7.50 200 200 0 0 CCW 0 11.18 7.28 200 200 0 0 CCW 0 11.38 7.67 200 200 0 0 CCW 0 11.58 7.23 200 200 0 0 CCW 0 11.78 4.82 200 200 0 0 CCW 0 11.98 5.16 200 200 0 0 CCW 0 12.18 5.56 200 200 0 0 CCW 0 12.38 5.97 200 200 0 0 CCW 0 12.58 6.73 200 200 0 0 CCW 0 12.78 7.44 200 200 0 0 CCW 0 12.98 7.96 200 200 0 0 CCW 0 13.18 10.70 200 200 0 0 CCW 0 13.38 12.16 200 200 0 0 CCW 0 13.58 12.86 200 200 0 0 CCW 0 13.78 13.98 200 200 0 0 CCW 0 13.98 14.56 200 200 0 0 CCW 0 14.18 15.57 200 200 0 0 CCW 0 14.38 16.86 200 200 0 0 CCW 0 14.58 17.39 200 200 0 0 CCW 0 14.78 17.37 200 200 0 0 CCW 0 14.98 17.60 200 200 0 0 CCW 0 15.18 17.96 200 200 0 0 CCW 0 15.38 18.34 200 200 0 0 CCW 0 15.58 18.72 200 200 0 0 CCW 0 15.78 19.10 200 200 0 0 CCW 0 15.98 19.50 200 200 0 0 CCW 0 16.18 19.88 200 200 0 0 CCW 0 16.38 20.21 200 200 0 0 CCW 0 16.58 20.99 200 200 0 0 CCW 0 16.78 22.92 200 200 0 0 CCW 0 16.98 25.77 200 200 0 0 CCW 0 17.18 26.87 200 200 0 0 CCW 0 17.38 28.11 200 200 0 0 CCW 0 17.58 29.87 200 200 0 0 CCW 0 17.78 30.60 200 200 0 0 CCW 0 17.98 30.97 200 200 0 0 CCW 0 18.18 31.40 200 200 0 0 CCW 0 18.38 31.87 200 200 0 0 CCW 0 18.58 32.27 200 200 0 0 CCW 0 18.78 33.46 200 200 0 0 CCW 0 18.98 36.06 200 200 0 0 CCW 0 19.18 39.47 200 200 0 0 CCW 0 19.38 46.26 200 200 0 0 CCW 0 19.58 53.56 200 200 0 0 CCW 0 19.78 57.95 200 200 0 0 CCW 0 19.98 62.18 200 200 0 0 CCW 0 20.18 68.34 200 200 0 0 CCW 0 20.38 74.71 200 200 0 0 CCW 0 20.58 80.05 200 200 0 0 CCW 0 20.78 84.66 200 200 0 0 CCW 0 20.98 91.72 200 200 0 0 CCW 0 21.18 100.94 200 200 0 0 CCW 0 21.38 109.92 200 200 0 0 CCW 0 21.58 116.38 200 200 0 0 CCW 0 21.78 118.67 200 200 0 0 CCW 0 21.98 119.04 200 200 0 0 CCW 0 22.18 119.33 200 200 0 0 CCW 0 22.38 117.98 200 200 0 0 CCW 0 22.58 109.04 200 200 0 0 CCW 0 22.78 99.40 200 200 0 0 CCW 0 22.98 92.47 200 200 0 0 CCW 0 23.18 85.81 200 200 0 0 CCW 0 23.38 81.29 200 200 0 0 CCW 0 23.58 74.51 200 200 0 0 CCW 0 23.78 67.58 200 200 0 0 CCW 0 23.98 59.32 200 200 0 0 CCW 0 24.18 53.99 200 200 0 0 CCW 0 24.38 50.39 200 200 0 0 CCW 0 24.58 46.73 200 200 0 0 CCW 0 24.78 42.14 200 200 0 0 CCW 0 24.98 39.26 200 200 0 0 CCW 0 25.18 39.60 200 200 0 0 CCW 0 25.38 39.97 200 200 0 0 CCW 0 25.58 40.36 200 200 0 0 CCW 0 25.78 40.73 200 200 0 0 CCW 0 25.98 41.11 200 200 0 0 CCW 0 26.18 40.66 200 200 0 0 CCW 0 26.38 39.16 200 200 0 0 CCW 0 26.58 36.13 200 200 0 0 CCW 0 26.78 35.51 200 200 0 0 CCW 0 26.98 35.92 200 200 0 0 CCW 0 27.18 36.30 200 200 0 0 CCW 0 27.38 36.67 200 200 0 0 CCW 0 27.58 37.06 200 200 0 0 CCW 0 27.78 37.46 200 200 0 0 CCW 0 27.98 37.85 200 200 0 0 CCW 0 28.18 38.22 200 200 0 0 CCW 0 28.38 38.60 200 200 0 0 CCW 0 28.58 38.97 200 200 0 0 CCW 0 28.78 39.35 200 200 0 0 CCW 0 28.98 39.72 200 200 0 0 CCW 0 29.18 40.10 200 200 0 0 CCW 0 29.38 40.48 200 200 0 0 CCW 0 29.58 40.86 200 200 0 0 CCW 0 29.78 41.23 200 200 0 0 CCW 0 29.98 41.61 200 200 0 0 CCW 0 30.18 41.98 200 200 0 0 CCW 0 30.38 42.37 200 200 0 0 CCW 0 30.58 42.74 200 200 0 0 CCW 0 30.78 43.12 200 200 0 0 CCW 0 30.98 43.50 200 200 0 0 CCW 0 31.18 43.87 200 200 0 0 CCW 0 31.38 44.25 200 200 0 0 CCW 0 31.58 44.63 200 200 0 0 CCW 0 31.78 45.01 200 200 0 0 CCW 0 31.98 45.40 200 200 0 0 CCW 0 32.18 45.78 200 200 0 0 CCW 0 32.38 46.15 200 200 0 0 CCW 0 32.58 46.53 200 200 0 0 CCW 0 32.78 46.91 200 200 0 0 CCW 0 32.98 47.29 200 200 0 0 CCW 0 33.18 47.67 200 200 0 0 CCW 0 |
Shown below are the yaw values plotted against time in Excel
The next step will be to port the updated software back into my two-wheel robot to confirm that heading-based turns can be accomplished automatically (this is something that I had going before, but…).
Stay tuned!
Frank
Pingback: MPU6050 FIFO Buffer Management Study - Paynter's Palace
Pingback: IMU Motor Noise Troubleshooting, Part III | Paynter's Palace
Pingback: Turn Rate PID Tuning | Paynter's Palace