diff --git a/.github/workflows/LibraryBuild.yml b/.github/workflows/LibraryBuild.yml index dae1285..abb8903 100644 --- a/.github/workflows/LibraryBuild.yml +++ b/.github/workflows/LibraryBuild.yml @@ -40,16 +40,6 @@ jobs: ############################################################################################################# arduino-boards-fqbn: - arduino:avr:uno -# - arduino:avr:leonardo -# - arduino:avr:mega -# - arduino:megaavr:nona4809:mode=off -# - arduino:sam:arduino_due_x -# - arduino:samd:arduino_zero_native -# - esp8266:esp8266:huzzah:eesz=4M3M,xtal=80 -# - esp32:esp32:featheresp32:FlashFreq=80 -# - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 -# - stm32duino:STM32F1:genericSTM32F103C -# - SparkFun:apollo3:amap3nano # Specify parameters for each board. # With sketches-exclude you may exclude specific examples for a board. Use a comma separated list. @@ -64,67 +54,6 @@ jobs: -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DENABLE_MOTOR_LIST_FUNCTIONS -# - arduino-boards-fqbn: arduino:avr:leonardo -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - -# - arduino-boards-fqbn: arduino:avr:mega -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: arduino:megaavr:nona4809:mode=off -# sketches-exclude: SpeedTest,EndPositionsTest,QuadrupedControl,RobotArmControl # No getVCCVoltageMillivolt(), no EasyButton -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: arduino:sam:arduino_due_x -# sketches-exclude: QuadrupedControl,RobotArmControl # QuadrupedControl RobotArmControl because of missing EEprom -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: arduino:samd:arduino_zero_native -# sketches-exclude: QuadrupedControl,RobotArmControl -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: esp8266:esp8266:huzzah:eesz=4M3M,xtal=80 -# platform-url: https://arduino.esp8266.com/stable/package_esp8266com_index.json -# sketches-exclude: QuadrupedControl,RobotArmControl,SpeedTest # SpeedTest because of only one analog input -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: esp32:esp32:featheresp32:FlashFreq=80 -# platform-url: https://dl.espressif.com/dl/package_esp32_index.json -# sketches-exclude: QuadrupedControl,RobotArmControl # Comma separated list of (unique substrings of) example names to exclude in build -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # STM version -# platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json -# sketches-exclude: QuadrupedControl,RobotArmControl -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: stm32duino:STM32F1:genericSTM32F103C # Roger Clark version -# platform-url: http://dan.drown.org/stm32duino/package_STM32duino_index.json -# sketches-exclude: QuadrupedControl,RobotArmControl -# build-properties: -# RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN - - -# - arduino-boards-fqbn: SparkFun:apollo3:amap3nano -# platform-url: https://raw.githubusercontent.com/sparkfun/Arduino_Boards/master/IDE_Board_Manager/package_sparkfun_index.json -# # 4/2020 For PCA9685_Expander, Wire cannot be found in cli, it works in the regular IDE. SpeedTest and EndPositionsTest give strange BFD assertions -# sketches-exclude: QuadrupedControl,RobotArmControl,PCA9685_Expander,PCA9685_ExpanderAndServo,PCA9685_ExpanderFor32Servos,SpeedTest,EndPositionsTest - - # Do not cancel all jobs / architectures if one job fails fail-fast: false diff --git a/README.md b/README.md index 4485665..c63fb64 100644 --- a/README.md +++ b/README.md @@ -4,11 +4,13 @@ Available as Arduino library "PWMMotorControl" ### [Version 1.0.0](https://github.com/ArminJo/PWMMotorControl/releases) [![License: GPL v3](https://img.shields.io/badge/License-GPLv3-blue.svg)](https://www.gnu.org/licenses/gpl-3.0) +[![Installation instructions](https://www.ardu-badge.com/badge/PWMMotorControl.svg?)](https://www.ardu-badge.com/PWMMotorControl) +[![Commits since latest](https://img.shields.io/github/commits-since/ArminJo/PWMMotorControl/latest)](https://github.com/ArminJo/PWMMotorControl/commits/master) [![Build Status](https://github.com/ArminJo/PWMMotorControl/workflows/LibraryBuild/badge.svg)](https://github.com/ArminJo/PWMMotorControl/actions) ![Hit Counter](https://visitor-badge.laobi.icu/badge?page_id=ArminJo_PWMMotorControl) - The PWMDcMotor.cpp controls **brushed DC motors** by PWM using standard full bridge IC's like **L298**, **TB6612** (new low loss dual full bridge IC), or **Adafruit_MotorShield** (using PCA9685 -> 2 x TB6612). -- The EncoderMotor.cpp.cpp controls a DC motor with attached encoder disc and fork light barrier to enable **driving a specified distance**. +- The EncoderMotor.cpp.cpp controls a DC motor with attached encoder disc and slot-type photo interrupters to enable **driving a specified distance**. - The CarMotorControl.cpp controls **2 motors simultaneously** like it is required for most **Robot Cars**. Basic commands are: @@ -30,8 +32,8 @@ Some options which are enabed by default can be disabled by defining a *inhibit* | Macro | Default | File | Description | |-|-|-|-| -| `USE_ENCODER_MOTOR_CONTROL` | disabled | PWMDCMotor.h | Use fork light barrier and an attached encoder disc to enable motor distance and speed sensing for closed loop control. | -| `USE_ADAFRUIT_MOTOR_SHIELD` | disabled | PWMDcMotor.h | 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 bride.
For full bride, analogWrite the millis() timer0 is used since we use pin 5 & 6. | +| `USE_ENCODER_MOTOR_CONTROL` | disabled | PWMDCMotor.h | Use slot-type photo interrupter and an attached encoder disc to enable motor distance and speed sensing for closed loop control. | +| `USE_ADAFRUIT_MOTOR_SHIELD` | disabled | PWMDcMotor.h | 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.
For full bridge, analogWrite the millis() timer0 is used since we use pin 5 & 6. | | `USE_OWN_LIBRARY_FOR_`
`ADAFRUIT_MOTOR_SHIELD` | enabled | PWMDcMotor.h | Saves around 694 bytes program memory.
Disable macro=`USE_STANDARD_LIBRARY_`
`FOR_ADAFRUIT_MOTOR_SHIELD` | | `SUPPORT_RAMP_UP` | enabled | PWMDcMotor.h | Saves around 300 bytes program memory.
Disable macro=`DO_NOT_SUPPORT_RAMP_UP` | @@ -50,7 +52,7 @@ These values are used by functions and the first 2 can be overwritten by set* fu |-|-|-|-| | `DEFAULT_START_SPEED` | 45/150 for 7.4/6.0 volt supply | PWMDCMotor.h | START_SPEED is the speed PWM value at which car starts to move. | | `DEFAULT_DRIVE_SPEED` | 80/255(max speed) for 7.4/6.0 volt supply | PWMDCMotor.h | The speed PWM value for going fixed distance. | -| `DEFAULT_DISTANCE_TO_TIME_FACTOR` | 135/300 for 7.4/6.0 volt supply | PWMDCMotor.h | The factor used to convert distance in 5mm steps to motor on time in milliseconds using the formula:
`computedMillisOf`
`MotorStopForDistance = 150 + (10 * ((aDistanceCount * DistanceToTimeFactor) / DriveSpeed))` | +| `DEFAULT_DISTANCE_TO_TIME_FACTOR` | 135/300 for 7.4/6.0 volt supply | PWMDCMotor.h | The factor used to convert distance in 5mm steps to motor on time in milliseconds using the formula:
`computedMillisOf`
`MotorStopForDistance = 150 + (10 * ((aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed))` | | `RAMP_UP_UPDATE_INTERVAL_MILLIS` | 16 | PWMDCMotor.h | The smaller the value the steeper the ramp. | | `RAMP_UP_UPDATE_INTERVAL_STEPS` | 16 | PWMDCMotor.h | Results in a ramp up time of 16 steps * 16 millis = 256 milliseconds. | @@ -79,8 +81,8 @@ To customize the RobotCar example to cover different extensions, there are some ![2 wheel car](https://github.com/ArminJo/Arduino-RobotCar/blob/master/pictures/2WheelDriveCar.jpg) 4 wheel car, like 2 WD car before, but with servo mounted head up. ![4 wheel car](https://github.com/ArminJo/Arduino-RobotCar/blob/master/pictures/4WheelDriveCar.jpg) -Encoder fork sensor -![Encoder fork sensor](https://github.com/ArminJo/Arduino-RobotCar/blob/master/pictures/ForkSensor.jpg) +Encoder slot-type photo interrupter sensor +![Encoder slot-type photo interrupter sensor](https://github.com/ArminJo/Arduino-RobotCar/blob/master/pictures/ForkSensor.jpg) Servo mounted head down ![Servo mounting](https://github.com/ArminJo/Arduino-RobotCar/blob/master/pictures/ServoAtTopBack.jpg) VIN sensing @@ -100,6 +102,9 @@ Automatic control page with detected wall at right - The tiny black bar is the rotation chosen by doCollisionDetection() function. # Revision History +### Version 1.1.0 +- Added and renamed functions. + ### Version 1.0.0 -Initial Arduino library version +Initial Arduino library version. diff --git a/examples/RobotCarBasic/RobotCar.h b/examples/RobotCarBasic/RobotCar.h deleted file mode 100644 index 1c2ed90..0000000 --- a/examples/RobotCarBasic/RobotCar.h +++ /dev/null @@ -1,214 +0,0 @@ -/* - * RobotCarMotorControl.h - * - * Created on: 29.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/Arduino-RobotCar. - * - * 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 SRC_ROBOTCAR_H_ -#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 CAR_HAS_IR_DISTANCE_SENSOR - -//#define CAR_HAS_TOF_DISTANCE_SENSOR - -//#define CAR_HAS_CAMERA - -//#define CAR_HAS_LASER - -/* - * For pan tilt we have 2 servos in total - */ -//#define CAR_HAS_PAN_SERVO -//#define CAR_HAS_TILT_SERVO -// -/* - * Plays melody after initial timeout has reached - * Enables the Play Melody button - */ -// #define ENABLE_RTTTL -// -#include "CarMotorControl.h" -extern CarMotorControl RobotCarMotorControl; - -/* - * Pin usage - * First the function of the nano board variant. For this variant the PWM is generated with analogWrite(). - */ -/* - * PIN I/O Function - * 2 I Right motor encoder interrupt input - * 3 I Left motor encoder interrupt input - * 4 O Left motor fwd / NC for UNO board - * 5 O Left motor PWM / NC for UNO board - * 6 O Right motor PWM / NC for UNO board - * 7 O Left motor back / NC for UNO board - * 8 O Right motor fwd / NC for UNO board - * 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 Right motor back / NC 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 - * 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 - * A7 O Camera supply control - */ - -#if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) // enable it in PWMDCMotor.h -/* - * Pins for direct motor control with PWM and full bridge - * Pins 9 + 10 are reserved for Servo - * 2 + 3 are reserved for encoder input - */ -#define PIN_LEFT_MOTOR_FORWARD 12 // Pin 9 is already reserved for distance servo -#define PIN_LEFT_MOTOR_BACKWARD 8 -#define PIN_LEFT_MOTOR_PWM 6 // Must be PWM capable - -#define PIN_RIGHT_MOTOR_FORWARD 4 -#define PIN_RIGHT_MOTOR_BACKWARD 7 -#define PIN_RIGHT_MOTOR_PWM 5 // Must be PWM capable -#endif - - -/* - * Servo pins - */ -#define PIN_DISTANCE_SERVO 9 // Servo Nr. 2 on Adafruit Motor Shield -#ifdef CAR_HAS_PAN_SERVO -#define PIN_PAN_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield -#endif -#ifdef CAR_HAS_TILT_SERVO -#define PIN_TILT_SERVO 11 -#endif - -#if defined(MONITOR_LIPO_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 -#endif - -/* - * Pins for US HC-SR04 distance sensor - */ -#define PIN_TRIGGER_OUT A0 // Connections on the Arduino Sensor Shield -#ifdef USE_US_SENSOR_1_PIN_MODE -#define PIN_IR_DISTANCE_SENSOR A1 // Otherwise available as US echo pin -#else -#define PIN_ECHO_IN A1 // used by Sharp IR distance sensor -#endif - -#ifdef CAR_HAS_LASER -#define PIN_LASER_OUT LED_BUILTIN -#endif - -/* - * Different pin layout for UNO with Adafruit motor shield and Nano (Nano hash full bridge) boards - */ -#ifdef USE_LAYOUT_FOR_NANO -/* - * Nano Layout - */ -# ifdef USE_ADAFRUIT_MOTOR_SHIELD -#error "Adafruit motor shield makes no sense for a Nano board!" -# endif -# ifdef CAR_HAS_CAMERA -#define PIN_CAMERA_SUPPLY_CONTROL A7 // Not available on UNO board -# endif -#define PIN_SPEAKER A6 // Not available on UNO board - -#else -/* - * UNO Layout - */ -# ifdef CAR_HAS_CAMERA -#define PIN_CAMERA_SUPPLY_CONTROL 4 -# endif -#define PIN_SPEAKER 11 -#endif - -/************************** - * End of pin definitions - **************************/ - -/* - * Timeouts for demo mode and inactivity remainder - */ -#define TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS 240000L // move Servo after 4 Minutes of inactivity -#define TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS 10000 // Start demo mode 10 seconds after boot up - -//#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 500 -#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 100 - -/* - * Servo timing correction. - * Values are for my SG90 servo. Servo is mounted head down, so values must be swapped! - */ -#define DISTANCE_SERVO_MIN_PULSE_WIDTH (MIN_PULSE_WIDTH - 40) // Value for 180 degree -#define DISTANCE_SERVO_MAX_PULSE_WIDTH (MAX_PULSE_WIDTH - 40) // Value for 0 degree, since servo is mounted head down. -#ifdef CAR_HAS_PAN_SERVO -extern Servo PanServo; -#endif -#ifdef CAR_HAS_TILT_SERVO -#define TILT_SERVO_MIN_VALUE 7 // since lower values will make an insane sound at my pan tilt device -extern Servo TiltServo; -#endif - -#define DISTANCE_TIMEOUT_CM 100 // do not measure and process distances greater than 100 cm - -/************************************************************************************ - * Definitions and declarations only used for GUI in RobotCarBlueDisplay.cpp example - ************************************************************************************/ -#define DISTANCE_TIMEOUT_COLOR COLOR_CYAN -#define DISTANCE_DISPLAY_PERIOD_MILLIS 500 - -#define MINIMUM_DISTANCE_TO_SIDE 21 -#define MINIMUM_DISTANCE_TO_FRONT 35 - -#if defined(MONITOR_LIPO_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 -#else -#endif -#define VOLTAGE_TOO_LOW_DELAY_ONLINE 3000 // display VIN every 500 ms for 4 seconds -#define VOLTAGE_TOO_LOW_DELAY_OFFLINE 1000 // wait for 1 seconds after double beep - -void readVINVoltage(); -#endif - -void resetServos(); -int doUserCollisionDetection(); - -#endif /* SRC_ROBOTCAR_H_ */ - -#pragma once diff --git a/examples/RobotCarBasic/RobotCarBasic.ino b/examples/RobotCarBasic/RobotCarBasic.ino index 30418b5..34a9bfc 100644 --- a/examples/RobotCarBasic/RobotCarBasic.ino +++ b/examples/RobotCarBasic/RobotCarBasic.ino @@ -7,6 +7,7 @@ * armin.joachimsmeyer@gmail.com * * This file is part of Arduino-RobotCar https://github.com/ArminJo/Arduino-RobotCar. + * 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 diff --git a/examples/RobotCarBlueDisplay/AutonomousDrive.cpp b/examples/RobotCarBlueDisplay/AutonomousDrive.cpp index 6c858a2..0a8a028 100644 --- a/examples/RobotCarBlueDisplay/AutonomousDrive.cpp +++ b/examples/RobotCarBlueDisplay/AutonomousDrive.cpp @@ -41,7 +41,7 @@ uint8_t sScanMode = SCAN_MODE_MINIMUM; // one of SCAN_MODE_MINIMUM, SCAN_MODE_MA bool sDoSlowScan = false; bool sRuningAutonomousDrive = false; // = (sDriveMode == MODE_AUTONOMOUS_DRIVE_BUILTIN || sDriveMode == MODE_AUTONOMOUS_DRIVE_USER || sDriveMode == MODE_FOLLOWER) is modified by buttons on this page -bool sSearchFollowerTarget = true; +bool sFollowerTargetFound = false; // One time flag to start with scanning for target. uint8_t sTurnMode = TURN_IN_PLACE; @@ -78,7 +78,7 @@ void startStopAutomomousDrive(bool aDoStart, uint8_t aDriveMode) { setStepMode(MODE_SINGLE_STEP); } else if (aDriveMode == MODE_FOLLOWER) { DistanceServoWriteAndDelay(90); // reset Servo - sSearchFollowerTarget = true; + sFollowerTargetFound = false; // Show distance sliders SliderUSDistance.drawSlider(); # if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR) @@ -108,7 +108,7 @@ void startStopAutomomousDrive(bool aDoStart, uint8_t aDriveMode) { int postProcessAndCollisionDetection() { doWallDetection(); - postProcessDistances(); + postProcessDistances(sCentimeterPerScanTimesTwo); int tNextDegreesToTurn; if (sDriveMode == MODE_AUTONOMOUS_DRIVE_BUILTIN) { tNextDegreesToTurn = doBuiltInCollisionDetection(); @@ -164,7 +164,7 @@ void driveAutonomousOneStep() { * Continuous mode, start car or let it run */ if (tCarIsStopped) { - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD, &loopGUI); + RobotCarMotorControl.startRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD, &loopGUI); } } @@ -243,250 +243,10 @@ void driveAutonomousOneStep() { } } -/* - * This documentation assumes 20 degrees stepping. - * By changing DEGREES_PER_STEP it can easily adopted to other degrees values. - * - * Assume the value of 20 and 40 degrees are distances to a wall. - * @return the clipped (aClipValue) distance to the wall of the vector at 0 degree. - * - * @param [out] aDegreeOfEndpointConnectingLine: The angle of the line from endpoint 0 degrees to given endpoints - * 0 means x values of given endpoints are the same >= wall is parallel / rectangular to the vector at 0 degree - * Positive means wall is more ore less in front, to avoid we must turn positive angle - * 90 means y values are the same => wall is in front - * Negative means we are heading away from wall - */ -uint8_t computeNeigbourValue(uint8_t aDegreesPerStepValue, uint8_t a2DegreesPerStepValue, uint8_t aClipValue, - int8_t * aDegreeOfEndpointConnectingLine) { - - /* - * Name of the variables are for DEGREES_PER_STEP = 20 only for better understanding. - * The computation of course works for other values of DEGREES_PER_STEP! - */ - float tYat20degrees = sin((PI / 180) * DEGREES_PER_STEP) * aDegreesPerStepValue; // e.g. 20 Degree -// assume current = 40 Degree - float tYat40degrees = sin((PI / 180) * (DEGREES_PER_STEP * 2)) * a2DegreesPerStepValue; // e.g. 40 Degree - -// char tStringBuffer[] = "A=_______ L=_______"; -// dtostrf(tYat40degrees, 7, 2, &tStringBuffer[2]); -// tStringBuffer[9] = ' '; -// dtostrf(tYat20degrees, 7, 2, &tStringBuffer[12]); -// BlueDisplay1.debugMessage(tStringBuffer); - - uint8_t tDistanceAtZeroDegree = aClipValue; - - /* - * if tY40degrees == tY20degrees the tInvGradient is infinite (distance at 0 is infinite) - */ - if (tYat40degrees > tYat20degrees) { - float tXat20degrees = cos((PI / 180) * DEGREES_PER_STEP) * aDegreesPerStepValue; // 20 Degree - float tXat40degrees = cos((PI / 180) * (DEGREES_PER_STEP * 2)) * a2DegreesPerStepValue; // 40 Degree - -// dtostrf(tXat40degrees, 7, 2, &tStringBuffer[2]); -// tStringBuffer[9] = ' '; -// dtostrf(tXat20degrees, 7, 2, &tStringBuffer[12]); -// BlueDisplay1.debugMessage(tStringBuffer); - - /* Here the graphic for 90 and 60 degrees (since we have no ASCII graphic symbols for 20 and 40 degree) - * In this function we have e.g. 40 and 20 degrees and compute 0 degrees! - * 90 degrees value - * |\ \==wall - * | \ 60 degrees value - * | /\ - * |/__\ 0 degrees value to be computed - */ - - /* - * InvGradient line represents the wall - * if tXat20degrees > tXat40degrees InvGradient is negative => X0 value is bigger than X20 one / right wall is in front if we look in 90 degrees direction - * if tXat20degrees == tXat40degrees InvGradient is 0 / wall is parallel right / 0 degree - * if tXat20degrees < tXat40degrees InvGradient is positive / right wall is behind / degrees is negative (from direction front which is 90 degrees) - */ - float tInvGradient = (tXat40degrees - tXat20degrees) / (tYat40degrees - tYat20degrees); - float tXatZeroDegree = tXat20degrees - (tInvGradient * tYat20degrees); - *aDegreeOfEndpointConnectingLine = -(atan(tInvGradient) * RAD_TO_DEG); -// tStringBuffer[0] = 'G'; -// tStringBuffer[10] = 'B'; -// dtostrf(tInvGradient, 7, 2, &tStringBuffer[2]); -// tStringBuffer[9] = ' '; -// dtostrf(tXZeroDegree, 7, 2, &tStringBuffer[12]); -// BlueDisplay1.debugMessage(tStringBuffer); - - if (tXatZeroDegree < 255) { - tDistanceAtZeroDegree = tXatZeroDegree + 0.5; - if (tDistanceAtZeroDegree > aClipValue) { - tDistanceAtZeroDegree = aClipValue; - } - } - } - return tDistanceAtZeroDegree; -} - -/* - * The problem of the ultrasonic values is, that you can only detect a wall with the ultrasonic sensor if the angle of the wall relative to sensor axis is approximately between 70 and 110 degree. - * For other angels the reflected ultrasonic beam can not not reach the receiver which leads to unrealistic great distances. - * - * Therefore I take samples every 18 degrees and if I get 2 adjacent short (< sCentimeterPerScanTimesTwo) distances, I assume a wall determined by these 2 samples. - * The (invalid) values 18 degrees right and left of these samples are then extrapolated by computeNeigbourValue(). - */ -//#define TRACE // only used for this function -void doWallDetection() { - uint8_t tTempDistancesArray[NUMBER_OF_DISTANCES]; - /* - * First copy all raw values - */ - memcpy(tTempDistancesArray, sForwardDistancesInfo.RawDistancesArray, NUMBER_OF_DISTANCES); - - uint8_t tCurrentAngleToCheck = START_DEGREES + (2 * DEGREES_PER_STEP); // first angle to adjust at index 2 - int8_t tDegreeOfConnectingLine; - sForwardDistancesInfo.WallRightAngleDegrees = 0; - sForwardDistancesInfo.WallLeftAngleDegrees = 0; - - /* - * Parse the array from 0 to STEPS_PER_SCAN - * Check values at i and i-1 and adjust value at i+1 - * i is index of CurrentValue - */ - uint8_t tLastDistance = tTempDistancesArray[0]; - uint8_t tCurrentDistance = tTempDistancesArray[1]; - for (uint8_t i = 1; i < STEPS_PER_SCAN; ++i) { - uint8_t tNextDistanceOriginal = tTempDistancesArray[i + 1]; - if (tLastDistance < sCentimeterPerScanTimesTwo && tCurrentDistance < sCentimeterPerScanTimesTwo) { - /* - * 2 adjacent short distances -> assume a wall -> adjust adjacent values - */ - - /* - * Use computeNeigbourValue the other way round - * 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, - &tDegreeOfConnectingLine); -#ifdef TRACE - BlueDisplay1.debug("i=", i); - BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); - BlueDisplay1.debug("Original distance @i+1=", tNextDistanceOriginal); - BlueDisplay1.debug("New distance @i+1=", tNextDistanceComputed); - BlueDisplay1.debug("Connecting degrees @i+1=", tDegreeOfConnectingLine); -#endif - - if (tNextDistanceOriginal > tNextDistanceComputed + 5) { - /* - * Adjust and draw next value if computed value is less than original value - 5 - * - * Start with i=1 and adjust for 2 at (2 * DEGREES_PER_STEP) + START_DEGREES. - * Since we use computeNeigbourValue the other way round, we must change sign of tDegreeOfConnectingLine! - * The formula is 90 - (180->sum of degrees in triangle - tCurrentAngleToCheck - (90 - tDegreeOfConnectingLine)->since we use it the other way round) - * Which leads to -90 + tCurrentAngleToCheck + 90 - tDegreeOfConnectingLine. - * If we then get a tDegreeOfConnectingLine value of 0 we have a wall at right rectangular to the vector at 2, - * Negative raw values means the wall is more in front / the the wall angle is greater, - */ - int tDegreeOfWallAngle = tCurrentAngleToCheck - tDegreeOfConnectingLine; -#ifdef TRACE - BlueDisplay1.debug("tDegreeOfWallAngle=", tDegreeOfWallAngle); -#endif - if (tDegreeOfWallAngle <= 90) { - // wall at right - sForwardDistancesInfo.WallRightAngleDegrees = tDegreeOfWallAngle; - } else { - // wall at left - sForwardDistancesInfo.WallLeftAngleDegrees = 180 - tDegreeOfWallAngle; - } - - // store and draw adjusted value - tTempDistancesArray[i + 1] = tNextDistanceComputed; - tNextDistanceOriginal = tNextDistanceComputed; - if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextDistanceComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); - } - } - } - tLastDistance = tCurrentDistance; - tCurrentDistance = tNextDistanceOriginal; - tCurrentAngleToCheck += DEGREES_PER_STEP; - } - - /* - * Go backwards through the array - */ - memcpy(sForwardDistancesInfo.ProcessedDistancesArray, tTempDistancesArray, NUMBER_OF_DISTANCES); - - tLastDistance = tTempDistancesArray[STEPS_PER_SCAN]; - tCurrentDistance = tTempDistancesArray[STEPS_PER_SCAN - 1]; - tCurrentAngleToCheck = 180 - (START_DEGREES + (2 * DEGREES_PER_STEP)); - - /* - * check values at i and i+1 and adjust value at i-1 - */ - for (uint8_t i = STEPS_PER_SCAN - 1; i > 0; --i) { - uint8_t tNextValue = tTempDistancesArray[i - 1]; - -// Do it only if none of the 3 values are processed before - if (tTempDistancesArray[i + 1] == sForwardDistancesInfo.RawDistancesArray[i + 1] - && tTempDistancesArray[i] == sForwardDistancesInfo.RawDistancesArray[i] - && tNextValue == sForwardDistancesInfo.RawDistancesArray[i - 1]) { - - /* - * check values at i+1 and i and adjust value at i-1 - */ - if (tLastDistance < sCentimeterPerScanTimesTwo && tCurrentDistance < sCentimeterPerScanTimesTwo) { - /* - * Wall detected -> adjust adjacent values - * Use computeNeigbourValue in the intended way, so do not change sign of tDegreeOfConnectingLine! - */ - uint8_t tNextValueComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, DISTANCE_TIMEOUT_CM, - &tDegreeOfConnectingLine); -#ifdef TRACE - BlueDisplay1.debug("i=", i); - BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); - BlueDisplay1.debug("Original distance @i-1=", tNextValue); - BlueDisplay1.debug("New distance @i-1=", tNextValueComputed); - BlueDisplay1.debug("Connecting degrees @i-1=", tDegreeOfConnectingLine); -#endif - if (tNextValue > tNextValueComputed + 5) { - // start with i = 8 and adjust for 7 - // degrees at index i-1 are ((i - 1) * DEGREES_PER_STEP) + START_DEGREES - int tWallBackwardDegrees = tCurrentAngleToCheck + tDegreeOfConnectingLine; -#ifdef TRACE - BlueDisplay1.debug("tWallBackwardDegrees=", tWallBackwardDegrees); -#endif - if (tWallBackwardDegrees <= 90) { - // wall at right - overwrite only if greater - if (sForwardDistancesInfo.WallRightAngleDegrees < tWallBackwardDegrees) { - sForwardDistancesInfo.WallRightAngleDegrees = tWallBackwardDegrees; -#ifdef TRACE - BlueDisplay1.debug("WallRightAngleDegrees=", sForwardDistancesInfo.WallRightAngleDegrees); -#endif - } - } else if (sForwardDistancesInfo.WallLeftAngleDegrees < (180 - tWallBackwardDegrees)) { - // wall at right - overwrite only if greater - sForwardDistancesInfo.WallLeftAngleDegrees = 180 - tWallBackwardDegrees; -#ifdef TRACE - BlueDisplay1.debug("WallLeftAngleDegrees=", sForwardDistancesInfo.WallLeftAngleDegrees); -#endif - - } - //Adjust and draw next value if original value is greater - sForwardDistancesInfo.ProcessedDistancesArray[i - 1] = tNextValueComputed; - tNextValue = tNextValueComputed; - if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextValueComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); - } - } - } - } - tLastDistance = tCurrentDistance; - tCurrentDistance = tNextValue; - tCurrentAngleToCheck -= DEGREES_PER_STEP; - - } -} - /* * Checks distances and returns degrees to turn - * 0 -> no turn, > 0 -> turn left, < 0 -> turn right, > 360 go back, since too close to wall + * Wall degree 0 -> wall parallel to our driving direction, 90 -> wall in front. + * 0 -> no turn, > 0 -> turn left, < 0 -> turn right, > 360 go back, since not free ahead */ int doBuiltInCollisionDetection() { int tDegreeToTurn = 0; @@ -503,48 +263,21 @@ int doBuiltInCollisionDetection() { if (sForwardDistancesInfo.ProcessedDistancesArray[INDEX_FORWARD_1] > sCentimeterPerScanTimesTwo && sForwardDistancesInfo.ProcessedDistancesArray[INDEX_FORWARD_2] > sCentimeterPerScanTimesTwo) { /* - * Free ahead, check if our side is near to the wall and make corrections + * Free ahead, currently do nothing */ - if (sForwardDistancesInfo.WallRightAngleDegrees != 0 || sForwardDistancesInfo.WallLeftAngleDegrees != 0) { - /* - * Wall detected - */ - if (sForwardDistancesInfo.WallRightAngleDegrees > sForwardDistancesInfo.WallLeftAngleDegrees) { - /* - * Wall at right => turn left - */ - tDegreeToTurn = sForwardDistancesInfo.WallRightAngleDegrees; - } else { - /* - * Wall at left => turn right - */ - tDegreeToTurn = -sForwardDistancesInfo.WallLeftAngleDegrees; - } - - } } else { - if (sForwardDistancesInfo.WallRightAngleDegrees != 0 || sForwardDistancesInfo.WallLeftAngleDegrees != 0) { + /* + * Not free ahead, must turn, check if another forward direction is suitable + */ + if (sForwardDistancesInfo.IndexOfDistanceGreaterThanThreshold < NUMBER_OF_DISTANCES) { /* - * Wall detected + * We have at least index with distance greater than threshold of sCentimeterPerScanTimesTwo */ - if (sForwardDistancesInfo.WallRightAngleDegrees > sForwardDistancesInfo.WallLeftAngleDegrees) { - /* - * Wall at right => turn left - */ - tDegreeToTurn = sForwardDistancesInfo.WallRightAngleDegrees; - } else { - /* - * Wall at left => turn right - */ - tDegreeToTurn = -sForwardDistancesInfo.WallLeftAngleDegrees; - } + tDegreeToTurn = ((sForwardDistancesInfo.IndexOfDistanceGreaterThanThreshold * DEGREES_PER_STEP) + START_DEGREES) - 90; } else { - /* - * Not free ahead, must turn, check if another forward direction is suitable - */ if (sForwardDistancesInfo.MaxDistance > sCentimeterPerScanTimesTwo) { /* - * Go to max distance + * Go to max distance if greater than threshold of sCentimeterPerScanTimesTwo, currently the same as above */ tDegreeToTurn = ((sForwardDistancesInfo.IndexOfMaxDistance * DEGREES_PER_STEP) + START_DEGREES) - 90; } else { @@ -561,115 +294,81 @@ int doBuiltInCollisionDetection() { /*************************************************** * Code for follower mode ***************************************************/ -bool sLastFollowerTargetFoundRight; +void checkSpeedAndGo(unsigned int aSpeed, uint8_t aRequestedDirection) { + if (aSpeed > RobotCarMotorControl.rightCarMotor.DriveSpeed * 2) { + aSpeed = RobotCarMotorControl.rightCarMotor.DriveSpeed * 2; + } + if (aSpeed > MAX_SPEED) { + aSpeed = MAX_SPEED; + } + RobotCarMotorControl.startRampUpAndWait(aSpeed, aRequestedDirection, &loopGUI); + +} + +/* + * Start with scanning for target, since sFollowerTargetFound is false initially. + * + * If mode == MODE_STEP_TO_NEXT_TURN, stop if the scan has found a target (sNextDegreesToTurn != SCAN_AGAIN). + * Start at next step with the turn until the next target has been searched for and found. + */ void driveFollowerModeOneStep() { unsigned int tCentimeter = getDistanceAndPlayTone(); - /* - * check if distance too high, then search target in other direction - */ - if (!sSearchFollowerTarget && tCentimeter > FOLLOWER_RESCAN_DISTANCE) { - // Stop and get new distance info - RobotCarMotorControl.stopMotors(); - - clearPrintedForwardDistancesInfos(); - // show current distance (as US distance), which triggers the rescan - showUSDistance(tCentimeter); + if (sStepMode == MODE_STEP_TO_NEXT_TURN && sNextDegreesToTurn != SCAN_AGAIN) { + // we had fount a target before -> wait for step signal + if (sDoStep) { + sDoStep = false; + RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_FORWARD, true); + //RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_IN_PLACE, true); + // reset flag, that we have to stop + sNextDegreesToTurn = SCAN_AGAIN; + } + return; // wait for step + } + if (!sFollowerTargetFound || tCentimeter > FOLLOWER_RESCAN_DISTANCE_CENTIMETER) { /* - * Check 70, 90 and 110 degree for moved target + * Distance too high, stop car and search target in front directions */ - uint8_t tDegreeForSearch; - int tDeltaDegree; - if (sLastFollowerTargetFoundRight) { - // Start searching at right - tDegreeForSearch = 70; - tDeltaDegree = 20; - } else { - // Start searching at left - tDegreeForSearch = 110; - tDeltaDegree = -20; - } - for (uint8_t i = 0; i < 3; ++i) { - DistanceServoWriteAndDelay(tDegreeForSearch, true); - tCentimeter = getDistanceAsCentiMeter(false); - if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) { - /* - * Determine color - */ - color16_t tColor; - tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan - if (tCentimeter <= FOLLOWER_MAX_DISTANCE) { - tColor = COLOR_GREEN; - } else if (tCentimeter < FOLLOWER_MIN_DISTANCE) { - tColor = COLOR_YELLOW; - } - /* - * Draw distance line - */ - BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tDegreeForSearch, - tColor, 3); - } - if (tCentimeter <= FOLLOWER_RESCAN_DISTANCE) { - break; - } - // prepare for next scan - loopGUI(); - tDegreeForSearch += tDeltaDegree; - } - if (tDegreeForSearch <= 90) { - sLastFollowerTargetFoundRight = true; - } - int8_t tDegreeToTurn = tDegreeForSearch - 90; - -// if (fillAndShowForwardDistancesInfo(true)) { -// // User canceled autonomous drive, ForwardDistancesInfo may be incomplete then -// return; -// } -// postProcessDistances(); -// int tDegreeToTurn = sForwardDistancesInfo.IndexOfMinDistance * DEGREES_PER_STEP + START_DEGREES - 90; -// tCentimeter = sForwardDistancesInfo.MinDistance; - - // reset distance servo to 90 degree - DistanceServoWriteAndDelay(90, false); + RobotCarMotorControl.stopMotors(); + clearPrintedForwardDistancesInfos(); + // show current distance (as US distance), which triggers the scan + showUSDistance(tCentimeter); - if (tCentimeter <= FOLLOWER_RESCAN_DISTANCE) { - sSearchFollowerTarget = true; // Moved target found -> turn and start new search again - /* - * Draw turn vector - */ -// BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tDegreeToTurn + 90, -// COLOR_BLACK); - // Print turn info - sprintf_P(sStringBuffer, PSTR("rotation:%3d\xB0 min:%2dcm"), tDegreeToTurn, 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); - /* - * Rotate car - */ - RobotCarMotorControl.rotateCar(tDegreeToTurn, TURN_FORWARD, true); -// RobotCarMotorControl.rotateCar(tDegreeToTurn, TURN_IN_PLACE); + scanForTarget(); + if (sNextDegreesToTurn != SCAN_AGAIN) { + sFollowerTargetFound = true; // (Moved) target found } + delayAndLoopGUI(50); // to display the values return; } - if (tCentimeter > FOLLOWER_MAX_DISTANCE) { -// Serial.println(F("Go forward")); - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD, &loopGUI); + unsigned int tSpeed; + if (tCentimeter > FOLLOWER_MAX_DISTANCE_CENTIMETER) { +// if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_FORWARD) { +// Serial.println(F("Go forward")); +// } + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (tCentimeter - FOLLOWER_MAX_DISTANCE_CENTIMETER) * 2; + checkSpeedAndGo(tSpeed, DIRECTION_FORWARD); - } else if (tCentimeter < FOLLOWER_MIN_DISTANCE) { -// Serial.println(F("Go backward")); - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_BACKWARD, &loopGUI); + } else if (tCentimeter < FOLLOWER_MIN_DISTANCE_CENTIMETER) { +// if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_BACKWARD) { +// Serial.println(F("Go backward")); +// } + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (FOLLOWER_MIN_DISTANCE_CENTIMETER - tCentimeter) * 4; + checkSpeedAndGo(tSpeed, DIRECTION_BACKWARD); } else { // Target found here :-) - sSearchFollowerTarget = false; + sFollowerTargetFound = true; + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != MOTOR_RELEASE) { // Serial.println(F("Stop")); - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + } } - delayAndLoopGUI(40); // the IR sensor takes 39 ms for one measurement + delayAndLoopGUI(100); // the IR sensor takes 39 ms for one measurement } unsigned int __attribute__((weak)) getDistanceAndPlayTone() { diff --git a/examples/RobotCarBlueDisplay/AutonomousDrive.h b/examples/RobotCarBlueDisplay/AutonomousDrive.h index e1b5116..41e8eb3 100644 --- a/examples/RobotCarBlueDisplay/AutonomousDrive.h +++ b/examples/RobotCarBlueDisplay/AutonomousDrive.h @@ -39,9 +39,10 @@ extern uint8_t sDriveMode; extern uint8_t sStepMode; extern bool sDoStep; -#define FOLLOWER_MIN_DISTANCE 22 -#define FOLLOWER_MAX_DISTANCE 30 -#define FOLLOWER_RESCAN_DISTANCE 50 // search if target moved to side +#define FOLLOWER_MIN_DISTANCE_CENTIMETER 22 +#define FOLLOWER_MAX_DISTANCE_CENTIMETER 30 +#define FOLLOWER_DELTA_DISTANCE_CENTIMETER (FOLLOWER_MAX_DISTANCE_CENTIMETER - FOLLOWER_MIN_DISTANCE_CENTIMETER) +#define FOLLOWER_RESCAN_DISTANCE_CENTIMETER 50 // search if target moved to side /* * Different result types acquired at one scan @@ -55,6 +56,7 @@ extern uint8_t sScanMode; #endif #define GO_BACK_AND_SCAN_AGAIN 360 // possible result of doBuiltInCollisionDetection() +#define SCAN_AGAIN 360 // possible result of scanForTarget() /* * Used for adaptive collision detection @@ -70,9 +72,6 @@ void startStopAutomomousDrive(bool aDoStart, uint8_t aDriveMode = MODE_MANUAL_DR void driveAutonomousOneStep(); void driveFollowerModeOneStep(); -void doWallDetection(); -int doBuiltInCollisionDetection(); - #endif /* SRC_AUTONOMOUSDRIVE_H_ */ #pragma once diff --git a/examples/RobotCarBlueDisplay/AutonomousDrivePage.cpp b/examples/RobotCarBlueDisplay/AutonomousDrivePage.cpp index 8cf55ae..b0634d1 100644 --- a/examples/RobotCarBlueDisplay/AutonomousDrivePage.cpp +++ b/examples/RobotCarBlueDisplay/AutonomousDrivePage.cpp @@ -124,10 +124,14 @@ void doChangeScanSpeed(BDButton * aTheTouchedButton, int16_t aValue) { } void doSingleScan(BDButton * aTheTouchedButton, int16_t aValue) { - clearPrintedForwardDistancesInfos(); - fillAndShowForwardDistancesInfo( true, true); + if (sDriveMode == MODE_FOLLOWER) { + scanForTarget(); + } else { + clearPrintedForwardDistancesInfos(); + fillAndShowForwardDistancesInfo(true, true); - postProcessAndCollisionDetection(); + postProcessAndCollisionDetection(); + } } void doStartStopFollowerMode(BDButton * aTheTouchedButton, int16_t aValue) { @@ -289,11 +293,12 @@ void drawCollisionDecision(int aDegreeToTurn, uint8_t aLengthOfVector, bool aDoC color16_t tColor = COLOR_BLUE; int tDegreeToDisplay = aDegreeToTurn; + if (tDegreeToDisplay == 180) { + tColor = COLOR_RED; + tDegreeToDisplay = 0; + } if (aDoClearVector) { tColor = COLOR_WHITE; - } else if (tDegreeToDisplay == 180) { - tColor = COLOR_PURPLE; - tDegreeToDisplay = 0; } BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, aLengthOfVector, tDegreeToDisplay + 90, diff --git a/examples/RobotCarBlueDisplay/Distance.cpp b/examples/RobotCarBlueDisplay/Distance.cpp index 908dddd..5b05aa7 100644 --- a/examples/RobotCarBlueDisplay/Distance.cpp +++ b/examples/RobotCarBlueDisplay/Distance.cpp @@ -27,6 +27,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo; Servo DistanceServo; uint8_t sLastServoAngleInDegrees; // 0 - 180 needed for optimized delay for servo repositioning +bool sLastFollowerTargetFoundRight; #ifdef CAR_HAS_TOF_DISTANCE_SENSOR // removing usage of SFEVL53L1X wrapper class saves 794 bytes @@ -154,6 +155,84 @@ void DistanceServoWriteAndDelay(uint8_t aTargetDegrees, bool doDelay) { } } +/* + * Stop, scan 70, 90 and 110 degree for moved target and sets sNextDegreesToTurn. + */ +void scanForTarget() { + uint8_t tDegreeForSearch; + int tDeltaDegree; + unsigned int tCentimeter; + /* + * Set start values according to last successful scan + */ + if (sLastFollowerTargetFoundRight) { + // Start searching at right + tDegreeForSearch = 70; + tDeltaDegree = 20; + } else { + // Start searching at left + tDegreeForSearch = 110; + tDeltaDegree = -20; + } + /* + * Scan and display 3 distances, but break prematurely if target found. + * The break is implemented to speed up finding the moved target. + */ + for (uint8_t i = 0; i < 3; ++i) { + DistanceServoWriteAndDelay(tDegreeForSearch, true); + tCentimeter = getDistanceAsCentiMeter(false); + sForwardDistancesInfo.RawDistancesArray[i] = tCentimeter; + if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) { + /* + * Determine color + */ + color16_t tColor; + tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan + if (tCentimeter <= FOLLOWER_MAX_DISTANCE_CENTIMETER) { + tColor = COLOR_GREEN; + } else if (tCentimeter < FOLLOWER_MIN_DISTANCE_CENTIMETER) { + tColor = COLOR_YELLOW; + } + + /* + * Draw distance line + */ + // Clear old line + BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, + sForwardDistancesInfo.RawDistancesArray[i], tDegreeForSearch, + COLOR_WHITE, 3); + BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tDegreeForSearch, + tColor, 3); + } + sForwardDistancesInfo.RawDistancesArray[i] = tCentimeter; + if (tCentimeter <= FOLLOWER_RESCAN_DISTANCE_CENTIMETER) { + break; + } + // prepare for next scan + loopGUI(); + tDegreeForSearch += tDeltaDegree; + } + + + int8_t tDegreeToTurn = tDegreeForSearch - 90; + + DistanceServoWriteAndDelay(90, false); + if (tCentimeter <= FOLLOWER_RESCAN_DISTANCE_CENTIMETER) { + /* + * Target found -> print turn info + */ + sprintf_P(sStringBuffer, PSTR("rotation:%3d\xB0 min:%2dcm"), tDegreeToTurn, 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); + + // store found direction + sLastFollowerTargetFoundRight = (tDegreeForSearch <= 90); + sNextDegreesToTurn = tDegreeToTurn; + } else { + sNextDegreesToTurn = SCAN_AGAIN; + } +} + /* * Get 7 distances starting at 9 degrees (right) increasing by 18 degrees up to 171 degrees (left) * Avoid 0 and 180 degrees since at this position the US sensor might see the wheels of the car as an obstacle. @@ -204,7 +283,7 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b return true; } - unsigned int tCentimeter = getDistanceAsCentiMeter(); + unsigned int tCentimeter = getDistanceAsCentiMeter(false); 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 @@ -226,7 +305,7 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b } /* - * Clear old and draw new line + * 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); @@ -244,18 +323,265 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b return false; } +/* + * This documentation assumes 20 degrees stepping. + * By changing DEGREES_PER_STEP it can easily adopted to other degrees values. + * + * Assume the value of 20 and 40 degrees are distances to a wall. + * @return the clipped (aClipValue) distance to the wall of the vector at 0 degree. + * + * @param [out] aDegreeOfEndpointConnectingLine: The angle of the line from endpoint 0 degrees to given endpoints + * 0 means x values of given endpoints are the same >= wall is parallel / rectangular to the vector at 0 degree + * Positive means wall is more ore less in front, to avoid we must turn positive angle + * 90 means y values are the same => wall is in front + * Negative means we are heading away from wall + */ +uint8_t computeNeigbourValue(uint8_t aDegreesPerStepValue, uint8_t a2DegreesPerStepValue, uint8_t aClipValue, + int8_t * aDegreeOfEndpointConnectingLine) { + + /* + * Name of the variables are for DEGREES_PER_STEP = 20 only for better understanding. + * The computation of course works for other values of DEGREES_PER_STEP! + */ + float tYat20degrees = sin((PI / 180) * DEGREES_PER_STEP) * aDegreesPerStepValue; // e.g. 20 Degree +// assume current = 40 Degree + float tYat40degrees = sin((PI / 180) * (DEGREES_PER_STEP * 2)) * a2DegreesPerStepValue; // e.g. 40 Degree + +// char tStringBuffer[] = "A=_______ L=_______"; +// dtostrf(tYat40degrees, 7, 2, &tStringBuffer[2]); +// tStringBuffer[9] = ' '; +// dtostrf(tYat20degrees, 7, 2, &tStringBuffer[12]); +// BlueDisplay1.debugMessage(tStringBuffer); + + uint8_t tDistanceAtZeroDegree = aClipValue; + + /* + * if tY40degrees == tY20degrees the tInvGradient is infinite (distance at 0 is infinite) + */ + if (tYat40degrees > tYat20degrees) { + float tXat20degrees = cos((PI / 180) * DEGREES_PER_STEP) * aDegreesPerStepValue; // 20 Degree + float tXat40degrees = cos((PI / 180) * (DEGREES_PER_STEP * 2)) * a2DegreesPerStepValue; // 40 Degree + +// dtostrf(tXat40degrees, 7, 2, &tStringBuffer[2]); +// tStringBuffer[9] = ' '; +// dtostrf(tXat20degrees, 7, 2, &tStringBuffer[12]); +// BlueDisplay1.debugMessage(tStringBuffer); + + /* Here the graphic for 90 and 60 degrees (since we have no ASCII graphic symbols for 20 and 40 degree) + * In this function we have e.g. 40 and 20 degrees and compute 0 degrees! + * 90 degrees value + * |\ \==wall + * | \ 60 degrees value + * | /\ + * |/__\ 0 degrees value to be computed + */ + + /* + * InvGradient line represents the wall + * if tXat20degrees > tXat40degrees InvGradient is negative => X0 value is bigger than X20 one / right wall is in front if we look in 90 degrees direction + * if tXat20degrees == tXat40degrees InvGradient is 0 / wall is parallel right / 0 degree + * if tXat20degrees < tXat40degrees InvGradient is positive / right wall is behind / degrees is negative (from direction front which is 90 degrees) + */ + float tInvGradient = (tXat40degrees - tXat20degrees) / (tYat40degrees - tYat20degrees); + float tXatZeroDegree = tXat20degrees - (tInvGradient * tYat20degrees); + *aDegreeOfEndpointConnectingLine = -(atan(tInvGradient) * RAD_TO_DEG); +// tStringBuffer[0] = 'G'; +// tStringBuffer[10] = 'B'; +// dtostrf(tInvGradient, 7, 2, &tStringBuffer[2]); +// tStringBuffer[9] = ' '; +// dtostrf(tXZeroDegree, 7, 2, &tStringBuffer[12]); +// BlueDisplay1.debugMessage(tStringBuffer); + + if (tXatZeroDegree < 255) { + tDistanceAtZeroDegree = tXatZeroDegree + 0.5; + if (tDistanceAtZeroDegree > aClipValue) { + tDistanceAtZeroDegree = aClipValue; + } + } + } + return tDistanceAtZeroDegree; +} + +/* + * The problem of the ultrasonic values is, that you can only detect a wall with the ultrasonic sensor if the angle of the wall relative to sensor axis is approximately between 70 and 110 degree. + * For other angels the reflected ultrasonic beam can not not reach the receiver which leads to unrealistic great distances. + * + * Therefore I take samples every 18 degrees and if I get 2 adjacent short (< sCentimeterPerScanTimesTwo) distances, I assume a wall determined by these 2 samples. + * The (invalid) values 18 degrees right and left of these samples are then extrapolated by computeNeigbourValue(). + */ +//#define TRACE // only used for this function +void doWallDetection() { + uint8_t tTempDistancesArray[NUMBER_OF_DISTANCES]; + /* + * First copy all raw values + */ + memcpy(tTempDistancesArray, sForwardDistancesInfo.RawDistancesArray, NUMBER_OF_DISTANCES); + + uint8_t tCurrentAngleToCheck = START_DEGREES + (2 * DEGREES_PER_STEP); // first angle to adjust at index 2 + int8_t tDegreeOfConnectingLine; + sForwardDistancesInfo.WallRightAngleDegrees = 0; + sForwardDistancesInfo.WallLeftAngleDegrees = 0; +// sForwardDistancesInfo.WallRightDistance = 0xFF; +// sForwardDistancesInfo.WallLeftDistance = 0xFF; + + /* + * Parse the array from 0 to STEPS_PER_SCAN + * Check values at i and i-1 and adjust value at i+1 + * i is index of CurrentDistance + */ + uint8_t tLastDistance = tTempDistancesArray[0]; + uint8_t tCurrentDistance = tTempDistancesArray[1]; + for (uint8_t i = 1; i < STEPS_PER_SCAN; ++i) { + uint8_t tNextDistanceOriginal = tTempDistancesArray[i + 1]; + if (tLastDistance < sCentimeterPerScanTimesTwo && tCurrentDistance < sCentimeterPerScanTimesTwo) { + /* + * 2 adjacent short distances -> assume a wall -> adjust adjacent values + */ + + /* + * Use computeNeigbourValue the other way round + * 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, + &tDegreeOfConnectingLine); +#ifdef TRACE + BlueDisplay1.debug("i=", i); + BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); + BlueDisplay1.debug("Original distance @i+1=", tNextDistanceOriginal); + BlueDisplay1.debug("New distance @i+1=", tNextDistanceComputed); + BlueDisplay1.debug("Connecting degrees @i+1=", tDegreeOfConnectingLine); +#endif + + if (tNextDistanceOriginal > tNextDistanceComputed + 5) { + /* + * Adjust and draw next value if computed value is less than original value - 5 + * + * Start with i=1 and adjust for 2 at (2 * DEGREES_PER_STEP) + START_DEGREES. + * Since we use computeNeigbourValue the other way round, we must change sign of tDegreeOfConnectingLine! + * The formula is 90 - (180->sum of degrees in triangle - tCurrentAngleToCheck - (90 - tDegreeOfConnectingLine)->since we use it the other way round) + * Which leads to -90 + tCurrentAngleToCheck + 90 - tDegreeOfConnectingLine. + * If we then get a tDegreeOfConnectingLine value of 0 we have a wall at right rectangular to the vector at 2, + * Negative raw values means the wall is more in front / the the wall angle is greater, + */ + int tDegreeOfWallAngle = tCurrentAngleToCheck - tDegreeOfConnectingLine; +#ifdef TRACE + BlueDisplay1.debug("tDegreeOfWallAngle=", tDegreeOfWallAngle); +#endif + if (tDegreeOfWallAngle <= 90) { + // wall at right + sForwardDistancesInfo.WallRightAngleDegrees = tDegreeOfWallAngle; +// sForwardDistancesInfo.WallRightDistance = tCurrentDistance; + } else { + // wall at left + sForwardDistancesInfo.WallLeftAngleDegrees = 180 - tDegreeOfWallAngle; +// sForwardDistancesInfo.WallLeftDistance = tCurrentDistance; + } + + // store and draw adjusted value + tTempDistancesArray[i + 1] = tNextDistanceComputed; + tNextDistanceOriginal = tNextDistanceComputed; + if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { + BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextDistanceComputed, + tCurrentAngleToCheck, COLOR_WHITE, 1); + } + } + } + tLastDistance = tCurrentDistance; + tCurrentDistance = tNextDistanceOriginal; + tCurrentAngleToCheck += DEGREES_PER_STEP; + } + + /* + * Go backwards through the array + */ + memcpy(sForwardDistancesInfo.ProcessedDistancesArray, tTempDistancesArray, NUMBER_OF_DISTANCES); + + tLastDistance = tTempDistancesArray[STEPS_PER_SCAN]; + tCurrentDistance = tTempDistancesArray[STEPS_PER_SCAN - 1]; + tCurrentAngleToCheck = 180 - (START_DEGREES + (2 * DEGREES_PER_STEP)); + + /* + * check values at i and i+1 and adjust value at i-1 + */ + for (uint8_t i = STEPS_PER_SCAN - 1; i > 0; --i) { + uint8_t tNextValue = tTempDistancesArray[i - 1]; + +// Do it only if none of the 3 values are processed before + if (tTempDistancesArray[i + 1] == sForwardDistancesInfo.RawDistancesArray[i + 1] + && tTempDistancesArray[i] == sForwardDistancesInfo.RawDistancesArray[i] + && tNextValue == sForwardDistancesInfo.RawDistancesArray[i - 1]) { + + /* + * check values at i+1 and i and adjust value at i-1 + */ + if (tLastDistance < sCentimeterPerScanTimesTwo && tCurrentDistance < sCentimeterPerScanTimesTwo) { + /* + * Wall detected -> adjust adjacent values + * Use computeNeigbourValue in the intended way, so do not change sign of tDegreeOfConnectingLine! + */ + uint8_t tNextValueComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, DISTANCE_TIMEOUT_CM, + &tDegreeOfConnectingLine); +#ifdef TRACE + BlueDisplay1.debug("i=", i); + BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); + BlueDisplay1.debug("Original distance @i-1=", tNextValue); + BlueDisplay1.debug("New distance @i-1=", tNextValueComputed); + BlueDisplay1.debug("Connecting degrees @i-1=", tDegreeOfConnectingLine); +#endif + if (tNextValue > tNextValueComputed + 5) { + // start with i = 8 and adjust for 7 + // degrees at index i-1 are ((i - 1) * DEGREES_PER_STEP) + START_DEGREES + int tWallBackwardDegrees = tCurrentAngleToCheck + tDegreeOfConnectingLine; +#ifdef TRACE + BlueDisplay1.debug("tWallBackwardDegrees=", tWallBackwardDegrees); +#endif + if (tWallBackwardDegrees <= 90) { + // wall at right - overwrite only if greater + if (sForwardDistancesInfo.WallRightAngleDegrees < tWallBackwardDegrees) { + sForwardDistancesInfo.WallRightAngleDegrees = tWallBackwardDegrees; +#ifdef TRACE + BlueDisplay1.debug("WallRightAngleDegrees=", sForwardDistancesInfo.WallRightAngleDegrees); +#endif + } + } else if (sForwardDistancesInfo.WallLeftAngleDegrees < (180 - tWallBackwardDegrees)) { + // wall at right - overwrite only if greater + sForwardDistancesInfo.WallLeftAngleDegrees = 180 - tWallBackwardDegrees; +#ifdef TRACE + BlueDisplay1.debug("WallLeftAngleDegrees=", sForwardDistancesInfo.WallLeftAngleDegrees); +#endif + + } + //Adjust and draw next value if original value is greater + sForwardDistancesInfo.ProcessedDistancesArray[i - 1] = tNextValueComputed; + tNextValue = tNextValueComputed; + if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { + BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextValueComputed, + tCurrentAngleToCheck, COLOR_WHITE, 1); + } + } + } + } + tLastDistance = tCurrentDistance; + tCurrentDistance = tNextValue; + tCurrentAngleToCheck -= DEGREES_PER_STEP; + + } +} + /* * Find min and max value. Prefer the headmost value if we have more than one choice * Do not use the values at 0 and 9 for minimum, since sometimes we measure the distance to the own wheels at this degrees. */ -void postProcessDistances() { +void postProcessDistances(uint8_t aDistanceThreshold) { unsigned int tMax = 0; unsigned int tMin = __UINT16_MAX__; // = 65535 + sForwardDistancesInfo.IndexOfDistanceGreaterThanThreshold = 0xFF; // scan simultaneously from 0 to 4 and 9 to 5 to prefer headmost values/indexes, if distances are the same. for (uint8_t i = 0; i < (NUMBER_OF_DISTANCES + 1) / 2; ++i) { uint8_t tIndex = i; for (uint8_t j = 0; j < 2; ++j) { uint8_t tDistance; + // Check if we have processed distances, otherwise take the raw ones if (sForwardDistancesInfo.ProcessedDistancesArray[0] != 0) { tDistance = sForwardDistancesInfo.ProcessedDistancesArray[tIndex]; } else { @@ -271,6 +597,9 @@ void postProcessDistances() { sForwardDistancesInfo.IndexOfMinDistance = tIndex; sForwardDistancesInfo.MinDistance = tDistance; } + if (tDistance >= aDistanceThreshold) { + sForwardDistancesInfo.IndexOfDistanceGreaterThanThreshold = tIndex; + } tIndex = STEPS_PER_SCAN - i; } } @@ -280,7 +609,7 @@ void readAndShowDistancePeriodically(uint16_t aPeriodMillis) { static long sLastUSMeasurementMillis; // Do not show distanced during (time critical) acceleration or deceleration - if (!RobotCarMotorControl.needsFastUpdates()) { + if (!RobotCarMotorControl.isStateRamp()) { long tMillis = millis(); if (sLastUSMeasurementMillis + aPeriodMillis < tMillis) { sLastUSMeasurementMillis = tMillis; diff --git a/examples/RobotCarBlueDisplay/Distance.h b/examples/RobotCarBlueDisplay/Distance.h index 6fb1ed2..22ce213 100644 --- a/examples/RobotCarBlueDisplay/Distance.h +++ b/examples/RobotCarBlueDisplay/Distance.h @@ -57,11 +57,14 @@ struct ForwardDistancesInfoStruct { uint8_t ProcessedDistancesArray[NUMBER_OF_DISTANCES]; // From 0 (right) to 180 degrees (left) with steps of 20 degrees uint8_t IndexOfMaxDistance; uint8_t IndexOfMinDistance; // do not take first (0) and last index for minimum (we may measure the distance to our wheel there) + uint8_t IndexOfDistanceGreaterThanThreshold; uint8_t MaxDistance; uint8_t MinDistance; // 0 degree => wall parallel to side of car. 90 degrees => wall in front of car. degrees of wall -> degrees to turn. int8_t WallRightAngleDegrees; int8_t WallLeftAngleDegrees; +// uint8_t WallRightDistance; +// uint8_t WallLeftDistance; }; extern ForwardDistancesInfoStruct sForwardDistancesInfo; @@ -75,8 +78,11 @@ extern int sLastDegreesTurned; void initDistance(); void DistanceServoWriteAndDelay(uint8_t aValue, bool doDelay = false); unsigned int getDistanceAsCentiMeter(bool doShow = false); +void scanForTarget(); bool fillAndShowForwardDistancesInfo(bool aDoFirstValue, bool aForceScan = false); -void postProcessDistances(); +void doWallDetection(); +int doBuiltInCollisionDetection(); +void postProcessDistances(uint8_t aDistanceThreshold); #ifdef CAR_HAS_TOF_DISTANCE_SENSOR #include "vl53l1x_class.h" diff --git a/examples/RobotCarBlueDisplay/HCSR04.cpp b/examples/RobotCarBlueDisplay/HCSR04.cpp index c3badb5..75c50cf 100644 --- a/examples/RobotCarBlueDisplay/HCSR04.cpp +++ b/examples/RobotCarBlueDisplay/HCSR04.cpp @@ -93,7 +93,6 @@ unsigned int getUSDistance(unsigned int aTimeoutMicros) { // need minimum 10 usec Trigger Pulse digitalWrite(sTriggerOutPin, HIGH); - // If in if (sHCSR04Mode == HCSR04_MODE_USE_1_PIN) { // do it AFTER digitalWrite to avoid spurious triggering by just switching pin to output diff --git a/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino b/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino index 471c649..c90a179 100644 --- a/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino +++ b/examples/RobotCarBlueDisplay/RobotCarBlueDisplay.ino @@ -8,6 +8,9 @@ * Manual control is by a GUI implemented with a Bluetooth HC-05 Module and the BlueDisplay library. * Just overwrite the 2 functions myOwnFillForwardDistancesInfo() and doUserCollisionDetection() to test your own skill. * + * If Bluetooth is not connected, after TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS (10 seconds) the car starts demo mode. + * After power up it runs in follower mode and after reset it runs in autonomous drive mode. + * * Program size of GUI is 63 percent. 27% vs. 90%. * * Copyright (C) 2016-2020 Armin Joachimsmeyer @@ -140,7 +143,7 @@ void setup() { RobotCarMotorControl.init(true); // true -> read from EEPROM #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, true); + PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, true); // true -> read from EEPROM #endif delay(100); diff --git a/examples/RobotCarBlueDisplay/TestPage.cpp b/examples/RobotCarBlueDisplay/TestPage.cpp index 778e40d..bfe9e50 100644 --- a/examples/RobotCarBlueDisplay/TestPage.cpp +++ b/examples/RobotCarBlueDisplay/TestPage.cpp @@ -47,7 +47,7 @@ bool sShowDebug = false; #pragma GCC diagnostic ignored "-Wunused-parameter" void doDistance(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.initGoDistanceCentimeter(aValue, sRobotCarDirection); + RobotCarMotorControl.startGoDistanceCentimeter(aValue, sRobotCarDirection); } void doShowDebug(BDButton * aTheTouchedButton, int16_t aValue) { @@ -55,7 +55,7 @@ void doShowDebug(BDButton * aTheTouchedButton, int16_t aValue) { } void doRotation(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.initRotateCar(aValue, sRobotCarDirection, true); + RobotCarMotorControl.startRotateCar(aValue, sRobotCarDirection, true); } /* diff --git a/examples/RobotCarFollower/HCSR04.cpp b/examples/RobotCarFollower/HCSR04.cpp index c3badb5..75c50cf 100644 --- a/examples/RobotCarFollower/HCSR04.cpp +++ b/examples/RobotCarFollower/HCSR04.cpp @@ -93,7 +93,6 @@ unsigned int getUSDistance(unsigned int aTimeoutMicros) { // need minimum 10 usec Trigger Pulse digitalWrite(sTriggerOutPin, HIGH); - // If in if (sHCSR04Mode == HCSR04_MODE_USE_1_PIN) { // do it AFTER digitalWrite to avoid spurious triggering by just switching pin to output diff --git a/examples/RobotCarFollower/RobotCar.h b/examples/RobotCarFollower/RobotCar.h deleted file mode 100644 index 1c2ed90..0000000 --- a/examples/RobotCarFollower/RobotCar.h +++ /dev/null @@ -1,214 +0,0 @@ -/* - * RobotCarMotorControl.h - * - * Created on: 29.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/Arduino-RobotCar. - * - * 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 SRC_ROBOTCAR_H_ -#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 CAR_HAS_IR_DISTANCE_SENSOR - -//#define CAR_HAS_TOF_DISTANCE_SENSOR - -//#define CAR_HAS_CAMERA - -//#define CAR_HAS_LASER - -/* - * For pan tilt we have 2 servos in total - */ -//#define CAR_HAS_PAN_SERVO -//#define CAR_HAS_TILT_SERVO -// -/* - * Plays melody after initial timeout has reached - * Enables the Play Melody button - */ -// #define ENABLE_RTTTL -// -#include "CarMotorControl.h" -extern CarMotorControl RobotCarMotorControl; - -/* - * Pin usage - * First the function of the nano board variant. For this variant the PWM is generated with analogWrite(). - */ -/* - * PIN I/O Function - * 2 I Right motor encoder interrupt input - * 3 I Left motor encoder interrupt input - * 4 O Left motor fwd / NC for UNO board - * 5 O Left motor PWM / NC for UNO board - * 6 O Right motor PWM / NC for UNO board - * 7 O Left motor back / NC for UNO board - * 8 O Right motor fwd / NC for UNO board - * 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 Right motor back / NC 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 - * 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 - * A7 O Camera supply control - */ - -#if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) // enable it in PWMDCMotor.h -/* - * Pins for direct motor control with PWM and full bridge - * Pins 9 + 10 are reserved for Servo - * 2 + 3 are reserved for encoder input - */ -#define PIN_LEFT_MOTOR_FORWARD 12 // Pin 9 is already reserved for distance servo -#define PIN_LEFT_MOTOR_BACKWARD 8 -#define PIN_LEFT_MOTOR_PWM 6 // Must be PWM capable - -#define PIN_RIGHT_MOTOR_FORWARD 4 -#define PIN_RIGHT_MOTOR_BACKWARD 7 -#define PIN_RIGHT_MOTOR_PWM 5 // Must be PWM capable -#endif - - -/* - * Servo pins - */ -#define PIN_DISTANCE_SERVO 9 // Servo Nr. 2 on Adafruit Motor Shield -#ifdef CAR_HAS_PAN_SERVO -#define PIN_PAN_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield -#endif -#ifdef CAR_HAS_TILT_SERVO -#define PIN_TILT_SERVO 11 -#endif - -#if defined(MONITOR_LIPO_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 -#endif - -/* - * Pins for US HC-SR04 distance sensor - */ -#define PIN_TRIGGER_OUT A0 // Connections on the Arduino Sensor Shield -#ifdef USE_US_SENSOR_1_PIN_MODE -#define PIN_IR_DISTANCE_SENSOR A1 // Otherwise available as US echo pin -#else -#define PIN_ECHO_IN A1 // used by Sharp IR distance sensor -#endif - -#ifdef CAR_HAS_LASER -#define PIN_LASER_OUT LED_BUILTIN -#endif - -/* - * Different pin layout for UNO with Adafruit motor shield and Nano (Nano hash full bridge) boards - */ -#ifdef USE_LAYOUT_FOR_NANO -/* - * Nano Layout - */ -# ifdef USE_ADAFRUIT_MOTOR_SHIELD -#error "Adafruit motor shield makes no sense for a Nano board!" -# endif -# ifdef CAR_HAS_CAMERA -#define PIN_CAMERA_SUPPLY_CONTROL A7 // Not available on UNO board -# endif -#define PIN_SPEAKER A6 // Not available on UNO board - -#else -/* - * UNO Layout - */ -# ifdef CAR_HAS_CAMERA -#define PIN_CAMERA_SUPPLY_CONTROL 4 -# endif -#define PIN_SPEAKER 11 -#endif - -/************************** - * End of pin definitions - **************************/ - -/* - * Timeouts for demo mode and inactivity remainder - */ -#define TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS 240000L // move Servo after 4 Minutes of inactivity -#define TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS 10000 // Start demo mode 10 seconds after boot up - -//#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 500 -#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 100 - -/* - * Servo timing correction. - * Values are for my SG90 servo. Servo is mounted head down, so values must be swapped! - */ -#define DISTANCE_SERVO_MIN_PULSE_WIDTH (MIN_PULSE_WIDTH - 40) // Value for 180 degree -#define DISTANCE_SERVO_MAX_PULSE_WIDTH (MAX_PULSE_WIDTH - 40) // Value for 0 degree, since servo is mounted head down. -#ifdef CAR_HAS_PAN_SERVO -extern Servo PanServo; -#endif -#ifdef CAR_HAS_TILT_SERVO -#define TILT_SERVO_MIN_VALUE 7 // since lower values will make an insane sound at my pan tilt device -extern Servo TiltServo; -#endif - -#define DISTANCE_TIMEOUT_CM 100 // do not measure and process distances greater than 100 cm - -/************************************************************************************ - * Definitions and declarations only used for GUI in RobotCarBlueDisplay.cpp example - ************************************************************************************/ -#define DISTANCE_TIMEOUT_COLOR COLOR_CYAN -#define DISTANCE_DISPLAY_PERIOD_MILLIS 500 - -#define MINIMUM_DISTANCE_TO_SIDE 21 -#define MINIMUM_DISTANCE_TO_FRONT 35 - -#if defined(MONITOR_LIPO_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 -#else -#endif -#define VOLTAGE_TOO_LOW_DELAY_ONLINE 3000 // display VIN every 500 ms for 4 seconds -#define VOLTAGE_TOO_LOW_DELAY_OFFLINE 1000 // wait for 1 seconds after double beep - -void readVINVoltage(); -#endif - -void resetServos(); -int doUserCollisionDetection(); - -#endif /* SRC_ROBOTCAR_H_ */ - -#pragma once diff --git a/examples/RobotCarFollower/RobotCarFollower.ino b/examples/RobotCarFollower/RobotCarFollower.ino index b6e5358..f0de1c3 100644 --- a/examples/RobotCarFollower/RobotCarFollower.ino +++ b/examples/RobotCarFollower/RobotCarFollower.ino @@ -1,12 +1,14 @@ /* * RobotCarFollower.cpp * - * Enables follower mode driving of a 2 or 4 wheel car with an Arduino and an Adafruit Motor Shield V2. - * To find the target to follow, a HC-SR04 Ultrasonic sensor mounted on a SG90 Servo scans the area on demand. + * Enables follower mode driving of a 2 or 4 wheel car with an Arduino. + * To find the target to follow, a HC-SR04 Ultrasonic sensor mounted on a SG90 Servo scans the area on demand. (not yet implemented) * * Copyright (C) 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 @@ -23,11 +25,31 @@ #include "Servo.h" #include "HCSR04.h" -#define VERSION_EXAMPLE "1.0" +#define DISTANCE_MINIMUM_CENTIMETER 20 // If measured distance is less than this value, go backwards +#define DISTANCE_MAXIMUM_CENTIMETER 30 // If measured distance is greater than this value, go forward +#define DISTANCE_DELTA_CENTIMETER (DISTANCE_MAXIMUM_CENTIMETER - DISTANCE_MINIMUM_CENTIMETER) + +//#define VIN_2_LIPO + +//#define PLOTTER_OUTPUT // Comment this out, if you want to see the result of the US distance sensor and resulting speed in Arduino plotter + +#if defined(VIN_2_LIPO) +// values for 2xLIPO / 7.4 volt +#define START_SPEED 30 // Speed PWM value at which car starts to move. +#define DRIVE_SPEED 60 // Speed PWM value used for going fixed distance. +#define MAX_SPEED_FOLLOWER 100 // Max speed PWM value used for follower. +#else +// Values for 4xAA / 6.0 volt +#define START_SPEED 80 // Speed PWM value at which car starts to move. +#define DRIVE_SPEED 150 // Speed PWM value used for going fixed distance. +#define MAX_SPEED_FOLLOWER 255 // Max speed PWM value used for follower. +#endif + +#define SPEED_COMPENSATION_RIGHT 0 // If positive, this value is subtracted from the speed of the right motor, if negative, -value is subtracted from the left speed. #if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) // enable it in PWMDCMotor.h /* - * Pins for direct motor control with PWM and full bridge + * Pins for direct motor control with PWM and a dual full bridge e.g. TB6612 or L298. * Pins 9 + 10 are reserved for Servo * 2 + 3 are reserved for encoder input */ @@ -50,7 +72,6 @@ //Car Control CarMotorControl RobotCarMotorControl; Servo DistanceServo; -//#define PLOTTER_OUTPUT // Comment this out, if you want to ses the result of the US distance sensor in Arduino plotter unsigned int getDistanceAndPlayTone(); @@ -62,9 +83,9 @@ void setup() { // Just to know which program is running on my Arduino #ifdef PLOTTER_OUTPUT - Serial.println(F("Distance[cm]")); + Serial.println(F("Distance[cm] Speed")); #else - Serial.println(F("START " __FILE__ "\r\nVersion " VERSION_EXAMPLE " from " __DATE__)); + Serial.println(F("START " __FILE__ " from " __DATE__ "\r\nUsing library version " VERSION_PWMMOTORCONTROL)); #endif #ifdef USE_ADAFRUIT_MOTOR_SHIELD @@ -73,7 +94,7 @@ void setup() { 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); #endif - RobotCarMotorControl.setValuesForFixedDistanceDriving(80, 200, 0); + RobotCarMotorControl.setValuesForFixedDistanceDriving(START_SPEED, DRIVE_SPEED, SPEED_COMPENSATION_RIGHT); DistanceServo.attach(PIN_DISTANCE_SERVO); DistanceServo.write(90); @@ -83,37 +104,92 @@ void setup() { delay(200); tone(PIN_SPEAKER, 2200, 100); + /* + * Do not start immediately with driving + */ delay(5000); - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD); - delay(1000); + + /* + * Tone feedback for start of driving + */ + tone(PIN_SPEAKER, 2200, 100); + delay(200); + tone(PIN_SPEAKER, 2200, 100); } -bool sFoundPerson = false; void loop() { unsigned int tCentimeter = getDistanceAndPlayTone(); + unsigned int tSpeed; + /* + * TODO check if distance too high, then search for target in a different direction + */ - if (tCentimeter > 30) { + if (tCentimeter > DISTANCE_MAXIMUM_CENTIMETER) { /* - * TODO check if distance too high, then search person in another direction + * Target too far -> drive forward with speed proportional to the gap */ -#ifndef PLOTTER_OUTPUT - Serial.println(F("Go forward")); + tSpeed = START_SPEED + (tCentimeter - DISTANCE_MAXIMUM_CENTIMETER) * 2; + if (tSpeed > MAX_SPEED_FOLLOWER) { + tSpeed = MAX_SPEED_FOLLOWER; + } +#ifdef PLOTTER_OUTPUT + Serial.print(tSpeed); +#else + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_FORWARD) { + Serial.println(F("Go forward")); + } + Serial.print(F("Speed=")); + Serial.print(tSpeed); #endif - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD); - } else if (tCentimeter < 22) { -#ifndef PLOTTER_OUTPUT - Serial.println(F("Go backward")); +#ifdef USE_ENCODER_MOTOR_CONTROL + RobotCarMotorControl.startGoDistanceCentimeter(tSpeed, (tCentimeter - DISTANCE_MAXIMUM_CENTIMETER) + DISTANCE_DELTA_CENTIMETER / 2, + DIRECTION_FORWARD); +#else + RobotCarMotorControl.setSpeedCompensated(tSpeed, DIRECTION_FORWARD); +#endif + + } else if (tCentimeter < DISTANCE_MINIMUM_CENTIMETER) { + /* + * Target too close -> drive backwards + */ + tSpeed = START_SPEED + (DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; + if (tSpeed > MAX_SPEED_FOLLOWER) { + tSpeed = MAX_SPEED_FOLLOWER; + } +#ifdef PLOTTER_OUTPUT + Serial.print(tSpeed); +#else + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_BACKWARD) { + Serial.println(F("Go backward")); + } + Serial.print(F("Speed=")); + Serial.print(tSpeed); +#endif +#ifdef USE_ENCODER_MOTOR_CONTROL + RobotCarMotorControl.startGoDistanceCentimeter(tSpeed, (DISTANCE_MINIMUM_CENTIMETER - tCentimeter) + DISTANCE_DELTA_CENTIMETER / 2, + DIRECTION_BACKWARD); +#else + RobotCarMotorControl.setSpeedCompensated(tSpeed, DIRECTION_BACKWARD); #endif - RobotCarMotorControl.initRampUpAndWaitForDriveSpeed(DIRECTION_BACKWARD); } else { - sFoundPerson = true; + /* + * Target is in the right distance -> stop once + */ + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != MOTOR_RELEASE) { #ifndef PLOTTER_OUTPUT - Serial.println(F("Stop")); + Serial.print(F("Stop")); #endif - RobotCarMotorControl.stopMotors(false); + RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + } } - delay(40); // the IR sensor takes 39 ms for one measurement + + Serial.println(); +#ifdef USE_ENCODER_MOTOR_CONTROL + RobotCarMotorControl.delayAndUpdateMotors(1000); +#else + delay(100); +#endif } unsigned int getDistanceAndPlayTone() { @@ -122,7 +198,8 @@ unsigned int getDistanceAndPlayTone() { */ unsigned int tCentimeter = getUSDistanceAsCentiMeter(); #ifdef PLOTTER_OUTPUT - Serial.println(tCentimeter); + Serial.print(tCentimeter); + Serial.print(' '); #else Serial.print("Distance="); Serial.print(tCentimeter); diff --git a/examples/RobotCarFollowerSimple/HCSR04.cpp b/examples/RobotCarFollowerSimple/HCSR04.cpp new file mode 100644 index 0000000..75c50cf --- /dev/null +++ b/examples/RobotCarFollowerSimple/HCSR04.cpp @@ -0,0 +1,300 @@ +/* + * HCSR04.cpp + * + * US Sensor (HC-SR04) functions. + * The non blocking functions are using pin change interrupts and need the PinChangeInterrupt library to be installed. + * + * Supports 1 Pin mode as you get on the HY-SRF05 if you connect OUT to ground. + * You can modify the HC-SR04 modules to 1 Pin mode by: + * Old module with 3 16 pin chips: Connect Trigger and Echo direct or use a resistor < 4.7 kOhm. + * If you remove both 10 kOhm pullup resistors you can use a connecting resistor < 47 kOhm, but I suggest to use 10 kOhm which is more reliable. + * Old module with 3 16 pin chips but with no pullup resistors near the connector row: Connect Trigger and Echo with a resistor > 200 Ohm. Use 10 kOhm. + * New module with 1 16 pin and 2 8 pin chips: Connect Trigger and Echo by a resistor > 200 Ohm and < 22 kOhm. + * All modules: Connect Trigger and Echo by a resistor of 4.7 kOhm. + * Some old HY-SRF05 modules of mine cannot be converted by adding a 4.7 kOhm resistor, + * since the output signal going low triggers the next measurement. But they work with removing the 10 kOhm pull up resistors and adding 10 kOhm. + * + * Sensitivity is increased by removing C3 / the low pass part of the 22 kHz Bandpass filter. + * After this the crosstalking of the output signal will be detected as a low distance. We can avoid this by changing R7 to 0 Ohm. + * + * Module Type | Characteristics | 3 Pin Mode | Increase sensitivity + * ------------------------------------------------------------------------------------------------------------ + * 3 * 14 pin IC's 2 transistors | C2 below right IC/U2 | 10 kOhm pin 1+2 middle IC | not needed, because of Max232 + * | right IC is Max232 | | + * 3 * 14 pin IC's 2 transistors | Transistor between | | -C2, R11=1.5MOhm, R12=0 + * | middle and right IC | | + * 3 * 14 pin IC's | R17 below right IC | 10 kOhm pin 1+2 middle IC | + * 1*4 2*8 pin IC's | | 10 kOhm pin 3+4 right IC | -C4, R7=1.5MOhm, R10=0 + * HY-SRF05 3 * 14 pin IC's | | 10 kOhm pin 1+2 middle IC | - bottom left C, R16=1.5MOhm, R15=? + * + * The CS100A module is not very sensitive at short or mid range but can detect up to 3m. Smallest distance is 2 cm. + * The amplified analog signal is available at pin 5 and the comparator output at pin 6. There you can see other echoes. + * 3 Pin mode is difficult since it retriggers itself at distances below 7 cm. + * + * Copyright (C) 2018-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 . + * + */ + +#include +#include "HCSR04.h" + +//#define DEBUG + +uint8_t sTriggerOutPin; // also used as aTriggerOutEchoInPin for 1 pin mode +uint8_t sEchoInPin; + +uint8_t sHCSR04Mode = HCSR04_MODE_UNITITIALIZED; + +/* + * @param aEchoInPin - If 0 then assume 1 pin mode + */ +void initUSDistancePins(uint8_t aTriggerOutPin, uint8_t aEchoInPin) { + sTriggerOutPin = aTriggerOutPin; + if (aEchoInPin == 0) { + sHCSR04Mode = HCSR04_MODE_USE_1_PIN; + } else { + sEchoInPin = aEchoInPin; + pinMode(aTriggerOutPin, OUTPUT); + pinMode(sEchoInPin, INPUT); + sHCSR04Mode = HCSR04_MODE_USE_2_PINS; + } +} +/* + * Using this determines one pin mode + */ +void initUSDistancePin(uint8_t aTriggerOutEchoInPin) { + sTriggerOutPin = aTriggerOutEchoInPin; + sHCSR04Mode = HCSR04_MODE_USE_1_PIN; +} + +/* + * Start of standard blocking implementation using pulseInLong() since PulseIn gives wrong (too small) results :-( + */ +unsigned int getUSDistance(unsigned int aTimeoutMicros) { + if (sHCSR04Mode == HCSR04_MODE_UNITITIALIZED) { + return 0; + } + +// need minimum 10 usec Trigger Pulse + digitalWrite(sTriggerOutPin, HIGH); + + if (sHCSR04Mode == HCSR04_MODE_USE_1_PIN) { + // do it AFTER digitalWrite to avoid spurious triggering by just switching pin to output + pinMode(sTriggerOutPin, OUTPUT); + } + +#ifdef DEBUG + delayMicroseconds(100); // to see it on scope +#else + delayMicroseconds(10); +#endif +// falling edge starts measurement after 400/600 microseconds (old/new modules) + digitalWrite(sTriggerOutPin, LOW); + + 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. + pinMode(sTriggerOutPin, INPUT); + tEchoInPin = sTriggerOutPin; + } else { + tEchoInPin = sEchoInPin; + } + + /* + * Get echo length. + * Speed of sound is: 331.5 + (0.6 * TemperatureCelsius). + * Exact value at 20 degree celsius is 343,46 m/s => 58,23 us per centimeter and 17,17 cm/ms (forth and back) + * Exact value at 10 degree celsius is 337,54 m/s => 59,25 us per centimeter and 16,877 cm/ms (forth and back) + * At 20 degree celsius => 50cm gives 2914 us, 2m gives 11655 us + * + * Use pulseInLong, this uses micros() as counter, relying on interrupts being enabled, which is not disturbed by (e.g. the 1 ms timer) interrupts. + * Only thing is that the pulse ends when we are in an interrupt routine, thus prolonging the measured pulse duration. + * Alternatively we can use pulseIn() in a noInterrupts() context, but this will effectively stop the millis() timer for duration of pulse / or timeout. + */ +#if ! defined(__AVR__) || defined(TEENSYDUINO) || defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) || defined(__AVR_ATtiny87__) || defined(__AVR_ATtiny167__) + noInterrupts(); + unsigned long tUSPulseMicros = pulseIn(tEchoInPin, HIGH, aTimeoutMicros); + interrupts(); +#else + unsigned long tUSPulseMicros = pulseInLong(tEchoInPin, HIGH, aTimeoutMicros); +#endif + if (tUSPulseMicros == 0) { +// timeout happened -> change value to timeout value. This eases comparison with different distances. + tUSPulseMicros = aTimeoutMicros; + } + return tUSPulseMicros; +} + +unsigned int getCentimeterFromUSMicroSeconds(unsigned int aDistanceMicros) { + // The reciprocal of formula in getUSDistanceAsCentiMeterWithCentimeterTimeout() + return (aDistanceMicros * 100L) / 5825; +} + +/* + * @return Distance in centimeter @20 degree (time in us/58.25) + * aTimeoutMicros/58.25 if timeout happens + * 0 if pins are not initialized + * timeout of 5825 micros is equivalent to 1 meter + * Default timeout of 20000 micro seconds is 3.43 meter + */ +unsigned int getUSDistanceAsCentiMeter(unsigned int aTimeoutMicros) { + unsigned int tDistanceMicros = getUSDistance(aTimeoutMicros); + if (tDistanceMicros == 0) { +// timeout happened + tDistanceMicros = aTimeoutMicros; + } + return (getCentimeterFromUSMicroSeconds(tDistanceMicros)); +} + +// 58,23 us per centimeter (forth and back) +unsigned int getUSDistanceAsCentiMeterWithCentimeterTimeout(unsigned int aTimeoutCentimeter) { +// The reciprocal of formula in getCentimeterFromUSMicroSeconds() + unsigned int tTimeoutMicros = ((aTimeoutCentimeter * 233L) + 2) / 4; // = * 58.25 (rounded by using +1) + return getUSDistanceAsCentiMeter(tTimeoutMicros); +} + +void testUSSensor(uint16_t aSecondsToTest) { + for (long i = 0; i < aSecondsToTest * 50; ++i) { + digitalWrite(sTriggerOutPin, HIGH); + delayMicroseconds(582); // pulse is as long as echo for 10 cm + // falling edge starts measurement + digitalWrite(sTriggerOutPin, LOW); + delay(20); // wait time for 3,43 meter to let the US pulse vanish + } +} + +/* + * The NON BLOCKING version only blocks for ca. 12 microseconds for code + generation of trigger pulse + * Be sure to have the right interrupt vector below. + * check with: while (!isUSDistanceMeasureFinished()) { }; + * Result is in sUSDistanceCentimeter; + */ + +// Comment out 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 +//#define USE_PIN_CHANGE_INTERRUPT_A0_TO_A5 // using PCINT1_vect - PORT C +#if (defined(USE_PIN_CHANGE_INTERRUPT_D0_TO_D7) | defined(USE_PIN_CHANGE_INTERRUPT_D8_TO_D13) | defined(USE_PIN_CHANGE_INTERRUPT_A0_TO_A5)) + +unsigned int sUSDistanceCentimeter; +volatile unsigned long sUSPulseMicros; + +volatile bool sUSValueIsValid = false; +volatile unsigned long sMicrosAtStartOfPulse; +unsigned int sTimeoutMicros; + +/* + * common code for all interrupt handler. + */ +void handlePCInterrupt(uint8_t aPortState) { + if (aPortState > 0) { + // start of pulse + sMicrosAtStartOfPulse = micros(); + } else { + // end of pulse + sUSPulseMicros = micros() - sMicrosAtStartOfPulse; + sUSValueIsValid = true; + } +#ifdef DEBUG +// for debugging purposes, echo to PIN 13 (do not forget to set it to OUTPUT!) +// digitalWrite(13, aPortState); +#endif +} +#endif // USE_PIN_CHANGE_INTERRUPT_D0_TO_D7 ... + +#if defined(USE_PIN_CHANGE_INTERRUPT_D0_TO_D7) +/* + * pin change interrupt for D0 to D7 here. + */ +ISR (PCINT2_vect) { +// read pin + uint8_t tPortState = (*portInputRegister(digitalPinToPort(sEchoInPin))) & bit((digitalPinToPCMSKbit(sEchoInPin))); + handlePCInterrupt(tPortState); +} +#endif + +#if defined(USE_PIN_CHANGE_INTERRUPT_D8_TO_D13) +/* + * pin change interrupt for D8 to D13 here. + * state of pin is echoed to output 13 for debugging purpose + */ +ISR (PCINT0_vect) { +// check pin + uint8_t tPortState = (*portInputRegister(digitalPinToPort(sEchoInPin))) & bit((digitalPinToPCMSKbit(sEchoInPin))); + handlePCInterrupt(tPortState); +} +#endif + +#if defined(USE_PIN_CHANGE_INTERRUPT_A0_TO_A5) +/* + * pin change interrupt for A0 to A5 here. + * state of pin is echoed to output 13 for debugging purpose + */ +ISR (PCINT1_vect) { +// check pin + uint8_t tPortState = (*portInputRegister(digitalPinToPort(sEchoInPin))) & bit((digitalPinToPCMSKbit(sEchoInPin))); + handlePCInterrupt(tPortState); +} +#endif + +#if (defined(USE_PIN_CHANGE_INTERRUPT_D0_TO_D7) | defined(USE_PIN_CHANGE_INTERRUPT_D8_TO_D13) | defined(USE_PIN_CHANGE_INTERRUPT_A0_TO_A5)) + +void startUSDistanceAsCentiMeterWithCentimeterTimeoutNonBlocking(unsigned int aTimeoutCentimeter) { +// need minimum 10 usec Trigger Pulse + digitalWrite(sTriggerOutPin, HIGH); + sUSValueIsValid = false; + sTimeoutMicros = ((aTimeoutCentimeter * 233) + 2) / 4; // = * 58.25 (rounded by using +1) + *digitalPinToPCMSK(sEchoInPin) |= bit(digitalPinToPCMSKbit(sEchoInPin));// enable pin for pin change interrupt +// the 2 registers exists only once! + PCICR |= bit(digitalPinToPCICRbit(sEchoInPin));// enable interrupt for the group + PCIFR |= bit(digitalPinToPCICRbit(sEchoInPin));// clear any outstanding interrupt + sUSPulseMicros = 0; + sMicrosAtStartOfPulse = 0; + +#ifdef DEBUG + delay(2); // to see it on scope +#else + delayMicroseconds(10); +#endif +// falling edge starts measurement and generates first interrupt + digitalWrite(sTriggerOutPin, LOW); +} + +/* + * Used to check by polling. + * If ISR interrupts these code, everything is fine, even if we get a timeout and a no null result + * since we are interested in the result and not in very exact interpreting of the timeout. + */ +bool isUSDistanceMeasureFinished() { + if (sUSValueIsValid) { + sUSDistanceCentimeter = getCentimeterFromUSMicroSeconds(sUSPulseMicros); + return true; + } + + if (sMicrosAtStartOfPulse != 0) { + if ((micros() - sMicrosAtStartOfPulse) >= sTimeoutMicros) { + // Timeout happened, value will be 0 + *digitalPinToPCMSK(sEchoInPin) &= ~(bit(digitalPinToPCMSKbit(sEchoInPin)));// disable pin for pin change interrupt + return true; + } + } + return false; +} +#endif // USE_PIN_CHANGE_INTERRUPT_D0_TO_D7 ... diff --git a/examples/RobotCarFollowerSimple/HCSR04.h b/examples/RobotCarFollowerSimple/HCSR04.h new file mode 100644 index 0000000..8dcdb05 --- /dev/null +++ b/examples/RobotCarFollowerSimple/HCSR04.h @@ -0,0 +1,68 @@ +/* + * HCSR04.h + * + * Supports 1 Pin mode as you get on the HY-SRF05 if you connect OUT to ground. + * You can modify the HC-SR04 modules to 1 Pin mode by: + * Old module with 3 16 pin chips: Connect Trigger and Echo direct or use a resistor < 4.7 kOhm. + * If you remove both 10 kOhm pullup resistor you can use a connecting resistor < 47 kOhm, but I suggest to use 10 kOhm which is more reliable. + * Old module with 3 16 pin chips but with no pullup resistors near the connector row: Connect Trigger and Echo with a resistor > 200 Ohm. Use 10 kOhm. + * New module with 1 16 pin and 2 8 pin chips: Connect Trigger and Echo by a resistor > 200 Ohm and < 22 kOhm. + * All modules: Connect Trigger and Echo by a resistor of 4.7 kOhm. + * + * Copyright (C) 2018-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 HCSR04_H_ +#define HCSR04_H_ + +#include + +#define US_DISTANCE_DEFAULT_TIMEOUT_MICROS 20000 +#define US_DISTANCE_TIMEOUT_MICROS_FOR_1_METER 5825 // Timeout of 5825 is 1 meter +#define US_DISTANCE_TIMEOUT_MICROS_FOR_2_METER 11650 // Timeout of 11650 is 2 meter +#define US_DISTANCE_TIMEOUT_MICROS_FOR_3_METER 17475 // Timeout of 17475 is 3 meter +#define US_DISTANCE_DEFAULT_TIMEOUT_CENTIMETER 343 // Timeout of 20000L is 3.43 meter + +void initUSDistancePins(uint8_t aTriggerOutPin, uint8_t aEchoInPin = 0); +void initUSDistancePin(uint8_t aTriggerOutEchoInPin); // Using this determines one pin mode +unsigned int getUSDistance(unsigned int aTimeoutMicros = US_DISTANCE_DEFAULT_TIMEOUT_MICROS); +unsigned int getCentimeterFromUSMicroSeconds(unsigned int aDistanceMicros); +unsigned int getUSDistanceAsCentiMeter(unsigned int aTimeoutMicros = US_DISTANCE_DEFAULT_TIMEOUT_MICROS); +unsigned int getUSDistanceAsCentiMeterWithCentimeterTimeout(unsigned int aTimeoutCentimeter); +void testUSSensor(uint16_t aSecondsToTest); + +#if (defined(USE_PIN_CHANGE_INTERRUPT_D0_TO_D7) | defined(USE_PIN_CHANGE_INTERRUPT_D8_TO_D13) | defined(USE_PIN_CHANGE_INTERRUPT_A0_TO_A5)) +/* + * Non blocking version + */ +void startUSDistanceAsCentiMeterWithCentimeterTimeoutNonBlocking(unsigned int aTimeoutCentimeter); +bool isUSDistanceMeasureFinished(); +extern unsigned int sUSDistanceCentimeter; +extern volatile unsigned long sUSPulseMicros; +#endif + +#define HCSR04_MODE_UNITITIALIZED 0 +#define HCSR04_MODE_USE_1_PIN 1 +#define HCSR04_MODE_USE_2_PINS 2 +extern uint8_t sHCSR04Mode; + +#endif // HCSR04_H_ + +#pragma once diff --git a/examples/RobotCarFollowerSimple/RobotCarFollowerSimple.ino b/examples/RobotCarFollowerSimple/RobotCarFollowerSimple.ino new file mode 100644 index 0000000..fc42e34 --- /dev/null +++ b/examples/RobotCarFollowerSimple/RobotCarFollowerSimple.ino @@ -0,0 +1,164 @@ +/* + * RobotCarFollowerSimple.cpp + * + * Enables follower mode driving of a 2 or 4 wheel car with an Arduino and a dual full bridge (e.g. TB6612 or L298) for motor control. + * + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-RobotCar 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" +#include "Servo.h" +#include "HCSR04.h" + +#define DISTANCE_MINIMUM_CENTIMETER 20 // If measured distance is less than this value, go backwards +#define DISTANCE_MAXIMUM_CENTIMETER 30 // If measured distance is greater than this value, go forward + +// Values for 4xAA / 6.0 volt +//#define START_SPEED 80 // Speed PWM value at which car starts to move. +//#define DRIVE_SPEED 150 // Speed PWM value used for going fixed distance. +//#define MAX_SPEED_FOLLOWER 255 // Max speed PWM value used for follower. +// values for 2xLIPO / 7.4 volt +#define START_SPEED 30 // Speed PWM value at which car starts to move. +#define DRIVE_SPEED 60 // Speed PWM value used for going fixed distance. +#define MAX_SPEED_FOLLOWER 100 // Max speed PWM value used for follower. + +#define SPEED_COMPENSATION_RIGHT 0 // If positive, this value is subtracted from the speed of the right motor, if negative, -value is subtracted from the left speed. + +/* + * Pins for direct motor control with PWM and a dual full bridge e.g. TB6612 or L298. + * Pins 9 + 10 are reserved for Servo + * 2 + 3 are reserved for encoder input + */ +#define PIN_LEFT_MOTOR_FORWARD 4 +#define PIN_LEFT_MOTOR_BACKWARD 7 +#define PIN_LEFT_MOTOR_PWM 5 // Must be PWM capable + +#define PIN_RIGHT_MOTOR_FORWARD 8 +#define PIN_RIGHT_MOTOR_BACKWARD 12 // Pin 9 is already reserved for distance servo +#define PIN_RIGHT_MOTOR_PWM 6 // Must be PWM capable + +#define PIN_DISTANCE_SERVO 9 // Servo Nr. 2 on Adafruit Motor Shield + +#define PIN_SPEAKER 11 + +#define PIN_TRIGGER_OUT A0 // Connections on the Arduino Sensor Shield +#define PIN_ECHO_IN A1 + +//Car Control +CarMotorControl RobotCarMotorControl; +Servo DistanceServo; + +unsigned int getDistanceAndPlayTone(); + +/* + * Start of robot car control program + */ +void setup() { + Serial.begin(115200); + + // Just to know which program is running on my Arduino + Serial.println(F("START " __FILE__ " from " __DATE__ "\r\nUsing library version " VERSION_PWMMOTORCONTROL)); + + 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); + + /* + * Set US servo to forward position + */ + DistanceServo.attach(PIN_DISTANCE_SERVO); + DistanceServo.write(90); + + initUSDistancePins(PIN_TRIGGER_OUT, PIN_ECHO_IN); + + /* + * Do not start immediately with driving + */ + delay(5000); + + /* + * Tone feedback for start of driving + */ + tone(PIN_SPEAKER, 2200, 100); + delay(200); + tone(PIN_SPEAKER, 2200, 100); +} + +void loop() { + + unsigned int tCentimeter = getDistanceAndPlayTone(); + unsigned int tSpeed; + + if (tCentimeter > DISTANCE_MAXIMUM_CENTIMETER) { + /* + * Target too far -> drive forward with speed proportional to the gap + */ + tSpeed = START_SPEED + (tCentimeter - DISTANCE_MAXIMUM_CENTIMETER) * 2; + if (tSpeed > MAX_SPEED_FOLLOWER) { + tSpeed = MAX_SPEED_FOLLOWER; + } + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_FORWARD) { + Serial.println(F("Go forward")); + } + Serial.print(F("Speed=")); + Serial.println(tSpeed); + + RobotCarMotorControl.setSpeedCompensated(tSpeed, DIRECTION_FORWARD); + + } else if (tCentimeter < DISTANCE_MINIMUM_CENTIMETER) { + /* + * Target too close -> drive backwards + */ + tSpeed = START_SPEED + (DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; + if (tSpeed > MAX_SPEED_FOLLOWER) { + tSpeed = MAX_SPEED_FOLLOWER; + } + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_BACKWARD) { + Serial.println(F("Go backward")); + } + Serial.print(F("Speed=")); + Serial.println(tSpeed); + + RobotCarMotorControl.setSpeedCompensated(tSpeed, DIRECTION_BACKWARD); + + } else { + /* + * Target is in the right distance -> stop once + */ + if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != MOTOR_RELEASE) { + Serial.println(F("Stop")); + RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + } + } + + delay(100); +} + +unsigned int getDistanceAndPlayTone() { + /* + * Get distance + */ + unsigned int tCentimeter = getUSDistanceAsCentiMeter(); + Serial.print("Distance="); + Serial.print(tCentimeter); + Serial.print("cm. "); + /* + * Play tone + */ + int tFrequency = map(tCentimeter, 0, 100, 110, 1760); // 4 octaves per meter + tone(PIN_SPEAKER, tFrequency); + return tCentimeter; +} diff --git a/examples/Square/Square.ino b/examples/Square/Square.ino new file mode 100644 index 0000000..2682af6 --- /dev/null +++ b/examples/Square/Square.ino @@ -0,0 +1,94 @@ +/* + * Square.cpp + * Example for driving a 50 cm square using CarMotorControl class + * + * Created on: 19.09.2020 + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-RobotCar 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" + +#if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) // enable / disable it in PWMDCMotor.h +/* + * Pins for direct motor control with PWM and a dual full bridge e.g. TB6612 or L298. + * Pins 9 + 10 are already used for Servo + * 2 + 3 are already used for encoder input + */ +#define PIN_LEFT_MOTOR_FORWARD 4 +#define PIN_LEFT_MOTOR_BACKWARD 7 +#define PIN_LEFT_MOTOR_PWM 5 // Must be PWM capable + +#define PIN_RIGHT_MOTOR_FORWARD 8 +#define PIN_RIGHT_MOTOR_BACKWARD 12 // Pin 9 is already reserved for distance servo +#define PIN_RIGHT_MOTOR_PWM 6 // Must be PWM capable +#endif + +CarMotorControl RobotCarMotorControl; +#define SIZE_OF_SQUARE 50 + +void setup() { +// initialize the digital pin as an output. + pinMode(LED_BUILTIN, OUTPUT); + + Serial.begin(115200); + +#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 +#endif + // Just to know which program is running on my Arduino + Serial.println(F("START " __FILE__ " from " __DATE__ "\r\nUsing library version " VERSION_PWMMOTORCONTROL)); + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + // For Adafruit Motor Shield v2 + RobotCarMotorControl.init(); +#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); +#endif + + /* + * You will need to change these values according to your motor, wheels and motor supply voltage. + */ + RobotCarMotorControl.setValuesForFixedDistanceDriving(DEFAULT_START_SPEED, DEFAULT_DRIVE_SPEED, 0); // Set compensation to 0 + // set Factor for 2 LIPOS + RobotCarMotorControl.setDistanceToTimeFactorForFixedDistanceDriving(DEFAULT_DISTANCE_TO_TIME_FACTOR); +#if defined(CAR_HAS_4_WHEELS) + RobotCarMotorControl.setFactorDegreeToCount(FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT); +#else + RobotCarMotorControl.setFactorDegreeToCount(FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT); +#endif + delay(2000); +} + +void loop() { + static uint8_t sMotorDirection = DIRECTION_FORWARD; + + /* + * Try the default minimum speed (from PWMDCMotor.h), at which the motor starts to move. + */ + RobotCarMotorControl.goDistanceCentimeter(SIZE_OF_SQUARE, sMotorDirection); + delay(1000); // wait for a second + /* + * Now set speed to the default maximum speed (from PWMDCMotor.h), at which the motor moves for fixed distance driving. + */ + RobotCarMotorControl.rotateCar(90, sMotorDirection); + + /* + * switch direction + */ + sMotorDirection = oppositeDIRECTION(sMotorDirection); + delay(5000); +} diff --git a/examples/Start/Start.ino b/examples/Start/Start.ino new file mode 100644 index 0000000..2b4a681 --- /dev/null +++ b/examples/Start/Start.ino @@ -0,0 +1,115 @@ +/* + * Start.cpp + * Example for controlling 2 motors without using CarMotorControl class + * + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-RobotCar 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" + +#if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) // enable / disable it in PWMDCMotor.h +/* + * Pins for direct motor control with PWM and a dual full bridge e.g. TB6612 or L298. + * Pins 9 + 10 are already used for Servo + * 2 + 3 are already used for encoder input + */ +#define PIN_LEFT_MOTOR_FORWARD 4 +#define PIN_LEFT_MOTOR_BACKWARD 7 +#define PIN_LEFT_MOTOR_PWM 5 // Must be PWM capable + +#define PIN_RIGHT_MOTOR_FORWARD 8 +#define PIN_RIGHT_MOTOR_BACKWARD 12 // Pin 9 is already reserved for distance servo +#define PIN_RIGHT_MOTOR_PWM 6 // Must be PWM capable +#endif + +PWMDcMotor rightMotor; +PWMDcMotor leftMotor; + +void setup() { +// initialize the digital pin as an output. + pinMode(LED_BUILTIN, OUTPUT); + + Serial.begin(115200); + +#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 +#endif + // Just to know which program is running on my Arduino + Serial.println(F("START " __FILE__ " from " __DATE__ "\r\nUsing library version " VERSION_PWMMOTORCONTROL)); + + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + // For Adafruit Motor Shield v2 + leftMotor.init(1); + rightMotor.init(2); +#else + leftMotor.init(PIN_LEFT_MOTOR_FORWARD, PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM); + rightMotor.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM); +#endif + + /* + * You will need to change these values according to your motor, wheels and motor supply voltage. + */ + leftMotor.setValuesForFixedDistanceDriving(DEFAULT_START_SPEED, DEFAULT_DRIVE_SPEED, 0); // Set compensation to 0 + rightMotor.setValuesForFixedDistanceDriving(DEFAULT_START_SPEED, DEFAULT_DRIVE_SPEED, 0); + // set Factor for 2 LIPOS + leftMotor.setDistanceToTimeFactorForFixedDistanceDriving(DEFAULT_DISTANCE_TO_TIME_FACTOR); + rightMotor.setDistanceToTimeFactorForFixedDistanceDriving(DEFAULT_DISTANCE_TO_TIME_FACTOR); + + delay(2000); +} + +void loop() { + static uint8_t sMotorDirection = DIRECTION_FORWARD; + + /* + * Try the default minimum speed (from PWMDCMotor.h), at which the motor starts to move. + */ + rightMotor.setSpeed(DEFAULT_START_SPEED, sMotorDirection); + delay(1000); // wait for a second + /* + * Now set speed to the default maximum speed (from PWMDCMotor.h), at which the motor moves for fixed distance driving. + */ + rightMotor.setSpeed(DEFAULT_DRIVE_SPEED, sMotorDirection); + delay(1000); // wait for a second + /* + * Stop motor + */ + rightMotor.stop(); + delay(2000); // wait for a second + /* + * Try to go a whole turn (21.5 cm for my wheels) + */ + rightMotor.goDistanceCount(DEFAULT_COUNTS_PER_FULL_ROTATION, sMotorDirection); + delay(2000); + + /* + * Run left motor + */ + leftMotor.setSpeed(DEFAULT_START_SPEED, sMotorDirection); + delay(1000); + leftMotor.setSpeed(DEFAULT_DRIVE_SPEED, sMotorDirection); + delay(1000); + leftMotor.stop(); + delay(2000); + leftMotor.goDistanceCount(DEFAULT_COUNTS_PER_FULL_ROTATION, sMotorDirection); + + /* + * switch direction + */ + sMotorDirection = oppositeDIRECTION(sMotorDirection); + delay(5000); +} diff --git a/keywords.txt b/keywords.txt index 9991129..e286d1b 100644 --- a/keywords.txt +++ b/keywords.txt @@ -12,30 +12,52 @@ CarMotorControl KEYWORD1 ####################################### # Methods and Functions (KEYWORD2) ####################################### +# From PWMDcMotor init KEYWORD2 -setMotorDriverMode KEYWORD2 setSpeed KEYWORD2 setSpeedCompensated KEYWORD2 stop KEYWORD2 setStopMode KEYWORD2 -initGoDistanceCount KEYWORD2 +setDefaultsForFixedDistanceDriving KEYWORD2 +setValuesForFixedDistanceDriving KEYWORD2 +setDistanceToTimeFactorForFixedDistanceDriving KEYWORD2 +setDriveSpeed KEYWORD2 + +startGoDistanceCount KEYWORD2 +startRampUp KEYWORD2 updateMotor KEYWORD2 -setEepromMotorValuesDefaults KEYWORD2 +goDistanceCount KEYWORD2 + readMotorValuesFromEeprom KEYWORD2 writeMotorvaluesToEeprom KEYWORD2 -initGoDistanceCount KEYWORD2 -synchronizeMotor KEYWORD2 +setMotorDriverMode KEYWORD2 +checkAndHandleDirectionChange KEYWORD2 + +# From EncoderMotor resetControlValues KEYWORD2 -initGoDistanceCentimeter KEYWORD2 +synchronizeMotor KEYWORD2 +calibrate KEYWORD2 + +# From CarMotorControl +startGoDistanceCentimeter KEYWORD2 goDistanceCentimeter KEYWORD2 -stopMotors KEYWORD2 -setDefaultsForFixedDistanceDriving KEYWORD2 +getCarDirectionOrBrakeMode KEYWORD2 +waitForDriveSpeed KEYWORD2 +startRampUpAndWait KEYWORD2 +startRampUpAndWaitForDriveSpeed KEYWORD2 +checkAndHandleDirectionChange KEYWORD2 updateMotors KEYWORD2 +delayAndUpdateMotors KEYWORD2 startCarAndWaitForFullSpeed KEYWORD2 +stopMotors KEYWORD2 stopCarAndWaitForIt KEYWORD2 waitUntilCarStopped KEYWORD2 isStopped KEYWORD2 -initRotateCar KEYWORD2 +isState KEYWORD2 +isStateRamp KEYWORD2 +# Rotation +setFactorDegreeToCount KEYWORD2 +startRotateCar KEYWORD2 rotateCar KEYWORD2 ####################################### diff --git a/library.json b/library.json index 497ff9f..a080826 100644 --- a/library.json +++ b/library.json @@ -6,7 +6,7 @@ "type": "git", "url": "https://github.com/ArminJo/PWMMotorControl" }, - "version": "1.0.0", + "version": "1.1.0", "exclude": "pictures", "authors": { "name": "Armin Joachimsmeyer", diff --git a/library.properties b/library.properties index 8035425..cabf982 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=PWMMotorControl -version=1.0.0 +version=1.1.0 author=Armin Joachimsmeyer maintainer=Armin Joachimsmeyer sentence=Control brushed DC motors by PWM and uses optional attached encoders to drive fixed distances. For L298 or TB6612, or Adafruit Motor Shield
-paragraph=With special CarMotorControl for easy control of 2 or 4 motors of the obstacle avoiding arduino robot car kits.
If forked light barriers are attached to the encoder discs of such a kit, deterministic distances and turns can be driven.
Examples include a follower car and an Bluetooth controlled obstacle avoiding car.

New: Initial version.
+paragraph=With special CarMotorControl for easy control of 2 or 4 motors of the obstacle avoiding arduino robot car kits.
If slot-type photo interrupters are attached to the encoder discs of such a kit, deterministic distances and turns can be driven.
Examples include a follower car and an Bluetooth controlled obstacle avoiding car.

New: Added and renamed functions.
category=Device Control url=https://github.com/ArminJo/PWMMotorControl -architectures=avr \ No newline at end of file +architectures=* \ No newline at end of file diff --git a/src/CarMotorControl.cpp b/src/CarMotorControl.cpp index 2db6734..062bf37 100644 --- a/src/CarMotorControl.cpp +++ b/src/CarMotorControl.cpp @@ -10,7 +10,7 @@ * Copyright (C) 2016-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -24,6 +24,8 @@ #include #include "CarMotorControl.h" +#define DEBUG // for development + CarMotorControl * sCarMotorControlPointerForISR; CarMotorControl::CarMotorControl() { // @suppress("Class members should be properly initialized") @@ -92,10 +94,44 @@ void CarMotorControl::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint } } +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); } @@ -104,6 +140,7 @@ void CarMotorControl::setSpeed(uint8_t aRequestedSpeed, uint8_t aRequestedDirect * 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); } @@ -124,11 +161,16 @@ void CarMotorControl::setSpeedCompensated(int aRequestedSpeed) { leftCarMotor.setSpeedCompensated(aRequestedSpeed); } +uint8_t CarMotorControl::getCarDirectionOrBrakeMode() { + return CarDirectionOrBrakeMode;; +} + /* * 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); } @@ -156,7 +198,7 @@ void CarMotorControl::resetControlValues() { * 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::needsFastUpdates() { +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); @@ -174,21 +216,36 @@ bool CarMotorControl::updateMotors() { return tMotorsNotStopped; } +void CarMotorControl::delayAndUpdateMotors(unsigned int aDelayMillis) { + uint32_t tStartMillis = millis(); + do { + updateMotors(); + } while (millis() - tStartMillis <= aDelayMillis); +} + #ifdef SUPPORT_RAMP_UP -void CarMotorControl::initRampUp(uint8_t aRequestedDirection) { - rightCarMotor.initRampUp(aRequestedDirection); - leftCarMotor.initRampUp(aRequestedDirection); +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. + * Blocking wait until both motors are at drive speed. 256 milliseconds for ramp up. */ -void CarMotorControl::waitForDriveSpeed() { - /* - * blocking wait for MOTOR_STATE_DRIVE_SPEED - */ +void CarMotorControl::waitForDriveSpeed(void (*aLoopCallback)(void)) { + bool tMotorsNotStopped; do { + if (aLoopCallback != NULL) { + aLoopCallback(); + } tMotorsNotStopped = rightCarMotor.updateMotor(); tMotorsNotStopped |= leftCarMotor.updateMotor(); } while (tMotorsNotStopped @@ -196,70 +253,89 @@ void CarMotorControl::waitForDriveSpeed() { } #endif -void CarMotorControl::initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { +/* + * 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 - rightCarMotor.initRampUp(aRequestedDirection); - leftCarMotor.initRampUp(aRequestedDirection); - /* - * blocking wait for MOTOR_STATE_DRIVE_SPEED, 256 milliseconds - */ - bool tMotorsNotStopped; // to check if motors are not stopped by aLoopCallback - do { - tMotorsNotStopped = rightCarMotor.updateMotor(); - tMotorsNotStopped |= leftCarMotor.updateMotor(); - if (aLoopCallback != NULL) { - aLoopCallback(); // this may stop motors - } - } while (tMotorsNotStopped - && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED)); + 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::initGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection) { - rightCarMotor.initGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); - leftCarMotor.initGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); +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)) { - initGoDistanceCentimeter(aDistanceCentimeter, aRequestedDirection); + startGoDistanceCentimeter(aDistanceCentimeter, aRequestedDirection); waitUntilCarStopped(aLoopCallback); } /* * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. */ -void CarMotorControl::initGoDistanceCentimeter(int aDistanceCentimeter) { - rightCarMotor.initGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); - leftCarMotor.initGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); +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)) { - initGoDistanceCentimeter(aDistanceCentimeter); + startGoDistanceCentimeter(aDistanceCentimeter); waitUntilCarStopped(aLoopCallback); } /* * Stop car with ramp and give DistanceCountAfterRampUp counts for braking. - * - * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN - * Use DistanceCountAfterRampUp as ramp down count - * Blocking wait for stop */ -void CarMotorControl::stopCarAndWaitForIt() { +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; @@ -267,10 +343,12 @@ void CarMotorControl::stopCarAndWaitForIt() { /* * blocking wait for stop */ - waitUntilCarStopped(); + waitUntilCarStopped(aLoopCallback); #else + (void) aLoopCallback; rightCarMotor.stop(); leftCarMotor.stop(); + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode #endif } @@ -285,6 +363,7 @@ void CarMotorControl::waitUntilCarStopped(void (*aLoopCallback)(void)) { aLoopCallback(); } } while (!isStopped()); + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode } bool CarMotorControl::isState(uint8_t aState) { @@ -310,10 +389,10 @@ void CarMotorControl::setFactorDegreeToCount(float aFactorDegreeToCount) { * @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::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { +void CarMotorControl::startRotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { int tDistanceCountRight; int tDistanceCountLeft; - uint16_t tDistanceCount; + unsigned int tDistanceCount; uint8_t tTurnDirectionRightMotor = aTurnDirection; // set turn direction as start direction uint8_t tTurnDirectionLeftMotor = aTurnDirection; @@ -338,7 +417,7 @@ void CarMotorControl::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirec 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 initGoDistanceCount() + // The right motor has turn direction TURN_IN_PLACE which is masked to TURN_FORWARD by startGoDistanceCount() tTurnDirectionLeftMotor = TURN_BACKWARD; } } else { @@ -358,8 +437,8 @@ void CarMotorControl::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirec } // This in turn sets CurrentDriveSpeed to DriveSpeed. - rightCarMotor.initGoDistanceCount(tDistanceCountRight, tTurnDirectionRightMotor); - leftCarMotor.initGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor); + rightCarMotor.startGoDistanceCount(tDistanceCountRight, tTurnDirectionRightMotor); + leftCarMotor.startGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor); if (aUseSlowSpeed) { // adjust CurrentDriveSpeed #if defined(SUPPORT_RAMP_UP) @@ -389,19 +468,32 @@ void CarMotorControl::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirec * @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(int16_t aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed, void (*aLoopCallback)(void)) { +void CarMotorControl::rotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed, void (*aLoopCallback)(void)) { if (aRotationDegrees != 0) { - initRotateCar(aRotationDegrees, aTurnDirection, aUseSlowSpeed); + startRotateCar(aRotationDegrees, aTurnDirection, aUseSlowSpeed); waitUntilCarStopped(aLoopCallback); } } #ifndef USE_ENCODER_MOTOR_CONTROL -void CarMotorControl::setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor) { +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 */ diff --git a/src/CarMotorControl.h b/src/CarMotorControl.h index b9406bd..152b501 100644 --- a/src/CarMotorControl.h +++ b/src/CarMotorControl.h @@ -7,7 +7,7 @@ * Copyright (C) 2016-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -59,45 +59,66 @@ class CarMotorControl { 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 setDriveSpeed(uint8_t aDriveSpeed); + #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(uint16_t aDistanceToTimeFactor); + void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); #endif #ifdef SUPPORT_RAMP_UP - void initRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); - void waitForDriveSpeed(); + void startRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); + void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); + void waitForDriveSpeed(void (*aLoopCallback)(void) = NULL); #endif - void initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL); + // 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 */ - void initGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection); // only setup values - void goDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilCarStopped // With signed distance - void initGoDistanceCentimeter(int aDistanceCentimeter); // only setup values + 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 initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = true); - void rotateCar(int16_t aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, bool aUseSlowSpeed = true, + 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(); // uses waitUntilCarStopped() - + void stopCarAndWaitForIt(void (*aLoopCallback)(void) = NULL); // uses waitUntilCarStopped() void waitUntilCarStopped(void (*aLoopCallback)(void) = NULL); /* @@ -105,7 +126,7 @@ class CarMotorControl { */ bool isStopped(); bool isState(uint8_t aState); - bool needsFastUpdates(); + bool isStateRamp(); // MOTOR_STATE_RAMP_UP OR MOTOR_STATE_RAMP_DOWN void resetControlValues(); @@ -120,8 +141,6 @@ class CarMotorControl { void setSpeed(int aRequestedSpeed); void setSpeedCompensated(int aRequestedSpeed); - float FactorDegreeToCount; - #ifdef USE_ENCODER_MOTOR_CONTROL EncoderMotor rightCarMotor; // 40 bytes RAM EncoderMotor leftCarMotor; @@ -135,3 +154,6 @@ class CarMotorControl { extern CarMotorControl * sCarMotorControlPointerForISR; #endif /* CARMOTORCONTROL_H_ */ + +#pragma once + diff --git a/src/EncoderMotor.cpp b/src/EncoderMotor.cpp index 1b5fd59..08186d3 100644 --- a/src/EncoderMotor.cpp +++ b/src/EncoderMotor.cpp @@ -1,7 +1,7 @@ /* * EncoderMotor.cpp * - * Functions for controlling a DC-motor which rotary encoder implemented by fork light barrier and an attached encoder disc (with 20 slots). + * Functions for controlling a DC-motor which rotary encoder implemented by slot-type photo interrupter and an attached encoder disc (with 20 slots). * Works with positive (unsigned) speed and direction or signed speed. * * Contains functions to go a specified distance. @@ -15,7 +15,7 @@ * Copyright (C) 2016-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -69,34 +69,31 @@ void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMP #endif /* - * If motor is still running, add aDistanceCount to current target distance + * If motor is already running, adjust TargetDistanceCount to go to aRequestedDistanceCount */ -void EncoderMotor::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection) { -// if (aDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { +void EncoderMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { +// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { // PanicWithLed(400, 22); // } - if (aDistanceCount == 0) { + if (aRequestedDistanceCount == 0) { return; } - checkAndHandleDirectionChange(aRequestedDirection); // this may reset MotorMovesFixedDistance - MotorMovesFixedDistance = true; - if (CurrentSpeed == 0) { - TargetDistanceCount = aDistanceCount; + TargetDistanceCount = aRequestedDistanceCount; #ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = DriveSpeed; + 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(DriveSpeed, aRequestedDirection); + setSpeedCompensated(aRequestedSpeed, aRequestedDirection); #endif } else { - TargetDistanceCount = EncoderCount + aDistanceCount; + TargetDistanceCount = EncoderCount + aRequestedDistanceCount; /* * prolong NextChangeMaxTargetCount for the new distance */ #ifdef SUPPORT_RAMP_UP MotorRampState = MOTOR_STATE_DRIVE_SPEED; - PWMDcMotor::setSpeed(CurrentDriveSpeed, aRequestedDirection); uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp; // guarantee minimal ramp down length if (tDistanceCountForRampDown < 3 && TargetDistanceCount > 6) { @@ -104,20 +101,26 @@ void EncoderMotor::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequest } NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown; #endif + PWMDcMotor::setSpeed(aRequestedSpeed, aRequestedDirection); } LastTargetDistanceCount = TargetDistanceCount; + MotorMovesFixedDistance = true; +} + +void EncoderMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { + startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); } /* - * if aDistanceCount < 0 then use DIRECTION_BACKWARD + * if aRequestedDistanceCount < 0 then use DIRECTION_BACKWARD * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. */ -void EncoderMotor::initGoDistanceCount(int16_t aDistanceCount) { - if (aDistanceCount < 0) { - aDistanceCount = -aDistanceCount; - initGoDistanceCount(aDistanceCount, DIRECTION_BACKWARD); +void EncoderMotor::startGoDistanceCount(int aRequestedDistanceCount) { + if (aRequestedDistanceCount < 0) { + aRequestedDistanceCount = -aRequestedDistanceCount; + startGoDistanceCount(aRequestedDistanceCount, DIRECTION_BACKWARD); } else { - initGoDistanceCount(aDistanceCount, DIRECTION_FORWARD); + startGoDistanceCount(aRequestedDistanceCount, DIRECTION_FORWARD); } } @@ -253,7 +256,7 @@ bool EncoderMotor::updateMotor() { * Computes motor speed compensation value in order to go exactly straight ahead * Compensate only at forward direction */ -void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t aCheckInterval) { +void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned int aCheckInterval) { if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) { return; } @@ -337,7 +340,7 @@ void EncoderMotor::resetControlValues() { ***************************************************/ void EncoderMotor::handleEncoderInterrupt() { long tMillis = millis(); - uint16_t tDeltaMillis = tMillis - EncoderTickLastMillis; + unsigned int tDeltaMillis = tMillis - EncoderTickLastMillis; if (tDeltaMillis <= ENCODER_SENSOR_MASK_MILLIS) { // signal is ringing CurrentVelocity = 99; @@ -421,11 +424,11 @@ bool EncoderMotor::updateAllMotors() { /* * Waits until distance is reached */ -void EncoderMotor::initRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { +void EncoderMotor::startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->initRampUp(aRequestedDirection); + tEncoderMotorControlPointer->startRampUp(aRequestedDirection); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } bool tMotorsNotStopped; // to check if motors are not stopped by aLoopCallback @@ -437,11 +440,11 @@ void EncoderMotor::initRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirect } while (tMotorsNotStopped && !EncoderMotor::allMotorsStarted()); } -void EncoderMotor::initGoDistanceCountForAll(int aDistanceCount) { +void EncoderMotor::startGoDistanceCountForAll(int aRequestedDistanceCount) { EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->initGoDistanceCount(aDistanceCount); + tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } } @@ -449,11 +452,11 @@ void EncoderMotor::initGoDistanceCountForAll(int aDistanceCount) { /* * Waits until distance is reached */ -void EncoderMotor::goDistanceCountForAll(int aDistanceCount, void (*aLoopCallback)(void)) { +void EncoderMotor::goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)) { EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->initGoDistanceCount(aDistanceCount); + tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } waitUntilAllMotorsStopped(aLoopCallback); diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h index a10c52a..c133415 100644 --- a/src/EncoderMotor.h +++ b/src/EncoderMotor.h @@ -5,7 +5,7 @@ * Copyright (C) 2016-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -22,7 +22,7 @@ #include "PWMDcMotor.h" #include -// maybe useful +// maybe useful especially for more than 2 motors //#define ENABLE_MOTOR_LIST_FUNCTIONS /* @@ -34,14 +34,6 @@ #define VELOCITY_SCALE_VALUE 500 // for computing of CurrentVelocity // Timeout for encoder ticks if motor is running #define ENCODER_TICKS_TIMEOUT_MILLIS 500 -/* - * Motor Control - */ -// Ticks for ramp down if external stop requested -#define RAMP_DOWN_MIN_TICKS 3 - -// Safety net. If difference between targetCount and current distanceCount is less than, adjust new targetCount -#define MAX_DISTANCE_DELTA 8 class EncoderMotor: public PWMDcMotor { public: @@ -55,14 +47,17 @@ class EncoderMotor: public PWMDcMotor { // virtual ~EncoderMotor(); /* - * Functions for going a fixed distance + * Functions for going a fixed distance, they "overwrite" PWMDCMotor functions */ - void initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection); - // not used yet - void initGoDistanceCount(int16_t aDistanceCount); - + 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); bool updateMotor(); - void synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t aCheckInterval); // Computes motor speed compensation value in order to go exactly straight ahead + + /* + * 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 /* @@ -80,9 +75,9 @@ class EncoderMotor: public PWMDcMotor { */ static bool updateAllMotors(); - static void initGoDistanceCountForAll(int aDistanceCount); - static void goDistanceCountForAll(int aDistanceCount, void (*aLoopCallback)(void)); - static void initRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); + static void startGoDistanceCountForAll(int aRequestedDistanceCount); + static void goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)); + static void startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); static void stopAllMotors(uint8_t aStopMode); static void waitUntilAllMotorsStopped(void (*aLoopCallback)(void)); @@ -111,16 +106,16 @@ class EncoderMotor: public PWMDcMotor { /* * Reset() resets all members from CurrentVelocity to (including) Debug to 0 */ - int16_t CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis + int CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis - uint16_t TargetDistanceCount; - uint16_t LastTargetDistanceCount; + unsigned int TargetDistanceCount; + unsigned int LastTargetDistanceCount; /* - * Distance optocoupler impulse counter. It is reset at initGoDistanceCount if motor was stopped. + * Distance optocoupler impulse counter. It is reset at startGoDistanceCount if motor was stopped. */ - volatile uint16_t EncoderCount; - volatile uint16_t LastRideEncoderCount; // count of last ride - from start of MOTOR_STATE_RAMP_UP to next MOTOR_STATE_RAMP_UP + 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 @@ -129,16 +124,19 @@ class EncoderMotor: public PWMDcMotor { * For ramp and state control */ uint8_t RampDeltaPerDistanceCount; - uint16_t NextChangeMaxTargetCount; // target count at which next change must be done + 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 - uint16_t DebugCount; // for different debug purposes of ramp optimisation + 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 #endif // do not delete it!!! It must be the last element in structure and is required for stopMotorAndReset() - uint16_t Debug; + unsigned int Debug; }; #endif /* SRC_ENCODERMOTORCONTROL_H_ */ + +#pragma once + diff --git a/src/PWMDcMotor.cpp b/src/PWMDcMotor.cpp index a49f7c0..f3946fa 100644 --- a/src/PWMDcMotor.cpp +++ b/src/PWMDcMotor.cpp @@ -1,7 +1,7 @@ /* * PWMDcMotor.cpp * - * Low level motor control for Adafruit_MotorShield OR breakout board with TB6612FNG / Driver IC for dual DC motor + * 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). @@ -11,7 +11,7 @@ * Copyright (C) 2016-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -141,7 +141,7 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin * @param aMotorDriverMode The mode can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance) */ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) { - CurrentDirectionOrBrakeMode = aMotorDriverMode; + CurrentDirectionOrBrakeMode = aMotorDriverMode; // The only statement which changes CurrentDirectionOrBrakeMode #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 @@ -196,20 +196,18 @@ bool PWMDcMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { bool tReturnValue = false; uint8_t tRequestedDirection = aRequestedDirection & DIRECTION_MASK; if (CurrentDirectionOrBrakeMode != tRequestedDirection) { - CurrentDirectionOrBrakeMode = tRequestedDirection; // The only statement which changes CurrentDirectionOrBrakeMode if (CurrentSpeed != 0) { -//#ifdef DEBUG +#ifdef DEBUG Serial.print(F("Motor mode change to ")); Serial.println(tRequestedDirection); -//#endif +#endif /* * Direction change requested but motor still running-> first stop motor */ stop(MOTOR_BRAKE); - delay(200); // give the motor a chance to stop TODO take last speed into account tReturnValue = true; } - setMotorDriverMode(tRequestedDirection); + setMotorDriverMode(tRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode } return tReturnValue; } @@ -244,7 +242,7 @@ void PWMDcMotor::setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection) * Subtracts SpeedCompensation from aRequestedSpeed before applying */ void PWMDcMotor::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { -// avoid underflow + // avoid underflow uint8_t tCurrentSpeed; if (aRequestedSpeed > SpeedCompensation) { tCurrentSpeed = aRequestedSpeed - SpeedCompensation; @@ -335,67 +333,109 @@ void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t a SpeedCompensation = aSpeedCompensation; } +void PWMDcMotor::setDriveSpeed(uint8_t aDriveSpeed) { + DriveSpeed = aDriveSpeed; +} + #ifdef SUPPORT_RAMP_UP -void PWMDcMotor::initRampUp(uint8_t aRequestedDirection) { +void PWMDcMotor::startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { checkAndHandleDirectionChange(aRequestedDirection); if (MotorRampState == MOTOR_STATE_STOPPED) { - CurrentDriveSpeed = DriveSpeed - SpeedCompensation; MotorRampState = MOTOR_STATE_START; + + // avoid underflow + uint8_t tCurrentDriveSpeed; + if (aRequestedSpeed > SpeedCompensation) { + tCurrentDriveSpeed = aRequestedSpeed - SpeedCompensation; + } else { + tCurrentDriveSpeed = 0; + } + CurrentDriveSpeed = tCurrentDriveSpeed; + } else if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) { + setSpeedCompensated(aRequestedSpeed, aRequestedDirection); } } + +void PWMDcMotor::startRampUp(uint8_t aRequestedDirection) { + startRampUp(DriveSpeed, aRequestedDirection); +} #endif #if !defined(USE_ENCODER_MOTOR_CONTROL) /* * Required for non encoder motors to estimate duration for a fixed distance */ -void PWMDcMotor::setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor) { +void PWMDcMotor::setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor) { DistanceToTimeFactor = aDistanceToTimeFactor; } /* - * @param aDistanceCount distance in 5mm resolution (to be compatible with 20 slot encoder discs and 20 cm wheel circumference) + * @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::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection) { -// if (aDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { +void PWMDcMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { +// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { // PanicWithLed(400, 22); // } - if (aDistanceCount == 0) { + if (aRequestedDistanceCount == 0) { return; } - checkAndHandleDirectionChange(aRequestedDirection); // this may reset MotorMovesFixedDistance - MotorMovesFixedDistance = true; if (CurrentSpeed == 0) { #ifdef SUPPORT_RAMP_UP MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = DriveSpeed; + CurrentDriveSpeed = aRequestedSpeed; + setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode #else - setSpeedCompensated(DriveSpeed, aRequestedDirection); + setSpeedCompensated(aRequestedSpeed, aRequestedDirection); +#endif + /* + * 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() + DEFAULT_MOTOR_START_UP_TIME_MILLIS + + (10 * (((uint32_t) aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + } else { +#ifdef SUPPORT_RAMP_UP + MotorRampState = MOTOR_STATE_START; + CurrentDriveSpeed = aRequestedSpeed; #endif + setSpeedCompensated(aRequestedSpeed, 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)); } + MotorMovesFixedDistance = true; +} - /* - * 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() + 150 + (10 * (((uint32_t)aDistanceCount * DistanceToTimeFactor) / DriveSpeed)); +void PWMDcMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { + startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); } /* * Signed DistanceCount */ -void PWMDcMotor::initGoDistanceCount(int16_t aDistanceCount) { - if (aDistanceCount < 0) { - aDistanceCount = -aDistanceCount; - initGoDistanceCount(aDistanceCount, DIRECTION_BACKWARD); +void PWMDcMotor::startGoDistanceCount(int aRequestedDistanceCount) { + if (aRequestedDistanceCount < 0) { + aRequestedDistanceCount = -aRequestedDistanceCount; + startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_BACKWARD); } else { - initGoDistanceCount(aDistanceCount, DIRECTION_FORWARD); + startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_FORWARD); } } -void PWMDcMotor::goDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection) { - initGoDistanceCount(aDistanceCount, aRequestedDirection); +/* + * Not used by CarControl + */ +void PWMDcMotor::goDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { + goDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +} + +void PWMDcMotor::goDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { + startGoDistanceCount(aRequestedSpeed, aRequestedDistanceCount, aRequestedDirection); while (millis() <= computedMillisOfMotorStopForDistance) { #ifdef SUPPORT_RAMP_UP updateMotor(); @@ -512,11 +552,11 @@ void PWMDcMotor::writeMotorvaluesToEeprom() { } } -void PanicWithLed(uint16_t aDelay, uint8_t aCount) { - for (uint8_t i = 0; i < aCount; ++i) { - digitalWrite(LED_BUILTIN, HIGH); - delay(aDelay); - digitalWrite(LED_BUILTIN, LOW); - delay(aDelay); - } -} +//void PanicWithLed(unsigned int aDelay, uint8_t aCount) { +// for (uint8_t i = 0; i < aCount; ++i) { +// digitalWrite(LED_BUILTIN, HIGH); +// delay(aDelay); +// digitalWrite(LED_BUILTIN, LOW); +// delay(aDelay); +// } +//} diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h index a1c1659..b87804b 100644 --- a/src/PWMDcMotor.h +++ b/src/PWMDcMotor.h @@ -9,7 +9,7 @@ * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * - * This file is part of Arduino-RobotCar https://github.com/ArminJo/PWMMotorControl. + * 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 @@ -25,9 +25,9 @@ #include -#define VERSION_PWMMOTORCONTROL "1.0.0" +#define VERSION_PWMMOTORCONTROL "1.1.0" #define VERSION_PWMMOTORCONTROL_MAJOR 1 -#define VERSION_PWMMOTORCONTROL_MINOR 0 +#define VERSION_PWMMOTORCONTROL_MINOR 1 /* * Comment this out, if you have encoder interrupts attached at pin 2 and 3 @@ -39,8 +39,8 @@ // /* * 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 bride. - * For full bride, analogWrite the millis() timer0 is used since we use pin 5 & 6. + * 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. + * For full bridge, analogWrite the millis() timer0 is used since we use pin 5 & 6. */ //#define USE_ADAFRUIT_MOTOR_SHIELD // @@ -55,9 +55,13 @@ * Disabling SUPPORT_RAMP_UP saves 7 bytes RAM per motor and around 300 bytes program memory */ #if ! DO_NOT_SUPPORT_RAMP_UP // if defined (externally), disables ramp up support -#define SUPPORT_RAMP_UP +#define SUPPORT_RAMP_UP // 256 milliseconds for ramp, see below #endif +/* + * Comment this out, if you use default settings and 2 LiPo Cells (around 7.4 volt) as Motor supply. + */ +//#define VIN_2_LIPO #define MAX_SPEED 255 /* @@ -67,10 +71,11 @@ #define DEFAULT_COUNTS_PER_FULL_ROTATION 40 #define DEFAULT_MILLIMETER_PER_COUNT 5 +#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 = 150 + (10 * ((aDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + * 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 */ @@ -179,36 +184,44 @@ class PWMDcMotor { void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber = 0); #endif - void setMotorDriverMode(uint8_t cmd); - bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); - /* * Basic motor commands */ - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection); - // not used yet 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 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 + /* + * Fixed distance driving + */ void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation = 0); void setDefaultsForFixedDistanceDriving(); + void setDriveSpeed(uint8_t aDriveSpeed); - void initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection); - - #ifdef SUPPORT_RAMP_UP - void initRampUp(uint8_t aRequestedDirection); +#ifdef SUPPORT_RAMP_UP + void startRampUp(uint8_t aRequestedDirection); + void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); #endif #ifndef USE_ENCODER_MOTOR_CONTROL - void setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor); - void goDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection); - // Signed distance count - void initGoDistanceCount(int16_t aDistanceCount); + // This function only makes sense for non encoder motors + void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); + + // 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); 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); #endif /* @@ -217,6 +230,12 @@ class PWMDcMotor { void readMotorValuesFromEeprom(); void writeMotorvaluesToEeprom(); + /* + * Internal functions + */ + void setMotorDriverMode(uint8_t cmd); + bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); + #if ! defined(USE_ADAFRUIT_MOTOR_SHIELD) || defined(USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD) uint8_t PWMPin; // PWM output pin / PCA9685 channel of Adafruit Motor Shield uint8_t ForwardPin; // if high, motor runs forward @@ -232,7 +251,7 @@ class PWMDcMotor { * 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; // The speed PWM value for going fixed distance. + uint8_t DriveSpeed; // Speed PWM value used for going fixed distance. /* * Positive value to be subtracted from TargetSpeed to get CurrentSpeed to compensate for different left and right motors @@ -263,11 +282,19 @@ class PWMDcMotor { #ifndef USE_ENCODER_MOTOR_CONTROL uint32_t computedMillisOfMotorStopForDistance; // Since we have no distance sensing, we must estimate a duration instead - uint16_t DistanceToTimeFactor; // Required for non encoder motors to estimate duration for a fixed distance + unsigned int DistanceToTimeFactor; // Required for non encoder motors to estimate duration for a fixed distance #endif }; -void PanicWithLed(uint16_t aDelay, uint8_t aCount); - +//void PanicWithLed(unsigned int aDelay, uint8_t aCount); +/* + * Version 1.1.0 - 10/2020 + * - Added and renamed functions. + * + * Version 1.0.0 - 9/2020 + * - Initial version. + */ #endif /* PWMDCMOTOR_H_ */ + +#pragma once