diff --git a/.github/workflows/TestCompile.yml b/.github/workflows/TestCompile.yml index 82b288c..e1e5d2d 100644 --- a/.github/workflows/TestCompile.yml +++ b/.github/workflows/TestCompile.yml @@ -34,7 +34,6 @@ jobs: build-properties: RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DUSE_ADAFRUIT_MOTOR_SHIELD @@ -44,7 +43,6 @@ jobs: RobotCarBlueDisplay: -DUSE_LAYOUT_FOR_NANO -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DCAR_HAS_PAN_SERVO @@ -58,7 +56,6 @@ jobs: build-properties: RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DCAR_HAS_TOF_DISTANCE_SENSOR @@ -76,22 +73,20 @@ jobs: ref: master path: CustomLibrary_BD # must contain string "Custom" - - name: Checkout new PWMMotorControl library - uses: actions/checkout@v2 - with: - repository: ArminJo/PWMMotorControl - ref: master - path: CustomLibrary_PWM # must contain string "Custom" +# Currently included in source directory +# - name: Checkout new PWMMotorControl library +# uses: actions/checkout@v2 +# with: +# repository: ArminJo/PWMMotorControl +# ref: master +# path: CustomLibrary_PWM # must contain string "Custom" - # Use the arduino-test-compile script, because it is faster - - name: Compile all examples using the bash script arduino-test-compile.sh - env: - # Passing parameters to the script by setting the appropriate ENV_* variables. - ENV_ARDUINO_BOARD_FQBN: ${{ matrix.arduino-boards-fqbn }} - ENV_REQUIRED_LIBRARIES: ${{ env.REQUIRED_LIBRARIES }} - ENV_EXAMPLES_BUILD_PROPERTIES: ${{ toJson(matrix.build-properties) }} - run: | - wget --quiet https://raw.githubusercontent.com/ArminJo/arduino-test-compile/master/arduino-test-compile.sh - ls -l arduino-test-compile.sh - chmod +x arduino-test-compile.sh - ./arduino-test-compile.sh + - 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 }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/README.md b/README.md index 00ea452..d1c7dd6 100644 --- a/README.md +++ b/README.md @@ -13,9 +13,9 @@ Just overwrite the function doUserCollisionDetection() to test your own skill. You may also overwrite the function fillAndShowForwardDistancesInfo(), if you use your own scanning method. -# Compile options / macros +# Compile options / macros for this software To customize the software to different car extensions, there are some compile options / macros available.
-Modify it by commenting them out or in, or change the values if applicable. Or define the macro with the -D compiler option for gobal compile (the latter is not possible with the Arduino IDE, so consider to use [Sloeber](https://eclipse.baeyens.it).
+Modify it by commenting them out or in, or change the values if applicable. Or define the macro with the -D compiler option for global compile (the latter is not possible with the Arduino IDE, so consider using [Sloeber](https://eclipse.baeyens.it).
Compile options for the used **PWMMotorControl library** like `USE_ENCODER_MOTOR_CONTROL` are described [here](https://github.com/ArminJo/PWMMotorControl#compile-options--macros-for-this-library). | Option | Default | File | Description | @@ -25,22 +25,24 @@ Compile options for the used **PWMMotorControl library** like `USE_ENCODER_MOTOR | `USE_US_SENSOR_1_PIN_MODE` | disabled | RobotCar.h | Use modified HC-SR04 modules or HY-SRF05 ones.
Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger pin. | | `CAR_HAS_IR_DISTANCE_SENSOR` | disabled | RobotCar.h | Use Sharp GP2Y0A21YK / 1080 IR distance sensor. | | `CAR_HAS_TOF_DISTANCE_SENSOR` | disabled | RobotCar.h | Use VL53L1X TimeOfFlight distance sensor. | -| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. | +| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. The Servo direction is reverse then. | | `CAR_HAS_CAMERA` | disabled | RobotCar.h | Enables the `Camera` button for the `PIN_CAMERA_SUPPLY_CONTROL` pin. | | `CAR_HAS_LASER` | disabled | RobotCar.h | Enables the `Laser` button for the `PIN_LASER_OUT` / `LED_BUILTIN` pin. | | `CAR_HAS_PAN_SERVO` | disabled | RobotCar.h | Enables the pan slider for the `PanServo` at the `PIN_PAN_SERVO` pin. | -| `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin.. | -| `MONITOR_LIPO_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. Requires 2 additional resistors at pin A2. | -| `VIN_VOLTAGE_CORRECTION` | undefined | RobotCar.h | Voltage to be subtracted from VIN voltage. E.g. if there is a series diode between LIPO and VIN set it to 0.8. | +| `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin. | +| `ENABLE_RTTTL` | undefined | RobotCar.h | Plays melody after initial timeout has reached. Enables the Melody button, which plays a random melody. | +| `MONITOR_VIN_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. VIN/11 at A2, 1MOhm to VIN, 100kOhm to ground. | +| `VIN_VOLTAGE_CORRECTION` | undefined or 0.8 for UNO | RobotCar.h | Voltage to be subtracted from VIN voltage for voltage monitoring. E.g. if there is a series diode between LIPO and VIN as on the UNO boards, set it to 0.8. | +| `SUPPORT_EEPROM_STORAGE` | disabled | RobotCar.h | Activates the buttons to store compensation and drive speed. | -### Modifying library properties with Arduino IDE -First use *Sketch/Show Sketch Folder (Ctrl+K)*.
+### Modifying compile options with Arduino IDE +First, use *Sketch > Show Sketch Folder (Ctrl+K)*.
If you did not yet stored the example as your own sketch, then you are instantly in the right library folder.
Otherwise you have to navigate to the parallel `libraries` folder and select the library you want to access.
In both cases the library files itself are located in the `src` directory.
-### Modifying library properties with Sloeber IDE -If you are using Sloeber as your IDE, you can easily define global symbols with *Properties/Arduino/CompileOptions*.
+### Modifying compile options with Sloeber IDE +If you are using Sloeber as your IDE, you can easily define global symbols with *Properties > Arduino > CompileOptions*.
![Sloeber settings](https://github.com/ArminJo/ServoEasing/blob/master/pictures/SloeberDefineSymbols.png) # Wall detection diff --git a/pictures/WIN_20161218_15_08_55_Pro.jpg b/pictures/WIN_20161218_15_08_55_Pro.jpg deleted file mode 100644 index 502e293..0000000 Binary files a/pictures/WIN_20161218_15_08_55_Pro.jpg and /dev/null differ diff --git a/src/ADCUtils.cpp b/src/ADCUtils.cpp index fde8b14..4d19337 100644 --- a/src/ADCUtils.cpp +++ b/src/ADCUtils.cpp @@ -22,8 +22,8 @@ * along with this program. If not, see . */ -#if defined(__AVR__) && (! defined(__AVR_ATmega4809__)) #include "ADCUtils.h" +#if defined(__AVR__) && defined(ADATE) // Union to speed up the combination of low and high bytes to a word // it is not optimal since the compiler still generates 2 unnecessary moves @@ -35,7 +35,7 @@ union Myword { } byte; uint16_t UWord; int16_t Word; - uint8_t * BytePointer; + uint8_t *BytePointer; }; /* @@ -45,18 +45,18 @@ uint16_t readADCChannel(uint8_t aChannelNumber) { Myword tUValue; ADMUX = aChannelNumber | (DEFAULT << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode - is default -// ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode + // ADCSRB = 0; // Only active if ADATE is set to 1. + // ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADIF) | ADC_PRESCALE); -// wait for single conversion to finish + // wait for single conversion to finish loop_until_bit_is_clear(ADCSRA, ADSC); -// Get value + // Get value tUValue.byte.LowByte = ADCL; tUValue.byte.HighByte = ADCH; return tUValue.UWord; -// return ADCL | (ADCH <<8); // needs 4 bytes more + // return ADCL | (ADCH <<8); // needs 4 bytes more } /* @@ -66,14 +66,14 @@ uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference) Myword tUValue; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default + // ADCSRB = 0; // Only active if ADATE is set to 1. // ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADIF) | ADC_PRESCALE); -// wait for single conversion to finish + // wait for single conversion to finish loop_until_bit_is_clear(ADCSRA, ADSC); -// Get value + // Get value tUValue.byte.LowByte = ADCL; tUValue.byte.HighByte = ADCH; return tUValue.UWord; @@ -81,31 +81,36 @@ uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference) /* * @return original ADMUX register content for optional later restoring values + * All experimental values are acquired by using the ADCSwitchingTest example from this library */ uint8_t checkAndWaitForReferenceAndChannelToSwitch(uint8_t aChannelNumber, uint8_t aReference) { uint8_t tOldADMUX = ADMUX; /* - * Must wait >= 7 us if reference has to be switched from 1.1 volt to VCC (seen on oscilloscope) - * Must wait >= 6000 us for Nano board >= 6200 for Uno board if reference has to be switched from VCC/DEFAULT to 1.1 volt/INTERNAL - * Must wait >= 1100 us if channel has to be switched to 1.1 volt internal channel from channel with read 5 volt input + * Must wait >= 7 us if reference has to be switched from 1.1 volt/INTERNAL to VCC/DEFAULT (seen on oscilloscope) + * This is done after the 2 ADC clock cycles required for Sample & Hold :-) + * + * Must wait >= 7600 us for Nano board >= 6200 for Uno board if reference has to be switched from VCC/DEFAULT to 1.1 volt/INTERNAL + * Must wait >= 200 us if channel has to be switched to 1.1 volt internal channel if S&H was at 5 Volt */ uint8_t tNewReference = (aReference << SHIFT_VALUE_FOR_REFERENCE); ADMUX = aChannelNumber | tNewReference; - if ((tOldADMUX & MASK_FOR_ADC_REFERENCE) != tNewReference) { - if (aReference == INTERNAL) { + if ((tOldADMUX & MASK_FOR_ADC_REFERENCE) != tNewReference && aReference == INTERNAL) { + /* + * Switch reference from DEFAULT to INTERNAL + */ + delayMicroseconds(8000); // experimental value is >= 7600 us for Nano board and 6200 for UNO board + } else if ((tOldADMUX & 0x0F) != aChannelNumber) { + if (aChannelNumber == ADC_1_1_VOLT_CHANNEL_MUX) { /* - * Switch reference from DEFAULT to INTERNAL + * Internal 1.1 Volt channel requires <= 200 us for Nano board */ - delayMicroseconds(6500); // experimental value is >= 5000 us for Nano board and 6200 for UNO board + delayMicroseconds(200); } else { - // Switch reference from INTERNAL to DEFAULT - delayMicroseconds(10); + /* + * 100 kOhm requires < 100 us, 1 MOhm requires 120 us S&H switching time + */ + delayMicroseconds(120); // experimental value is <= 1100 us for Nano board } - } else if (aChannelNumber == ADC_1_1_VOLT_CHANNEL_MUX && (tOldADMUX & 0x0F) != aChannelNumber) { - /* - * Switch to (high impedance) 1.1 volt channel - */ - delayMicroseconds(1200); // experimental value is >= 1100 us for Nano board } return tOldADMUX; } @@ -121,8 +126,8 @@ uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t a uint16_t tSumValue = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE); for (uint8_t i = 0; i < _BV(aOversampleExponent); i++) { @@ -132,7 +137,34 @@ uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t a */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished + // Add value + tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here + // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! + } + ADCSRA &= ~_BV(ADATE); // Disable auto-triggering (free running mode) + return (tSumValue >> aOversampleExponent); +} + +/* + * Use ADC_PRESCALE32 which gives 26 us conversion time and good linearity for 16 MHz Arduino + */ +uint16_t readADCChannelWithReferenceOversampleFast(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent) { + uint16_t tSumValue = 0; + ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); + + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE32); + + for (uint8_t i = 0; i < _BV(aOversampleExponent); i++) { + /* + * wait for free running conversion to finish. + * Do not wait for ADSC here, since ADSC is only low for 1 ADC Clock cycle on free running conversion. + */ + loop_until_bit_is_set(ADCSRA, ADIF); + + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // Add value tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! @@ -149,8 +181,8 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t uint16_t tSumValue = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE); for (uint8_t i = 0; i < aNumberOfSamples; i++) { @@ -160,7 +192,7 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // Add value tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! @@ -170,16 +202,17 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t } /* - * use ADC_PRESCALE16 which gives 13 us conversion time and good linearity + * use ADC_PRESCALE32 which gives 26 us conversion time and good linearity + * @return the maximum of aNumberOfSamples measurements. */ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReference, uint16_t aNumberOfSamples) { uint16_t tADCValue = 0; uint16_t tMaximum = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag - ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE16); + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE32); for (uint16_t i = 0; i < aNumberOfSamples; i++) { /* @@ -188,7 +221,7 @@ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReferen */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // check value tADCValue = ADCL | (ADCH << 8); if (tADCValue > tMaximum) { @@ -200,10 +233,10 @@ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReferen } /* - * use ADC_PRESCALE16 which gives 13 us conversion time and good linearity + * use ADC_PRESCALE32 which gives 26 us conversion time and good linearity */ uint16_t readADCChannelWithReferenceMaxMicros(uint8_t aChannelNumber, uint8_t aReference, uint16_t aMicrosecondsToAquire) { - uint16_t tNumberOfSamples = aMicrosecondsToAquire / 13; + uint16_t tNumberOfSamples = aMicrosecondsToAquire / 26; return readADCChannelWithReferenceMax(aChannelNumber, aReference, tNumberOfSamples); } @@ -219,7 +252,7 @@ uint16_t readUntil4ConsecutiveValuesAreEqual(uint8_t aChannelNumber, uint8_t aDe tValues[0] = readADCChannel(aChannelNumber); for (int i = 1; i < 4; ++i) { - delay(aDelay); // only 3 delays! + delay(aDelay); // Only 3 delays! tValues[i] = readADCChannel(aChannelNumber); } @@ -261,30 +294,30 @@ uint16_t readUntil4ConsecutiveValuesAreEqual(uint8_t aChannelNumber, uint8_t aDe } /* - * Versions without handling of switched reference and channel. - * Use only if reference (DEFAULT, INTERNAL) is known to be at the right value (DEFAULT for VCC and INTERNAL for temperature) - * and register ADMUX may be overwritten. - * Use it for example if you only call getVCCVoltageSimple() or getTemperatureSimple() in your program. - * Calling both will lead to wrong values since of reference and channel switching. + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only call getVCCVoltageSimple() or getVCCVoltageMillivoltSimple() in your program. + * !!! Resolution is only 20 millivolt !!! */ float getVCCVoltageSimple(void) { -// use AVCC with external capacitor at AREF pin as reference + // use AVCC with (optional) external capacitor at AREF pin as reference float tVCC = readADCChannelWithReferenceMultiSamples(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT, 4); return ((1023 * 1.1 * 4) / tVCC); } /* - * Will at most times be sufficient since switching reference to default is quite fast. + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only call getVCCVoltageSimple() or getVCCVoltageMillivoltSimple() in your program. + * !!! Resolution is only 20 millivolt !!! */ uint16_t getVCCVoltageMillivoltSimple(void) { -// use AVCC with external capacitor at AREF pin as reference + // use AVCC with external capacitor at AREF pin as reference uint16_t tVCC = readADCChannelWithReferenceMultiSamples(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT, 4); return ((1023L * 1100 * 4) / tVCC); } /* - * Do not check for changing reference or channel. - * Will give wrong result if used at any time after analogRead(); + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only use INTERNAL reference (call getTemperatureSimple()) in your program. */ float getTemperatureSimple(void) { #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) @@ -302,7 +335,8 @@ float getVCCVoltage(void) { /* * Read value of 1.1 volt internal channel using VCC as reference. - * Waits for reference and channel switching. + * Handles reference and channel switching by introducing the appropriate delays. + * !!! Resolution is only 20 millivolt !!! */ uint16_t getVCCVoltageMillivolt(void) { uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT); @@ -314,24 +348,28 @@ uint16_t getVCCVoltageMillivolt(void) { return ((1023L * 1100) / tVCC); } -void printVCCVoltageMillivolt(Print* aSerial) { +void printVCCVoltageMillivolt(Print *aSerial) { aSerial->print(F("VCC=")); aSerial->print(getVCCVoltageMillivolt()); aSerial->println(" mV"); } /* - * Version which restore the ADC Channel and handle reference switching. + * Handles reference and channel switching by introducing the appropriate delays. */ float getTemperature(void) { #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) return 0.0; #else -// use internal 1.1 volt as reference + // use internal 1.1 volt as reference uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(ADC_TEMPERATURE_CHANNEL_MUX, INTERNAL); float tTemp = (readADCChannelWithReferenceOversample(ADC_TEMPERATURE_CHANNEL_MUX, INTERNAL, 2) - 317); ADMUX = tOldADMUX; return (tTemp / 1.22); #endif } +#elif defined(ARDUINO_ARCH_APOLLO3) + void ADCUtilsDummyToAvoidBFDAssertions(){ + ; + } #endif // defined(__AVR__) diff --git a/src/ADCUtils.h b/src/ADCUtils.h index 1e51918..c0fe1b2 100644 --- a/src/ADCUtils.h +++ b/src/ADCUtils.h @@ -1,7 +1,7 @@ /* * ADCUtils.h * - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Copyright (C) 2016-2021 Armin Joachimsmeyer * Email: armin.joachimsmeyer@gmail.com * * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. @@ -26,14 +26,15 @@ #if defined(__AVR__) && (! defined(__AVR_ATmega4809__)) #include +#if defined(ADATE) // PRESCALE4 => 13 * 4 = 52 microseconds per ADC conversion at 1 MHz Clock => 19,2 kHz #define ADC_PRESCALE2 1 // 26 microseconds per ADC conversion at 1 MHz #define ADC_PRESCALE4 2 // 52 microseconds per ADC conversion at 1 MHz // PRESCALE8 => 13 * 8 = 104 microseconds per ADC sample at 1 MHz Clock => 9,6 kHz #define ADC_PRESCALE8 3 // 104 microseconds per ADC conversion at 1 MHz -#define ADC_PRESCALE16 4 // 208 microseconds per ADC conversion at 1 MHz -#define ADC_PRESCALE32 5 // 416 microseconds per ADC conversion at 1 MHz +#define ADC_PRESCALE16 4 // 13/208 microseconds per ADC conversion at 16/1 MHz - degradations in linearity at 16 MHz +#define ADC_PRESCALE32 5 // 26/416 microseconds per ADC conversion at 16/1 MHz - very good linearity at 16 MHz #define ADC_PRESCALE64 6 // 52 microseconds per ADC conversion at 16 MHz #define ADC_PRESCALE128 7 // 104 microseconds per ADC conversion at 16 MHz --- Arduino default @@ -102,12 +103,16 @@ #define ADC_1_1_VOLT_CHANNEL_MUX 0x1E #define ADC_GND_CHANNEL_MUX 0x1F #define INTERNAL INTERNAL1V1 + +#else +#error "No temperature channel definitions specified for this AVR CPU" #endif uint16_t readADCChannel(uint8_t aChannelNumber); uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference); uint16_t readADCChannelWithOversample(uint8_t aChannelNumber, uint8_t aOversampleExponent); uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent); +uint16_t readADCChannelWithReferenceOversampleFast(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent); uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t aReference, uint8_t aNumberOfSamples); uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReference, uint16_t aNumberOfSamples); uint16_t readADCChannelWithReferenceMaxMicros(uint8_t aChannelNumber, uint8_t aReference, uint16_t aMicrosecondsToAquire); @@ -124,6 +129,7 @@ uint16_t getVCCVoltageMillivolt(void); void printVCCVoltageMillivolt(Print* aSerial); float getTemperature(void); +#endif // defined(ADATE) #endif // defined(__AVR__) #endif /* SRC_ADCUTILS_H_ */ diff --git a/src/AutonomousDrive.cpp b/src/AutonomousDrive.cpp index 55bb579..f9894cf 100644 --- a/src/AutonomousDrive.cpp +++ b/src/AutonomousDrive.cpp @@ -26,9 +26,10 @@ #include "RobotCar.h" #include "RobotCarGui.h" +#include "Distance.h" #include "HCSR04.h" -#include "Distance.h" +#include "pitches.h" uint8_t sDriveMode = MODE_MANUAL_DRIVE; // one of MODE_MANUAL_DRIVE, MODE_AUTONOMOUS_DRIVE_BUILTIN, MODE_AUTONOMOUS_DRIVE_USER or MODE_FOLLOWER @@ -97,7 +98,7 @@ void startStopAutomomousDrive(bool aDoStart, uint8_t aDriveMode) { #endif DistanceServoWriteAndDelay(90); sDriveMode = MODE_MANUAL_DRIVE; - RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + RobotCarMotorControl.stop(MOTOR_RELEASE); } // manage on off buttons @@ -141,11 +142,10 @@ void driveAutonomousOneStep() { */ if (sNextDegreesToTurn == GO_BACK_AND_SCAN_AGAIN) { // go backwards and do a new scan - RobotCarMotorControl.goDistanceCentimeter(10, DIRECTION_BACKWARD, &loopGUI); + RobotCarMotorControl.goDistanceMillimeter(100, DIRECTION_BACKWARD, &loopGUI); } else { // rotate and go - RobotCarMotorControl.rotateCar(sNextDegreesToTurn, sTurnMode, true, &loopGUI); -// RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_FORWARD, true, &loopGUI); + RobotCarMotorControl.rotate(sNextDegreesToTurn, sTurnMode, &loopGUI); // wait to really stop after turning delay(100); sLastDegreesTurned = sNextDegreesToTurn; @@ -157,13 +157,13 @@ void driveAutonomousOneStep() { */ if (sStepMode == MODE_SINGLE_STEP) { // Go fixed distance - RobotCarMotorControl.goDistanceCentimeter(sCentimeterPerScan, DIRECTION_FORWARD, &loopGUI); + RobotCarMotorControl.goDistanceMillimeter(sCentimeterPerScan*10, DIRECTION_FORWARD, &loopGUI); } else /* * Continuous mode, start car or let it run */ if (tCarIsStopped) { - RobotCarMotorControl.startRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD, &loopGUI); + RobotCarMotorControl.startRampUpAndWaitForDriveSpeedPWM(DIRECTION_FORWARD, &loopGUI); } } @@ -204,7 +204,7 @@ void driveAutonomousOneStep() { char tStringBuffer[6]; sprintf_P(tStringBuffer, PSTR("%2d%s"), sCentimeterPerScan, "cm"); BlueDisplay1.drawText(0, BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND, tStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); } } @@ -215,7 +215,7 @@ void driveAutonomousOneStep() { /* * Stop if rotation requested or single step */ - RobotCarMotorControl.stopCarAndWaitForIt(); + RobotCarMotorControl.stopAndWaitForIt(); #ifdef USE_ENCODER_MOTOR_CONTROL #ifdef ENABLE_PATH_INFO_PAGE @@ -303,11 +303,11 @@ int doBuiltInCollisionDetection() { ***************************************************/ void checkSpeedAndGo(unsigned int aSpeed, uint8_t aRequestedDirection) { - if (aSpeed > RobotCarMotorControl.rightCarMotor.DriveSpeed * 2) { - aSpeed = RobotCarMotorControl.rightCarMotor.DriveSpeed * 2; + if (aSpeed > RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * 2) { + aSpeed = RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * 2; } - if (aSpeed > MAX_SPEED) { - aSpeed = MAX_SPEED; + if (aSpeed > MAX_SPEED_PWM) { + aSpeed = MAX_SPEED_PWM; } RobotCarMotorControl.startRampUpAndWait(aSpeed, aRequestedDirection, &loopGUI); @@ -343,8 +343,7 @@ void driveFollowerModeOneStep() { /* * we had a pending turn */ - RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_FORWARD, true); - //RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_IN_PLACE, true); + RobotCarMotorControl.rotate(sNextDegreesToTurn, TURN_FORWARD); sNextDegreesToTurn = 0; } else { @@ -357,7 +356,7 @@ void driveFollowerModeOneStep() { if (tCentimeter > FOLLOWER_DISTANCE_TARGET_SCAN_CENTIMETER) { // trigger scanning in the next loop // Stop car, clear display area and show distance - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); clearPrintedForwardDistancesInfos(); // show current distance (as US distance), which triggers the scan showUSDistance(tCentimeter, true); @@ -376,20 +375,20 @@ void driveFollowerModeOneStep() { // if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_FORWARD) { // Serial.println(F("Go forward")); // } - tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (tCentimeter - FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER) * 2; + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeedPWM + (tCentimeter - FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER) * 2; checkSpeedAndGo(tSpeed, DIRECTION_FORWARD); } else if (tCentimeter < FOLLOWER_DISTANCE_MINIMUM_CENTIMETER) { // if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_BACKWARD) { // Serial.println(F("Go backward")); // } - tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (FOLLOWER_DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeedPWM + (FOLLOWER_DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; checkSpeedAndGo(tSpeed, DIRECTION_BACKWARD); } else { if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != MOTOR_RELEASE) { // Serial.println(F("Stop")); - RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + RobotCarMotorControl.stop(MOTOR_RELEASE); } } } @@ -401,11 +400,11 @@ unsigned int __attribute__((weak)) getDistanceAndPlayTone() { /* * Get distance; timeout is 1 meter */ - unsigned int tCentimeter = getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM_FOLLOWER); + unsigned int tCentimeter = getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM_FOLLOWER, true); /* * play tone */ - int tFrequency = map(tCentimeter, 0, 100, 100, 2000); - tone(PIN_BUZZER, tFrequency); + uint8_t tIndex = map(tCentimeter, 0, 100, 0, ARRAY_SIZE_NOTE_C5_TO_C7_PENTATONIC - 1); + tone(PIN_BUZZER, NoteC5ToC7Pentatonic[tIndex]); return tCentimeter; } diff --git a/src/AutonomousDrivePage.cpp b/src/AutonomousDrivePage.cpp index 0e61dd4..3dbb3d7 100644 --- a/src/AutonomousDrivePage.cpp +++ b/src/AutonomousDrivePage.cpp @@ -68,7 +68,7 @@ void setStepModeButtonCaption(); */ void setStepMode(uint8_t aStepMode) { if (aStepMode == MODE_SINGLE_STEP) { - RobotCarMotorControl.stopCarAndWaitForIt(); + RobotCarMotorControl.stopAndWaitForIt(); } else if (aStepMode > MODE_SINGLE_STEP) { aStepMode = MODE_CONTINUOUS; } @@ -161,45 +161,46 @@ void handleAutomomousDriveRadioButtons() { } void initAutonomousDrivePage(void) { - TouchButtonStepMode.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6 + 1, COLOR_BLUE, + TouchButtonStepMode.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6 + 1, COLOR16_BLUE, reinterpret_cast(sStepModeButtonStringContinuousStepToTurn), TEXT_SIZE_9, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doNextStepMode); - TouchButtonSingleScan.init(0, BUTTON_HEIGHT_6_LINE_2, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_BLUE, F("Scan"), + TouchButtonSingleScan.init(0, BUTTON_HEIGHT_6_LINE_2, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_BLUE, F("Scan"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doSingleScan); - TouchButtonStep.init(0, BUTTON_HEIGHT_6_LINE_3, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_BLUE, F("Step"), + TouchButtonStep.init(0, BUTTON_HEIGHT_6_LINE_3, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_BLUE, F("Step"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doStep); // use sDriveMode for reconnect during demo mode - TouchButtonFollower.init(0, BUTTON_HEIGHT_6_LINE_4, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_RED, F("Follow"), - TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_FOLLOWER), - &doStartStopFollowerMode); + TouchButtonStartStopUserAutonomousDrive.init(0, BUTTON_HEIGHT_6_LINE_4, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_RED, F("Start\nUser"), + TEXT_SIZE_13, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_AUTONOMOUS_DRIVE_USER), + &doStartStopTestUser); + TouchButtonStartStopUserAutonomousDrive.setCaptionForValueTrue(F("Stop\nUser")); TouchButtonScanSpeed.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Scan slow"), TEXT_SIZE_16, + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Scan slow"), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doChangeScanSpeed); TouchButtonScanSpeed.setCaptionForValueTrue("Scan fast"); #if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR) TouchButtonScanMode.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_RED, reinterpret_cast(sScanModeButtonStringMinMax), + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_RED, reinterpret_cast(sScanModeButtonStringMinMax), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, SCAN_MODE_MINIMUM, &doScanMode); #endif // use sDriveMode for reconnect during demo mode - TouchButtonStartStopBuiltInAutonomousDrive.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, + 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_AUTONOMOUS_DRIVE_BUILTIN), &doStartStopAutomomousDrive); TouchButtonStartStopBuiltInAutonomousDrive.setCaptionForValueTrue(F("Stop")); - TouchButtonStartStopUserAutonomousDrive.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, - COLOR_RED, F("Start\nUser"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, - &doStartStopTestUser); - TouchButtonStartStopUserAutonomousDrive.setCaptionForValueTrue(F("Stop\nUser")); + 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")); #ifdef ENABLE_PATH_INFO_PAGE - TouchButtonPathInfoPage.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR_RED, F("Show\nPath"), TEXT_SIZE_22, + TouchButtonPathInfoPage.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR16_RED, F("Show\nPath"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_SHOW_PATH, &GUISwitchPages); #endif } @@ -254,7 +255,7 @@ void stopAutonomousDrivePage(void) { */ void clearPrintedForwardDistancesInfos() { BlueDisplay1.fillRect(BUTTON_WIDTH_3_5 + 1, BUTTON_HEIGHT_4 + 1, LAYOUT_320_WIDTH, - BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER) - 1, COLOR_WHITE); + BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER) - 1, COLOR16_WHITE); } /* @@ -273,15 +274,15 @@ void drawForwardDistancesInfos() { * Determine color */ uint8_t tDistance = sForwardDistancesInfo.RawDistancesArray[i]; - tColor = COLOR_ORANGE; + tColor = COLOR16_ORANGE; if (tDistance >= DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE) { tDistance = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE; - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } if (tDistance > sCentimeterPerScanTimesTwo) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tDistance < sCentimeterPerScan) { - tColor = COLOR_RED; + tColor = COLOR16_RED; } /* @@ -298,15 +299,15 @@ void drawForwardDistancesInfos() { void drawCollisionDecision(int aDegreeToTurn, uint8_t aLengthOfVector, bool aDoClearVector) { if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - color16_t tColor = COLOR_BLUE; + color16_t tColor = COLOR16_BLUE; int tDegreeToDisplay = aDegreeToTurn; if (tDegreeToDisplay == 180) { - tColor = COLOR_RED; + tColor = COLOR16_RED; tDegreeToDisplay = 0; } if (aDoClearVector) { - tColor = COLOR_WHITE; + tColor = COLOR16_WHITE; } BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, aLengthOfVector, tDegreeToDisplay + 90, @@ -315,7 +316,7 @@ void drawCollisionDecision(int aDegreeToTurn, uint8_t aLengthOfVector, bool aDoC sprintf_P(sStringBuffer, PSTR("wall%4d\xB0 rotation: %3d\xB0 wall%4d\xB0"), sForwardDistancesInfo.WallLeftAngleDegrees, aDegreeToTurn, sForwardDistancesInfo.WallRightAngleDegrees); // \xB0 is degree character BlueDisplay1.drawText(BUTTON_WIDTH_3_5_POS_2, US_DISTANCE_MAP_ORIGIN_Y + TEXT_SIZE_11, sStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); } } } diff --git a/src/BTSensorDrivePage.cpp b/src/BTSensorDrivePage.cpp index 5419664..f6edd6d 100644 --- a/src/BTSensorDrivePage.cpp +++ b/src/BTSensorDrivePage.cpp @@ -23,26 +23,25 @@ #include "RobotCar.h" #include "RobotCarGui.h" -//#pragma GCC diagnostic ignored "-Wunused-parameter" - // 4 Sliders for accelerometer BDSlider SliderForward; // Y negative BDSlider SliderBackward; // Y positive BDSlider SliderRight; // X negative BDSlider SliderLeft; // X positive -#define SLIDER_BACKGROUND_COLOR COLOR_YELLOW -#define SLIDER_BAR_COLOR COLOR_GREEN -#define SLIDER_THRESHOLD_COLOR COLOR_BLUE +#define SLIDER_BACKGROUND_COLOR COLOR16_YELLOW +#define SLIDER_BAR_COLOR COLOR16_GREEN +#define SLIDER_THRESHOLD_COLOR COLOR16_BLUE #define SENSOR_SLIDER_WIDTH (DISPLAY_WIDTH / 16) #define VERTICAL_SLIDER_LENTGH ((DISPLAY_HEIGHT / 4) + (DISPLAY_HEIGHT / 10)) -#define SLIDER_SPEED_THRESHOLD DEFAULT_START_SPEED -#define SPEED_DEAD_BAND (DEFAULT_START_SPEED / 2) +#define SLIDER_SPEED_THRESHOLD DEFAULT_START_SPEED_PWM +#define SPEED_SENSOR_DEAD_BAND 20 +#define SPEED_DEAD_BAND (DEFAULT_START_SPEED_PWM / 2) #define HORIZONTAL_SLIDER_LENTGH (DISPLAY_HEIGHT / 4) #define SLIDER_LEFT_RIGHT_THRESHOLD (HORIZONTAL_SLIDER_LENTGH / 4) -#define LEFT_RIGHT_DEAD_BAND 4 +#define LEFT_RIGHT_SENSOR_DEAD_BAND 10 #define SENSOR_SLIDER_CENTER_X ((DISPLAY_WIDTH - SENSOR_SLIDER_WIDTH) / 2) #define SENSOR_SLIDER_CENTER_Y ((DISPLAY_HEIGHT / 2) + (DISPLAY_HEIGHT / 16)) @@ -55,18 +54,72 @@ uint8_t sSensorChangeCallCountForZeroAdjustment; float sYZeroValueAdded; // The accumulator for the values of the first 8 calls. float sYZeroValue = 0; -int sLastSpeedValue = 0; -int sLastLeftRightValue = 0; +#if (VERSION_BLUE_DISPLAY_MAJOR <= 2) && (VERSION_BLUE_DISPLAY_MINOR <= 1) +// positiveNegativeSlider is available since 2.2 +/* + * To show a signed value on two sliders positioned back to back (one of it is inverse or has a negative length value) + */ +struct positiveNegativeSlider { + BDSlider *positiveSliderPtr; + BDSlider *negativeSliderPtr; + int lastSliderValue; // positive value with sensor dead band applied + BDSlider *lastZeroSlider; // to decide if we draw new zero slider +}; +/* + * @return aValue with aSliderDeadBand applied + */ +int setPositiveNegativeSliders(struct positiveNegativeSlider *aSliderStructPtr, int aValue, uint8_t aSliderDeadBand) { + BDSlider *tValueSlider = aSliderStructPtr->positiveSliderPtr; + BDSlider *tZeroSlider = aSliderStructPtr->negativeSliderPtr; + if (aValue < 0) { + aValue = -aValue; + tValueSlider = tZeroSlider; + tZeroSlider = aSliderStructPtr->positiveSliderPtr; + } -uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeed) { - // overflow handling since analogWrite only accepts byte values - if (aSpeed > 0xFF) { - aSpeed = 0xFF; + /* + * Now we have a positive value for dead band and slider length + */ + if (aValue > aSliderDeadBand) { + // dead band subtraction -> resulting values start at 0 + aValue -= aSliderDeadBand; + } else { + aValue = 0; } - if (aSpeed <= SPEED_DEAD_BAND) { - aSpeed = 0; + + /* + * Draw slider value if values changed + */ + if (aSliderStructPtr->lastSliderValue != aValue) { + aSliderStructPtr->lastSliderValue = aValue; + tValueSlider->setValueAndDrawBar(aValue); + if (aSliderStructPtr->lastZeroSlider != tZeroSlider) { + aSliderStructPtr->lastZeroSlider = tZeroSlider; + // the sign has changed, clear old value + tZeroSlider->setValueAndDrawBar(0); + } + } + + /* + * Restore sign for aValue with dead band applied + */ + if (tZeroSlider == aSliderStructPtr->positiveSliderPtr) { + aValue = -aValue; } - return aSpeed; + return aValue; +} +#endif + +struct positiveNegativeSlider sAccelerationLeftRightSliders; +struct positiveNegativeSlider sAccelerationForwardBackwardSliders; + +uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeedPWM) { + aSpeedPWM += SPEED_DEAD_BAND; + // overflow handling since analogWrite only accepts byte values + if (aSpeedPWM > MAX_SPEED_PWM) { + aSpeedPWM = MAX_SPEED_PWM; + } + return aSpeedPWM; } /* @@ -79,13 +132,15 @@ uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeed) { * positive -> left down * negative -> right down */ -void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallbackInfo) { +void doSensorChange(uint8_t aSensorType, struct SensorCallback *aSensorCallbackInfo) { + (void) aSensorType; // to avoid -Wunused-parameter + if (sSensorChangeCallCountForZeroAdjustment < CALLS_FOR_ZERO_ADJUSTMENT) { if (sSensorChangeCallCountForZeroAdjustment == 0) { // init values sYZeroValueAdded = 0; } - // Add for zero adjustment + // Sum for zero adjustment sYZeroValueAdded += aSensorCallbackInfo->ValueY; sSensorChangeCallCountForZeroAdjustment++; } else if (sSensorChangeCallCountForZeroAdjustment == CALLS_FOR_ZERO_ADJUSTMENT) { @@ -95,69 +150,43 @@ void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallback sYZeroValue = sYZeroValueAdded / CALLS_FOR_ZERO_ADJUSTMENT; BlueDisplay1.playTone(24); // feedback for zero value acquired } else { + /* * regular operation here * left right handling */ - // Scale value - int tLeftRightValue = aSensorCallbackInfo->ValueX * 8.0; - - BDSlider * tValueSlider = &SliderLeft; - BDSlider * tZeroSlider = &SliderRight; - if (tLeftRightValue < 0) { - tLeftRightValue = -tLeftRightValue; - tValueSlider = &SliderRight; - tZeroSlider = &SliderLeft; - } - if (tLeftRightValue > LEFT_RIGHT_DEAD_BAND) { - // dead band subtraction - tLeftRightValue -= LEFT_RIGHT_DEAD_BAND; - } else { - tLeftRightValue = 0; - } +#ifdef CAR_HAS_4_WHEELS + int tLeftRightValue = aSensorCallbackInfo->ValueX * 12.0; +#else + int tLeftRightValue = aSensorCallbackInfo->ValueX * 8.0; // Scale value +#endif + tLeftRightValue = setPositiveNegativeSliders(&sAccelerationLeftRightSliders, tLeftRightValue, LEFT_RIGHT_SENSOR_DEAD_BAND); - if (sLastLeftRightValue != tLeftRightValue) { - sLastLeftRightValue = tLeftRightValue; - - tValueSlider->setValueAndDrawBar(tLeftRightValue); - tZeroSlider->setValueAndDrawBar(0); - } + /* + * forward backward handling + */ + int tSpeedPWMValue = -((aSensorCallbackInfo->ValueY - sYZeroValue) * (MAX_SPEED_PWM / 10)); // Scale value + tSpeedPWMValue = setPositiveNegativeSliders(&sAccelerationForwardBackwardSliders, tSpeedPWMValue, SPEED_SENSOR_DEAD_BAND); /* - * restore sign for speed setting below + * Print speed as value of bottom slider */ - if (tZeroSlider == &SliderLeft) { - tLeftRightValue = -tLeftRightValue; - } + sprintf(sStringBuffer, "%4d", tSpeedPWMValue); + SliderBackward.printValue(sStringBuffer); /* - * forward backward handling + * Get direction */ - int tSpeedValue = -((aSensorCallbackInfo->ValueY - sYZeroValue) * (MAX_SPEED / 7)); - if (sLastSpeedValue != tSpeedValue) { - sLastSpeedValue = tSpeedValue; - if (tSpeedValue >= 0) { - // Forward - tSpeedValue = speedOverflowAndDeadBandHandling(tSpeedValue); - - RobotCarMotorControl.setSpeedCompensated(tSpeedValue, DIRECTION_FORWARD, tLeftRightValue); - SliderBackward.setValueAndDrawBar(0); - SliderForward.setValueAndDrawBar(tSpeedValue); - - } else { - // Backward - tSpeedValue = speedOverflowAndDeadBandHandling(-tSpeedValue); - - RobotCarMotorControl.setSpeedCompensated(tSpeedValue, DIRECTION_BACKWARD, tLeftRightValue); - SliderForward.setValueAndDrawBar(0); - SliderBackward.setValueAndDrawBar(tSpeedValue); - } - /* - * Print speed as value of bottom slider - */ - sprintf(sStringBuffer, "%3d", tSpeedValue); - SliderBackward.printValue(sStringBuffer); + uint8_t tDirection = DIRECTION_FORWARD; + if (tSpeedPWMValue < 0) { + tSpeedPWMValue = -tSpeedPWMValue; + tDirection = DIRECTION_BACKWARD; } + + RobotCarMotorControl.rightCarMotor.setSpeedPWMCompensated(speedOverflowAndDeadBandHandling(tSpeedPWMValue + tLeftRightValue), + tDirection); + RobotCarMotorControl.leftCarMotor.setSpeedPWMCompensated(speedOverflowAndDeadBandHandling(tSpeedPWMValue - tLeftRightValue), + tDirection); } } @@ -176,8 +205,10 @@ void initBTSensorDrivePage(void) { SliderBackward.init(SENSOR_SLIDER_CENTER_X, SENSOR_SLIDER_CENTER_Y, SENSOR_SLIDER_WIDTH, -(VERTICAL_SLIDER_LENTGH), SLIDER_SPEED_THRESHOLD, 0, SLIDER_BACKGROUND_COLOR, SLIDER_BAR_COLOR, FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); // SliderBackward.setBarThresholdColor(SLIDER_THRESHOLD_COLOR); - SliderForward.setScaleFactor((float) MAX_SPEED / (float) VERTICAL_SLIDER_LENTGH); - SliderBackward.setScaleFactor((float) MAX_SPEED / (float) VERTICAL_SLIDER_LENTGH); + SliderForward.setScaleFactor((float) MAX_SPEED_PWM / (float) VERTICAL_SLIDER_LENTGH); + SliderBackward.setScaleFactor((float) MAX_SPEED_PWM / (float) VERTICAL_SLIDER_LENTGH); + sAccelerationForwardBackwardSliders.positiveSliderPtr = &SliderForward; + sAccelerationForwardBackwardSliders.negativeSliderPtr = &SliderBackward; // Position slider right from at middle of screen SliderRight.init(SENSOR_SLIDER_CENTER_X + SENSOR_SLIDER_WIDTH, SENSOR_SLIDER_CENTER_Y - (SENSOR_SLIDER_WIDTH / 2), @@ -192,6 +223,8 @@ void initBTSensorDrivePage(void) { // SliderLeft.setBarThresholdColor(SLIDER_THRESHOLD_COLOR); SliderRight.setScaleFactor(1.2); SliderLeft.setScaleFactor(1.2); + sAccelerationLeftRightSliders.positiveSliderPtr = &SliderLeft; + sAccelerationLeftRightSliders.negativeSliderPtr = &SliderRight; } void drawBTSensorDrivePage(void) { @@ -207,6 +240,7 @@ void drawBTSensorDrivePage(void) { } void startBTSensorDrivePage(void) { + doReset(NULL, 0); drawBTSensorDrivePage(); } diff --git a/src/CarMotorControl.cpp b/src/CarMotorControl.cpp deleted file mode 100644 index 6775907..0000000 --- a/src/CarMotorControl.cpp +++ /dev/null @@ -1,615 +0,0 @@ -/* - * CarMotorControl.cpp - * - * Contains functions for control of the 2 motors of a car like setDirection, goDistanceCentimeter() and rotateCar(). - * Checks input of PIN aPinFor2WDDetection since we need different factors for rotating a 4 wheel and a 2 wheel car. - * - * Requires EncoderMotor.cpp - * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include -#include "CarMotorControl.h" - -//#define DEBUG // Only for development - -CarMotorControl * sCarMotorControlPointerForISR; - -CarMotorControl::CarMotorControl() { // @suppress("Class members should be properly initialized") - sCarMotorControlPointerForISR = this; -} - -#ifdef USE_ADAFRUIT_MOTOR_SHIELD -void CarMotorControl::init(bool aReadFromEeprom) { - leftCarMotor.init(1, aReadFromEeprom); - rightCarMotor.init(2, aReadFromEeprom); - -#if defined(CAR_HAS_4_WHEELS) - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT; -#else - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT; -#endif - -# ifdef USE_ENCODER_MOTOR_CONTROL - /* - * For slot type optocoupler interrupts on pin PD2 + PD3 - */ - EncoderMotor::enableINT0AndINT1Interrupts(); -# endif -} - -#else -void CarMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, - uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, bool aReadFromEeprom) { - if (aReadFromEeprom) { - leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin, 1); - rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin, 2); - } else { - leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin); - rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin); - } - - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_DEFAULT; - -# ifdef USE_ENCODER_MOTOR_CONTROL - /* - * For slot type optocoupler interrupts on pin PD2 + PD3 - */ - EncoderMotor::enableINT0AndINT1Interrupts(); -# endif -} -#endif - -/* - * Sets default values for min and max speed, factor for distance to time conversion for non encoder motors and reset speed compensation - * Is called automatically at init if parameter aReadFromEeprom is set to false - */ -void CarMotorControl::setDefaultsForFixedDistanceDriving() { - rightCarMotor.setDefaultsForFixedDistanceDriving(); - leftCarMotor.setDefaultsForFixedDistanceDriving(); -} - -/** - * @param aSpeedCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. - * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. - */ -void CarMotorControl::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight) { - if (aSpeedCompensationRight > 0) { - rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, aSpeedCompensationRight); - leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, 0); - } else { - rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, 0); - leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, -aSpeedCompensationRight); - } -} - -/** - * @param aSpeedCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. - * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. - */ -void CarMotorControl::setSpeedCompensation(int8_t aSpeedCompensationRight) { - if (aSpeedCompensationRight > 0) { - if (leftCarMotor.SpeedCompensation >= aSpeedCompensationRight) { - leftCarMotor.SpeedCompensation -= aSpeedCompensationRight; - } else { - rightCarMotor.SpeedCompensation += aSpeedCompensationRight; - } - } else { - aSpeedCompensationRight = -aSpeedCompensationRight; - if (rightCarMotor.SpeedCompensation >= aSpeedCompensationRight) { - rightCarMotor.SpeedCompensation -= aSpeedCompensationRight; - } else { - leftCarMotor.SpeedCompensation += aSpeedCompensationRight; - } - } - PWMDcMotor::MotorValuesHaveChanged = true; -} - -void CarMotorControl::setDriveSpeed(uint8_t aDriveSpeed) { - rightCarMotor.setDriveSpeed(aDriveSpeed); - leftCarMotor.setDriveSpeed(aDriveSpeed); -} - -/* - * @return true if direction has changed and motor has stopped - */ -bool CarMotorControl::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { - bool tReturnValue = false; - uint8_t tRequestedDirection = aRequestedDirection & DIRECTION_MASK; - if (CarDirectionOrBrakeMode != tRequestedDirection) { -#ifdef DEBUG - Serial.print(F("Motor mode change to ")); - Serial.println(tRequestedDirection); -#endif - uint8_t tMaxSpeed = max(rightCarMotor.CurrentSpeed, leftCarMotor.CurrentSpeed); - if (tMaxSpeed > 0) { - /* - * Direction change requested but motor still running-> first stop motor - */ -#ifdef DEBUG - Serial.println(F("First stop motor and wait")); -#endif - stopMotors(MOTOR_BRAKE); - delay(tMaxSpeed / 2); // to let motors stop - tReturnValue = true; - } - CarDirectionOrBrakeMode = tRequestedDirection; // The only statement which changes CarDirectionOrBrakeMode to DIRECTION_FORWARD or DIRECTION_BACKWARD - } - return tReturnValue; -} - -/* - * Direct motor control, no state or flag handling - */ -void CarMotorControl::setSpeed(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.setSpeed(aRequestedSpeed, aRequestedDirection); - leftCarMotor.setSpeed(aRequestedSpeed, aRequestedDirection); -} - -/* - * Sets speed adjusted by current compensation value and handle motor state and flags - */ -void CarMotorControl::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -} - -/* - * Sets speed adjusted by current compensation value and handle motor state and flags - * @param aLeftRightSpeed if positive, this value is subtracted from the left motor value, if negative subtracted from the right motor value - * - */ -void CarMotorControl::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, int8_t aLeftRightSpeed) { - checkAndHandleDirectionChange(aRequestedDirection); -#ifdef USE_ENCODER_MOTOR_CONTROL - EncoderMotor * tMotorWithModifiedSpeed; -#else - PWMDcMotor * tMotorWithModifiedSpeed; -#endif - if (aLeftRightSpeed >= 0) { - rightCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - tMotorWithModifiedSpeed = &leftCarMotor; - } else { - aLeftRightSpeed = -aLeftRightSpeed; - leftCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - tMotorWithModifiedSpeed = &rightCarMotor; - } - - if (aRequestedSpeed >= aLeftRightSpeed) { - tMotorWithModifiedSpeed->setSpeedCompensated(aRequestedSpeed - aLeftRightSpeed, aRequestedDirection); - } else { - tMotorWithModifiedSpeed->setSpeedCompensated(0, aRequestedDirection); - } -} - -/* - * Direct motor control, no state or flag handling - */ -void CarMotorControl::setSpeed(int aRequestedSpeed) { - rightCarMotor.setSpeed(aRequestedSpeed); - leftCarMotor.setSpeed(aRequestedSpeed); -} - -/* - * Sets signed speed adjusted by current compensation value and handle motor state and flags - */ -void CarMotorControl::setSpeedCompensated(int aRequestedSpeed) { - rightCarMotor.setSpeedCompensated(aRequestedSpeed); - leftCarMotor.setSpeedCompensated(aRequestedSpeed); -} - -uint8_t CarMotorControl::getCarDirectionOrBrakeMode() { - return CarDirectionOrBrakeMode;; -} - -void CarMotorControl::writeMotorvaluesToEeprom(){ - rightCarMotor.writeMotorvaluesToEeprom(); - leftCarMotor.writeMotorvaluesToEeprom(); -} - -/* - * Stop car - * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE - */ -void CarMotorControl::stopMotors(uint8_t aStopMode) { - CarDirectionOrBrakeMode = aStopMode; - rightCarMotor.stop(aStopMode); - leftCarMotor.stop(aStopMode); -} - -/* - * @param aStopMode MOTOR_BRAKE or MOTOR_RELEASE - */ -void CarMotorControl::setStopMode(uint8_t aStopMode) { - rightCarMotor.setStopMode(aStopMode); - leftCarMotor.setStopMode(aStopMode); -} - -/* - * Stop car and reset all control values as speed, distances, debug values etc. to 0x00 - */ -void CarMotorControl::resetControlValues() { -#ifdef USE_ENCODER_MOTOR_CONTROL - rightCarMotor.resetControlValues(); - leftCarMotor.resetControlValues(); -#endif -} - -/* - * If motor is accelerating or decelerating then updateMotor needs to be called at a fast rate otherwise it will not work correctly - * Used to suppress time consuming display of motor values - */ -bool CarMotorControl::isStateRamp() { -#ifdef SUPPORT_RAMP_UP - return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP - || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP); -#else - return false; -#endif -} - -/* - * @return true if not stopped (motor expects another update) - */ -bool CarMotorControl::updateMotors() { - bool tMotorsNotStopped = rightCarMotor.updateMotor(); - tMotorsNotStopped |= leftCarMotor.updateMotor(); - return tMotorsNotStopped; -} - -void CarMotorControl::delayAndUpdateMotors(unsigned int aDelayMillis) { - uint32_t tStartMillis = millis(); - do { - updateMotors(); - } while (millis() - tStartMillis <= aDelayMillis); -} - -#ifdef SUPPORT_RAMP_UP -void CarMotorControl::startRampUp(uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startRampUp(aRequestedDirection); - leftCarMotor.startRampUp(aRequestedDirection); -} - -void CarMotorControl::startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startRampUp(aRequestedSpeed, aRequestedDirection); - leftCarMotor.startRampUp(aRequestedSpeed, aRequestedDirection); -} - -/* - * Blocking wait until both motors are at drive speed. 256 milliseconds for ramp up. - */ -void CarMotorControl::waitForDriveSpeed(void (*aLoopCallback)(void)) { - - bool tMotorsNotStopped; - do { - if (aLoopCallback != NULL) { - aLoopCallback(); - } - tMotorsNotStopped = rightCarMotor.updateMotor(); - tMotorsNotStopped |= leftCarMotor.updateMotor(); - } while (tMotorsNotStopped - && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED)); -} -#endif - -/* - * If ramp up is not supported, this functions just sets the speed and returns immediately. - * 256 milliseconds for ramp up. - */ -void CarMotorControl::startRampUpAndWait(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { -#ifdef SUPPORT_RAMP_UP - startRampUp(aRequestedSpeed, aRequestedDirection); - waitForDriveSpeed(aLoopCallback); -#else - checkAndHandleDirectionChange(aRequestedDirection); - (void) aLoopCallback; - rightCarMotor.setSpeedCompensated(aDriveSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(aDriveSpeed, aRequestedDirection); -#endif -} - -void CarMotorControl::startRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { -#ifdef SUPPORT_RAMP_UP - startRampUp(aRequestedDirection); - waitForDriveSpeed(aLoopCallback); -#else - (void) aLoopCallback; - rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection); -#endif -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - * Motor is started by the first call to updateMotors(). - */ -void CarMotorControl::startGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); - leftCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - */ -void CarMotorControl::startGoDistanceCentimeter(uint8_t aRequestedSpeed, unsigned int aDistanceCentimeter, - uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceCount(aRequestedSpeed, aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, - aRequestedDirection); - leftCarMotor.startGoDistanceCount(aRequestedSpeed, aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, - aRequestedDirection); -} - -void CarMotorControl::goDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection, - void (*aLoopCallback)(void)) { - startGoDistanceCentimeter(aDistanceCentimeter, aRequestedDirection); - waitUntilCarStopped(aLoopCallback); -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - */ -void CarMotorControl::startGoDistanceCentimeter(int aDistanceCentimeter) { - rightCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); - leftCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); -} -/** - * Wait until distance is reached - * @param aLoopCallback called until car has stopped to avoid blocking - */ -void CarMotorControl::goDistanceCentimeter(int aDistanceCentimeter, void (*aLoopCallback)(void)) { - startGoDistanceCentimeter(aDistanceCentimeter); - waitUntilCarStopped(aLoopCallback); -} - -/* - * Stop car with ramp and give DistanceCountAfterRampUp counts for braking. - */ -void CarMotorControl::stopCarAndWaitForIt(void (*aLoopCallback)(void)) { - if (isStopped()) { - return; - } -#if defined(USE_ENCODER_MOTOR_CONTROL) && defined(SUPPORT_RAMP_UP) - /* - * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN - * Use DistanceCountAfterRampUp as ramp down count - */ - rightCarMotor.NextChangeMaxTargetCount = rightCarMotor.EncoderCount; - rightCarMotor.TargetDistanceCount = rightCarMotor.EncoderCount + rightCarMotor.DistanceCountAfterRampUp; - leftCarMotor.NextChangeMaxTargetCount = leftCarMotor.EncoderCount; - leftCarMotor.TargetDistanceCount = leftCarMotor.EncoderCount + leftCarMotor.DistanceCountAfterRampUp; - /* - * blocking wait for stop - */ - waitUntilCarStopped(aLoopCallback); -#else - (void) aLoopCallback; - rightCarMotor.stop(); - leftCarMotor.stop(); - CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode -#endif -} - -/* - * Wait with optional wait loop callback - */ -void CarMotorControl::waitUntilCarStopped(void (*aLoopCallback)(void)) { - do { - rightCarMotor.updateMotor(); - leftCarMotor.updateMotor(); - if (aLoopCallback != NULL) { - aLoopCallback(); - } - } while (!isStopped()); - CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode -} - -bool CarMotorControl::isState(uint8_t aState) { -#if defined(SUPPORT_RAMP_UP) - return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState); -#else - (void) aState; - return false; -#endif -} - -bool CarMotorControl::isStopped() { - return (rightCarMotor.CurrentSpeed == 0 && leftCarMotor.CurrentSpeed == 0); -} - -void CarMotorControl::setFactorDegreeToCount(float aFactorDegreeToCount) { - FactorDegreeToCount = aFactorDegreeToCount; -} - -/** - * Set distances and speed for 2 motors to turn the requested angle - * @param aRotationDegrees positive -> turn left, negative -> turn right - * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE - * @param aUseSlowSpeed true -> use slower speed (1.5 times StartSpeed) instead of DriveSpeed for rotation to be more exact - */ -void CarMotorControl::startRotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { - int tDistanceCountRight; - int tDistanceCountLeft; - unsigned int tDistanceCount; - - uint8_t tTurnDirectionRightMotor = aTurnDirection; // set turn direction as start direction - uint8_t tTurnDirectionLeftMotor = aTurnDirection; - if (aTurnDirection == TURN_BACKWARD) { - // swap turn sign / (left / right) to move other motor - aRotationDegrees = -aRotationDegrees; - } - - /* - * Here aRotationDegrees is modified for TURN_FORWARD - */ - if (aRotationDegrees > 0) { - tDistanceCount = (aRotationDegrees * FactorDegreeToCount) + 0.5; - /* - * Move right motor (except for TURN_IN_PLACE) - * Here we have a left turn and a direction TURN_FORWARD or TURN_IN_PLACE - * OR a right turn and a direction TURN_BACKWARD - */ - tDistanceCountRight = tDistanceCount; - tDistanceCountLeft = 0; - - if (aTurnDirection == TURN_IN_PLACE) { - tDistanceCountRight /= 2; - tDistanceCountLeft = tDistanceCountRight; - // The right motor has turn direction TURN_IN_PLACE which is masked to TURN_FORWARD by startGoDistanceCount() - tTurnDirectionLeftMotor = TURN_BACKWARD; - } - } else { - tDistanceCount = (-aRotationDegrees * FactorDegreeToCount) + 0.5; - /* - * Move left motor (except for TURN_IN_PLACE) - * Here we have a right turn and a direction TURN_FORWARD or TURN_IN_PLACE - * OR a left turn and a direction TURN_BACKWARD - */ - tDistanceCountLeft = tDistanceCount; - tDistanceCountRight = 0; - if (aTurnDirection == TURN_IN_PLACE) { - tDistanceCountLeft /= 2; - tDistanceCountRight = tDistanceCountLeft; - tTurnDirectionRightMotor = TURN_BACKWARD; - } - } - - // This in turn sets CurrentDriveSpeed to DriveSpeed. - rightCarMotor.startGoDistanceCount(tDistanceCountRight, tTurnDirectionRightMotor); - leftCarMotor.startGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor); - if (aUseSlowSpeed) { - // adjust CurrentDriveSpeed -#if defined(SUPPORT_RAMP_UP) - // avoid overflow, the reduced speed is almost max speed then. - if (rightCarMotor.StartSpeed < 160) { - rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2; - } - if (leftCarMotor.StartSpeed < 160) { - leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2; - } -#else - if (tDistanceCountRight > 0) { - rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2, - rightCarMotor.CurrentDirectionOrBrakeMode); - } - if (tDistanceCountLeft > 0) { - leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2, - leftCarMotor.CurrentDirectionOrBrakeMode); - } -#endif - } -} - -/** - * @param aRotationDegrees positive -> turn left, negative -> turn right - * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE (default) - * @param aUseSlowSpeed true (default) -> use slower speed (1.5 times StartSpeed) instead of DriveSpeed for rotation to be more exact - * @param aLoopCallback avoid blocking and call aLoopCallback on waiting for stop - */ -void CarMotorControl::rotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed, void (*aLoopCallback)(void)) { - if (aRotationDegrees != 0) { - startRotateCar(aRotationDegrees, aTurnDirection, aUseSlowSpeed); - waitUntilCarStopped(aLoopCallback); - } -} - -#ifndef USE_ENCODER_MOTOR_CONTROL -void CarMotorControl::setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor) { - rightCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor); - leftCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor); -} - -#else - -/* - * Get count / distance value from right motor - */ -unsigned int CarMotorControl::getDistanceCount() { - return (rightCarMotor.EncoderCount); -} - -int CarMotorControl::getDistanceCentimeter() { - return (rightCarMotor.EncoderCount / FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); -} - -/* - * generates a rising ramp and detects the first movement -> this sets dead band / minimum Speed - */ -void CarMotorControl::calibrate() { - stopMotors(); - resetControlValues(); - - rightCarMotor.StartSpeed = 0; - leftCarMotor.StartSpeed = 0; - - uint8_t tMotorMovingCount = 0; - - /* - * increase motor speed by 1 until motor moves - */ - for (uint8_t tSpeed = 20; tSpeed != 0xFF; ++tSpeed) { - if (rightCarMotor.StartSpeed == 0) { - rightCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD); - } - if (leftCarMotor.StartSpeed == 0) { - leftCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD); - } - - delay(100); - /* - * Check if wheel moved - */ - - /* - * Store speed after 6 counts (3cm) - */ - if (rightCarMotor.StartSpeed == 0 && rightCarMotor.EncoderCount > 6) { - rightCarMotor.StartSpeed = tSpeed; - tMotorMovingCount++; - } - if (leftCarMotor.StartSpeed == 0 && leftCarMotor.EncoderCount > 6) { - leftCarMotor.StartSpeed = tSpeed; - tMotorMovingCount++; - } - if (tMotorMovingCount >= 2) { - // Do not end loop if one motor still not moving - break; - } - } - - /* - * TODO calibrate StopSpeed separately - */ - - writeMotorvaluesToEeprom(); - stopMotors(); -} - -// ISR for PIN PD2 / RIGHT -ISR(INT0_vect) { - sCarMotorControlPointerForISR->rightCarMotor.handleEncoderInterrupt(); -} - -// ISR for PIN PD3 / LEFT -ISR(INT1_vect) { - sCarMotorControlPointerForISR->leftCarMotor.handleEncoderInterrupt(); -} -#endif // USE_ENCODER_MOTOR_CONTROL diff --git a/src/CarMotorControl.h b/src/CarMotorControl.h deleted file mode 100644 index 795977c..0000000 --- a/src/CarMotorControl.h +++ /dev/null @@ -1,162 +0,0 @@ -/* - * CarMotorControl.h - * - * Motor control for a car with 2 encoder motors - * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef CARMOTORCONTROL_H_ -#define CARMOTORCONTROL_H_ - -#include "EncoderMotor.h" -#include - -/* - * Some factors depending on wheel diameter and encoder resolution - */ -#if ! defined(FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT) -#define FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT 2 // Exact value is 1.86, but integer saves program space and time -#endif -// Values for 20 slot encoder discs -> 40 ticks per turn. Circumference of the wheel is 21.5 cm, Distance between two wheels is around 13 cm -#define FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT 0.4277777 -#define FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT 0.8 // estimated, with slip - -#if ! defined(FACTOR_DEGREE_TO_COUNT_DEFAULT) -# if defined(CAR_HAS_4_WHEELS) -#define FACTOR_DEGREE_TO_COUNT_DEFAULT FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT -# else -#define FACTOR_DEGREE_TO_COUNT_DEFAULT FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT -# endif -#endif - -// turn directions -#define TURN_FORWARD DIRECTION_FORWARD // 0 -#define TURN_BACKWARD DIRECTION_BACKWARD // 1 -#define TURN_IN_PLACE 2 - -class CarMotorControl { -public: - - CarMotorControl(); -// virtual ~CarMotorControl(); - -#ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(bool aReadFromEeprom = false); -#else - void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aLeftMotorForwardPin, - uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, bool aReadFromEeprom = false); -#endif - - void setDefaultsForFixedDistanceDriving(); - void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight); - void setSpeedCompensation(int8_t aSpeedCompensationRight); - void setDriveSpeed(uint8_t aDriveSpeed); - void writeMotorvaluesToEeprom(); - -#ifdef USE_ENCODER_MOTOR_CONTROL - void calibrate(); - // retrieves values from right motor - unsigned int getDistanceCount(); - int getDistanceCentimeter(); -#else - // makes no sense for encoder motor - void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); -#endif - -#ifdef SUPPORT_RAMP_UP - void startRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); - void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void waitForDriveSpeed(void (*aLoopCallback)(void) = NULL); -#endif - // If ramp up is not supported, these functions just sets the speed and return immediately - void startRampUpAndWait(uint8_t aRequestedSpeed, uint8_t aRequestedDirection = DIRECTION_FORWARD, - void (*aLoopCallback)(void) = NULL); - void startRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL); - - /* - * For car direction handling - */ - uint8_t getCarDirectionOrBrakeMode(); - uint8_t CarDirectionOrBrakeMode; - - /* - * Functions for moving a fixed distance - */ - // With signed distance - void startGoDistanceCentimeter(uint8_t aRequestedSpeed, unsigned int aDistanceCentimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceCentimeter(int aDistanceCentimeter); // only setup values, no movement -> use updateMotors() - - void goDistanceCentimeter(int aDistanceCentimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilCarStopped - void goDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilCarStopped - - bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally - - /* - * Functions for rotation - */ - void setFactorDegreeToCount(float aFactorDegreeToCount); - void startRotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = true); - void rotateCar(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, bool aUseSlowSpeed = true, - void (*aLoopCallback)(void) = NULL); - float FactorDegreeToCount; - - bool updateMotors(); - void delayAndUpdateMotors(unsigned int aDelayMillis); - - /* - * Start/Stop functions for infinite distance - */ - void stopCarAndWaitForIt(void (*aLoopCallback)(void) = NULL); // uses waitUntilCarStopped() - void waitUntilCarStopped(void (*aLoopCallback)(void) = NULL); - - /* - * Check motor state functions - */ - bool isStopped(); - bool isState(uint8_t aState); - bool isStateRamp(); // MOTOR_STATE_RAMP_UP OR MOTOR_STATE_RAMP_DOWN - - void resetControlValues(); - - /* - * Functions, which directly call motor functions for both motors - */ - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, int8_t aLeftRightSpeed); - void stopMotors(uint8_t aStopMode = STOP_MODE_KEEP); - - void setSpeed(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void setStopMode(uint8_t aStopMode); - void setSpeed(int aRequestedSpeed); - void setSpeedCompensated(int aRequestedSpeed); - -#ifdef USE_ENCODER_MOTOR_CONTROL - EncoderMotor rightCarMotor; // 40 bytes RAM - EncoderMotor leftCarMotor; -#else - PWMDcMotor rightCarMotor; - PWMDcMotor leftCarMotor; -#endif -}; - -// Pointer to the last and only! instance for use by ISR -extern CarMotorControl * sCarMotorControlPointerForISR; - -#endif /* CARMOTORCONTROL_H_ */ - -#pragma once - diff --git a/src/CarPWMMotorControl.cpp b/src/CarPWMMotorControl.cpp new file mode 100644 index 0000000..98ebbc9 --- /dev/null +++ b/src/CarPWMMotorControl.cpp @@ -0,0 +1,828 @@ +/* + * CarPWMMotorControl.cpp + * + * Contains functions for control of the 2 motors of a car like setDirection, goDistanceMillimeter() and rotate(). + * Checks input of PIN aPinFor2WDDetection since we need different factors for rotating a 4 wheel and a 2 wheel car. + * + * Requires EncoderMotor.cpp + * + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include "CarPWMMotorControl.h" + +#define DEBUG // Only for development + +CarPWMMotorControl::CarPWMMotorControl() { // @suppress("Class members should be properly initialized") +} + +#ifdef USE_MPU6050_IMU +/* + * This must be done when the car is not moving, best after at least 100 ms after boot up. + */ +void CarPWMMotorControl::calculateAndPrintIMUOffsets(Print *aSerial) { + IMUData.calculateSpeedAndTurnOffsets(); + IMUData.printSpeedAndTurnOffsets(aSerial); +} +#endif +/* + * If no parameter and we have encoder motors, we use a fixed assignment of rightCarMotor interrupts to INT0 / Pin2 and leftCarMotor to INT1 / Pin3 + */ +#ifdef USE_ADAFRUIT_MOTOR_SHIELD +void CarPWMMotorControl::init() { +# ifdef USE_ENCODER_MOTOR_CONTROL + leftCarMotor.init(1, INT1); + rightCarMotor.init(2, INT0); +# else + leftCarMotor.init(1); + rightCarMotor.init(2); +# endif + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); +# else +# if defined(CAR_HAS_4_WHEELS) + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT; +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT; +# endif +# endif +} + +#else // USE_ADAFRUIT_MOTOR_SHIELD +void CarPWMMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, + uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin) { + leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin); + rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin); + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); + +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_DEFAULT; +# endif +# ifdef USE_ENCODER_MOTOR_CONTROL + /* + * For slot type optocoupler interrupts on pin PD2 + PD3 + */ + rightCarMotor.attachInterrupt(INT0); + leftCarMotor.attachInterrupt(INT1); +# endif +} + +# ifdef USE_ENCODER_MOTOR_CONTROL +/* + * With parameters aRightInterruptNumber + aLeftInterruptNumber + */ +void CarPWMMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, + uint8_t aRightInterruptNumber, uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, + uint8_t aLeftInterruptNumber) { + leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin, aLeftInterruptNumber); + rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin, aRightInterruptNumber); + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_DEFAULT; +# endif +} +# endif // USE_ENCODER_MOTOR_CONTROL +#endif // USE_ADAFRUIT_MOTOR_SHIELD + +/* + * Sets default values for min and max speed, factor for distance to time conversion for non encoder motors and reset speed compensation + * Is called automatically at init if parameter aReadFromEeprom is set to false + */ +void CarPWMMotorControl::setDefaultsForFixedDistanceDriving() { + rightCarMotor.setDefaultsForFixedDistanceDriving(); + leftCarMotor.setDefaultsForFixedDistanceDriving(); +} + +/** + * @param aSpeedPWMCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. + * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. + */ +void CarPWMMotorControl::setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, + int8_t aSpeedPWMCompensationRight) { + if (aSpeedPWMCompensationRight >= 0) { + rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, aSpeedPWMCompensationRight); + leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, 0); + } else { + rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, 0); + leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, -aSpeedPWMCompensationRight); + } +} + +/** + * @param aSpeedPWMCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. + * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. + */ +void CarPWMMotorControl::changeSpeedPWMCompensation(int8_t aSpeedPWMCompensationRight) { + if (aSpeedPWMCompensationRight > 0) { + if (leftCarMotor.SpeedPWMCompensation >= aSpeedPWMCompensationRight) { + leftCarMotor.SpeedPWMCompensation -= aSpeedPWMCompensationRight; + } else { + rightCarMotor.SpeedPWMCompensation += aSpeedPWMCompensationRight; + } + } else { + aSpeedPWMCompensationRight = -aSpeedPWMCompensationRight; + if (rightCarMotor.SpeedPWMCompensation >= aSpeedPWMCompensationRight) { + rightCarMotor.SpeedPWMCompensation -= aSpeedPWMCompensationRight; + } else { + leftCarMotor.SpeedPWMCompensation += aSpeedPWMCompensationRight; + } + } + PWMDcMotor::MotorControlValuesHaveChanged = true; +} + +void CarPWMMotorControl::setStartSpeedPWM(uint8_t aStartSpeedPWM) { + rightCarMotor.setStartSpeedPWM(aStartSpeedPWM); + leftCarMotor.setStartSpeedPWM(aStartSpeedPWM); +} + +void CarPWMMotorControl::setDriveSpeedPWM(uint8_t aDriveSpeedPWM) { + rightCarMotor.setDriveSpeedPWM(aDriveSpeedPWM); + leftCarMotor.setDriveSpeedPWM(aDriveSpeedPWM); +} + +/* + * @return true if direction has changed and motor has stopped + */ +bool CarPWMMotorControl::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { + bool tReturnValue = false; + if (CarDirectionOrBrakeMode != aRequestedDirection) { + uint8_t tMaxCurrentSpeedPWM = max(rightCarMotor.CurrentSpeedPWM, leftCarMotor.CurrentSpeedPWM); + if (tMaxCurrentSpeedPWM > 0) { + /* + * Direction change requested but motor still running-> first stop motor + */ +#ifdef DEBUG + Serial.println(F("First stop motor and wait")); +#endif + stop(MOTOR_BRAKE); +// delay(((tMaxCurrentSpeedPWM * tMaxCurrentSpeedPWM) >> 8) * 2); // to let motors stop + delay(tMaxCurrentSpeedPWM); // to let motors stop + tReturnValue = true; + } +#ifdef DEBUG + Serial.print(F("Change car mode from ")); + Serial.print(CarDirectionOrBrakeMode); + Serial.print(F(" to ")); + Serial.println(aRequestedDirection); +#endif + CarDirectionOrBrakeMode = aRequestedDirection; // The only statement which changes CarDirectionOrBrakeMode to DIRECTION_FORWARD or DIRECTION_BACKWARD + } + return tReturnValue; +} + +/* + * Direct motor control, no state or flag handling + */ +void CarPWMMotorControl::setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Sets speed adjusted by current compensation value and keeps direction + */ +void CarPWMMotorControl::changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM) { + rightCarMotor.changeSpeedPWMCompensated(aRequestedSpeedPWM); + leftCarMotor.changeSpeedPWMCompensated(aRequestedSpeedPWM); +} + +/* + * Sets speed adjusted by current compensation value and handle motor state and flags + */ +void CarPWMMotorControl::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Sets speed adjusted by current compensation value and handle motor state and flags + * @param aLeftRightSpeedPWM if positive, this value is subtracted from the left motor value, if negative subtracted from the right motor value + * + */ +void CarPWMMotorControl::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, int8_t aLeftRightSpeedPWM) { + checkAndHandleDirectionChange(aRequestedDirection); +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor *tMotorWithModifiedSpeedPWM; +#else + PWMDcMotor *tMotorWithModifiedSpeedPWM; +#endif + if (aLeftRightSpeedPWM >= 0) { + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + tMotorWithModifiedSpeedPWM = &leftCarMotor; + } else { + aLeftRightSpeedPWM = -aLeftRightSpeedPWM; + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + tMotorWithModifiedSpeedPWM = &rightCarMotor; + } + + if (aRequestedSpeedPWM >= aLeftRightSpeedPWM) { + tMotorWithModifiedSpeedPWM->setSpeedPWMCompensated(aRequestedSpeedPWM - aLeftRightSpeedPWM, aRequestedDirection); + } else { + tMotorWithModifiedSpeedPWM->setSpeedPWMCompensated(0, aRequestedDirection); + } +} + +/* + * Direct motor control, no state or flag handling + */ +void CarPWMMotorControl::setSpeedPWM(int aRequestedSpeedPWM) { + rightCarMotor.setSpeedPWM(aRequestedSpeedPWM); + leftCarMotor.setSpeedPWM(aRequestedSpeedPWM); +} + +/* + * Sets signed SpeedPWM adjusted by current compensation value and handle motor state and flags + */ +void CarPWMMotorControl::setSpeedPWMCompensated(int aRequestedSpeedPWM) { + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM); + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM); +} + +uint8_t CarPWMMotorControl::getCarDirectionOrBrakeMode() { + return CarDirectionOrBrakeMode;; +} + +void CarPWMMotorControl::readMotorValuesFromEeprom() { + leftCarMotor.readMotorValuesFromEeprom(0); + rightCarMotor.readMotorValuesFromEeprom(1); +} + +void CarPWMMotorControl::writeMotorValuesToEeprom() { + leftCarMotor.writeMotorValuesToEeprom(0); + rightCarMotor.writeMotorValuesToEeprom(1); +} + +/* + * Stop car + * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE + */ +void CarPWMMotorControl::stop(uint8_t aStopMode) { + rightCarMotor.stop(aStopMode); + leftCarMotor.stop(aStopMode); + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode, STOP_MODE_KEEP is evaluated here +} + +/* + * @param aStopMode MOTOR_BRAKE or MOTOR_RELEASE + */ +void CarPWMMotorControl::setStopMode(uint8_t aStopMode) { + rightCarMotor.setStopMode(aStopMode); + leftCarMotor.setStopMode(aStopMode); +} + +/* + * Stop car and reset all control values as SpeedPWM, distances, debug values etc. to 0x00 + */ +void CarPWMMotorControl::resetControlValues() { +#ifdef USE_ENCODER_MOTOR_CONTROL + rightCarMotor.resetEncoderControlValues(); + leftCarMotor.resetEncoderControlValues(); +#endif +} + +/* + * If motor is accelerating or decelerating then updateMotor needs to be called at a fast rate otherwise it will not work correctly + * Used to suppress time consuming display of motor values + */ +bool CarPWMMotorControl::isStateRamp() { + return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP + || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP); +} + +#ifdef USE_MPU6050_IMU +void CarPWMMotorControl::updateIMUData() { + if (IMUData.readCarDataFromMPU6050Fifo()) { + if (IMUData.AcceleratorForwardOffset != 0) { + if (CarTurn2DegreesPerSecondFromIMU != IMUData.getGyroscopePan2DegreePerSecond()) { + CarTurn2DegreesPerSecondFromIMU = IMUData.getGyroscopePan2DegreePerSecond(); +// PWMDcMotor::SensorValuesHaveChanged = true; is not displayed + } + if (CarTurnAngleHalfDegreesFromIMU != IMUData.getTurnAngleHalfDegree()) { + CarTurnAngleHalfDegreesFromIMU = IMUData.getTurnAngleHalfDegree(); + PWMDcMotor::SensorValuesHaveChanged = true; + } + if (CarSpeedCmPerSecondFromIMU != (unsigned int) abs(IMUData.getSpeedCmPerSecond())) { + CarSpeedCmPerSecondFromIMU = abs(IMUData.getSpeedCmPerSecond()); + PWMDcMotor::SensorValuesHaveChanged = true; + } + if (CarDistanceMillimeterFromIMU != (unsigned int) abs(IMUData.getDistanceMillimeter())) { + CarDistanceMillimeterFromIMU = abs(IMUData.getDistanceMillimeter()); + PWMDcMotor::SensorValuesHaveChanged = true; + } + } + } +} +#endif + +/* + * @return true if not stopped (motor expects another update) + */ +#define TURN_OVERRUN_HALF_ANGLE 1 // 1/2 degree overrun after stop(MOTOR_BRAKE) +#define RAMP_DOWN_MILLIMETER 50 +#define STOP_OVERRUN_MILLIMETER 10 // 1 cm overrun after stop(MOTOR_BRAKE) +/* + * If IMU data are available, rotation is always handled here. + * For non encoder motors also distance driving is handled here. + * @return true if not stopped (motor expects another update) + */ +bool CarPWMMotorControl::updateMotors() { +#ifdef USE_MPU6050_IMU + bool tReturnValue = true; + updateIMUData(); + if (CarRequestedRotationDegrees != 0) { + /* + * Using ramps for the rotation SpeedPWMs used makes no sense + */ +# ifdef TRACE + Serial.println(CarTurnAngleHalfDegreesFromIMU); + delay(10); +# endif + // putting abs(CarTurnAngleHalfDegreesFromIMU) also into a variable increases code size by 8 + int tRequestedRotationDegreesForCompare = abs(CarRequestedRotationDegrees * 2); + int tCarTurnAngleHalfDegreesFromIMUForCompare = abs(CarTurnAngleHalfDegreesFromIMU); + if ((tCarTurnAngleHalfDegreesFromIMUForCompare + TURN_OVERRUN_HALF_ANGLE) >= tRequestedRotationDegreesForCompare) { + stop(MOTOR_BRAKE); + CarRequestedRotationDegrees = 0; + tReturnValue = false; + } else if ((tCarTurnAngleHalfDegreesFromIMUForCompare + getTurnDistanceHalfDegree()) + >= tRequestedRotationDegreesForCompare) { + Serial.print(getTurnDistanceHalfDegree()); + // Reduce SpeedPWM just before target angle is reached if motors are not stopped we run for extra 2 to 4 degree + changeSpeedPWMCompensated(rightCarMotor.StartSpeedPWM); + } + } else { + if (CarRequestedDistanceMillimeter != 0) { +# ifndef USE_ENCODER_MOTOR_CONTROL + if (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP || rightCarMotor.MotorRampState == MOTOR_STATE_DRIVE + || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN) { + unsigned int tBrakingDistanceMillimeter = getBrakingDistanceMillimeter(); +#ifdef DEBUG + Serial.print(F("Dist=")); + Serial.print(CarDistanceMillimeterFromIMU); + Serial.print(F(" Breakdist=")); + Serial.print(tBrakingDistanceMillimeter); + Serial.print(F(" St=")); + Serial.print(rightCarMotor.MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(rightCarMotor.CurrentSpeedPWM); +#endif + if (CarDistanceMillimeterFromIMU >= CarRequestedDistanceMillimeter) { + CarRequestedDistanceMillimeter = 0; + stop(MOTOR_BRAKE); + } + // Transition criteria to brake/ramp down is: Target distance - braking distance reached + if (rightCarMotor.MotorRampState != MOTOR_STATE_RAMP_DOWN + && (CarDistanceMillimeterFromIMU + tBrakingDistanceMillimeter) >= CarRequestedDistanceMillimeter) { + // Start braking + startRampDown(); + } + } +# endif // USE_ENCODER_MOTOR_CONTROL + } + /* + * In case of IMU distance driving only ramp up and down are managed by these calls + */ + tReturnValue = rightCarMotor.updateMotor(); + tReturnValue |= leftCarMotor.updateMotor(); + } + +#else // USE_MPU6050_IMU +bool tReturnValue = rightCarMotor.updateMotor(); +tReturnValue |= leftCarMotor.updateMotor(); +#endif // USE_MPU6050_IMU + + return tReturnValue;; +} + +/* + * @return true if not stopped (motor expects another update) + */ +bool CarPWMMotorControl::updateMotors(void (*aLoopCallback)(void)) { + if (aLoopCallback != NULL) { + aLoopCallback(); + } + return updateMotors(); +} + +void CarPWMMotorControl::delayAndUpdateMotors(unsigned int aDelayMillis) { + uint32_t tStartMillis = millis(); + do { + updateMotors(); + } while (millis() - tStartMillis <= aDelayMillis); +} + +void CarPWMMotorControl::startRampUp(uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.startRampUp(aRequestedDirection); + leftCarMotor.startRampUp(aRequestedDirection); +} + +void CarPWMMotorControl::startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.startRampUp(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.startRampUp(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Blocking wait until both motors are at drive SpeedPWM. 256 milliseconds for ramp up. + */ +void CarPWMMotorControl::waitForDriveSpeedPWM(void (*aLoopCallback)(void)) { + while (updateMotors(aLoopCallback) + && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE)) { + ; + } +} + +/* + * If ramp up is not supported, this functions just sets the SpeedPWM and returns immediately. + * 256 milliseconds for ramp up. + */ +void CarPWMMotorControl::startRampUpAndWait(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + waitForDriveSpeedPWM(aLoopCallback); +} + +void CarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + startRampUp(aRequestedDirection); + waitForDriveSpeedPWM(aLoopCallback); +} + +void CarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} + +/* + * initialize motorInfo fields LastDirection and CurrentSpeedPWM + */ +void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + + checkAndHandleDirectionChange(aRequestedDirection); + +#ifdef USE_MPU6050_IMU + IMUData.resetCarData(); + CarRequestedDistanceMillimeter = aRequestedDistanceMillimeter; +#endif + +#if defined(USE_MPU6050_IMU) && !defined(USE_ENCODER_MOTOR_CONTROL) + // for non encoder motor we use the IMU distance, and require only the ramp up + startRampUp(aRequestedSpeedPWM, aRequestedDirection); +#else + rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + leftCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +#endif +} + +void CarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, + void (*aLoopCallback)(void)) { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + waitUntilStopped(aLoopCallback); +} + +void CarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} + +/** + * Wait until distance is reached + * @param aLoopCallback called until car has stopped to avoid blocking + */ +void CarPWMMotorControl::goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) { + startGoDistanceMillimeter(aRequestedDistanceMillimeter); + waitUntilStopped(aLoopCallback); +} + +/* + * Stop car with ramp and give DistanceCountAfterRampUp counts for braking. + */ +void CarPWMMotorControl::stopAndWaitForIt(void (*aLoopCallback)(void)) { + if (isStopped()) { + return; + } + + rightCarMotor.startRampDown(); + leftCarMotor.startRampDown(); + /* + * blocking wait for stop + */ + waitUntilStopped(aLoopCallback); +} + +void CarPWMMotorControl::startRampDown() { + if (isStopped()) { + return; + } + /* + * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE to MOTOR_STATE_RAMP_DOWN + * Use DistanceCountAfterRampUp as ramp down count + */ + rightCarMotor.startRampDown(); + leftCarMotor.startRampDown(); +} + +/* + * Wait with optional wait loop callback + */ +void CarPWMMotorControl::waitUntilStopped(void (*aLoopCallback)(void)) { + while (updateMotors(aLoopCallback)) { + ; + } + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode +} + +bool CarPWMMotorControl::isState(uint8_t aState) { + return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState); +} + +bool CarPWMMotorControl::isStopped() { + return (rightCarMotor.CurrentSpeedPWM == 0 && leftCarMotor.CurrentSpeedPWM == 0); +} + +void CarPWMMotorControl::setFactorDegreeToMillimeter(float aFactorDegreeToMillimeter) { +#ifndef USE_MPU6050_IMU + FactorDegreeToMillimeter = aFactorDegreeToMillimeter; +#else + (void) aFactorDegreeToMillimeter; +#endif +} + +/** + * Set distances and SpeedPWM for 2 motors to turn the requested angle + * @param aRotationDegrees positive -> turn left, negative -> turn right + * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE + * @param aUseSlowSpeed true -> use slower SpeedPWM (1.5 times StartSpeedPWM) instead of DriveSpeedPWM for rotation to be more exact + */ +void CarPWMMotorControl::startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { + /* + * We have 6 cases + * - aTurnDirection = TURN_FORWARD + -> left, right motor F, left 0 - -> right, right motor 0, left F + * - aTurnDirection = TURN_BACKWARD + -> left, right motor 0, left B - -> right, right motor B, left 0 + * - aTurnDirection = TURN_IN_PLACE + -> left, right motor F, left B - -> right, right motor B, left F + * Turn direction TURN_IN_PLACE is masked to TURN_FORWARD + */ + +#ifdef DEBUG + Serial.print(F("RotationDegrees=")); + Serial.print(aRotationDegrees); + Serial.print(F(" TurnDirection=")); + Serial.println(aTurnDirection); + Serial.flush(); +#endif + +#ifdef USE_MPU6050_IMU + IMUData.resetCarData(); + CarRequestedRotationDegrees = aRotationDegrees; +#endif + + /* + * Handle positive and negative rotation degrees + */ +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor *tRightMotorIfPositiveTurn; + EncoderMotor *tLeftMotorIfPositiveTurn; +#else + PWMDcMotor *tRightMotorIfPositiveTurn; + PWMDcMotor *tLeftMotorIfPositiveTurn; +#endif + if (aRotationDegrees >= 0) { + tRightMotorIfPositiveTurn = &rightCarMotor; + tLeftMotorIfPositiveTurn = &leftCarMotor; + } else { +// swap turn sign and left / right motors + aRotationDegrees = -aRotationDegrees; + tRightMotorIfPositiveTurn = &leftCarMotor; + tLeftMotorIfPositiveTurn = &rightCarMotor; + } + + /* + * Here aRotationDegrees is positive + * Now handle different turn directions + */ +#ifdef USE_MPU6050_IMU + unsigned int tDistanceMillimeter = 2000; // Dummy value for distance - equivalent to #define tDistanceCount 200 give a timeout of around 10 wheel rotations +#else + unsigned int tDistanceMillimeter = (aRotationDegrees * FactorDegreeToMillimeter) + 0.5; +#endif + + unsigned int tDistanceMillimeterRight; + unsigned int tDistanceMillimeterLeft; + + if (aTurnDirection == TURN_FORWARD) { + tDistanceMillimeterRight = tDistanceMillimeter; + tDistanceMillimeterLeft = 0; + } else if (aTurnDirection == TURN_BACKWARD) { + tDistanceMillimeterRight = 0; + tDistanceMillimeterLeft = tDistanceMillimeter; + } else { + tDistanceMillimeterRight = tDistanceMillimeter / 2; + tDistanceMillimeterLeft = tDistanceMillimeter / 2; + } + + /* + * Handle slow speed flag and reduce turn SpeedPWMs + */ + uint8_t tTurnSpeedPWMRight = tRightMotorIfPositiveTurn->DriveSpeedPWM; + uint8_t tTurnSpeedPWMLeft = tLeftMotorIfPositiveTurn->DriveSpeedPWM; + if (aUseSlowSpeed) { +// avoid overflow, the reduced SpeedPWM is almost max SpeedPWM then. + if (tRightMotorIfPositiveTurn->StartSpeedPWM < 160) { + tTurnSpeedPWMRight = tRightMotorIfPositiveTurn->StartSpeedPWM + (tRightMotorIfPositiveTurn->StartSpeedPWM / 2); + } + if (tLeftMotorIfPositiveTurn->StartSpeedPWM < 160) { + tTurnSpeedPWMLeft = tLeftMotorIfPositiveTurn->StartSpeedPWM + (tLeftMotorIfPositiveTurn->StartSpeedPWM / 2); + } + } + +#ifdef DEBUG + Serial.print(F("TurnSpeedPWMRight=")); + Serial.print(tTurnSpeedPWMRight); +# ifndef USE_MPU6050_IMU + Serial.print(F(" DistanceMillimeterRight=")); + Serial.print(tDistanceMillimeterRight); +# endif + Serial.println(); +#endif + +#ifdef USE_MPU6050_IMU + // We do not really have ramps for turn speed + if (tDistanceMillimeterRight > 0) { + tRightMotorIfPositiveTurn->setSpeedPWMCompensated(tTurnSpeedPWMRight, DIRECTION_FORWARD); + } + if (tDistanceMillimeterLeft > 0) { + tLeftMotorIfPositiveTurn->setSpeedPWMCompensated(tTurnSpeedPWMLeft, DIRECTION_BACKWARD); + } +#else + tRightMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD); + tLeftMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD); +#endif +} + +/** + * @param aRotationDegrees positive -> turn left (counterclockwise), negative -> turn right + * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE (default) + * @param aUseSlowSpeed true (default) -> use slower SpeedPWM (1.5 times StartSpeedPWM) instead of DriveSpeedPWM for rotation to be more exact. + * Does not really work for 4WD cars. + * TODO remove? since only sensible for encoder motors. + * @param aLoopCallback avoid blocking and call aLoopCallback on waiting for stop + */ +void CarPWMMotorControl::rotate(int aRotationDegrees, uint8_t aTurnDirection, void (*aLoopCallback)(void), bool aUseSlowSpeed) { + if (aRotationDegrees != 0) { + startRotate(aRotationDegrees, aTurnDirection, aUseSlowSpeed); + waitUntilStopped(aLoopCallback); + } +} + +#ifdef USE_ENCODER_MOTOR_CONTROL +/* + * Get count / distance value from right motor + */ +unsigned int CarPWMMotorControl::getDistanceCount() { + return (rightCarMotor.EncoderCount); +} + +unsigned int CarPWMMotorControl::getDistanceMillimeter() { + return (rightCarMotor.getDistanceMillimeter()); +} + +#else +void CarPWMMotorControl::setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond) { + rightCarMotor.setMillimeterPerSecondForFixedDistanceDriving(aMillimeterPerSecond); + leftCarMotor.setMillimeterPerSecondForFixedDistanceDriving(aMillimeterPerSecond); +} + +#endif // USE_ENCODER_MOTOR_CONTROL + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +unsigned int CarPWMMotorControl::getBrakingDistanceMillimeter() { +# ifdef USE_ENCODER_MOTOR_CONTROL + return rightCarMotor.getBrakingDistanceMillimeter(); +# else + unsigned int tCarSpeedCmPerSecond = CarSpeedCmPerSecondFromIMU; +// return (tCarSpeedCmPerSecond * tCarSpeedCmPerSecond * 100) / RAMP_DECELERATION_TIMES_2; // overflow! + return (tCarSpeedCmPerSecond * tCarSpeedCmPerSecond) / (RAMP_DECELERATION_TIMES_2 / 100); +# endif +} + +# ifdef USE_MPU6050_IMU +uint8_t CarPWMMotorControl::getTurnDistanceHalfDegree() { + uint8_t tCarTurn2DegreesPerSecondFromIMU = CarTurn2DegreesPerSecondFromIMU; + return ((tCarTurn2DegreesPerSecondFromIMU * tCarTurn2DegreesPerSecondFromIMU) / 20); +} +# endif + +/* + * Generates a rising ramp and detects the first movement -> this sets dead band / minimum SpeedPWM + * aLoopCallback is responsible for calling readCarDataFromMPU6050Fifo(); + */ +void CarPWMMotorControl::calibrate(void (*aLoopCallback)(void)) { + stop(); + resetControlValues(); + + rightCarMotor.StartSpeedPWM = 0; + leftCarMotor.StartSpeedPWM = 0; + +# ifdef USE_ENCODER_MOTOR_CONTROL + uint8_t tMotorMovingCount = 0; +# else + IMUData.resetOffsetDataAndWait(); +# endif + + /* + * increase motor SpeedPWM by 1 every 200 ms until motor moves + */ + for (uint8_t tSpeedPWM = 20; tSpeedPWM != MAX_SPEED_PWM; ++tSpeedPWM) { +// as long as no start speed is computed increase speed + if (rightCarMotor.StartSpeedPWM == 0) { + // as long as no start speed is computed, increase motor speed + rightCarMotor.setSpeedPWM(tSpeedPWM, DIRECTION_FORWARD); + } + if (leftCarMotor.StartSpeedPWM == 0) { + leftCarMotor.setSpeedPWM(tSpeedPWM, DIRECTION_FORWARD); + } + + /* + * Active delay of 200 ms + */ + uint32_t tStartMillis = millis(); + do { + if (aLoopCallback != NULL) { + aLoopCallback(); + } + if (isStopped()) { + // we were stopped by aLoopCallback() + return; + } +# ifdef USE_ENCODER_MOTOR_CONTROL + delay(10); +# else + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + updateIMUData(); +# endif + } while (millis() - tStartMillis <= 200); + + /* + * Check if wheel moved + */ +# ifdef USE_ENCODER_MOTOR_CONTROL + /* + * Store speed after 6 counts (3cm) + */ + if (rightCarMotor.StartSpeedPWM == 0 && rightCarMotor.EncoderCount > 6) { + rightCarMotor.setStartSpeedPWM(tSpeedPWM); + tMotorMovingCount++; + } + if (leftCarMotor.StartSpeedPWM == 0 && leftCarMotor.EncoderCount > 6) { + leftCarMotor.setStartSpeedPWM(tSpeedPWM); + tMotorMovingCount++; + } + if (tMotorMovingCount >= 2) { + // Do not end loop if one motor is still not moving + break; + } + +# else + if (abs(IMUData.getSpeedCmPerSecond()) >= 10) { + setStartSpeedPWM(tSpeedPWM); + break; + } +# endif + } + stop(); +} +#endif // defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) diff --git a/src/CarPWMMotorControl.h b/src/CarPWMMotorControl.h new file mode 100644 index 0000000..d18eb71 --- /dev/null +++ b/src/CarPWMMotorControl.h @@ -0,0 +1,206 @@ +/* + * CarPWMMotorControl.h + * + * Motor control for a car with 2 encoder motors + * + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef CarPWMMotorControl_H_ +#define CarPWMMotorControl_H_ + +#include "EncoderMotor.h" + +/* + * Use GY-521 MPU6050 breakout board connected by I2C for support of precise turning and speed / distance calibration. + * Connectors point to the rear. + */ +//#define USE_MPU6050_IMU +#ifdef USE_MPU6050_IMU +#include "IMUCarData.h" +#endif +#include + +/* + * Values for 20 slot encoder discs. Circumference of the wheel is 22.0 cm + * Distance between two wheels is around 14 cm -> 360 degree are 82 cm + */ +#define FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT 2.2777 +#define FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT 5.0 // estimated, with slip + +#if ! defined(FACTOR_DEGREE_TO_MILLIMETER_DEFAULT) +# if defined(CAR_HAS_4_WHEELS) +#define FACTOR_DEGREE_TO_MILLIMETER_DEFAULT FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT +# else +#define FACTOR_DEGREE_TO_MILLIMETER_DEFAULT FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT +# endif +#endif + +// turn directions +#define TURN_FORWARD DIRECTION_FORWARD // 0 +#define TURN_BACKWARD DIRECTION_BACKWARD // 1 +#define TURN_IN_PLACE 2 + +class CarPWMMotorControl { +public: + + CarPWMMotorControl(); +// virtual ~CarPWMMotorControl(); + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + void init(); +#else + void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aLeftMotorForwardPin, + uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin); + void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aRightInterruptNumber, + uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, uint8_t aLeftInterruptNumber); +#endif // USE_ADAFRUIT_MOTOR_SHIELD + + void setDefaultsForFixedDistanceDriving(); + void setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, int8_t aSpeedPWMCompensationRight); + void changeSpeedPWMCompensation(int8_t aSpeedPWMCompensationRight); + void setStartSpeedPWM(uint8_t aStartSpeedPWM); + void setDriveSpeedPWM(uint8_t aDriveSpeedPWM); + + void writeMotorValuesToEeprom(); + void readMotorValuesFromEeprom(); + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + void calibrate(void (*aLoopCallback)(void)); // aLoopCallback must call readCarDataFromMPU6050Fifo() + unsigned int getBrakingDistanceMillimeter(); + uint8_t getTurnDistanceHalfDegree(); +#endif + +#ifdef USE_MPU6050_IMU + void updateIMUData(); + void calculateAndPrintIMUOffsets(Print *aSerial); +#endif + +#ifdef USE_ENCODER_MOTOR_CONTROL + // retrieves values from right motor + unsigned int getDistanceCount(); + unsigned int getDistanceMillimeter(); +#else + // makes no sense for encoder motor + void setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond); +#endif + + // If ramp up is not supported, these functions just sets the speed and return immediately + void startRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); + void startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void waitForDriveSpeedPWM(void (*aLoopCallback)(void) = NULL); + void startRampUpAndWait(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection = DIRECTION_FORWARD, + void (*aLoopCallback)(void) = NULL); + void startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL); + + /* + * For car direction handling + */ + uint8_t getCarDirectionOrBrakeMode(); + uint8_t CarDirectionOrBrakeMode; + + /* + * 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 goDistanceMillimeter(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 + + bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally + + /* + * Functions for rotation + */ + void setFactorDegreeToMillimeter(float aFactorDegreeToMillimeter); +#if defined(CAR_HAS_4_WHEELS) + // slow speed does not really work for 4 WD cars + void startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = false); + void rotate(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, + void (*aLoopCallback)(void) = NULL, bool aUseSlowSpeed = false); +#else + void startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = true); + void rotate(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, + void (*aLoopCallback)(void) = NULL, bool aUseSlowSpeed = true); +#endif + +#ifdef USE_MPU6050_IMU + IMUCarData IMUData; + int CarRequestedRotationDegrees; // 0 -> car is moving forward / backward + int CarTurnAngleHalfDegreesFromIMU; // Read from Gyroscope + int8_t CarTurn2DegreesPerSecondFromIMU; // Read from Gyroscope + // Read from Accelerator + unsigned int CarSpeedCmPerSecondFromIMU; + unsigned int CarRequestedDistanceMillimeter; + unsigned int CarDistanceMillimeterFromIMU; +#else + float FactorDegreeToMillimeter; +#endif + + bool updateMotors(); + bool updateMotors(void (*aLoopCallback)(void)); + void delayAndUpdateMotors(unsigned int aDelayMillis); + + /* + * Start/Stop functions + */ + void startRampDown(); + void stopAndWaitForIt(void (*aLoopCallback)(void) = NULL); // uses waitUntilStopped() + void waitUntilStopped(void (*aLoopCallback)(void) = NULL); + + /* + * Check motor state functions + */ + bool isStopped(); + bool isState(uint8_t aState); + bool isStateRamp(); // MOTOR_STATE_RAMP_UP OR MOTOR_STATE_RAMP_DOWN + + void resetControlValues(); + + /* + * Functions, which directly call motor functions for both motors + */ + void changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, int8_t aLeftRightSpeedPWM); + void stop(uint8_t aStopMode = STOP_MODE_KEEP); // STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE + + void setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setStopMode(uint8_t aStopMode); + void setSpeedPWM(int aRequestedSpeedPWM); + void setSpeedPWMCompensated(int aRequestedSpeedPWM); + +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor rightCarMotor; // 40 bytes RAM + EncoderMotor leftCarMotor; +#else + PWMDcMotor rightCarMotor; + PWMDcMotor leftCarMotor; +#endif +}; + +#endif /* CarPWMMotorControl_H_ */ + +#pragma once + diff --git a/src/DebugLevel.h b/src/DebugLevel.h new file mode 100644 index 0000000..cbb2d94 --- /dev/null +++ b/src/DebugLevel.h @@ -0,0 +1,52 @@ +/* + * DebugLevel.h + * Include to propagate debug levels + * + * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Email: armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. + * + * Arduino-Utils is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef DEBUGLEVEL_H_ +#define DEBUGLEVEL_H_ + +// Propagate debug level +#ifdef TRACE // Information you need to understand details of a function or if you hunt a bug. +# ifndef DEBUG +#define DEBUG // Information need to understand the operating of your program. E.g. function calls and values of control variables. +# endif +#endif +#ifdef DEBUG +# ifndef INFO +#define INFO // Information you want to see in regular operation to see what the program is doing. E.g. "START ../src/LightToTone.cpp Version 1.2 from Dec 31 2019" or "Now playing Muppets melody". +# endif +#endif +#ifdef INFO +# ifndef WARN +#define WARN // Information that the program may encounter problems, like small Heap/Stack area. +# endif +#endif +#ifdef WARN +# ifndef ERROR +#define ERROR // Informations to explain why the program will not run. E.g. not enough Ram for all created objects. +# endif +#endif + +#endif /* DEBUGLEVEL_H_ */ + +#pragma once diff --git a/src/Distance.cpp b/src/Distance.cpp index 7a036e4..8033d31 100644 --- a/src/Distance.cpp +++ b/src/Distance.cpp @@ -25,7 +25,9 @@ ForwardDistancesInfoStruct sForwardDistancesInfo; +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) Servo DistanceServo; +#endif uint8_t sLastServoAngleInDegrees; // 0 - 180 needed for optimized delay for servo repositioning. Only set by DistanceServoWriteAndDelay() #ifdef CAR_HAS_TOF_DISTANCE_SENSOR @@ -118,7 +120,11 @@ void DistanceServoWriteAndDelay(uint8_t aTargetDegrees, bool doDelay) { // The servo is top down and therefore inverted aTargetDegrees = 180 - aTargetDegrees; #endif +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) DistanceServo.write(aTargetDegrees); +#else + write10(aTargetDegrees); +#endif /* * Delay @@ -179,7 +185,7 @@ int scanForTarget() { */ for (uint8_t i = 0; i < 3; ++i) { DistanceServoWriteAndDelay(tScanDegree, true); - tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_FOLLOWER); + tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_FOLLOWER, true); uint8_t tCurrentIndex; if (tDeltaDegree > 0) { @@ -194,17 +200,17 @@ int scanForTarget() { * Determine color and draw distance line */ color16_t tColor; - tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan + tColor = COLOR16_RED; // tCentimeter <= sCentimeterPerScan if (tCentimeter <= FOLLOWER_DISTANCE_TARGET_SCAN_CENTIMETER) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tCentimeter < FOLLOWER_DISTANCE_MINIMUM_CENTIMETER) { - tColor = COLOR_YELLOW; + tColor = COLOR16_YELLOW; } // Clear old line BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, sForwardDistancesInfo.RawDistancesArray[tCurrentIndex], tScanDegree, - COLOR_WHITE, 3); + COLOR16_WHITE, 3); BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tScanDegree, tColor, 3); } sForwardDistancesInfo.RawDistancesArray[tCurrentIndex] = tCentimeter; @@ -223,7 +229,7 @@ int scanForTarget() { */ sprintf_P(sStringBuffer, PSTR("rotation:%3d\xB0 min:%2dcm"), tScanDegree - 90, tCentimeter); // \xB0 is degree character BlueDisplay1.drawText(BUTTON_WIDTH_3_5_POS_2, US_DISTANCE_MAP_ORIGIN_Y + TEXT_SIZE_11, sStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); // reset distance servo direction DistanceServoWriteAndDelay(90, false); @@ -282,33 +288,32 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b // User sent an event -> stop and return now return true; } - - unsigned int tCentimeter = getDistanceAsCentiMeter(false); + unsigned int tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, true); if ((tIndex == INDEX_FORWARD_1 || tIndex == INDEX_FORWARD_2) && tCentimeter <= sCentimeterPerScanTimesTwo) { /* * Emergency motor stop if index is forward and measured distance is less than distance driven during two scans */ - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) { /* * Determine color */ - tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan + tColor = COLOR16_RED; // tCentimeter <= sCentimeterPerScan if (tCentimeter >= DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE) { tColor = DISTANCE_TIMEOUT_COLOR; } else if (tCentimeter > sCentimeterPerScanTimesTwo) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tCentimeter > sCentimeterPerScan) { - tColor = COLOR_YELLOW; + tColor = COLOR16_YELLOW; } /* * Clear old and draw new distance line */ BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, - sForwardDistancesInfo.RawDistancesArray[tIndex], tCurrentDegrees, COLOR_WHITE, 3); + sForwardDistancesInfo.RawDistancesArray[tIndex], tCurrentDegrees, COLOR16_WHITE, 3); BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tCurrentDegrees, tColor, 3); } @@ -337,7 +342,7 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b * Negative means we are heading away from wall */ uint8_t computeNeigbourValue(uint8_t aDegreesPerStepValue, uint8_t a2DegreesPerStepValue, uint8_t aClipValue, - int8_t * aDegreeOfEndpointConnectingLine) { + int8_t *aDegreeOfEndpointConnectingLine) { /* * Name of the variables are for DEGREES_PER_STEP = 20 only for better understanding. @@ -443,7 +448,7 @@ void doWallDetection() { * i.e. put 20 degrees to 40 degrees parameter and vice versa in order to use the 0 degree value as the 60 degrees one */ uint8_t tNextDistanceComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, - DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); + DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); #ifdef TRACE BlueDisplay1.debug("i=", i); BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); @@ -482,7 +487,7 @@ void doWallDetection() { tNextDistanceOriginal = tNextDistanceComputed; if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextDistanceComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); + tCurrentAngleToCheck, COLOR16_WHITE, 1); } } } @@ -520,7 +525,7 @@ void doWallDetection() { * Use computeNeigbourValue in the intended way, so do not change sign of tDegreeOfConnectingLine! */ uint8_t tNextValueComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, - DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); + DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); #ifdef TRACE BlueDisplay1.debug("i=", i); BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); @@ -556,7 +561,7 @@ void doWallDetection() { tNextValue = tNextValueComputed; if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextValueComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); + tCurrentAngleToCheck, COLOR16_WHITE, 1); } } } @@ -605,23 +610,27 @@ void postProcessDistances(uint8_t aDistanceThreshold) { } } -void readAndShowDistancePeriodically(uint16_t aPeriodMillis) { +void readAndShowDistancePeriodically() { static long sLastUSMeasurementMillis; // Do not show distanced during (time critical) acceleration or deceleration if (!RobotCarMotorControl.isStateRamp()) { long tMillis = millis(); - if (sLastUSMeasurementMillis + aPeriodMillis < tMillis) { + if (tMillis - sLastUSMeasurementMillis >= DISTANCE_DISPLAY_PERIOD_MILLIS) { sLastUSMeasurementMillis = tMillis; - getDistanceAsCentiMeter(true); + getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM, false); } } } /* * Timeout is DISTANCE_TIMEOUT_CM (1 meter) + * aWaitForCurrentMeasurmentToEnd for IR Distance sensors if true, wait for the current measurement to end, since the sensor was recently moved. */ -unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { +unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout, bool aWaitForCurrentMeasurementToEnd) { +#if !defined(CAR_HAS_IR_DISTANCE_SENSOR) + (void) aWaitForCurrentMeasurementToEnd; // not used +#endif #ifdef CAR_HAS_TOF_DISTANCE_SENSOR if (sScanMode != SCAN_MODE_US) { sToFDistanceSensor.VL53L1X_StartRanging(); @@ -629,6 +638,9 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { #endif unsigned int tCentimeter = getUSDistanceAsCentiMeterWithCentimeterTimeout(aDistanceTimeout); + if (tCentimeter == 0) { + tCentimeter = aDistanceTimeout; + } if (doShow) { showUSDistance(tCentimeter); } @@ -637,7 +649,7 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { if (sScanMode != SCAN_MODE_US) { # if defined(CAR_HAS_IR_DISTANCE_SENSOR) if (sScanMode != SCAN_MODE_US) { - tIRCentimeter = getIRDistanceAsCentimeter(); + tIRCentimeter = getIRDistanceAsCentimeter(aWaitForCurrentMeasurementToEnd); if (doShow) { showIRDistance(tIRCentimeter); } @@ -672,8 +684,30 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { /* * The 1080 needs 39 ms for each measurement cycle */ -uint8_t getIRDistanceAsCentimeter() { - float tVolt = analogRead(PIN_IR_DISTANCE_SENSOR); +uint8_t getIRDistanceAsCentimeter(bool aWaitForCurrentMeasurementToEnd) { + uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(PIN_IR_DISTANCE_SENSOR, DEFAULT); + // check for voltage changed then a new measurement is started + if (aWaitForCurrentMeasurementToEnd) { + uint16_t tOldValue = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 4 samples +// int16_t tOldValue = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + uint32_t tStartMillis = millis(); + do { + int16_t tNewValue = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 100 us +// int16_t tNewValue = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + if (abs(tOldValue-tNewValue) > IR_SENSOR_NEW_MEASUREMENT_THRESHOLD) { + // assume, that voltage has changed because of the end of a measurement + break; + } + loopGUI(); + } while (millis() - tStartMillis <= IR_SENSOR_MEASUREMENT_TIME_MILLIS); + // now a new measurement has started, wait for the result + delayAndLoopGUI(IR_SENSOR_NEW_MEASUREMENT_THRESHOLD); // the IR sensor takes 39 ms for one measurement + } + + float tVolt = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 4 samples +// float tVolt = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + ADMUX = tOldADMUX; // Switch back (to INTERNAL) + // * 0.004887585 for 1023 = 5V // Model 1080 / GP2Y0A21YK0F return (29.988 * pow(tVolt * 0.004887585, -1.173)) + 0.5; // see https://github.com/guillaume-rico/SharpIR/blob/master/SharpIR.cpp diff --git a/src/Distance.h b/src/Distance.h index e9f1202..7cfa3b5 100644 --- a/src/Distance.h +++ b/src/Distance.h @@ -20,13 +20,20 @@ #define DISTANCE_H_ #include + +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) #include +#else +#include "LightweightServo.h" +#endif /* - * Comment this out / enable this if the distance servo is mounted head down to detect small obstacles. + * Activate this, if the distance servo is mounted head down to detect small obstacles. */ //#define DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) extern Servo DistanceServo; +#endif /* * Constants for fillAndShowForwardDistancesInfo(), doWallDetection etc. @@ -36,10 +43,11 @@ extern Servo DistanceServo; #define STEPS_PER_SCAN (NUMBER_OF_DISTANCES - 1) // -> 162 degrees for 18 DEGREES_PER_STEP, 153 for 17 degrees #define START_DEGREES ((180 - (DEGREES_PER_STEP * STEPS_PER_SCAN)) / 2) // 9 for 18, 13,5 for 17 - we need it symmetrical in the 180 degrees range +#define DISTANCE_TIMEOUT_CM 200 // do not measure distances greater than 200 cm #define DISTANCE_TIMEOUT_CM_FOLLOWER 130 // do not measure and process distances greater than 130 cm #define DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE 100 // do not measure and process distances greater than 100 cm -#define DISTANCE_TIMEOUT_COLOR COLOR_CYAN +#define DISTANCE_TIMEOUT_COLOR COLOR16_CYAN #define DISTANCE_DISPLAY_PERIOD_MILLIS 500 // for future use maybe @@ -83,7 +91,8 @@ extern int sLastDegreesTurned; void initDistance(); void DistanceServoWriteAndDelay(uint8_t aValue, bool doDelay = false); -unsigned int getDistanceAsCentiMeter(bool doShow = false, uint8_t aDistanceTimeout = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE); +unsigned int getDistanceAsCentiMeter(bool doShow = false, uint8_t aDistanceTimeout = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, + bool aWaitForCurrentMeasurementToEnd = false); int scanForTarget(); bool fillAndShowForwardDistancesInfo(bool aDoFirstValue, bool aForceScan = false); void doWallDetection(); @@ -99,7 +108,9 @@ uint8_t readToFDistanceAsCentimeter(); // no start of measurement, just read res #endif #ifdef CAR_HAS_IR_DISTANCE_SENSOR -uint8_t getIRDistanceAsCentimeter(); +uint8_t getIRDistanceAsCentimeter(bool aWaitForCurrentMeasurementToEnd); +#define IR_SENSOR_NEW_MEASUREMENT_THRESHOLD 2 // If the output value changes by this amount, we can assume that a new measurement is started +#define IR_SENSOR_MEASUREMENT_TIME_MILLIS 41 // the IR sensor takes 39 ms for one measurement #endif #endif // DISTANCE_H_ diff --git a/src/EncoderMotor.cpp b/src/EncoderMotor.cpp index 98e2bff..e41e5e4 100644 --- a/src/EncoderMotor.cpp +++ b/src/EncoderMotor.cpp @@ -11,12 +11,17 @@ * * Tested for Adafruit Motor Shield and plain TB6612 breakout board. * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -27,9 +32,14 @@ */ #include +#if defined(USE_ENCODER_MOTOR_CONTROL) #include "EncoderMotor.h" -volatile bool EncoderMotor::EncoderTickCounterHasChanged; +//#define TRACE +//#define DEBUG + +EncoderMotor *sPointerForInt0ISR; +EncoderMotor *sPointerForInt1ISR; EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly initialized") PWMDcMotor() { @@ -37,7 +47,6 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in /* * The list version saves 100 bytes and is more flexible, compared with the array version */ - MotorValuesEepromStorageNumber = EncoderMotor::sNumberOfMotorControls; EncoderMotor::sNumberOfMotorControls++; NextMotorControl = NULL; if (sMotorControlListStart == NULL) { @@ -45,7 +54,7 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in sMotorControlListStart = this; } else { // put object in control list - EncoderMotor * tObjectPointer = sMotorControlListStart; + EncoderMotor *tObjectPointer = sMotorControlListStart; // search last list element while (tObjectPointer->NextMotorControl != NULL) { tObjectPointer = tObjectPointer->NextMotorControl; @@ -57,70 +66,71 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in } #ifdef USE_ADAFRUIT_MOTOR_SHIELD -void EncoderMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { - PWMDcMotor::init(aMotorNumber, aReadFromEeprom); // create with the default frequency 1.6KHz - resetControlValues(); +/* + * aMotorNumber from 1 to 2 + * If no parameter, we use a fixed assignment of rightCarMotor interrupts to INT0 / Pin2 and leftCarMotor to INT1 / Pin3 + * Currently motors 3 and 4 are not required/supported by own library for Adafruit Motor Shield + */ +void EncoderMotor::init(uint8_t aMotorNumber) { + PWMDcMotor::init(aMotorNumber); // create with the default frequency 1.6KHz + resetEncoderControlValues(); +} +void EncoderMotor::init(uint8_t aMotorNumber, uint8_t aInterruptNumber) { + PWMDcMotor::init(aMotorNumber); // create with the default frequency 1.6KHz + resetEncoderControlValues(); + attachInterrupt(aInterruptNumber); } #else -void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber) { - PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin, aMotorNumber); - resetControlValues(); +void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin) { + PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin); + resetEncoderControlValues(); +} + +void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aInterruptNumber) { + PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin); + resetEncoderControlValues(); + attachInterrupt(aInterruptNumber); } #endif /* - * If motor is already running, adjust TargetDistanceCount to go to aRequestedDistanceCount + * If motor is already running, adjust TargetDistanceCount to go to aRequestedDistanceMillimeter */ -void EncoderMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { -// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { -// PanicWithLed(400, 22); -// } - if (aRequestedDistanceCount == 0) { +void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + if (aRequestedDistanceMillimeter == 0) { + stop(DefaultStopMode); // In case motor was running return; } - if (CurrentSpeed == 0) { - TargetDistanceCount = aRequestedDistanceCount; -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; // This in turn resets EncoderCount etc. at first call of updateMotor() - CurrentDriveSpeed = aRequestedSpeed; - setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode -#else - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -#endif + if (CurrentSpeedPWM == 0) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + TargetDistanceMillimeter = aRequestedDistanceMillimeter; } else { - TargetDistanceCount = EncoderCount + aRequestedDistanceCount; /* - * prolong NextChangeMaxTargetCount for the new distance + * Already moving */ -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp; - // guarantee minimal ramp down length - if (tDistanceCountForRampDown < 3 && TargetDistanceCount > 6) { - tDistanceCountForRampDown = 3; - } - NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown; -#endif - PWMDcMotor::setSpeed(aRequestedSpeed, aRequestedDirection); + MotorRampState = MOTOR_STATE_DRIVE; // must be set, since we may be moving until now without ramp control + TargetDistanceMillimeter = getDistanceMillimeter() + aRequestedDistanceMillimeter; + PWMDcMotor::setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); } - LastTargetDistanceCount = TargetDistanceCount; - MotorMovesFixedDistance = true; + LastTargetDistanceMillimeter = TargetDistanceMillimeter; + CheckDistanceInUpdateMotor = true; } -void EncoderMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } /* - * if aRequestedDistanceCount < 0 then use DIRECTION_BACKWARD - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. + * if aRequestedDistanceMillimeter < 0 then use DIRECTION_BACKWARD + * initialize motorInfo fields DirectionForward, RequestedDriveSpeedPWM, DistanceTickCounter and optional NextChangeMaxTargetCount. */ -void EncoderMotor::startGoDistanceCount(int aRequestedDistanceCount) { - if (aRequestedDistanceCount < 0) { - aRequestedDistanceCount = -aRequestedDistanceCount; - startGoDistanceCount(aRequestedDistanceCount, DIRECTION_BACKWARD); +void EncoderMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceCount(aRequestedDistanceCount, DIRECTION_FORWARD); + startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } @@ -129,164 +139,189 @@ void EncoderMotor::startGoDistanceCount(int aRequestedDistanceCount) { */ bool EncoderMotor::updateMotor() { unsigned long tMillis = millis(); -#ifdef SUPPORT_RAMP_UP - uint8_t tNewSpeed = CurrentSpeed; + uint8_t tNewSpeedPWM = CurrentSpeedPWM; if (MotorRampState == MOTOR_STATE_START) { - // --> RAMP_UP - MotorRampState = MOTOR_STATE_RAMP_UP; - LastRideEncoderCount = 0; - EncoderCount = 0; + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; /* * Start motor */ - tNewSpeed = StartSpeed; - NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS; - NextChangeMaxTargetCount = TargetDistanceCount / 2; - // initialize for timeout detection - EncoderTickLastMillis = tMillis - ENCODER_SENSOR_MASK_MILLIS - 1; - - RampDelta = RAMP_UP_VALUE_DELTA; // 16 steps a 16 millis for ramp up => 256 milliseconds - if (RampDelta < 2) { - RampDelta = 2; + if (RequestedDriveSpeedPWM > RAMP_VALUE_UP_OFFSET_SPEED_PWM) { + // start with ramp to avoid spinning wheels + tNewSpeedPWM = RAMP_VALUE_UP_OFFSET_SPEED_PWM; // start immediately with speed offset (3 volt) + // --> RAMP_UP + MotorRampState = MOTOR_STATE_RAMP_UP; + } else { + // Motor ramp not required, go direct to drive speed. + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; } - DebugCount = 0; - Debug = 0; - } - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_RAMP_UP) { /* - * Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds - * or until more than half of distance is done - * Distance required for ramp is 0 to 10 or more, increasing with increasing CurrentDriveSpeed + * Init Encoder values */ - if (tMillis >= NextRampChangeMillis) { - NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS; - tNewSpeed = tNewSpeed + RampDelta; - // Clip value and check for 8 bit overflow - if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) { - tNewSpeed = CurrentDriveSpeed; - } + LastRideEncoderCount = 0; + EncoderCount = 0; + // initialize for timeout detection + LastEncoderInterruptMillis = tMillis - ENCODER_SENSOR_RING_MILLIS - 1; + } else if (MotorRampState == MOTOR_STATE_RAMP_UP) { + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis += RAMP_INTERVAL_MILLIS; /* - * Transition criteria is: - * Max Speed reached or more than half of distance is done + * Increase motor speed by RAMP_UP_VALUE_DELTA every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + * Transition criteria to next state is: + * Drive speed reached or target distance - braking distance reached */ - if (tNewSpeed == CurrentDriveSpeed || (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount)) { - // --> DRIVE_SPEED - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - - DistanceCountAfterRampUp = EncoderCount; - uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp; - // guarantee minimal ramp down length - if (tDistanceCountForRampDown < 3 && TargetDistanceCount > 6) { - tDistanceCountForRampDown = 3; + if (tNewSpeedPWM == RequestedDriveSpeedPWM + || (CheckDistanceInUpdateMotor + && getDistanceMillimeter() + getBrakingDistanceMillimeter() >= TargetDistanceMillimeter)) { + // RequestedDriveSpeedPWM reached switch to --> DRIVE_SPEED_PWM and check immediately for next transition to RAMP_DOWN + MotorRampState = MOTOR_STATE_DRIVE; + } else { + tNewSpeedPWM = tNewSpeedPWM + RampSpeedPWMDelta; + // Clip value and check for 8 bit overflow + if (tNewSpeedPWM > RequestedDriveSpeedPWM || tNewSpeedPWM <= RampSpeedPWMDelta) { + // do not change state here to let motor run at RequestedDriveSpeedPWM for one interval + tNewSpeedPWM = RequestedDriveSpeedPWM; } - NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown; } +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } } - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) { + // do not use "else if" since we must immediately check for next transition to RAMP_DOWN + if (MotorRampState == MOTOR_STATE_DRIVE) { /* - * Wait until ramp down count is reached + * Wait until target distance - braking distance reached */ - if (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount) { + if (CheckDistanceInUpdateMotor && getDistanceMillimeter() + getBrakingDistanceMillimeter() >= TargetDistanceMillimeter) { + if (CurrentSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + tNewSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + } else { + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } // --> RAMP_DOWN MotorRampState = MOTOR_STATE_RAMP_DOWN; - /* - * Ramp to reach StartSpeed after 1/2 of remaining distance - * TODO reduce speed just here? e.g. to 1.5 of start speed? - */ - RampDeltaPerDistanceCount = ((CurrentSpeed - StartSpeed) * 2) / ((TargetDistanceCount - EncoderCount)) + 1; +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Dist=")); + Serial.print(getDistanceMillimeter()); + Serial.print(F(" Breakdist=")); + Serial.print(getBrakingDistanceMillimeter()); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } } - // do not use else if since state can be changed in code before + // do not use "else if" since we must immediately check for next transition to STOPPED if (MotorRampState == MOTOR_STATE_RAMP_DOWN) { - DebugCount++; - - /* - * Decrease motor speed depending on distance to target count - */ - if (EncoderCount >= NextChangeMaxTargetCount) { - Debug++; - NextChangeMaxTargetCount++; - if (tNewSpeed > RampDeltaPerDistanceCount) { - tNewSpeed -= RampDeltaPerDistanceCount; + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Decrease motor speed RAMP_UP_UPDATE_INTERVAL_STEPS times every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tNewSpeedPWM > (RAMP_DOWN_VALUE_DELTA + RAMP_VALUE_DOWN_MIN_SPEED_PWM)) { + tNewSpeedPWM -= RAMP_DOWN_VALUE_DELTA; } else { - tNewSpeed = StartSpeed; - } - // safety net for slow speed - if (tNewSpeed < StartSpeed) { - tNewSpeed = StartSpeed; + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; } +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } + } // End of motor state machine - if (tNewSpeed != CurrentSpeed) { - PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode); - } +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.println(MotorRampState); +#endif + if (tNewSpeedPWM != CurrentSpeedPWM) { +#ifdef TRACE + Serial.print(F("Ns=")); + Serial.println(tNewSpeedPWM); #endif + PWMDcMotor::setSpeedPWM(tNewSpeedPWM, CurrentDirectionOrBrakeMode); + } /* - * Check if target count is reached or encoder tick timeout + * Check if target distance is reached or encoder tick timeout */ - if (CurrentSpeed > 0) { - if (MotorMovesFixedDistance - && (EncoderCount >= TargetDistanceCount || tMillis > (EncoderTickLastMillis + ENCODER_TICKS_TIMEOUT_MILLIS))) { -#ifdef SUPPORT_RAMP_UP - DebugSpeedAtTargetCountReached = CurrentSpeed; - MotorRampState = MOTOR_STATE_STOPPED; + if (CurrentSpeedPWM > 0) { + if (CheckDistanceInUpdateMotor + && (getDistanceMillimeter() >= TargetDistanceMillimeter + || tMillis > (LastEncoderInterruptMillis + ENCODER_SENSOR_TIMEOUT_MILLIS))) { + stop(MOTOR_BRAKE); // this sets MOTOR_STATE_STOPPED; +#ifdef DEBUG + Serial.print(PWMPin); + if(tMillis > (LastEncoderInterruptMillis + ENCODER_SENSOR_TIMEOUT_MILLIS)){ + Serial.print(F(" Encoder timeout: dist=")); + } else{ + Serial.print(F(" Reached: dist=")); + } + Serial.print(getDistanceMillimeter()); + Serial.print(F(" Breakdist=")); + Serial.println(getBrakingDistanceMillimeter()); #endif - stop(MOTOR_BRAKE); - return false; + return false; // need no more calls to update() } + return true; // still running } - - return true; + return false; // current speed == 0 } /* * Computes motor speed compensation value in order to go exactly straight ahead * Compensate only at forward direction */ -void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned int aCheckInterval) { - if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) { +void EncoderMotor::synchronizeMotor(EncoderMotor *aOtherMotorControl, unsigned int aCheckInterval) { + if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD || aOtherMotorControl->CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) { return; } static long sNextMotorSyncMillis; long tMillis = millis(); if (tMillis >= sNextMotorSyncMillis) { sNextMotorSyncMillis += aCheckInterval; -#ifdef SUPPORT_RAMP_UP // only synchronize if manually operated or at full speed - if ((MotorRampState == MOTOR_STATE_STOPPED && aOtherMotorControl->MotorRampState == MOTOR_STATE_STOPPED && CurrentSpeed > 0) - || (MotorRampState == MOTOR_STATE_DRIVE_SPEED && aOtherMotorControl->MotorRampState == MOTOR_STATE_DRIVE_SPEED)) { -#endif - MotorValuesHaveChanged = false; + if ((MotorRampState == MOTOR_STATE_STOPPED && aOtherMotorControl->MotorRampState == MOTOR_STATE_STOPPED && CurrentSpeedPWM > 0) + || (MotorRampState == MOTOR_STATE_DRIVE && aOtherMotorControl->MotorRampState == MOTOR_STATE_DRIVE)) { + MotorControlValuesHaveChanged = false; if (EncoderCount >= (aOtherMotorControl->EncoderCount + 2)) { EncoderCount = aOtherMotorControl->EncoderCount; /* * This motor is too fast, first try to reduce other motors compensation */ - if (aOtherMotorControl->SpeedCompensation >= 2) { - aOtherMotorControl->SpeedCompensation -= 2; - aOtherMotorControl->CurrentSpeed += 2; - aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + if (aOtherMotorControl->SpeedPWMCompensation >= 2) { + aOtherMotorControl->SpeedPWMCompensation -= 2; + aOtherMotorControl->CurrentSpeedPWM += 2; + aOtherMotorControl->setSpeedPWM(aOtherMotorControl->CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; EncoderCount = aOtherMotorControl->EncoderCount; - } else if (CurrentSpeed > StartSpeed) { + } else if (CurrentSpeedPWM > StartSpeedPWM) { /* * else increase this motors compensation */ - SpeedCompensation += 2; - CurrentSpeed -= 2; - PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + SpeedPWMCompensation += 2; + CurrentSpeedPWM -= 2; + PWMDcMotor::setSpeedPWM(CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; } } else if (aOtherMotorControl->EncoderCount >= (EncoderCount + 2)) { @@ -294,28 +329,22 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned /* * Other motor is too fast, first try to reduce this motors compensation */ - if (SpeedCompensation >= 2) { - SpeedCompensation -= 2; - CurrentSpeed += 2; - PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; - } else if (aOtherMotorControl->CurrentSpeed > aOtherMotorControl->StartSpeed) { + if (SpeedPWMCompensation >= 2) { + SpeedPWMCompensation -= 2; + CurrentSpeedPWM += 2; + PWMDcMotor::setSpeedPWM(CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; + } else if (aOtherMotorControl->CurrentSpeedPWM > aOtherMotorControl->StartSpeedPWM) { /* * else increase other motors compensation */ - aOtherMotorControl->SpeedCompensation += 2; - aOtherMotorControl->CurrentSpeed -= 2; - aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + aOtherMotorControl->SpeedPWMCompensation += 2; + aOtherMotorControl->CurrentSpeedPWM -= 2; + aOtherMotorControl->setSpeedPWM(aOtherMotorControl->CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; } } - - if (MotorValuesHaveChanged) { - writeMotorvaluesToEeprom(); - } -#ifdef SUPPORT_RAMP_UP } -#endif } } @@ -325,36 +354,225 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned /* * Reset all control values as distances, debug values etc. to 0x00 */ -void EncoderMotor::resetControlValues() { - memset(reinterpret_cast(&CurrentVelocity), 0, - (((uint8_t *) &Debug) + sizeof(Debug)) - reinterpret_cast(&CurrentVelocity)); +void EncoderMotor::resetEncoderControlValues() { + memset(reinterpret_cast(&TargetDistanceMillimeter), 0, + (((uint8_t*) &Debug) + sizeof(Debug)) - reinterpret_cast(&TargetDistanceMillimeter)); // to force display of initial values - EncoderTickCounterHasChanged = true; + SensorValuesHaveChanged = true; } /*************************************************** * Encoder functions ***************************************************/ +/* + * Attaches INT0 or INT1 interrupt to this EncoderMotor + * Interrupt is enabled on rising edges + * We can not use both edges since the on and off times of the opto interrupter are too different + * aInterruptNumber can be one of INT0 (at pin D2) or INT1 (at pin D3) for Atmega328 + */ +void EncoderMotor::attachInterrupt(uint8_t aInterruptNumber) { + if (aInterruptNumber > 1) { + return; + } + + if (aInterruptNumber == 0) { + sPointerForInt0ISR = this; +// interrupt on any logical change + EICRA |= (_BV(ISC00) | _BV(ISC01)); +// clear interrupt bit + EIFR |= _BV(INTF0); +// enable interrupt on next change + EIMSK |= _BV(INT0); + } else { + sPointerForInt1ISR = this; + EICRA |= (_BV(ISC10) | _BV(ISC11)); + EIFR |= _BV(INTF1); + EIMSK |= _BV(INT1); + } +} + +/* + * Reset EncoderInterruptDeltaMillis, EncoderInterruptMillisArray, MillisArrayIndex and AverageSpeedIsValid + */ +void EncoderMotor::resetSpeedValues() { +#ifdef SUPPORT_AVERAGE_SPEED + memset((void*) &EncoderInterruptDeltaMillis, 0, + ((uint8_t*) &AverageSpeedIsValid + sizeof(AverageSpeedIsValid)) - (uint8_t*) &EncoderInterruptDeltaMillis); +#else + EncoderInterruptDeltaMillis = 0; +#endif +} + +uint8_t EncoderMotor::getDirection() { + return LastDirection; +} + +unsigned int EncoderMotor::getDistanceMillimeter() { + return LastRideEncoderCount * FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT; +} + +unsigned int EncoderMotor::getDistanceCentimeter() { + return (LastRideEncoderCount * FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT) / 10; +} + +unsigned int EncoderMotor::getBrakingDistanceMillimeter() { + unsigned int tSpeedCmPerSecond = getSpeed(); +// return (tSpeedCmPerSecond * tSpeedCmPerSecond * 100) / RAMP_DECELERATION_TIMES_2; // overflow! + return (tSpeedCmPerSecond * tSpeedCmPerSecond) / (RAMP_DECELERATION_TIMES_2 / 100); +} + +/* + * Speed is in cm/s for a 20 slot encoder disc + * Reset speed values after 1 second + */ +unsigned int EncoderMotor::getSpeed() { + if (millis() - LastEncoderInterruptMillis > 1000) { + resetSpeedValues(); // Reset speed values after 1 second + } + unsigned long tEncoderInterruptDeltaMillis = EncoderInterruptDeltaMillis; + if (tEncoderInterruptDeltaMillis == 0) { + return 0; + } + return (SPEED_SCALE_VALUE / tEncoderInterruptDeltaMillis); +} + +#ifdef SUPPORT_AVERAGE_SPEED +/* + * Speed is in cm/s for a 20 slot encoder disc + * Average is computed over the full revolution to compensate for unequal distances of the laser cut encoder discs. + * If we do not have 21 timestamps, average is computed over the existing ones + */ +unsigned int EncoderMotor::getAverageSpeed() { + int tAverageSpeed = 0; + /* + * First check for timeout + */ + if (millis() - LastEncoderInterruptMillis > 1000) { + resetSpeedValues(); // Reset speed values after 1 second + } else { + int8_t tMillisArrayIndex = MillisArrayIndex; + if (!AverageSpeedIsValid) { + tMillisArrayIndex--; + if (tMillisArrayIndex > 0) { + // here MillisArray is not completely filled and MillisArrayIndex had no wrap around + tAverageSpeed = (SPEED_SCALE_VALUE * tMillisArrayIndex) + / (EncoderInterruptMillisArray[tMillisArrayIndex] - EncoderInterruptMillisArray[0]); + } + } else { + // tMillisArrayIndex points to the next value to write == the oldest value to overwrite + unsigned long tOldestEncoderInterruptMillis = EncoderInterruptMillisArray[tMillisArrayIndex]; + + // get index of current value + tMillisArrayIndex--; + if (tMillisArrayIndex < 0) { + // wrap around + tMillisArrayIndex = AVERAGE_SPEED_BUFFER_SIZE - 1; + } + tAverageSpeed = (SPEED_SCALE_VALUE * AVERAGE_SPEED_SAMPLE_SIZE) + / (EncoderInterruptMillisArray[tMillisArrayIndex] - tOldestEncoderInterruptMillis); + } + } + return tAverageSpeed; +} + +/* + * @param aLengthOfAverage only values from 1 to 20 are valid! + */ +unsigned int EncoderMotor::getAverageSpeed(uint8_t aLengthOfAverage) { + if (!AverageSpeedIsValid && MillisArrayIndex < aLengthOfAverage) { + // cannot compute requested average + return 0; + } + // get index of aLengthOfAverage counts before + int8_t tHistoricIndex = (MillisArrayIndex - 1) - aLengthOfAverage; + if (tHistoricIndex < 0) { + // wrap around + tHistoricIndex += AVERAGE_SPEED_BUFFER_SIZE; + } + int tAverageSpeed = (SPEED_SCALE_VALUE * aLengthOfAverage) + / (LastEncoderInterruptMillis - EncoderInterruptMillisArray[tHistoricIndex]); + + return tAverageSpeed; +} +#endif + +/* + * Print caption for Serial Plotter + * Space but NO println() at the end, to enable additional information to be printed + */ +void EncoderMotor::printEncoderDataCaption(Print *aSerial) { +// aSerial->print(F("PWM[2] Speed[cm/s] SpeedAvg.Of10[cm/s] Count[22cm/20LSB] ")); + aSerial->print(F("PWM[2] Speed[cm/s] Distance[cm] ")); +} + +bool EncoderMotor::printEncoderDataPeriodically(Print *aSerial, uint16_t aPeriodMillis) { + static unsigned long sLastPrintMillis; + + if ((millis() - sLastPrintMillis) >= aPeriodMillis) { // print data every n ms + sLastPrintMillis = millis(); + printEncoderData(aSerial); + return true; + } + return false; +} + +void EncoderMotor::printEncoderData(Print *aSerial) { + aSerial->print(CurrentSpeedPWM / 2); // = PWM, scale it for plotter + aSerial->print(" "); + aSerial->print(getSpeed()); + aSerial->print(" "); +#ifdef SUPPORT_AVERAGE_SPEED +// aSerial->print(getAverageSpeed(10)); +// aSerial->print(" "); +#endif + aSerial->print(getDistanceCentimeter()); + aSerial->print(" "); +} + void EncoderMotor::handleEncoderInterrupt() { long tMillis = millis(); - unsigned int tDeltaMillis = tMillis - EncoderTickLastMillis; - if (tDeltaMillis <= ENCODER_SENSOR_MASK_MILLIS) { -// signal is ringing - CurrentVelocity = 99; + unsigned long tDeltaMillis = tMillis - LastEncoderInterruptMillis; + if (tDeltaMillis <= ENCODER_SENSOR_RING_MILLIS) { + // assume signal is ringing and do nothing } else { - EncoderTickLastMillis = tMillis; + LastEncoderInterruptMillis = tMillis; +#ifdef SUPPORT_AVERAGE_SPEED + uint8_t tMillisArrayIndex = MillisArrayIndex; +#endif + if (tDeltaMillis < ENCODER_SENSOR_TIMEOUT_MILLIS) { + EncoderInterruptDeltaMillis = tDeltaMillis; + } else { + // timeout + EncoderInterruptDeltaMillis = 0; +#ifdef SUPPORT_AVERAGE_SPEED + tMillisArrayIndex = 0; + AverageSpeedIsValid = false; +#endif + } +#ifdef SUPPORT_AVERAGE_SPEED + EncoderInterruptMillisArray[tMillisArrayIndex++] = tMillis; + if (tMillisArrayIndex >= AVERAGE_SPEED_BUFFER_SIZE) { + tMillisArrayIndex = 0; + AverageSpeedIsValid = true; + } + MillisArrayIndex = tMillisArrayIndex; +#endif + EncoderCount++; LastRideEncoderCount++; - CurrentVelocity = VELOCITY_SCALE_VALUE / tDeltaMillis; - EncoderTickCounterHasChanged = true; + SensorValuesHaveChanged = true; } } -// The code for the interrupt is placed at the calling class since we need a fixed relation between ISR and EncoderMotor -// //ISR for PIN PD2 / RIGHT -// ISR(INT0_vect) { -// myCar.rightMotorControl.handleEncoderInterrupt(); -// } +// ISR for PIN PD2 / RIGHT +ISR(INT0_vect) { + sPointerForInt0ISR->handleEncoderInterrupt(); +} + +// ISR for PIN PD3 / LEFT +ISR(INT1_vect) { + sPointerForInt1ISR->handleEncoderInterrupt(); +} /****************************************************************************************** * Static methods @@ -362,39 +580,16 @@ void EncoderMotor::handleEncoderInterrupt() { /* * Enable both interrupts INT0/D2 or INT1/D3 */ -void EncoderMotor::enableINT0AndINT1Interrupts() { +void EncoderMotor::enableINT0AndINT1InterruptsOnRisingEdge() { // interrupt on any logical change - EICRA |= (_BV(ISC00) | _BV(ISC10)); + EICRA |= (_BV(ISC00) | _BV(ISC01) | _BV(ISC10) | _BV(ISC11)); // clear interrupt bit EIFR |= (_BV(INTF0) | _BV(INTF1)); // enable interrupt on next change EIMSK |= (_BV(INT0) | _BV(INT1)); } -/* - * Enable only one interrupt - * aIntPinNumber can be one of INT0/D2 or INT1/D3 for Atmega328 - */ -void EncoderMotor::enableInterruptOnBothEdges(uint8_t aIntPinNumber) { - if (aIntPinNumber > 1) { - return; - } - - if (aIntPinNumber == 0) { -// interrupt on any logical change - EICRA |= (_BV(ISC00)); -// clear interrupt bit - EIFR |= _BV(INTF0); -// enable interrupt on next change - EIMSK |= _BV(INT0); - } else { - EICRA |= (_BV(ISC10)); - EIFR |= _BV(INTF1); - EIMSK |= _BV(INT1); - } -} - #ifdef ENABLE_MOTOR_LIST_FUNCTIONS /* * The list version saves 100 bytes and is more flexible, compared with the array version @@ -408,7 +603,7 @@ EncoderMotor * EncoderMotor::sMotorControlListStart = NULL; *****************************************************/ bool EncoderMotor::updateAllMotors() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list bool tMotorsNotStopped = false; // to check if motors are not stopped by aLoopCallback while (tEncoderMotorControlPointer != NULL) { @@ -421,8 +616,8 @@ bool EncoderMotor::updateAllMotors() { /* * Waits until distance is reached */ -void EncoderMotor::startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::startRampUpAndWaitForDriveSpeedPWMForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { tEncoderMotorControlPointer->startRampUp(aRequestedDirection); @@ -437,11 +632,11 @@ void EncoderMotor::startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirec } while (tMotorsNotStopped && !EncoderMotor::allMotorsStarted()); } -void EncoderMotor::startGoDistanceCountForAll(int aRequestedDistanceCount) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::startGoDistanceMillimeterForAll(int aRequestedDistanceMillimeter) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); + tEncoderMotorControlPointer->startGoDistanceMillimeter(aRequestedDistanceMillimeter); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } } @@ -449,22 +644,22 @@ void EncoderMotor::startGoDistanceCountForAll(int aRequestedDistanceCount) { /* * Waits until distance is reached */ -void EncoderMotor::goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::goDistanceMillimeterForAll(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); + tEncoderMotorControlPointer->startGoDistanceMillimeter(aRequestedDistanceMillimeter); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } waitUntilAllMotorsStopped(aLoopCallback); } bool EncoderMotor::allMotorsStarted() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; bool tAllAreStarted = true; // walk through list while (tEncoderMotorControlPointer != NULL) { - if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_DRIVE_SPEED) { + if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_DRIVE) { tAllAreStarted = false; } tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; @@ -473,7 +668,7 @@ bool EncoderMotor::allMotorsStarted() { } bool EncoderMotor::allMotorsStopped() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; bool tAllAreStopped = true; // walk through list while (tEncoderMotorControlPointer != NULL) { @@ -486,17 +681,13 @@ bool EncoderMotor::allMotorsStopped() { } /* - * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN - * Use DistanceCountAfterRampUp as ramp down count - * Busy waits for stop + * Start ramp down and busy wait for stop */ void EncoderMotor::stopAllMotorsAndWaitUntilStopped() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->NextChangeMaxTargetCount = tEncoderMotorControlPointer->EncoderCount; - tEncoderMotorControlPointer->TargetDistanceCount = tEncoderMotorControlPointer->EncoderCount - + tEncoderMotorControlPointer->DistanceCountAfterRampUp; + tEncoderMotorControlPointer->startRampDown(); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } @@ -518,7 +709,7 @@ void EncoderMotor::waitUntilAllMotorsStopped(void (*aLoopCallback)(void)) { } void EncoderMotor::stopAllMotors(uint8_t aStopMode) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { tEncoderMotorControlPointer->stop(aStopMode); @@ -526,3 +717,4 @@ void EncoderMotor::stopAllMotors(uint8_t aStopMode) { } } #endif +#endif diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h index c133415..649aa8a 100644 --- a/src/EncoderMotor.h +++ b/src/EncoderMotor.h @@ -1,12 +1,17 @@ /* * EncoderMotor.h * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -22,52 +27,85 @@ #include "PWMDcMotor.h" #include +#define SUPPORT_AVERAGE_SPEED +#define AVERAGE_SPEED_SAMPLE_SIZE 20 +#define AVERAGE_SPEED_BUFFER_SIZE 21 // one more than samples, because speed is the difference between 2 samples + // maybe useful especially for more than 2 motors //#define ENABLE_MOTOR_LIST_FUNCTIONS /* - * Encoders generate 110 Hz at max speed => 8 ms per period - * since duty cycle of encoder disk impulse is 1/3 choose 3 millis as ringing mask. - * 3 ms means 2.1 to 3.0 ms depending on the Arduino ms trigger. + * 20 slot Encoder generates 4 to 5 Hz at min speed and 110 Hz at max speed => 200 to 8 ms per period + */ +#define ENCODER_COUNTS_PER_FULL_ROTATION 20 +#define ENCODER_SENSOR_TIMEOUT_MILLIS 400L // Timeout for encoder ticks if motor is running +#define ENCODER_SENSOR_RING_MILLIS 4 + +/* + * Some factors depending on wheel diameter and encoder resolution + */ +#if ! defined(FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT) +// Exact value is 220 mm / 20 = 11 +#define FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT ((DEFAULT_CIRCUMFERENCE_MILLIMETER + (ENCODER_COUNTS_PER_FULL_ROTATION / 2)) / ENCODER_COUNTS_PER_FULL_ROTATION) // = 11 +#endif + +/* + * The millis per tick have the unit [ms]/ (circumference[cm]/countsPerCircumference) -> ms/cm + * To get cm/s, use (circumference[cm]/countsPerCircumference) * 1000 / millis per tick */ -#define ENCODER_SENSOR_MASK_MILLIS 3 -#define VELOCITY_SCALE_VALUE 500 // for computing of CurrentVelocity -// Timeout for encoder ticks if motor is running -#define ENCODER_TICKS_TIMEOUT_MILLIS 500 +#define SPEED_SCALE_VALUE ((100L * DEFAULT_CIRCUMFERENCE_MILLIMETER) / ENCODER_COUNTS_PER_FULL_ROTATION) // 1100 class EncoderMotor: public PWMDcMotor { public: EncoderMotor(); #ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(uint8_t aMotorNumber, bool aReadFromEeprom = false); + void init(uint8_t aMotorNumber); + void init(uint8_t aMotorNumber, uint8_t aInterruptNumber); #else - void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber = 0); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aInterruptNumber); #endif // virtual ~EncoderMotor(); /* * Functions for going a fixed distance, they "overwrite" PWMDCMotor functions */ - void startGoDistanceCount(int aRequestedDistanceCount); // Signed distance count - void startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + 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); bool updateMotor(); /* * Functions especially for encoder motors */ - void synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned int aCheckInterval); // Computes motor speed compensation value in order to go exactly straight ahead - static void calibrate(); // Generates a rising ramp and detects the first movement -> this sets StartSpeed / dead band + void synchronizeMotor(EncoderMotor *aOtherMotorControl, unsigned int aCheckInterval); // Computes motor speed compensation value in order to go exactly straight ahead + static void calibrate(); // Generates a rising ramp and detects the first movement -> this sets StartSpeedPWM / dead band /* * Encoder interrupt handling */ void handleEncoderInterrupt(); - static void enableINT0AndINT1Interrupts(); - static void enableInterruptOnBothEdges(uint8_t aIntPinNumber); + void attachInterrupt(uint8_t aInterruptPinNumber); + static void enableINT0AndINT1InterruptsOnRisingEdge(); + + uint8_t getDirection(); + unsigned int getDistanceMillimeter(); + unsigned int getDistanceCentimeter(); + unsigned int getBrakingDistanceMillimeter(); + + unsigned int getSpeed(); +#ifdef SUPPORT_AVERAGE_SPEED + unsigned int getAverageSpeed(); + unsigned int getAverageSpeed(uint8_t aLengthOfAverage); +#endif + + void printEncoderDataCaption(Print *aSerial); + bool printEncoderDataPeriodically(Print *aSerial, uint16_t aPeriodMillis); + void printEncoderData(Print *aSerial); - void resetControlValues(); // Shutdown and reset all control values and sets direction to forward + void resetEncoderControlValues(); + void resetSpeedValues(); #ifdef ENABLE_MOTOR_LIST_FUNCTIONS /* @@ -75,9 +113,9 @@ class EncoderMotor: public PWMDcMotor { */ static bool updateAllMotors(); - static void startGoDistanceCountForAll(int aRequestedDistanceCount); - static void goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)); - static void startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); + static void startGoDistanceMillimeterForAll(int aRequestedDistanceMillimeter); + static void goDistanceMillimeterForAll(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)); + static void startRampUpAndWaitForDriveSpeedPWMForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); static void stopAllMotors(uint8_t aStopMode); static void waitUntilAllMotorsStopped(void (*aLoopCallback)(void)); @@ -95,40 +133,32 @@ class EncoderMotor: public PWMDcMotor { EncoderMotor * NextMotorControl; #endif + /************************************************************** + * Variables required for going a fixed distance with encoder + **************************************************************/ /* - * Flags for display update control - */ - volatile static bool EncoderTickCounterHasChanged; - - /********************************************************** - * Variables required for going a fixed distance - **********************************************************/ - /* - * Reset() resets all members from CurrentVelocity to (including) Debug to 0 + * Reset() resets all members from TargetDistanceCount to (including) Debug to 0 */ - int CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis - - unsigned int TargetDistanceCount; - unsigned int LastTargetDistanceCount; + unsigned int TargetDistanceMillimeter; + unsigned int LastTargetDistanceMillimeter; /* * Distance optocoupler impulse counter. It is reset at startGoDistanceCount if motor was stopped. */ volatile unsigned int EncoderCount; volatile unsigned int LastRideEncoderCount; // count of last ride - from start of MOTOR_STATE_RAMP_UP to next MOTOR_STATE_RAMP_UP - // The next value is volatile, but volatile increases the code size by 20 bites without any logical improvement - unsigned long EncoderTickLastMillis; // used for debouncing and lock/timeout detection + // Flag e.g. for display update control + volatile unsigned long LastEncoderInterruptMillis; // used internal for debouncing and lock/timeout detection -#ifdef SUPPORT_RAMP_UP /* - * For ramp and state control + * for speed computation + * Do not rearrange, since reset is done with memset(). */ - uint8_t RampDeltaPerDistanceCount; - unsigned int NextChangeMaxTargetCount; // target count at which next change must be done - - uint8_t DistanceCountAfterRampUp; // number of ticks at the transition from MOTOR_STATE_RAMP_UP to MOTOR_STATE_FULL_SPEED to be used for computing ramp down start ticks - unsigned int DebugCount; // for different debug purposes of ramp optimisation - uint8_t DebugSpeedAtTargetCountReached; // for debug of ramp down + volatile unsigned long EncoderInterruptDeltaMillis; // Used to get speed +#ifdef SUPPORT_AVERAGE_SPEED + volatile unsigned int EncoderInterruptMillisArray[AVERAGE_SPEED_BUFFER_SIZE]; // store for 20 deltas + volatile uint8_t MillisArrayIndex; // Index of the next value to write == the oldest value to overwrite. 0 to 20|(AVERAGE_SPEED_BUFFER_SIZE-1) + volatile bool AverageSpeedIsValid; // true if 11 values are written since last timeout #endif // do not delete it!!! It must be the last element in structure and is required for stopMotorAndReset() @@ -136,6 +166,9 @@ class EncoderMotor: public PWMDcMotor { }; +extern EncoderMotor * sPointerForInt0ISR; +extern EncoderMotor * sPointerForInt1ISR; + #endif /* SRC_ENCODERMOTORCONTROL_H_ */ #pragma once diff --git a/src/HCSR04.cpp b/src/HCSR04.cpp index 2e9d3d9..caabc11 100644 --- a/src/HCSR04.cpp +++ b/src/HCSR04.cpp @@ -112,7 +112,8 @@ unsigned int getUSDistance(unsigned int aTimeoutMicros) { uint8_t tEchoInPin; if (sHCSR04Mode == HCSR04_MODE_USE_1_PIN) { - delayMicroseconds(10); // allow for 10 us low before switching to input which is high because of the modules pullup resistor. + // allow for 20 us low (20 us instead of 10 us also supports the JSN-SR04T) before switching to input which is high because of the modules pullup resistor. + delayMicroseconds(20); pinMode(sTriggerOutPin, INPUT); tEchoInPin = sTriggerOutPin; } else { @@ -185,7 +186,7 @@ void testUSSensor(uint16_t aSecondsToTest) { * Result is in sUSDistanceCentimeter; */ -// Comment out the line according to the sEchoInPin if using the non blocking version +// Activate the line according to the sEchoInPin if using the non blocking version // or define it as symbol for the compiler e.g. -DUSE_PIN_CHANGE_INTERRUPT_D0_TO_D7 //#define USE_PIN_CHANGE_INTERRUPT_D0_TO_D7 // using PCINT2_vect - PORT D //#define USE_PIN_CHANGE_INTERRUPT_D8_TO_D13 // using PCINT0_vect - PORT B - Pin 13 is feedback output diff --git a/src/HomePage.cpp b/src/HomePage.cpp index 1a3e40b..52c5263 100644 --- a/src/HomePage.cpp +++ b/src/HomePage.cpp @@ -54,13 +54,13 @@ BDSlider SliderTilt; // Here we get values from 0 to 180 degrees from scaled slider #ifdef CAR_HAS_PAN_SERVO -void doHorizontalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doHorizontalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { PanServo.write(aValue); } #endif #ifdef CAR_HAS_TILT_SERVO -void doVerticalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doVerticalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { TiltServo.write(aValue); } #endif @@ -85,31 +85,31 @@ void doPlayMelody(BDButton * aTheTouchedButton, int16_t aValue) { void initHomePage(void) { - TouchButtonTestPage.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Test"), - TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_TEST, &GUISwitchPages); + TouchButtonBTSensorDrivePage.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, + F("Sensor\nDrive"), TEXT_SIZE_18, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_BT_SENSOR_CONTROL, &GUISwitchPages); - TouchButtonAutomaticDrivePage.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Automatic\nControl"), - TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_AUTOMATIC_CONTROL, &GUISwitchPages); + TouchButtonAutomaticDrivePage.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, + F("Automatic\nControl"), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_AUTOMATIC_CONTROL, &GUISwitchPages); - TouchButtonBTSensorDrivePage.init(BUTTON_WIDTH_3_POS_3, - BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_RED, - F("Sensor drive"), TEXT_SIZE_12, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_BT_SENSOR_CONTROL, &GUISwitchPages); + TouchButtonTestPage.init(BUTTON_WIDTH_3_POS_3, + BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_RED, + F("Test"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_TEST, &GUISwitchPages); #ifdef CAR_HAS_CAMERA - TouchButtonCameraOnOff.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_4, - TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Camera"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, + TouchButtonCameraOnOff.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, + TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Cam"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doCameraSupplyOnOff); #endif #ifdef ENABLE_RTTTL TouchButtonMelody.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, BUTTON_HEIGHT_8, COLOR_BLACK, F("Melody"), TEXT_SIZE_22, + BUTTON_WIDTH_3, BUTTON_HEIGHT_8, COLOR16_BLACK, F("Melody"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, sPlayMelody, &doPlayMelody); #endif #ifdef CAR_HAS_LASER TouchButtonLaser.init(0, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Laser"), TEXT_SIZE_22, + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Laser"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doLaserOnOff); #endif @@ -127,8 +127,12 @@ void drawHomePage(void) { #else char tCarTypeString[] = "2WD"; #endif - - BlueDisplay1.drawText(HEADER_X + (3 * TEXT_SIZE_22_WIDTH), (3 * TEXT_SIZE_22_HEIGHT), tCarTypeString); +#ifdef CAR_HAS_CAMERA + BlueDisplay1.drawText(HEADER_X + (2 * TEXT_SIZE_22_WIDTH), (2 * TEXT_SIZE_22_HEIGHT) + TEXT_SIZE_11_HEIGHT - 2, tCarTypeString, + TEXT_SIZE_11, COLOR16_RED, COLOR16_NO_BACKGROUND); +#else + BlueDisplay1.drawText(HEADER_X + (2 * TEXT_SIZE_22_WIDTH), (3 * TEXT_SIZE_22_HEIGHT), tCarTypeString); +#endif TouchButtonRobotCarStartStop.drawButton(); #ifdef CAR_HAS_CAMERA @@ -145,12 +149,14 @@ void drawHomePage(void) { TouchButtonAutomaticDrivePage.drawButton(); TouchButtonDirection.drawButton(); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.drawButton(); #endif TouchButtonCompensationLeft.drawButton(); TouchButtonCompensationRight.drawButton(); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonCompensationStore.drawButton(); +#endif SliderUSPosition.setValueAndDrawBar(sLastServoAngleInDegrees); SliderUSPosition.drawSlider(); @@ -173,13 +179,16 @@ void drawHomePage(void) { SliderSpeedRight.drawSlider(); SliderSpeedLeft.drawSlider(); #endif - PWMDcMotor::MotorValuesHaveChanged = true; // trigger drawing of values + PWMDcMotor::MotorControlValuesHaveChanged = true; // trigger drawing of values } void startHomePage(void) { - TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4); -#ifdef USE_ENCODER_MOTOR_CONTROL - TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4); + TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3); +#endif +#if defined(CAR_HAS_TILT_SERVO) && defined(SUPPORT_EEPROM_STORAGE) + TouchButtonCompensationStore.setPosition(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5); #endif drawHomePage(); } @@ -189,8 +198,11 @@ void loopHomePage(void) { void stopHomePage(void) { TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_6); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2); +#endif +#if defined(CAR_HAS_TILT_SERVO) && defined(SUPPORT_EEPROM_STORAGE) + TouchButtonCompensationStore.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4); #endif startStopRobotCar(false); } diff --git a/src/IMUCarData.cpp b/src/IMUCarData.cpp new file mode 100644 index 0000000..37527c8 --- /dev/null +++ b/src/IMUCarData.cpp @@ -0,0 +1,539 @@ +/* + * CarIMUData.cpp + * + * Functions for getting IMU data from MPU6050 for car control. + * + * Created on: 19.11.2020 + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include "Wire.h" +#include "IMUCarData.h" + +#define FIFO_NUMBER_OF_ACCEL_VALUES 3 +#define FIFO_NUMBER_OF_GYRO_VALUES 1 +#define FIFO_CHUNK_SIZE ((FIFO_NUMBER_OF_ACCEL_VALUES + FIFO_NUMBER_OF_GYRO_VALUES) * 2) + +//#define DEBUG_WITHOUT_SERIAL // For debugging without Serial development +#ifdef DEBUG_WITHOUT_SERIAL +#include "DigitalWriteFast.h" +#endif +//#define DEBUG // Only for development +//#define WARN // Information that the program may encounter problems, like small Heap/Stack area. +//#define AUTO_OFFSET_DEBUG // Show auto offset values as dummy values for the plotter output +#include "DebugLevel.h" // Include to propagate debug levels + +int8_t IMUCarData::getAcceleratorForward15MilliG() { + return AcceleratorForward.Byte.HighByte; +} + +int8_t IMUCarData::getAcceleratorForward4MilliG() { + return AcceleratorForward.Word >> 6; // /64 -> 256 per g +} + +int8_t IMUCarData::getAcceleratorForwardLowPass8() { +// return AcceleratorForwardLowPass8.Byte.HighByte; + return AcceleratorForwardLowPass8.Word.HighWord >> 6; // 256 per g +} + +int8_t IMUCarData::getAcceleratorForwardLowPass4() { + return AcceleratorForwardLowPass4.Word.HighWord >> 6; // 256 per g +} + +// shift 14 -> 1 (0.981) cm/s , 12 -> 2.5 mm/s, 8 -> 0.15328 mm/s +int IMUCarData::getSpeedCmPerSecond() { + return Speed.Long >> (14 - RATE_SHIFT); +} +/* + * 0.15 mm/s resolution for 1kHz sampling rate + */ +int IMUCarData::getSpeedFastWithHigherResolution() { + return Speed.Long >> 8; +} + +/* + * Distance is the sum of Speed >> 8. + * At 1 kHz: with 1 cm/s (shift (14-8)) we have 1 cm (or shift (24-8)) after 1 second (1000 additions) + */ +int IMUCarData::getDistanceCm() { +#if SAMPLE_RATE == 1000 + return Distance.Word.HighWord; // 0.958 cm since we shift by 10 and do not divide by 1000 +#else + return Distance.Long >> (16 - (2 * RATE_SHIFT)); // 0.958 cm since we shift by additional 10 and do not divide by 1000 +#endif +} + +int IMUCarData::getDistanceMillimeter() { +#if SAMPLE_RATE == 1000 + LongUnion tDistance; + tDistance.Long = Distance.Long * 10; // use full resolution for * 10 + return tDistance.Word.HighWord; // saves 10 bytes +// return ((Distance.Long * 10 ) >> 16) ; // 0.958 mm +#else + return (Distance.Long * 10) >> (16 - (2 * RATE_SHIFT)); // 0.958 cm since we shift by additional 10 and do not divide by 1000 +#endif +} + +/* + * Gyroscope + */ +int IMUCarData::getGyroscopePan2DegreePerSecond() { + return GyroscopePan.Byte.HighByte; +} + +int IMUCarData::getTurnAngleHalfDegree() { +#if SAMPLE_RATE == 1000 + return TurnAngle.Word.HighWord; +#else + return TurnAngle.Long >> (16 - RATE_SHIFT); +#endif +} + +int IMUCarData::getTurnAngleDegree() { +#if SAMPLE_RATE == 1000 + return TurnAngle.Word.HighWord >> 1; +#elif SAMPLE_RATE == 500 + return TurnAngle.Word.HighWord; +#else + return TurnAngle.Long >> (16 - (RATE_SHIFT - 1)); +#endif +} + +/* + * Print caption for Serial Plotter + * Space but NO println() at the end, to enable additional information to be printed + */ +void IMUCarData::printIMUCarDataCaption(Print *aSerial) { +// aSerial->print(F("AccelForward[4mg/LSB] AccelForwardLP8 Speed[cm/s] Distance[cm] GyroZ[2dps/LSB] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelForward[4mg/LSB] AccelForwardAverage AccelForwardLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelAvg.[4mg/LSB] AccelLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range + aSerial->print(F("AccelLP4 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +} + +/* + * read from FIFO and does auto offset (if enabled) + */ +void IMUCarData::delayAndReadIMUCarDataData(unsigned long aMillisDelay) { + while (aMillisDelay > 0) { + readCarDataFromMPU6050Fifo(); + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + aMillisDelay -= DELAY_TO_NEXT_IMU_DATA_MILLIS; + } +} + +bool IMUCarData::printIMUCarDataDataPeriodically(Print *aSerial, uint16_t aPeriodMillis) { + static unsigned long sLastPrintMillis; + + if ((millis() - sLastPrintMillis) >= aPeriodMillis) { // print data every n ms + sLastPrintMillis = millis(); + readCarDataFromMPU6050Fifo(); + printIMUCarData(aSerial); + return true; + } + return false; +} + +void IMUCarData::printIMUCarData(Print *aSerial) { +// aSerial->print(getAcceleratorForward4MilliG()); +// aSerial->print(" "); + // Too noisy +// aSerial->print(getAcceleratorForward4MilliG()); +// aSerial->print(" "); + aSerial->print(getAcceleratorForwardLowPass4()); + aSerial->print(" "); + aSerial->print(abs(getSpeedCmPerSecond())); + aSerial->print(" "); + aSerial->print(abs(getDistanceCm())); + aSerial->print(" "); +// aSerial->print(getGyroscopePan2DegreePerSecond()); +// aSerial->print(" "); + aSerial->print(getTurnAngleHalfDegree()); + aSerial->print(" "); +} + +unsigned int IMUCarData::getMPU6050SampleRate() { + return (SAMPLE_RATE); +} + +/* + * Read raw 14 vales. Requires 500 us. + * Sets AcceleratorForward and GyroscopePan + */ +void IMUCarData::readCarDataFromMPU6050() { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(MPU6050_RA_ACCEL_XOUT_H); + Wire.endTransmission(false); + Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) 14, (uint8_t) true); + +#ifdef USE_ACCELERATOR_Y_FOR_SPEED +// skip x value + Wire.read(); + Wire.read(); +#endif +// read forward value + AcceleratorForward.Byte.HighByte = Wire.read(); + AcceleratorForward.Byte.LowByte = Wire.read(); +#ifdef USE_NEGATIVE_ACCELERATION_FOR_SPEED + AcceleratorForward.Word = (-AcceleratorForward.Word) - AcceleratorForwardOffset; +#else + AcceleratorForward.Word = AcceleratorForward.Word - AcceleratorForwardOffset; +#endif + + AcceleratorForwardLowPass8.Long += ((((int32_t) AcceleratorForward.Word) << 16) - AcceleratorForwardLowPass8.Long) >> 8; // Fixed point 2.0 us + AcceleratorForwardLowPass4.Long += ((((int32_t) AcceleratorForward.Word) << 16) - AcceleratorForwardLowPass4.Long) >> 4; // Fixed point 2.0 us + Speed.Long += AcceleratorForward.Word; + Distance.Long += Speed.Long >> 8; + +#ifndef USE_ACCELERATOR_Y_FOR_SPEED + // skip y value + Wire.read(); + Wire.read(); +#endif + +// skip z, temp and 2 gyroscope values value + for (uint8_t i = 0; i < 8; i++) { + Wire.read(); + } + +// read pan (Z) value + GyroscopePan.Byte.HighByte = Wire.read(); + GyroscopePan.Byte.LowByte = Wire.read(); + GyroscopePan.Word -= GyroscopePanOffset; + TurnAngle.Long += GyroscopePan.Word; +} + +/* + * @return true, if data might have changed + */ +bool IMUCarData::readCarDataFromMPU6050Fifo() { +#ifdef DEBUG_WITHOUT_SERIAL + digitalToggleFast(12); +#endif + if (millis() - LastFifoCheckMillis < DELAY_TO_NEXT_IMU_DATA_MILLIS) { + return false; // no new data expected + } + + /* + * Read FIFO count only once at start of the function + */ + uint16_t tFifoCount = MPU6050ReadWordSwapped(MPU6050_RA_FIFO_COUNTH); + LastFifoCheckMillis = millis(); + if (tFifoCount == 1024) { + resetMPU6050FifoAndCarData(); + return true; + } + /* + * 660 /1200 us @400kHz for 16 / 32 byte transfer. + * 140 us between each block transfer + * 1000 us for next 32 bytes transfer => ~3.1 ms for 10 chunks + */ +#ifdef DEBUG + Serial.print(tFifoCount); + Serial.print("|"); +#endif + int32_t tAcceleratorForward = 0; + int32_t tGyroscopePan = 0; + int8_t tReadChunckCount = 0; + while (tFifoCount >= FIFO_CHUNK_SIZE) { + /* + * Use only multiple of FIFO_CHUNK_SIZE as read length and clip read length to I2C BUFFER_LENGTH + */ + uint8_t tChunkCount = tFifoCount / FIFO_CHUNK_SIZE; + if (tChunkCount > BUFFER_LENGTH / FIFO_CHUNK_SIZE) { + tChunkCount = BUFFER_LENGTH / FIFO_CHUNK_SIZE; // we can get 4 chunks of data with the 32 byte buffer of the Wire library + } + // this reads max 28 bytes or 4 chunks + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(MPU6050_RA_FIFO_R_W); + Wire.endTransmission(false); + uint8_t tReceivedCount = Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) (tChunkCount * FIFO_CHUNK_SIZE), + (uint8_t) true); +#ifdef WARN + if (tReceivedCount != (tChunkCount * FIFO_CHUNK_SIZE)) { + // should never happen + Serial.print(tReceivedCount); + Serial.print(F(" received bytes are not equal requested quantity of ")); + Serial.println(tChunkCount * FIFO_CHUNK_SIZE); + } +#endif + for (uint8_t j = 0; j < tReceivedCount; j += FIFO_CHUNK_SIZE) { + // we must read all 3 values + for (uint8_t i = 0; i < FIFO_NUMBER_OF_ACCEL_VALUES; i++) { + WordUnion tAcceleratorValue; + tAcceleratorValue.Byte.HighByte = Wire.read(); + tAcceleratorValue.Byte.LowByte = Wire.read(); + // process only forward value +#ifdef USE_ACCELERATOR_Y_FOR_SPEED + if (i == 1) { +#else + if (i == 0) { +#endif +#ifdef USE_NEGATIVE_ACCELERATION_FOR_SPEED + tAcceleratorValue.Word = (-tAcceleratorValue.Word) - AcceleratorForwardOffset; +#else + tAcceleratorValue.Word = tAcceleratorValue.Word - AcceleratorForwardOffset; +#endif + tAcceleratorForward += tAcceleratorValue.Word; + AcceleratorForwardLowPass8.Long += + ((((int32_t) tAcceleratorValue.Word) << 16) - AcceleratorForwardLowPass8.Long) >> 8; // Fixed point 2.0 us + AcceleratorForwardLowPass4.Long += + ((((int32_t) tAcceleratorValue.Word) << 16) - AcceleratorForwardLowPass4.Long) >> 4; // Fixed point 2.0 us + + Speed.Long += tAcceleratorValue.Word; + Distance.Long += Speed.Long >> 8; + tReadChunckCount++; + } + } + WordUnion tValue; + tValue.Byte.HighByte = Wire.read(); + tValue.Byte.LowByte = Wire.read(); + tValue.Word = tValue.Word - GyroscopePanOffset; + tGyroscopePan += tValue.Word; + // Compute turn angle + TurnAngle.Long += tValue.Word; + } + tFifoCount -= tReceivedCount; + + } // while tFifoCount >= FIFO_CHUNK_SIZE + sCountOfUndisturbedFifoChunks += tReadChunckCount; + // compute average of read values + AcceleratorForward.Word = tAcceleratorForward / (int32_t) tReadChunckCount; + GyroscopePan.Word = tGyroscopePan / (int32_t) tReadChunckCount; + + /* + * Get initial offset values + */ + if (AcceleratorForwardOffset == 0 && sCountOfUndisturbedFifoChunks >= NUMBER_OF_OFFSET_CALIBRATION_SAMPLES) { + /* + * Take the first NUMBER_OF_OFFSET_CALIBRATION_SAMPLES values for auto offset + * My experience is, that the Gyro offset is changing in the first seconds after power up and needs a recalibration + */ + AcceleratorForwardOffset = Speed.Long / sCountOfUndisturbedFifoChunks; + GyroscopePanOffset = TurnAngle.Long / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; + resetCarData(); // reset temporarily used values +#ifdef DEBUG + printSpeedAndTurnOffsets(&Serial); +#endif + } + +#ifndef DISABLE_AUTO_OFFSET + doAutoOffset(); +#endif + return true; +} + +/* + * Automatic offset acquisition 100 ms after boot and offset correction if no acceleration takes place + */ +void IMUCarData::doAutoOffset() { + // do not auto adjust if we have no initial offsets, which are required for decision if sensor has not moved + if (AcceleratorForwardOffset != 0) { + /* + * Adjust Offsets, if sensor has not moved after NUMBER_OF_OFFSET_CALIBRATION_SAMPLES + * Requires 420 bytes FLASH + */ + int16_t tAccelDifference = abs(AcceleratorForwardLowPass8.Word.HighWord - AcceleratorForward.Word); + if (tAccelDifference > ACCEL_MOVE_THRESHOLD || GyroscopePan.Word < -(GYRO_MOVE_THRESHOLD) + || GYRO_MOVE_THRESHOLD < GyroscopePan.Word) { // using abs() costs 6 byte FLASH +#ifdef AUTO_OFFSET_DEBUG + // just to show the removed deltas in Arduino Plotter + if (AcceleratorForward.Word < -(ACCEL_MOVE_THRESHOLD) || ACCEL_MOVE_THRESHOLD < AcceleratorForward.Word) { + AcceleratorForward.Word = 64 * 10 * ((Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks); + } else { + GyroscopePan.Word = 10 * 256 * ((TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks); + } +#endif + sCountOfUndisturbedFifoChunks = 0; // reset count + sSpeedSnapshot = Speed.Long; + sTurnSnapshot = TurnAngle.Long; + } else { + /* + * Do we have enough samples for auto offset? + */ + if (sCountOfUndisturbedFifoChunks >= NUMBER_OF_OFFSET_CALIBRATION_SAMPLES) { + if (abs(Speed.Long - sSpeedSnapshot) > sCountOfUndisturbedFifoChunks) { + // difference is higher than 1 per sample, so adjust accelerator offset + AcceleratorForwardOffset += (Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; +#ifdef AUTO_OFFSET_DEBUG + // just to show in Arduino Plotter - 5 for each accelerator offset increment + AcceleratorForward.Word = 64 * 5 * ((Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks); +#endif + sSpeedSnapshot = 0; + Speed.Long = 0; + Distance.Long = 0; + + sCountOfUndisturbedFifoChunks = 0; // reset count + } + + if (abs(TurnAngle.Long - sTurnSnapshot) > sCountOfUndisturbedFifoChunks) { + // adjust gyroscope offset and reset gyroscope to last value + GyroscopePanOffset += (TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; +#ifdef AUTO_OFFSET_DEBUG + // just to show in Arduino Plotter - 5 for each gyroscope offset increment + GyroscopePan.Word = 5 * 256 * ((TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks); +#endif + TurnAngle.Long = sTurnSnapshot; + + sCountOfUndisturbedFifoChunks = 0; // reset count + } + } + } + } +} + +/* + * Read all data at 1 kHz rate direct from registers, not from FIFO + * and compute and store only the required values in one turn. + */ +void IMUCarData::calculateSpeedAndTurnOffsets() { + + resetCarData(); + AcceleratorForwardOffset = 0; + GyroscopePanOffset = 0; + + uint32_t LastDataMillis = millis(); + for (int i = 0; i < NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; i++) { + // get data every ms + while (millis() == LastDataMillis) { + ; + } + LastDataMillis = millis(); + readCarDataFromMPU6050(); + } + AcceleratorForwardOffset = Speed.Long / NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; + GyroscopePanOffset = TurnAngle.Long / NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; + + resetCarData(); +} + +void IMUCarData::printSpeedAndTurnOffsets(Print *aSerial) { + aSerial->print(F("Speed offset=")); + aSerial->print(AcceleratorForwardOffset); + aSerial->print('|'); + aSerial->print(AcceleratorForwardOffset * ACCEL_RAW_TO_G_FOR_2G_RANGE); + aSerial->print(F(" turn offset=")); + aSerial->print(GyroscopePanOffset); + aSerial->print('|'); + aSerial->println(GyroscopePanOffset * GYRO_RAW_TO_DEGREE_PER_SECOND_FOR_250DPS_RANGE); +} + +void IMUCarData::reset() { + initMPU6050FifoForCarData(); // resets also CarData +} + +void IMUCarData::resetCarData() { + AcceleratorForwardLowPass8.ULong = 0; + AcceleratorForwardLowPass4.ULong = 0; + Speed.ULong = 0; + Distance.Long = 0; + TurnAngle.ULong = 0; +} + +void IMUCarData::resetOffsetData() { + /* + * Reset both offset data, since they are used to comupte the new sums, which in turn gives the new offsets + */ + AcceleratorForwardOffset = 0; + GyroscopePanOffset = 0; + sCountOfUndisturbedFifoChunks = 0; + resetMPU6050FifoAndCarData(); // flush FIFO content and reset car data. Speed.Long and TurnAngle.Long serve as temporarily accumulator for offset value +} + +void IMUCarData::resetOffsetDataAndWait() { + resetOffsetData(); + while (AcceleratorForwardOffset == 0) { + readCarDataFromMPU6050Fifo(); + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + } +} + +void IMUCarData::resetMPU6050FifoAndCarData() { + MPU6050WriteByte(MPU6050_RA_USER_CTRL, _BV(MPU6050_USERCTRL_FIFO_RESET_BIT)); // Reset FIFO + MPU6050WriteByte(MPU6050_RA_USER_CTRL, _BV(MPU6050_USERCTRL_FIFO_EN_BIT)); // enable FIFO + resetCarData(); // values might be invalid +} + +void IMUCarData::initMPU6050FifoForCarData() { + initMPU6050(); + MPU6050WriteByte(MPU6050_RA_FIFO_EN, _BV(MPU6050_ACCEL_FIFO_EN_BIT) | _BV(MPU6050_ZG_FIFO_EN_BIT)); // FIFO: all Accel axes + Gyro Z enabled + resetOffsetData(); +} + +/* + * I2C fast mode is supported by MPU6050 + */ +void IMUCarData::initWire() { + Wire.begin(); + Wire.setClock(400000); + Wire.setWireTimeout(5000); // Sets timeout to 5 ms. default is 25 ms. +} + +void IMUCarData::initMPU6050() { + initWire(); + MPU6050WriteByte(MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO); // use recommended gyro reference: PLL with Z axis gyroscope reference +#if SAMPLE_RATE == 1000 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x00); // // Set sample rate to 1 kHz, divider is minimum (1) + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_188); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz +#elif SAMPLE_RATE == 500 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x01); // // Set sample rate to 1/2 kHz, divider is 2 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42); // ext input disabled, DLPF enabled: ~50 Hz Sample freq = 1kHz +#elif SAMPLE_RATE == 250 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x03); // // Set sample rate to 1/4 kHz, divider is 4 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_20); // ext input disabled, DLPF enabled: ~20 Hz Sample freq = 1kHz +#else + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x07); // // Set sample rate to 1/8 kHz, divider is 8 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_10); // ext input disabled, DLPF enabled: ~10 Hz Sample freq = 1kHz +#endif + +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_10); // ext input disabled, DLPF enabled: ~10 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_20); // ext input disabled, DLPF enabled: ~20 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42); // ext input disabled, DLPF enabled: ~50 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_98); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_188); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz + +// the next is default and never changed by us +// MPU6050WriteByte(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_2 << (MPU6050_ACONFIG_AFS_SEL_BIT - MPU6050_ACONFIG_AFS_SEL_LENGTH + 1)); // range +/- 2g +// MPU6050WriteByte(MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_250 << (MPU6050_GCONFIG_FS_SEL_BIT - MPU6050_GCONFIG_FS_SEL_LENGTH + 1)); // range +/- 250 - default +} + +void IMUCarData::MPU6050WriteByte(uint8_t aRegisterNumber, uint8_t aData) { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(aRegisterNumber); + Wire.write(aData); + Wire.endTransmission(); +} + +/* + * Read high byte first + */ +uint16_t IMUCarData::MPU6050ReadWordSwapped(uint8_t aRegisterNumber) { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(aRegisterNumber); + Wire.endTransmission(false); + Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) 2, (uint8_t) true); + WordUnion tWord; + tWord.UByte.HighByte = Wire.read(); + tWord.UByte.LowByte = Wire.read(); + return tWord.UWord; +} diff --git a/src/IMUCarData.h b/src/IMUCarData.h new file mode 100644 index 0000000..1db04c7 --- /dev/null +++ b/src/IMUCarData.h @@ -0,0 +1,158 @@ +/* + * CarIMUData.h + * + * Functions for getting IMU data from MPU6050 for car control. + * + * Created on: 19.11.2020 + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef CAR_IMU_DATA_H_ +#define CAR_IMU_DATA_H_ + +#include +#include "MPU6050Defines.h" +#include "LongUnion.h" + +/* + * Activate this if the y axis of the GY-521 MPU6050 breakout board points forward / backward, i.e. connectors are at the right. + */ +// #define USE_ACCELERATOR_Y_FOR_SPEED +/* + * Activate this if the axis of the GY-521 MPU6050 breakout board points backwards. + */ +// #define USE_NEGATIVE_ACCELERATION_FOR_SPEED +// #define DISABLE_AUTO_OFFSET // default is enabled +// +#define NUMBER_OF_OFFSET_CALIBRATION_SAMPLES 512 +#define ACCEL_MOVE_THRESHOLD 128 +#define GYRO_MOVE_THRESHOLD 64 + +#ifndef SAMPLE_RATE +#define SAMPLE_RATE 1000 +//#define SAMPLE_RATE 500 +//#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 +#elif SAMPLE_RATE == 500 +#define RATE_SHIFT 1 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 2 +#elif SAMPLE_RATE == 250 +#define RATE_SHIFT 2 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 4 +#elif SAMPLE_RATE == 125 +#define RATE_SHIFT 3 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 8 +#else +#error SAMPLE_RATE must be 1000, 500, 250 or 125 +#endif + +#define ACCEL_RAW_TO_G_FOR_2G_RANGE (4.0/65536.0) +#define GYRO_RAW_TO_DEGREE_PER_SECOND_FOR_250DPS_RANGE (500.0/65536.0) // 0.00762939 or 1/131.072 + +class IMUCarData { +public: + +// IMUCarData(); + + void initWire(); // Is called by initMPU6050() + void initMPU6050(); + void initMPU6050FifoForCarData(); + unsigned int getMPU6050SampleRate(); // just returns SAMPLE_RATE + + void resetCarData(); + void resetMPU6050FifoAndCarData(); + void reset(); + void resetOffsetData(); + void resetOffsetDataAndWait(); + + void readCarDataFromMPU6050(); + bool readCarDataFromMPU6050Fifo(); + + void delayAndReadIMUCarDataData(unsigned long aMillisDelay); + + void printIMUCarDataCaption(Print *aSerial); + bool printIMUCarDataDataPeriodically(Print *aSerial, uint16_t aPeriodMillis); + void printIMUCarData(Print *aSerial); + + int8_t getAcceleratorForward15MilliG(); + int8_t getAcceleratorForward4MilliG(); + int8_t getAcceleratorForwardLowPass8(); + int8_t getAcceleratorForwardLowPass4(); + int getSpeedCmPerSecond(); + int getSpeedFastWithHigherResolution(); // faster and shorter call + int getDistanceCm(); + int getDistanceMillimeter(); + + int getGyroscopePan2DegreePerSecond(); + int getTurnAngleHalfDegree(); + int getTurnAngleDegree(); + + void doAutoOffset(); + void calculateSpeedAndTurnOffsets(); + + void printSpeedAndTurnOffsets(Print *aSerial); + + uint8_t MPU6050ReadByte(uint8_t aRegisterNumber); + uint16_t MPU6050ReadWordSwapped(uint8_t aRegisterNumber); + void MPU6050WriteByte(uint8_t aRegisterNumber, uint8_t aData); + + /* + * AcceleratorForwardOffset == 0 means, offsets are uninitialized and values are wrong + */ + bool OffsetsHaveChanged; + int16_t AcceleratorForwardOffset; // At 1kHz, offset 1 gives speed 4 per second / 1 per 4 second for 0.625 mm/s / 1cm/s resolution + WordUnion AcceleratorForward; // Average of every bulk read from the fifo. 16384 LSB per g => 61 ug per LSB, ~16 mg per 256 LSB + LongUnion AcceleratorForwardLowPass4; + LongUnion AcceleratorForwardLowPass8; + /* + * Speed is the sum of AcceleratorForward. + * At 1 kHz and 1 g (~10 m/s^2): Per sample (1/1000 second) we have 16384 (0x4000) and 10/1000 m/s (1 cm/s). + */ + LongUnion Speed; // shift 14 -> 1 (0.981) cm/s , 12 -> 2.5 mm/s, 8 -> 0.15328 mm/s + /* + * Distance is the sum of Speed >> 8. + * At 1 kHz: with 1 cm/s (shift (14-8)) we have 1 cm (or shift (24-8)) after 1 second (1000 additions) + */ + LongUnion Distance; + + int16_t GyroscopePanOffset; + WordUnion GyroscopePan; // Values without offset, +/-250 | 500 degree per second at 16 bit full range -> ~2 dps per 256 LSB + /* + * 1000 samples / second + * => 17 bit LSB per degree at +/-250 dps range + * The upper word has a resolution of 1/2 degree at 1000 samples per second + */ + LongUnion TurnAngle; + uint32_t LastFifoCheckMillis; + + /* + * Variables for AutoOffset + */ + int16_t sCountOfUndisturbedFifoChunks = 0; // signed, since it is used in formulas with other signed values + int32_t sSpeedSnapshot; + int32_t sTurnSnapshot; +}; + +#endif /* CAR_IMU_DATA_H_ */ + +#pragma once diff --git a/src/LightweightServo.cpp b/src/LightweightServo.cpp new file mode 100644 index 0000000..cd0e889 --- /dev/null +++ b/src/LightweightServo.cpp @@ -0,0 +1,220 @@ +/* + * LightweightServo.cpp + * + * Lightweight Servo implementation only for pin 9 and 10 using only timer1 hardware and no interrupts or other overhead. + * 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... + * + * Copyright (C) 2019 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of ServoEasing https://github.com/ArminJo/ServoEasing. + * + * ServoEasing is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +#include "LightweightServo.h" + +/* + * Variables to enable adjustment for different servo types + * 544 and 2400 are values compatible with standard arduino values + * 4 bytes RAM compared to 48 bytes for standard Arduino library + */ +int sMicrosecondsForServo0Degree = 544; +int sMicrosecondsForServo180Degree = 2400; + +/* + * 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 + */ +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 + 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. +} + +/* + * 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 ATMega328. + * Attention - the selected pin is set to OUTPUT here! + * 54 bytes code size + */ +void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { + + uint8_t tNewTCCR1A = TCCR1A & (_BV(COM1A1) | _BV(COM1B1)); // keep existing COM1A1 and COM1B1 settings + tNewTCCR1A |= _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 + + if (aUsePin9) { + DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction + tNewTCCR1A |= _BV(COM1A1); // non-inverting Compare Output mode OC1A + OCR1A = 0xFFFF; // 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 + OCR1B = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. + } + 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 +} + +/* + * Disables Pin 10! + */ +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 + OCR1A = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. +} +/* + * Disables Pin 9! + */ +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 + OCR1B = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. +} + +void deinitLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { + if (aUsePin9) { + DDRB &= ~(_BV(DDB1)); // set OC1A = PortB1 -> PIN 9 to input direction + TCCR1A &= ~(_BV(COM1A1)); // disable non-inverting Compare Output mode OC1A + } + if (aUsePin10) { + DDRB &= ~(_BV(DDB2)); // set OC1B = PortB2 -> PIN 10 to input direction + TCCR1A &= ~(_BV(COM1B1)); // disable non-inverting Compare Output mode OC1B + } +} + +/* + * If value is below 180 then assume degree, otherwise assume microseconds + * If aUpdateFast then enable starting a new output pulse if more than 5 ms since last one, some servo might react faster in this mode. + * If aUsePin9 is false, then Pin10 is used + * 236 / 186(without auto init) bytes code size + */ +int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast) { + if (aDegree <= 180) { + aDegree = DegreeToMicrosecondsLightweightServo(aDegree); + } + writeMicrosecondsLightweightServo(aDegree, aUsePin9, aUpdateFast); + return aDegree; +} + +void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast) { +#ifndef DISABLE_SERVO_TIMER_AUTO_INITIALIZE + // auto initialize + if ((TCCR1B != (_BV(WGM13) | _BV(WGM12) | _BV(CS11))) || (aUsePin9 && ((TCCR1A & ~_BV(COM1B1)) != (_BV(COM1A1) | _BV(WGM11)))) + || (!aUsePin9 && ((TCCR1A & ~_BV(COM1A1)) != (_BV(COM1B1) | _BV(WGM11))))) { + initLightweightServoPin9_10(aUsePin9, !aUsePin9); + } +#endif + // since the resolution is 1/2 of microsecond + aMicroseconds *= 2; + if (aUpdateFast) { + uint16_t tTimerCount = TCNT1; + if (tTimerCount > 5000) { + // more than 2.5 ms since last pulse -> start a new one + TCNT1 = ICR1 - 1; + } + } + if (aUsePin9) { + OCR1A = aMicroseconds; + } else { + OCR1B = aMicroseconds; + } +} + +/* + * 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 + */ +void write9(int aDegree, bool aUpdateFast) { + writeLightweightServo(aDegree, true, aUpdateFast); +} + +void writeMicroseconds9(int aMicroseconds, bool aUpdateFast) { + writeMicrosecondsLightweightServo(aMicroseconds, true, aUpdateFast); +} + +/* + * Without auto initialize! + */ +void writeMicroseconds9Direct(int aMicroseconds) { + OCR1A = aMicroseconds * 2; +} + +/* + * Pin 10 / Channel B + */ +void write10(int aDegree, bool aUpdateFast) { + writeLightweightServo(aDegree, false, aUpdateFast); +} + +void writeMicroseconds10(int aMicroseconds, bool aUpdateFast) { + writeMicrosecondsLightweightServo(aMicroseconds, false, aUpdateFast); +} + +/* + * Without auto initialize! + */ +void writeMicroseconds10Direct(int aMicroseconds) { + OCR1B = aMicroseconds * 2; +} + +/* + * Conversion functions + */ +int DegreeToMicrosecondsLightweightServo(int aDegree) { + return (map(aDegree, 0, 180, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree)); +} + +int MicrosecondsToDegreeLightweightServo(int aMicroseconds) { + return map(aMicroseconds, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree, 0, 180); +} + +#endif + diff --git a/src/LightweightServo.h b/src/LightweightServo.h new file mode 100644 index 0000000..9084063 --- /dev/null +++ b/src/LightweightServo.h @@ -0,0 +1,78 @@ +/* + * LightweightServo.h + * + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of ServoEasing https://github.com/ArminJo/ServoEasing. + * + * ServoEasing is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef LIGHTWEIGHT_SERVO_H_ +#define LIGHTWEIGHT_SERVO_H_ + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) + +#define VERSION_LIGHTWEIGHT_SERVO "1.1.0" +#define VERSION_LIGHTWEIGHT_SERVO_MAJOR 1 +#define VERSION_LIGHTWEIGHT_SERVO_MINOR 1 + +#include + +/* + * Commenting out this saves 70 bytes flash memory. You must then use the init function initLightweightServoPin9And10() manually. + */ +//#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE +#define ISR1_COUNT_FOR_20_MILLIS 40000 // you can modify this if you have servos which accept a higher rate + +/* + * Lightweight servo library + * Uses timer1 and Pin 9 + 10 as output + */ +void initLightweightServoPin9And10(); +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); + +void write10(int aDegree, bool aUpdateFast = false); // setLightweightServoPulsePin10 Channel B +void writeMicroseconds10(int aMicroseconds, bool aUpdateFast = false); +void writeMicroseconds10Direct(int aMicroseconds); + +// convenience functions +int DegreeToMicrosecondsLightweightServo(int aDegree); +int MicrosecondsToDegreeLightweightServo(int aMicroseconds); + +#endif // AVR_ATmega328 + +/* + * Version 1.1.0 - 11/2020 + * - Improved API. + */ + +#endif // LIGHTWEIGHT_SERVO_H_ + +#pragma once diff --git a/src/LongUnion.h b/src/LongUnion.h new file mode 100644 index 0000000..b0b2c2d --- /dev/null +++ b/src/LongUnion.h @@ -0,0 +1,94 @@ +/* + * LongUnion.h + * + * Copyright (C) 2020 Armin Joachimsmeyer + * Email: armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. + * + * Arduino-Utils is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef LONG_UNION_H +#define LONG_UNION_H + +#include +#include + +/** + * Union to specify parts / manifestations of a 16 bit Word without casts and shifts. + * It also supports the compiler generating small code. + */ +union WordUnion { + struct { + uint8_t LowByte; + uint8_t HighByte; + } UByte; + struct { + int8_t LowByte; + int8_t HighByte; + } Byte; + uint8_t UBytes[2]; + int8_t Bytes[2]; + uint16_t UWord; + int16_t Word; + uint8_t * BytePointer; +}; + +/** + * Union to specify parts / manifestations of a 32 bit Long without casts and shifts. + * It also supports the compiler generating small code. + */ +union LongUnion { + struct { + uint8_t LowByte; + uint8_t MidLowByte; + uint8_t MidHighByte; + uint8_t HighByte; + } UByte; + struct { + int8_t LowByte; + int8_t MidLowByte; + int8_t MidHighByte; + int8_t HighByte; + } Byte; + struct { + uint8_t LowByte; + WordUnion MidWord; + uint8_t HighByte; + } ByteWord; + struct { + int16_t LowWord; + int16_t HighWord; + } Word; + struct { + WordUnion LowWord; + WordUnion HighWord; + } WordUnion; + struct { + uint16_t LowWord; + uint16_t HighWord; + } UWord; + uint8_t UBytes[4]; + int8_t Bytes[4]; + uint16_t UWords[2]; + int16_t Words[2]; + uint32_t ULong; + int32_t Long; +}; + +#endif // LONG_UNION_H + +#pragma once diff --git a/src/MPU6050Defines.h b/src/MPU6050Defines.h new file mode 100644 index 0000000..221d94f --- /dev/null +++ b/src/MPU6050Defines.h @@ -0,0 +1,428 @@ +/* + * MPU6050Defines.h + * Contains only the defines for MPU6050. + * Copied from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 + */ + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +//#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#else +//#define PROGMEM /* empty */ +//#define pgm_read_byte(x) (*(x)) +//#define pgm_read_word(x) (*(x)) +//#define pgm_read_float(x) (*(x)) +//#define PSTR(STR) STR +#endif + + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +// TODO: figure out what these actually do +// UMPL source code is not very obivous +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 + +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +#endif /* _MPU6050_H_ */ diff --git a/src/PWMDcMotor.cpp b/src/PWMDcMotor.cpp index 1da8722..482c07a 100644 --- a/src/PWMDcMotor.cpp +++ b/src/PWMDcMotor.cpp @@ -4,15 +4,23 @@ * Low level motor control for Adafruit_MotorShield OR breakout board with TB6612 or L298 driver IC for two DC motors. * * Motor control has 2 parameters: - * 1. Speed / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). + * 1. SpeedPWM / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). * 2. Direction / MotorDriverMode. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance) * + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. + * * Created on: 12.05.2019 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -26,36 +34,43 @@ #include "PWMDcMotor.h" -bool PWMDcMotor::MotorValuesHaveChanged; // true if DefaultStopMode, StartSpeed, DriveSpeed or SpeedCompensation have changed - for printing -bool PWMDcMotor::SpeedOrMotorModeHasChanged; // - for printing +//#define TRACE +//#define DEBUG + +// Flags e.g. for display update control +#if defined(USE_MPU6050_IMU) || defined(USE_ENCODER_MOTOR_CONTROL) +volatile bool PWMDcMotor::SensorValuesHaveChanged; // true if encoder count and derived encoder speed, or one of the TMU data have changed +#endif +bool PWMDcMotor::MotorControlValuesHaveChanged; // true if DefaultStopMode, StartSpeedPWM, DriveSpeedPWM or SpeedPWMCompensation have changed +bool PWMDcMotor::MotorPWMHasChanged; // true if CurrentSpeedPWM has changed PWMDcMotor::PWMDcMotor() { // @suppress("Class members should be properly initialized") } #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD -void PWMDcMotor::I2CWriteByte(uint8_t aAddress, uint8_t aData) { - Wire.beginTransmission(0x60); +void PWMDcMotor::PCA9685WriteByte(uint8_t aAddress, uint8_t aData) { + Wire.beginTransmission(PCA9685_DEFAULT_ADDRESS); Wire.write(aAddress); Wire.write(aData); - Wire.endTransmission(); + Wire.endTransmission(true); } -void PWMDcMotor::I2CSetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff) { - Wire.beginTransmission(0x60); +void PWMDcMotor::PCA9685SetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff) { + Wire.beginTransmission(PCA9685_DEFAULT_ADDRESS); Wire.write((PCA9685_FIRST_PWM_REGISTER) + 4 * aPin); Wire.write(aOn); Wire.write(aOn >> 8); Wire.write(aOff); Wire.write(aOff >> 8); - Wire.endTransmission(); + Wire.endTransmission(true); } -void PWMDcMotor::I2CSetPin(uint8_t aPin, bool aSetToOn) { +void PWMDcMotor::PCA9685SetPin(uint8_t aPin, bool aSetToOn) { if (aSetToOn) { - I2CSetPWM(aPin, 4096, 0); + PCA9685SetPWM(aPin, 4096, 0); } else { - I2CSetPWM(aPin, 0, 0); + PCA9685SetPWM(aPin, 0, 0); } } @@ -68,8 +83,7 @@ Adafruit_MotorShield sAdafruitMotorShield = Adafruit_MotorShield(); * aMotorNumber from 1 to 2 * Currently motors 3 and 4 are not required/supported by own library for Adafruit Motor Shield */ -void PWMDcMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { - +void PWMDcMotor::init(uint8_t aMotorNumber) { # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD if (aMotorNumber == 1) { // Set PCA9685 channel numbers for Adafruit Motor Shield @@ -83,40 +97,36 @@ void PWMDcMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { } Wire.begin(); + Wire.setClock(400000); # if defined (ARDUINO_ARCH_AVR) // Other platforms do not have this new function Wire.setWireTimeout(5000); // Sets timeout to 5 ms. default is 25 ms. # endif +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" MotorNumber=")); + Serial.println(aMotorNumber); +#endif // Reset PCA9685 Wire.beginTransmission(PCA9685_GENERAL_CALL_ADDRESS); Wire.write(PCA9685_SOFTWARE_RESET); - Wire.endTransmission(); + Wire.endTransmission(true); // Set expander to 1600 HZ - I2CWriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_SLEEP)); // go to sleep - I2CWriteByte(PCA9685_PRESCALE_REGISTER, PCA9685_PRESCALER_FOR_1600_HZ); // set the prescaler + PCA9685WriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_SLEEP)); // go to sleep + PCA9685WriteByte(PCA9685_PRESCALE_REGISTER, PCA9685_PRESCALER_FOR_1600_HZ); // set the prescaler delay(2); // > 500 us before the restart bit according to datasheet - I2CWriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_RESTART) | _BV(PCA9685_MODE_1_AUTOINCREMENT)); // reset sleep and enable auto increment + PCA9685WriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_RESTART) | _BV(PCA9685_MODE_1_AUTOINCREMENT)); // reset sleep and enable auto increment # else Adafruit_MotorShield_DcMotor = sAdafruitMotorShield.getMotor(aMotorNumber); sAdafruitMotorShield.begin(); # endif - if (!aReadFromEeprom) { - MotorValuesEepromStorageNumber = 0; - } else { - MotorValuesEepromStorageNumber = aMotorNumber; - } - // setDefaultsForFixedDistanceDriving() is called by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 - readMotorValuesFromEeprom(); + setDefaultsForFixedDistanceDriving(); stop(DEFAULT_STOP_MODE); } #else // USE_ADAFRUIT_MOTOR_SHIELD -/* - * @param aMotorNumber if 0 do not read from EEPROM, otherwise block number - */ -void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber) { - MotorValuesEepromStorageNumber = aMotorNumber; +void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin) { ForwardPin = aForwardPin; BackwardPin = aBackwardPin; PWMPin = aPWMPin; @@ -128,8 +138,6 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin // set defaults setDefaultsForFixedDistanceDriving(); - - readMotorValuesFromEeprom(); // reads values only if MotorValuesEepromStorageNumber / aMotorNumber is != 0 stop(DEFAULT_STOP_MODE); } @@ -141,25 +149,29 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin */ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) { CurrentDirectionOrBrakeMode = aMotorDriverMode; // The only statement which changes CurrentDirectionOrBrakeMode + if (!(aMotorDriverMode & STOP_MODE_OR_MASK)) { + // set only directions, no brake modes + LastDirection = aMotorDriverMode; + } #ifdef USE_ADAFRUIT_MOTOR_SHIELD // until here DIRECTION_FORWARD is 0 back is 1, Adafruit library starts with 1 # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD switch (aMotorDriverMode) { case DIRECTION_FORWARD: - I2CSetPin(BackwardPin, LOW); // take low first to avoid 'break' - I2CSetPin(ForwardPin, HIGH); + PCA9685SetPin(BackwardPin, LOW); // take low first to avoid 'break' + PCA9685SetPin(ForwardPin, HIGH); break; case DIRECTION_BACKWARD: - I2CSetPin(ForwardPin, LOW); // take low first to avoid 'break' - I2CSetPin(BackwardPin, HIGH); + PCA9685SetPin(ForwardPin, LOW); // take low first to avoid 'break' + PCA9685SetPin(BackwardPin, HIGH); break; case MOTOR_BRAKE: - I2CSetPin(ForwardPin, HIGH); - I2CSetPin(BackwardPin, HIGH); + PCA9685SetPin(ForwardPin, HIGH); + PCA9685SetPin(BackwardPin, HIGH); break; case MOTOR_RELEASE: - I2CSetPin(ForwardPin, LOW); - I2CSetPin(BackwardPin, LOW); + PCA9685SetPin(ForwardPin, LOW); + PCA9685SetPin(BackwardPin, LOW); break; } # else @@ -189,24 +201,27 @@ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) { } /* - * @return true if direction has changed and motor has stopped + * @return true if direction has changed AND motor was stopped */ bool PWMDcMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { + aRequestedDirection &= DIRECTION_MASK; // since we are never called with "brake directions" but may be called with TURN_IN_PLACE direction bool tReturnValue = false; - uint8_t tRequestedDirection = aRequestedDirection & DIRECTION_MASK; - if (CurrentDirectionOrBrakeMode != tRequestedDirection) { - if (CurrentSpeed != 0) { -#ifdef DEBUG - Serial.print(F("Motor mode change to ")); - Serial.println(tRequestedDirection); -#endif + if (CurrentDirectionOrBrakeMode != aRequestedDirection) { + if (CurrentSpeedPWM != 0) { /* * Direction change requested but motor still running-> first stop motor */ stop(MOTOR_BRAKE); tReturnValue = true; } - setMotorDriverMode(tRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Change motor mode from ")); + Serial.print(CurrentDirectionOrBrakeMode); + Serial.print(F(" to ")); + Serial.println(aRequestedDirection); +#endif + setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode } return tReturnValue; } @@ -215,63 +230,88 @@ bool PWMDcMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { * @brief Control the DC Motor speed/throttle * @param speed The 8-bit PWM value, 0 is off, 255 is on forward -255 is on backward * First set driver mode, then set PWM + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. */ -void PWMDcMotor::setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection) { - if (aSpeedRequested == 0) { +void PWMDcMotor::setSpeedPWM(uint8_t aSpeedPWMRequested, uint8_t aRequestedDirection) { + if (aSpeedPWMRequested == 0) { stop(DefaultStopMode); } else { checkAndHandleDirectionChange(aRequestedDirection); - if (CurrentSpeed != aSpeedRequested) { - CurrentSpeed = aSpeedRequested; // The only statement which sets CurrentSpeed to a value != 0 - SpeedOrMotorModeHasChanged = true; + if (CurrentSpeedPWM != aSpeedPWMRequested) { + CurrentSpeedPWM = aSpeedPWMRequested; // The only statement which sets CurrentSpeedPWM to a value != 0 +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" SpeedPWM=")); + Serial.println(CurrentSpeedPWM); +#endif + MotorPWMHasChanged = true; #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD - I2CSetPWM(PWMPin, 0, 16 * aSpeedRequested); + PCA9685SetPWM(PWMPin, 0, 16 * aSpeedPWMRequested); # else - Adafruit_MotorShield_DcMotor->setSpeed(aSpeedRequested); + Adafruit_MotorShield_DcMotor->setSpeedPWM(aSpeedPWMRequested); # endif #else - analogWrite(PWMPin, aSpeedRequested); + analogWrite(PWMPin, aSpeedPWMRequested); #endif } } } /* - * Subtracts SpeedCompensation from aRequestedSpeed before applying + * Keeps direction + */ +void PWMDcMotor::changeSpeedPWM(uint8_t aSpeedPWMRequested) { + if (!(CurrentDirectionOrBrakeMode & STOP_MODE_OR_MASK)) { + setSpeedPWM(aSpeedPWMRequested, CurrentDirectionOrBrakeMode); // output PWM value to motor + } +} + +/* + * Subtracts SpeedPWMCompensation from aRequestedSpeedPWM before applying */ -void PWMDcMotor::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { +void PWMDcMotor::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { // avoid underflow - uint8_t tCurrentSpeed; - if (aRequestedSpeed > SpeedCompensation) { - tCurrentSpeed = aRequestedSpeed - SpeedCompensation; + uint8_t tCurrentSpeedPWM; + if (aRequestedSpeedPWM > SpeedPWMCompensation) { + tCurrentSpeedPWM = aRequestedSpeedPWM - SpeedPWMCompensation; } else { - tCurrentSpeed = 0; + tCurrentSpeedPWM = 0; + } + setSpeedPWM(tCurrentSpeedPWM, aRequestedDirection); // output PWM value to motor +} + +/* + * Keeps direction + */ +void PWMDcMotor::changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM) { + if (!(CurrentDirectionOrBrakeMode & STOP_MODE_OR_MASK)) { + setSpeedPWMCompensated(aRequestedSpeedPWM, CurrentDirectionOrBrakeMode); } - setSpeed(tCurrentSpeed, aRequestedDirection); // output PWM value to motor } /* * Signed speed */ -void PWMDcMotor::setSpeed(int aSpeedRequested) { - if (aSpeedRequested < 0) { - aSpeedRequested = -aSpeedRequested; - setSpeed(aSpeedRequested, DIRECTION_BACKWARD); +void PWMDcMotor::setSpeedPWM(int aSpeedPWMRequested) { + if (aSpeedPWMRequested < 0) { + aSpeedPWMRequested = -aSpeedPWMRequested; + setSpeedPWM(aSpeedPWMRequested, DIRECTION_BACKWARD); } else { - setSpeed(aSpeedRequested, DIRECTION_FORWARD); + setSpeedPWM(aSpeedPWMRequested, DIRECTION_FORWARD); } } -void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) { +void PWMDcMotor::setSpeedPWMCompensated(int aRequestedSpeedPWM) { uint8_t tDirection; - if (aRequestedSpeed > 0) { + if (aRequestedSpeedPWM > 0) { tDirection = DIRECTION_FORWARD; } else { tDirection = DIRECTION_BACKWARD; - aRequestedSpeed = -aRequestedSpeed; + aRequestedSpeedPWM = -aRequestedSpeedPWM; } - setSpeedCompensated(aRequestedSpeed, tDirection); + setSpeedPWMCompensated(aRequestedSpeedPWM, tDirection); } /* @@ -280,20 +320,19 @@ void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) { */ void PWMDcMotor::stop(uint8_t aStopMode) { - CurrentSpeed = 0; // The only statement which sets CurrentSpeed to 0 - SpeedOrMotorModeHasChanged = true; - MotorMovesFixedDistance = false; -#ifdef SUPPORT_RAMP_UP + CurrentSpeedPWM = 0; // The only statement which sets CurrentSpeedPWM to 0 + MotorPWMHasChanged = true; + CheckDistanceInUpdateMotor = false; MotorRampState = MOTOR_STATE_STOPPED; -#endif + #ifndef USE_ENCODER_MOTOR_CONTROL - MotorMovesFixedDistance = false; + CheckDistanceInUpdateMotor = false; #endif #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD - I2CSetPWM(PWMPin, 0, 0); + PCA9685SetPWM(PWMPin, 0, 0); # else - Adafruit_MotorShield_DcMotor->setSpeed(0); + Adafruit_MotorShield_DcMotor->setSpeedPWM(0); # endif #else analogWrite(PWMPin, 0); @@ -301,267 +340,374 @@ void PWMDcMotor::stop(uint8_t aStopMode) { if (aStopMode == STOP_MODE_KEEP) { aStopMode = DefaultStopMode; } - setMotorDriverMode(CheckStopMODE(aStopMode)); + setMotorDriverMode(ForceStopMODE(aStopMode)); +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Stop motor StopMode=")); + Serial.println(ForceStopMODE(aStopMode)); +#endif } /* * @param aStopMode used for speed == 0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE */ void PWMDcMotor::setStopMode(uint8_t aStopMode) { - DefaultStopMode = CheckStopMODE(aStopMode); - MotorValuesHaveChanged = true; + DefaultStopMode = ForceStopMODE(aStopMode); + MotorControlValuesHaveChanged = true; } /****************************************************************************************** * Distance functions *****************************************************************************************/ /* - * StartSpeed (at which car starts to move) for 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 - * setDefaultsForFixedDistanceDriving() is called at init by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 + * setDefaultsForFixedDistanceDriving() is called at init */ void PWMDcMotor::setDefaultsForFixedDistanceDriving() { - StartSpeed = DEFAULT_START_SPEED; - DriveSpeed = DEFAULT_DRIVE_SPEED; - SpeedCompensation = 0; + StartSpeedPWM = DEFAULT_START_SPEED_PWM; + DriveSpeedPWM = DEFAULT_DRIVE_SPEED_PWM; + SpeedPWMCompensation = 0; #ifndef USE_ENCODER_MOTOR_CONTROL - DistanceToTimeFactor = DEFAULT_DISTANCE_TO_TIME_FACTOR; + MillisPerMillimeter = DEFAULT_MILLIS_PER_MILLIMETER; #endif - MotorValuesHaveChanged = true; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation) { - StartSpeed = aStartSpeed; - DriveSpeed = aDriveSpeed; - SpeedCompensation = aSpeedCompensation; - MotorValuesHaveChanged = true; +void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, uint8_t aSpeedPWMCompensation) { + StartSpeedPWM = aStartSpeedPWM; + DriveSpeedPWM = aDriveSpeedPWM; + SpeedPWMCompensation = aSpeedPWMCompensation; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setSpeedCompensation(uint8_t aSpeedCompensation) { - SpeedCompensation = aSpeedCompensation; - MotorValuesHaveChanged = true; +void PWMDcMotor::setSpeedPWMCompensation(uint8_t aSpeedPWMCompensation) { + SpeedPWMCompensation = aSpeedPWMCompensation; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setDriveSpeed(uint8_t aDriveSpeed) { - DriveSpeed = aDriveSpeed; - MotorValuesHaveChanged = true; +void PWMDcMotor::setStartSpeedPWM(uint8_t aStartSpeedPWM) { + StartSpeedPWM = aStartSpeedPWM; + MotorControlValuesHaveChanged = true; } -#ifdef SUPPORT_RAMP_UP -void PWMDcMotor::startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - if (MotorRampState == MOTOR_STATE_STOPPED) { - MotorRampState = MOTOR_STATE_START; +void PWMDcMotor::setDriveSpeedPWM(uint8_t aDriveSpeedPWM) { + DriveSpeedPWM = aDriveSpeedPWM; + MotorControlValuesHaveChanged = true; +} + +void PWMDcMotor::startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Ramp up to ")); + Serial.print(aRequestedSpeedPWM); + Serial.print(F(" Dir=")); + Serial.print(aRequestedDirection); + Serial.print(F(" CurrentSpeedPWM=")); + Serial.print(CurrentSpeedPWM); + Serial.print(F(" MotorMode=")); + Serial.print(CurrentDirectionOrBrakeMode); + Serial.println(); +#endif - // avoid underflow - uint8_t tCurrentDriveSpeed; - if (aRequestedSpeed > SpeedCompensation) { - tCurrentDriveSpeed = aRequestedSpeed - SpeedCompensation; + if (CurrentSpeedPWM == 0) { // equivalent to MotorRampState == MOTOR_STATE_STOPPED + checkAndHandleDirectionChange(aRequestedDirection); + MotorRampState = MOTOR_STATE_START; + /* + * Set target speed for ramp up + */ + uint8_t tRequestedDriveSpeedPWM; + if (aRequestedSpeedPWM > SpeedPWMCompensation) { + tRequestedDriveSpeedPWM = aRequestedSpeedPWM - SpeedPWMCompensation; } else { - tCurrentDriveSpeed = 0; + tRequestedDriveSpeedPWM = 0; } - CurrentDriveSpeed = tCurrentDriveSpeed; - } else if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) { - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); + RequestedDriveSpeedPWM = tRequestedDriveSpeedPWM; + RampSpeedPWMDelta = RAMP_UP_VALUE_DELTA; // 20V / s + } else if (MotorRampState == MOTOR_STATE_DRIVE) { + // motor is running, -> just change speed + setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); } + // else ramp is in mode MOTOR_STATE_RAMP_UP -> do nothing, let the ramp go on +# ifdef DEBUG + Serial.print(F("MotorRampState=")); + Serial.print(MotorRampState); + Serial.println(); +# endif } void PWMDcMotor::startRampUp(uint8_t aRequestedDirection) { - startRampUp(DriveSpeed, aRequestedDirection); + startRampUp(DriveSpeedPWM, aRequestedDirection); +} + +void PWMDcMotor::startRampDown() { + MotorRampState = MOTOR_STATE_RAMP_DOWN; +// set only the variables for later evaluation in updateMotor() below + if (CurrentSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + CurrentSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + } else { + CurrentSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } } -#endif #if !defined(USE_ENCODER_MOTOR_CONTROL) +/* + * @return true if not stopped (motor expects another update) + */ +bool PWMDcMotor::updateMotor() { + unsigned long tMillis = millis(); + uint8_t tNewSpeedPWM = CurrentSpeedPWM; + + if (MotorRampState == MOTOR_STATE_START) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Start motor + */ + if (RequestedDriveSpeedPWM > RAMP_VALUE_UP_OFFSET_SPEED_PWM) { + // start with ramp to avoid spinning wheels + tNewSpeedPWM = RAMP_VALUE_UP_OFFSET_SPEED_PWM; // start immediately with speed offset (3 volt) + // --> RAMP_UP + MotorRampState = MOTOR_STATE_RAMP_UP; + } else { + // Motor ramp not required, go direct to drive speed. + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; + } + } else if (MotorRampState == MOTOR_STATE_RAMP_UP) { + /* + * Increase motor speed by RAMP_UP_VALUE_DELTA every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis += RAMP_INTERVAL_MILLIS; + tNewSpeedPWM = tNewSpeedPWM + RampSpeedPWMDelta; + /* + * Transition criteria is: RequestedDriveSpeedPWM reached. + * Then check immediately for timeout + */ + // Clip value and check for 8 bit overflow + if (tNewSpeedPWM >= RequestedDriveSpeedPWM || tNewSpeedPWM <= RampSpeedPWMDelta) { + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; + } + } + } + + if (MotorRampState == MOTOR_STATE_DRIVE) { + /* + * Time based distance. Ramp down is included in time formula. + */ + if (CheckDistanceInUpdateMotor && millis() >= computedMillisOfMotorStopForDistance) { + + if (tNewSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + tNewSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + // --> RAMP_DOWN + MotorRampState = MOTOR_STATE_RAMP_DOWN; + } else { + // --> STOPPED + tNewSpeedPWM = 0; + } + } + } + + if (MotorRampState == MOTOR_STATE_RAMP_DOWN) { + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Decrease motor speed RAMP_UP_UPDATE_INTERVAL_STEPS times every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tNewSpeedPWM > (RAMP_DOWN_VALUE_DELTA + RAMP_VALUE_DOWN_MIN_SPEED_PWM)) { + tNewSpeedPWM -= RAMP_DOWN_VALUE_DELTA; + } else { + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } + } + } +// End of motor state machine + +#ifdef TRACE + Serial.print(F("St=")); + Serial.println(MotorRampState); +#endif + if (tNewSpeedPWM != CurrentSpeedPWM) { +#ifdef TRACE + Serial.print(F("Ns=")); + Serial.println(tNewSpeedPWM); +#endif + PWMDcMotor::setSpeedPWM(tNewSpeedPWM, CurrentDirectionOrBrakeMode); // sets MOTOR_STATE_STOPPED if speed is 0 + } + + /* + * Check if target milliseconds are reached + */ + if (CurrentSpeedPWM > 0) { + if (CheckDistanceInUpdateMotor && millis() > computedMillisOfMotorStopForDistance) { + stop(DefaultStopMode); // resets CheckDistanceInUpdateMotor and sets MOTOR_STATE_STOPPED; + return false; // need no more calls to update() + } + return true; // still running + } + return false; // current speed == 0 +} + /* * Required for non encoder motors to estimate duration for a fixed distance */ -void PWMDcMotor::setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor) { - DistanceToTimeFactor = aDistanceToTimeFactor; +void PWMDcMotor::setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond) { + MillisPerMillimeter = 1000 / aMillimeterPerSecond; } /* - * @param aRequestedDistanceCount distance in 5mm resolution (to be compatible with 20 slot encoder discs and 20 cm wheel circumference) * If motor is already running just update speed and new time */ -void PWMDcMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { -// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { -// PanicWithLed(400, 22); -// } - if (aRequestedDistanceCount == 0) { +void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + if (aRequestedDistanceMillimeter == 0) { + stop(DefaultStopMode); // In case motor was running return; } - if (CurrentSpeed == 0) { -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = aRequestedSpeed; - setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode -#else - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -#endif + if (CurrentSpeedPWM == 0) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + /* - * Estimate duration for given distance - * use 32 bit intermediate to avoid overflow (this also saves around 50 bytes of program memory by using slower functions instead of faster inline code) + * Estimate duration for given distance# + * MillisPerMillimeter is defined for DEFAULT_DRIVE_SPEED_PWM is */ - computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS - + (10 * (((uint32_t) aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + if (aRequestedSpeedPWM == DEFAULT_DRIVE_SPEED_PWM) { + computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS + + (((uint16_t) aRequestedDistanceMillimeter * MillisPerMillimeter)); + } else { + computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS + + (((uint32_t) aRequestedDistanceMillimeter * MillisPerMillimeter * DriveSpeedPWM) / DEFAULT_DRIVE_SPEED_PWM); + } + } else { -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = aRequestedSpeed; -#endif - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); + MotorRampState = MOTOR_STATE_DRIVE; + setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); /* * Estimate duration for given distance * use 32 bit intermediate to avoid overflow (this also saves around 50 bytes of program memory by using slower functions instead of faster inline code) */ computedMillisOfMotorStopForDistance = millis() - + (10 * (((uint32_t) aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + + (((uint32_t) aRequestedDistanceMillimeter * MillisPerMillimeter * DriveSpeedPWM) / DEFAULT_DRIVE_SPEED_PWM); } - MotorMovesFixedDistance = true; + CheckDistanceInUpdateMotor = true; } -void PWMDcMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } /* * Signed DistanceCount */ -void PWMDcMotor::startGoDistanceCount(int aRequestedDistanceCount) { - if (aRequestedDistanceCount < 0) { - aRequestedDistanceCount = -aRequestedDistanceCount; - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_BACKWARD); +void PWMDcMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_FORWARD); + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } /* * Not used by CarControl */ -void PWMDcMotor::goDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - goDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + goDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } -void PWMDcMotor::goDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(aRequestedSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); while (millis() <= computedMillisOfMotorStopForDistance) { -#ifdef SUPPORT_RAMP_UP updateMotor(); -#endif } stop(DefaultStopMode); } -/* - * @return true if not stopped (motor expects another update) - */ -bool PWMDcMotor::updateMotor() { -#ifdef SUPPORT_RAMP_UP - unsigned long tMillis = millis(); - uint8_t tNewSpeed = CurrentSpeed; - - if (MotorRampState == MOTOR_STATE_START) { - // --> RAMP_UP - MotorRampState = MOTOR_STATE_RAMP_UP; - /* - * Set ramp values, 16 steps a 16 millis for ramp up => 256 milliseconds - */ - NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS; - RampDelta = RAMP_UP_VALUE_DELTA; // ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS) - if (RampDelta < 2) { - RampDelta = 2; - } - /* - * Start motor - */ - tNewSpeed = StartSpeed; - } - - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_RAMP_UP) { - /* - * Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds - * or until more than half of distance is done - * Distance required for ramp is 0 to 10 or more, increasing with increasing CurrentDriveSpeed - */ - if (tMillis >= NextRampChangeMillis) { - NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS; - tNewSpeed = tNewSpeed + RampDelta; - // Clip value and check for 8 bit overflow - if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) { - tNewSpeed = CurrentDriveSpeed; - } - - /* - * Transition criteria is: - * Max Speed reached or more than half of distance is done - */ - if (tNewSpeed == CurrentDriveSpeed) { - // --> DRIVE_SPEED - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - } - } - } - // End of motor state machine - - if (tNewSpeed != CurrentSpeed) { - PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode); - } -#endif - - /* - * Check if target milliseconds are reached - */ - if (CurrentSpeed > 0) { - if (MotorMovesFixedDistance && millis() > computedMillisOfMotorStopForDistance) { - stop(DefaultStopMode); // resets MotorMovesFixedDistance - return false; - } - } - return true; -} #endif // !defined(USE_ENCODER_MOTOR_CONTROL) /******************************************************************************************** * EEPROM functions * Uses the start of EEPROM for storage of EepromMotorInfoStruct's for motor number 1 to n - * setDefaultsForFixedDistanceDriving() is called by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 ********************************************************************************************/ -void PWMDcMotor::readMotorValuesFromEeprom() { - if (MotorValuesEepromStorageNumber != 0) { - EepromMotorInfoStruct tEepromMotorInfo; - eeprom_read_block((void*) &tEepromMotorInfo, (void*) ((MotorValuesEepromStorageNumber - 1) * sizeof(EepromMotorInfoStruct)), - sizeof(EepromMotorInfoStruct)); +void PWMDcMotor::readMotorValuesFromEeprom(uint8_t aMotorValuesEepromStorageNumber) { + EepromMotorInfoStruct tEepromMotorInfo; + eeprom_read_block((void*) &tEepromMotorInfo, (void*) ((aMotorValuesEepromStorageNumber) * sizeof(EepromMotorInfoStruct)), + sizeof(EepromMotorInfoStruct)); - /* - * Overwrite with values if valid - */ - if (tEepromMotorInfo.StartSpeed < 150 && tEepromMotorInfo.StartSpeed > 20) { - StartSpeed = tEepromMotorInfo.StartSpeed; - if (tEepromMotorInfo.DriveSpeed > 40) { - DriveSpeed = tEepromMotorInfo.DriveSpeed; - } - if (tEepromMotorInfo.SpeedCompensation < 24) { - SpeedCompensation = tEepromMotorInfo.SpeedCompensation; - } + /* + * Overwrite with values if valid + */ + if (tEepromMotorInfo.StartSpeedPWM < 150 && tEepromMotorInfo.StartSpeedPWM > 20) { + StartSpeedPWM = tEepromMotorInfo.StartSpeedPWM; + if (tEepromMotorInfo.DriveSpeedPWM > 40) { + DriveSpeedPWM = tEepromMotorInfo.DriveSpeedPWM; + } + if (tEepromMotorInfo.SpeedPWMCompensation < 24) { + SpeedPWMCompensation = tEepromMotorInfo.SpeedPWMCompensation; } - MotorValuesHaveChanged = true; - } else { - setDefaultsForFixedDistanceDriving(); } + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::writeMotorvaluesToEeprom() { - if (MotorValuesEepromStorageNumber != 0) { - EepromMotorInfoStruct tEepromMotorInfo; - tEepromMotorInfo.StartSpeed = StartSpeed; - tEepromMotorInfo.DriveSpeed = DriveSpeed; - tEepromMotorInfo.SpeedCompensation = SpeedCompensation; +void PWMDcMotor::writeMotorValuesToEeprom(uint8_t aMotorValuesEepromStorageNumber) { + EepromMotorInfoStruct tEepromMotorInfo; + tEepromMotorInfo.StartSpeedPWM = StartSpeedPWM; + tEepromMotorInfo.DriveSpeedPWM = DriveSpeedPWM; + tEepromMotorInfo.SpeedPWMCompensation = SpeedPWMCompensation; - eeprom_write_block((void*) &tEepromMotorInfo, - (void*) ((MotorValuesEepromStorageNumber - 1) * sizeof(EepromMotorInfoStruct)), sizeof(EepromMotorInfoStruct)); - } + eeprom_write_block((void*) &tEepromMotorInfo, (void*) ((aMotorValuesEepromStorageNumber) * sizeof(EepromMotorInfoStruct)), + sizeof(EepromMotorInfoStruct)); +} + +const char StringNot[] PROGMEM = { " not" }; +const char StringDefined[] PROGMEM = { " defined" }; + +void PWMDcMotor::printSettings(Print *aSerial) { + aSerial->println(); + aSerial->println(F("Settings from PWMDcMotor.h:")); + + aSerial->print(F("USE_ENCODER_MOTOR_CONTROL:")); +#ifndef USE_ENCODER_MOTOR_CONTROL + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); + + aSerial->print(F("USE_ADAFRUIT_MOTOR_SHIELD:")); +#ifndef USE_ADAFRUIT_MOTOR_SHIELD + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + aSerial->print(F("USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD:")); +#ifndef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); +#endif + + aSerial->print(F("DEFAULT_MOTOR_START_UP_TIME_MILLIS=")); + aSerial->println(DEFAULT_MOTOR_START_UP_TIME_MILLIS); + + aSerial->print(F("FULL_BRIDGE_OUTPUT_MILLIVOLT=")); + aSerial->print(FULL_BRIDGE_OUTPUT_MILLIVOLT); + aSerial->print(F("mV (= FULL_BRIDGE_INPUT_MILLIVOLT|")); + aSerial->print(FULL_BRIDGE_INPUT_MILLIVOLT); + aSerial->print(F("mV - FULL_BRIDGE_LOSS_MILLIVOLT|")); + aSerial->print(FULL_BRIDGE_LOSS_MILLIVOLT); + aSerial->println(F("mV)")); + + aSerial->print(F("DEFAULT_START_SPEED_PWM=")); + aSerial->print(DEFAULT_START_SPEED_PWM); + aSerial->print(F(", DEFAULT_DRIVE_SPEED_PWM=")); + aSerial->println(DEFAULT_DRIVE_SPEED_PWM); + + aSerial->print(F("DEFAULT_MILLIS_PER_MILLIMETER=")); + aSerial->println(DEFAULT_MILLIS_PER_MILLIMETER); + aSerial->println(); } //void PanicWithLed(unsigned int aDelay, uint8_t aCount) { diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h index 8c006d8..32b2e07 100644 --- a/src/PWMDcMotor.h +++ b/src/PWMDcMotor.h @@ -1,10 +1,18 @@ /* * PWMDCMotor.h * - * Motor control has 2 technical dimensions - * 1. Motor driver control / direction. Can be FORWARD, BACKWARD, BRAKE (motor connections are shortened) - * or RELEASE (motor connections are high impedance). - * 2. Speed / PWM which is ignored for BRAKE or RELEASE. + * Motor control has 2 parameters: + * 1. SpeedPWM / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). + * 2. Direction / MotorDriverMode. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance) + * + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. + * + * Distance is computed in 3 different ways. + * Without IMU or Encoder: - distance is converted to a time for riding. + * With IMU: - distance is measured by IMU. + * With encoder: - distance is measured by Encoder. + * * * Created on: 12.05.2019 * Copyright (C) 2019-2020 Armin Joachimsmeyer @@ -12,6 +20,11 @@ * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -26,24 +39,33 @@ #include -#define VERSION_PWMMOTORCONTROL "1.1.1" -#define VERSION_PWMMOTORCONTROL_MAJOR 1 -#define VERSION_PWMMOTORCONTROL_MINOR 1 +#define VERSION_PWMMOTORCONTROL "2.0.0" +#define VERSION_PWMMOTORCONTROL_MAJOR 2 +#define VERSION_PWMMOTORCONTROL_MINOR 0 +// The change log is at the bottom of the file /* - * Comment this out, if you have encoder interrupts attached at pin 2 and 3 + * Activate this, if you have encoder interrupts attached at pin 2 and 3 * and want to use the methods of the EncoderMotor class for fixed distance / closed loop driving. * Enabling it will disable no longer required PWMDCMotor class variables and functions - * and force the usage of the EncoderMotor class in CarMotorControl. + * and force the usage of the EncoderMotor class in CarPWMMotorControl. */ //#define USE_ENCODER_MOTOR_CONTROL // /* - * Use Adafruit Motor Shield v2 connected by I2C instead of simple TB6612 or L298 breakout board. - * This disables tone output by using motor as loudspeaker, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bridge. + * Use Adafruit Motor Shield v2 connected by I2C instead of TB6612 or L298 breakout board. + * This disables using motor as buzzer, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bridge. * For full bridge, analogWrite the millis() timer0 is used since we use pin 5 & 6. */ //#define USE_ADAFRUIT_MOTOR_SHIELD +#if defined(USE_ADAFRUIT_MOTOR_SHIELD) +# if !defined(MOSFET_BRIDGE_USED) +#define MOSFET_BRIDGE_USED +# endif +# if !defined(FULL_BRIDGE_LOSS_MILLIVOLT) +#define FULL_BRIDGE_LOSS_MILLIVOLT 0 +# endif +#endif // /* * Own library saves me 694 bytes program memory @@ -53,65 +75,88 @@ #endif /* - * Disabling SUPPORT_RAMP_UP saves 7 bytes RAM per motor and around 300 bytes program memory + * Activate this, if you use default settings and 2 LiPo Cells (around 7.4 volt) as Motor supply. */ -#if ! DO_NOT_SUPPORT_RAMP_UP // if defined (externally), disables ramp up support -#define SUPPORT_RAMP_UP // 256 milliseconds for ramp, see below -#endif - +//#define VIN_2_LIPO /* - * Comment this out, if you use default settings and 2 LiPo Cells (around 7.4 volt) as Motor supply. + * Helper macro for getting a macro definition as string */ -//#define VIN_2_LIPO -#define MAX_SPEED 255 +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +#define MAX_SPEED_PWM 255 /* - * This values are chosen to be compatible with 20 slot encoder discs, giving 20 on and 20 off counts per full rotation. - * At a circumference of around 20 cm (21.5 cm actual) this gives 5 mm per count. + * Circumference of my smart car wheel */ -#define DEFAULT_COUNTS_PER_FULL_ROTATION 40 -#define DEFAULT_MILLIMETER_PER_COUNT 5 +#define DEFAULT_CIRCUMFERENCE_MILLIMETER 220 -#define DEFAULT_MOTOR_START_UP_TIME_MILLIS 150 // constant value for the for the formula below /* - * DEFAULT_DISTANCE_TO_TIME_FACTOR is the factor used to convert distance in 5mm steps to motor on time in milliseconds. I depends on motor supply voltage. - * Currently formula is: - * computedMillisOfMotorStopForDistance = DEFAULT_MOTOR_START_UP_TIME_MILLIS + (10 * ((aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); - * - * DEFAULT_START_SPEED is the speed PWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 3,6 volt (USB supply) is appr. 70 to 100 + * I measured maximum positive acceleration with spinning wheels as 250 cm/s^2 on varnished wood. + * I measured maximum negative acceleration with blocking wheels as 350 cm/s^2 on varnished wood. + * This corresponds to 5 cm/s every 20 ms */ -#define DEFAULT_START_SPEED_7_4_VOLT 45 -#define DEFAULT_DRIVE_SPEED_7_4_VOLT 80 -#define DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT 135 // for 2 x LIPO batteries (7.4 volt). - -#define DEFAULT_START_SPEED_6_VOLT 140 -#define DEFAULT_DRIVE_SPEED_6_VOLT 220 -#define DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT 300 // for 4 x AA batteries (6 volt). - -// Default values - used if EEPROM values are invalid -#if defined(VIN_2_LIPO) -# if !defined(DEFAULT_START_SPEED) -#define DEFAULT_START_SPEED DEFAULT_START_SPEED_7_4_VOLT -# endif -# if !defined(DEFAULT_DRIVE_SPEED) -#define DEFAULT_DRIVE_SPEED DEFAULT_DRIVE_SPEED_7_4_VOLT -# endif -# if !defined(DEFAULT_DISTANCE_TO_TIME_FACTOR) -#define DEFAULT_DISTANCE_TO_TIME_FACTOR DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT +#define RAMP_INTERVAL_MILLIS 20 // The smaller the value the steeper the ramp +#define RAMP_VALUE_UP_OFFSET_MILLIVOLT 2300 // Start positive or negative acceleration with this voltage offset in order to get a reasonable acceleration for ramps +#define RAMP_VALUE_DOWN_OFFSET_MILLIVOLT 2500 // Start positive or negative acceleration with this voltage offset in order to get a reasonable acceleration for ramps +#define RAMP_VALUE_UP_OFFSET_SPEED_PWM ((RAMP_VALUE_UP_OFFSET_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_VALUE_DOWN_OFFSET_SPEED_PWM ((RAMP_VALUE_DOWN_OFFSET_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_VALUE_DOWN_MIN_SPEED_PWM (((DEFAULT_DRIVE_MILLIVOLT / 2) * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_UP_VALUE_DELTA (SPEED_FOR_8_VOLT / (1000 / RAMP_INTERVAL_MILLIS)) // Results in a ramp up voltage of 10V/s = 1.0 volt per 100 ms +#define RAMP_DOWN_VALUE_DELTA ((SPEED_FOR_8_VOLT * 2)/ (1000 / RAMP_INTERVAL_MILLIS)) // Results in a ramp up voltage of 20V/s = 1.0 volt per 100 ms +#define RAMP_DECELERATION_TIMES_2 3500 // Take half of the observed maximum. This depends on the type of tires and the mass of the car + +#define DEFAULT_MOTOR_START_UP_TIME_MILLIS 15 // 15 to 20, constant value for the for the formula below + +#if !defined(FULL_BRIDGE_INPUT_MILLIVOLT) +# if defined(VIN_2_LIPO) +#define FULL_BRIDGE_INPUT_MILLIVOLT 7400 // for 2 x LIPO batteries (7.4 volt). +# else +#define FULL_BRIDGE_INPUT_MILLIVOLT 6000 // for 4 x AA batteries (6 volt). # endif +#endif -#else -# if !defined(DEFAULT_START_SPEED) -#define DEFAULT_START_SPEED DEFAULT_START_SPEED_6_VOLT -# endif -# if !defined(DEFAULT_DRIVE_SPEED) -#define DEFAULT_DRIVE_SPEED DEFAULT_DRIVE_SPEED_6_VOLT -# endif -# if !defined(DEFAULT_DISTANCE_TO_TIME_FACTOR) -#define DEFAULT_DISTANCE_TO_TIME_FACTOR DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT +#if !defined(FULL_BRIDGE_LOSS_MILLIVOLT) +# if defined(MOSFET_BRIDGE_USED) +// Speed is almost linear to 1/2 PWM in cm/s without any offset, only with dead band +#define FULL_BRIDGE_LOSS_MILLIVOLT 0 +# else +// Speed is not linear to PWM and has an offset +// Effective voltage loss includes loss by switching to high impedance at inactive for bipolar full bridges like L298 +#define FULL_BRIDGE_LOSS_MILLIVOLT 2200 // Effective voltage loss # endif #endif +#if !defined(FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define FULL_BRIDGE_OUTPUT_MILLIVOLT (FULL_BRIDGE_INPUT_MILLIVOLT - FULL_BRIDGE_LOSS_MILLIVOLT) +#endif + +#define DEFAULT_START_MILLIVOLT 1100 // Start voltage -motors start to turn- is 1.1 volt +#define DEFAULT_DRIVE_MILLIVOLT 2000 // Drive voltage is 2.0 volt +#define SPEED_FOR_1_VOLT ((1000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define SPEED_FOR_8_VOLT ((8000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define SPEED_FOR_10_VOLT ((10000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) + +// Default values - used if EEPROM values are invalid or nor available +#if !defined(DEFAULT_START_SPEED_PWM) +// DEFAULT_START_SPEED_PWM is the speed PWM value at which motor starts to move. 70|127 for 4 volt 37|68 for 7.4 volt +#define DEFAULT_START_SPEED_PWM ((DEFAULT_START_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#endif +#if !defined(DEFAULT_DRIVE_SPEED_PWM) +// At 2 volt I measured around 32 cm/s. 68 for 7.4 volt +#define DEFAULT_DRIVE_SPEED_PWM ((DEFAULT_DRIVE_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#endif + +#if !defined(DEFAULT_MILLIMETER_PER_SECOND) +// At 2 volt (DEFAULT_DRIVE_MILLIVOLT) we have around 1.25 rotation per second -> 25 distance/encoder counts per second -> 27 cm / second +#define DEFAULT_MILLIMETER_PER_SECOND 320 // at DEFAULT_DRIVE_MILLIVOLT motor supply +#define DEFAULT_MILLIS_PER_MILLIMETER (1000 / DEFAULT_MILLIMETER_PER_SECOND) +#endif +/* + * Currently formula used to convert distance in 11 mm steps to motor on time in milliseconds is: + * computedMillisOfMotorStopForDistance = DEFAULT_MOTOR_START_UP_TIME_MILLIS + (((aRequestedDistanceCount * MillisPerMillimeter) / DriveSpeedPWM)); + */ + // Motor directions and stop modes. Are used for parameter aMotorDriverMode and sequence is determined by the Adafruit library API. #define DIRECTION_FORWARD 0 #define DIRECTION_BACKWARD 1 @@ -124,22 +169,22 @@ #define STOP_MODE_AND_MASK 0x03 #define STOP_MODE_OR_MASK 0x02 #define DEFAULT_STOP_MODE MOTOR_RELEASE -#define CheckStopMODE(aStopMode) ((aStopMode & STOP_MODE_AND_MASK) | STOP_MODE_OR_MASK) +#define ForceStopMODE(aStopMode) ((aStopMode & STOP_MODE_AND_MASK) | STOP_MODE_OR_MASK) #ifdef USE_ADAFRUIT_MOTOR_SHIELD #include # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD // some PCA9685 specific constants +#define PCA9685_DEFAULT_ADDRESS 0x60 #define PCA9685_GENERAL_CALL_ADDRESS 0x00 -#define PCA9685_SOFTWARE_RESET 6 -#define PCA9685_DEFAULT_ADDRESS 0x40 -#define PCA9685_MAX_CHANNELS 16 // 16 PWM channels on each PCA9685 expansion module -#define PCA9685_MODE1_REGISTER 0x0 -#define PCA9685_MODE_1_RESTART 7 -#define PCA9685_MODE_1_AUTOINCREMENT 5 -#define PCA9685_MODE_1_SLEEP 4 -#define PCA9685_FIRST_PWM_REGISTER 0x06 -#define PCA9685_PRESCALE_REGISTER 0xFE +#define PCA9685_SOFTWARE_RESET 6 +#define PCA9685_MAX_CHANNELS 16 // 16 PWM channels on each PCA9685 expansion module +#define PCA9685_MODE1_REGISTER 0x00 +#define PCA9685_MODE_1_RESTART 7 +#define PCA9685_MODE_1_AUTOINCREMENT 5 +#define PCA9685_MODE_1_SLEEP 4 +#define PCA9685_FIRST_PWM_REGISTER 0x06 +#define PCA9685_PRESCALE_REGISTER 0xFE #define PCA9685_PRESCALER_FOR_1600_HZ ((25000000L /(4096L * 1600))-1) // = 3 at 1600 Hz @@ -150,93 +195,92 @@ #endif // USE_ADAFRUIT_MOTOR_SHIELD struct EepromMotorInfoStruct { - uint8_t StartSpeed; - uint8_t StopSpeed; - uint8_t DriveSpeed; - uint8_t SpeedCompensation; + uint8_t StartSpeedPWM; + uint8_t StopSpeedPWM; + uint8_t DriveSpeedPWM; + uint8_t SpeedPWMCompensation; }; /* - * For ramp control + * Ramp control + * Ramp up speed in 16 steps every 16 millis from from StartSpeedPWM to RequestedDriveSpeedPWM. */ #define MOTOR_STATE_STOPPED 0 #define MOTOR_STATE_START 1 #define MOTOR_STATE_RAMP_UP 2 -#define MOTOR_STATE_DRIVE_SPEED 3 +#define MOTOR_STATE_DRIVE 3 #define MOTOR_STATE_RAMP_DOWN 4 -#ifdef SUPPORT_RAMP_UP -#define RAMP_UP_UPDATE_INTERVAL_MILLIS 16 // The smaller the value the steeper the ramp -#define RAMP_UP_UPDATE_INTERVAL_STEPS 16 // Results in a ramp up time of 16 steps * 16 millis = 256 milliseconds -#define RAMP_UP_VALUE_DELTA ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS) -#endif - class PWMDcMotor { public: PWMDcMotor(); #ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(uint8_t aMotorNumber, bool aReadFromEeprom = false); + void init(uint8_t aMotorNumber); # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD /* * Own internal functions for communicating with the PCA9685 Expander IC on the Adafruit motor shield */ - void I2CWriteByte(uint8_t aAddress, uint8_t aData); - void I2CSetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff); - void I2CSetPin(uint8_t aPin, bool aSetToOn); + void PCA9685WriteByte(uint8_t aAddress, uint8_t aData); + void PCA9685SetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff); + void PCA9685SetPin(uint8_t aPin, bool aSetToOn); # else - Adafruit_DCMotor * Adafruit_MotorShield_DcMotor; + Adafruit_DCMotor *Adafruit_MotorShield_DcMotor; # endif #else - void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber = 0); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin); #endif /* * Basic motor commands */ - void setSpeed(int aRequestedSpeed); - void setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection); - void setSpeedCompensated(int aRequestedSpeed); - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); + void setSpeedPWM(int aRequestedSpeedPWM); + void changeSpeedPWM(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setSpeedPWMCompensated(int aRequestedSpeedPWM); + void changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); void stop(uint8_t aStopMode = STOP_MODE_KEEP); // STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE - void setStopMode(uint8_t aStopMode); // mode for Speed==0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE + void setStopMode(uint8_t aStopMode); // mode for SpeedPWM==0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE /* * Fixed distance driving */ - void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation = 0); + void setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, uint8_t aSpeedPWMCompensation = 0); void setDefaultsForFixedDistanceDriving(); - void setSpeedCompensation(uint8_t aSpeedCompensation); - void setDriveSpeed(uint8_t aDriveSpeed); + void setSpeedPWMCompensation(uint8_t aSpeedPWMCompensation); + void setStartSpeedPWM(uint8_t aStartSpeedPWM); + void setDriveSpeedPWM(uint8_t aDriveSpeedPWM); -#ifdef SUPPORT_RAMP_UP void startRampUp(uint8_t aRequestedDirection); - void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); -#endif + void startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void startRampDown(); -#ifndef USE_ENCODER_MOTOR_CONTROL +#ifndef USE_ENCODER_MOTOR_CONTROL // required here, since we cannot access the computedMillisOfMotorStopForDistance and MillisPerMillimeter for the functions below // This function only makes sense for non encoder motors - void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); + void setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond); // These functions are implemented by encoder motor too - void startGoDistanceCount(int aRequestedDistanceCount); // Signed distance count - void startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + 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); bool updateMotor(); /* * Implementation for non encoder motors. Not used by CarControl. */ - void goDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void goDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + void goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); #endif /* - * EEPROM functions to read and store control values (DriveSpeed, SpeedCompensation) + * EEPROM functions to read and store control values (DriveSpeedPWM, SpeedPWMCompensation) */ - void readMotorValuesFromEeprom(); - void writeMotorvaluesToEeprom(); + void readMotorValuesFromEeprom(uint8_t aMotorValuesEepromStorageNumber); + void writeMotorValuesToEeprom(uint8_t aMotorValuesEepromStorageNumber); + + static void printSettings(Print *aSerial); /* * Internal functions @@ -250,56 +294,58 @@ class PWMDcMotor { uint8_t BackwardPin; #endif - int8_t MotorValuesEepromStorageNumber; // number of EEPROM EepromMotorInfoStruct block address for one PWMDcMotor. 0 means no corresponding EEPROM block. Set by init. /********************************** - * Start of EEPROM values + * Start of values for EEPROM *********************************/ /* - * Minimum speed setting at which motor starts moving. Depend on current voltage, load and surface. + * Minimum SpeedPWM setting at which motor starts moving. Depend on current voltage, load and surface. * Is set by calibrate() and then stored (with the other values) in eeprom. */ - uint8_t StartSpeed; // Speed PWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 - uint8_t DriveSpeed; // Speed PWM value used for going fixed distance. + uint8_t StartSpeedPWM; // SpeedPWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 + uint8_t DriveSpeedPWM; // SpeedPWM value used for going fixed distance. /* - * Positive value to be subtracted from TargetSpeed to get CurrentSpeed to compensate for different left and right motors - * Currently SpeedCompensation is in steps of 2 and only one motor can have a positive value, the other is set to zero. + * Positive value to be subtracted from TargetPWM to get CurrentSpeedPWM to compensate for different left and right motors + * Currently SpeedPWMCompensation is in steps of 2 and only one motor can have a positive value, the other is set to zero. * Value is computed in synchronizeMotor() */ - uint8_t SpeedCompensation; + uint8_t SpeedPWMCompensation; /********************************** * End of EEPROM values *********************************/ - uint8_t DefaultStopMode; // used for speed == 0 and STOP_MODE_KEEP - static bool MotorValuesHaveChanged; // true if DefaultStopMode, StartSpeed, DriveSpeed or SpeedCompensation have changed - for printing + uint8_t DefaultStopMode; // used for PWM == 0 and STOP_MODE_KEEP + static bool MotorControlValuesHaveChanged; // true if DefaultStopMode, StartSpeedPWM, DriveSpeedPWM or SpeedPWMCompensation have changed - for printing +#if defined(USE_MPU6050_IMU) || defined(USE_ENCODER_MOTOR_CONTROL) + volatile static bool SensorValuesHaveChanged; // true if encoder data or IMU data have changed +#endif - uint8_t CurrentSpeed; - uint8_t CurrentDirectionOrBrakeMode; // (of CurrentSpeed etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD, MOTOR_BRAKE, MOTOR_RELEASE - static bool SpeedOrMotorModeHasChanged; + uint8_t CurrentSpeedPWM; + uint8_t CurrentDirectionOrBrakeMode; // (of CurrentSpeedPWM etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD, MOTOR_BRAKE, MOTOR_RELEASE + uint8_t LastDirection; // Used for speed and distance. Contains DIRECTION_FORWARD, DIRECTION_BACKWARD but not MOTOR_BRAKE, MOTOR_RELEASE. + static bool MotorPWMHasChanged; - bool MotorMovesFixedDistance; // if true, stop if end distance condition is true + bool CheckDistanceInUpdateMotor; -#ifdef SUPPORT_RAMP_UP /* * For ramp control */ - uint8_t MotorRampState; // MOTOR_STATE_STOPPED, MOTOR_STATE_START, MOTOR_STATE_RAMP_UP, MOTOR_STATE_DRIVE_SPEED, MOTOR_STATE_RAMP_DOWN - uint8_t CurrentDriveSpeed; // DriveSpeed - SpeedCompensation; The DriveSpeed used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeed + uint8_t MotorRampState; // MOTOR_STATE_STOPPED, MOTOR_STATE_START, MOTOR_STATE_RAMP_UP, MOTOR_STATE_DRIVE, MOTOR_STATE_RAMP_DOWN + uint8_t RequestedDriveSpeedPWM; // DriveSpeedPWM - SpeedPWMCompensation; The DriveSpeedPWM used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeedPWM - uint8_t RampDelta; + uint8_t RampSpeedPWMDelta; // TODO is still constant, so remove? unsigned long NextRampChangeMillis; -#endif -#ifndef USE_ENCODER_MOTOR_CONTROL +#ifndef USE_ENCODER_MOTOR_CONTROL // this saves 5 bytes ram if we know, that we do not use the PWMDcMotor distance functions uint32_t computedMillisOfMotorStopForDistance; // Since we have no distance sensing, we must estimate a duration instead - unsigned int DistanceToTimeFactor; // Required for non encoder motors to estimate duration for a fixed distance + uint8_t MillisPerMillimeter; // Value for 2 volt motor effective voltage at DEFAULT_DRIVE_SPEED_PWM. Required for non encoder motors to estimate duration for a fixed distance #endif }; -//void PanicWithLed(unsigned int aDelay, uint8_t aCount); /* - * Version 1.1.0 - 10/2020 + * Version 2.0.0 - 11/2020 + * - IMU / MPU6050 support. + * - Support of off the shelf smart cars. * - Added and renamed functions. * * Version 1.0.0 - 9/2020 diff --git a/src/PathInfoPage.cpp b/src/PathInfoPage.cpp index 52acdb9..0dac202 100644 --- a/src/PathInfoPage.cpp +++ b/src/PathInfoPage.cpp @@ -37,10 +37,10 @@ void doResetPath(BDButton * aTheTouchedButton, int16_t aValue) { } void initPathInfoPage(void) { - TouchButtonResetPath.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_RED, F("Clear"), TEXT_SIZE_22, + TouchButtonResetPath.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_RED, F("Clear"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doResetPath); - TouchButtonBackSmall.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR_RED, F("Back"), TEXT_SIZE_22, + TouchButtonBackSmall.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR16_RED, F("Back"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &GUISwitchPages); } @@ -203,7 +203,7 @@ void DrawPath() { while (tXDeltaPtr <= sXPathDeltaPtr) { int tYDisplayDelta = (-(*tXDeltaPtr++)) >> tScaleShift; int tXDisplayDelta = (-(*tYDeltaPtr++)) >> tScaleShift; - BlueDisplay1.drawLineRel(tXDisplayPos, tYDisplayPos, tXDisplayDelta, tYDisplayDelta, COLOR_RED); + BlueDisplay1.drawLineRel(tXDisplayPos, tYDisplayPos, tXDisplayDelta, tYDisplayDelta, COLOR16_RED); tXDisplayPos += tXDisplayDelta; tYDisplayPos += tYDisplayDelta; } diff --git a/src/RobotCar.h b/src/RobotCar.h index c935f92..58859d9 100644 --- a/src/RobotCar.h +++ b/src/RobotCar.h @@ -20,14 +20,13 @@ #define SRC_ROBOTCAR_H_ #include -#include //#define CAR_HAS_4_WHEELS //#define USE_LAYOUT_FOR_NANO // Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger. -//#define USE_US_SENSOR_1_PIN_MODE // Comment it out, if you use modified HC-SR04 modules or HY-SRF05 ones. +//#define USE_US_SENSOR_1_PIN_MODE // Activate it, if you use modified HC-SR04 modules or HY-SRF05 ones. //#define CAR_HAS_IR_DISTANCE_SENSOR @@ -49,8 +48,25 @@ */ // #define ENABLE_RTTTL // -#include "CarMotorControl.h" -extern CarMotorControl RobotCarMotorControl; +/* + * Shows VIN voltage and monitors it for undervoltage. VIN/11 at A2, 1MOhm to VIN, 100kOhm to ground + */ +//#define MONITOR_VIN_VOLTAGE +#if !defined(VIN_VOLTAGE_CORRECTION) +# ifdef ARDUINO_AVR_UNO +#define VIN_VOLTAGE_CORRECTION 0.8 +# endif +#endif +/* + * Activates the buttons to store compensation and drive speed + */ +//#define SUPPORT_EEPROM_STORAGE +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) +#include +#endif + +#include "CarPWMMotorControl.h" +extern CarPWMMotorControl RobotCarMotorControl; /* * Pin usage @@ -65,19 +81,19 @@ extern CarMotorControl RobotCarMotorControl; * 6 O Left motor PWM / NC for UNO board - connected to ENA * 7 O Right motor back / NC for UNO board - connected to IN3 * 8 O Left motor fwd / NC for UNO board - connected to IN2 - * 9 O Servo US distance - Servo Nr. 2 on Adafruit Motor Shield - * 10 O Servo laser pan - Servo Nr. 1 on Adafruit Motor Shield - * 11 O Servo laser tilt / Speaker for UNO board - * 12 O Left motor back / NC for UNO board - connected to IN1 + * 9 O Left motor back / NC for UNO board - connected to IN1 + * 10 O Servo US distance - Servo Nr. 2 on Adafruit Motor Shield + * 11 O Servo laser pan + * 12 O Servo laser tilt / Speaker for UNO board * 13 O Laser power * * A0 O US trigger (and echo in 1 pin US sensor mode) - * A1 I IR distance (needs 1 pin US sensor mode) / US echo - * A2 I VIN/11, 1MOhm to VIN, 100kOhm to ground. - * A3 IP NC + * A1 I US echo + * A2 I VIN/11, 1MOhm to VIN, 100kOhm to ground + * A3 I IR remote control signal in / IR distance / Speaker for Nano board * A4 SDA NC for Nano / I2C for UNO board motor shield * A5 SCL NC for Nano / I2C for UNO board motor shield - * A6 O Speaker for Nano board / not available on UNO board + * A6 O * A7 O Camera supply control */ @@ -91,24 +107,28 @@ extern CarMotorControl RobotCarMotorControl; #define PIN_RIGHT_MOTOR_BACKWARD 7 // IN3 #define PIN_RIGHT_MOTOR_PWM 5 // ENB - Must be PWM capable -#define PIN_LEFT_MOTOR_FORWARD 12 // IN1 - Pin 9 is already reserved for distance servo +#define PIN_LEFT_MOTOR_FORWARD 9 // IN1 #define PIN_LEFT_MOTOR_BACKWARD 8 // IN2 #define PIN_LEFT_MOTOR_PWM 6 // ENA - Must be PWM capable #endif +#ifdef USE_ENCODER_MOTOR_CONTROL +#define RIGHT_MOTOR_INTERRUPT INT0 // Pin 2 +#define LEFT_MOTOR_INTERRUPT INT1 // Pin 3 +#endif /* * Servo pins */ -#define PIN_DISTANCE_SERVO 9 // Servo Nr. 2 on Adafruit Motor Shield +#define PIN_DISTANCE_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield #ifdef CAR_HAS_PAN_SERVO -#define PIN_PAN_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield +#define PIN_PAN_SERVO 11 #endif #ifdef CAR_HAS_TILT_SERVO -#define PIN_TILT_SERVO 11 +#define PIN_TILT_SERVO 12 #endif -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) // Pin A0 for VCC monitoring - ADC channel 2 // Assume an attached resistor network of 100k / 10k from VCC to ground (divider by 11) #define VIN_11TH_IN_CHANNEL 2 // = A2 @@ -150,7 +170,7 @@ extern CarMotorControl RobotCarMotorControl; # ifdef CAR_HAS_CAMERA #define PIN_CAMERA_SUPPLY_CONTROL 4 # endif -#define PIN_BUZZER 11 +#define PIN_BUZZER 12 #endif /************************** @@ -180,20 +200,19 @@ extern Servo PanServo; extern Servo TiltServo; #endif - /************************************************************************************ * Definitions and declarations only used for GUI in RobotCarBlueDisplay.cpp example ************************************************************************************/ #define MINIMUM_DISTANCE_TO_SIDE 21 #define MINIMUM_DISTANCE_TO_FRONT 35 -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) #include "ADCUtils.h" extern float sVINVoltage; -#if defined(MONITOR_LIPO_VOLTAGE) -#define VOLTAGE_LOW_THRESHOLD 6.9 // Formula: 2 * 3.5 volt - voltage loss: 25 mV GND + 45 mV VIN + 35 mV Battery holder internal -#define VOLTAGE_USB_THRESHOLD 5.5 +#if defined(MONITOR_VIN_VOLTAGE) +#define VOLTAGE_LIPO_LOW_THRESHOLD 6.9 // Formula: 2 * 3.5 volt - voltage loss: 25 mV GND + 45 mV VIN + 35 mV Battery holder internal +#define VOLTAGE_USB_THRESHOLD 5.5 #else #endif #define VOLTAGE_TOO_LOW_DELAY_ONLINE 3000 // display VIN every 500 ms for 4 seconds diff --git a/src/RobotCarBlueDisplay.ino b/src/RobotCarBlueDisplay.ino index 7414110..cf574a3 100644 --- a/src/RobotCarBlueDisplay.ino +++ b/src/RobotCarBlueDisplay.ino @@ -39,12 +39,15 @@ #include #endif +//#define DEBUG_TRACE_INIT +//#include "AvrTracing.cpp.h" + /**************************************************************************** * Change this if you have reprogrammed the hc05 module for other baud rate ***************************************************************************/ #ifndef BLUETOOTH_BAUD_RATE -#define BLUETOOTH_BAUD_RATE BAUD_115200 -//#define BLUETOOTH_BAUD_RATE BAUD_9600 +//#define BLUETOOTH_BAUD_RATE BAUD_115200 +#define BLUETOOTH_BAUD_RATE BAUD_9600 #endif #define VERSION_EXAMPLE "3.0" @@ -52,7 +55,7 @@ /* * Car Control */ -CarMotorControl RobotCarMotorControl; +CarPWMMotorControl RobotCarMotorControl; float sVINVoltage; #ifdef ENABLE_RTTTL @@ -146,26 +149,33 @@ void setup() { tone(PIN_BUZZER, 2200, 50); // Booted - // initialize motors + initSerial(BLUETOOTH_BAUD_RATE); +// initTrace(); +// printNumberOfPushesForISR(); + + // initialize motors, this also stops motors #ifdef USE_ADAFRUIT_MOTOR_SHIELD - RobotCarMotorControl.init(true); // true -> read from EEPROM + RobotCarMotorControl.init(); #else # ifdef USE_ENCODER_MOTOR_CONTROL - RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, PIN_LEFT_MOTOR_FORWARD, - PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, true); // true -> read from EEPROM + RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, RIGHT_MOTOR_INTERRUPT, PIN_LEFT_MOTOR_FORWARD, + PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, LEFT_MOTOR_INTERRUPT); # else RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, PIN_LEFT_MOTOR_FORWARD, - PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, false); // false -> do NOT read from EEPROM + PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM); # endif #endif - RobotCarMotorControl.stopMotors(); // in case motors were running before + +#ifdef SUPPORT_EEPROM_STORAGE + RobotCarMotorControl.readMotorValuesFromEeprom(); +#endif + + sRuningAutonomousDrive = false; delay(100); tone(PIN_BUZZER, 2200, 50); // motor initialized - sLastServoAngleInDegrees = 90; - - initSerial(BLUETOOTH_BAUD_RATE); + sLastServoAngleInDegrees = 90; // is required before setupGUI() // Must be after RobotCarMotorControl.init, since it tries to stop motors in connect callback setupGUI(); // this enables output by BlueDisplay1 and lasts around 100 milliseconds @@ -178,8 +188,8 @@ void setup() { // BlueDisplay1.debug("sMCUSR=", sMCUSR); } else { #if !defined(USE_SIMPLE_SERIAL) && !defined(USE_SERIAL1) // print it now if not printed above -#if defined(__AVR_ATmega32U4__) || defined(SERIAL_USB) || defined(SERIAL_PORT_USBVIRTUAL) - delay(2000); // To be able to connect Serial monitor after reset and before first printout +#if defined(__AVR_ATmega32U4__) || defined(SERIAL_USB) || defined(SERIAL_PORT_USBVIRTUAL) || defined(ARDUINO_attiny3217) + delay(4000); // To be able to connect Serial monitor after reset or power up and before first printout #endif // Just to know which program is running on my Arduino Serial.println(F("START " __FILE__ "\r\nVersion " VERSION_EXAMPLE " from " __DATE__)); @@ -200,18 +210,16 @@ void setup() { #endif initDistance(); -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) readVINVoltage(); randomSeed(sVINVoltage * 100); #endif -// initTrace(); - delay(100); if (sBootReasonWasPowerUp) { - tone(PIN_BUZZER, 2200, 50); // power up finished + tone(PIN_BUZZER, 1000, 50); // power up finished } else { - tone(PIN_BUZZER, 2200, 300); // long tone, reset finished - only detectable with Optiboot 8.0 + tone(PIN_BUZZER, 1000, 300); // long tone, reset finished - only detectable with Optiboot 8.0 } } @@ -222,10 +230,11 @@ void loop() { */ if ((!BlueDisplay1.isConnectionEstablished()) && (millis() < (TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + 1000)) && (millis() > TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) && (sVINVoltage > VOLTAGE_USB_THRESHOLD) #endif ) { + /* * Timeout just reached, play melody and start autonomous drive */ @@ -235,6 +244,7 @@ void loop() { #else delayAndLoopGUI(6000); // delay needed for millis() check above! #endif + // check again, maybe we are connected now if (!BlueDisplay1.isConnectionEstablished()) { // Set right page for reconnect GUISwitchPages(NULL, PAGE_AUTOMATIC_CONTROL); @@ -246,6 +256,10 @@ void loop() { } } + /* + * Required, if we use rotation, ramps and fixed distance driving + */ + RobotCarMotorControl.updateMotors(); /* * check for user input and update display output */ @@ -257,7 +271,11 @@ void loop() { 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 fillAndShowForwardDistancesInfo(true, true); +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) DistanceServo.write(90); // set servo back to normal +#else + write10(90); +#endif } #ifdef ENABLE_RTTTL @@ -265,25 +283,12 @@ void loop() { * check for playing melody */ if (sPlayMelody) { - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); playRandomMelody(); } #endif - if (sCurrentPage == PAGE_TEST) { - /* - * Direct speed control by GUI - */ - if (RobotCarMotorControl.updateMotors()) { -#ifdef USE_ENCODER_MOTOR_CONTROL - // At least one motor is moving here - RobotCarMotorControl.rightCarMotor.synchronizeMotor(&RobotCarMotorControl.leftCarMotor, - MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS); -#endif - } - } - if (sRuningAutonomousDrive) { if (sDriveMode == MODE_FOLLOWER) { driveFollowerModeOneStep(); @@ -293,21 +298,18 @@ void loop() { } } -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) void readVINVoltage() { - uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(VIN_11TH_IN_CHANNEL, INTERNAL); - uint16_t tVIN = readADCChannelWithReferenceOversample(VIN_11TH_IN_CHANNEL, INTERNAL, 2); // 4 samples + uint16_t tVIN = readADCChannelWithReferenceOversampleFast(VIN_11TH_IN_CHANNEL, INTERNAL, 2); // 4 samples // BlueDisplay1.debug("VIN Raw=", tVIN); - // Switch back (to DEFAULT) - ADMUX = tOldADMUX; -// assume resistor network of 1Mk / 100k (divider by 11) -// tVCC * 0,01181640625 +// assume resistor network of 1MOhm / 100kOhm (divider by 11) +// tVIN * 0,01182795 #ifdef VIN_VOLTAGE_CORRECTION // we have a diode (requires 0.8 volt) between LIPO and VIN - sVINVoltage = (tVIN * ((11.0 * 1.07) / 1023)) + VIN_VOLTAGE_CORRECTION; + sVINVoltage = (tVIN * ((11.0 * 1.1) / 1023)) + VIN_VOLTAGE_CORRECTION; #else - sVINVoltage = tVIN * ((11.0 * 1.07) / 1023); + sVINVoltage = tVIN * ((11.0 * 1.1) / 1023); #endif } #endif @@ -323,7 +325,8 @@ void playRandomMelody() { // BlueDisplay1.debug("Play melody"); #if defined(USE_ADAFRUIT_MOTOR_SHIELD) - startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesSmall, ARRAY_SIZE_MELODIES_SMALL); + startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesTiny, ARRAY_SIZE_MELODIES_TINY); +// startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesSmall, ARRAY_SIZE_MELODIES_SMALL); #else OCR2B = 0; bitWrite(TIMSK2, OCIE2B, 1); // enable interrupt for inverted pin handling @@ -391,6 +394,7 @@ Servo TiltServo; #endif void resetServos() { + sLastServoAngleInDegrees = 0; // to force setting of 90 degree DistanceServoWriteAndDelay(90, false); #ifdef CAR_HAS_PAN_SERVO PanServo.write(90); @@ -401,10 +405,16 @@ void resetServos() { } void initServos() { +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) // DistanceServo.attach(PIN_DISTANCE_SERVO, DISTANCE_SERVO_MIN_PULSE_WIDTH, DISTANCE_SERVO_MAX_PULSE_WIDTH); DistanceServo.attach(PIN_DISTANCE_SERVO); +#else + // Servo for distance sensor + initLightweightServoPin10(); +#endif + #ifdef CAR_HAS_PAN_SERVO -// initialize and set Laser pan servo +// initialize and set laser pan servo PanServo.attach(PIN_PAN_SERVO); #endif #ifdef CAR_HAS_TILT_SERVO diff --git a/src/RobotCarCommonGui.cpp b/src/RobotCarCommonGui.cpp index 88c3004..a4e9405 100644 --- a/src/RobotCarCommonGui.cpp +++ b/src/RobotCarCommonGui.cpp @@ -27,6 +27,9 @@ #include "RobotCar.h" #include "RobotCarGui.h" #include "Distance.h" +#ifdef USE_MPU6050_IMU +#include "IMUCarData.h" +#endif // a string buffer for BD info output char sStringBuffer[128]; @@ -36,14 +39,14 @@ bool sSensorCallbacksEnabled; uint8_t sCurrentPage; BDButton TouchButtonBack; BDButton TouchButtonAutomaticDrivePage; -#ifdef USE_ENCODER_MOTOR_CONTROL -BDButton TouchButtonCalibrate; -#else -BDButton TouchButtonCompensation; -#endif BDButton TouchButtonCompensationRight; BDButton TouchButtonCompensationLeft; +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +BDButton TouchButtonCalibrate; +#endif +#ifdef SUPPORT_EEPROM_STORAGE BDButton TouchButtonCompensationStore; +#endif BDButton TouchButtonRobotCarStartStop; //bool sRobotCarMoving; // main start flag. If true motors are running and will be updated by main loop. @@ -68,8 +71,6 @@ BDSlider SliderIRDistance; unsigned int sSliderIRLastCentimeter; #endif -uint32_t sMillisOfNextVCCInfo = 0; - void setupGUI(void) { sCurrentPage = PAGE_HOME; @@ -86,15 +87,21 @@ void delayAndLoopGUI(uint16_t aDelayMillis) { } void loopGUI(void) { + /* + * Update IMU data first, they may be displayed later on + */ +#ifdef USE_MPU6050_IMU + // if we are going a fixed distance or are turning, updateTurnAngle() is yet called by updateMotors(), + // but updateIMUData has a timer to avoid multiple reading + RobotCarMotorControl.updateIMUData(); +#endif if (BlueDisplay1.isConnectionEstablished()) { - // do not show anything during motor speed ramps - if (!RobotCarMotorControl.isState(MOTOR_STATE_RAMP_UP)) { - /* - * All functions are empty until now - * actions are centrally managed below - */ + /* + * All functions are empty until now + * actions are centrally managed below + */ // if (sCurrentPage == PAGE_HOME) { // loopHomePage(); // } else if (sCurrentPage == PAGE_TEST) { @@ -112,32 +119,26 @@ void loopGUI(void) { // for all but PathInfo page if (sCurrentPage != PAGE_SHOW_PATH) { #endif -#if defined(MONITOR_LIPO_VOLTAGE) - readCheckAndPrintVinPeriodically(); -#endif - // For Home, sensorDrive and Test page - if (sCurrentPage != PAGE_AUTOMATIC_CONTROL) { - if (sCurrentPage != PAGE_BT_SENSOR_CONTROL) { - // For Home and Test page - readAndShowDistancePeriodically(DISTANCE_DISPLAY_PERIOD_MILLIS); - } - printMotorValues(); -#ifdef USE_ENCODER_MOTOR_CONTROL - displayVelocitySliderValues(); - /* - * Print changed tick values - */ - if (EncoderMotor::EncoderTickCounterHasChanged) { - EncoderMotor::EncoderTickCounterHasChanged = false; - printMotorDistanceValues(); - } +#if defined(MONITOR_VIN_VOLTAGE) + readCheckAndPrintVinPeriodically(); #endif + // For Home, sensorDrive and Test page + if (sCurrentPage != PAGE_AUTOMATIC_CONTROL) { + if (sCurrentPage != PAGE_BT_SENSOR_CONTROL) { + // For Home and Test page + readAndShowDistancePeriodically(); } +#if defined(USE_MPU6050_IMU) + printIMUOffsetValues(); +#endif + printMotorValuesPeriodically(); + } #ifdef ENABLE_PATH_INFO_PAGE } #endif - } + } + /* * Check if receive buffer contains an event * Do it at end since it may change the condition for calling this function and its printing @@ -156,16 +157,19 @@ void startStopRobotCar(bool aDoStart) { * Start for sensor drive */ sSensorChangeCallCountForZeroAdjustment = 0; - registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_UI, FLAG_SENSOR_NO_FILTER, + registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_NORMAL, FLAG_SENSOR_NO_FILTER, &doSensorChange); // Lock the screen orientation to avoid screen flip while rotating the smartphone BlueDisplay1.setScreenOrientationLock(FLAG_SCREEN_ORIENTATION_LOCK_CURRENT); sSensorCallbacksEnabled = true; - } else if (sLastSpeedSliderValue > 0) { + } else { + if (sLastSpeedSliderValue == 0) { + sLastSpeedSliderValue = RobotCarMotorControl.rightCarMotor.StartSpeedPWM; + } /* * Start car to last speed slider value */ - RobotCarMotorControl.setSpeedCompensated(sLastSpeedSliderValue, sRobotCarDirection); + RobotCarMotorControl.setSpeedPWMCompensated(sLastSpeedSliderValue, sRobotCarDirection); tSpeedSliderValue = sLastSpeedSliderValue; } } else { @@ -173,21 +177,21 @@ void startStopRobotCar(bool aDoStart) { * Stop car */ if (sRuningAutonomousDrive) { - startStopAutomomousDrive(false); + startStopAutomomousDrive(false, MODE_MANUAL_DRIVE); } if (sSensorCallbacksEnabled) { /* * Global stop for sensor drive */ - registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_UI, FLAG_SENSOR_NO_FILTER, NULL); + registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_NORMAL, FLAG_SENSOR_NO_FILTER, NULL); BlueDisplay1.setScreenOrientationLock(FLAG_SCREEN_ORIENTATION_LOCK_UNLOCK); sSensorCallbacksEnabled = false; } - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } bool tShowValues = sCurrentPage == PAGE_HOME || sCurrentPage == PAGE_TEST; - // set button value always just for the case we are called by another than Stop button callback +// set button value always just for the case we are called by another than Stop button callback TouchButtonRobotCarStartStop.setValue(!RobotCarMotorControl.isStopped() || sSensorCallbacksEnabled, tShowValues); if (tShowValues) { SliderSpeed.setValueAndDrawBar(tSpeedSliderValue); @@ -197,14 +201,22 @@ void startStopRobotCar(bool aDoStart) { #pragma GCC diagnostic ignored "-Wunused-parameter" -void doRobotCarStartStop(BDButton * aTheTouchedButton, int16_t aDoStart) { +void doStartStopRobotCar(BDButton *aTheTouchedButton, int16_t aDoStart) { startStopRobotCar(aDoStart); } -#ifdef USE_ENCODER_MOTOR_CONTROL -void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.calibrate(); - printMotorValues(); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +void doCalibrate(BDButton *aTheTouchedButton, int16_t aValue) { + TouchButtonRobotCarStartStop.setValueAndDraw(RobotCarMotorControl.isStopped()); + if (RobotCarMotorControl.isStopped()) { + RobotCarMotorControl.calibrate(&loopGUI); +#ifdef SUPPORT_EEPROM_STORAGE + RobotCarMotorControl.writeMotorValuesToEeprom(); +#endif + } else { + // second / recursive call of doCalibrate() + RobotCarMotorControl.stop(); + } } #endif @@ -212,7 +224,7 @@ void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue) { * Minimum Speed is 30 for USB power and no load, 50 for load * Minimum Speed is 20 for 2 Lithium 18650 battery power and no load, 25 for load */ -void doSpeedSlider(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doSpeedSlider(BDSlider *aTheTouchedSlider, uint16_t aValue) { if (aValue != sLastSpeedSliderValue) { sLastSpeedSliderValue = aValue; @@ -220,64 +232,41 @@ void doSpeedSlider(BDSlider * aTheTouchedSlider, uint16_t aValue) { // handle GUI startStopRobotCar(true); } else { - RobotCarMotorControl.setSpeedCompensated(aValue, sRobotCarDirection); + RobotCarMotorControl.setSpeedPWMCompensated(aValue, sRobotCarDirection); } } } -void doUSServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doUSServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { DistanceServoWriteAndDelay(aValue); } -#ifdef USE_ENCODER_MOTOR_CONTROL -/* - * Display velocity as slider values - */ -void displayVelocitySliderValues() { - EncoderMotor * tMotorInfo = &RobotCarMotorControl.leftCarMotor; - BDSlider * tSliderPtr = &SliderSpeedLeft; - uint16_t tXPos = 0; - for (int i = 0; i < 2; ++i) { - if (EncoderMotor::EncoderTickCounterHasChanged) { - tSliderPtr->setValueAndDrawBar(tMotorInfo->CurrentVelocity); - } - tMotorInfo = &RobotCarMotorControl.rightCarMotor; - tSliderPtr = &SliderSpeedRight; - tXPos += BUTTON_WIDTH_16 + 4; - } -} -#endif - /* * Stops car and change direction */ -void doSetDirection(BDButton * aTheTouchedButton, int16_t aValue) { +void doSetDirection(BDButton *aTheTouchedButton, int16_t aValue) { sRobotCarDirection = !aValue; // use inverse value since true is forward BUT 0 is DIRECTION_FORWARD // Stop fixed directions and turns using RobotCarMotorControl if (!RobotCarMotorControl.isStopped()) { - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } // Stop direct movement by slider startStopRobotCar(false); } -void doStoreRightMotorSpeedCompensation(float aRightMotorSpeedCompensation) { - int8_t tRightMotorSpeedCompensation = aRightMotorSpeedCompensation; - RobotCarMotorControl.setValuesForFixedDistanceDriving(RobotCarMotorControl.rightCarMotor.StartSpeed, - RobotCarMotorControl.rightCarMotor.DriveSpeed, tRightMotorSpeedCompensation); -} - /* - * Request speed value as number + * changes speed compensation by +1 or -1 */ -void doSetCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedCompensation) { - RobotCarMotorControl.setSpeedCompensation(aRightMotorSpeedCompensation); +void doSetCompensation(BDButton *aTheTouchedButton, int16_t aRightMotorSpeedPWMCompensation) { + RobotCarMotorControl.changeSpeedPWMCompensation(aRightMotorSpeedPWMCompensation); } -void doStoreCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedCompensation) { - RobotCarMotorControl.writeMotorvaluesToEeprom(); +#ifdef SUPPORT_EEPROM_STORAGE +void doStoreCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedPWMCompensation) { + RobotCarMotorControl.writeMotorValuesToEeprom(); } +#endif void startCurrentPage() { switch (sCurrentPage) { @@ -307,7 +296,7 @@ void startCurrentPage() { * Stop old page and start new one * @param aValue the Page number of the new page number */ -void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { +void GUISwitchPages(BDButton *aTheTouchedButton, int16_t aValue) { /* * Stop old page */ @@ -332,6 +321,9 @@ void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { #endif } +#ifdef USE_MPU6050_IMU + RobotCarMotorControl.IMUData.resetOffsetData(); // just to have a fresh start +#endif /* * Start new page */ @@ -340,9 +332,6 @@ void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { } void initRobotCarDisplay(void) { -// Stop any demo movement. - RobotCarMotorControl.stopMotors(); - sRuningAutonomousDrive = false; BlueDisplay1.setFlagsAndSize(BD_FLAG_FIRST_RESET_ALL | BD_FLAG_TOUCH_BASIC_DISABLE | BD_FLAG_USE_MAX_SIZE, DISPLAY_WIDTH, DISPLAY_HEIGHT); @@ -354,43 +343,45 @@ void initRobotCarDisplay(void) { /* * Common control buttons */ - TouchButtonRobotCarStartStop.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_BLUE, F("Start"), - TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doRobotCarStartStop); + 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, sRuningAutonomousDrive, &doStartStopRobotCar); TouchButtonRobotCarStartStop.setCaptionForValueTrue(F("Stop")); - TouchButtonBack.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Back"), + 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); -#ifdef USE_ENCODER_MOTOR_CONTROL - TouchButtonCalibrate.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("CAL"), +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + TouchButtonCalibrate.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_RED, F("CAL"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doCalibrate); #endif // Direction Button value true is forward, false is backward BUT 0 is DIRECTION_FORWARD!!! - TouchButtonDirection.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLACK, F("\x88"), + TouchButtonDirection.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLACK, F("\x88"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, true, &doSetDirection); TouchButtonDirection.setCaptionForValueTrue("\x87"); - TouchButtonCompensationLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, - BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR_BLUE, F("<-Co"), TEXT_SIZE_11, + TouchButtonCompensationLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4, + BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR16_BLUE, F("<-Co"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -1, &doSetCompensation); - TouchButtonCompensationRight.init(BUTTON_WIDTH_8_POS_5 - (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8_LINE_5, - BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR_BLUE, F("mp->"), TEXT_SIZE_11, + TouchButtonCompensationRight.init(BUTTON_WIDTH_8_POS_5 - (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8_LINE_4, + BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR16_BLUE, F("mp->"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doSetCompensation); - TouchButtonCompensationStore.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, +#ifdef SUPPORT_EEPROM_STORAGE + TouchButtonCompensationStore.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("Store"), TEXT_SIZE_10, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doStoreCompensation); +#endif /* * Speed Sliders */ - SliderSpeed.init(0, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, SPEED_SLIDER_SIZE, 200, 0, COLOR_YELLOW, SLIDER_DEFAULT_BAR_COLOR, + SliderSpeed.init(0, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, SPEED_SLIDER_SIZE, 200, 0, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doSpeedSlider); SliderSpeed.setScaleFactor(255.0 / SPEED_SLIDER_SIZE); // Slider is virtually 2 times larger than displayed, values were divided by 2 - SliderSpeedLeft.init(BUTTON_WIDTH_6 + 4, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, + SliderSpeedLeft.init(MOTOR_INFO_START_X, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderSpeedLeft.setValueFormatString("%3d"); // Since we also send values grater 100 - SliderSpeedRight.init(BUTTON_WIDTH_6 + 4 + BUTTON_WIDTH_16 + 8, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, + SliderSpeedRight.init(MOTOR_INFO_START_X + BUTTON_WIDTH_16 + 8, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderSpeedRight.setValueFormatString("%3d"); @@ -417,9 +408,9 @@ void initRobotCarDisplay(void) { #endif SliderUSPosition.init(POS_X_US_POSITION_SLIDER - BUTTON_WIDTH_6, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, US_SLIDER_SIZE, 90, 90, - COLOR_YELLOW, + COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doUSServoPosition); - SliderUSPosition.setBarThresholdColor(COLOR_BLUE); + SliderUSPosition.setBarThresholdColor(COLOR16_BLUE); SliderUSPosition.setScaleFactor(180.0 / US_SLIDER_SIZE); // Values from 0 to 180 degrees SliderUSPosition.setValueUnitString("\xB0"); // \xB0 is degree character @@ -433,22 +424,22 @@ void initRobotCarDisplay(void) { // Small US distance slider with captions and without cm units SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); - SliderUSDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR_BLACK, - COLOR_WHITE); + SliderUSDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR16_BLACK, + COLOR16_WHITE); SliderUSDistance.setCaption("US"); // below caption - left aligned SliderUSDistance.setPrintValueProperties(11, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, - 4 + TEXT_SIZE_10_HEIGHT, COLOR_BLACK, COLOR_WHITE); + 4 + TEXT_SIZE_10_HEIGHT, COLOR16_BLACK, COLOR16_WHITE); #else - // Big US distance slider without caption but with cm units POS_X_THIRD_SLIDER because it is the position of the left edge +// Big US distance slider without caption but with cm units POS_X_THIRD_SLIDER because it is the position of the left edge SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - BUTTON_WIDTH_10, SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, BUTTON_WIDTH_10, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, - FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, + SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderUSDistance.setValueUnitString("cm"); #endif - SliderUSDistance.setScaleFactor(2); // Slider is virtually 2 times larger, values were divided by 2 + SliderUSDistance.setScaleFactor(DISTANCE_SLIDER_SCALE_FACTOR); // Slider is virtually 2 times larger, values were divided by 2 SliderUSDistance.setBarThresholdColor(DISTANCE_TIMEOUT_COLOR); /* @@ -458,25 +449,25 @@ void initRobotCarDisplay(void) { // Small IR distance slider with captions and without cm units SliderIRDistance.init(POS_X_THIRD_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); - SliderIRDistance.setScaleFactor(2); // Slider is virtually 2 times larger, values were divided by 2 + SliderIRDistance.setScaleFactor(DISTANCE_SLIDER_SCALE_FACTOR); // Slider is virtually 2 times larger, values were divided by 2 SliderIRDistance.setBarThresholdColor(DISTANCE_TIMEOUT_COLOR); // Caption properties - SliderIRDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR_BLACK, - COLOR_WHITE); + SliderIRDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR16_BLACK, + COLOR16_WHITE); // Captions SliderIRDistance.setCaption("IR"); // value below caption - right aligned SliderIRDistance.setPrintValueProperties(11, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, - 4 + TEXT_SIZE_10_HEIGHT, COLOR_BLACK, COLOR_WHITE); + 4 + TEXT_SIZE_10_HEIGHT, COLOR16_BLACK, COLOR16_WHITE); #endif #ifdef CAR_HAS_PAN_SERVO // left of SliderUSPosition - SliderPan.init(POS_X_PAN_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, 90, COLOR_YELLOW, + SliderPan.init(POS_X_PAN_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, 90, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doHorizontalServoPosition); - SliderPan.setBarThresholdColor(COLOR_BLUE); + SliderPan.setBarThresholdColor(COLOR16_BLUE); // scale slider values SliderPan.setScaleFactor(180.0 / LASER_SLIDER_SIZE); // Values from 0 to 180 degrees SliderPan.setValueUnitString("\xB0"); // \xB0 is degree character @@ -484,9 +475,8 @@ void initRobotCarDisplay(void) { #ifdef CAR_HAS_TILT_SERVO SliderTilt.init(POS_X_TILT_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, - TILT_SERVO_MIN_VALUE, COLOR_YELLOW, - SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doVerticalServoPosition); - SliderTilt.setBarThresholdColor(COLOR_BLUE); + TILT_SERVO_MIN_VALUE, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doVerticalServoPosition); + SliderTilt.setBarThresholdColor(COLOR16_BLUE); // scale slider values SliderTilt.setScaleFactor(180.0 / LASER_SLIDER_SIZE); // Values from 0 to 180 degrees SliderTilt.setValueUnitString("\xB0"); // \xB0 is degree character @@ -504,18 +494,18 @@ void initRobotCarDisplay(void) { void drawCommonGui(void) { clearDisplayAndDisableButtonsAndSliders(); - BlueDisplay1.drawText(HEADER_X, TEXT_SIZE_22_HEIGHT, F("Robot Car"), TEXT_SIZE_22, COLOR_BLUE, - COLOR_NO_BACKGROUND); + BlueDisplay1.drawText(HEADER_X, TEXT_SIZE_22_HEIGHT, F("Robot Car"), TEXT_SIZE_22, COLOR16_BLUE, + COLOR16_NO_BACKGROUND); } -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) /* * Print VIN (used as motor supply) periodically * returns true if voltage was printed */ void readAndPrintVin() { char tDataBuffer[18]; - char tVCCString[6]; + char tVCCString[5]; readVINVoltage(); dtostrf(sVINVoltage, 4, 2, tVCCString); sprintf_P(tDataBuffer, PSTR("%s volt"), tVCCString); @@ -534,24 +524,24 @@ void readAndPrintVin() { tPosX = BUTTON_WIDTH_3_POS_3 - BUTTON_DEFAULT_SPACING - (8 * TEXT_SIZE_11_WIDTH); tPosY = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; } - BlueDisplay1.drawText(tPosX, tPosY, tDataBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); + BlueDisplay1.drawText(tPosX, tPosY, tDataBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); } void checkForLowVoltage() { static uint8_t sLowVoltageCount = 0; - if (sVINVoltage < VOLTAGE_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD) { + if (sVINVoltage < VOLTAGE_LIPO_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD) { sLowVoltageCount++; } else if (sLowVoltageCount > 1) { sLowVoltageCount--; } if (sLowVoltageCount > 2) { // Here more than 2 consecutive times (for 6 seconds) low voltage detected - RobotCarMotorControl.stopMotors(); + startStopRobotCar(false); if (BlueDisplay1.isConnectionEstablished()) { drawCommonGui(); - BlueDisplay1.drawText(10, 50, F("Battery voltage"), TEXT_SIZE_33, COLOR_RED, COLOR_WHITE); + BlueDisplay1.drawText(10, 50, F("Battery voltage"), TEXT_SIZE_33, COLOR16_RED, COLOR16_WHITE); // print current "too low" voltage char tDataBuffer[18]; char tVCCString[6]; @@ -572,7 +562,7 @@ void checkForLowVoltage() { delayMillisWithCheckAndHandleEvents(500); // and wait tLoopCount--; readVINVoltage(); // read new VCC value - } while (tLoopCount > 0 || (sVINVoltage < VOLTAGE_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD)); + } while (tLoopCount > 0 || (sVINVoltage < VOLTAGE_LIPO_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD)); // refresh current page GUISwitchPages(NULL, 0); } else { @@ -582,53 +572,154 @@ void checkForLowVoltage() { } void readCheckAndPrintVinPeriodically() { + static uint32_t sMillisOfLastVCCInfo; uint32_t tMillis = millis(); - if (tMillis >= sMillisOfNextVCCInfo) { - sMillisOfNextVCCInfo = tMillis + PRINT_VOLTAGE_PERIOD_MILLIS; + if (tMillis - sMillisOfLastVCCInfo >= PRINT_VOLTAGE_PERIOD_MILLIS) { + sMillisOfLastVCCInfo = tMillis; readAndPrintVin(); checkForLowVoltage(); + PWMDcMotor::MotorControlValuesHaveChanged = true; // to signal, that PWM voltages may have changed } } -#endif - -void printMotorSpeed() { - if (PWMDcMotor::SpeedOrMotorModeHasChanged) { - PWMDcMotor::SpeedOrMotorModeHasChanged = false; -// position below caption of speed slider - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + TEXT_SIZE_11_HEIGHT; - sprintf_P(sStringBuffer, PSTR("speed%3d %3d"), RobotCarMotorControl.leftCarMotor.CurrentSpeed, - RobotCarMotorControl.rightCarMotor.CurrentSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); - } -} +#endif // defined(MONITOR_VIN_VOLTAGE) -void printMotorValues() { - printMotorSpeed(); - if (PWMDcMotor::MotorValuesHaveChanged) { - PWMDcMotor::MotorValuesHaveChanged = false; -// position below speed values - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + (2 * TEXT_SIZE_11_HEIGHT); - sprintf_P(sStringBuffer, PSTR("comp. %2d %2d"), RobotCarMotorControl.leftCarMotor.SpeedCompensation, - RobotCarMotorControl.rightCarMotor.SpeedCompensation); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); +/* + * Checks MotorControlValuesHaveChanged and print values + */ +void printMotorValuesPeriodically() { + static long sLastPrintMillis; - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("min. %3d %3d"), RobotCarMotorControl.leftCarMotor.StartSpeed, - RobotCarMotorControl.rightCarMotor.StartSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); + uint32_t tMillis = millis(); + if (tMillis - sLastPrintMillis >= PRINT_MOTOR_INFO_PERIOD_MILLIS) { + sLastPrintMillis = tMillis; + /* + * Print speed value + */ + if (PWMDcMotor::MotorPWMHasChanged) { + PWMDcMotor::MotorPWMHasChanged = false; + // position below caption of speed slider + uint16_t tYPos = MOTOR_INFO_START_Y + TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("PWM %3d %3d"), RobotCarMotorControl.leftCarMotor.CurrentSpeedPWM, + RobotCarMotorControl.rightCarMotor.CurrentSpeedPWM); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); + } + /* + * Print motor control values + */ + if (PWMDcMotor::MotorControlValuesHaveChanged) { + PWMDcMotor::MotorControlValuesHaveChanged = false; + // position below speed values + uint16_t tYPos = MOTOR_INFO_START_Y + (2 * TEXT_SIZE_11); + sprintf_P(sStringBuffer, PSTR("lcomp %2d"), + RobotCarMotorControl.rightCarMotor.SpeedPWMCompensation + - RobotCarMotorControl.leftCarMotor.SpeedPWMCompensation); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); + + tYPos += TEXT_SIZE_11; + char tVCCString[5]; +#ifdef MONITOR_VIN_VOLTAGE + dtostrf( + (((float) RobotCarMotorControl.rightCarMotor.StartSpeedPWM * sVINVoltage) + - (FULL_BRIDGE_LOSS_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#else + dtostrf( + (RobotCarMotorControl.rightCarMotor.StartSpeedPWM * (FULL_BRIDGE_OUTPUT_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#endif + sprintf_P(sStringBuffer, PSTR("st.%3d %sV"), RobotCarMotorControl.rightCarMotor.StartSpeedPWM, tVCCString); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; +#ifdef MONITOR_VIN_VOLTAGE + dtostrf( + (((float) RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * sVINVoltage) + - (FULL_BRIDGE_LOSS_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#else + dtostrf( + (RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * (FULL_BRIDGE_OUTPUT_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#endif + sprintf_P(sStringBuffer, PSTR("drv%3d %sV"), RobotCarMotorControl.rightCarMotor.DriveSpeedPWM, tVCCString); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("max. %3d %3d"), RobotCarMotorControl.leftCarMotor.DriveSpeed, - RobotCarMotorControl.rightCarMotor.DriveSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); + } #ifdef USE_ENCODER_MOTOR_CONTROL if (sShowDebug && sCurrentPage == PAGE_TEST) { printMotorDebugValues(); } #endif +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + printMotorSensorValues(); +#endif + } +} + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +/* + * Display speed as slider values encoder values have precedence over IMU values + */ +void displayMotorSpeedSliderValues() { +# if defined(USE_ENCODER_MOTOR_CONTROL) + EncoderMotor *tMotorInfo = &RobotCarMotorControl.leftCarMotor; + BDSlider *tSliderPtr = &SliderSpeedLeft; + for (int i = 0; i < 2; ++i) { + tSliderPtr->setValueAndDrawBar(tMotorInfo->getSpeed()); + tMotorInfo = &RobotCarMotorControl.rightCarMotor; + tSliderPtr = &SliderSpeedRight; + } +# else + int tCarSpeedCmPerSecondFromIMU = abs(RobotCarMotorControl.CarSpeedCmPerSecondFromIMU); + SliderSpeedLeft.setValueAndDrawBar(tCarSpeedCmPerSecondFromIMU); + SliderSpeedRight.setValueAndDrawBar(tCarSpeedCmPerSecondFromIMU); +# endif +} + +/* + * Encoder speed has precedence over IMU speed + */ +void printMotorSensorValues() { + if (PWMDcMotor::SensorValuesHaveChanged) { + PWMDcMotor::SensorValuesHaveChanged = false; + +# if defined(USE_ENCODER_MOTOR_CONTROL) + /* + * Print encoder counts + */ + uint16_t tYPos; + if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { + tYPos = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; + } else { + tYPos = MOTOR_INFO_START_Y; + } + sprintf_P(sStringBuffer, PSTR("cnt.%4d%4d"), RobotCarMotorControl.leftCarMotor.EncoderCount, + RobotCarMotorControl.rightCarMotor.EncoderCount); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); +# endif + +# if defined(USE_MPU6050_IMU) + /* + * Print distance and rotation from IMU + */ + sprintf_P(sStringBuffer, PSTR("%5dcm%4d\xB0"), RobotCarMotorControl.IMUData.getDistanceCm(), + RobotCarMotorControl.CarTurnAngleHalfDegreesFromIMU / 2); + BlueDisplay1.drawText(MOTOR_INFO_START_X, MOTOR_INFO_START_Y, sStringBuffer); +# endif + + displayMotorSpeedSliderValues(); + } +} +#endif // defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + +#if defined(USE_MPU6050_IMU) +void printIMUOffsetValues() { + if (RobotCarMotorControl.IMUData.OffsetsHaveChanged) { + RobotCarMotorControl.IMUData.OffsetsHaveChanged = false; + sprintf_P(sStringBuffer, PSTR("off.%4d%4d"), RobotCarMotorControl.IMUData.AcceleratorForwardOffset, + RobotCarMotorControl.IMUData.GyroscopePanOffset); + BlueDisplay1.drawText(MOTOR_INFO_START_X, MOTOR_INFO_START_Y + (5 * TEXT_SIZE_11), sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, + COLOR16_WHITE); } } +#endif // defined(USE_MPU6050_IMU) #ifdef USE_ENCODER_MOTOR_CONTROL /* @@ -638,45 +729,41 @@ void printMotorDebugValues() { /* * Debug info */ - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + TEXT_SIZE_11_HEIGHT + (4 * TEXT_SIZE_11); -#ifdef SUPPORT_RAMP_UP - sprintf_P(sStringBuffer, PSTR("ramp1%3d %3d"), RobotCarMotorControl.leftCarMotor.DistanceCountAfterRampUp, - RobotCarMotorControl.rightCarMotor.DistanceCountAfterRampUp); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.DebugSpeedAtTargetCountReached, - RobotCarMotorControl.rightCarMotor.DebugSpeedAtTargetCountReached); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("dcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.DebugCount, - RobotCarMotorControl.rightCarMotor.DebugCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - tYPos += TEXT_SIZE_11; -#endif - sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug, - RobotCarMotorControl.rightCarMotor.Debug); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("tcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.LastTargetDistanceCount, - RobotCarMotorControl.rightCarMotor.LastTargetDistanceCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); -} +// if (EncoderMotor::EncoderRampUpDataHaveChanged) { +// EncoderMotor::EncoderRampUpDataHaveChanged = false; -void printMotorDistanceValues() { - uint16_t tYPos; - if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - tYPos = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; - } else { - tYPos = SPEED_SLIDER_SIZE / 2 + 25; - } - sprintf_P(sStringBuffer, PSTR("cnt.%4d%4d"), RobotCarMotorControl.leftCarMotor.EncoderCount, - RobotCarMotorControl.rightCarMotor.EncoderCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); +# if defined(USE_MPU6050_IMU) + uint16_t tYPos = MOTOR_INFO_START_Y + (6 * TEXT_SIZE_11); +# else + uint16_t tYPos = MOTOR_INFO_START_Y + (5 * TEXT_SIZE_11); +# endif + +# ifdef SUPPORT_RAMP_UP + sprintf_P(sStringBuffer, PSTR("ramp1%3d %3d"), RobotCarMotorControl.leftCarMotor.DistanceCountAfterRampUp, + RobotCarMotorControl.rightCarMotor.DistanceCountAfterRampUp); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.DebugSpeedAtTargetCountReached, + RobotCarMotorControl.rightCarMotor.DebugSpeedAtTargetCountReached); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; +# endif + + sprintf_P(sStringBuffer, PSTR("tcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.LastTargetDistanceMillimeter, + RobotCarMotorControl.rightCarMotor.LastTargetDistanceMillimeter); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + +# if !defined(USE_MPU6050_IMU) // no space if IMU data is displayed + tYPos += TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug, + RobotCarMotorControl.rightCarMotor.Debug); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); +# endif +// } } -#endif +#endif // USE_ENCODER_MOTOR_CONTROL void showUSDistance(unsigned int aCentimeter, bool aForceDraw) { // feedback as slider length diff --git a/src/RobotCarGui.h b/src/RobotCarGui.h index 24783c7..9e31eed 100644 --- a/src/RobotCarGui.h +++ b/src/RobotCarGui.h @@ -31,7 +31,7 @@ #define PATH_LENGTH_MAX 100 #define PRINT_VOLTAGE_PERIOD_MILLIS 2000 -extern uint32_t sMillisOfNextVCCInfo; +#define PRINT_MOTOR_INFO_PERIOD_MILLIS 200 // a string buffer for BD info output extern char sStringBuffer[128]; @@ -46,10 +46,14 @@ extern char sStringBuffer[128]; #define US_SLIDER_SIZE BUTTON_HEIGHT_4_LINE_3 // 128 #define LASER_SLIDER_SIZE BUTTON_HEIGHT_4_LINE_3 // 128 #define DISTANCE_SLIDER_SIZE (BUTTON_HEIGHT_4_LINE_3 - BUTTON_HEIGHT_8) // 104 +#define DISTANCE_SLIDER_SCALE_FACTOR 2 // Slider is virtually 2 times larger, values were divided by 2 #define US_DISTANCE_MAP_ORIGIN_X 200 #define US_DISTANCE_MAP_ORIGIN_Y 150 +#define MOTOR_INFO_START_X (BUTTON_WIDTH_6 + 4) +#define MOTOR_INFO_START_Y (SPEED_SLIDER_SIZE / 2 + 26) + #define PAGE_HOME 0 // Manual control page #define PAGE_AUTOMATIC_CONTROL 1 #define PAGE_BT_SENSOR_CONTROL 2 @@ -88,11 +92,11 @@ void loopAutonomousDrivePage(void); void stopAutonomousDrivePage(void); void handleAutomomousDriveRadioButtons(); -void doStartStopFollowerMode(BDButton * aTheTouchedButton, int16_t aValue); -void doStartStopAutomomousDrive(BDButton * aTheTouchedButton, int16_t aValue); -void doStartStopTestUser(BDButton * aTheTouchedButton, int16_t aValue); +void doStartStopFollowerMode(BDButton *aTheTouchedButton, int16_t aValue); +void doStartStopAutomomousDrive(BDButton *aTheTouchedButton, int16_t aValue); +void doStartStopTestUser(BDButton *aTheTouchedButton, int16_t aValue); -void doStartStopAutonomousForPathPage(BDButton * aTheTouchedButton, int16_t aValue); +void doStartStopAutonomousForPathPage(BDButton *aTheTouchedButton, int16_t aValue); void setStepMode(uint8_t aStepMode); // from BTSensorDrivePage @@ -103,7 +107,7 @@ void loopBTSensorDrivePage(void); void stopBTSensorDrivePage(void); extern uint8_t sSensorChangeCallCountForZeroAdjustment; -void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallbackInfo); +void doSensorChange(uint8_t aSensorType, struct SensorCallback *aSensorCallbackInfo); // from TestPage extern bool sShowDebug; @@ -120,8 +124,8 @@ extern BDButton TouchButtonMelody; extern bool sPlayMelody; #endif -extern void doHorizontalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue); -extern void doVerticalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue); +extern void doHorizontalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue); +extern void doVerticalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue); void initHomePage(void); void drawHomePage(void); @@ -136,7 +140,7 @@ extern uint8_t sCurrentPage; extern BDButton TouchButtonAutomaticDrivePage; extern BDButton TouchButtonReset; extern BDButton TouchButtonBack; -void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue); +void GUISwitchPages(BDButton *aTheTouchedButton, int16_t aValue); void startCurrentPage(); /* @@ -145,17 +149,20 @@ void startCurrentPage(); extern BDButton TouchButtonRobotCarStartStop; void setStartStopButtonValue(); void startStopRobotCar(bool aDoStart); -void doRobotCarStartStop(BDButton * aTheTochedButton, int16_t aDoStart); +void doStartStopRobotCar(BDButton *aTheTochedButton, int16_t aDoStart); +void doReset(BDButton *aTheTochedButton, int16_t aValue); extern BDButton TouchButtonDirection; -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) extern BDButton TouchButtonCalibrate; -void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue); +void doCalibrate(BDButton *aTheTouchedButton, int16_t aValue); #endif extern BDButton TouchButtonCompensationRight; extern BDButton TouchButtonCompensationLeft; +#ifdef SUPPORT_EEPROM_STORAGE extern BDButton TouchButtonCompensationStore; +#endif extern BDSlider SliderSpeed; extern uint16_t sLastSpeedSliderValue; @@ -177,8 +184,12 @@ extern BDSlider SliderPan; extern BDSlider SliderTilt; #endif -#ifdef USE_ENCODER_MOTOR_CONTROL -void displayVelocitySliderValues(); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +void displayMotorSpeedSliderValues(); +void printMotorSensorValues(); +#endif +#if defined(USE_MPU6050_IMU) +void printIMUOffsetValues(); #endif void drawCommonGui(void); @@ -189,18 +200,18 @@ void setupGUI(void); void loopGUI(void); void initRobotCarDisplay(void); -void readAndShowDistancePeriodically(uint16_t aPeriodMillis); +void readAndShowDistancePeriodically(); void rotate(int16_t aRotationDegrees, bool inPlace = true); void showDistance(int aCentimeter); -void printMotorSpeed(); -void printMotorValues(); +void printAndDisplayMotorSpeed(); +void printMotorValuesPeriodically(); #ifdef USE_ENCODER_MOTOR_CONTROL void printMotorDebugValues(); void printMotorDistanceValues(); #endif -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) void readAndPrintVin(); void readCheckAndPrintVinPeriodically(); #endif diff --git a/src/TestPage.cpp b/src/TestPage.cpp index bf57cc6..e2c590e 100644 --- a/src/TestPage.cpp +++ b/src/TestPage.cpp @@ -28,7 +28,9 @@ */ BDButton TouchButtonReset; +#ifdef SUPPORT_EEPROM_STORAGE BDButton TouchButtonGetAndStoreSpeed; +#endif BDButton TouchButtonDebug; @@ -46,18 +48,37 @@ BDButton TouchButton360Degree; bool sShowDebug = false; #pragma GCC diagnostic ignored "-Wunused-parameter" -void doDistance(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.startGoDistanceCentimeter(aValue, sRobotCarDirection); +void doDistance(BDButton *aTheTouchedButton, int16_t aValue) { + RobotCarMotorControl.startGoDistanceMillimeter(aValue, sRobotCarDirection); } -void doShowDebug(BDButton * aTheTouchedButton, int16_t aValue) { +void doShowDebug(BDButton *aTheTouchedButton, int16_t aValue) { sShowDebug = aValue; } -void doRotation(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.startRotateCar(aValue, sRobotCarDirection, true); +/* + * stop and reset motors + * reset IMU data and compute new offsets + */ +void doReset(BDButton *aTheTouchedButton, int16_t aValue) { + startStopRobotCar(false); + RobotCarMotorControl.resetControlValues(); + sLastSpeedSliderValue = 0; } +void doRotation(BDButton *aTheTouchedButton, int16_t aValue) { + if (aValue == 360) { + // use in place for 360 degree and change turn direction according to sRobotCarDirection + if (sRobotCarDirection != DIRECTION_FORWARD) { + aValue = -aValue; + } + RobotCarMotorControl.startRotate(aValue, TURN_IN_PLACE); + } else { + RobotCarMotorControl.startRotate(aValue, sRobotCarDirection); + } +} + +#ifdef SUPPORT_EEPROM_STORAGE /* * Callback handler for user speed input * Store user speed input as DriveSpeed @@ -69,64 +90,108 @@ void doStoreSpeed(float aValue) { RobotCarMotorControl.rightCarMotor.DriveSpeed = tValue; // use the same value here ! RobotCarMotorControl.leftCarMotor.DriveSpeed = tValue; - RobotCarMotorControl.writeMotorvaluesToEeprom(); + RobotCarMotorControl.writeMotorValuesToEeprom(); } printMotorValues(); } + /* * Request speed value as number from user */ void doGetSpeedAsNumber(BDButton * aTheTouchedButton, int16_t aValue) { BlueDisplay1.getNumberWithShortPrompt(&doStoreSpeed, "Drive speed [11-255]", sLastSpeedSliderValue); } +#endif + +//const struct ButtonInit ButtonReset PROGMEM { BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, +//COLOR16_BLUE, TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset }; +// +//const struct ButtonInit Button5cm PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 50, &doDistance }; +//const struct ButtonInit Button10cm PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 100, &doDistance }; +// +//const struct ButtonInit Button20cm PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 200, &doDistance }; +//const struct ButtonInit Button40cm PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 400, &doDistance }; +//const struct ButtonInit ButtonDebug PROGMEM { BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_RED, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doShowDebug }; +// +//const struct ButtonInit Button45DegreeLeft PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 45, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button45DegreeRight PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -45, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button360Degree PROGMEM { BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 360, &doRotation }; // \xB0 is degree character +// +//const struct ButtonInit Button90DegreeLeft PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 90, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button90DegreeRight PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -90, &doRotation }; // \xB0 is degree character /* - * stop and reset motors + * replacing parameter init with structure init INCREASES code size by 82 bytes */ -void doReset(BDButton * aTheTouchedButton, int16_t aValue) { - startStopRobotCar(false); - RobotCarMotorControl.resetControlValues(); -} - void initTestPage(void) { /* * Control buttons */ - TouchButtonReset.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, - COLOR_BLUE, F("Reset"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset); + COLOR16_BLUE, F("Reset"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonGetAndStoreSpeed.init(0, BUTTON_HEIGHT_4_LINE_4 - BUTTON_HEIGHT_6 - BUTTON_DEFAULT_SPACING_QUARTER, BUTTON_WIDTH_6, - BUTTON_HEIGHT_6, COLOR_BLUE, F("Set\nspeed"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doGetSpeedAsNumber); - + BUTTON_HEIGHT_6, COLOR16_BLUE, F("Set\nspeed"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doGetSpeedAsNumber); +#endif /* * Test buttons * Many calls requires 36 bytes code + sometimes 52 bytes to clean up the stack. */ - TouchButton5cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("5cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 5, &doDistance); - TouchButton10cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("10cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 10, &doDistance); - - TouchButton20cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("20cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 20, &doDistance); - TouchButton40cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("40cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 40, &doDistance); - TouchButtonDebug.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("dbg"), + TouchButton5cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("5cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 50, &doDistance); + TouchButton10cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("10cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 100, &doDistance); + + TouchButton20cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("20cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 200, &doDistance); + TouchButton40cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("40cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 400, &doDistance); + TouchButtonDebug.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_RED, F("dbg"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doShowDebug); - TouchButton45DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, +// TouchButtonReset.init(&ButtonReset, F("Reset")); +// TouchButton5cm.init(&Button5cm, F("5cm")); +// TouchButton10cm.init(&Button10cm, F("10cm")); +// +// TouchButton20cm.init(&Button20cm, F("20cm")); +// TouchButton40cm.init(&Button40cm, F("40cm")); +// TouchButtonDebug.init(&ButtonDebug, F("dbg")); + + TouchButton45DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("45\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 45, &doRotation); // \xB0 is degree character - TouchButton45DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton45DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("-45\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -45, &doRotation); // \xB0 is degree character - TouchButton360Degree.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton360Degree.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("360\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 360, &doRotation); // \xB0 is degree character - TouchButton90DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton90DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("90\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 90, &doRotation); // \xB0 is degree character - TouchButton90DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton90DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("-90\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -90, &doRotation); // \xB0 is degree character + +// TouchButton45DegreeLeft.init(&Button45DegreeLeft, F("45\xB0")); // \xB0 is degree character +// TouchButton45DegreeRight.init(&Button45DegreeRight, F("-45\xB0")); // \xB0 is degree character +// TouchButton360Degree.init(&Button360Degree, F("360\xB0")); // \xB0 is degree character +// +// TouchButton90DegreeLeft.init(&Button90DegreeLeft, F("90\xB0")); // \xB0 is degree character +// TouchButton90DegreeRight.init(&Button90DegreeRight, F("-90\xB0")); // \xB0 is degree character } void drawTestPage(void) { @@ -139,7 +204,7 @@ void drawTestPage(void) { TouchButton5cm.drawButton(); TouchButton10cm.drawButton(); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.drawButton(); #endif @@ -153,7 +218,9 @@ void drawTestPage(void) { TouchButtonCompensationLeft.drawButton(); TouchButtonCompensationRight.drawButton(); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonCompensationStore.drawButton(); +#endif TouchButton90DegreeLeft.drawButton(); TouchButton90DegreeRight.drawButton(); @@ -164,7 +231,9 @@ void drawTestPage(void) { SliderSpeedRight.drawSlider(); SliderSpeedLeft.drawSlider(); #endif +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonGetAndStoreSpeed.drawButton(); +#endif SliderUSPosition.setValueAndDrawBar(sLastServoAngleInDegrees); SliderUSPosition.drawSlider(); @@ -174,7 +243,7 @@ void drawTestPage(void) { SliderIRDistance.drawSlider(); # endif - PWMDcMotor::MotorValuesHaveChanged = true; // trigger drawing of values + PWMDcMotor::MotorControlValuesHaveChanged = true; // trigger drawing of values } /* diff --git a/src/digitalWriteFast.h b/src/digitalWriteFast.h index 3bbfc97..6267ec1 100644 --- a/src/digitalWriteFast.h +++ b/src/digitalWriteFast.h @@ -1,12 +1,14 @@ /* - Optimized digital functions for AVR microcontrollers - by Watterott electronic (www.watterott.com) - based on http://code.google.com/p/digitalwritefast + Optimized digital functions for AVR microcontrollers + by Watterott electronic (www.watterott.com) + based on http://code.google.com/p/digitalwritefast */ #ifndef __digitalWriteFast_h_ #define __digitalWriteFast_h_ 1 +//#define SANGUINO_PINOUT //define for Sanguino pinout + // general macros/defines #ifndef BIT_READ # define BIT_READ(value, bit) ((value) & (1UL << (bit))) @@ -22,7 +24,7 @@ #endif #ifndef SWAP -#define SWAP(x,y) do{ (x)=(x)^(y); (y)=(x)^(y); (x)=(x)^(y); }while(0) +# define SWAP(x,y) do{ (x)=(x)^(y); (y)=(x)^(y); (x)=(x)^(y); }while(0) #endif #ifndef DEC @@ -38,8 +40,9 @@ # define BIN (2) #endif + // workarounds for ARM microcontrollers -#if (!defined(__AVR__) && !defined(ESP8266) || \ +#if (!defined(__AVR__) || \ defined(ARDUINO_ARCH_SAM) || \ defined(ARDUINO_ARCH_SAMD)) @@ -96,6 +99,7 @@ #endif + // digital functions //#ifndef digitalPinToPortReg #define SPI_SW_SS_PIN (10) //SS on Uno (for software SPI) @@ -103,6 +107,7 @@ #define SPI_SW_MISO_PIN (12) //MISO on Uno (for software SPI) #define SPI_SW_SCK_PIN (13) //SCK on Uno (for software SPI) + // --- Arduino Due and SAM3X8E based boards --- #if (defined(ARDUINO_SAM_DUE) || \ defined(__SAM3X8E__)) @@ -210,21 +215,21 @@ // --- Arduino MightyCore standard pinout --- -#elif defined(__AVR_ATmega1284P__) || \ - defined(__AVR_ATmega1284P__) || \ - defined(__AVR_ATmega644P__) || \ - defined(__AVR_ATmega644__) || \ - defined(__AVR_ATmega324PB__) || \ - defined(__AVR_ATmega324PA__) || \ - defined(__AVR_ATmega324P__) || \ - defined(__AVR_ATmega324A__) || \ - defined(__AVR_ATmega164P__) || \ - defined(__AVR_ATmega164A__) || \ - defined(__AVR_ATmega32__) || \ - defined(__AVR_ATmega16__) || \ - defined(__AVR_ATmega8535__) && \ - !defined(BOBUINO_PINOUT) && \ - !defined(SANGUINO_PINOUT) +#elif (defined(__AVR_ATmega1284P__) || \ + defined(__AVR_ATmega1284__) || \ + defined(__AVR_ATmega644P__) || \ + defined(__AVR_ATmega644A__) || \ + defined(__AVR_ATmega644__) || \ + defined(__AVR_ATmega324PB__) || \ + defined(__AVR_ATmega324PA__) || \ + defined(__AVR_ATmega324P__) || \ + defined(__AVR_ATmega324A__) || \ + defined(__AVR_ATmega164P__) || \ + defined(__AVR_ATmega164A__) || \ + defined(__AVR_ATmega32__) || \ + defined(__AVR_ATmega16__) || \ + defined(__AVR_ATmega8535__)) && \ + !defined(BOBUINO_PINOUT) #define UART_RX_PIN (8) //PD0 #define UART_TX_PIN (9) //PD1 @@ -241,22 +246,33 @@ #define __digitalPinToPortReg(P) \ (((P) >= 0 && (P) <= 7) ? &PORTB : (((P) >= 8 && (P) <= 15) ? &PORTD : (((P) >= 16 && (P) <= 23) ? &PORTC : (((P) >= 24 && (P) <= 31) ? &PORTA : &PORTE)))) #define __digitalPinToDDRReg(P) \ -(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 8 && (P) <= 15) ? &DDRC : (((P) >= 24 && (P) <= 31) ? &DDRA : &DDRE)))) +(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 16 && (P) <= 23) ? &DDRC : (((P) >= 24 && (P) <= 31) ? &DDRA : &DDRE)))) #define __digitalPinToPINReg(P) \ -(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 8 && (P) <= 15) ? &PINC : (((P) >= 24 && (P) <= 31) ? &PINA : &PINE)))) +(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 16 && (P) <= 23) ? &PINC : (((P) >= 24 && (P) <= 31) ? &PINA : &PINE)))) +# if defined(SANGUINO_PINOUT) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (((P) >= 16 && (P) <= 23) ? (7 - ((P) - 24)) : (P) - 32)))) +# else //MightyCore Pinout #define __digitalPinToBit(P) \ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (((P) >= 16 && (P) <= 23) ? (P) - 24 : (P) - 32)))) +# endif #else #define __digitalPinToPortReg(P) \ (((P) >= 0 && (P) <= 7) ? &PORTB : (((P) >= 8 && (P) <= 15) ? &PORTD : (((P) >= 16 && (P) <= 23) ? &PORTC : &PORTA))) #define __digitalPinToDDRReg(P) \ -(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 8 && (P) <= 15) ? &DDRC : &DDRA))) +(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 16 && (P) <= 23) ? &DDRC : &DDRA))) #define __digitalPinToPINReg(P) \ -(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 8 && (P) <= 15) ? &PINC : &PINA))) +(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 16 && (P) <= 23) ? &PINC : &PINA))) +# if defined(SANGUINO_PINOUT) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (7 - ((P) - 24))))) +# else //MightyCore Pinout #define __digitalPinToBit(P) \ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (P) - 24))) +# endif #endif + // --- Arduino Leonardo and ATmega16U4/32U4 based boards --- #elif (defined(ARDUINO_AVR_LEONARDO) || \ defined(__AVR_ATmega16U4__) || \ @@ -330,7 +346,9 @@ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) #endif -#elif defined(__AVR_ATmega4809__) // Uno WiFi Rev 2, Nano Every + +// --- Arduino Uno WiFi Rev 2, Nano Every --- +#elif defined(__AVR_ATmega4809__) #define UART_RX_PIN (0) //PB0 #define UART_TX_PIN (1) //PB1 @@ -353,48 +371,56 @@ (((P) == 2 || (P) == 9 || (P) == 11 || (P) == 17) ? 0 : ((P) == 7 || (P) == 10 || (P) == 12 || (P) == 16) ? 1 : ((P) == 5 || (P) == 13 || (P) == 15 || (P) == 18) ? 2 : ((P) == 9 || (P) == 14 || (P) == 19) ? 3 : ((P) == 6 || (P) == 20) ? 4 : ((P) == 3 || (P) == 21) ? 5 : 6 ) +// TinyCore +// https://raw.githubusercontent.com/xukangmin/TinyCore/master/avr/package/package_tinycore_index.json +// https://docs.tinycore.dev/en/latest/ +#elif defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) +#define __digitalPinToPortReg(P) ((P) <= 5 ? &VPORTB.OUT : ((P) <= 9 ? &VPORTC.OUT : ((P) <= 16 ? &VPORTA.OUT : ((P) <= 18 ? &VPORTB.OUT : &VPORTC.OUT)))) +#define __digitalPinToDDRReg(P) ((P) <= 5 ? &VPORTB.DIR : ((P) <= 9 ? &VPORTC.DIR : ((P) <= 16 ? &VPORTA.DIR : ((P) <= 18 ? &VPORTB.DIR : &VPORTC.DIR)))) +#define __digitalPinToPINReg(P) ((P) <= 5 ? &VPORTB.IN : ((P) <= 9 ? &VPORTC.IN : ((P) <= 16 ? &VPORTA.IN : ((P) <= 18 ? &VPORTB.IN : &VPORTC.IN)))) +#define __digitalPinToBit(P) ( (P) <= 3 ? (3 - P) : ((P) <= 5 ? (P) : ((P) <= 9 ? (P - 6) : ((P) <= 16 ? ((P) - 9) : ((P) <= 18 ? ((P) - 11) : ((P) - 15))))) ) + + // --- ATtinyX5 --- #elif defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) // we have only PORTB #define __digitalPinToPortReg(P) (&PORTB) #define __digitalPinToDDRReg(P) (&DDRB) #define __digitalPinToPINReg(P) (&PINB) -#define __digitalPinToBit(P) (( (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) +#define __digitalPinToBit(P) (((P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) // --- ATtiny88 --- #elif defined(__AVR_ATtiny88__) -# if defined(ARDUINO_AVR_DIGISPARKPRO) +# if defined(ARDUINO_AVR_DIGISPARKPRO) #define __digitalPinToPortReg(P) ((P) <= 7 ? &PORTD : ((P) <= 14 ? &PORTB : ((P) <= 18 ? &PORTA : &PORTC))) #define __digitalPinToDDRReg(P) ((P) <= 7 ? &DDRD : ((P) <= 14 ? &DDRB : ((P) <= 18 ? &DDRA : &DDRC))) #define __digitalPinToPINReg(P) ((P) <= 7 ? &PIND : ((P) <= 14 ? &PINB : ((P) <= 18 ? &PINA : &PINC))) #define __digitalPinToBit(P) ( (P) <= 7 ? (P) : ((P) <= 13 ? ((P) - 8) : ((P) == 14 ? 7 : ((P) <= 16 ? ((P) - 14) : ((P) <= 18 ? ((P) - 17) : ((P) == 25 ? 7 : ((P) - 19)))))) ) -# else +# else #define __digitalPinToPortReg(P) ((P) <= 7 ? &PORTD : ((P) <= 15 ? &PORTB : ((P) <= 22 ? &PORTC : ((P) <= 26 ? &PORTA : &PORTC)))) #define __digitalPinToDDRReg(P) ((P) <= 7 ? &DDRD : ((P) <= 15 ? &DDRB : ((P) <= 22 ? &DDRC : ((P) <= 26 ? &DDRA : &DDRC)))) #define __digitalPinToPINReg(P) ((P) <= 7 ? &PIND : ((P) <= 15 ? &PINB : ((P) <= 22 ? &PINC : ((P) <= 26 ? &PINA : &PINC)))) #define __digitalPinToBit(P) ((P) <= 15 ? ((P) & 0x7) : ((P) == 16 ? (7) : ((P) <= 22 ? ((P) - 17) : ((P) == 27 ? (6) : ((P) - 23))))) -# endif +# endif // --- ATtinyX4 + ATtinyX7 --- #elif defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) || defined(__AVR_ATtiny87__) || defined(__AVR_ATtiny167__) -# if defined(ARDUINO_AVR_DIGISPARKPRO) -/// Strange enumeration of pins on Digispark board and core library +# if defined(ARDUINO_AVR_DIGISPARKPRO) +// Strange enumeration of pins on Digispark board and core library #define __digitalPinToPortReg(P) (((P) <= 4) ? &PORTB : &PORTA) #define __digitalPinToDDRReg(P) (((P) <= 4) ? &DDRB : &DDRA) #define __digitalPinToPINReg(P) (((P) <= 4) ? &PINB : &PINA) #define __digitalPinToBit(P) (((P) <= 2) ? (P) : (((P) == 3) ? 6 : (((P) == 4) ? 3 : (((P) == 5) ? 7 : (P) - 6 )))) - -# else -// ATtinyX4: PORTA for 0 to 7, PORTB for 8 to 11 -// ATtinyX7: PORTA for 0 to 7, PORTB for 8 to 15 +# else +// ATtinyX4: PORTA for 0 to 7, PORTB for 8 to 11 +// ATtinyX7: PORTA for 0 to 7, PORTB for 8 to 15 #define __digitalPinToPortReg(P) (((P) <= 7) ? &PORTA : &PORTB) #define __digitalPinToDDRReg(P) (((P) <= 7) ? &DDRA : &DDRB) #define __digitalPinToPINReg(P) (((P) <= 7) ? &PINA : &PINB) #define __digitalPinToBit(P) (((P) <= 7) ? (P) : (P) - 8 ) -# endif - +# endif // --- Other --- #else @@ -411,6 +437,7 @@ #endif //#endif //#ifndef digitalPinToPortReg + #ifndef digitalWriteFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalWriteFast(P, V) \ @@ -424,6 +451,7 @@ if (__builtin_constant_p(P)) { \ #endif #endif + #ifndef pinModeFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define pinModeFast(P, V) \ @@ -442,6 +470,7 @@ if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ #endif #endif + #ifndef digitalReadFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalReadFast(P) ( (int) __digitalReadFast((P)) ) @@ -454,6 +483,7 @@ if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ #endif #endif + #ifndef digitalToggleFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalToggleFast(P) \ @@ -467,4 +497,5 @@ if (__builtin_constant_p(P)) { \ #endif #endif + #endif //__digitalWriteFast_h_