diff --git a/.github/workflows/LibraryBuild.yml b/.github/workflows/LibraryBuild.yml index b347b5d..d6dd5d8 100644 --- a/.github/workflows/LibraryBuild.yml +++ b/.github/workflows/LibraryBuild.yml @@ -1,7 +1,7 @@ # LibraryBuild.yml # Github workflow script to test compile all examples of an Arduino library repository. # -# Copyright (C) 2020 Armin Joachimsmeyer +# Copyright (C) 2020-2024 Armin Joachimsmeyer # https://github.com/ArminJo/Github-Actions # @@ -27,7 +27,7 @@ jobs: env: # Comma separated list without double quotes around the list. - REQUIRED_LIBRARIES: Servo,ServoEasing,Adafruit Motor Shield V2 Library,PlayRtttl,BlueDisplay + REQUIRED_LIBRARIES: Servo,ServoEasing,Adafruit Motor Shield V2 Library,PlayRtttl # ,BlueDisplay strategy: matrix: @@ -44,7 +44,7 @@ jobs: arduino-boards-fqbn: - arduino:avr:uno - arduino:avr:uno|full - - esp32:esp32:featheresp32:FlashFreq=80 + - esp32:esp32:esp32cam # Specify parameters for each board. # With sketches-exclude you may exclude specific examples for a board. Use a comma separated list. @@ -68,21 +68,15 @@ jobs: -DBREADBOARD_4WD_FULL_CONFIGURATION -DUS_SENSOR_SUPPORTS_1_PIN_MODE - - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 + - arduino-boards-fqbn: esp32:esp32:esp32cam platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json - required-libraries: ESP32Servo@0.13.0 # contains analogWrite.h - sketches-exclude: PrintMotorDiagram,PrintCarValuesWithIMU,RobotCarBasic,RobotCarBlueDisplay,SmartCarFollower,LineFollower # no Encoder support yet + required-libraries: ESP32Servo + sketches-exclude: PrintMotorDiagram,PrintCarValuesWithIMU,RobotCarBlueDisplay,LineFollower # no Encoder support yet, no sensor input build-properties: # the flags were put in compiler.cpp.extra_flags - All: -DCAR_IS_ESP32_CAM_BASED - MecanumWheelCar: -DDUMMY # undefine CAR_IS_ESP32_CAM_BASED - RobotCarBlueDisplay: - -DCAR_IS_ESP32_CAM_BASED - -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DBREADBOARD_4WD_FULL_CONFIGURATION - -DUS_SENSOR_SUPPORTS_1_PIN_MODE + All: -DCAR_IS_ESP32_CAM_BASED -MMD -c # see https://github.com/espressif/arduino-esp32/issues/8815 + MecanumWheelCar: -DDUMMY -MMD -c # this undefines CAR_IS_ESP32_CAM_BASED - # Do not cancel all jobs / architectures if one job fails - fail-fast: false +# fail-fast: false # false -> do not cancel all jobs / architectures if one job fails # This is the list of steps this job will run. steps: @@ -99,14 +93,14 @@ jobs: path: CustomBlueDisplay # must contain string "Custom" # No need to put "Custom" library in the required-libraries list - - name: Compile all examples using the arduino-test-compile action uses: ArminJo/arduino-test-compile@master with: arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} - arduino-platform: ${{ matrix.arduino-platform }} platform-url: ${{ matrix.platform-url }} required-libraries: ${{ env.REQUIRED_LIBRARIES }},${{ matrix.required-libraries }} sketches-exclude: ${{ matrix.sketches-exclude }} build-properties: ${{ toJson(matrix.build-properties) }} - extra-arduino-lib-install-args: "--no-deps" + extra-arduino-lib-install-args: "--no-deps" # suppress dependency resolving for libraries, here the "Adafruit Motor Shield V2 Library" +# debug-install: true + debug-compile: true \ No newline at end of file diff --git a/examples/Basic/Basic.ino b/examples/Basic/Basic.ino index e16c135..609569a 100644 --- a/examples/Basic/Basic.ino +++ b/examples/Basic/Basic.ino @@ -1,9 +1,9 @@ /* * Basic.cpp * - * Implements basic car movements. + * Implements forward and backward movement for each 300 ms. * - * Copyright (C) 2023 Armin Joachimsmeyer + * Copyright (C) 2023-2024 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. @@ -42,7 +42,7 @@ //#define L298_2WD_2LI_ION_VIN_IR_CONFIGURATION // L298_2WD_2LI_ION_BASIC + VIN voltage divider + IR distance //#define L298_2WD_2LI_ION_VIN_IR_IMU_CONFIGURATION // L298_2WD_2LI_ION_BASIC + VIN voltage divider + IR distance + MPU6050 #include "RobotCarConfigurations.h" // sets e.g. CAR_HAS_ENCODERS, USE_ADAFRUIT_MOTOR_SHIELD -#include "RobotCarPinDefinitionsAndMore.h" +#include "RobotCarPinDefinitionsAndMore.h" // Pinout depends on settings like CAR_HAS_ENCODERS etc. #include "PWMDcMotor.hpp" PWMDcMotor rightCarMotor; diff --git a/examples/Basic/RobotCarConfigurations.h b/examples/Basic/RobotCarConfigurations.h index b5b99ec..fc07c5d 100644 --- a/examples/Basic/RobotCarConfigurations.h +++ b/examples/Basic/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/Basic/RobotCarPinDefinitionsAndMore.h b/examples/Basic/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/Basic/RobotCarPinDefinitionsAndMore.h +++ b/examples/Basic/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/BasicDistance/ADCUtils.hpp b/examples/BasicDistance/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/examples/BasicDistance/ADCUtils.hpp +++ b/examples/BasicDistance/ADCUtils.hpp @@ -607,6 +607,8 @@ uint16_t getVoltageMillivoltWith_1_1VoltReference(uint8_t aADCChannelForVoltageM /* * Return true if sVCCVoltageMillivolt is > 4.3 V and < 4.95 V + * This does not really work for the UNO board, because it has no series Diode in the USB VCC + * and therefore a very low voltage drop. */ bool isVCCUSBPowered() { readVCCVoltageMillivolt(); diff --git a/examples/BasicDistance/RobotCarConfigurations.h b/examples/BasicDistance/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/BasicDistance/RobotCarConfigurations.h +++ b/examples/BasicDistance/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/BasicDistance/RobotCarPinDefinitionsAndMore.h b/examples/BasicDistance/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/BasicDistance/RobotCarPinDefinitionsAndMore.h +++ b/examples/BasicDistance/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/BasicIRControl/RobotCarConfigurations.h b/examples/BasicIRControl/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/BasicIRControl/RobotCarConfigurations.h +++ b/examples/BasicIRControl/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/BasicIRControl/RobotCarPinDefinitionsAndMore.h b/examples/BasicIRControl/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/BasicIRControl/RobotCarPinDefinitionsAndMore.h +++ b/examples/BasicIRControl/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/BasicIRControl/TinyIRReceiver.hpp b/examples/BasicIRControl/TinyIRReceiver.hpp index 39b3414..e651585 100644 --- a/examples/BasicIRControl/TinyIRReceiver.hpp +++ b/examples/BasicIRControl/TinyIRReceiver.hpp @@ -114,17 +114,18 @@ volatile TinyIRReceiverCallbackDataStruct TinyIRReceiverData; #warning "IR_INPUT_PIN is deprecated, use IR_RECEIVE_PIN" #define IR_RECEIVE_PIN IR_INPUT_PIN #endif + #if !defined(IR_RECEIVE_PIN) -#if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) +# if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 10" #define IR_RECEIVE_PIN 10 -#elif defined(__AVR_ATtiny816__) +# elif defined(__AVR_ATtiny816__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 14" #define IR_RECEIVE_PIN 14 -#else +# else #warning "IR_RECEIVE_PIN is not defined, so it is set to 2" #define IR_RECEIVE_PIN 2 -#endif +# endif #endif #if !defined(IR_FEEDBACK_LED_PIN) && defined(LED_BUILTIN) @@ -323,7 +324,7 @@ void IRPinChangeInterruptHandler(void) { * Check address parity * Address is sent first and contained in the lower word */ - if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[1])) { + if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[1])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; // here we can have the repeat flag already set #else @@ -336,7 +337,7 @@ void IRPinChangeInterruptHandler(void) { * Check command parity */ #if (TINY_RECEIVER_ADDRESS_BITS > 0) - if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[3])) { + if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[3])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; #else diff --git a/examples/BasicMecanum/RobotCarConfigurations.h b/examples/BasicMecanum/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/BasicMecanum/RobotCarConfigurations.h +++ b/examples/BasicMecanum/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/BasicMecanum/RobotCarPinDefinitionsAndMore.h b/examples/BasicMecanum/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/BasicMecanum/RobotCarPinDefinitionsAndMore.h +++ b/examples/BasicMecanum/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/IRDispatcherControl/RobotCarConfigurations.h b/examples/IRDispatcherControl/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/IRDispatcherControl/RobotCarConfigurations.h +++ b/examples/IRDispatcherControl/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/IRDispatcherControl/RobotCarPinDefinitionsAndMore.h b/examples/IRDispatcherControl/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/IRDispatcherControl/RobotCarPinDefinitionsAndMore.h +++ b/examples/IRDispatcherControl/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/IRDispatcherControl/RobotCarUtils.hpp b/examples/IRDispatcherControl/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/IRDispatcherControl/RobotCarUtils.hpp +++ b/examples/IRDispatcherControl/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/IRDispatcherControl/TinyIRReceiver.hpp b/examples/IRDispatcherControl/TinyIRReceiver.hpp index 39b3414..e651585 100644 --- a/examples/IRDispatcherControl/TinyIRReceiver.hpp +++ b/examples/IRDispatcherControl/TinyIRReceiver.hpp @@ -114,17 +114,18 @@ volatile TinyIRReceiverCallbackDataStruct TinyIRReceiverData; #warning "IR_INPUT_PIN is deprecated, use IR_RECEIVE_PIN" #define IR_RECEIVE_PIN IR_INPUT_PIN #endif + #if !defined(IR_RECEIVE_PIN) -#if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) +# if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 10" #define IR_RECEIVE_PIN 10 -#elif defined(__AVR_ATtiny816__) +# elif defined(__AVR_ATtiny816__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 14" #define IR_RECEIVE_PIN 14 -#else +# else #warning "IR_RECEIVE_PIN is not defined, so it is set to 2" #define IR_RECEIVE_PIN 2 -#endif +# endif #endif #if !defined(IR_FEEDBACK_LED_PIN) && defined(LED_BUILTIN) @@ -323,7 +324,7 @@ void IRPinChangeInterruptHandler(void) { * Check address parity * Address is sent first and contained in the lower word */ - if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[1])) { + if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[1])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; // here we can have the repeat flag already set #else @@ -336,7 +337,7 @@ void IRPinChangeInterruptHandler(void) { * Check command parity */ #if (TINY_RECEIVER_ADDRESS_BITS > 0) - if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[3])) { + if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[3])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; #else diff --git a/examples/LineFollower/RobotCarConfigurations.h b/examples/LineFollower/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/LineFollower/RobotCarConfigurations.h +++ b/examples/LineFollower/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/LineFollower/RobotCarPinDefinitionsAndMore.h b/examples/LineFollower/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/LineFollower/RobotCarPinDefinitionsAndMore.h +++ b/examples/LineFollower/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/LineFollower/RobotCarUtils.hpp b/examples/LineFollower/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/LineFollower/RobotCarUtils.hpp +++ b/examples/LineFollower/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/MecanumWheelCar/ADCUtils.hpp b/examples/MecanumWheelCar/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/examples/MecanumWheelCar/ADCUtils.hpp +++ b/examples/MecanumWheelCar/ADCUtils.hpp @@ -607,6 +607,8 @@ uint16_t getVoltageMillivoltWith_1_1VoltReference(uint8_t aADCChannelForVoltageM /* * Return true if sVCCVoltageMillivolt is > 4.3 V and < 4.95 V + * This does not really work for the UNO board, because it has no series Diode in the USB VCC + * and therefore a very low voltage drop. */ bool isVCCUSBPowered() { readVCCVoltageMillivolt(); diff --git a/examples/MecanumWheelCar/MecanumWheelCar.ino b/examples/MecanumWheelCar/MecanumWheelCar.ino index 0203272..6dcd6b8 100644 --- a/examples/MecanumWheelCar/MecanumWheelCar.ino +++ b/examples/MecanumWheelCar/MecanumWheelCar.ino @@ -51,9 +51,6 @@ void doTurnDemo(); void doSquareAndStar(); void setup() { -// initialize the digital pin as an output. - pinMode(LED_BUILTIN, OUTPUT); - Serial.begin(115200); #if defined(__AVR_ATmega32U4__) || defined(SERIAL_PORT_USBVIRTUAL) || defined(SERIAL_USB) /*stm32duino*/|| defined(USBCON) /*STM32_stm32*/ \ diff --git a/examples/MecanumWheelCar/RobotCarConfigurations.h b/examples/MecanumWheelCar/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/MecanumWheelCar/RobotCarConfigurations.h +++ b/examples/MecanumWheelCar/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/MecanumWheelCar/RobotCarPinDefinitionsAndMore.h b/examples/MecanumWheelCar/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/MecanumWheelCar/RobotCarPinDefinitionsAndMore.h +++ b/examples/MecanumWheelCar/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/MecanumWheelCar/RobotCarUtils.hpp b/examples/MecanumWheelCar/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/MecanumWheelCar/RobotCarUtils.hpp +++ b/examples/MecanumWheelCar/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/PrintCarValuesWithIMU/ADCUtils.hpp b/examples/PrintCarValuesWithIMU/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/examples/PrintCarValuesWithIMU/ADCUtils.hpp +++ b/examples/PrintCarValuesWithIMU/ADCUtils.hpp @@ -607,6 +607,8 @@ uint16_t getVoltageMillivoltWith_1_1VoltReference(uint8_t aADCChannelForVoltageM /* * Return true if sVCCVoltageMillivolt is > 4.3 V and < 4.95 V + * This does not really work for the UNO board, because it has no series Diode in the USB VCC + * and therefore a very low voltage drop. */ bool isVCCUSBPowered() { readVCCVoltageMillivolt(); diff --git a/examples/PrintCarValuesWithIMU/RobotCarPinDefinitionsAndMore.h b/examples/PrintCarValuesWithIMU/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/PrintCarValuesWithIMU/RobotCarPinDefinitionsAndMore.h +++ b/examples/PrintCarValuesWithIMU/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/PrintMotorDiagram/PrintMotorDiagram.ino b/examples/PrintMotorDiagram/PrintMotorDiagram.ino index 2f9aef2..d0a6e8f 100644 --- a/examples/PrintMotorDiagram/PrintMotorDiagram.ino +++ b/examples/PrintMotorDiagram/PrintMotorDiagram.ino @@ -56,9 +56,6 @@ EncoderMotor MotorUnderTest; //#define ENABLE_EXTRA_NON_PLOTTER_OUTPUT // Generate verbose output for SerialMonitor but this not compatible with Arduino Plotter void setup() { -// initialize the digital pin as an output. - pinMode(LED_BUILTIN, OUTPUT); - Serial.begin(115200); #if defined(__AVR_ATmega32U4__) || defined(SERIAL_PORT_USBVIRTUAL) || defined(SERIAL_USB) /*stm32duino*/|| defined(USBCON) /*STM32_stm32*/ \ diff --git a/examples/PrintMotorDiagram/RobotCarPinDefinitionsAndMore.h b/examples/PrintMotorDiagram/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/PrintMotorDiagram/RobotCarPinDefinitionsAndMore.h +++ b/examples/PrintMotorDiagram/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/RobotCarBasic/RobotCarBasic.ino b/examples/RobotCarBasic/RobotCarBasic.ino index e960e77..7a29c74 100644 --- a/examples/RobotCarBasic/RobotCarBasic.ino +++ b/examples/RobotCarBasic/RobotCarBasic.ino @@ -44,7 +44,6 @@ #include "RobotCarConfigurations.h" // sets e.g. CAR_HAS_ENCODERS, USE_ADAFRUIT_MOTOR_SHIELD #include "RobotCarPinDefinitionsAndMore.h" -#include "Servo.h" #include "Distance.hpp" #include "CarPWMMotorControl.hpp" #include "RobotCarUtils.hpp" diff --git a/examples/RobotCarBasic/RobotCarConfigurations.h b/examples/RobotCarBasic/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/RobotCarBasic/RobotCarConfigurations.h +++ b/examples/RobotCarBasic/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/RobotCarBasic/RobotCarPinDefinitionsAndMore.h b/examples/RobotCarBasic/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/RobotCarBasic/RobotCarPinDefinitionsAndMore.h +++ b/examples/RobotCarBasic/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/RobotCarBasic/RobotCarUtils.hpp b/examples/RobotCarBasic/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/RobotCarBasic/RobotCarUtils.hpp +++ b/examples/RobotCarBasic/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/RobotCarBlueDisplay/AutonomousDrive.hpp b/examples/RobotCarBlueDisplay/AutonomousDrive.hpp index 0a14f28..f49c867 100644 --- a/examples/RobotCarBlueDisplay/AutonomousDrive.hpp +++ b/examples/RobotCarBlueDisplay/AutonomousDrive.hpp @@ -288,7 +288,7 @@ void driveCollisonAvoidingOneStep() { #if !defined(ENABLE_USER_PROVIDED_COLLISION_DETECTION) if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - char tStringBuffer[6]; + char tStringBuffer[11]; sprintf_P(tStringBuffer, PSTR("%2d%s"), sCentimetersDrivenPerScan, "cm/scan"); BlueDisplay1.drawText(TEXT_SIZE_11_WIDTH, BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_HEIGHT - TEXT_SIZE_11_DECEND, tStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); diff --git a/examples/RobotCarBlueDisplay/AutonomousDrivePage.hpp b/examples/RobotCarBlueDisplay/AutonomousDrivePage.hpp index 9120dd5..a4226e7 100644 --- a/examples/RobotCarBlueDisplay/AutonomousDrivePage.hpp +++ b/examples/RobotCarBlueDisplay/AutonomousDrivePage.hpp @@ -309,9 +309,11 @@ void startAutonomousDrivePage(void) { SliderIROrTofDistance.setPosition(POS_X_DISTANCE_POSITION_SLIDER - (TEXT_SIZE_11_WIDTH + BUTTON_WIDTH_10), SLIDER_SHIFTED_Y_POS); #endif drawAutonomousDrivePage(); +#if defined(VIN_ATTENUATED_INPUT_PIN) if(!isPWMCalibrated) { calibrateDriveSpeedPWMAndPrint(); } +#endif } // currently not used diff --git a/examples/RobotCarBlueDisplay/LightweightServo.h b/examples/RobotCarBlueDisplay/LightweightServo.h index 5b27fa3..c4b1231 100644 --- a/examples/RobotCarBlueDisplay/LightweightServo.h +++ b/examples/RobotCarBlueDisplay/LightweightServo.h @@ -24,37 +24,59 @@ #ifndef _LIGHTWEIGHT_SERVO_H #define _LIGHTWEIGHT_SERVO_H -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) -#define VERSION_LIGHTWEIGHT_SERVO "1.1.0" -#define VERSION_LIGHTWEIGHT_SERVO_MAJOR 1 -#define VERSION_LIGHTWEIGHT_SERVO_MINOR 1 +#define VERSION_LIGHTWEIGHT_SERVO "2.0.0" +#define VERSION_LIGHTWEIGHT_SERVO_MAJOR 2 +#define VERSION_LIGHTWEIGHT_SERVO_MINOR 0 #include /* - * Activating this saves 70 bytes program space. You must then use the init functions initLightweightServoPin*() manually. + * Activating this saves 40 bytes program space. You must then use the init functions initLightweightServoPin*() manually. */ //#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE -#define ISR1_COUNT_FOR_20_MILLIS (F_CPU / (8 * 50)) // 40000 For 50 Hz, 20 ms using a prescaler of 8. You can modify this if you have servos which accept a higher rate -#define ISR1_COUNT_FOR_2_5_MILLIS (F_CPU / (8 * 400)) // 5000 For 400 Hz, 2.5 ms using a prescaler of 8. +// +#define ISR_COUNT_FOR_20_MILLIS (F_CPU / (8 * 50)) // 40000 For 50 Hz, 20 ms using a prescaler of 8. You can modify this if you have servos which accept a higher rate +#define ISR_COUNT_FOR_2_5_MILLIS (F_CPU / (8 * 400)) // 5000 For 400 Hz, 2.5 ms using a prescaler of 8. + +#if defined(__AVR_ATmega2560__) +#define LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN 46 +#define LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN 45 +#define LEIGHTWEIGHT_SERVO_CHANNEL_C_PIN 44 +#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) +#define LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN 9 +#define LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN 10 +#endif + +void initLightweightServoPins(); // currently only pin 44 = OC5C/PL5, 45 = OC5B/PL4 + 46 = OC5A/PL3 on 2560 and pin 9 = OC1A + 10 = OC1B on 328 +void checkAndInitLightweightServoPin(uint8_t aPin); +void deinitLightweightServoPin(uint8_t aPin); // Set pin to input and disable non-inverting Compare Output mode + +int writeLightweightServoPin(int aDegree, uint8_t aPin, bool aUpdateFast = false); +void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast = false); + +void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int a180DegreeValue); +void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds); + +// convenience functions +int DegreeToMicrosecondsLightweightServo(int aDegree); +int MicrosecondsToDegreeLightweightServo(int aMicroseconds); + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) +#define ISR1_COUNT_FOR_20_MILLIS ISR_COUNT_FOR_20_MILLIS +#define ISR1_COUNT_FOR_2_5_MILLIS ISR_COUNT_FOR_2_5_MILLIS /* - * Lightweight servo library + * Old and fast ATmega328 functions * Uses timer1 and Pin 9 + 10 as output */ -void initLightweightServoPin9And10(); +void initLightweightServoPin9And10() __attribute__ ((deprecated ("Renamed to initLightweightServoPins()"))); // deprecated void initLightweightServoPin9(); // Disables Pin 10! void initLightweightServoPin10(); // Disables Pin 9! void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10); void deinitLightweightServoPin9_10(bool aUsePin9, bool aUsePin10); -void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int a180DegreeValue); -void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds); - -int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast = false); -void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast = false); - void write9(int aDegree, bool aUpdateFast = false); // setLightweightServoPulsePin9 Channel A void writeMicroseconds9(int aMicroseconds, bool aUpdateFast = false); void writeMicroseconds9Direct(int aMicroseconds); @@ -63,13 +85,17 @@ void write10(int aDegree, bool aUpdateFast = false); // setLightweightServoPulse void writeMicroseconds10(int aMicroseconds, bool aUpdateFast = false); void writeMicroseconds10Direct(int aMicroseconds); -// convenience functions -int DegreeToMicrosecondsLightweightServo(int aDegree); -int MicrosecondsToDegreeLightweightServo(int aMicroseconds); +int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast = false); +void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast = false); +#endif // Old and fast ATmega328 functions -#endif // AVR_ATmega328 +#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) /* + * Version 2.0.0 - 10/2024 + * - Improved API. + * - Support for ATmega2560. + * * Version 1.1.0 - 11/2020 * - Improved API. */ diff --git a/examples/RobotCarBlueDisplay/LightweightServo.hpp b/examples/RobotCarBlueDisplay/LightweightServo.hpp index 0da2967..1362790 100644 --- a/examples/RobotCarBlueDisplay/LightweightServo.hpp +++ b/examples/RobotCarBlueDisplay/LightweightServo.hpp @@ -29,9 +29,15 @@ #ifndef _LIGHTWEIGHT_SERVO_HPP #define _LIGHTWEIGHT_SERVO_HPP -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) #include "LightweightServo.h" +#if defined(DEBUG) +#define LOCAL_DEBUG +#else +//#define LOCAL_DEBUG // This enables debug output only for this file +#endif + /* * Variables to enable adjustment for different servo types * 544 and 2400 are values compatible with standard arduino values @@ -40,22 +46,17 @@ int sMicrosecondsForServo0Degree = 544; int sMicrosecondsForServo180Degree = 2400; +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) /* * Use 16 bit timer1 for generating 2 servo signals entirely by hardware without any interrupts. * Use FastPWM mode and generate pulse at start of the 20 ms period * The 2 servo signals are tied to pin 9 and 10 of an 328. * Attention - both pins are set to OUTPUT here! * 32 bytes code size + * Supports pin 9 = OC1A + 10 = OC1B */ void initLightweightServoPin9And10() { - /* - * Periods below 20 ms gives problems with long signals i.e. the positioning is not possible - */ - DDRB |= _BV(DDB1) | _BV(DDB2); // set pins OC1A = PortB1 -> PIN 9 and OC1B = PortB2 -> PIN 10 to output direction - TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 - non-inverting Compare Output mode OC1A+OC1B - TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM mode mode bits WGM13 + WGM12 - other available prescaler are 1 and 64 :-( - ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms - // do not set counter here, since with counter = 0 (default) no output signal is generated. + initLightweightServoPins(); } /* @@ -74,7 +75,7 @@ void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction tNewTCCR1A |= _BV(COM1A1); // non-inverting Compare Output mode OC1A OCR1A = UINT16_MAX; // Set counter > ICR1 here, to avoid output signal generation. - } + } if (aUsePin10) { DDRB |= _BV(DDB2); // set OC1B = PortB2 -> PIN 10 to output direction tNewTCCR1A |= _BV(COM1B1); // non-inverting Compare Output mode OC1B @@ -82,7 +83,7 @@ void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { } TCCR1A = tNewTCCR1A; TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 - ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + ICR1 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms } /* @@ -92,7 +93,7 @@ void initLightweightServoPin9() { DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction TCCR1A = _BV(WGM11) | _BV(COM1A1); // FastPWM Mode mode TOP (20 ms) determined by ICR1, non-inverting Compare Output mode OC1A TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 - ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + ICR1 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms OCR1A = UINT16_MAX; // Set counter > ICR1 here, to avoid output signal generation. } /* @@ -102,7 +103,7 @@ void initLightweightServoPin10() { DDRB |= _BV(DDB2); // set OC1B = PortB2 -> PIN 10 to output direction TCCR1A = _BV(WGM11) | _BV(COM1B1); // FastPWM Mode mode TOP (20 ms) determined by ICR1, non-inverting Compare Output mode OC1B TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 - ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + ICR1 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms OCR1B = UINT16_MAX; // Set counter > ICR1 here, to avoid output signal generation. } @@ -147,7 +148,7 @@ void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aU #endif if (aUpdateFast) { uint16_t tTimerCount = TCNT1; - if (tTimerCount > ISR1_COUNT_FOR_2_5_MILLIS) { + if (tTimerCount > ISR_COUNT_FOR_2_5_MILLIS) { // more than 2.5 ms since last pulse -> start a new one TCNT1 = ICR1 - 1; } @@ -159,21 +160,6 @@ void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aU } } -/* - * Sets the period of the servo pulses. Reasonable values are 2500 to 20000 microseconds. - * No parameter checking is done here! - */ -void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds) { - ICR1 = aRefreshPeriodMicroseconds * 2; -} -/* - * Set the mapping pulse width values for 0 and 180 degree - */ -void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree) { - sMicrosecondsForServo0Degree = aMicrosecondsForServo0Degree; - sMicrosecondsForServo180Degree = aMicrosecondsForServo180Degree; -} - /* * Pin 9 / Channel A. If value is below 180 then assume degree, otherwise assume microseconds */ @@ -219,6 +205,205 @@ void writeMicroseconds10Direct(int aMicroseconds) { #endif OCR1B = aMicroseconds; } +#endif + +/* + * Use 16 bit timer1 for generating 2 servo signals entirely by hardware without any interrupts. + * Use FastPWM mode and generate pulse at start of the 20 ms period + * Attention - both pins are set to OUTPUT here! + * 32 bytes code size + * Assume, that PRR1 PRTIM5 bit is low, which enables timer 5 + * Currently only pin 44 = OC5C/PL5, 45 = OC5B/PL4 + 46 = OC5A/PL3 on 2560 and pin 9 = OC1A + 10 = OC1B on 328 + */ +void initLightweightServoPins() { +#if defined(__AVR_ATmega2560__) + /* + * Periods below 20 ms gives problems with long signals i.e. the positioning is not possible + */ + DDRL |= _BV(DDL3) | _BV(DDL4) | _BV(DDL5); // Required! Set pins pin 44 = OC5C/PL5, 45 = OC5B/PL4 + 46 = OC5A/PL3 to output direction + TCCR5A = _BV(COM5A1) | _BV(COM5B1) | _BV(COM5C1) | _BV(WGM51); // FastPWM Mode mode TOP (20 ms) determined by ICR1 - non-inverting Compare Output mode OC1A+OC1B + TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51); // set prescaler to 8, FastPWM mode mode bits WGM53 + WGM52 - other available prescaler are 1 and 64 :-( + ICR5 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms +#else + DDRB |= _BV(DDB1) | _BV(DDB2); // Required! Set pins OC1A = PortB1 -> PIN 9 and OC1B = PortB2 -> PIN 10 to output direction + TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 - non-inverting Compare Output mode OC1A+OC1B + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM mode mode bits WGM13 + WGM12 - other available prescaler are 1 and 64 :-( + ICR1 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms +#endif + // do not set counter here, since with counter = 0 (default) no output signal is generated. +} + +/* + * Use 16 bit timer for generating 2 servo signals entirely by hardware without any interrupts. + * Use FastPWM mode and generate pulse at start of the 20 ms period + * Attention - the selected pin is set to OUTPUT here! + * 54 bytes code size + * Set pin to output (required) and enable non-inverting Compare Output mode + * Set output compare > ICRx, to avoid output signal generation. + */ +void checkAndInitLightweightServoPin(uint8_t aPin) { + bool tPinWasNotInitialized = false; + // Mask all other bits to zero! +#if defined(__AVR_ATmega2560__) + uint8_t tNewTCCR5A = TCCR5A & (_BV(COM5A1) | _BV(COM5B1) | _BV(COM5C1) | _BV(WGM51)); + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR5A & (_BV(COM5A1)))) { + OCR5A = UINT16_MAX; + DDRL |= (_BV(DDL3)); + tNewTCCR5A |= (_BV(COM5A1)) | _BV(WGM51); // FastPWM Mode mode TOP (20 ms) determined by ICR + tPinWasNotInitialized = true; + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR5A & (_BV(COM5B1)))) { + OCR5B = UINT16_MAX; + DDRL |= (_BV(DDL4)); + tNewTCCR5A |= (_BV(COM5B1)) | _BV(WGM51); + tPinWasNotInitialized = true; + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_C_PIN && !(TCCR5A & (_BV(COM5C1)))) { + OCR5C = UINT16_MAX; + DDRL |= (_BV(DDL5)); + tNewTCCR5A |= (_BV(COM5C1)) | _BV(WGM51); + tPinWasNotInitialized = true; + } +#else + uint8_t tNewTCCR1A = TCCR1A & (_BV(COM1A1) | _BV(COM1B1)); // keep existing COM1A1 and COM1B1 settings + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR1A & (_BV(COM1A1)))) { + OCR1A = UINT16_MAX; + DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction + tNewTCCR1A |= (_BV(COM1A1)) | _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 + tPinWasNotInitialized = true; + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR1A & (_BV(COM1B1)))) { + OCR1B = UINT16_MAX; + DDRB |= _BV(DDB2); // set OC1B = PortB2 -> PIN 10 to output direction + tNewTCCR1A |= (_BV(COM1B1)) | _BV(WGM11); + tPinWasNotInitialized = true; + } +#endif + + if (tPinWasNotInitialized) { +#if defined(LOCAL_DEBUG) + Serial.print(F("Auto initialize pin ")); + Serial.print(aPin); + Serial.print(F(" TCCR5A=0x")); + Serial.println(tNewTCCR5A,HEX); +#endif + /* + * Initialize timer with constant values + */ +#if defined(__AVR_ATmega2560__) + TCCR5A = tNewTCCR5A; + TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51); // set prescaler to 8, FastPWM mode mode bits WGM53 + WGM52 - other available prescaler are 1 and 64 :-( + /* + * Periods below 20 ms gives problems with long signals i.e. the positioning is not possible + */ + ICR5 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms + // do not set counter here, since with counter = 0 (default) no output signal is generated. +#else + TCCR1A = tNewTCCR1A; + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 + ICR1 = ISR_COUNT_FOR_20_MILLIS; // set period to 20 ms +#endif + } +} + +/* + * Set pin to input and disable non-inverting Compare Output mode + */ +void deinitLightweightServoPin(uint8_t aPin) { +#if defined(__AVR_ATmega2560__) + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + DDRL &= ~(_BV(DDL3)); + TCCR5A &= ~(_BV(COM5A1)); + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) { + DDRL &= ~(_BV(DDL4)); + TCCR5A &= ~(_BV(COM5B1)); + } else { + DDRL &= ~(_BV(DDL5)); + TCCR5A &= ~(_BV(COM5C1)); + } +#else + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + DDRB &= ~(_BV(DDB1)); // set OC1A = PortB1 -> PIN 9 to input direction + TCCR1A &= ~(_BV(COM1A1)); // disable non-inverting Compare Output mode OC1A + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) { + DDRB &= ~(_BV(DDB2)); // set OC1B = PortB2 -> PIN 10 to input direction + TCCR1A &= ~(_BV(COM1B1)); // disable non-inverting Compare Output mode OC1B + } +#endif +} + +/* + * @param aDegree - If value is below 180 then assume degree, otherwise assume microseconds + * @param aUpdateFast - If true, enable starting a new output pulse if more than 5 ms since last one, some servo might react faster in this mode. + * @param aUsePin9 - If false, then Pin10 is used + * 236 / 186(without auto init) bytes code size + */ +int writeLightweightServoPin(int aDegree, uint8_t aPin, bool aUpdateFast) { + if (aDegree <= 180) { + aDegree = DegreeToMicrosecondsLightweightServo(aDegree); + } + writeMicrosecondsLightweightServoPin(aDegree, aPin, aUpdateFast); + return aDegree; +} + +void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast) { +#if defined(LOCAL_DEBUG) + Serial.print(F(" Micros=")); // trailing space required if called by _writeMicrosecondsOrUnits() + Serial.print(aMicroseconds); + Serial.print(F(" pin=")); + Serial.println(aPin); +#endif +#if !defined(DISABLE_SERVO_TIMER_AUTO_INITIALIZE) + checkAndInitLightweightServoPin(aPin); +#endif + // since the resolution is 1/2 of microsecond for 16 MHz CPU clock and prescaler of 8 +#if (F_CPU == 16000000L) + aMicroseconds *= 2; +#elif (F_CPU < 8000000L) // for 8 MHz resolution is exactly 1 microsecond :-) + aMicroseconds /= (8000000L / F_CPU); +#endif +#if defined(__AVR_ATmega2560__) + if (aUpdateFast) { + uint16_t tTimerCount = TCNT5; + if (tTimerCount > ISR_COUNT_FOR_2_5_MILLIS) { + // more than 2.5 ms since last pulse -> start a new one + TCNT5 = ICR5 - 1; + } + } + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + OCR5A = aMicroseconds; + } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) { + OCR5B = aMicroseconds; + } else { + OCR5C = aMicroseconds; + } +#else + if (aUpdateFast) { + uint16_t tTimerCount = TCNT1; + if (tTimerCount > ISR_COUNT_FOR_2_5_MILLIS) { + // more than 2.5 ms since last pulse -> start a new one + TCNT1 = ICR1 - 1; + } + } + if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + OCR1A = aMicroseconds; + } else { + OCR1B = aMicroseconds; + } +#endif +} + +/* + * Sets the period of the servo pulses. Reasonable values are 2500 to 20000 microseconds. + * No parameter checking is done here! + */ +void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds) { + ICR1 = aRefreshPeriodMicroseconds * 2; +} +/* + * Set the mapping pulse width values for 0 and 180 degree + */ +void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree) { + sMicrosecondsForServo0Degree = aMicrosecondsForServo0Degree; + sMicrosecondsForServo180Degree = aMicrosecondsForServo180Degree; +} /* * Conversion functions @@ -231,5 +416,8 @@ int MicrosecondsToDegreeLightweightServo(int aMicroseconds) { return map(aMicroseconds, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree, 0, 180); } -#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +#if defined(LOCAL_DEBUG) +#undef LOCAL_DEBUG +#endif +#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega2560__) #endif // _LIGHTWEIGHT_SERVO_HPP diff --git a/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino b/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino index 4352742..42b24dd 100644 --- a/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino +++ b/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino @@ -11,7 +11,7 @@ * Define ENABLE_USER_PROVIDED_COLLISION_DETECTION and overwrite the 2 functions myOwnFillForwardDistancesInfo() * and doUserCollisionAvoiding() to test your own skill. * - * If Bluetooth is not connected, after TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS (10 seconds) the car starts demo mode. + * If Bluetooth is not connected, after TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS (30 seconds) the car starts demo mode. * After power up it runs in follower mode and after reset it runs in autonomous drive mode. * * Program size of GUI is 63 percent of 32kByte. @@ -32,16 +32,15 @@ #include -#define VERSION_EXAMPLE "2.0.1" +#define VERSION_EXAMPLE "2.1.0" //#define DEBUG //#define TRACE /* * Timeouts for demo mode and inactivity remainder */ -#define TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS 240000L // move Servo after 4 Minutes of inactivity -#define TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS 30000 // Start demo mode 30 seconds after boot up -#define TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS 30000 // Start demo mode 30 seconds after boot up +#define ATTENTION_AFTER_LAST_BD_COMMAND_MILLIS 240000L // move Servo after 4 Minutes of inactivity +#define TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS 30000 // Start demo mode 30 seconds after boot up /* * Car configuration @@ -102,7 +101,9 @@ Servo TiltServo; #else #define ENABLE_SERIAL_OUTPUT // To avoid the double negation !defined(NO_SERIAL_OUTPUT) #endif +#if defined(VIN_ATTENUATED_INPUT_PIN) #define MONITOR_VIN_VOLTAGE // Enable monitoring of VIN voltage for exact movements, if available. Check at startup. +#endif #if !defined(ADC_INTERNAL_REFERENCE_MILLIVOLT) && (defined(MONITOR_VIN_VOLTAGE) || defined(CAR_HAS_IR_DISTANCE_SENSOR)) // Must be defined before #include "BlueDisplay.hpp" #define ADC_INTERNAL_REFERENCE_MILLIVOLT 1100L // Change to value measured at the AREF pin. If value > real AREF voltage, measured values are > real values @@ -148,6 +149,7 @@ int doUserCollisionAvoiding(); #include "CarPWMMotorControl.hpp" // after BlueDisplay.hpp +#define VOLTAGE_USB_POWERED_UPPER_THRESHOLD_MILLIVOLT 4975 // Because Uno boards lack the series diode and have a low voltage drop #include "RobotCarUtils.hpp" // after BlueDisplay.hpp #if defined(USE_MPU6050_IMU) @@ -223,8 +225,10 @@ void setup() { * Configure first set of pins */ // initialize the digital pin as an output. +#if defined(LED_BUILTIN) pinMode(LED_BUILTIN, OUTPUT); digitalWrite(LED_BUILTIN, LOW); // on my Uno R3 the LED is on otherwise +#endif #if defined(CAR_HAS_LASER) && (LASER_OUT_PIN != LED_BUILTIN) pinMode(LASER_OUT_PIN, OUTPUT); #endif @@ -371,7 +375,9 @@ void loop() { // first get EEPROM values, in order to not work with the values we accidently set before in a former calibration RobotCar.readCarValuesFromEeprom(); displayRotationValues(); +#if defined(VIN_ATTENUATED_INPUT_PIN) calibrateDriveSpeedPWMAndPrint(); +#endif # if !defined(USE_MPU6050_IMU) && (defined(CAR_HAS_4_WHEELS) || defined(CAR_HAS_4_MECANUM_WHEELS) || !defined(USE_ENCODER_MOTOR_CONTROL)) if (!delayMillisAndCheckForStop(3000)) { // time to rearrange car calibrateRotation(); @@ -391,16 +397,19 @@ void loop() { * After 30 seconds of being disconnected, run the demo. * Do not run it if the car is connected to USB (e.g. for programming or debugging), which can be tested only for a Li-ion supply :-(. */ - if (!sTimeoutDemoDisable && (millis() > TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS)) { + if (!sTimeoutDemoDisable && (millis() > TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS)) { sTimeoutDemoDisable = true; +#if defined(ADC_UTILS_ARE_AVAILABLE) if (isVCCUSBPowered()) { -#if defined(ENABLE_SERIAL_OUTPUT) +# if defined(ENABLE_SERIAL_OUTPUT) Serial.print(F("Timeout and USB powered with ")); Serial.print(sVCCVoltageMillivolt); Serial.println(F(" mV -> skip follower demo")); +# endif + } else #endif - } else { + { /* * Timeout just reached and not USB powered, play melody and start autonomous drive */ @@ -431,8 +440,8 @@ void loop() { /* * After 4 minutes of user inactivity, make noise by scanning with US Servo and repeat it every 2. minute */ - if (BlueDisplay1.isConnectionEstablished() && sMillisOfLastReceivedBDEvent + TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS < millis()) { - sMillisOfLastReceivedBDEvent = millis() - (TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS / 2); // adjust sMillisOfLastReceivedBDEvent to have the next scan in 2 minutes + if (BlueDisplay1.isConnectionEstablished() && sMillisOfLastReceivedBDEvent + ATTENTION_AFTER_LAST_BD_COMMAND_MILLIS < millis()) { + sMillisOfLastReceivedBDEvent = millis() - (ATTENTION_AFTER_LAST_BD_COMMAND_MILLIS / 2); // adjust sMillisOfLastReceivedBDEvent to have the next scan in 2 minutes #if defined(CAR_HAS_DISTANCE_SERVO) fillAndShowForwardDistancesInfo(true, true); # if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) diff --git a/examples/RobotCarBlueDisplay/RobotCarCommonGui.hpp b/examples/RobotCarBlueDisplay/RobotCarCommonGui.hpp index 5577983..cec2055 100644 --- a/examples/RobotCarBlueDisplay/RobotCarCommonGui.hpp +++ b/examples/RobotCarBlueDisplay/RobotCarCommonGui.hpp @@ -284,7 +284,7 @@ void displayRotationValues() { #endif void doCalibrate(BDButton *aTheTouchedButton, int16_t aValue) { -#if defined(USE_MPU6050_IMU) +#if defined(USE_MPU6050_IMU) && defined(VIN_ATTENUATED_INPUT_PIN) calibrateDriveSpeedPWMAndPrint(); // Calibrate only drive speed PWM #else doCalibration = true; // set flag for main loop @@ -364,8 +364,10 @@ void startCurrentPage() { startHomePage(); break; } +#if defined(MONITOR_VIN_VOLTAGE) forceDisplayOfVin(); readAndPrintVin(); +#endif } /* diff --git a/examples/RobotCarBlueDisplay/RobotCarConfigurations.h b/examples/RobotCarBlueDisplay/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/RobotCarBlueDisplay/RobotCarConfigurations.h +++ b/examples/RobotCarBlueDisplay/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/RobotCarBlueDisplay/RobotCarPinDefinitionsAndMore.h b/examples/RobotCarBlueDisplay/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/RobotCarBlueDisplay/RobotCarPinDefinitionsAndMore.h +++ b/examples/RobotCarBlueDisplay/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/RobotCarBlueDisplay/RobotCarTestPage.hpp b/examples/RobotCarBlueDisplay/RobotCarTestPage.hpp index 28c4fe1..527ba3d 100644 --- a/examples/RobotCarBlueDisplay/RobotCarTestPage.hpp +++ b/examples/RobotCarBlueDisplay/RobotCarTestPage.hpp @@ -239,9 +239,11 @@ void drawTestPage(void) { void startTestPage(void) { doReset(NULL, 0); drawTestPage(); +#if defined(VIN_ATTENUATED_INPUT_PIN) if (!isPWMCalibrated) { calibrateDriveSpeedPWMAndPrint(); } +#endif } void loopTestPage(void) { diff --git a/examples/RobotCarBlueDisplay/RobotCarUtils.hpp b/examples/RobotCarBlueDisplay/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/RobotCarBlueDisplay/RobotCarUtils.hpp +++ b/examples/RobotCarBlueDisplay/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/SmartCarFollower/ADCUtils.hpp b/examples/SmartCarFollower/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/examples/SmartCarFollower/ADCUtils.hpp +++ b/examples/SmartCarFollower/ADCUtils.hpp @@ -607,6 +607,8 @@ uint16_t getVoltageMillivoltWith_1_1VoltReference(uint8_t aADCChannelForVoltageM /* * Return true if sVCCVoltageMillivolt is > 4.3 V and < 4.95 V + * This does not really work for the UNO board, because it has no series Diode in the USB VCC + * and therefore a very low voltage drop. */ bool isVCCUSBPowered() { readVCCVoltageMillivolt(); diff --git a/examples/SmartCarFollower/RobotCarConfigurations.h b/examples/SmartCarFollower/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/SmartCarFollower/RobotCarConfigurations.h +++ b/examples/SmartCarFollower/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/SmartCarFollower/RobotCarPinDefinitionsAndMore.h b/examples/SmartCarFollower/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/SmartCarFollower/RobotCarPinDefinitionsAndMore.h +++ b/examples/SmartCarFollower/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/SmartCarFollower/RobotCarUtils.hpp b/examples/SmartCarFollower/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/SmartCarFollower/RobotCarUtils.hpp +++ b/examples/SmartCarFollower/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/SmartCarFollower/SmartCarFollower.ino b/examples/SmartCarFollower/SmartCarFollower.ino index e084374..6a152e0 100644 --- a/examples/SmartCarFollower/SmartCarFollower.ino +++ b/examples/SmartCarFollower/SmartCarFollower.ino @@ -129,7 +129,6 @@ bool sVINProvided = false; * 5 CurrentCompensatedSpeedPWM=0 DriveSpeedPWM=53 DriveSpeedPWMFor2Volt=64 SpeedPWMCompensation=0 CurrentDirection=S */ #define LOCAL_INFO // Enable info just for IRCommandDispatcher to show "A=0x0 C=0x1D - Received IR data" and "Run non blocking command: default speed - Called car command" -#include "IRCommandDispatcher.hpp" #include "IRCommandDispatcher.hpp" // must be before #include "RobotCarUtils.hpp" bool sIRReceiverIsAttached = false; #else @@ -208,7 +207,7 @@ void setup() { /* * Detect USB connection and signal end of boot */ -#if !defined(ESP32) +#if defined(ADC_UTILS_ARE_AVAILABLE) # if defined(VIN_ATTENUATED_INPUT_PIN) sVINProvided = isVINProvided(); Serial.println(); diff --git a/examples/SmartCarFollower/TinyIRReceiver.hpp b/examples/SmartCarFollower/TinyIRReceiver.hpp index 39b3414..e651585 100644 --- a/examples/SmartCarFollower/TinyIRReceiver.hpp +++ b/examples/SmartCarFollower/TinyIRReceiver.hpp @@ -114,17 +114,18 @@ volatile TinyIRReceiverCallbackDataStruct TinyIRReceiverData; #warning "IR_INPUT_PIN is deprecated, use IR_RECEIVE_PIN" #define IR_RECEIVE_PIN IR_INPUT_PIN #endif + #if !defined(IR_RECEIVE_PIN) -#if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) +# if defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 10" #define IR_RECEIVE_PIN 10 -#elif defined(__AVR_ATtiny816__) +# elif defined(__AVR_ATtiny816__) #warning "IR_RECEIVE_PIN is not defined, so it is set to 14" #define IR_RECEIVE_PIN 14 -#else +# else #warning "IR_RECEIVE_PIN is not defined, so it is set to 2" #define IR_RECEIVE_PIN 2 -#endif +# endif #endif #if !defined(IR_FEEDBACK_LED_PIN) && defined(LED_BUILTIN) @@ -323,7 +324,7 @@ void IRPinChangeInterruptHandler(void) { * Check address parity * Address is sent first and contained in the lower word */ - if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[1])) { + if (TinyIRReceiverControl.IRRawData.UBytes[0] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[1])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; // here we can have the repeat flag already set #else @@ -336,7 +337,7 @@ void IRPinChangeInterruptHandler(void) { * Check command parity */ #if (TINY_RECEIVER_ADDRESS_BITS > 0) - if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t) (~TinyIRReceiverControl.IRRawData.UBytes[3])) { + if (TinyIRReceiverControl.IRRawData.UBytes[2] != (uint8_t)(~TinyIRReceiverControl.IRRawData.UBytes[3])) { #if defined(ENABLE_NEC2_REPEATS) TinyIRReceiverControl.Flags |= IRDATA_FLAGS_PARITY_FAILED; #else diff --git a/examples/SmartCarFollowerSimple/ADCUtils.hpp b/examples/SmartCarFollowerSimple/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/examples/SmartCarFollowerSimple/ADCUtils.hpp +++ b/examples/SmartCarFollowerSimple/ADCUtils.hpp @@ -607,6 +607,8 @@ uint16_t getVoltageMillivoltWith_1_1VoltReference(uint8_t aADCChannelForVoltageM /* * Return true if sVCCVoltageMillivolt is > 4.3 V and < 4.95 V + * This does not really work for the UNO board, because it has no series Diode in the USB VCC + * and therefore a very low voltage drop. */ bool isVCCUSBPowered() { readVCCVoltageMillivolt(); diff --git a/examples/SmartCarFollowerSimple/RobotCarConfigurations.h b/examples/SmartCarFollowerSimple/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/examples/SmartCarFollowerSimple/RobotCarConfigurations.h +++ b/examples/SmartCarFollowerSimple/RobotCarConfigurations.h @@ -317,6 +317,9 @@ * BASIC CONFIGURATION for ESP32-Cam car * Car controlled by an ESP32-Cam module */ +#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack +#define CAR_IS_ESP32_CAM_BASED +#endif #if defined(CAR_IS_ESP32_CAM_BASED) #define CAR_HAS_US_DISTANCE_SENSOR // A HC-SR04 ultrasonic distance sensor is mounted (default for most China smart cars) #define CAR_HAS_DISTANCE_SERVO // Distance sensor is mounted on a pan servo (default for most China smart cars) diff --git a/examples/SmartCarFollowerSimple/RobotCarPinDefinitionsAndMore.h b/examples/SmartCarFollowerSimple/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/SmartCarFollowerSimple/RobotCarPinDefinitionsAndMore.h +++ b/examples/SmartCarFollowerSimple/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/SmartCarFollowerSimple/RobotCarUtils.hpp b/examples/SmartCarFollowerSimple/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/examples/SmartCarFollowerSimple/RobotCarUtils.hpp +++ b/examples/SmartCarFollowerSimple/RobotCarUtils.hpp @@ -46,7 +46,7 @@ * ENABLE_RTTTL_FOR_CAR * VIN_VOLTAGE_CORRECTION * ADC_INTERNAL_REFERENCE_MILLIVOLT - * TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + * TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS * FOLLOWER_DISTANCE_MINIMUM_CENTIMETER * FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER * US_DISTANCE_SENSOR_ENABLE_PIN @@ -149,7 +149,11 @@ void printProgramOptions(Print *aSerial) { aSerial->println(); #if defined(USE_BLUE_DISPLAY_GUI) - aSerial->println(F("If not powered by USB, run follower demo after " STR(TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms")); +# if defined(ADC_UTILS_ARE_AVAILABLE) + aSerial->print( + F("If not powered by USB, run follower demo after " STR(TIMEOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) " ms. USBpowered=")); + aSerial->println(isVCCUSBPowered()); +# endif #endif aSerial->println( @@ -178,7 +182,7 @@ void initRobotCarPWMMotorControl() { BACK_LEFT_MOTOR_FORWARD_PIN, BACK_LEFT_MOTOR_BACKWARD_PIN); #else RobotCar.init(RIGHT_MOTOR_FORWARD_PIN, RIGHT_MOTOR_BACKWARD_PIN, RIGHT_MOTOR_PWM_PIN, LEFT_MOTOR_FORWARD_PIN, - LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); + LEFT_MOTOR_BACKWARD_PIN, LEFT_MOTOR_PWM_PIN); #endif } @@ -346,7 +350,7 @@ void calibrateDriveSpeedPWMAndPrint() { RobotCar.stop(); #if defined(USE_BLUE_DISPLAY_GUI) - isPWMCalibrated = true; + isPWMCalibrated = true; #endif } #endif // #if defined(VIN_ATTENUATED_INPUT_PIN) @@ -477,9 +481,7 @@ bool calibrateRotation(turn_direction_t aTurnDirection) { void testDriveTwoTurnsBothDirections() { #define NUMBER_OF_TEST_DRIVES 2 #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x a full turn i.e. ")); - Serial.print(DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x a full turn i.e. " STR(DEFAULT_CIRCUMFERENCE_MILLIMETER) " mm, both directions")); #endif for (int i = 0; i < NUMBER_OF_TEST_DRIVES; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER); @@ -500,9 +502,7 @@ void testDriveTwoTurnsBothDirections() { void testDriveTwoTurnsIn5PartsBothDirections() { uint8_t tDirection = DIRECTION_FORWARD; #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space - Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. ")); - Serial.print(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER); - Serial.println(F(" mm, both directions")); + Serial.print(F("Move the wheels 2x 1/8 + 1/4 + 1/2 + 1 turn i.e. " STR(2 * DEFAULT_CIRCUMFERENCE_MILLIMETER)" mm, both directions")); #endif for (int i = 0; i < 2; ++i) { RobotCar.goDistanceMillimeter(DEFAULT_CIRCUMFERENCE_MILLIMETER / 8, tDirection); diff --git a/examples/Square/RobotCarPinDefinitionsAndMore.h b/examples/Square/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/Square/RobotCarPinDefinitionsAndMore.h +++ b/examples/Square/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/examples/Square/Square.ino b/examples/Square/Square.ino index 12f1484..79d7359 100644 --- a/examples/Square/Square.ino +++ b/examples/Square/Square.ino @@ -53,9 +53,6 @@ #define SIZE_OF_SQUARE_MILLIMETER 400 void setup() { -// initialize the digital pin as an output. - pinMode(LED_BUILTIN, OUTPUT); - Serial.begin(115200); #if defined(__AVR_ATmega32U4__) || defined(SERIAL_PORT_USBVIRTUAL) || defined(SERIAL_USB) /*stm32duino*/|| defined(USBCON) /*STM32_stm32*/ \ diff --git a/examples/Start/RobotCarPinDefinitionsAndMore.h b/examples/Start/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/examples/Start/RobotCarPinDefinitionsAndMore.h +++ b/examples/Start/RobotCarPinDefinitionsAndMore.h @@ -219,6 +219,7 @@ #define LEFT_MOTOR_FORWARD_PIN 14 // IN1 #define LEFT_MOTOR_BACKWARD_PIN 15 // IN2 #define LEFT_MOTOR_PWM_PIN 13 // ENA - Must be PWM capable +#define ESP32_LEDC_MOTOR_CHANNEL 4 // leave first 4 channel for other purposes e.g. Servo and Light (channel 2) // Not tested :-( #define RIGHT_MOTOR_INTERRUPT 12 @@ -231,11 +232,6 @@ #define BUZZER_PIN 23 #endif -// for ESP32 LED_BUILTIN is defined as: static const uint8_t LED_BUILTIN 2 -# if !defined(LED_BUILTIN) && !defined(CAR_IS_ESP32_CAM_BASED) -#define LED_BUILTIN PB1 -# endif - #else // NANO_BASED // Uno based // Pin A0 for VCC monitoring - ADC channel 2 diff --git a/library.properties b/library.properties index 6a6ff95..5701a36 100644 --- a/library.properties +++ b/library.properties @@ -2,8 +2,8 @@ name=PWMMotorControl version=2.1.0 author=Armin Joachimsmeyer maintainer=Armin Joachimsmeyer -sentence=Control brushed DC motors by PWM and uses optional attached encoders to drive fixed distances. For L298 or TB6612, or Adafruit Motor Shield
-paragraph=With special CarPWMMotorControl for easy control of 2 or 4 motors of the obstacle avoiding arduino robot car kits.
If slot-type photo interrupters or IMU / MPU6050 are attached to the encoder discs of such a kit, deterministic distances and turns can be driven.
Examples include a follower car and an Bluetooth controlled obstacle avoiding car.

