Refactored

.github/workflows/TestCompile.yml

@@ -42,6 +42,7 @@ jobs:
           - arduino-boards-fqbn: arduino:avr:uno|TB6612Breakout # Nano board with TB6612 breakout board
             build-properties:
               RobotCarBlueDisplay:
+                -DUSE_LAYOUT_FOR_NANO
                 -DBLUETOOTH_BAUD_RATE=BAUD_115200
                 -DUSE_STANDARD_SERIAL
                 -DUSE_US_SENSOR_1_PIN_MODE

README.md

@@ -17,20 +17,27 @@ You may also overwrite the function fillAndShowForwardDistancesInfo(), if you us
 | Option | Default | File | Description |
 |-|-|-|-|
 | `USE_ENCODER_MOTOR_CONTROL` | disabled | PWMDCMotor.h | Use fork light barrier and an attached encoder disc to enable motor distance and speed sensing. |
+| `USE_ADAFRUIT_MOTOR_SHIELD` | disabled | PWMDcMotor.h | | Use Adafruit Motor Shield v2 connected by I2C instead of simple TB6612 or L298 breakout board.<br/>
+This disables tone output by using motor as loudspeaker, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bride.<br/>
+For full bride, analogWrite the millis() timer0 is used since we use pin 5 & 6. |
 | `USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD` |  enabled | PWMDcMotor.h | Saves 694 bytes program memory  |
-| `CAR_HAS_4_WHEELS` | `CAR_HAS_2_WHEELS` | RobotCar.h | Use modified formula dor turning the car. |
+| `SUPPORT_RAMP_UP` | enabled | PWMDcMotor.h | `DO_NOT_SUPPORT_RAMP_UP` | Saves around 300 bytes program memory |
+|-|-|-|-|
+| `USE_LAYOUT_FOR_NANO` | disabled | RobotCar.h | Use different pinout for Nano board. It has A6 and A7 available as pins. |
+| `CAR_HAS_4_WHEELS` | disabled | RobotCar.h | Use modified formula dor turning the car. |
 | `USE_US_SENSOR_1_PIN_MODE` | disabled | RobotCar.h | Use modified HC-SR04 modules or HY-SRF05 ones.</br>Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger pin. |
 | `CAR_HAS_IR_DISTANCE_SENSOR` | disabled | RobotCar.h | Use Sharp GP2Y0A21YK / 1080 IR distance sensor. |
 | `CAR_HAS_TOF_DISTANCE_SENSOR` | disabled | RobotCar.h | Use VL53L1X TimeOfFlight distance sensor. |
+| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. |
 | `CAR_HAS_CAMERA` | disabled | RobotCar.h | Enables the `Camera` button for the `PIN_CAMERA_SUPPLY_CONTROL` pin. |
 | `CAR_HAS_LASER` | disabled | RobotCar.h | Enables the `Laser` button for the `PIN_LASER_OUT` / `LED_BUILTIN` pin. |
 | `CAR_HAS_PAN_SERVO` | disabled | RobotCar.h | Enables the pan slider for the `PanServo` at the `PIN_PAN_SERVO` pin. |
 | `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin.. |
-| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. |
 | `MONITOR_LIPO_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. Requires 2 additional resistors at pin A2. |
 | `VIN_VOLTAGE_CORRECTION` | undefined | RobotCar.h | Voltage to be substracted from VIN voltage. E.g. if there is a series diode between LIPO and VIN set it to 0.8. |
 
 
+
 # Wall detection
 The problem of the ultrasonic values is, that you can only detect a wall with the ultrasonic sensor if the angle of the wall relative to sensor axis is approximately between 70 and 110 degree.
 For other angels the reflected ultrasonic beam can not not reach the receiver which leads to unrealistic great distances.<br/>

src/AutonomousDrive.cpp

