Added move Controll

src/moveControl.cpp

@@ -0,0 +1,57 @@
+#include "moveControl.h"
+
+#include <Arduino.h>
+
+MoveControl::MoveControl() {
+
+}
+
+void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor) {
+    this->left_motor = left_motor;
+    this->right_motor = right_motor;
+}
+
+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::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) {
+    this->speed = speed;
+}