Implement speedometer and encoder

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;
+}

old/wheelEncoder.h

@@ -0,0 +1,24 @@
+#ifndef WHEEL_ENCODER_H
+#define WHEEL_ENCODER_H
+
+#include <cstdint>
+#include <Arduino.h>
+#include <driver/gpio.h>
+#include <driver/pcnt.h>
+
+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

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

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
@@ -25,3 +31,9 @@
 #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
+
+//Encoder config
+#define RUN_SPEEDOMETER_DELAY 500 // Normal 10 untested
+#define ENC_FILTER 2400
+#define ENC_STEPS 1024
+#define WHEEL_DIAMETER 0.1263 //meter

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) {

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);

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;
+}

src/moveControl.h

@@ -4,22 +4,25 @@
 #include <cstdint>
 
 #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

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;
+    }
+}

src/speedometer.h

@@ -0,0 +1,26 @@
+#ifndef SPEEDOMETER_H
+#define SPEEDOMETER_H
+
+#include <cstdint>
+#include <ESP32Encoder.h>
+#include <math.h>
+
+#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