@@ -575,7 +575,7 @@ void driveFollowerModeOneStep() {
         RobotCarMotorControl.stopMotors();
 
         clearPrintedForwardDistancesInfos();
-        // show current distance
+        // show current distance (as US distance), which triggers the rescan
         showUSDistance(tCentimeter);
 
         /*
@@ -594,7 +594,7 @@ void driveFollowerModeOneStep() {
         }
         for (uint8_t i = 0; i < 3; ++i) {
             DistanceServoWriteAndDelay(tDegreeForSearch, true);
-            tCentimeter = getDistanceAsCentiMeter();
+            tCentimeter = getDistanceAsCentiMeter(false);
             if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) {
                 /*
                  * Determine color
@@ -669,17 +669,14 @@ void driveFollowerModeOneStep() {
 //        Serial.println(F("Stop"));
         RobotCarMotorControl.stopMotors();
     }
-
-    // show distance bars
-    showUSDistance(tCentimeter);
     delayAndLoopGUI(40); // the IR sensor takes 39 ms for one measurement
 }
 
 unsigned int __attribute__((weak)) getDistanceAndPlayTone() {
     /*
      * Get distance; timeout is 1 meter
      */
-    unsigned int tCentimeter = getDistanceAsCentiMeter();
+    unsigned int tCentimeter = getDistanceAsCentiMeter(true);
     /*
      * play tone
      */

src/CarMotorControl.cpp

@@ -161,9 +161,9 @@ void CarMotorControl::resetControlValues() {
  * Used to suppress time consuming display of motor values
  */
 bool CarMotorControl::needsFastUpdates() {
-#ifdef USE_ENCODER_MOTOR_CONTROL
-    return (rightCarMotor.State == MOTOR_STATE_RAMP_DOWN || rightCarMotor.State == MOTOR_STATE_RAMP_UP
-            || leftCarMotor.State == MOTOR_STATE_RAMP_DOWN || leftCarMotor.State == MOTOR_STATE_RAMP_UP);
+#ifdef SUPPORT_RAMP_UP
+    return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP
+            || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP);
 #else
     return false;
 #endif
@@ -178,22 +178,34 @@ bool CarMotorControl::updateMotors() {
     return tMotorsNotStopped;
 }
 
+#ifdef SUPPORT_RAMP_UP
 void CarMotorControl::initRampUp(uint8_t aRequestedDirection) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
     rightCarMotor.initRampUp(aRequestedDirection);
     leftCarMotor.initRampUp(aRequestedDirection);
-#else
-    rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection);
-    leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection);
-#endif
 }
 
+/*
+ * Blocking wait until both motors are at drive speed.
+ */
+void CarMotorControl::waitForDriveSpeed() {
+    /*
+     * blocking wait for MOTOR_STATE_DRIVE_SPEED
+     */
+    bool tMotorsNotStopped;
+    do {
+        tMotorsNotStopped = rightCarMotor.updateMotor();
+        tMotorsNotStopped |= leftCarMotor.updateMotor();
+    } while (tMotorsNotStopped
+            && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED));
+}
+#endif
+
 void CarMotorControl::initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
+#ifdef SUPPORT_RAMP_UP
     rightCarMotor.initRampUp(aRequestedDirection);
     leftCarMotor.initRampUp(aRequestedDirection);
     /*
-     * blocking wait for MOTOR_STATE_FULL_SPEED, 256 milliseconds
+     * blocking wait for MOTOR_STATE_DRIVE_SPEED, 256 milliseconds
      */
     bool tMotorsNotStopped; // to check if motors are not stopped by aLoopCallback
     do {
@@ -202,8 +214,10 @@ void CarMotorControl::initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection
         if (aLoopCallback != NULL) {
             aLoopCallback(); // this may stop motors
         }
-    } while (tMotorsNotStopped && (rightCarMotor.State != MOTOR_STATE_FULL_SPEED || leftCarMotor.State != MOTOR_STATE_FULL_SPEED));
+    } while (tMotorsNotStopped
+            && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED));
 #else
+    (void) aLoopCallback;
     rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection);
     leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection);
 #endif
