Added move Controll

src/motorControl.cpp

@@ -0,0 +1,155 @@
+#include "motorControl.h"
+
+#include <Arduino.h>
+
+#include "config.h"
+
+MotorControl::MotorControl() {
+
+}
+
+void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
+    this->pwm_pin = pwm_pin;
+    this->pwm_channel = pwm_channel;
+    this->direction_pin_1 = direction_pin_1;
+    this->direction_pin_2 = direction_pin_2;
+
+    pinMode(this->direction_pin_1, OUTPUT);
+    pinMode(this->direction_pin_2, OUTPUT);
+
+    digitalWrite(this->direction_pin_1, LOW);
+    digitalWrite(this->direction_pin_2, LOW);
+
+    ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
+    ledcAttachPin(this->pwm_pin, this->pwm_channel);
+    ledcWrite(this->pwm_channel, 0);
+}
+
+void MotorControl::runMotorControl() {
+    uint64_t now_millis = millis();
+    if (now_millis - this->last_millis > UPDATE_TIME) {
+
+        // Absolute difference between target_speed and speed
+        uint8_t abs_difference = abs(this->target_speed - this->speed);
+
+        // Difference between target_speed and speed
+        int16_t difference = this->target_speed - this->speed;
+
+        // Check that the target speed is close to 0 and that the abs_difference is lower than SPEED_STEPS
+        if (abs(this->target_speed) <  SPEED_STEPS && abs_difference < SPEED_STEPS) {
+            this->setSpeed(0);
+            return;
+        }
+
+        // Correct speed
+        if (abs_difference < SPEED_STEPS) {
+            return;
+        }
+
+        // Positive or negative tagret speed
+        if (this->target_speed >= 0) {
+
+            // Positive or negative speed
+            if (this->speed >= 0) {
+                if (difference > 0) {
+                    this->accelerate(SPEED_STEPS);
+                } else {
+                    this->accelerate(-SPEED_STEPS);
+                }
+            } else {
+                this->accelerate(SPEED_STEPS);
+            }
+
+        } else {
+
+            // Positive or negative speed
+            if (this->speed >= 0) {
+                this->accelerate(-SPEED_STEPS);
+            } else {
+                if (difference > 0) {
+                    this->accelerate(SPEED_STEPS);
+                } else {
+                    this->accelerate(-SPEED_STEPS);
+                }
+            }
+        }
+    }
+
+    this->last_millis = millis();
+}
+
+void MotorControl::setTargetSpeed(int16_t speed) {
+    //TODO: Exceptionhandling
+    if (speed <= 100 || speed <= -100) {
+        this->target_speed = speed;
+    } else {
+        Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
+    }
+}
+
+void MotorControl::stop() {
+    this->target_speed = 0;
+}
+
+void MotorControl::emergencyStop() {
+    setSpeed(0);
+}
+
+void MotorControl::toString() {
+    Serial.printf("Target speed: %d, speed: %d, direction: %d \n", this->target_speed, this->speed, this->direction);
+}
+
+int16_t MotorControl::getSpeed() {
+    return this->speed;
+}
+
+bool MotorControl::isTargetSpeedReached() {
+    if (this->target_speed == this->speed) {
+        return true;
+    }
+    return false;
+}
+
+void MotorControl::setSpeed(int16_t speed) {
+    //TODO: Exceptionhandling
+    if (speed <= 100 || speed <= -100) {
+        this->speed = speed;
+    } else {
+        Serial.println("Invalid Argument in MotorControl::setSpeed");
+        return;
+    }
+
+    if (speed == 0) {
+        this->direction = 0;
+        digitalWrite(this->direction_pin_1, LOW);
+        digitalWrite(this->direction_pin_2, LOW);
+        ledcWrite(this->pwm_channel, 0);
+        return;
+    }
+
+    uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);
+
+    if ((this->direction == 1 || this->direction == 0) && speed < 0){  // new direction backward
+        this->direction = 2;
+        digitalWrite(this->direction_pin_1, LOW);
+        digitalWrite(this->direction_pin_2, HIGH);
+    } else if ((this->direction == 2 || this->direction == 0) && speed > 0){  // new direction forward
+        this->direction = 1;
+        digitalWrite(this->direction_pin_1, HIGH);
+        digitalWrite(this->direction_pin_2, LOW);
+    }
+
+    ledcWrite(this->pwm_channel, pwm_val);
+    Serial.printf("pwm_val: %d, ", pwm_val);
+}
+
+void MotorControl::accelerate(int8_t acc) {
+    //TODO: Exceptionhandling
+    //TODO: make a stop befor a direction change
+    if (abs(acc) > 2 * SPEED_STEPS) {
+        Serial.println("Invalid Argument in MotorControl::accelerate");
+        return;
+    }
+
+    this->setSpeed(speed + acc);
+}