From a33b4a2e4c4105160a996678d1aceaa914af2ab2 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 1 Apr 2021 20:59:51 +0200 Subject: [PATCH] improved moveControl --- src/main.cpp | 68 +++++++------------------------------------- src/motorControl.cpp | 21 ++++++++++++-- src/motorControl.h | 3 ++ src/moveControl.cpp | 63 ++++++++++++++++++++++++++++++++++++++-- src/moveControl.h | 11 +++++-- src/speedometer.cpp | 31 +++++++++++++++++--- src/speedometer.h | 8 +++++- 7 files changed, 136 insertions(+), 69 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c46c93f..0543432 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,7 +10,6 @@ #include void gamepadInput(); -int8_t modSpeed(int8_t dir, int8_t speed); MotorControl left_motor; MotorControl right_motor; @@ -34,88 +33,43 @@ void setup() { } void loop() { - if (millis() - last_millis > 700) { + if (millis() - last_millis > 400) { gamepadInput(); // left_motor.toString(); + // Serial.printf("speed: %f, ", speedometer_right.getSpeed()); // right_motor.toString(); // Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed()); last_millis = millis(); } - left_motor.runMotorControl(); + // left_motor.runMotorControl(); right_motor.runMotorControl(); - speedometer_left.runSpeedometer(); + // speedometer_left.runSpeedometer(); speedometer_right.runSpeedometer(); - moveController.runMoveControl(); + // moveController.runMoveControl(); } void gamepadInput() { Dabble.processInput(); //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile. //Serial.print("KeyPressed: "); - static int8_t speed_left = 0; if (GamePad.isUpPressed()) { - speed_left = modSpeed(1, speed_left); + moveController.setSpeed(1.3); } else if (GamePad.isDownPressed()) { - speed_left = modSpeed(-1, speed_left); + moveController.setSpeed(0); } else { - speed_left = modSpeed(0, speed_left); } - static int8_t speed_right = 0; if (GamePad.isTrianglePressed()) { - speed_right = modSpeed(1, speed_right); + right_motor.setTargetSpeed(20); } else if (GamePad.isCrossPressed()) { - speed_right = modSpeed(-1, speed_right); - } else { - speed_right = modSpeed(0, speed_right); + right_motor.setTargetSpeed(30); + } else if (GamePad.isCirclePressed()){ + right_motor.setTargetSpeed(0); } - left_motor.setTargetSpeed(speed_left); - right_motor.setTargetSpeed(speed_right); - // int a = GamePad.getAngle(); // int b = GamePad.getRadius(); // Serial.printf("Winkel: %d, Radius: %d \n", a, b); } - -int8_t modSpeed(int8_t dir, int8_t speed) { - int8_t new_speed = 0; - if (dir == 1) { - if (speed > 0) { - new_speed = speed + 5; - } else if (speed == 0) { - new_speed = 5; - } else { - new_speed = speed + 5; - } - } else if (dir == 0) { - if (speed > 0) { - new_speed = speed - 5; - } else if (speed == 0) { - new_speed = 0; - } else { - new_speed = speed + 5; - } - } else if (dir == -1) { - if (speed > 0) { - new_speed = speed - 5; - } else if (speed == 0) { - new_speed = -5; - } else { - new_speed = speed - 5; - } - } - - if (new_speed < -100) { - new_speed = -100; - } - - if (new_speed > 100) { - new_speed = 100; - } - - return new_speed; -} - diff --git a/src/motorControl.cpp b/src/motorControl.cpp index d10ffcc..9dbf870 100644 --- a/src/motorControl.cpp +++ b/src/motorControl.cpp @@ -78,7 +78,7 @@ void MotorControl::runMotorControl() { void MotorControl::setTargetSpeed(int16_t speed) { //TODO: Exceptionhandling - if (speed <= 100 || speed <= -100) { + if (speed <= 100 && speed >= -100) { this->target_speed = speed; } else { Serial.println("Invalid Argument in MotorControl::setTargetSpeed"); @@ -101,10 +101,25 @@ int16_t MotorControl::getSpeed() { return this->speed; } +int16_t MotorControl::getTargetSpeed() { + return this->target_speed; +} + bool MotorControl::isTargetSpeedReached() { - if (this->target_speed == this->speed) { + if (this->target_speed == this->speed) + return true; + return false; +} + +bool MotorControl::isAccelerationPositive() { + if (speed < target_speed) + return true; + return false; +} + +bool MotorControl::isAccelerationNegative() { + if (speed > target_speed) return true; - } return false; } diff --git a/src/motorControl.h b/src/motorControl.h index d08b442..ef5f97f 100644 --- a/src/motorControl.h +++ b/src/motorControl.h @@ -15,7 +15,10 @@ class MotorControl { void toString(); int16_t getSpeed(); + int16_t getTargetSpeed(); bool isTargetSpeedReached(); + bool isAccelerationPositive(); + bool isAccelerationNegative(); private: void setSpeed(int16_t speed); diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 009dcc7..759ec93 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -15,13 +15,72 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor, } void MoveControl::runMoveControl() { + static uint64_t last_millis = 0; + if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) { + return; + } + last_millis = millis(); + + this->calcWheelSpeed(); + //this->regulateMotorPower(this->left_motor, this->left_speedometer, this->wheelspeed_left_target); + this->regulateMotorPower(this->right_motor, this->right_speedometer, this->wheelspeed_right_target); } -void MoveControl::setSpeed(int8_t speed) { - this->speed = speed; +void MoveControl::setSpeed(double speed) { + this->x_speed = speed; } void MoveControl::setRotationspeed(double speed) { this->rotation_speed = speed; } + +void MoveControl::calcWheelSpeed() { + // original formula: + // (1 / r) / 1 b \ / x \ = / Xl \ + // \ 1 -b / \ T / \ Xr / + + // (1 / r) * 1 + const static double A = 15.82278481; + // (1 / r) * b + const static double B = 2.096518987; + + this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2); + this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2); +} + +void MoveControl::regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed) { + if (tar_speed > 0) { + // Direction FORWARD + if (cur_speed->getSpeed() - tar_speed < 0) { + // Too fast + if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { + motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS); + } + } else { + // Too slow + if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { + motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); + } + } + + } else if (tar_speed < 0) { + // Direction BACKWARD + if (cur_speed->getSpeed() - tar_speed > 0) { + // Too fast + if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { + motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS); + } + } else { + // Too slow + if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { + motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); + } + } + + + } else { + // Direction STOP + motor->setTargetSpeed(0); + } +} diff --git a/src/moveControl.h b/src/moveControl.h index 1e75fdd..0458cbc 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -5,6 +5,7 @@ #include "motorControl.h" #include "speedometer.h" +#include "config.h" class MoveControl { public: @@ -12,17 +13,23 @@ class MoveControl { void init(MotorControl *left_motor, MotorControl *right_motor, Speedometer *left_encoder, Speedometer *right_encoder); void runMoveControl(); - void setSpeed(int8_t speed); + void setSpeed(double speed); void setRotationspeed(double speed); private: + void calcWheelSpeed(); + static void regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed); + MotorControl *left_motor; MotorControl *right_motor; Speedometer *left_speedometer; Speedometer *right_speedometer; - int8_t speed = 0; + double x_speed = 0; double rotation_speed = 0; + + double wheelspeed_left_target = 0; + double wheelspeed_right_target = 0; }; #endif // MOVE_CONTROL_H \ No newline at end of file diff --git a/src/speedometer.cpp b/src/speedometer.cpp index 4bb07d2..bbf2f58 100644 --- a/src/speedometer.cpp +++ b/src/speedometer.cpp @@ -1,7 +1,9 @@ #include "speedometer.h" Speedometer::Speedometer() { - + for (int i = 0; i < BUF_SIZE; i++) { + this->buf[i] = 0; + } } void Speedometer::init(uint8_t pinA, uint8_t pinB) { @@ -21,7 +23,8 @@ void Speedometer::runSpeedometer() { uint16_t elapsed_time = time - this->last_millis; this->last_millis = time; - int64_t count = encoder.getCount(); + int16_t count = encoder.getCount(); + this->addValToBuf(count); encoder.clearCount(); uint8_t direction = STOP; @@ -31,7 +34,7 @@ void Speedometer::runSpeedometer() { direction = BACKWARD; } - count = abs(count); // Absolute time in milliseconds + count = abs(this->getAverage()); // Absolute time in milliseconds double n = (double)count / ENC_STEPS; // Wheel revolutions in absolute time double u = (double)n / ((double)elapsed_time / 1000); // Wheel revolutions per second double ms = u * (WHEEL_DIAMETER * PI); // Speed in m/s @@ -44,7 +47,9 @@ void Speedometer::runSpeedometer() { this->speed = 0; } - //Serial.printf("time: %d, count: %d, n: %f, u: %f, ms: %f \n", time, count, n, u, ms); + Serial.printf("time: %d, ", time); + Serial.printf("count: %d, ",count); + Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms); } double Speedometer::getSpeed() { @@ -59,4 +64,22 @@ uint8_t Speedometer::getDirection() { } else { return STOP; } +} + +void Speedometer::addValToBuf(int16_t val) { + this->buf[this->bufPos] = val; + this->bufPos++; + + if (this->bufPos == BUF_SIZE) { + this->bufPos = 0; + } +} + +int16_t Speedometer::getAverage() { + int16_t sum = 0; + for (int i = 0; i < BUF_SIZE; i++) { + sum += this->buf[i]; + } + + return sum / BUF_SIZE; } \ No newline at end of file diff --git a/src/speedometer.h b/src/speedometer.h index d92b476..c0edc47 100644 --- a/src/speedometer.h +++ b/src/speedometer.h @@ -17,10 +17,16 @@ class Speedometer { private: + void addValToBuf(int16_t val); + int16_t getAverage(); + ESP32Encoder encoder; - + double speed = 0; uint32_t last_millis = 0; + + uint8_t bufPos = 0; + int16_t buf[BUF_SIZE]; }; #endif // SPEEDOMETER_H \ No newline at end of file