diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 195c077..c4c6f7d 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -23,6 +23,7 @@ #define CHANNEL_LEFT LEDC_CHANNEL_3 #define CHANNEL_RIGHT LEDC_CHANNEL_4 #define DUTY_RES LEDC_TIMER_13_BIT // Set duty resolution to 13 bits +#define PHASE_180_DEG (1 << (DUTY_RES))/2 // (2**DUTY_RES)/2 Set phase to 180 degrees #define FREQUENCY (5000) // Frequency in Hertz. Set frequency at 5 kHz #define DEFAULT_BASE_VALUE 3900 @@ -32,7 +33,7 @@ class Motor{ public: - Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel); + Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase=0); /** * @brief Initializes the motor @@ -80,12 +81,17 @@ class Motor{ ledc_channel_t channel; Power* powerManager; uint16_t duty; + // The phase of the pwm signal, expressed as a number betweeen 0 and + // the maximum value representable by the pwm timer resolution. + int phase; }; class Motion{ protected: static inline uint16_t RIGHT_MOTOR_DUTY = DEFAULT_BASE_VALUE; static inline uint16_t LEFT_MOTOR_DUTY = DEFAULT_BASE_VALUE; + static inline int LEFT_MOTOR_PHASE = 0; + static inline int RIGHT_MOTOR_PHASE = PHASE_180_DEG; static const int MOTOR_RIGHT_PIN = 11; static const int MOTOR_LEFT_PIN = 12; static void moveTask(void * args); @@ -101,8 +107,8 @@ protected: public: //Instances of the motors, so they can also be used from outside to set values for the motors directly. - static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT); - static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT); + static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT,LEFT_MOTOR_PHASE); + static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT,RIGHT_MOTOR_PHASE); //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection) static inline MotionDetection detection; diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp index ea6e9db..6482ded 100644 --- a/src/motion/Motor.cpp +++ b/src/motion/Motor.cpp @@ -4,24 +4,27 @@ #define MOTOR_LEFT_PIN 12 #define MOTOR_RIGHT_PIN 11 -Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel) { - this->pin = pin; - this->channel = channel; - this->timer = timer; - this->duty = 0; +Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase){ + this->pin = pin; + this->channel = channel; + this->timer = timer; + this->duty = 0; + this->phase = phase; }; -void Motor::begin(void) { - pinMode(this->pin, OUTPUT); - ledc_channel_config_t channelConfig = {.gpio_num = this->pin, - .speed_mode = LEDC_MODE, - .channel = this->channel, - .intr_type = LEDC_INTR_DISABLE, - .timer_sel = this->timer, - .duty = 0, // Set duty to 0% - .hpoint = 0}; - ledc_channel_config(&channelConfig); - Serial.println("Motor begin done"); +void Motor::begin(void){ + pinMode(this->pin,OUTPUT); + ledc_channel_config_t channelConfig = { + .gpio_num = this->pin, + .speed_mode = LEDC_MODE, + .channel = this->channel, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = this->timer, + .duty = 0, // Set duty to 0% + .hpoint = this->phase + }; + ledc_channel_config(&channelConfig); + Serial.println("Motor begin done"); }; bool Motor::setSpeed(uint16_t duty) {