@@ -241,19 +255,17 @@ void CarMotorControl::goDistanceCentimeter(int aDistanceCentimeter, void (*aLoop
 /*
  * Stop car with ramp and give DistanceCountAfterRampUp counts for braking.
  *
- * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_FULL_SPEED to MOTOR_STATE_RAMP_DOWN
+ * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN
  * Use DistanceCountAfterRampUp as ramp down count
  * Blocking wait for stop
  */
 void CarMotorControl::stopCarAndWaitForIt() {
     if (isStopped()) {
         return;
     }
-#ifdef USE_ENCODER_MOTOR_CONTROL
-    rightCarMotor.doOnlyRampUp = false;
+#if defined(USE_ENCODER_MOTOR_CONTROL) && defined(SUPPORT_RAMP_UP)
     rightCarMotor.NextChangeMaxTargetCount = rightCarMotor.EncoderCount;
     rightCarMotor.TargetDistanceCount = rightCarMotor.EncoderCount + rightCarMotor.DistanceCountAfterRampUp;
-    leftCarMotor.doOnlyRampUp = false;
     leftCarMotor.NextChangeMaxTargetCount = leftCarMotor.EncoderCount;
     leftCarMotor.TargetDistanceCount = leftCarMotor.EncoderCount + leftCarMotor.DistanceCountAfterRampUp;
     /*
@@ -280,8 +292,8 @@ void CarMotorControl::waitUntilCarStopped(void (*aLoopCallback)(void)) {
 }
 
 bool CarMotorControl::isState(uint8_t aState) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
-    return (rightCarMotor.State == aState && leftCarMotor.State == aState);
+#if defined(SUPPORT_RAMP_UP)
+    return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState);
 #else
     (void) aState;
     return false;
@@ -354,12 +366,23 @@ void CarMotorControl::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirec
     leftCarMotor.initGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor);
     if (aUseSlowSpeed) {
         // adjust CurrentDriveSpeed
-#ifdef USE_ENCODER_MOTOR_CONTROL
-        rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2;
-        leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2;
+#if defined(SUPPORT_RAMP_UP)
+        // avoid overflow, the reduced speed is almost max speed then.
+        if (rightCarMotor.StartSpeed < 160) {
+            rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2;
+        }
+        if (leftCarMotor.StartSpeed < 160) {
+            leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2;
+        }
 #else
-        rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2, rightCarMotor.CurrentDirection);
-        leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2, leftCarMotor.CurrentDirection);
+        if (tDistanceCountRight > 0) {
+            rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2,
+                    rightCarMotor.CurrentDirectionOrBrakeMode);
+        }
+        if (tDistanceCountLeft > 0) {
+            leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2,
+                    leftCarMotor.CurrentDirectionOrBrakeMode);
+        }
 #endif
     }
 }
@@ -383,21 +406,6 @@ void CarMotorControl::setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aD
     leftCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor);
 }
 #else
-/*
- * Start motor for "infinite" distance and then blocking wait until both motors are at drive speed
- * If motor is still running, just update motor.
- */
-void CarMotorControl::waitForDriveSpeed() {
-    /*
-     * blocking wait for MOTOR_STATE_FULL_SPEED
-     */
-    bool tMotorsNotStopped;
-    do {
-        tMotorsNotStopped = rightCarMotor.updateMotor();
-        tMotorsNotStopped |= leftCarMotor.updateMotor();
-    } while (tMotorsNotStopped && (rightCarMotor.State != MOTOR_STATE_FULL_SPEED || leftCarMotor.State != MOTOR_STATE_FULL_SPEED));
-}
-
 /*
  * generates a rising ramp and detects the first movement -> this sets dead band / minimum Speed
  */
@@ -408,24 +416,24 @@ void CarMotorControl::calibrate() {
     rightCarMotor.StartSpeed = 0;
     leftCarMotor.StartSpeed = 0;
 
+    uint8_t tMotorMovingCount = 0;
+
     /*
      * increase motor speed by 1 until motor moves
      */
     for (uint8_t tSpeed = 20; tSpeed != 0xFF; ++tSpeed) {
         if (rightCarMotor.StartSpeed == 0) {
             rightCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD);
-            rightCarMotor.CurrentSpeed = tSpeed;
         }
         if (leftCarMotor.StartSpeed == 0) {
             leftCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD);
-            leftCarMotor.CurrentSpeed = tSpeed;
         }
 
         delay(100);
         /*
          * Check if wheel moved
          */
-        uint8_t tMotorMovingCount = 0;
+
         /*
          * Store speed after 6 counts (3cm)
          */

src/CarMotorControl.h

@@ -53,15 +53,18 @@ class CarMotorControl {
     void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight);
 #ifdef USE_ENCODER_MOTOR_CONTROL
     void calibrate();
-    void waitForDriveSpeed();
 #else
     // makes no sense for encoder motor
     void setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor);
 #endif
 
+#ifdef SUPPORT_RAMP_UP
     void initRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD);
+    void waitForDriveSpeed();
+#endif
     void initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection = DIRECTION_FORWARD,  void (*aLoopCallback)(void) = NULL);
 
