diff --git a/docs/usage.md b/docs/usage.md index 3e993ed..d6d2669 100644 --- a/docs/usage.md +++ b/docs/usage.md @@ -134,6 +134,20 @@ Before flight you need to calibrate the accelerometer: 1. Access the console using QGroundControl (recommended) or Serial Monitor. 2. Type `ca` command there and follow the instructions. +### Setup motors + +If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively). + +If using brushless motors and ESCs: + +1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical). +2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical). + +Reboot the drone to apply the changes. + +> [!CAUTION] +> **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them. + ### Check everything works 1. Check the IMU is working: perform `imu` command and check its output: diff --git a/flix/motors.ino b/flix/motors.ino index 3dde15c..1dee5fa 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -1,24 +1,19 @@ // Copyright (c) 2023 Oleg Kalachev // Repository: https://github.com/okalachev/flix -// Motors output control using MOSFETs -// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400) +// PWM control for motors #include "util.h" -#define MOTOR_0_PIN 12 // rear left -#define MOTOR_1_PIN 13 // rear right -#define MOTOR_2_PIN 14 // front right -#define MOTOR_3_PIN 15 // front left - -#define PWM_FREQUENCY 78000 -#define PWM_RESOLUTION 10 -#define PWM_STOP 0 -#define PWM_MIN 0 -#define PWM_MAX 1000000 / PWM_FREQUENCY - float motors[4]; // normalized motor thrusts in range [0..1] +int motorPins[4] = {12, 13, 14, 15}; // 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 MOTOR_REAR_LEFT = 0; const int MOTOR_REAR_RIGHT = 1; const int MOTOR_FRONT_RIGHT = 2; @@ -26,30 +21,30 @@ const int MOTOR_FRONT_LEFT = 3; void setupMotors() { print("Setup Motors\n"); - // configure pins - ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION); - ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION); - ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION); - ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION); - + for (int i = 0; i < 4; i++) { + ledcAttach(motorPins[i], pwmFrequency, pwmResolution); + } sendMotors(); print("Motors initialized\n"); } -int getDutyCycle(float value) { - value = constrain(value, 0, 1); - float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX); - if (value == 0) pwm = PWM_STOP; - float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1); - return round(duty); +void sendMotors() { + for (int i = 0; i < 4; i++) { + ledcWrite(motorPins[i], getDutyCycle(motors[i])); + } } -void sendMotors() { - ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0])); - ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1])); - ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2])); - ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3])); +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() { diff --git a/flix/parameters.ino b/flix/parameters.ino index 498bfb7..77aa21a 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -63,6 +63,16 @@ Parameter parameters[] = { // estimate {"EST_ACC_WEIGHT", &accWeight}, {"EST_RATES_LPF_A", &ratesFilter.alpha}, + // motors + {"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]}, + {"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]}, + {"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]}, + {"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]}, + {"MOT_PWM_FREQ", &pwmFrequency}, + {"MOT_PWM_RES", &pwmResolution}, + {"MOT_PWM_STOP", &pwmStop}, + {"MOT_PWM_MIN", &pwmMin}, + {"MOT_PWM_MAX", &pwmMax}, // rc {"RC_ZERO_0", &channelZero[0]}, {"RC_ZERO_1", &channelZero[1]}, diff --git a/gazebo/flix.h b/gazebo/flix.h index 8d13f5e..b3b3cb7 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -32,6 +32,7 @@ void controlRates(); void controlTorque(); const char* getModeName(); void sendMotors(); +int getDutyCycle(float value); bool motorsActive(); void testMotor(int n); void print(const char* format, ...);