From 248a3f8d7ea0432ddbe4015a5463f8cdbe56e6db Mon Sep 17 00:00:00 2001 From: Armin Date: Mon, 25 Nov 2024 16:42:25 +0100 Subject: [PATCH] Improved version and compatible to latest BlueDisplay --- src/ADCUtils.hpp | 2 + src/AutonomousDrive.hpp | 2 +- src/AutonomousDrivePage.hpp | 42 ++-- src/CarPWMMotorControl.h | 11 +- src/CarPWMMotorControl.hpp | 45 ++-- src/Distance.hpp | 19 +- src/EncoderMotor.h | 5 +- src/EncoderMotor.hpp | 24 +- src/IMUCarData.h | 4 +- src/IMUCarData.hpp | 1 - src/LightweightServo.h | 81 +++++-- src/LightweightServo.hpp | 302 ++++++++++++++++++++++--- src/MecanumWheelCarPWMMotorControl.h | 9 +- src/MecanumWheelCarPWMMotorControl.hpp | 27 ++- src/PWMDcMotor.h | 28 ++- src/PWMDcMotor.hpp | 30 ++- src/RobotCarBlueDisplay.ino | 35 +-- src/RobotCarCommonGui.hpp | 21 +- src/RobotCarConfigurations.h | 3 + src/RobotCarPinDefinitionsAndMore.h | 6 +- src/RobotCarTestPage.hpp | 2 + src/RobotCarUtils.hpp | 20 +- src/digitalWriteFast.h | 15 +- 23 files changed, 565 insertions(+), 169 deletions(-) diff --git a/src/ADCUtils.hpp b/src/ADCUtils.hpp index 73030b4..ebf1902 100644 --- a/src/ADCUtils.hpp +++ b/src/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/src/AutonomousDrive.hpp b/src/AutonomousDrive.hpp index 0a14f28..f49c867 100644 --- a/src/AutonomousDrive.hpp +++ b/src/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/src/AutonomousDrivePage.hpp b/src/AutonomousDrivePage.hpp index 9120dd5..7816dd4 100644 --- a/src/AutonomousDrivePage.hpp +++ b/src/AutonomousDrivePage.hpp @@ -31,7 +31,7 @@ BDButton TouchButtonStepMode; const char sStepModeButtonStringContinuousStepToTurn[] PROGMEM = "Continuous\n->\nStep to turn"; const char sStepModeButtonStringStepToTurnSingleStep[] PROGMEM = "Step to turn\n->\nSingle step"; const char sStepModeButtonStringSingleStepContinuous[] PROGMEM = "Single step\n->\nContinuous"; -const char *const sStepModeButtonCaptionStringArray[] PROGMEM = { sStepModeButtonStringContinuousStepToTurn, +const char *const sStepModeButtonTextStringArray[] PROGMEM = { sStepModeButtonStringContinuousStepToTurn, sStepModeButtonStringStepToTurnSingleStep, sStepModeButtonStringSingleStepContinuous }; #if defined(ENABLE_DISTANCE_FEEDBACK_MODE) @@ -39,7 +39,7 @@ BDButton TouchButtonDistanceFeedbackMode; const char sDistanceFeedbackModeNoTone[] PROGMEM = "No tone"; const char sDistanceFeedbackModePentatonic[] PROGMEM = "Pentatonic"; const char sDistanceFeedbackModeContinuous[] PROGMEM = "Continuous"; -const char *const sDistanceFeedbackModeButtonCaptionStringArray[] PROGMEM = { sDistanceFeedbackModeNoTone, +const char *const sDistanceFeedbackModeButtonTextStringArray[] PROGMEM = { sDistanceFeedbackModeNoTone, sDistanceFeedbackModePentatonic, sDistanceFeedbackModeContinuous }; #endif @@ -57,13 +57,13 @@ const char sDistanceSourceModeButtonStringMaxUS[] PROGMEM = "Max->US"; # if defined(CAR_HAS_IR_DISTANCE_SENSOR) const char sDistanceSourceModeButtonStringUSIr[] PROGMEM = "US->IR"; const char sDistanceSourceModeButtonStringIrMin[] PROGMEM = "IR->Min"; -const char *const sDistanceSourceModeButtonCaptionStringArray[] PROGMEM = { sDistanceSourceModeButtonStringMinMax, +const char *const sDistanceSourceModeButtonTextStringArray[] PROGMEM = { sDistanceSourceModeButtonStringMinMax, sDistanceSourceModeButtonStringMaxUS, sDistanceSourceModeButtonStringUSIr, sDistanceSourceModeButtonStringIrMin }; # else const char sDistanceSourceModeButtonStringUSTof[] PROGMEM = "US->ToF"; const char sDistanceSourceModeButtonStringTofMin[] PROGMEM = "ToF->Min"; -const char * const sDistanceSourceModeButtonCaptionStringArray[] PROGMEM = { sDistanceSourceModeButtonStringMinMax, sDistanceSourceModeButtonStringMaxUS, +const char * const sDistanceSourceModeButtonTextStringArray[] PROGMEM = { sDistanceSourceModeButtonStringMinMax, sDistanceSourceModeButtonStringMaxUS, sDistanceSourceModeButtonStringUSTof, sDistanceSourceModeButtonStringTofMin}; # endif #endif @@ -75,8 +75,8 @@ BDButton TouchButtonStartStopBuiltInAutonomousDrive; BDButton TouchButtonFollower; #if defined(ENABLE_DISTANCE_FEEDBACK_MODE) -void setDistanceFeedbackModeButtonCaption() { - TouchButtonDistanceFeedbackMode.setCaptionFromStringArrayPGM(sDistanceFeedbackModeButtonCaptionStringArray, sDistanceFeedbackMode, true); +void setDistanceFeedbackModeButtonText() { + TouchButtonDistanceFeedbackMode.setTextFromStringArrayPGM(sDistanceFeedbackModeButtonTextStringArray, sDistanceFeedbackMode, true); } #pragma GCC diagnostic ignored "-Wunused-parameter" @@ -86,12 +86,12 @@ void doNextDistanceFeedbackMode(BDButton *aTheTouchedButton, int16_t aValue) { sDistanceFeedbackMode = DISTANCE_FEEDBACK_NO_TONE; noTone(BUZZER_PIN); } - setDistanceFeedbackModeButtonCaption(); + setDistanceFeedbackModeButtonText(); } #endif -void setStepModeButtonCaption() { - TouchButtonStepMode.setCaptionFromStringArray((const __FlashStringHelper* const*) sStepModeButtonCaptionStringArray, sStepMode, (sCurrentPage == PAGE_AUTOMATIC_CONTROL)); +void setStepModeButtonText() { + TouchButtonStepMode.setTextFromStringArray((const __FlashStringHelper* const*) sStepModeButtonTextStringArray, sStepMode, (sCurrentPage == PAGE_AUTOMATIC_CONTROL)); } /* @@ -105,7 +105,7 @@ void setStepMode(uint8_t aStepMode) { sDoStep = true; } sStepMode = aStepMode; - setStepModeButtonCaption(); + setStepModeButtonText(); } #pragma GCC diagnostic ignored "-Wunused-parameter" @@ -132,8 +132,8 @@ void doStep(BDButton *aTheTouchedButton, int16_t aValue) { } #if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR) -void setScanModeButtonCaption() { - TouchButtonScanMode.setCaptionFromStringArray(sDistanceSourceModeButtonCaptionStringArray, sDistanceSourceMode); +void setScanModeButtonText() { + TouchButtonScanMode.setTextFromStringArray(sDistanceSourceModeButtonTextStringArray, sDistanceSourceMode); } void doDistanceSourceMode(BDButton *aTheTouchedButton, int16_t aValue) { @@ -141,7 +141,7 @@ void doDistanceSourceMode(BDButton *aTheTouchedButton, int16_t aValue) { if (sDistanceSourceMode > DISTANCE_LAST_SOURCE_MODE) { sDistanceSourceMode = DISTANCE_SOURCE_MODE_MINIMUM; } - setScanModeButtonCaption(); + setScanModeButtonText(); TouchButtonScanMode.drawButton(); } @@ -203,7 +203,7 @@ void initAutonomousDrivePage(void) { TouchButtonScanSpeed.init(0, BUTTON_HEIGHT_6_LINE_4, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_8, COLOR16_BLACK, F("Scan slow"), TEXT_SIZE_14, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doChangeScanSpeed); - TouchButtonScanSpeed.setCaptionForValueTrue("Scan fast"); + TouchButtonScanSpeed.setTextForValueTrue("Scan fast"); #if defined(ENABLE_PATH_INFO_PAGE) TouchButtonPathInfoPage.init(BUTTON_WIDTH_3_POS_3, 0, BUTTON_WIDTH_3, BUTTON_HEIGHT_6, COLOR16_RED, F("Show Path"), TEXT_SIZE_11, @@ -217,7 +217,7 @@ void initAutonomousDrivePage(void) { BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_RED, F("Start User"), TEXT_SIZE_14, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_COLLISION_AVOIDING_USER), &doStartStopTestUser); - TouchButtonStartStopUserAutonomousDrive.setCaptionForValueTrue(F("Stop User")); + TouchButtonStartStopUserAutonomousDrive.setTextForValueTrue(F("Stop User")); #endif #if defined(ENABLE_DISTANCE_FEEDBACK_MODE) @@ -238,12 +238,12 @@ void initAutonomousDrivePage(void) { TouchButtonStartStopBuiltInAutonomousDrive.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, F("Start\nBuiltin"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_COLLISION_AVOIDING_BUILTIN), &doStartStopAutomomousDrive); - TouchButtonStartStopBuiltInAutonomousDrive.setCaptionForValueTrue(F("Stop")); + TouchButtonStartStopBuiltInAutonomousDrive.setTextForValueTrue(F("Stop")); TouchButtonFollower.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, F("Start\nFollow"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_FOLLOWER), &doStartStopFollowerMode); - TouchButtonFollower.setCaptionForValueTrue(F("Stop\nFollow")); + TouchButtonFollower.setTextForValueTrue(F("Stop\nFollow")); } void drawAutonomousDrivePage(void) { @@ -288,12 +288,12 @@ void startAutonomousDrivePage(void) { handleAutomomousDriveRadioButtons(); // restore last step and scan mode - setStepModeButtonCaption(); + setStepModeButtonText(); #if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR) - setScanModeButtonCaption(); + setScanModeButtonText(); #endif #if defined(ENABLE_DISTANCE_FEEDBACK_MODE) - setDistanceFeedbackModeButtonCaption(); + setDistanceFeedbackModeButtonText(); #endif #if defined(ENABLE_PATH_INFO_PAGE) #define SLIDER_SHIFTED_Y_POS BUTTON_HEIGHT_6 @@ -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/src/CarPWMMotorControl.h b/src/CarPWMMotorControl.h index e65b526..d444eb8 100644 --- a/src/CarPWMMotorControl.h +++ b/src/CarPWMMotorControl.h @@ -141,14 +141,17 @@ class CarPWMMotorControl { * Functions for moving a fixed distance */ // With signed distance - void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, - uint8_t aRequestedDirection); // only setup values - void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors() - + void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values + void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection)__attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed()."))); // only setup values void goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped + void goDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // only setup values + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection); // only setup values bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally diff --git a/src/CarPWMMotorControl.hpp b/src/CarPWMMotorControl.hpp index 6f28fa7..25967ac 100644 --- a/src/CarPWMMotorControl.hpp +++ b/src/CarPWMMotorControl.hpp @@ -553,13 +553,17 @@ void CarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDi } void CarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); } +void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} /* * initialize motorInfo fields LastDirection and CompensatedSpeedPWM */ -void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, +void CarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { #if defined(USE_MPU6050_IMU) @@ -572,26 +576,34 @@ void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, u setSpeedPWMWithRamp(aRequestedSpeedPWM, aRequestedDirection); #else checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); - leftCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + rightCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + leftCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); #endif } void CarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); waitUntilStopped(aLoopCallback); } void CarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { if (aRequestedDistanceMillimeter < 0) { aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } +void CarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} /** * Wait until distance is reached * @param aLoopCallback called until car has stopped to avoid blocking @@ -601,6 +613,11 @@ void CarPWMMotorControl::goDistanceMillimeter(int aRequestedDistanceMillimeter, waitUntilStopped(aLoopCallback); } +void CarPWMMotorControl::goDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) { + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter); + waitUntilStopped(aLoopCallback); +} + /* * Stop car (with ramp) */ @@ -797,8 +814,8 @@ void CarPWMMotorControl::startRotate(int aRotationDegrees, turn_direction_t aTur tLeftMotorIfPositiveTurn->setSpeedPWMAndDirection(tTurnSpeedPWMLeft, DIRECTION_BACKWARD); } #else - tRightMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD); - tLeftMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD); + tRightMotorIfPositiveTurn->startGoDistanceMillimeterWithSpeed(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD); + tLeftMotorIfPositiveTurn->startGoDistanceMillimeterWithSpeed(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD); #endif } @@ -942,16 +959,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/Distance.hpp b/src/Distance.hpp index 5cbcee2..50e2109 100644 --- a/src/Distance.hpp +++ b/src/Distance.hpp @@ -33,12 +33,16 @@ #include "HCSR04.hpp" // include sources #include "RobotCarConfigurations.h" // helps the pretty printer / Ctrl F -#if defined(CAR_HAS_SERVO) && defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) -#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE // saves 70 bytes program space +#if defined(CAR_HAS_DISTANCE_SERVO) +# if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) #include "LightweightServo.hpp" -#endif +# if defined(_LIGHTWEIGHT_SERVO_HPP) +LightweightServo DistanceServo; // The pan servo instance for distance sensor +# else // LightweightServo is not applicable for this CPU +#undef USE_LIGHTWEIGHT_SERVO_LIBRARY +# endif +# endif // defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) -#if defined(CAR_HAS_DISTANCE_SERVO) # if !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) Servo DistanceServo; // The pan servo instance for distance sensor # endif @@ -87,7 +91,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo; void initDistance() { getDistanceModesFromPins(); -#if defined(CAR_HAS_DISTANCE_SERVO) && !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) +#if defined(CAR_HAS_DISTANCE_SERVO) DistanceServo.attach(DISTANCE_SERVO_PIN); #endif @@ -492,11 +496,8 @@ void DistanceServoWriteAndWaitForStop(uint8_t aTargetDegrees, bool doWaitForStop // The servo is top down and therefore inverted aTargetDegrees = 180 - aTargetDegrees; #endif -#if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY) - write10(aTargetDegrees); -#else DistanceServo.write(aTargetDegrees); -#endif + /* * Delay until stopped diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h index 4ad7ee2..566208d 100644 --- a/src/EncoderMotor.h +++ b/src/EncoderMotor.h @@ -1,7 +1,6 @@ /* * EncoderMotor.h * - * Created on: 12.05.2019 * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * @@ -79,7 +78,9 @@ class EncoderMotor: public PWMDcMotor { */ void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance count void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); - void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection)__attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed()."))); + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // Signed distance count + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); bool updateMotor(); /* diff --git a/src/EncoderMotor.hpp b/src/EncoderMotor.hpp index d5a506d..2438f6b 100644 --- a/src/EncoderMotor.hpp +++ b/src/EncoderMotor.hpp @@ -11,8 +11,7 @@ * * Tested for Adafruit Motor Shield and plain TB6612 breakout board. * - * Created on: 12.05.2019 - * Copyright (C) 2019-2022 Armin Joachimsmeyer + * Copyright (C) 2019-2024 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. @@ -90,10 +89,14 @@ void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMP } #endif +void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + startGoDistanceMillimeterWithSpeed( aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} /* * If motor is already running, adjust TargetDistanceMillimeter to go to aRequestedDistanceMillimeter */ -void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, +void EncoderMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { if (aRequestedDistanceMillimeter == 0) { stop(DefaultStopMode); // In case motor was running @@ -118,7 +121,7 @@ void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigne } void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { - startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); } /* @@ -128,9 +131,18 @@ void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMill void EncoderMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { if (aRequestedDistanceMillimeter < 0) { aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; - startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} + +void EncoderMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_FORWARD); + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } diff --git a/src/IMUCarData.h b/src/IMUCarData.h index f2404d8..11650bf 100644 --- a/src/IMUCarData.h +++ b/src/IMUCarData.h @@ -3,7 +3,6 @@ * * Functions for getting IMU data from MPU6050 for car control. * - * Created on: 19.11.2020 * Copyright (C) 2020-2022 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * @@ -45,6 +44,7 @@ extern bool sOnlyPlotterOutput; // from main program //#define SAMPLE_RATE 250 //#define SAMPLE_RATE 125 #endif + #if SAMPLE_RATE == 1000 #define RATE_SHIFT 0 #define DELAY_TO_NEXT_IMU_DATA_MILLIS 1 @@ -75,7 +75,7 @@ class IMUCarData { bool initMPU6050FifoForCarData(); bool initMPU6050CarDataAndCalculateAllOffsetsAndWait(); - unsigned int getMPU6050SampleRate(); // just returns SAMPLE_RATE + unsigned int getMPU6050SampleRate(); // just returns ORIGINAL_SAMPLE_RATE void readCarDataFromMPU6050(); bool readCarDataFromMPU6050Fifo(); diff --git a/src/IMUCarData.hpp b/src/IMUCarData.hpp index 935f428..fd7ea45 100644 --- a/src/IMUCarData.hpp +++ b/src/IMUCarData.hpp @@ -9,7 +9,6 @@ * Automatic offset recalculation is done, if we do not detect any movement for NUMBER_OF_OFFSET_CALIBRATION_SAMPLES samples. * It can be activated (default) or suspended. * - * Created on: 19.11.2020 * Copyright (C) 2020-2022 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * diff --git a/src/LightweightServo.h b/src/LightweightServo.h index 5b27fa3..115646d 100644 --- a/src/LightweightServo.h +++ b/src/LightweightServo.h @@ -24,37 +24,60 @@ #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 LIGHTWEIGHT_SERVO_CHANNEL_A_PIN 46 +#define LIGHTWEIGHT_SERVO_CHANNEL_B_PIN 45 +#define LIGHTWEIGHT_SERVO_CHANNEL_C_PIN 44 +#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) +#define LIGHTWEIGHT_SERVO_CHANNEL_A_PIN 9 +#define LIGHTWEIGHT_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); +// The last parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, if false, but saves around 60 bytes anyway +void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast = false, bool aDoAutoInit = true); + +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 +86,33 @@ 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 +class LightweightServo { +public: + uint8_t attach(int aPin); + uint8_t attach(int aPin, int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree); + void detach(); + void write(int aTargetDegreeOrMicrosecond); + void writeMicroseconds(int aTargetMicrosecond); // Write pulse width in microseconds + /* + * Variables to enable adjustment for different servo types + * 544 and 2400 are values compatible with standard arduino values + */ + int MicrosecondsForServo0Degree = 544; + int MicrosecondsForServo180Degree = 2400; + uint8_t LightweightServoPin; +}; + +#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/src/LightweightServo.hpp b/src/LightweightServo.hpp index 0da2967..b9ca849 100644 --- a/src/LightweightServo.hpp +++ b/src/LightweightServo.hpp @@ -1,7 +1,10 @@ /* * LightweightServo.hpp * - * Lightweight Servo implementation only for pin 9 and 10 using only timer1 hardware and no interrupts or other overhead. + * Lightweight Servo implementation timer hardware and no interrupts or other overhead. + * Supported pins: + * ATmega328: pin 44 = OC5C/PL5, 45 = OC5B/PL4 and 46 = OC5A/PL3 using only timer5 hardware + * ATmega2560 pin 9 = OC1A and 10 = OC1B on 328 using only timer1 hardware * Provides auto initialization. * 300 bytes code size / 4 bytes RAM including auto initialization compared to 700 / 48 bytes for Arduino Servo library. * 8 bytes for each call to setLightweightServoPulse... @@ -26,11 +29,17 @@ * */ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) +#include "LightweightServo.h" + #ifndef _LIGHTWEIGHT_SERVO_HPP #define _LIGHTWEIGHT_SERVO_HPP -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) -#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 @@ -40,22 +49,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 +78,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 +86,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 +96,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 +106,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 +151,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 +163,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 +208,214 @@ 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 + * Supported pins: + * ATmega328: pin 44 = OC5C/PL5, 45 = OC5B/PL4 and 46 = OC5A/PL3 using only timer5 hardware + * ATmega2560 pin 9 = OC1A and 10 = OC1B on 328 using only timer1 hardware + */ +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 == LIGHTWEIGHT_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 == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR5A & (_BV(COM5B1)))) { + OCR5B = UINT16_MAX; + DDRL |= (_BV(DDL4)); + tNewTCCR5A |= (_BV(COM5B1)) | _BV(WGM51); + tPinWasNotInitialized = true; + } else if (aPin == LIGHTWEIGHT_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 == LIGHTWEIGHT_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 == LIGHTWEIGHT_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 + * Init state is reflected by COMXX1 register bit + */ +void deinitLightweightServoPin(uint8_t aPin) { +#if defined(__AVR_ATmega2560__) + if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + DDRL &= ~(_BV(DDL3)); + TCCR5A &= ~(_BV(COM5A1)); + } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN) { + DDRL &= ~(_BV(DDL4)); + TCCR5A &= ~(_BV(COM5B1)); + } else { + DDRL &= ~(_BV(DDL5)); + TCCR5A &= ~(_BV(COM5C1)); + } +#else + if (aPin == LIGHTWEIGHT_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 == LIGHTWEIGHT_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, bool aDoAutoInit) { +#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) + if (aDoAutoInit) { + 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 == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) { + OCR5A = aMicroseconds; +} else if (aPin == LIGHTWEIGHT_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 == LIGHTWEIGHT_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) { +#if defined(__AVR_ATmega2560__) + ICR5 = aRefreshPeriodMicroseconds * 2; +#else + ICR1 = aRefreshPeriodMicroseconds * 2; +#endif +} +/* + * 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 +428,46 @@ int MicrosecondsToDegreeLightweightServo(int aMicroseconds) { return map(aMicroseconds, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree, 0, 180); } -#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +/* + * LightweightServo class functions + */ +uint8_t LightweightServo::attach(int aPin) { + LightweightServoPin = aPin; + checkAndInitLightweightServoPin(aPin); + return aPin; +} + +/* + * do not use parameters aMicrosecondsForServo0Degree and aMicrosecondsForServo180Degree + */ +uint8_t LightweightServo::attach(int aPin, int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree) { + LightweightServoPin = aPin; + MicrosecondsForServo0Degree = aMicrosecondsForServo0Degree; + MicrosecondsForServo180Degree = aMicrosecondsForServo180Degree; + checkAndInitLightweightServoPin(aPin); + return aPin; +} + +void LightweightServo::detach() { + deinitLightweightServoPin(LightweightServoPin); +} + +void LightweightServo::write(int aTargetDegreeOrMicrosecond) { + if (aTargetDegreeOrMicrosecond <= 180) { + aTargetDegreeOrMicrosecond = (map(aTargetDegreeOrMicrosecond, 0, 180, MicrosecondsForServo0Degree, + MicrosecondsForServo180Degree)); + } + // The last false parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, but saves around 60 bytes anyway + writeMicrosecondsLightweightServoPin(aTargetDegreeOrMicrosecond, LightweightServoPin, false, false); +} + +void LightweightServo::writeMicroseconds(int aTargetMicrosecond){ + // The last false parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, but saves around 60 bytes anyway + writeMicrosecondsLightweightServoPin(aTargetMicrosecond, LightweightServoPin, false, false); +} + +#if defined(LOCAL_DEBUG) +#undef LOCAL_DEBUG +#endif #endif // _LIGHTWEIGHT_SERVO_HPP +#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega2560__) diff --git a/src/MecanumWheelCarPWMMotorControl.h b/src/MecanumWheelCarPWMMotorControl.h index e731d13..ed55f2d 100644 --- a/src/MecanumWheelCarPWMMotorControl.h +++ b/src/MecanumWheelCarPWMMotorControl.h @@ -3,7 +3,7 @@ * * Contains functions for control of the 4 motors of a mecanum wheel car. * - * Copyright (C) 2022 Armin Joachimsmeyer + * Copyright (C) 2022-2024 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. @@ -71,10 +71,13 @@ class MecanumWheelCarPWMMotorControl : public CarPWMMotorControl { * Functions for moving a fixed distance */ // With signed distance + void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors() + void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) __attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed()."))); + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors() + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors() void goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, diff --git a/src/MecanumWheelCarPWMMotorControl.hpp b/src/MecanumWheelCarPWMMotorControl.hpp index 973a191..2433e08 100644 --- a/src/MecanumWheelCarPWMMotorControl.hpp +++ b/src/MecanumWheelCarPWMMotorControl.hpp @@ -424,13 +424,17 @@ void MecanumWheelCarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } +void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, + unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} /* * initialize motorInfo fields LastDirection and CompensatedSpeedPWM */ -void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, +void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { #if defined(USE_MPU6050_IMU) @@ -443,23 +447,32 @@ void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequeste setSpeedPWMWithRamp(aRequestedSpeedPWM, aRequestedDirection); #else checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + rightCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); setDirection(aRequestedDirection); // this sets the direction for all the other motors #endif } void MecanumWheelCarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); waitUntilStopped(aLoopCallback); } void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { if (aRequestedDistanceMillimeter < 0) { aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} + +void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } @@ -567,7 +580,7 @@ void MecanumWheelCarPWMMotorControl::startRotate(int aRotationDegrees, turn_dire tTurnSpeed = DEFAULT_START_SPEED_PWM; } // Use direction set by setDirection() above - rightCarMotor.startGoDistanceMillimeter(tTurnSpeed, tDistanceMillimeter, rightCarMotor.getDirection()); + rightCarMotor.startGoDistanceMillimeterWithSpeed(tTurnSpeed, tDistanceMillimeter, rightCarMotor.getDirection()); #if defined(LOCAL_DEBUG) Serial.print(F("RotationDegrees=")); Serial.print(aRotationDegrees); diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h index ab085b4..1a2cd67 100644 --- a/src/PWMDcMotor.h +++ b/src/PWMDcMotor.h @@ -14,7 +14,7 @@ * With encoder: - distance is measured by Encoder. * * - * Copyright (C) 2019-2022 Armin Joachimsmeyer + * Copyright (C) 2019-2024 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. @@ -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) @@ -361,7 +378,11 @@ class PWMDcMotor { void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) __attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed()."))); + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // Signed distance + void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + uint32_t convertMillimeterToMillis(uint8_t aSpeedPWM, unsigned int aRequestedDistanceMillimeter); unsigned int convertMillisToMillimeter(uint8_t aSpeedPWM, unsigned int aMillis); unsigned int convertMillisToCentimeterFor2Volt(unsigned int aMillis); @@ -445,6 +466,11 @@ class PWMDcMotor { }; /* + * Version 2.2.0 - 11/2024 + * - Added 2 functions startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, ...). + * - ESP32 core 3.x support. + * - Improved examples, especially follower examples. + * * Version 2.1.0 - 09/2023 * - Added convertMillimeterToMillis() etc. * - Added Variable computedMillisOfMotorForDistance. diff --git a/src/PWMDcMotor.hpp b/src/PWMDcMotor.hpp index 1de29e5..256855d 100644 --- a/src/PWMDcMotor.hpp +++ b/src/PWMDcMotor.hpp @@ -19,8 +19,7 @@ * A fixed speed compensation PWM value to be subtracted can be specified. * * - * Created on: 12.05.2019 - * Copyright (C) 2019-2022 Armin Joachimsmeyer + * Copyright (C) 2019-2024 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. @@ -203,6 +202,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); } @@ -804,7 +806,7 @@ void PWMDcMotor::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, */ void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { - startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); #if defined(DO_NOT_SUPPORT_RAMP) delay(computedMillisOfMotorForDistance); #else @@ -816,7 +818,7 @@ void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int a } void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { - startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection); } /* @@ -825,9 +827,18 @@ void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillim void PWMDcMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { if (aRequestedDistanceMillimeter < 0) { aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; - startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} + +void PWMDcMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } @@ -845,10 +856,15 @@ unsigned int PWMDcMotor::convertMillisToCentimeterFor2Volt(unsigned int aMillis) return aMillis / MillisPerCentimeter; } +void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} + /* * If motor is already running, just update speed and new stop time */ -void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, +void PWMDcMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { if (aRequestedDistanceMillimeter == 0) { stop(STOP_MODE_BRAKE); // In case motor was running diff --git a/src/RobotCarBlueDisplay.ino b/src/RobotCarBlueDisplay.ino index 4352742..f87bdcc 100644 --- a/src/RobotCarBlueDisplay.ino +++ b/src/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 @@ -98,11 +97,13 @@ Servo TiltServo; */ //#define NO_SERIAL_OUTPUT // Saves up to 2532 bytes of program memory for L298_2WD_2LI_ION_BASIC_CONFIGURATION #if defined(NO_SERIAL_OUTPUT) // No printing of any info with Serial.print or we get "multiple definition of __vector_18". -#define USE_SIMPLE_SERIAL // We can use simple serial here, since no Serial.print are active. Saves up to 2172 bytes in BlueDisplay library. +#define BD_USE_SIMPLE_SERIAL // We can use simple serial here, since no Serial.print are active. Saves up to 2172 bytes in BlueDisplay library. #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 @@ -263,7 +267,7 @@ void setup() { } else { #if defined(ENABLE_SERIAL_OUTPUT) // requires 1504 bytes program space -# if !defined(USE_SIMPLE_SERIAL) && !defined(USE_SERIAL1) // print it now if not printed above +# if !defined(BD_USE_SIMPLE_SERIAL) && !defined(BD_USE_SERIAL1) // print it now if not printed above # if defined(__AVR_ATmega32U4__) || defined(SERIAL_PORT_USBVIRTUAL) || defined(SERIAL_USB) /*stm32duino*/|| defined(USBCON) /*STM32_stm32*/ \ || defined(SERIALUSB_PID) || defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_attiny3217) delay(4000); // To be able to connect Serial monitor after reset or power up and before first print out. Do not wait for an attached Serial Monitor! @@ -275,7 +279,7 @@ void setup() { # endif #endif } -#if !defined(USE_SIMPLE_SERIAL) && !defined(USE_SERIAL1) // print it now if not printed above +#if !defined(BD_USE_SIMPLE_SERIAL) && !defined(BD_USE_SERIAL1) // print it now if not printed above /* * Print this always, it can be seen in the app log */ @@ -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/src/RobotCarCommonGui.hpp b/src/RobotCarCommonGui.hpp index 5577983..ead262b 100644 --- a/src/RobotCarCommonGui.hpp +++ b/src/RobotCarCommonGui.hpp @@ -76,8 +76,13 @@ unsigned int sLastSliderIROrTofCentimeter; void setupGUI(void) { sCurrentPage = PAGE_HOME; - // Register callback handler and check for connection - // This leads to call to initDisplay() and startCurrentPage() after connect + /* + * Register callback handler and wait for 300 ms if Bluetooth connection is still active. + * For ESP32 and after power on of the Bluetooth module (HC-05) at other platforms, Bluetooth connection is most likely not active here. + * + * If active, mCurrentDisplaySize and mHostUnixTimestamp are set and initDisplay() and drawGui() functions are called. + * If not active, the periodic call of checkAndHandleEvents() in the main loop waits for the (re)connection and then performs the same actions. + */ BlueDisplay1.initCommunication(&initRobotCarDisplay, &startCurrentPage); } @@ -284,7 +289,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 @@ -315,12 +320,12 @@ void doSetDirection(BDButton *aTheTouchedButton, int16_t aDirection) { uint8_t tNextDirection = aDirection + 1; if (tNextDirection > DIRECTION_BACKWARD) { tNextDirection = DIRECTION_STOP; - TouchButtonDirection.setCaption(F("o")); + TouchButtonDirection.setText(F("o")); } else if (tNextDirection == DIRECTION_FORWARD) { - TouchButtonDirection.setCaption(F("\x87")); + TouchButtonDirection.setText(F("\x87")); } else { // backward - TouchButtonDirection.setCaption(F("\x88")); + TouchButtonDirection.setText(F("\x88")); } sRobotCarDirection = tNextDirection; TouchButtonDirection.setValueAndDraw(tNextDirection); @@ -364,8 +369,10 @@ void startCurrentPage() { startHomePage(); break; } +#if defined(MONITOR_VIN_VOLTAGE) forceDisplayOfVin(); readAndPrintVin(); +#endif } /* @@ -445,7 +452,7 @@ void initCommonGui() { */ TouchButtonRobotCarStartStop.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_BLUE, F("Start"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doStartStopRobotCar); - TouchButtonRobotCarStartStop.setCaptionForValueTrue(F("Stop")); + TouchButtonRobotCarStartStop.setTextForValueTrue(F("Stop")); TouchButtonBack.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, F("Back"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_HOME, &GUISwitchPages); diff --git a/src/RobotCarConfigurations.h b/src/RobotCarConfigurations.h index b5b99ec..ba7f32d 100644 --- a/src/RobotCarConfigurations.h +++ b/src/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/src/RobotCarPinDefinitionsAndMore.h b/src/RobotCarPinDefinitionsAndMore.h index 6738a58..7cfad54 100644 --- a/src/RobotCarPinDefinitionsAndMore.h +++ b/src/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/src/RobotCarTestPage.hpp b/src/RobotCarTestPage.hpp index 28c4fe1..527ba3d 100644 --- a/src/RobotCarTestPage.hpp +++ b/src/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/src/RobotCarUtils.hpp b/src/RobotCarUtils.hpp index bc5140d..6596b5a 100644 --- a/src/RobotCarUtils.hpp +++ b/src/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/src/digitalWriteFast.h b/src/digitalWriteFast.h index 1bb8eb5..a36a71f 100644 --- a/src/digitalWriteFast.h +++ b/src/digitalWriteFast.h @@ -11,6 +11,14 @@ #ifndef __digitalWriteFast_h_ #define __digitalWriteFast_h_ 1 +//#define THROW_ERROR_IF_NOT_FAST // If activated, an error is thrown if pin is not a compile time constant +void NonConstantsUsedForPinModeFast( void ) __attribute__ (( error("Parameter for pinModeFast() function is not constant") )); +void NonConstantsUsedForDigitalWriteFast( void ) __attribute__ (( error("Parameter for digitalWriteFast() function is not constant") )); +void NonConstantsUsedForDigitalToggleFast( void ) __attribute__ (( error("Parameter for digitalToggleFast() function is not constant") )); +int NonConstantsUsedForDigitalReadFast( void ) __attribute__ (( error("Parameter for digitalReadFast() function is not constant") )); + +#if !defined(MEGATINYCORE) // megaTinyCore has it own digitalWriteFast function set, except digitalToggleFast(). + //#define SANGUINO_PINOUT // define for Sanguino pinout // general macros/defines @@ -312,12 +320,6 @@ #endif - -void NonConstantsUsedForPinModeFast( void ) __attribute__ (( error("Parameter for pinModeFast() function is not constant") )); -void NonConstantsUsedForDigitalWriteFast( void ) __attribute__ (( error("Parameter for digitalWriteFast() function is not constant") )); -void NonConstantsUsedForDigitalToggleFast( void ) __attribute__ (( error("Parameter for digitalToggleFast() function is not constant") )); -int NonConstantsUsedForDigitalReadFast( void ) __attribute__ (( error("Parameter for digitalReadFast() function is not constant") )); - #if !defined(digitalWriteFast) # if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) && defined(__digitalPinToPortReg) # if defined(THROW_ERROR_IF_NOT_FAST) @@ -416,4 +418,5 @@ if (__builtin_constant_p(P)) { \ # endif #endif // !defined(digitalToggleFast) +#endif // !defined(MEGATINYCORE) #endif //__digitalWriteFast_h_