diff --git a/.github/workflows/TestCompile.yml b/.github/workflows/TestCompile.yml
index bdfdd66..82b288c 100644
--- a/.github/workflows/TestCompile.yml
+++ b/.github/workflows/TestCompile.yml
@@ -42,6 +42,7 @@ jobs:
- arduino-boards-fqbn: arduino:avr:uno|TB6612Breakout # Nano board with TB6612 breakout board
build-properties:
RobotCarBlueDisplay:
+ -DUSE_LAYOUT_FOR_NANO
-DBLUETOOTH_BAUD_RATE=BAUD_115200
-DUSE_STANDARD_SERIAL
-DUSE_US_SENSOR_1_PIN_MODE
diff --git a/README.md b/README.md
index b16eeb2..3efe75c 100644
--- a/README.md
+++ b/README.md
@@ -17,20 +17,27 @@ You may also overwrite the function fillAndShowForwardDistancesInfo(), if you us
| Option | Default | File | Description |
|-|-|-|-|
| `USE_ENCODER_MOTOR_CONTROL` | disabled | PWMDCMotor.h | Use fork light barrier and an attached encoder disc to enable motor distance and speed sensing. |
+| `USE_ADAFRUIT_MOTOR_SHIELD` | disabled | PWMDcMotor.h | | Use Adafruit Motor Shield v2 connected by I2C instead of simple TB6612 or L298 breakout board.
+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_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD` | enabled | PWMDcMotor.h | Saves 694 bytes program memory |
-| `CAR_HAS_4_WHEELS` | `CAR_HAS_2_WHEELS` | RobotCar.h | Use modified formula dor turning the car. |
+| `SUPPORT_RAMP_UP` | enabled | PWMDcMotor.h | `DO_NOT_SUPPORT_RAMP_UP` | Saves around 300 bytes program memory |
+|-|-|-|-|
+| `USE_LAYOUT_FOR_NANO` | disabled | RobotCar.h | Use different pinout for Nano board. It has A6 and A7 available as pins. |
+| `CAR_HAS_4_WHEELS` | disabled | RobotCar.h | Use modified formula dor turning the car. |
| `USE_US_SENSOR_1_PIN_MODE` | disabled | RobotCar.h | Use modified HC-SR04 modules or HY-SRF05 ones.Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger pin. |
| `CAR_HAS_IR_DISTANCE_SENSOR` | disabled | RobotCar.h | Use Sharp GP2Y0A21YK / 1080 IR distance sensor. |
| `CAR_HAS_TOF_DISTANCE_SENSOR` | disabled | RobotCar.h | Use VL53L1X TimeOfFlight distance sensor. |
+| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. |
| `CAR_HAS_CAMERA` | disabled | RobotCar.h | Enables the `Camera` button for the `PIN_CAMERA_SUPPLY_CONTROL` pin. |
| `CAR_HAS_LASER` | disabled | RobotCar.h | Enables the `Laser` button for the `PIN_LASER_OUT` / `LED_BUILTIN` pin. |
| `CAR_HAS_PAN_SERVO` | disabled | RobotCar.h | Enables the pan slider for the `PanServo` at the `PIN_PAN_SERVO` pin. |
| `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin.. |
-| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. |
| `MONITOR_LIPO_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. Requires 2 additional resistors at pin A2. |
| `VIN_VOLTAGE_CORRECTION` | undefined | RobotCar.h | Voltage to be substracted from VIN voltage. E.g. if there is a series diode between LIPO and VIN set it to 0.8. |
+
# Wall detection
The problem of the ultrasonic values is, that you can only detect a wall with the ultrasonic sensor if the angle of the wall relative to sensor axis is approximately between 70 and 110 degree.
For other angels the reflected ultrasonic beam can not not reach the receiver which leads to unrealistic great distances.
diff --git a/src/AutonomousDrive.cpp b/src/AutonomousDrive.cpp
index 4e49bc8..106baf9 100644
--- a/src/AutonomousDrive.cpp
+++ b/src/AutonomousDrive.cpp
@@ -575,7 +575,7 @@ void driveFollowerModeOneStep() {
RobotCarMotorControl.stopMotors();
clearPrintedForwardDistancesInfos();
- // show current distance
+ // show current distance (as US distance), which triggers the rescan
showUSDistance(tCentimeter);
/*
@@ -594,7 +594,7 @@ void driveFollowerModeOneStep() {
}
for (uint8_t i = 0; i < 3; ++i) {
DistanceServoWriteAndDelay(tDegreeForSearch, true);
- tCentimeter = getDistanceAsCentiMeter();
+ tCentimeter = getDistanceAsCentiMeter(false);
if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) {
/*
* Determine color
@@ -669,9 +669,6 @@ void driveFollowerModeOneStep() {
// Serial.println(F("Stop"));
RobotCarMotorControl.stopMotors();
}
-
- // show distance bars
- showUSDistance(tCentimeter);
delayAndLoopGUI(40); // the IR sensor takes 39 ms for one measurement
}
@@ -679,7 +676,7 @@ unsigned int __attribute__((weak)) getDistanceAndPlayTone() {
/*
* Get distance; timeout is 1 meter
*/
- unsigned int tCentimeter = getDistanceAsCentiMeter();
+ unsigned int tCentimeter = getDistanceAsCentiMeter(true);
/*
* play tone
*/
diff --git a/src/CarMotorControl.cpp b/src/CarMotorControl.cpp
index e78cbf6..929af85 100644
--- a/src/CarMotorControl.cpp
+++ b/src/CarMotorControl.cpp
@@ -161,9 +161,9 @@ void CarMotorControl::resetControlValues() {
* Used to suppress time consuming display of motor values
*/
bool CarMotorControl::needsFastUpdates() {
-#ifdef USE_ENCODER_MOTOR_CONTROL
- return (rightCarMotor.State == MOTOR_STATE_RAMP_DOWN || rightCarMotor.State == MOTOR_STATE_RAMP_UP
- || leftCarMotor.State == MOTOR_STATE_RAMP_DOWN || leftCarMotor.State == MOTOR_STATE_RAMP_UP);
+#ifdef SUPPORT_RAMP_UP
+ return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP
+ || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP);
#else
return false;
#endif
@@ -178,22 +178,34 @@ bool CarMotorControl::updateMotors() {
return tMotorsNotStopped;
}
+#ifdef SUPPORT_RAMP_UP
void CarMotorControl::initRampUp(uint8_t aRequestedDirection) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
rightCarMotor.initRampUp(aRequestedDirection);
leftCarMotor.initRampUp(aRequestedDirection);
-#else
- rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection);
- leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection);
-#endif
}
+/*
+ * Blocking wait until both motors are at drive speed.
+ */
+void CarMotorControl::waitForDriveSpeed() {
+ /*
+ * blocking wait for MOTOR_STATE_DRIVE_SPEED
+ */
+ bool tMotorsNotStopped;
+ do {
+ tMotorsNotStopped = rightCarMotor.updateMotor();
+ tMotorsNotStopped |= leftCarMotor.updateMotor();
+ } while (tMotorsNotStopped
+ && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED));
+}
+#endif
+
void CarMotorControl::initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
+#ifdef SUPPORT_RAMP_UP
rightCarMotor.initRampUp(aRequestedDirection);
leftCarMotor.initRampUp(aRequestedDirection);
/*
- * blocking wait for MOTOR_STATE_FULL_SPEED, 256 milliseconds
+ * blocking wait for MOTOR_STATE_DRIVE_SPEED, 256 milliseconds
*/
bool tMotorsNotStopped; // to check if motors are not stopped by aLoopCallback
do {
@@ -202,8 +214,10 @@ void CarMotorControl::initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection
if (aLoopCallback != NULL) {
aLoopCallback(); // this may stop motors
}
- } while (tMotorsNotStopped && (rightCarMotor.State != MOTOR_STATE_FULL_SPEED || leftCarMotor.State != MOTOR_STATE_FULL_SPEED));
+ } while (tMotorsNotStopped
+ && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED));
#else
+ (void) aLoopCallback;
rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection);
leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection);
#endif
@@ -241,7 +255,7 @@ void CarMotorControl::goDistanceCentimeter(int aDistanceCentimeter, void (*aLoop
/*
* Stop car with ramp and give DistanceCountAfterRampUp counts for braking.
*
- * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_FULL_SPEED to MOTOR_STATE_RAMP_DOWN
+ * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN
* Use DistanceCountAfterRampUp as ramp down count
* Blocking wait for stop
*/
@@ -249,11 +263,9 @@ void CarMotorControl::stopCarAndWaitForIt() {
if (isStopped()) {
return;
}
-#ifdef USE_ENCODER_MOTOR_CONTROL
- rightCarMotor.doOnlyRampUp = false;
+#if defined(USE_ENCODER_MOTOR_CONTROL) && defined(SUPPORT_RAMP_UP)
rightCarMotor.NextChangeMaxTargetCount = rightCarMotor.EncoderCount;
rightCarMotor.TargetDistanceCount = rightCarMotor.EncoderCount + rightCarMotor.DistanceCountAfterRampUp;
- leftCarMotor.doOnlyRampUp = false;
leftCarMotor.NextChangeMaxTargetCount = leftCarMotor.EncoderCount;
leftCarMotor.TargetDistanceCount = leftCarMotor.EncoderCount + leftCarMotor.DistanceCountAfterRampUp;
/*
@@ -280,8 +292,8 @@ void CarMotorControl::waitUntilCarStopped(void (*aLoopCallback)(void)) {
}
bool CarMotorControl::isState(uint8_t aState) {
-#ifdef USE_ENCODER_MOTOR_CONTROL
- return (rightCarMotor.State == aState && leftCarMotor.State == aState);
+#if defined(SUPPORT_RAMP_UP)
+ return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState);
#else
(void) aState;
return false;
@@ -354,12 +366,23 @@ void CarMotorControl::initRotateCar(int16_t aRotationDegrees, uint8_t aTurnDirec
leftCarMotor.initGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor);
if (aUseSlowSpeed) {
// adjust CurrentDriveSpeed
-#ifdef USE_ENCODER_MOTOR_CONTROL
- rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2;
- leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2;
+#if defined(SUPPORT_RAMP_UP)
+ // avoid overflow, the reduced speed is almost max speed then.
+ if (rightCarMotor.StartSpeed < 160) {
+ rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2;
+ }
+ if (leftCarMotor.StartSpeed < 160) {
+ leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2;
+ }
#else
- rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2, rightCarMotor.CurrentDirection);
- leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2, leftCarMotor.CurrentDirection);
+ if (tDistanceCountRight > 0) {
+ rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2,
+ rightCarMotor.CurrentDirectionOrBrakeMode);
+ }
+ if (tDistanceCountLeft > 0) {
+ leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2,
+ leftCarMotor.CurrentDirectionOrBrakeMode);
+ }
#endif
}
}
@@ -383,21 +406,6 @@ void CarMotorControl::setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aD
leftCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor);
}
#else
-/*
- * Start motor for "infinite" distance and then blocking wait until both motors are at drive speed
- * If motor is still running, just update motor.
- */
-void CarMotorControl::waitForDriveSpeed() {
- /*
- * blocking wait for MOTOR_STATE_FULL_SPEED
- */
- bool tMotorsNotStopped;
- do {
- tMotorsNotStopped = rightCarMotor.updateMotor();
- tMotorsNotStopped |= leftCarMotor.updateMotor();
- } while (tMotorsNotStopped && (rightCarMotor.State != MOTOR_STATE_FULL_SPEED || leftCarMotor.State != MOTOR_STATE_FULL_SPEED));
-}
-
/*
* generates a rising ramp and detects the first movement -> this sets dead band / minimum Speed
*/
@@ -408,24 +416,24 @@ void CarMotorControl::calibrate() {
rightCarMotor.StartSpeed = 0;
leftCarMotor.StartSpeed = 0;
+ uint8_t tMotorMovingCount = 0;
+
/*
* increase motor speed by 1 until motor moves
*/
for (uint8_t tSpeed = 20; tSpeed != 0xFF; ++tSpeed) {
if (rightCarMotor.StartSpeed == 0) {
rightCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD);
- rightCarMotor.CurrentSpeed = tSpeed;
}
if (leftCarMotor.StartSpeed == 0) {
leftCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD);
- leftCarMotor.CurrentSpeed = tSpeed;
}
delay(100);
/*
* Check if wheel moved
*/
- uint8_t tMotorMovingCount = 0;
+
/*
* Store speed after 6 counts (3cm)
*/
diff --git a/src/CarMotorControl.h b/src/CarMotorControl.h
index b982d09..8aa3058 100644
--- a/src/CarMotorControl.h
+++ b/src/CarMotorControl.h
@@ -53,15 +53,18 @@ class CarMotorControl {
void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight);
#ifdef USE_ENCODER_MOTOR_CONTROL
void calibrate();
- void waitForDriveSpeed();
#else
// makes no sense for encoder motor
void setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor);
#endif
+#ifdef SUPPORT_RAMP_UP
void initRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD);
+ void waitForDriveSpeed();
+#endif
void initRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL);
+
/*
* Functions for moving a fixed distance
*/
diff --git a/src/Distance.cpp b/src/Distance.cpp
index 6257c54..8d1424b 100644
--- a/src/Distance.cpp
+++ b/src/Distance.cpp
@@ -284,13 +284,7 @@ void readAndShowDistancePeriodically(uint16_t aPeriodMillis) {
long tMillis = millis();
if (sLastUSMeasurementMillis + aPeriodMillis < tMillis) {
sLastUSMeasurementMillis = tMillis;
-#ifdef CAR_HAS_IR_DISTANCE_SENSOR
- showIRDistance(getIRDistanceAsCentimeter());
-#elif CAR_HAS_TOF_DISTANCE_SENSOR
- showIRDistance(getToFDistanceAsCentimeter());
-#endif
- // feedback as slider length
- showUSDistance(getUSDistanceAsCentiMeterWithCentimeterTimeout(300));
+ getDistanceAsCentiMeter(true);
}
}
}
@@ -298,7 +292,7 @@ void readAndShowDistancePeriodically(uint16_t aPeriodMillis) {
/*
* Timeout is DISTANCE_TIMEOUT_CM (1 meter)
*/
-unsigned int getDistanceAsCentiMeter() {
+unsigned int getDistanceAsCentiMeter(bool doShow) {
#ifdef CAR_HAS_TOF_DISTANCE_SENSOR
if (sScanMode != SCAN_MODE_US) {
sToFDistanceSensor.VL53L1X_StartRanging();
@@ -306,16 +300,25 @@ unsigned int getDistanceAsCentiMeter() {
#endif
unsigned int tCentimeter = getUSDistanceAsCentiMeterWithCentimeterTimeout(DISTANCE_TIMEOUT_CM);
+ if (doShow) {
+ showUSDistance(tCentimeter);
+ }
#if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR)
unsigned int tIRCentimeter;
if (sScanMode != SCAN_MODE_US) {
# if defined(CAR_HAS_IR_DISTANCE_SENSOR)
if (sScanMode != SCAN_MODE_US) {
tIRCentimeter = getIRDistanceAsCentimeter();
+ if (doShow) {
+ showIRDistance(tIRCentimeter);
+ }
}
# elif defined(CAR_HAS_TOF_DISTANCE_SENSOR)
if (sScanMode != SCAN_MODE_US) {
tIRCentimeter = readToFDistanceAsCentimeter();
+ if(doShow){
+ showIRDistance(tIRCentimeter);
+ }
}
# endif
if (sScanMode == SCAN_MODE_IR) {
diff --git a/src/Distance.h b/src/Distance.h
index f2d1ddf..9097ec9 100644
--- a/src/Distance.h
+++ b/src/Distance.h
@@ -36,6 +36,11 @@ extern Servo DistanceServo;
#define STEPS_PER_SCAN (NUMBER_OF_DISTANCES - 1) // -> 162 degrees for 18 DEGREES_PER_STEP, 153 for 17 degrees
#define START_DEGREES ((180 - (DEGREES_PER_STEP * STEPS_PER_SCAN)) / 2) // 9 for 18, 13,5 for 17 - we need it symmetrical in the 180 degrees range
+// for future use maybe
+//#define SERVO_CURRENT_LOW_THRESHOLD 100
+//#define SERVO_INITIAL_DELAY 5
+//#define SERVO_CURRENT_LOW_MILLIS_FOR_SERVO_STOPPED 12
+
/*
* Index definitions for ForwardDistancesInfoStruct
*/
@@ -69,7 +74,7 @@ extern int sLastDegreesTurned;
void initDistance();
void DistanceServoWriteAndDelay(uint8_t aValue, bool doDelay = false);
-unsigned int getDistanceAsCentiMeter();
+unsigned int getDistanceAsCentiMeter(bool doShow = false);
bool fillAndShowForwardDistancesInfo(bool aDoFirstValue, bool aForceScan = false);
void postProcessDistances();
diff --git a/src/EncoderMotor.cpp b/src/EncoderMotor.cpp
index 721d731..1b5fd59 100644
--- a/src/EncoderMotor.cpp
+++ b/src/EncoderMotor.cpp
@@ -37,7 +37,7 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in
/*
* The list version saves 100 bytes and is more flexible, compared with the array version
*/
- EncoderMotorNumber = EncoderMotor::sNumberOfMotorControls;
+ MotorValuesEepromStorageNumber = EncoderMotor::sNumberOfMotorControls;
EncoderMotor::sNumberOfMotorControls++;
NextMotorControl = NULL;
if (sMotorControlListStart == NULL) {
@@ -68,73 +68,44 @@ void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMP
}
#endif
-void EncoderMotor::initRampUp(uint8_t aRequestedDirection) {
- checkAndHandleDirectionChange(aRequestedDirection);
- CurrentDriveSpeed = DriveSpeed - SpeedCompensation;
- doOnlyRampUp = true;
-}
-
-bool EncoderMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) {
- if (CurrentDirection != aRequestedDirection) {
- CurrentDirection = aRequestedDirection;
- if (State != MOTOR_STATE_STOPPED) {
-#ifdef DEBUG
- Serial.print(F("Direction change to"));
- Serial.println(tRequestedDirection);
-#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
-// LastTargetDistanceCount = EncoderCount; // Reset LastTargetDistanceCount on direction change
- return true;
- }
- }
- return false;
-}
-
/*
* If motor is still running, add aDistanceCount to current target distance
*/
void EncoderMotor::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection) {
// if (aDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) {
-// PanicWithLed(200, 10);
+// PanicWithLed(400, 22);
// }
- aRequestedDirection &= DIRECTION_MASK;
- checkAndHandleDirectionChange(aRequestedDirection);
-
- if (State == MOTOR_STATE_STOPPED) {
- CurrentDriveSpeed = DriveSpeed - SpeedCompensation;
-// /*
-// * Start the motor and compensate for last distance delta
-// * Compensation is only valid if direction does not change.
-// */
-// // Positive if driven too far, negative if driven too short
-// int8_t tLastDelta = (int) EncoderCount - (int) LastTargetDistanceCount;
-// if (abs(tLastDelta) <= MAX_DISTANCE_DELTA && (int) aDistanceCount >= tLastDelta) {
-// // compensate with last delta
-// TargetDistanceCount = (int) aDistanceCount - tLastDelta;
-// } else {
- TargetDistanceCount = aDistanceCount;
-// }
+ if (aDistanceCount == 0) {
+ return;
+ }
+ checkAndHandleDirectionChange(aRequestedDirection); // this may reset MotorMovesFixedDistance
+ MotorMovesFixedDistance = true;
+ if (CurrentSpeed == 0) {
+ TargetDistanceCount = aDistanceCount;
+#ifdef SUPPORT_RAMP_UP
+ MotorRampState = MOTOR_STATE_START;
+ CurrentDriveSpeed = DriveSpeed;
+#else
+ setSpeedCompensated(DriveSpeed, aRequestedDirection);
+#endif
} else {
+ TargetDistanceCount = EncoderCount + aDistanceCount;
/*
- * prolong values for the new distance
+ * prolong NextChangeMaxTargetCount for the new distance
*/
- State = MOTOR_STATE_FULL_SPEED;
- PWMDcMotor::setSpeed(CurrentDriveSpeed, CurrentDirection);
- TargetDistanceCount = EncoderCount + aDistanceCount;
+#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) {
tDistanceCountForRampDown = 3;
}
NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown;
+#endif
}
LastTargetDistanceCount = TargetDistanceCount;
- doOnlyRampUp = false;
}
/*
@@ -142,13 +113,12 @@ void EncoderMotor::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequest
* initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount.
*/
void EncoderMotor::initGoDistanceCount(int16_t aDistanceCount) {
- uint8_t tRequestedDirection = DIRECTION_FORWARD;
-
if (aDistanceCount < 0) {
aDistanceCount = -aDistanceCount;
- tRequestedDirection = DIRECTION_BACKWARD;
+ initGoDistanceCount(aDistanceCount, DIRECTION_BACKWARD);
+ } else {
+ initGoDistanceCount(aDistanceCount, DIRECTION_FORWARD);
}
- initGoDistanceCount(aDistanceCount, tRequestedDirection);
}
/*
@@ -156,32 +126,33 @@ void EncoderMotor::initGoDistanceCount(int16_t aDistanceCount) {
*/
bool EncoderMotor::updateMotor() {
unsigned long tMillis = millis();
-
- if (State == MOTOR_STATE_STOPPED) {
- if (TargetDistanceCount > 0 || doOnlyRampUp) {
- // --> RAMP_UP
- State = MOTOR_STATE_RAMP_UP;
- LastRideEncoderCount = 0;
- EncoderCount = 0;
- /*
- * Start motor
- */
- NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS;
- NextChangeMaxTargetCount = TargetDistanceCount / 2;
- // initialize for timeout detection
- EncoderTickLastMillis = tMillis - ENCODER_SENSOR_MASK_MILLIS - 1;
-
- RampDelta = RAMP_UP_VALUE_DELTA; // 16 steps a 16 millis for ramp up => 256 milliseconds
- if (RampDelta < 2) {
- RampDelta = 2;
- }
- DebugCount = 0;
- Debug = 0;
-
- PWMDcMotor::setSpeed(StartSpeed, CurrentDirection);
+#ifdef SUPPORT_RAMP_UP
+ uint8_t tNewSpeed = CurrentSpeed;
+
+ if (MotorRampState == MOTOR_STATE_START) {
+ // --> RAMP_UP
+ MotorRampState = MOTOR_STATE_RAMP_UP;
+ LastRideEncoderCount = 0;
+ EncoderCount = 0;
+ /*
+ * Start motor
+ */
+ tNewSpeed = StartSpeed;
+ NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS;
+ NextChangeMaxTargetCount = TargetDistanceCount / 2;
+ // initialize for timeout detection
+ EncoderTickLastMillis = tMillis - ENCODER_SENSOR_MASK_MILLIS - 1;
+
+ RampDelta = RAMP_UP_VALUE_DELTA; // 16 steps a 16 millis for ramp up => 256 milliseconds
+ if (RampDelta < 2) {
+ RampDelta = 2;
}
+ DebugCount = 0;
+ Debug = 0;
+ }
- } else if (State == MOTOR_STATE_RAMP_UP) {
+ // do not use else if since state can be changed in code before
+ if (MotorRampState == MOTOR_STATE_RAMP_UP) {
/*
* Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds
* or until more than half of distance is done
@@ -189,19 +160,19 @@ bool EncoderMotor::updateMotor() {
*/
if (tMillis >= NextRampChangeMillis) {
NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS;
- uint8_t tCurrentSpeed = CurrentSpeed + RampDelta;
+ tNewSpeed = tNewSpeed + RampDelta;
// Clip value and check for 8 bit overflow
- if (tCurrentSpeed > CurrentDriveSpeed || tCurrentSpeed <= RampDelta) {
- tCurrentSpeed = CurrentDriveSpeed;
+ if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) {
+ tNewSpeed = CurrentDriveSpeed;
}
/*
* Transition criteria is:
* Max Speed reached or more than half of distance is done
*/
- if (tCurrentSpeed == CurrentDriveSpeed || (!doOnlyRampUp && EncoderCount >= NextChangeMaxTargetCount)) {
- // --> FULL_SPEED
- State = MOTOR_STATE_FULL_SPEED;
+ if (tNewSpeed == CurrentDriveSpeed || (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount)) {
+ // --> DRIVE_SPEED
+ MotorRampState = MOTOR_STATE_DRIVE_SPEED;
DistanceCountAfterRampUp = EncoderCount;
uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp;
@@ -211,18 +182,17 @@ bool EncoderMotor::updateMotor() {
}
NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown;
}
- PWMDcMotor::setSpeed(tCurrentSpeed, CurrentDirection);
}
}
// do not use else if since state can be changed in code before
- if (State == MOTOR_STATE_FULL_SPEED) {
+ if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) {
/*
* Wait until ramp down count is reached
*/
- if (!doOnlyRampUp && EncoderCount >= NextChangeMaxTargetCount) {
+ if (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount) {
// --> RAMP_DOWN
- State = MOTOR_STATE_RAMP_DOWN;
+ MotorRampState = MOTOR_STATE_RAMP_DOWN;
/*
* Ramp to reach StartSpeed after 1/2 of remaining distance
* TODO reduce speed just here? e.g. to 1.5 of start speed?
@@ -232,7 +202,7 @@ bool EncoderMotor::updateMotor() {
}
// do not use else if since state can be changed in code before
- if (State == MOTOR_STATE_RAMP_DOWN) {
+ if (MotorRampState == MOTOR_STATE_RAMP_DOWN) {
DebugCount++;
/*
@@ -241,42 +211,41 @@ bool EncoderMotor::updateMotor() {
if (EncoderCount >= NextChangeMaxTargetCount) {
Debug++;
NextChangeMaxTargetCount++;
- if (CurrentSpeed > RampDeltaPerDistanceCount) {
- CurrentSpeed -= RampDeltaPerDistanceCount;
+ if (tNewSpeed > RampDeltaPerDistanceCount) {
+ tNewSpeed -= RampDeltaPerDistanceCount;
} else {
- CurrentSpeed = StartSpeed;
+ tNewSpeed = StartSpeed;
}
// safety net for slow speed
- if (CurrentSpeed < StartSpeed) {
- CurrentSpeed = StartSpeed;
+ if (tNewSpeed < StartSpeed) {
+ tNewSpeed = StartSpeed;
}
// not really needed here since output is disabled during ramps
MotorValuesHaveChanged = true;
}
- /*
- * Check if target count is reached
- */
- if (EncoderCount >= TargetDistanceCount) {
- SpeedAtTargetCountReached = CurrentSpeed;
- stop(MOTOR_BRAKE);
- return false;
- } else {
- PWMDcMotor::setSpeed(CurrentSpeed, CurrentDirection);
- }
}
+ // End of motor state machine
+
+ if (tNewSpeed != CurrentSpeed) {
+ PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode);
+ }
+#endif
/*
- * Check for encoder tick timeout
+ * Check if target count is reached or encoder tick timeout
*/
- if (State != MOTOR_STATE_STOPPED && tMillis > (EncoderTickLastMillis + RAMP_DOWN_TIMEOUT_MILLIS)) {
-// No encoder tick in the last 500 ms -> stop motor
- SpeedAtTargetCountReached = CurrentSpeed;
- stop(MOTOR_BRAKE);
-#ifdef DEBUG
- Serial.println(F("Encoder timeout -> stop motor"));
+ if (CurrentSpeed > 0) {
+ if (MotorMovesFixedDistance
+ && (EncoderCount >= TargetDistanceCount || tMillis > (EncoderTickLastMillis + ENCODER_TICKS_TIMEOUT_MILLIS))) {
+#ifdef SUPPORT_RAMP_UP
+ DebugSpeedAtTargetCountReached = CurrentSpeed;
+ MotorRampState = MOTOR_STATE_STOPPED;
#endif
- return false;
+ stop(MOTOR_BRAKE);
+ return false;
+ }
}
+
return true;
}
@@ -285,17 +254,18 @@ bool EncoderMotor::updateMotor() {
* Compensate only at forward direction
*/
void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t aCheckInterval) {
- if (CurrentDirection == DIRECTION_BACKWARD) {
+ if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) {
return;
}
static long sNextMotorSyncMillis;
long tMillis = millis();
if (tMillis >= sNextMotorSyncMillis) {
sNextMotorSyncMillis += aCheckInterval;
+#ifdef SUPPORT_RAMP_UP
// only synchronize if manually operated or at full speed
- if ((State == MOTOR_STATE_STOPPED && aOtherMotorControl->State == MOTOR_STATE_STOPPED && CurrentSpeed > 0)
- || (State == MOTOR_STATE_FULL_SPEED && aOtherMotorControl->State == MOTOR_STATE_FULL_SPEED)) {
-
+ if ((MotorRampState == MOTOR_STATE_STOPPED && aOtherMotorControl->MotorRampState == MOTOR_STATE_STOPPED && CurrentSpeed > 0)
+ || (MotorRampState == MOTOR_STATE_DRIVE_SPEED && aOtherMotorControl->MotorRampState == MOTOR_STATE_DRIVE_SPEED)) {
+#endif
MotorValuesHaveChanged = false;
if (EncoderCount >= (aOtherMotorControl->EncoderCount + 2)) {
EncoderCount = aOtherMotorControl->EncoderCount;
@@ -305,7 +275,7 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t
if (aOtherMotorControl->SpeedCompensation >= 2) {
aOtherMotorControl->SpeedCompensation -= 2;
aOtherMotorControl->CurrentSpeed += 2;
- aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, CurrentDirection);
+ aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD);
MotorValuesHaveChanged = true;
EncoderCount = aOtherMotorControl->EncoderCount;
} else if (CurrentSpeed > StartSpeed) {
@@ -314,7 +284,7 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t
*/
SpeedCompensation += 2;
CurrentSpeed -= 2;
- PWMDcMotor::setSpeed(CurrentSpeed, CurrentDirection);
+ PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD);
MotorValuesHaveChanged = true;
}
@@ -326,7 +296,7 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t
if (SpeedCompensation >= 2) {
SpeedCompensation -= 2;
CurrentSpeed += 2;
- PWMDcMotor::setSpeed(CurrentSpeed, CurrentDirection);
+ PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD);
MotorValuesHaveChanged = true;
} else if (aOtherMotorControl->CurrentSpeed > aOtherMotorControl->StartSpeed) {
/*
@@ -334,61 +304,30 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, uint16_t
*/
aOtherMotorControl->SpeedCompensation += 2;
aOtherMotorControl->CurrentSpeed -= 2;
- aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, CurrentDirection);
+ aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD);
MotorValuesHaveChanged = true;
}
}
- if (MotorValuesHaveChanged && State == MOTOR_STATE_FULL_SPEED) {
+ if (MotorValuesHaveChanged) {
writeMotorvaluesToEeprom();
}
+#ifdef SUPPORT_RAMP_UP
}
+#endif
}
}
/*************************
* Direct motor control
*************************/
-void EncoderMotor::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) {
- if (aRequestedSpeed == 0) {
- stop();
- } else {
- PWMDcMotor::setSpeedCompensated(aRequestedSpeed, aRequestedDirection);
- }
-}
-
-void EncoderMotor::setSpeedCompensated(int aRequestedSpeed) {
- uint8_t tDirection;
- if (aRequestedSpeed > 0) {
- tDirection = DIRECTION_FORWARD;
- } else {
- tDirection = DIRECTION_BACKWARD;
- aRequestedSpeed = -aRequestedSpeed;
- }
- setSpeedCompensated(aRequestedSpeed, tDirection);
-}
-
-/*
- * The one and only place where State is set to MOTOR_STATE_STOPPED
- * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE
- */
-void EncoderMotor::stop(uint8_t aStopMode) {
- /*
- * Set state to MOTOR_STATE_STOPPED
- */
- State = MOTOR_STATE_STOPPED;
- TargetDistanceCount = 0;
- doOnlyRampUp = false;
-
- PWMDcMotor::stop(aStopMode);
-}
-
/*
* Reset all control values as distances, debug values to 0x00
* Leave calibration and compensation values unaffected.
*/
void EncoderMotor::resetControlValues() {
- memset(&CurrentDriveSpeed, 0, (((uint8_t *) &Debug) + sizeof(Debug)) - &CurrentDriveSpeed);
+ memset(reinterpret_cast(&CurrentVelocity), 0,
+ (((uint8_t *) &Debug) + sizeof(Debug)) - reinterpret_cast(&CurrentVelocity));
// to force display of initial values
EncoderTickCounterHasChanged = true;
}
@@ -468,28 +407,34 @@ EncoderMotor * EncoderMotor::sMotorControlListStart = NULL;
* If you have 2 motors, better use CarControl
*****************************************************/
-void EncoderMotor::updateAllMotors() {
+bool EncoderMotor::updateAllMotors() {
EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart;
// walk through list
+ bool tMotorsNotStopped = false; // to check if motors are not stopped by aLoopCallback
while (tEncoderMotorControlPointer != NULL) {
- tEncoderMotorControlPointer->updateMotor();
+ tMotorsNotStopped |= tEncoderMotorControlPointer->updateMotor();
tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl;
}
+ return tMotorsNotStopped;
}
/*
* Waits until distance is reached
*/
-void EncoderMotor::startAndWaitForFullSpeedForAll() {
+void EncoderMotor::initRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) {
EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart;
// walk through list
while (tEncoderMotorControlPointer != NULL) {
- tEncoderMotorControlPointer->initGoDistanceCount(INFINITE_DISTANCE_COUNT);
+ tEncoderMotorControlPointer->initRampUp(aRequestedDirection);
tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl;
}
+ bool tMotorsNotStopped; // to check if motors are not stopped by aLoopCallback
do {
- EncoderMotor::updateAllMotors();
- } while (!EncoderMotor::allMotorsStarted());
+ tMotorsNotStopped = EncoderMotor::updateAllMotors();
+ if (aLoopCallback != NULL) {
+ aLoopCallback(); // this may stop motors
+ }
+ } while (tMotorsNotStopped && !EncoderMotor::allMotorsStarted());
}
void EncoderMotor::initGoDistanceCountForAll(int aDistanceCount) {
@@ -519,7 +464,7 @@ bool EncoderMotor::allMotorsStarted() {
bool tAllAreStarted = true;
// walk through list
while (tEncoderMotorControlPointer != NULL) {
- if (tEncoderMotorControlPointer->State != MOTOR_STATE_FULL_SPEED) {
+ if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_DRIVE_SPEED) {
tAllAreStarted = false;
}
tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl;
@@ -532,7 +477,7 @@ bool EncoderMotor::allMotorsStopped() {
bool tAllAreStopped = true;
// walk through list
while (tEncoderMotorControlPointer != NULL) {
- if (tEncoderMotorControlPointer->State != MOTOR_STATE_STOPPED) {
+ if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_STOPPED) {
tAllAreStopped = false;
}
tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl;
@@ -541,7 +486,7 @@ bool EncoderMotor::allMotorsStopped() {
}
/*
- * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_FULL_SPEED to MOTOR_STATE_RAMP_DOWN
+ * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN
* Use DistanceCountAfterRampUp as ramp down count
* Busy waits for stop
*/
@@ -563,15 +508,6 @@ void EncoderMotor::stopAllMotorsAndWaitUntilStopped() {
}
}
-void EncoderMotor::stopAllMotorsAndReset() {
- EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart;
-// walk through list
- while (tEncoderMotorControlPointer != NULL) {
- tEncoderMotorControlPointer->stopMotorAndReset();
- tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl;
- }
-}
-
void EncoderMotor::waitUntilAllMotorsStopped(void (*aLoopCallback)(void)) {
do {
EncoderMotor::updateAllMotors();
diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h
index 31c86d9..a10c52a 100644
--- a/src/EncoderMotor.h
+++ b/src/EncoderMotor.h
@@ -22,6 +22,9 @@
#include "PWMDcMotor.h"
#include
+// maybe useful
+//#define ENABLE_MOTOR_LIST_FUNCTIONS
+
/*
* Encoders generate 110 Hz at max speed => 8 ms per period
* since duty cycle of encoder disk impulse is 1/3 choose 3 millis as ringing mask.
@@ -29,38 +32,17 @@
*/
#define ENCODER_SENSOR_MASK_MILLIS 3
#define VELOCITY_SCALE_VALUE 500 // for computing of CurrentVelocity
-
+// Timeout for encoder ticks if motor is running
+#define ENCODER_TICKS_TIMEOUT_MILLIS 500
/*
* Motor Control
*/
-// the smaller the value the steeper the ramp
-#define RAMP_UP_UPDATE_INTERVAL_MILLIS 16
-#define RAMP_UP_UPDATE_INTERVAL_STEPS 16
-// gives 16 steps a 16 millis for ramp up => 256 milliseconds
-#define RAMP_UP_VALUE_DELTA ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS)
-
-// timeout after ramp down was finished to switch off motor
-#define RAMP_DOWN_TIMEOUT_MILLIS 500
-
// 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
-#define MOTOR_STATE_STOPPED 0
-#define MOTOR_STATE_RAMP_UP 1
-#define MOTOR_STATE_FULL_SPEED 2
-#define MOTOR_STATE_RAMP_DOWN 3
-
-//#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 500
-#define MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS 100
-
-//#define SERVO_CURRENT_LOW_THRESHOLD 100
-#define SERVO_INITIAL_DELAY 5
-
-#define SERVO_CURRENT_LOW_MILLIS_FOR_SERVO_STOPPED 12
-
class EncoderMotor: public PWMDcMotor {
public:
@@ -72,8 +54,6 @@ class EncoderMotor: public PWMDcMotor {
#endif
// virtual ~EncoderMotor();
- bool checkAndHandleDirectionChange(uint8_t aRequestedDirection);
- void initRampUp(uint8_t aRequestedDirection);
/*
* Functions for going a fixed distance
*/
@@ -92,33 +72,19 @@ class EncoderMotor: public PWMDcMotor {
static void enableINT0AndINT1Interrupts();
static void enableInterruptOnBothEdges(uint8_t aIntPinNumber);
- /*
- * Direct motor control
- */
- void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection);
- // not used yet
- void setSpeedCompensated(int aRequestedSpeed);
-
- /**
- * Sets CurrentSpeed (PWM) to 0 and driver mode to aStopMode
- * Resets State and TargetDistanceCount variables.
- * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE
- */
- void stop(uint8_t aStopMode = STOP_MODE_KEEP);
void resetControlValues(); // Shutdown and reset all control values and sets direction to forward
#ifdef ENABLE_MOTOR_LIST_FUNCTIONS
/*
* Static convenience functions affecting all motors. If you have 2 motors, better use CarControl
*/
- static void updateAllMotors();
+ static bool updateAllMotors();
static void initGoDistanceCountForAll(int aDistanceCount);
static void goDistanceCountForAll(int aDistanceCount, void (*aLoopCallback)(void));
- static void startAndWaitForFullSpeedForAll(void);
+ static void initRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void));
static void stopAllMotors(uint8_t aStopMode);
- static void stopAllMotorsAndReset();
static void waitUntilAllMotorsStopped(void (*aLoopCallback)(void));
static void stopAllMotorsAndWaitUntilStopped();
@@ -143,13 +109,10 @@ class EncoderMotor: public PWMDcMotor {
* Variables required for going a fixed distance
**********************************************************/
/*
- * Reset() resets all members from CurrentDriveSpeed to (including) Debug to 0
+ * Reset() resets all members from CurrentVelocity to (including) Debug to 0
*/
- uint8_t CurrentDriveSpeed; // DriveSpeed - SpeedCompensation; The DriveSpeed used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeed
- volatile int16_t CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis
+ int16_t CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis
- uint8_t State; // MOTOR_STATE_STOPPED, MOTOR_STATE_RAMP_UP, MOTOR_STATE_FULL_SPEED, MOTOR_STATE_RAMP_DOWN
- bool doOnlyRampUp;
uint16_t TargetDistanceCount;
uint16_t LastTargetDistanceCount;
@@ -161,17 +124,17 @@ class EncoderMotor: public PWMDcMotor {
// 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
+#ifdef SUPPORT_RAMP_UP
/*
* For ramp and state control
*/
- uint8_t RampDelta;
uint8_t RampDeltaPerDistanceCount;
- uint16_t NextChangeMaxTargetCount; // target count at which next change must be done
- unsigned long NextRampChangeMillis;
+ uint16_t 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;
- uint8_t SpeedAtTargetCountReached;
+ 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 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;
diff --git a/src/HomePage.cpp b/src/HomePage.cpp
index 6ee4db2..fabf216 100644
--- a/src/HomePage.cpp
+++ b/src/HomePage.cpp
@@ -138,6 +138,8 @@ void drawHomePage(void) {
TouchButtonDirection.drawButton();
#ifdef USE_ENCODER_MOTOR_CONTROL
TouchButtonCalibrate.drawButton();
+#else
+ TouchButtonCompensation.drawButton();
#endif
SliderUSPosition.setValueAndDrawBar(sLastServoAngleInDegrees);
@@ -168,6 +170,8 @@ void startHomePage(void) {
TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4);
#ifdef USE_ENCODER_MOTOR_CONTROL
TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4);
+#else
+ TouchButtonCompensation.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4);
#endif
TouchButtonNextPage.setCaption(F("Automatic\nControl"));
@@ -181,6 +185,8 @@ void stopHomePage(void) {
TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5);
#ifdef USE_ENCODER_MOTOR_CONTROL
TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2);
+#else
+ TouchButtonCompensation.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2);
#endif
startStopRobotCar(false);
}
diff --git a/src/PWMDcMotor.cpp b/src/PWMDcMotor.cpp
index 1963abf..a49f7c0 100644
--- a/src/PWMDcMotor.cpp
+++ b/src/PWMDcMotor.cpp
@@ -121,7 +121,7 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin
ForwardPin = aForwardPin;
BackwardPin = aBackwardPin;
PWMPin = aPWMPin;
- StopMode = MOTOR_RELEASE;
+ DefaultStopMode = MOTOR_RELEASE;
pinMode(aForwardPin, OUTPUT);
pinMode(aBackwardPin, OUTPUT);
@@ -141,6 +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;
#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
@@ -188,48 +189,69 @@ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) {
#endif // USE_ADAFRUIT_MOTOR_SHIELD
}
+/*
+ * @return true if direction has changed and motor has stopped
+ */
+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
+ Serial.print(F("Motor mode change to "));
+ Serial.println(tRequestedDirection);
+//#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);
+ }
+ return tReturnValue;
+}
+
/*
* @brief Control the DC Motor speed/throttle
* @param speed The 8-bit PWM value, 0 is off, 255 is on forward -255 is on backward
* First set driver mode, then set PWM
*/
-void PWMDcMotor::setSpeed(uint8_t aSpeedRequested, uint8_t aDirection) {
- CurrentSpeed = aSpeedRequested;
- MotorValuesHaveChanged = true;
-
+void PWMDcMotor::setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection) {
if (aSpeedRequested == 0) {
- setMotorDriverMode(StopMode);
+ stop(DefaultStopMode);
} else {
- setMotorDriverMode(aDirection);
- }
+ checkAndHandleDirectionChange(aRequestedDirection);
+ MotorValuesHaveChanged = true;
+ if (CurrentSpeed != aSpeedRequested) {
+ CurrentSpeed = aSpeedRequested; // The only statement which sets CurrentSpeed to a value != 0
#ifdef USE_ADAFRUIT_MOTOR_SHIELD
# ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD
- I2CSetPWM(PWMPin, 0, 16 * aSpeedRequested);
+ I2CSetPWM(PWMPin, 0, 16 * aSpeedRequested);
# else
- Adafruit_MotorShield_DcMotor->setSpeed(aSpeedRequested);
+ Adafruit_MotorShield_DcMotor->setSpeed(aSpeedRequested);
# endif
#else
- analogWrite(PWMPin, aSpeedRequested);
+ analogWrite(PWMPin, aSpeedRequested);
#endif
+ }
+ }
}
/*
* Subtracts SpeedCompensation from aRequestedSpeed before applying
*/
void PWMDcMotor::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) {
- aRequestedDirection &= DIRECTION_MASK;
- if (aRequestedSpeed == 0) {
- stop(StopMode);
- } else {
- CurrentDirection = aRequestedDirection;
// avoid underflow
- if (aRequestedSpeed > SpeedCompensation) {
- CurrentSpeed = aRequestedSpeed - SpeedCompensation;
- } else {
- CurrentSpeed = 0;
- }
- setSpeed(CurrentSpeed, CurrentDirection); // output PWM value to motor
+ uint8_t tCurrentSpeed;
+ if (aRequestedSpeed > SpeedCompensation) {
+ tCurrentSpeed = aRequestedSpeed - SpeedCompensation;
+ } else {
+ tCurrentSpeed = 0;
}
+ setSpeed(tCurrentSpeed, aRequestedDirection); // output PWM value to motor
}
/*
@@ -243,6 +265,7 @@ void PWMDcMotor::setSpeed(int aSpeedRequested) {
setSpeed(aSpeedRequested, DIRECTION_FORWARD);
}
}
+
void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) {
uint8_t tDirection;
if (aRequestedSpeed > 0) {
@@ -256,33 +279,39 @@ void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) {
/*
* First set PWM to 0 then disable driver
- * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE
+ * @param aStopMode STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE
*/
void PWMDcMotor::stop(uint8_t aStopMode) {
- CurrentSpeed = 0;
+
+ CurrentSpeed = 0; // The only statement which sets CurrentSpeed to 0
MotorValuesHaveChanged = true;
+ MotorMovesFixedDistance = false;
+#ifdef SUPPORT_RAMP_UP
+ MotorRampState = MOTOR_STATE_STOPPED;
+#endif
#ifndef USE_ENCODER_MOTOR_CONTROL
MotorMovesFixedDistance = false;
#endif
#ifdef USE_ADAFRUIT_MOTOR_SHIELD
# ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD
- setSpeed(0, DIRECTION_FORWARD);
+ I2CSetPWM(PWMPin, 0, 0);
# else
- Adafruit_MotorShield_DcMotor->setSpeed(0);
+ Adafruit_MotorShield_DcMotor->setSpeed(0);
# endif
#else
analogWrite(PWMPin, 0);
#endif
- if (aStopMode != StopMode) {
- setMotorDriverMode(CheckStopMODE(aStopMode));
+ if (aStopMode == STOP_MODE_KEEP) {
+ aStopMode = DefaultStopMode;
}
+ setMotorDriverMode(CheckStopMODE(aStopMode));
}
/*
* @param aStopMode used for speed == 0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE
*/
void PWMDcMotor::setStopMode(uint8_t aStopMode) {
- StopMode = CheckStopMODE(aStopMode);
+ DefaultStopMode = CheckStopMODE(aStopMode);
}
/******************************************************************************************
@@ -306,7 +335,17 @@ void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t a
SpeedCompensation = aSpeedCompensation;
}
-#ifndef USE_ENCODER_MOTOR_CONTROL
+#ifdef SUPPORT_RAMP_UP
+void PWMDcMotor::initRampUp(uint8_t aRequestedDirection) {
+ checkAndHandleDirectionChange(aRequestedDirection);
+ if (MotorRampState == MOTOR_STATE_STOPPED) {
+ CurrentDriveSpeed = DriveSpeed - SpeedCompensation;
+ MotorRampState = MOTOR_STATE_START;
+ }
+}
+#endif
+
+#if !defined(USE_ENCODER_MOTOR_CONTROL)
/*
* Required for non encoder motors to estimate duration for a fixed distance
*/
@@ -321,46 +360,119 @@ void PWMDcMotor::initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequested
// if (aDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) {
// PanicWithLed(400, 22);
// }
- setSpeedCompensated(DriveSpeed, aRequestedDirection);
+ if (aDistanceCount == 0) {
+ return;
+ }
+ checkAndHandleDirectionChange(aRequestedDirection); // this may reset MotorMovesFixedDistance
+ MotorMovesFixedDistance = true;
+
+ if (CurrentSpeed == 0) {
+#ifdef SUPPORT_RAMP_UP
+ MotorRampState = MOTOR_STATE_START;
+ CurrentDriveSpeed = DriveSpeed;
+#else
+ setSpeedCompensated(DriveSpeed, aRequestedDirection);
+#endif
+ }
+
/*
- * Estimate duration
+ * 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() + 30 + ((aDistanceCount * DistanceToTimeFactor * 10) / (DriveSpeed - StartSpeed));
- MotorMovesFixedDistance = true;
+ computedMillisOfMotorStopForDistance = millis() + 150 + (10 * (((uint32_t)aDistanceCount * DistanceToTimeFactor) / DriveSpeed));
}
/*
- * Signed count
+ * Signed DistanceCount
*/
void PWMDcMotor::initGoDistanceCount(int16_t aDistanceCount) {
- uint8_t tRequestedDirection = DIRECTION_FORWARD;
-
if (aDistanceCount < 0) {
aDistanceCount = -aDistanceCount;
- tRequestedDirection = DIRECTION_BACKWARD;
+ initGoDistanceCount(aDistanceCount, DIRECTION_BACKWARD);
+ } else {
+ initGoDistanceCount(aDistanceCount, DIRECTION_FORWARD);
}
- initGoDistanceCount(aDistanceCount, tRequestedDirection);
}
void PWMDcMotor::goDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection) {
initGoDistanceCount(aDistanceCount, aRequestedDirection);
while (millis() <= computedMillisOfMotorStopForDistance) {
- ; // just wait
+#ifdef SUPPORT_RAMP_UP
+ updateMotor();
+#endif
}
- stop(StopMode);
+ stop(DefaultStopMode);
}
/*
* @return true if not stopped (motor expects another update)
*/
bool PWMDcMotor::updateMotor() {
- if (MotorMovesFixedDistance && CurrentSpeed != 0 && millis() > computedMillisOfMotorStopForDistance) {
- stop(StopMode); // resets MotorMovesFixedDistance
- return false;
+#ifdef SUPPORT_RAMP_UP
+ unsigned long tMillis = millis();
+ uint8_t tNewSpeed = CurrentSpeed;
+
+ if (MotorRampState == MOTOR_STATE_START) {
+ // --> RAMP_UP
+ MotorRampState = MOTOR_STATE_RAMP_UP;
+ /*
+ * Set ramp values, 16 steps a 16 millis for ramp up => 256 milliseconds
+ */
+ NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS;
+ RampDelta = RAMP_UP_VALUE_DELTA; // ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS)
+ if (RampDelta < 2) {
+ RampDelta = 2;
+ }
+ /*
+ * Start motor
+ */
+ tNewSpeed = StartSpeed;
+ }
+
+ // do not use else if since state can be changed in code before
+ if (MotorRampState == MOTOR_STATE_RAMP_UP) {
+ /*
+ * Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds
+ * or until more than half of distance is done
+ * Distance required for ramp is 0 to 10 or more, increasing with increasing CurrentDriveSpeed
+ */
+ if (tMillis >= NextRampChangeMillis) {
+ NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS;
+ tNewSpeed = tNewSpeed + RampDelta;
+ // Clip value and check for 8 bit overflow
+ if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) {
+ tNewSpeed = CurrentDriveSpeed;
+ }
+
+ /*
+ * Transition criteria is:
+ * Max Speed reached or more than half of distance is done
+ */
+ if (tNewSpeed == CurrentDriveSpeed) {
+ // --> DRIVE_SPEED
+ MotorRampState = MOTOR_STATE_DRIVE_SPEED;
+ }
+ }
+ }
+ // End of motor state machine
+
+ if (tNewSpeed != CurrentSpeed) {
+ PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode);
+ }
+#endif
+
+ /*
+ * Check if target milliseconds are reached
+ */
+ if (CurrentSpeed > 0) {
+ if (MotorMovesFixedDistance && millis() > computedMillisOfMotorStopForDistance) {
+ stop(DefaultStopMode); // resets MotorMovesFixedDistance
+ return false;
+ }
}
return true;
}
-#endif // USE_ENCODER_MOTOR_CONTROL
+#endif // !defined(USE_ENCODER_MOTOR_CONTROL)
/********************************************************************************************
* EEPROM functions
diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h
index ebab35a..254efe3 100644
--- a/src/PWMDcMotor.h
+++ b/src/PWMDcMotor.h
@@ -1,6 +1,10 @@
/*
* PWMDCMotor.h
*
+ * Motor control has 2 technical dimensions
+ * 1. Motor driver control. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance)
+ * 2. Speed / PWM which is ignored for BRAKE or RELEASE
+ *
* Created on: 12.05.2019
* Copyright (C) 2019-2020 Armin Joachimsmeyer
* armin.joachimsmeyer@gmail.com
@@ -47,6 +51,13 @@
#define USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD
#endif
+/*
+ * 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
+#endif
+
/*
* Version 1.0.0 - 9/2020
* - Initial version.
@@ -63,17 +74,17 @@
/*
* 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 = 30 + ((aDistanceCount * DistanceToTimeFactor * 10) / (DriveSpeed - StartSpeed));
+ * computedMillisOfMotorStopForDistance = 150 + (10 * ((aDistanceCount * DistanceToTimeFactor) / (DriveSpeed - StartSpeed)));
*
* 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
*/
#define DEFAULT_START_SPEED_7_4_VOLT 45
#define DEFAULT_DRIVE_SPEED_7_4_VOLT 80
-#define DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT 50 // for 2 x LIPO batteries (7.4 volt).
+#define DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT 135 // for 2 x LIPO batteries (7.4 volt).
#define DEFAULT_START_SPEED_6_VOLT 150
#define DEFAULT_DRIVE_SPEED_6_VOLT 255
-#define DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT 80 // for 4 x AA batteries (6 volt).
+#define DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT 300 // for 4 x AA batteries (6 volt).
// Default values - used if EEPROM values are invalid
#if defined(VIN_2_LIPO)
@@ -136,10 +147,22 @@ struct EepromMotorInfoStruct {
};
/*
- * Motor control has 2 technical dimensions
- * 1. Motor driver control. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance)
- * 2. Speed / PWM which is ignored for BRAKE or RELEASE
+ * For ramp control
*/
+#define MOTOR_STATE_STOPPED 0
+#define MOTOR_STATE_START 1
+#define MOTOR_STATE_RAMP_UP 2
+#define MOTOR_STATE_DRIVE_SPEED 3
+#define MOTOR_STATE_RAMP_DOWN 4
+
+#ifdef SUPPORT_RAMP_UP
+// the smaller the value the steeper the ramp
+#define RAMP_UP_UPDATE_INTERVAL_MILLIS 16
+#define RAMP_UP_UPDATE_INTERVAL_STEPS 16
+// gives 16 steps a 16 millis for ramp up => 256 milliseconds
+#define RAMP_UP_VALUE_DELTA ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS)
+#endif
+
class PWMDcMotor {
public:
PWMDcMotor();
@@ -161,28 +184,31 @@ class PWMDcMotor {
#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 aDirection);
+ void setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection);
// not used yet
void setSpeed(int aRequestedSpeed);
void setSpeedCompensated(int aRequestedSpeed);
- /*
- * Sets CurrentSpeed (PWM) to 0 and driver mode to parameter aStopMode
- * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE
- */
- void stop(uint8_t aStopMode = STOP_MODE_KEEP); // ; MOTOR_BRAKE or MOTOR_RELEASE
+ void stop(uint8_t aStopMode = STOP_MODE_KEEP); // STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE
void setStopMode(uint8_t aStopMode); // mode for Speed==0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE
void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation = 0);
void setDefaultsForFixedDistanceDriving();
+
+ void initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection);
+
+ #ifdef SUPPORT_RAMP_UP
+ void initRampUp(uint8_t aRequestedDirection);
+#endif
+
#ifndef USE_ENCODER_MOTOR_CONTROL
void setDistanceToTimeFactorForFixedDistanceDriving(uint16_t aDistanceToTimeFactor);
- void initGoDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection);
void goDistanceCount(uint16_t aDistanceCount, uint8_t aRequestedDirection);
// Signed distance count
void initGoDistanceCount(int16_t aDistanceCount);
@@ -222,13 +248,24 @@ class PWMDcMotor {
* End of EEPROM values
*********************************/
uint8_t CurrentSpeed;
- uint8_t CurrentDirection; // (of CurrentSpeed etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD
+ uint8_t CurrentDirectionOrBrakeMode; // (of CurrentSpeed etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD, MOTOR_BRAKE, MOTOR_RELEASE
static bool MotorValuesHaveChanged;
- uint8_t StopMode; // used for speed == 0
+ uint8_t DefaultStopMode; // used for speed == 0 and STOP_MODE_KEEP
+ bool MotorMovesFixedDistance; // if true, stop if end distance condition reached
+
+#ifdef SUPPORT_RAMP_UP
+ /*
+ * For ramp control
+ */
+ uint8_t MotorRampState; // MOTOR_STATE_STOPPED, MOTOR_STATE_START, MOTOR_STATE_RAMP_UP, MOTOR_STATE_DRIVE_SPEED, MOTOR_STATE_RAMP_DOWN
+ uint8_t CurrentDriveSpeed; // DriveSpeed - SpeedCompensation; The DriveSpeed used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeed
+
+ uint8_t RampDelta;
+ unsigned long NextRampChangeMillis;
+#endif
#ifndef USE_ENCODER_MOTOR_CONTROL
- bool MotorMovesFixedDistance; // if true, stop if computedMillisOfMotorStopForDistance are reached
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
#endif
diff --git a/src/RobotCar.h b/src/RobotCar.h
index 2c20854..5a14472 100644
--- a/src/RobotCar.h
+++ b/src/RobotCar.h
@@ -24,6 +24,8 @@
//#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.
@@ -85,15 +87,16 @@ extern CarMotorControl RobotCarMotorControl;
* 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_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 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_RIGHT_MOTOR_FORWARD 4
+#define PIN_RIGHT_MOTOR_BACKWARD 7
+#define PIN_RIGHT_MOTOR_PWM 5 // Must be PWM capable
#endif
+
/*
* Servo pins
*/
@@ -115,31 +118,40 @@ extern CarMotorControl RobotCarMotorControl;
* Pins for US HC-SR04 distance sensor
*/
#define PIN_TRIGGER_OUT A0 // Connections on the Arduino Sensor Shield
-#ifndef USE_US_SENSOR_1_PIN_MODE
+#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 and Nano (Nano hash full bridge) boards
+ * Different pin layout for UNO with Adafruit motor shield and Nano (Nano hash full bridge) boards
*/
-#ifdef USE_ADAFRUIT_MOTOR_SHIELD
-# ifdef USE_US_SENSOR_1_PIN_MODE
-// Otherwise available as US echo pin
-#define PIN_IR_DISTANCE_SENSOR A1
+#ifdef USE_LAYOUT_FOR_NANO
+/*
+ * Nano Layout
+ */
+# ifdef USE_ADAFRUIT_MOTOR_SHIELD
+#error "Adafruit motor shield makes no sense for a Nano board!"
# endif
-#define PIN_SPEAKER 11
-
-#else // USE_ADAFRUIT_MOTOR_SHIELD
-#ifdef CAR_HAS_CAMERA
+# ifdef CAR_HAS_CAMERA
#define PIN_CAMERA_SUPPLY_CONTROL A7 // Not available on UNO board
-#endif
+# endif
#define PIN_SPEAKER A6 // Not available on UNO board
-#endif // USE_ADAFRUIT_MOTOR_SHIELD
+
+#else
+/*
+ * UNO Layout
+ */
+# ifdef CAR_HAS_CAMERA
+#define PIN_CAMERA_SUPPLY_CONTROL 4
+# endif
+#define PIN_SPEAKER 11
+#endif
/**************************
* End of pin definitions
@@ -151,6 +163,9 @@ extern CarMotorControl RobotCarMotorControl;
#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!
@@ -191,7 +206,6 @@ extern float sVINVoltage;
void readVINVoltage();
#endif
-
void resetServos();
int doUserCollisionDetection();
diff --git a/src/RobotCarBlueDisplay.ino b/src/RobotCarBlueDisplay.ino
index 14b1237..471c649 100644
--- a/src/RobotCarBlueDisplay.ino
+++ b/src/RobotCarBlueDisplay.ino
@@ -143,11 +143,6 @@ void setup() {
PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, true);
#endif
-#if defined(VIN_4_AA)
- // Hack for my simple car with different motors
- RobotCarMotorControl.setValuesForFixedDistanceDriving(DEFAULT_START_SPEED, DEFAULT_DRIVE_SPEED, -10);
-#endif
-
delay(100);
tone(PIN_SPEAKER, 2200, 50); // motor initialized
diff --git a/src/RobotCarCommonGui.cpp b/src/RobotCarCommonGui.cpp
index 558997a..d81f99a 100644
--- a/src/RobotCarCommonGui.cpp
+++ b/src/RobotCarCommonGui.cpp
@@ -37,6 +37,8 @@ BDButton TouchButtonBack;
BDButton TouchButtonNextPage;
#ifdef USE_ENCODER_MOTOR_CONTROL
BDButton TouchButtonCalibrate;
+#else
+BDButton TouchButtonCompensation;
#endif
BDButton TouchButtonRobotCarStartStop;
@@ -85,7 +87,7 @@ void loopGUI(void) {
if (BlueDisplay1.isConnectionEstablished()) {
// do not show anything during motor speed ramps
- if (RobotCarMotorControl.isStopped() || RobotCarMotorControl.isState(MOTOR_STATE_FULL_SPEED)) {
+ if (RobotCarMotorControl.isStopped() || RobotCarMotorControl.isState(MOTOR_STATE_DRIVE_SPEED)) {
if (sCurrentPage == PAGE_HOME) {
loopHomePage();
@@ -192,6 +194,7 @@ void doSpeedSlider(BDSlider * aTheTouchedSlider, uint16_t aValue) {
sLastSpeedSliderValue = aValue;
if (RobotCarMotorControl.isStopped()) {
+ // handle GUI
startStopRobotCar(true);
} else {
RobotCarMotorControl.setSpeedCompensated(aValue, sRobotCarDirection);
@@ -236,6 +239,21 @@ void doSetDirection(BDButton * aTheTouchedButton, int16_t aValue) {
startStopRobotCar(false);
}
+void doStoreRightMotorSpeedCompensation(float aRightMotorSpeedCompensation) {
+ int8_t tRightMotorSpeedCompensation = aRightMotorSpeedCompensation;
+ RobotCarMotorControl.setValuesForFixedDistanceDriving(RobotCarMotorControl.rightCarMotor.StartSpeed,
+ RobotCarMotorControl.rightCarMotor.DriveSpeed, tRightMotorSpeedCompensation);
+ printMotorValues();
+}
+
+/*
+ * Request speed value as number
+ */
+void doGetRightMotorSpeedCompensationAsNumber(BDButton * aTheTouchedButton, int16_t aValue) {
+ BlueDisplay1.getNumberWithShortPrompt(&doStoreRightMotorSpeedCompensation, "Right speed compensation [-128 to 127]",
+ RobotCarMotorControl.rightCarMotor.SpeedCompensation);
+}
+
void startCurrentPage() {
switch (sCurrentPage) {
case PAGE_HOME:
@@ -327,6 +345,9 @@ void initRobotCarDisplay(void) {
#ifdef USE_ENCODER_MOTOR_CONTROL
TouchButtonCalibrate.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("CAL"),
TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doCalibrate);
+#else
+ TouchButtonCompensation.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("Comp"),
+ TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doGetRightMotorSpeedCompensationAsNumber);
#endif
// Direction Button value true is forward, false is backward BUT 0 is DIRECTION_FORWARD!!!
@@ -386,8 +407,9 @@ void initRobotCarDisplay(void) {
*/
#if defined(US_DISTANCE_SLIDER_IS_SMALL)
// Small US distance slider with captions and without cm units
- SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2,
- DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR,
+ SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8,
+ (BUTTON_WIDTH_10 / 2) - 2,
+ DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR,
FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL);
SliderUSDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR_BLACK,
COLOR_WHITE);
@@ -410,8 +432,9 @@ void initRobotCarDisplay(void) {
*/
#if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR)
// Small IR distance slider with captions and without cm units
- SliderIRDistance.init(POS_X_THIRD_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2,
- DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR,
+ SliderIRDistance.init(POS_X_THIRD_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8,
+ (BUTTON_WIDTH_10 / 2) - 2,
+ DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR,
FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL);
SliderIRDistance.setScaleFactor(2); // Slider is virtually 2 times larger, values were divided by 2
SliderIRDistance.setBarThresholdColor(DISTANCE_TIMEOUT_COLOR);
@@ -571,22 +594,27 @@ void printMotorDebugValues() {
* Debug info
*/
uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + TEXT_SIZE_11_HEIGHT + (4 * TEXT_SIZE_11);
+#ifdef SUPPORT_RAMP_UP
sprintf_P(sStringBuffer, PSTR("ramp1%3d %3d"), RobotCarMotorControl.leftCarMotor.DistanceCountAfterRampUp,
RobotCarMotorControl.rightCarMotor.DistanceCountAfterRampUp);
BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
+
tYPos += TEXT_SIZE_11;
- sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.SpeedAtTargetCountReached,
- RobotCarMotorControl.rightCarMotor.SpeedAtTargetCountReached);
- BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
- tYPos += TEXT_SIZE_11;
- sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug,
- RobotCarMotorControl.rightCarMotor.Debug);
+ sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.DebugSpeedAtTargetCountReached,
+ RobotCarMotorControl.rightCarMotor.DebugSpeedAtTargetCountReached);
BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
+
tYPos += TEXT_SIZE_11;
sprintf_P(sStringBuffer, PSTR("dcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.DebugCount,
RobotCarMotorControl.rightCarMotor.DebugCount);
BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
tYPos += TEXT_SIZE_11;
+#endif
+ sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug,
+ RobotCarMotorControl.rightCarMotor.Debug);
+ BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
+
+ tYPos += TEXT_SIZE_11;
sprintf_P(sStringBuffer, PSTR("tcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.LastTargetDistanceCount,
RobotCarMotorControl.rightCarMotor.LastTargetDistanceCount);
BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer);
diff --git a/src/RobotCarGui.h b/src/RobotCarGui.h
index f3eb73a..1235796 100644
--- a/src/RobotCarGui.h
+++ b/src/RobotCarGui.h
@@ -144,6 +144,8 @@ extern BDButton TouchButtonDirection;
#ifdef USE_ENCODER_MOTOR_CONTROL
extern BDButton TouchButtonCalibrate;
void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue);
+#else
+extern BDButton TouchButtonCompensation;
#endif
extern BDSlider SliderSpeed;
diff --git a/src/TestPage.cpp b/src/TestPage.cpp
index c187f41..778e40d 100644
--- a/src/TestPage.cpp
+++ b/src/TestPage.cpp
@@ -170,6 +170,8 @@ void drawTestPage(void) {
# endif
#ifdef USE_ENCODER_MOTOR_CONTROL
TouchButtonCalibrate.drawButton();
+#else
+ TouchButtonCompensation.drawButton();
#endif
PWMDcMotor::MotorValuesHaveChanged = true; // trigger drawing of values
}