diff options
Diffstat (limited to 'lib/motors/motors.cpp')
| -rw-r--r-- | lib/motors/motors.cpp | 143 |
1 files changed, 59 insertions, 84 deletions
diff --git a/lib/motors/motors.cpp b/lib/motors/motors.cpp index 843de22..1ad4fb2 100644 --- a/lib/motors/motors.cpp +++ b/lib/motors/motors.cpp @@ -8,139 +8,114 @@ #define MOTOR_FREQ 5000
#define MOTOR_RES 10
-int MOTORS::scaleSpeed(int speed)
-{
+MOTORS::MOTORS() {}
+
+int MOTORS::scaleSpeed(int speed) {
return map(speed, 0, 100, 0, (1 << MOTOR_RES) - 1);
}
-MOTORS::MOTORS()
-{
-}
+void MOTORS::init() {
+ ledcSetup(LEFT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+ ledcAttachPin(EN12, LEFT_CHANNEL);
-void MOTORS::stop()
-{
- ledcWrite(RIGHT_CHANNEL, 0);
- ledcWrite(LEFT_CHANNEL, 0);
+ ledcSetup(RIGHT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+ ledcAttachPin(EN34, RIGHT_CHANNEL);
+
+ pinMode(IN1, OUTPUT);
+ pinMode(IN2, OUTPUT);
+ pinMode(IN3, OUTPUT);
+ pinMode(IN4, OUTPUT);
}
-void MOTORS::rightStop()
-{
+void MOTORS::stop() {
+ ledcWrite(LEFT_CHANNEL, 0);
ledcWrite(RIGHT_CHANNEL, 0);
}
-void MOTORS::leftStop()
-{
+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::rightStop() {
+ ledcWrite(RIGHT_CHANNEL, 0);
}
-void MOTORS::leftBackward(int speed, int duration)
-{
+void MOTORS::leftBackward(int speed, int duration) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
- ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+ ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
- if (duration > 0)
- {
- delay(duration);
- stop();
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
+ leftStop();
}
}
-void MOTORS::leftForward(int speed, int duration)
-{
+void MOTORS::leftForward(int speed, int duration) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
- ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+ ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
- if (duration > 0)
- {
- delay(duration);
- stop();
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
+ leftStop();
}
}
-void MOTORS::rightBackward(int speed, int duration)
-{
+void MOTORS::rightBackward(int speed, int duration) {
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
- ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
- ledcWrite(RIGHT_CHANNEL, 0);
+ ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
- if (duration > 0)
- {
- delay(duration);
- stop();
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
+ rightStop();
}
}
-void MOTORS::rightForward(int speed, int duration)
-{
+void MOTORS::rightForward(int speed, int duration) {
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
- ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
- ledcWrite(RIGHT_CHANNEL, 0);
+ ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
- if (duration > 0)
- {
- delay(duration);
- stop();
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
+ rightStop();
}
}
-void MOTORS::forward(int speed, int duration)
-{
- leftForward(speed);
- rightForward(speed);
- if (duration > 0)
- {
- delay(duration);
- stop();
+void MOTORS::forward(int speed, int duration) {
+ leftForward(speed, 0);
+ rightForward(speed, 0);
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
+ stop();
}
}
-void MOTORS::backward(int speed, int duration)
-{
- leftBackward(speed);
- rightBackward(speed);
- if (duration > 0)
- {
- delay(duration);
+void MOTORS::backward(int speed, int duration) {
+ leftBackward(speed, 0);
+ rightBackward(speed, 0);
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
stop();
}
}
-void MOTORS::leftTurn(int speed, int duration)
-{
- leftForward(speed);
- rightBackward(speed);
- if (duration > 0)
- {
- delay(duration);
+void MOTORS::leftTurn(int speed, int duration) {
+ leftBackward(speed, 0);
+ rightForward(speed, 0);
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
stop();
}
}
-void MOTORS::rightTurn(int speed, int duration)
-{
- leftBackward(speed);
- rightForward(speed);
- if (duration > 0)
- {
- delay(duration);
+void MOTORS::rightTurn(int speed, int duration) {
+ leftForward(speed, 0);
+ rightBackward(speed, 0);
+ if (duration > 0) {
+ vTaskDelay(duration / portTICK_PERIOD_MS);
stop();
}
}
|