67 lines
1.5 KiB
C++
67 lines
1.5 KiB
C++
#include <Arduino.h>
|
|
#include "config.h"
|
|
#include "actuators.h"
|
|
|
|
bool actuatorsInit() {
|
|
pinMode(PIN_LB, OUTPUT);
|
|
pinMode(PIN_LF, OUTPUT);
|
|
pinMode(PIN_RB, OUTPUT);
|
|
pinMode(PIN_RF, OUTPUT);
|
|
|
|
ledcSetup(PWM_CH_L, PWM_FREQ, PWM_RES);
|
|
ledcSetup(PWM_CH_R, PWM_FREQ, PWM_RES);
|
|
|
|
ledcAttachPin(L_PWM_PIN, PWM_CH_L);
|
|
ledcAttachPin(R_PWM_PIN, PWM_CH_R);
|
|
|
|
Serial.println("Actuators initialized");
|
|
Serial.printf(" L_PWM → GPIO%d\n", L_PWM_PIN);
|
|
Serial.printf(" R_PWM → GPIO%d\n", R_PWM_PIN);
|
|
Serial.printf(" L_DIR → GPIO%d, %d\n", PIN_LB, PIN_LF);
|
|
Serial.printf(" R_DIR → GPIO%d, %d\n", PIN_RB, PIN_RF);
|
|
|
|
actuatorsSetSpeed(150, 150);
|
|
actuatorsStop();
|
|
return true;
|
|
}
|
|
|
|
void actuatorsStop() {
|
|
digitalWrite(PIN_LB, HIGH);
|
|
digitalWrite(PIN_LF, HIGH);
|
|
digitalWrite(PIN_RB, HIGH);
|
|
digitalWrite(PIN_RF, HIGH);
|
|
}
|
|
|
|
void actuatorsForward() {
|
|
digitalWrite(PIN_LB, LOW);
|
|
digitalWrite(PIN_LF, HIGH);
|
|
digitalWrite(PIN_RB, LOW);
|
|
digitalWrite(PIN_RF, HIGH);
|
|
}
|
|
|
|
void actuatorsBack() {
|
|
digitalWrite(PIN_LB, HIGH);
|
|
digitalWrite(PIN_LF, LOW);
|
|
digitalWrite(PIN_RB, HIGH);
|
|
digitalWrite(PIN_RF, LOW);
|
|
}
|
|
|
|
void actuatorsLeft() {
|
|
digitalWrite(PIN_LB, HIGH);
|
|
digitalWrite(PIN_LF, LOW);
|
|
digitalWrite(PIN_RB, LOW);
|
|
digitalWrite(PIN_RF, HIGH);
|
|
}
|
|
|
|
void actuatorsRight() {
|
|
digitalWrite(PIN_LB, LOW);
|
|
digitalWrite(PIN_LF, HIGH);
|
|
digitalWrite(PIN_RB, HIGH);
|
|
digitalWrite(PIN_RF, LOW);
|
|
}
|
|
|
|
void actuatorsSetSpeed(int l, int r) {
|
|
ledcWrite(PWM_CH_L, constrain(l, 0, 255));
|
|
ledcWrite(PWM_CH_R, constrain(r, 0, 255));
|
|
}
|