+
     /*
      * Functions for moving a fixed distance
      */

src/Distance.cpp

@@ -284,38 +284,41 @@ void readAndShowDistancePeriodically(uint16_t aPeriodMillis) {
         long tMillis = millis();
         if (sLastUSMeasurementMillis + aPeriodMillis < tMillis) {
             sLastUSMeasurementMillis = tMillis;
-#ifdef CAR_HAS_IR_DISTANCE_SENSOR
-            showIRDistance(getIRDistanceAsCentimeter());
-#elif CAR_HAS_TOF_DISTANCE_SENSOR
-            showIRDistance(getToFDistanceAsCentimeter());
-#endif
-            // feedback as slider length
-            showUSDistance(getUSDistanceAsCentiMeterWithCentimeterTimeout(300));
+            getDistanceAsCentiMeter(true);
         }
     }
 }
 
 /*
  * Timeout is DISTANCE_TIMEOUT_CM (1 meter)
  */
-unsigned int getDistanceAsCentiMeter() {
+unsigned int getDistanceAsCentiMeter(bool doShow) {
 #ifdef CAR_HAS_TOF_DISTANCE_SENSOR
     if (sScanMode != SCAN_MODE_US) {
         sToFDistanceSensor.VL53L1X_StartRanging();
     }
 #endif
 
     unsigned int tCentimeter = getUSDistanceAsCentiMeterWithCentimeterTimeout(DISTANCE_TIMEOUT_CM);
+    if (doShow) {
+        showUSDistance(tCentimeter);
+    }
 #if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR)
     unsigned int tIRCentimeter;
     if (sScanMode != SCAN_MODE_US) {
 #  if defined(CAR_HAS_IR_DISTANCE_SENSOR)
         if (sScanMode != SCAN_MODE_US) {
             tIRCentimeter = getIRDistanceAsCentimeter();
+            if (doShow) {
+                showIRDistance(tIRCentimeter);
+            }
         }
 #  elif defined(CAR_HAS_TOF_DISTANCE_SENSOR)
         if (sScanMode != SCAN_MODE_US) {
             tIRCentimeter = readToFDistanceAsCentimeter();
+            if(doShow){
+                showIRDistance(tIRCentimeter);
+            }
         }
 #  endif
         if (sScanMode == SCAN_MODE_IR) {

src/Distance.h

@@ -36,6 +36,11 @@ extern Servo DistanceServo;
 #define STEPS_PER_SCAN      (NUMBER_OF_DISTANCES - 1) // -> 162 degrees for 18 DEGREES_PER_STEP, 153 for 17 degrees
 #define START_DEGREES       ((180 - (DEGREES_PER_STEP * STEPS_PER_SCAN)) / 2) // 9 for 18, 13,5 for 17 - we need it symmetrical in the 180 degrees range
 
+// for future use maybe
+//#define SERVO_CURRENT_LOW_THRESHOLD 100
+//#define SERVO_INITIAL_DELAY 5
+//#define SERVO_CURRENT_LOW_MILLIS_FOR_SERVO_STOPPED 12
+
 /*
  * Index definitions for ForwardDistancesInfoStruct
  */
@@ -69,7 +74,7 @@ extern int sLastDegreesTurned;
 
 void initDistance();
 void DistanceServoWriteAndDelay(uint8_t aValue, bool doDelay = false);
-unsigned int getDistanceAsCentiMeter();
+unsigned int getDistanceAsCentiMeter(bool doShow = false);
 bool fillAndShowForwardDistancesInfo(bool aDoFirstValue, bool aForceScan = false);
 void postProcessDistances();