diff options
| author | EnricoGuccii <partyka.003@proton.me> | 2026-05-29 23:34:00 +0200 |
|---|---|---|
| committer | EnricoGuccii <partyka.003@proton.me> | 2026-05-29 23:34:00 +0200 |
| commit | d3b283c69e9cf5ac6fe18c25be77a9b0488b9454 (patch) | |
| tree | b39c0bda024eb053a0daab7b938be72a7d3e470e /lib/motors | |
| parent | 4bd0a060d100c7fb9ad657c939b954b081b62016 (diff) | |
new file structure, and buzzer removed
Diffstat (limited to 'lib/motors')
| -rw-r--r-- | lib/motors/motors.cpp | 146 | ||||
| -rw-r--r-- | lib/motors/motors.h | 29 |
2 files changed, 175 insertions, 0 deletions
diff --git a/lib/motors/motors.cpp b/lib/motors/motors.cpp new file mode 100644 index 0000000..843de22 --- /dev/null +++ b/lib/motors/motors.cpp @@ -0,0 +1,146 @@ +#include <Arduino.h>
+#include "settings.h"
+#include "motors.h"
+
+#define RIGHT_CHANNEL 4
+#define LEFT_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(RIGHT_CHANNEL, 0);
+ ledcWrite(LEFT_CHANNEL, 0);
+}
+
+void MOTORS::rightStop()
+{
+ ledcWrite(RIGHT_CHANNEL, 0);
+}
+
+void MOTORS::leftStop()
+{
+ ledcWrite(LEFT_CHANNEL, 0);
+}
+
+void MOTORS::init()
+{
+ ledcSetup(LEFT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+ ledcAttachPin(EN34, LEFT_CHANNEL);
+
+ ledcSetup(RIGHT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+ ledcAttachPin(EN12, RIGHT_CHANNEL);
+
+ pinMode(IN1, OUTPUT);
+ pinMode(IN2, OUTPUT);
+ pinMode(IN3, OUTPUT);
+ pinMode(IN4, OUTPUT);
+}
+
+void MOTORS::leftBackward(int speed, int duration)
+{
+ digitalWrite(IN1, HIGH);
+ digitalWrite(IN2, LOW);
+ ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::leftForward(int speed, int duration)
+{
+ digitalWrite(IN1, LOW);
+ digitalWrite(IN2, HIGH);
+ ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::rightBackward(int speed, int duration)
+{
+ digitalWrite(IN3, HIGH);
+ digitalWrite(IN4, LOW);
+ ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
+ ledcWrite(RIGHT_CHANNEL, 0);
+
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::rightForward(int speed, int duration)
+{
+ digitalWrite(IN3, LOW);
+ digitalWrite(IN4, HIGH);
+ ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
+ ledcWrite(RIGHT_CHANNEL, 0);
+
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::forward(int speed, int duration)
+{
+ leftForward(speed);
+ rightForward(speed);
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::backward(int speed, int duration)
+{
+ leftBackward(speed);
+ rightBackward(speed);
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::leftTurn(int speed, int duration)
+{
+ leftForward(speed);
+ rightBackward(speed);
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
+
+void MOTORS::rightTurn(int speed, int duration)
+{
+ leftBackward(speed);
+ rightForward(speed);
+ if (duration > 0)
+ {
+ delay(duration);
+ stop();
+ }
+}
diff --git a/lib/motors/motors.h b/lib/motors/motors.h new file mode 100644 index 0000000..b7cc7f1 --- /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 |