New: New functions and improved examples.
+sentence=Control brushed DC motors by PWM and uses optional attached encoders to drive fixed distances. For L298 or TB6612, or Adafruit Motor Shield +paragraph=With special CarPWMMotorControl for easy control of 2 or 4 motors of the obstacle avoiding arduino robot car kits. If slot-type photo interrupters or IMU / MPU6050 are attached to the encoder discs of such a kit, deterministic distances and turns can be driven. Examples include a follower car and an Bluetooth controlled obstacle avoiding car. category=Device Control url=https://github.com/ArminJo/PWMMotorControl architectures=* diff --git a/src/CarPWMMotorControl.hpp b/src/CarPWMMotorControl.hpp index 6f28fa7..3dbb1ff 100644 --- a/src/CarPWMMotorControl.hpp +++ b/src/CarPWMMotorControl.hpp @@ -942,16 +942,16 @@ uint8_t CarPWMMotorControl::getTurnDistanceHalfDegree() { void CarPWMMotorControl::printCalibrationValues(Print *aSerial) { aSerial->println(F("Calibration values:")); - aSerial->print(F("mm/256 deg=")); aSerial->print(RobotCar.MillimeterPer256Degree); - aSerial->print(F(" inPlace=")); - aSerial->println(RobotCar.MillimeterPer256DegreeInPlace); - aSerial->print(F("2 volt PWM right=")); + aSerial->print(F(" mm per 256 deg, ")); + aSerial->print(RobotCar.MillimeterPer256DegreeInPlace); + aSerial->println(F(" mm for inPlace")); + aSerial->print(F("PWM for 2 volt: right=")); #if defined(CAR_HAS_4_MECANUM_WHEELS) aSerial->println(RobotCar.rightCarMotor.DriveSpeedPWMFor2Volt); #else aSerial->print(RobotCar.rightCarMotor.DriveSpeedPWMFor2Volt); - aSerial->print(F(" left=")); + aSerial->print(F(", left=")); aSerial->println(RobotCar.leftCarMotor.DriveSpeedPWMFor2Volt); #endif aSerial->println(); diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h index ab085b4..08f81b6 100644 --- a/src/PWMDcMotor.h +++ b/src/PWMDcMotor.h @@ -73,6 +73,23 @@ #define _USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD // to avoid double negations #endif +/* + * Each two ESP32 channels share the same frequency and resolution, and if we allocate an even channel + * and the next (odd) ledc channel is initialized with another frequency/resolution e.g. for Servo, + * our channel also gets this frequency/resolution. + */ +#if defined(ESP32) +# if !defined ESP32_LEDC_MOTOR_CHANNEL +#define ESP32_LEDC_MOTOR_CHANNEL 4 // 4 channels / 2 timers before +# endif +# if !defined ESP32_LEDC_MOTOR_CHANNEL_FREQUENCY +#define ESP32_LEDC_MOTOR_CHANNEL_FREQUENCY 1000 // 1 kHz +# endif +# if !defined ESP32_LEDC_MOTOR_CHANNEL_RESOLUTION +#define ESP32_LEDC_MOTOR_CHANNEL_RESOLUTION 8 // 8 bit +# endif +#endif + #if defined(USE_ADAFRUIT_MOTOR_SHIELD) //This disables using motor as buzzer, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bridge. # if !defined(FULL_BRIDGE_LOSS_MILLIVOLT) diff --git a/src/PWMDcMotor.hpp b/src/PWMDcMotor.hpp index 1de29e5..f18bb72 100644 --- a/src/PWMDcMotor.hpp +++ b/src/PWMDcMotor.hpp @@ -203,6 +203,9 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin // Set DriveSpeedPWM, SpeedPWMCompensation and MillisPerCentimeter defaults setDefaultsForFixedDistanceDriving(); +#if defined(ESP32) + ledcAttachChannel(aPWMPin, ESP32_LEDC_MOTOR_CHANNEL_FREQUENCY, ESP32_LEDC_MOTOR_CHANNEL_RESOLUTION, ESP32_LEDC_MOTOR_CHANNEL); // 1000 Hz, 8 bit, channel 2 +#endif stop(DEFAULT_STOP_MODE); }