add motors

6 files changed, 188 insertions, 3 deletions

diff --git a/lib/buzzer/buzzer.cpp b/lib/buzzer/buzzer.cpp
index a83ec22..df67ac7 100644
--- a/lib/buzzer/buzzer.cpp
+++ b/lib/buzzer/buzzer.cpp
@@ -4,10 +4,15 @@
 
 #define BUZZER_CHANNEL 3
 #define BUZZER_RESOLUTION 8
+#define BUZZER_FREQ 2000
 
 BUZZER::BUZZER()
 {
-    ledcSetup(BUZZER_CHANNEL, 2000, BUZZER_RESOLUTION);
+}
+
+void BUZZER::init()
+{
+    ledcSetup(BUZZER_CHANNEL, BUZZER_FREQ, BUZZER_RESOLUTION);
     ledcAttachPin(BUZZER_PIN, BUZZER_CHANNEL);
 }
 
@@ -23,7 +28,6 @@ void BUZZER::playTone(uint16_t frequency, uint32_t duration)
 
 void BUZZER::playMelody(const uint16_t melody[][2], size_t length)
 {
-    
     for (size_t i = 0; i < length; i++)
     {
         playTone(melody[i][0], melody[i][1]);
diff --git a/lib/buzzer/buzzer.h b/lib/buzzer/buzzer.h
index f71a90a..717a79c 100644
--- a/lib/buzzer/buzzer.h
+++ b/lib/buzzer/buzzer.h
@@ -8,6 +8,7 @@ class BUZZER
 public:
     BUZZER();
 
+    void init();
     void playTone(uint16_t frequency, uint32_t duration = 0);
     void stop();
     void playMelody(const uint16_t melody[][2], size_t length);
diff --git a/lib/motors/motors.cpp b/lib/motors/motors.cpp
new file mode 100644
index 0000000..d0e71b4
--- /dev/null
+++ b/lib/motors/motors.cpp
@@ -0,0 +1,146 @@
+#include <Arduino.h>
+#include "settings.h"
+#include "motors.h"
+
+#define LEFT_CHANNEL 4
+#define RIGHT_CHANNEL 5
+
+#define MOTOR_FREQ 5000
+#define MOTOR_RES 10
+
+int MOTORS::scaleSpeed(int speed)
+{
+    return map(speed, 0, 100, 0, (1 << MOTOR_RES) - 1);
+}
+
+MOTORS::MOTORS()
+{
+}
+
+void MOTORS::stop()
+{
+    ledcWrite(LEFT_CHANNEL, 0);
+    ledcWrite(RIGHT_CHANNEL, 0);
+}
+
+void MOTORS::leftStop()
+{
+    ledcWrite(LEFT_CHANNEL, 0);
+}
+
+void MOTORS::rightStop()
+{
+    ledcWrite(RIGHT_CHANNEL, 0);
+}
+
+void MOTORS::init()
+{
+    ledcSetup(RIGHT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+    ledcAttachPin(EN34, RIGHT_CHANNEL);
+
+    ledcSetup(LEFT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+    ledcAttachPin(EN12, LEFT_CHANNEL);
+
+    pinMode(IN1, OUTPUT);
+    pinMode(IN2, OUTPUT);
+    pinMode(IN3, OUTPUT);
+    pinMode(IN4, OUTPUT);
+}
+
+void MOTORS::leftForward(int speed, int duration)
+{
+    digitalWrite(IN1, HIGH);
+    digitalWrite(IN2, LOW);
+    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
+
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::leftBackward(int speed, int duration)
+{
+    digitalWrite(IN1, LOW);
+    digitalWrite(IN2, HIGH);
+    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
+
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::rightForward(int speed, int duration)
+{
+    digitalWrite(IN3, HIGH);
+    digitalWrite(IN4, LOW);
+    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+    ledcWrite(LEFT_CHANNEL, 0);
+
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::rightBackward(int speed, int duration)
+{
+    digitalWrite(IN3, LOW);
+    digitalWrite(IN4, HIGH);
+    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+    ledcWrite(LEFT_CHANNEL, 0);
+
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::forward(int speed, int duration)
+{
+    rightForward(speed);
+    leftForward(speed);
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::backward(int speed, int duration)
+{
+    rightBackward(speed);
+    leftBackward(speed);
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::leftTurn(int speed, int duration)
+{
+    rightForward(speed);
+    leftBackward(speed);
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
+
+void MOTORS::rightTurn(int speed, int duration)
+{
+    rightBackward(speed);
+    leftForward(speed);
+    if (duration > 0)
+    {
+        delay(duration);
+        stop();
+    }
+}
\ No newline at end of file
diff --git a/lib/motors/motors.h b/lib/motors/motors.h
new file mode 100644
index 0000000..bcba09c
--- /dev/null
+++ b/lib/motors/motors.h
@@ -0,0 +1,29 @@
+#include <Arduino.h>
+#include "settings.h"
+
+#pragma once
+
+class MOTORS
+{
+public:
+    MOTORS();
+    void init();
+
+    void forward(int speed, int duration = 0);
+    void backward(int speed, int duration = 0);
+
+    void leftForward(int speed, int duration = 0);
+    void leftBackward(int speed, int duration = 0);
+    void rightForward(int speed, int duration = 0);
+    void rightBackward(int speed, int duration = 0);
+
+    void leftTurn(int speed, int duration = 0);
+    void rightTurn(int speed, int duration = 0);
+
+    void leftStop();
+    void rightStop();
+    void stop();
+
+private:
+    int scaleSpeed(int speed);
+};
\ No newline at end of file
diff --git a/lib/rgb/rgb.cpp b/lib/rgb/rgb.cpp
index 249e7f0..e66d264 100644
--- a/lib/rgb/rgb.cpp
+++ b/lib/rgb/rgb.cpp
@@ -14,8 +14,12 @@ int RGB::scaleColor(uint8_t val)
 {
   return map(val, 0, 255, 0, (1 << PWM_RES) - 1);
 }
-
 RGB::RGB()
+    : currentR(0), currentG(0), currentB(0)
+{
+}
+
+void RGB::init()
 {
   ledcSetup(RED_CHANNEL, PWM_FREQ, PWM_RES);
   ledcAttachPin(RED_PIN, RED_CHANNEL);
diff --git a/lib/rgb/rgb.h b/lib/rgb/rgb.h
index 3850f0a..fddeae8 100644
--- a/lib/rgb/rgb.h
+++ b/lib/rgb/rgb.h
@@ -11,6 +11,7 @@ public:
     uint8_t currentB;
 
     RGB();
+    void init();
     void setColorRGB(uint8_t red, uint8_t green, uint8_t blue, bool saveColor);
     void fadeColor(uint8_t red, uint8_t green, uint8_t blue, int duration);
     void blink(int cycles, int interval);