diff --git a/old/wheelEncoder.cpp b/old/wheelEncoder.cpp new file mode 100644 index 0000000..aa9cd89 --- /dev/null +++ b/old/wheelEncoder.cpp @@ -0,0 +1,19 @@ +#include "wheelEncoder.h" + +WheelEncoder::WheelEncoder() { + +} + +void WheelEncoder::initEncoder(uint8_t pinA, uint8_t pinB) { + this->pinA = pinA; + this->pinB = pinB; + + pinMode(this->pinA, INPUT_PULLDOWN); + pinMode(this->pinB, INPUT_PULLDOWN); + + +} + +int64_t WheelEncoder::getCount() { + return this->count; +} \ No newline at end of file diff --git a/old/wheelEncoder.h b/old/wheelEncoder.h new file mode 100644 index 0000000..6c20dd2 --- /dev/null +++ b/old/wheelEncoder.h @@ -0,0 +1,24 @@ +#ifndef WHEEL_ENCODER_H +#define WHEEL_ENCODER_H + +#include +#include +#include +#include + +class WheelEncoder { + public: + WheelEncoder(); + void initEncoder(uint8_t pinA, uint8_t pinB); + int64_t getCount(); + + private: + uint8_t pinA; + uint8_t pinB; + bool init = false; + + volatile int64_t count = 0; + +}; + +#endif // WHEEL_ENCODER_H \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 5376795..1e1e236 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,6 +14,7 @@ board = esp32doit-devkit-v1 framework = arduino monitor_speed = 115200 monitor_port = COM12 -lib_deps = stempedia/DabbleESP32@^1.5.1 - +lib_deps = + stempedia/DabbleESP32@^1.5.1 + madhephaestus/ESP32Encoder@^0.4.0 upload_port = COM12 diff --git a/src/config.h b/src/config.h index 9968266..50701b5 100644 --- a/src/config.h +++ b/src/config.h @@ -1,10 +1,15 @@ -//Pin Configs -#define M_DIR_11 15 -#define M_DIR_12 0 -#define M_PWM_1 5 -#define M_DIR_21 4 -#define M_DIR_22 2 -#define M_PWM_2 18 +//Pin config +#define M_DIR_11 27 +#define M_DIR_12 12 +#define M_PWM_1 13 +#define M_ENCODE_1A 33 +#define M_ENCODE_1B 32 + +#define M_DIR_21 23 +#define M_DIR_22 14 +#define M_PWM_2 22 +#define M_ENCODE_2A 25 +#define M_ENCODE_2B 26 //Motors #define LEFT_MOTOR 1 @@ -17,6 +22,7 @@ #define BACKWARD 2 //MotorControl config +#define RUN_MOTOR_CONTROL_DELAY 300 // Normal 50 ? #define PWM_FREQ 16000 #define PWM_RES 8 #define PWM_CHANNEL_M1 0 @@ -24,4 +30,10 @@ #define UPDATE_TIME 25 #define SPEED_STEPS 5 // A total of 20 levels ( 100 / SPEED_STEPS ) * UPDATE_TIME = 500ms #define PWM_MIN 150 -#define PWM_MAX 245 // Max 98% of 2^PWM_RES \ No newline at end of file +#define PWM_MAX 245 // Max 98% of 2^PWM_RES + +//Encoder config +#define RUN_SPEEDOMETER_DELAY 500 // Normal 10 untested +#define ENC_FILTER 2400 +#define ENC_STEPS 1024 +#define WHEEL_DIAMETER 0.1263 //meter \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index ac5b507..c46c93f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,6 +2,8 @@ #include "config.h" #include "motorControl.h" +#include "moveControl.h" +#include "speedometer.h" #define CUSTOM_SETTINGS #define INCLUDE_GAMEPAD_MODULE @@ -12,6 +14,9 @@ int8_t modSpeed(int8_t dir, int8_t speed); MotorControl left_motor; MotorControl right_motor; +Speedometer speedometer_left; +Speedometer speedometer_right; +MoveControl moveController; uint64_t last_millis = 0; @@ -22,20 +27,27 @@ void setup() { left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12); right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22); + speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B); + speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B); + + moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right); } void loop() { - if (millis() - last_millis > 300) { + if (millis() - last_millis > 700) { gamepadInput(); - Serial.print("Motor Links: "); - left_motor.toString(); - Serial.print("Motor Rechts: "); - right_motor.toString(); + // left_motor.toString(); + // right_motor.toString(); + // Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed()); last_millis = millis(); } + left_motor.runMotorControl(); right_motor.runMotorControl(); + speedometer_left.runSpeedometer(); + speedometer_right.runSpeedometer(); + moveController.runMoveControl(); } void gamepadInput() { @@ -62,6 +74,10 @@ void gamepadInput() { 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) { diff --git a/src/motorControl.cpp b/src/motorControl.cpp index 747b4b8..d10ffcc 100644 --- a/src/motorControl.cpp +++ b/src/motorControl.cpp @@ -26,7 +26,7 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_ } void MotorControl::runMotorControl() { - if (millis() - this->last_millis > 300) { + if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) { // Absolute difference between target_speed and speed uint8_t abs_difference = abs(this->target_speed - this->speed); diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 2c5d952..009dcc7 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -6,52 +6,22 @@ MoveControl::MoveControl() { } -void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor) { +void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor, + Speedometer *left_speedometer, Speedometer *right_speedometer) { this->left_motor = left_motor; this->right_motor = right_motor; + this->left_speedometer = left_speedometer; + this->right_speedometer = right_speedometer; } -void MoveControl::rotate(int16_t degree) { - this->left_motor->setTargetSpeed(0); - this->right_motor->setTargetSpeed(0); - - while ( !(left_motor->isTargetSpeedReached() && right_motor->isTargetSpeedReached()) ) { - delay(5); - } - - uint8_t left = 0; - uint8_t right = 0; - if (degree < 0) { - left = -this->speed; - right = this->speed; - } else if (degree > 0) { - left = this->speed; - right = -this->speed; - } - - this->left_motor->setTargetSpeed(this->speed); - this->right_motor->setTargetSpeed(this->speed); - - delay(abs(degree)); - - this->left_motor->setTargetSpeed(0); - this->right_motor->setTargetSpeed(0); -} - -void MoveControl::rotate(int16_t degree, uint8_t radius) { +void MoveControl::runMoveControl() { } -void MoveControl::forward() { - this->left_motor->setTargetSpeed(this->speed); - this->right_motor->setTargetSpeed(this->speed); -} - -void MoveControl::backward() { - this->left_motor->setTargetSpeed(-this->speed); - this->right_motor->setTargetSpeed(-this->speed); -} - -void MoveControl::setSpeed(uint8_t speed) { +void MoveControl::setSpeed(int8_t speed) { this->speed = speed; } + +void MoveControl::setRotationspeed(double speed) { + this->rotation_speed = speed; +} diff --git a/src/moveControl.h b/src/moveControl.h index 641e893..1e75fdd 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -4,22 +4,25 @@ #include #include "motorControl.h" +#include "speedometer.h" class MoveControl { public: MoveControl(); - void init(MotorControl *left_motor, MotorControl *right_motor); - void rotate(int16_t degree); - void rotate(int16_t degree, uint8_t radius); - void forward(); - void backward(); - void setSpeed(uint8_t speed); + void init(MotorControl *left_motor, MotorControl *right_motor, + Speedometer *left_encoder, Speedometer *right_encoder); + void runMoveControl(); + void setSpeed(int8_t speed); + void setRotationspeed(double speed); private: MotorControl *left_motor; MotorControl *right_motor; + Speedometer *left_speedometer; + Speedometer *right_speedometer; - uint8_t speed = 0; + int8_t speed = 0; + double rotation_speed = 0; }; #endif // MOVE_CONTROL_H \ No newline at end of file diff --git a/src/speedometer.cpp b/src/speedometer.cpp new file mode 100644 index 0000000..4bb07d2 --- /dev/null +++ b/src/speedometer.cpp @@ -0,0 +1,62 @@ +#include "speedometer.h" + +Speedometer::Speedometer() { + +} + +void Speedometer::init(uint8_t pinA, uint8_t pinB) { + ESP32Encoder::useInternalWeakPullResistors=DOWN; + this->encoder.attachFullQuad(pinA, pinB); + this->encoder.setFilter(ENC_FILTER); +} + +void Speedometer::runSpeedometer() { + uint32_t time = millis(); + + //Cancel if delay is not reached + if (time - this->last_millis < RUN_SPEEDOMETER_DELAY) { + return; + } + + uint16_t elapsed_time = time - this->last_millis; + this->last_millis = time; + + int64_t count = encoder.getCount(); + encoder.clearCount(); + + uint8_t direction = STOP; + if (count > 0) { + direction = FORWARD; + } else if (count < 0) { + direction = BACKWARD; + } + + count = abs(count); // 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 + + if (direction == FORWARD) { + this->speed = ms; + } else if (direction == BACKWARD) { + this->speed = ms * (-1); + } else { + this->speed = 0; + } + + //Serial.printf("time: %d, count: %d, n: %f, u: %f, ms: %f \n", time, count, n, u, ms); +} + +double Speedometer::getSpeed() { + return this->speed; +} + +uint8_t Speedometer::getDirection() { + if (this->speed > 0) { + return FORWARD; + } else if (this->speed < 0) { + return BACKWARD; + } else { + return STOP; + } +} \ No newline at end of file diff --git a/src/speedometer.h b/src/speedometer.h new file mode 100644 index 0000000..d92b476 --- /dev/null +++ b/src/speedometer.h @@ -0,0 +1,26 @@ +#ifndef SPEEDOMETER_H +#define SPEEDOMETER_H + +#include +#include +#include + +#include "config.h" + +class Speedometer { + public: + Speedometer(); + void init(uint8_t pinA, uint8_t pinB); + void runSpeedometer(); + double getSpeed(); + uint8_t getDirection(); + + + private: + ESP32Encoder encoder; + + double speed = 0; + uint32_t last_millis = 0; +}; + +#endif // SPEEDOMETER_H \ No newline at end of file