robotic-manipulator/arduino/robot_arm/pca9685_servo_driver.h at master · mbed92/robotic-manipulator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#ifndef ROBOT_ARM_PCA9685_SERVO_DRIVER_H
#define ROBOT_ARM_PCA9685_SERVO_DRIVER_H

#include <Adafruit_PWMServoDriver.h>
#include <Arduino.h>

struct Pca9685ServoDriverConfig {
    uint16_t servo_frequency_hz;
};

class Pca9685ServoDriver {
  public:
    static constexpr uint8_t CHANNEL_COUNT = 16;
    static constexpr uint16_t SERVO_PWM_PERIOD_US = 20000;
    static constexpr uint16_t TICKS_PER_PERIOD = 4096;

    explicit Pca9685ServoDriver(
        const Pca9685ServoDriverConfig &config = {50});

    bool begin();
    void setServoUs(uint8_t channel, uint16_t target_us);
    bool currentServoUs(uint8_t channel, uint16_t &pwm_us) const;

  private:
    Adafruit_PWMServoDriver pca_;
    Pca9685ServoDriverConfig config_;
    uint16_t current_pwm_us_[CHANNEL_COUNT];
    bool current_pwm_known_[CHANNEL_COUNT];

    void writeServoUs(uint8_t channel, uint16_t pwm_us);
    static uint16_t pwmUsToTicks(uint16_t pwm_us);
};

#endif