mirror of
https://github.com/okalachev/flix.git
synced 2026-09-06 00:10:58 +00:00
Oleg Kalachev
647d68f1b7
The original names are overly verbose. Make motor pins disabled by default on all platforms except classic esp32. Make config more clear.
64 lines
1.7 KiB
Arduino
64 lines
1.7 KiB
Arduino
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// PWM control for motors
|
|
|
|
#include "util.h"
|
|
|
|
float motors[4]; // normalized motor thrusts in range [0..1]
|
|
|
|
int motorPins[4] = {-1, -1, -1, -1}; // default pin numbers
|
|
int pwmFrequency = 78000;
|
|
int pwmResolution = 10;
|
|
int pwmStop = 0;
|
|
int pwmMin = 0;
|
|
int pwmMax = -1; // -1 means duty cycle mode
|
|
|
|
const int MOT_RL = 0, MOT_RR = 1, MOT_FR = 2, MOT_FL = 3;
|
|
|
|
void setupMotors() {
|
|
print("Setup motors\n");
|
|
// Configure pins
|
|
for (int i = 0; i < 4; i++) {
|
|
if (motorPins[i] < 0) continue; // skip unassigned motors
|
|
ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
|
|
pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
|
|
}
|
|
sendMotors();
|
|
}
|
|
|
|
void sendMotors() {
|
|
for (int i = 0; i < 4; i++) {
|
|
if (motorPins[i] < 0) continue; // skip unassigned motors
|
|
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
|
|
}
|
|
}
|
|
|
|
int getDutyCycle(float value) {
|
|
value = constrain(value, 0, 1);
|
|
|
|
if (pwmMax >= 0) { // pwm mode
|
|
float pwm = mapf(value, 0, 1, pwmMin, pwmMax);
|
|
if (value == 0) pwm = pwmStop;
|
|
float duty = mapf(pwm, 0, 1000000 / pwmFrequency, 0, (1 << pwmResolution) - 1);
|
|
return round(duty);
|
|
} else { // duty cycle mode
|
|
return round(value * ((1 << pwmResolution) - 1));
|
|
}
|
|
}
|
|
|
|
bool motorsActive() {
|
|
return motors[0] != 0 || motors[1] != 0 || motors[2] != 0 || motors[3] != 0;
|
|
}
|
|
|
|
void testMotor(int n, float thrust) {
|
|
print("Testing motor %d\n", n);
|
|
motors[n] = thrust;
|
|
delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
|
|
sendMotors();
|
|
pause(3);
|
|
motors[n] = 0;
|
|
sendMotors();
|
|
print("Done\n");
|
|
}
|