diff --git a/PCA9685_servo.cpp b/PCA9685_servo.cpp new file mode 100644 index 0000000..9bcda44 --- /dev/null +++ b/PCA9685_servo.cpp @@ -0,0 +1,345 @@ +#include "PCA9685_servo.h" + +/// @brief PCA9685_servo constructor. +/// @param controller PCA9685_servo_controller pointer +/// @param channel channel number of the servo on the PCA9685 +/// @param PWMmin minimal PWM signal for the servo. This is not the minimal pulse width of the servo, but rather the pulse length count. Min and max values usually within (150-600) range. +/// @param PWMmax maximal PWM signal for the servo. Just like the PWMmin, to be determined experimentally, by slowly raising the value and checking the motion range. +PCA9685_servo::PCA9685_servo(PCA9685_servo_driver *controller, uint8_t channel, uint16_t PWMmin, uint16_t PWMmax) : + _controller{controller}, _channel{channel}, _PWMmin{PWMmin}, _PWMmax{PWMmax} +{} + +/// @brief Routine to call every loop iteration. +/// @note Enclose this in a for()/while() loop and call every time, passing +/// TEllapsed argument containing the time passed since loop() was last called. +/// @note E.g. +/// @note TNow = time_us_64(); // time now in microseconds +/// @note TEllapsed = TNow - TPrevious; // time, in microseconds, since the last loop +/// @note TPrevious = TNow; // store this ready for the next loop +/// @note servo.loop(TEllapsed); +/// @param TEllapsed time since last function call in microseconds +void PCA9685_servo::loop(uint64_t TEllapsed) +{ + switch (_mode) + { + case MODE_FAST: + { + if (_isMoving) + { + // has enough time ellasped to allow us to have moved into position? + _Tcntr += TEllapsed; + + if (_Tcntr >= _TFastDuration) + { + // we have finished moving + _isMoving = 0; + if (onStopMove != NULL) onStopMove(0); + } + } + + break; + } + + case MODE_TCONSTANT: + case MODE_SCONSTANT: + { + if (_isMoving) + { + // has enough time ellasped that we are due to move again? + _Tcntr += TEllapsed; + if (_Tcntr >= _TConstantPeriod) + { + _Tcntr = 0; + // we need to move by one degree + if (_currentAngle < _targetAngle) + { + // increase angle + _currentAngle++; + + uint16_t PWM = (uint16_t) _controller->map(_currentAngle, -90, 90, _PWMmin, _PWMmax); + + _controller->setPWM(_channel, PWM); + } + else + { + // decrease angle + _currentAngle--; + + uint16_t PWM = (uint16_t) _controller->map(_currentAngle, -90, 90, _PWMmin, _PWMmax); + + _controller->setPWM(_channel, PWM); + } + } + + // have we finished moving? + if (_currentAngle == _targetAngle) + { + _isMoving = 0; + if (onStopMove != NULL) onStopMove(0); + } + } + break; + } + } + + return; +} + +/// @brief Set target position for the servo. +/// @param Angle target position in degrees +void PCA9685_servo::setPosition(int8_t angle) +{ + _startAngle = _currentAngle; + if (angle < _minAngle) angle = _minAngle; + if (angle > _maxAngle) angle = _maxAngle; + + switch (_mode) + { + // move to the set point as quickly as possible + case MODE_FAST: + { + _currentAngle = angle; + + uint16_t PWM = (uint16_t) _controller->map(angle, -90, 90, _PWMmin, _PWMmax); + + _controller->setPWM(_channel, PWM); + _isMoving = 1; + _Tcntr = 0; + if (onStartMove != NULL) onStartMove(0); + break; + } + + // move to the set point gradually over a constant time + case MODE_TCONSTANT: + { + _targetAngle = angle; // where we wish to end up + uint64_t AngleDelta = abs(angle - _currentAngle); // how many degrees we need to move + + // how long between 1 degree movements? + _TConstantPeriod = _TConstantDuration / AngleDelta; + _isMoving = 1; + _Tcntr = 0; + if (onStartMove != NULL) onStartMove(0); + break; + } + + // move to the set point gradually at a set angular velocity + case MODE_SCONSTANT: + { + _targetAngle = angle; // where we wish to end up + _TConstantPeriod = _SConstantPeriod; + + _isMoving = 1; + _Tcntr = 0; + if (onStartMove != NULL) onStartMove(0); + break; + } + } + + return; +} + +/// @brief Set relative target position for the servo (in realtion to current angle). +/// @param angle relative angle in degrees +/// @note Does not work in MODE_FAST mode. +void PCA9685_servo::setRelativePosition(int8_t angle) +{ + int16_t newAngle = _currentAngle + angle; + if (newAngle < _minAngle) newAngle = _minAngle; + if (newAngle > _maxAngle) newAngle = _maxAngle; + setPosition(newAngle); +} + +/// @brief Get the current angle of the servo. +/// @param +/// @return current angle of the servo. +int8_t PCA9685_servo::getPosition(void) +{ + return _currentAngle; +} + +void PCA9685_servo::setRange(int8_t minAngle, int8_t maxAngle) +{ + _minAngle = minAngle; + _maxAngle = maxAngle; + _midAngle = ((_maxAngle - _minAngle) / 2) + _minAngle; + return; +} + +void PCA9685_servo::setRange(int8_t minAngle, int8_t midAngle, int8_t maxAngle) +{ + _minAngle = minAngle; + _maxAngle = maxAngle; + _midAngle = midAngle; + return; +} + +uint8_t PCA9685_servo::getInvertMode(void) +{ + return _invert; +} + +void PCA9685_servo::setInvertMode(uint8_t invert) +{ + _invert = invert; +} + +/// @brief Set servo position to its max or min angle value. +/// @param value 0 if movement to max, -1 if movement to min. +void PCA9685_servo::throwServo(uint8_t value) +{ + if (_invert) + { + // inverted operation + if (value == 0) + { + setPosition(_maxAngle); + } + else + { + setPosition(_minAngle); + } + } + else + { + // normal operation + if (value == 0) + { + setPosition(_minAngle); + } + else + { + setPosition(_maxAngle); + } + } + return; +} + +int8_t PCA9685_servo::getMinAngle(void) +{ + return _minAngle; +} + +int8_t PCA9685_servo::getMaxAngle(void) +{ + return _maxAngle; +} + +int8_t PCA9685_servo::getMidAngle(void) +{ + return _midAngle; +} + +uint8_t PCA9685_servo::getMode(void) +{ + return _mode; +} + +uint16_t PCA9685_servo::getAddress(void) +{ + return _address; +} + +/// @brief Check if servo is in motion. +/// @param +/// @return true if servo in motion, false otherwise. +uint8_t PCA9685_servo::isMoving(void) +{ + return _isMoving; +} + +uint64_t PCA9685_servo::getTConstantDuration(void) +{ + return _TConstantDuration; +} + +uint64_t PCA9685_servo::getSConstantPeriod(void) +{ + return _SConstantPeriod; +} + +void PCA9685_servo::setMinAngle(int8_t angle) +{ + if (_currentAngle < angle) + { + _currentAngle = angle; + setPosition(_currentAngle); + } + + _minAngle = angle; + return; +} + +void PCA9685_servo::setMidAngle(int8_t angle) +{ + if ((angle >= _minAngle) && (angle <= _maxAngle)) + { + _midAngle = angle; + } + + return; +} + +void PCA9685_servo::setMaxAngle(int8_t angle) +{ + if (_currentAngle > angle) + { + _currentAngle = angle; + setPosition(_currentAngle); + } + _maxAngle = angle; + return; +} + +/// @brief Set movement type. +/// @param mode mode type {MODE_FAST, MODE_TCONSTANT, MODE_SCONSTANT} +void PCA9685_servo::setMode(uint8_t mode) +{ + _mode = mode; + return; +} + +/// @brief Set duration of movement in MODE_TCONSTANT mode, takes effect only after setPosition() is called. +/// @param TConstantDuration duration of movement in microseconds +void PCA9685_servo::setTConstantDuration(uint64_t TConstantDuration) +{ + _TConstantDuration = TConstantDuration; + return; +} + +/// @brief Set interval time between each step (changing servo angle) in MODE_SCONSTANT mode, takes effect only after setPosition() is called. +/// @param SConstantPeriod interval time in microseconds +void PCA9685_servo::setSConstantPeriod(uint64_t SConstantPeriod) +{ + _SConstantPeriod = SConstantPeriod; + return; +} + +/// @brief Set the pin number on the PCA9685 the servo is connected to. +/// @param address pin number (0-15) +void PCA9685_servo::setAddress(uint16_t address) +{ + _address = address; + return; +} + +/// @brief Changes the speed of current movement, only use if you want to change the speed in MODE_TCONSTANT during the movement. +/// Otherwise use setTConstantDuration +/// @param TConstantDuration is the duration of the movement in microseconds +void PCA9685_servo::changeSpeedConstT(uint64_t TConstantDuration) +{ + // _targetAngle = Angle; // where we wish to end up + uint64_t AngleDelta = abs(_targetAngle - _currentAngle); // how many degrees we need to move + _TConstantDuration = TConstantDuration; + // how long between 1 degree movements? + _TConstantPeriod = _TConstantDuration / AngleDelta; +} + +/// @brief Changes the angular velocity used in MODE_SCONSTANT and MODE_TCONSTANT. +/// @param ang_vel is the angular velocity in degrees/s +void PCA9685_servo::setAngularVelocity(double angVel) +{ + double time_interval_s = 1 / angVel; + _SConstantPeriod = time_interval_s * 1e6; + _TConstantPeriod = _SConstantPeriod; +} \ No newline at end of file diff --git a/PCA9685_servo.h b/PCA9685_servo.h new file mode 100644 index 0000000..99fef84 --- /dev/null +++ b/PCA9685_servo.h @@ -0,0 +1,86 @@ +#ifndef PCA9685_servo_h +#define PCA9685_servo_h + +#include "PCA9685_servo_driver.h" +#include +#include + +#define MODE_FAST 0 // move to the set point as quickly as the servo can manage +#define MODE_TCONSTANT 1 // move to the set point over a constant time period +#define MODE_SCONSTANT 2 // move to the set point at a constant angular velocity + +class PCA9685_servo +{ +public: + PCA9685_servo(PCA9685_servo_driver *controller, uint8_t channel, uint16_t PWMmin = 104, uint16_t PWMmax = 508); + void setPosition(int8_t angle); // set the desired position, in degrees + void setRelativePosition(int8_t angle); // set the desired position relative to the current angle, in degrees + int8_t getPosition(void); + void setRange(int8_t minAngle, int8_t maxAngle); // set the allowable range of operation, in degrees + void setRange(int8_t minAngle, int8_t midAngle, int8_t maxAngle); // set the allowable range of operation, in degrees + int8_t getMinAngle(void); // return minimum set angle + int8_t getMaxAngle(void); // return mid set point angle + int8_t getMidAngle(void); // return maximum set angle + uint8_t getMode(void); + uint8_t isMoving(void); // returns 1 if the servo is moving, 0 otherwise + uint64_t getTConstantDuration(void); // returns the number of microseconds to use as the constant movement duration + uint64_t getSConstantPeriod(void); + uint16_t getAddress(void); + uint8_t getInvertMode(void); + + void setMinAngle(int8_t angle); + void setMidAngle(int8_t angle); + void setMaxAngle(int8_t angle); + void setMode(uint8_t Mode); + void setTConstantDuration(uint64_t TConstantDuration); // sets the time constant for the movement duration, in microseconds + void setSConstantPeriod(uint64_t SConstantPeriod); // sets the number of microseconds to move 1 degree + void setAddress(uint16_t address); + void setInvertMode(uint8_t invert); + + void throwServo(uint8_t value); // 0 = unthrow, 1 = throw + + void loop(uint64_t TEllapsed); // call every loop iteration, fEllapsedTime + + void changeSpeedConstT(uint64_t TConstantDuration); // change speed of the servo in MODE_TCONSTANT mode + void setAngularVelocity(double angVel); // set the speed of the servo in MODE_SCONSTANT mode deg/s + + // callback function pointers + + /// @brief Function pointer to a function that is to be called when the movement starts. + /// @note E.g. + /// @note void StartMoveHandler(uint16_t Address) { INTERNAL_LED(1); } + /// @note servo.onStartMove = StartMoveHandler; + void (*onStartMove)(uint16_t address) = NULL; + /// @brief Function pointer to a function that is to be called when the movement stops. + /// @note E.g. + /// @note void StopMoveHandler(uint16_t Address) { INTERNAL_LED(0); } + /// @note servo.onStopMove = StartMoveHandler; + void (*onStopMove)(uint16_t address) = NULL; + +private: + PCA9685_servo_driver *_controller; + uint8_t _channel; + uint16_t _PWMmin; + uint16_t _PWMmax; + + uint8_t _invert = 0; // 0 = normal, 1 = invert + uint16_t _address = 0; // the servos address, basically a user variable that can be set as desired + int8_t _minAngle = -90; + int8_t _maxAngle = 90; + int8_t _midAngle = 0; + int8_t _currentAngle = 0; // the angle the servo is currently set to be at + int8_t _targetAngle = 0; + int8_t _startAngle = 0; // the angle that the movement starts from + + uint64_t _Tcntr = 0; // an ellapsed time counter + uint64_t _TFastDuration = 500000; // time in microseconds to allow for a "fast" move + uint64_t _TConstantDuration = 2000000; // time in microseconds to allow for a constant time move + uint64_t _TConstantPeriod = 0; + uint64_t _SConstantPeriod = 100000; // number of microseconds between each angle for a constant speed + + uint8_t _isMoving = 0; // a flag to say if we are currently moving + + uint8_t _mode = 0; +}; + +#endif diff --git a/PCA9685_servo_driver.cpp b/PCA9685_servo_driver.cpp new file mode 100644 index 0000000..de98ae6 --- /dev/null +++ b/PCA9685_servo_driver.cpp @@ -0,0 +1,257 @@ +#include "PCA9685_servo_driver.h" + +/// @brief PCA9685_servo_driver constructor. +/// @param i2c i2c_inst_t pointer, for Pi Pico usually just 'i2c0' or 'i2c1', depending on the i2c interface used +/// @param SDA pin number of the SDA connection on the microcontroller +/// @param SCL pin number of the SCL connection on the microcontroller +/// @param i2c_address I2c address of the PCA9685, default is 0x40 +PCA9685_servo_driver::PCA9685_servo_driver(i2c_inst_t *i2c, uint SDA, uint SCL, uint8_t i2c_address) : + _i2c{i2c}, _SDA{SDA}, _SCL{SCL}, _i2c_address{i2c_address} +{} + +/// @brief Setups the I2C interface and hardware +/// @param baudrate baudrate for the I2C interface +void PCA9685_servo_driver::begin(uint baudrate) +{ + // initialise I2C + i2c_init(_i2c, baudrate); + gpio_set_function(_SDA, GPIO_FUNC_I2C); + gpio_set_function(_SCL, GPIO_FUNC_I2C); + gpio_pull_up(_SDA); + gpio_pull_up(_SCL); + + // configure the PCA9685 for driving servos () + writeRegister(PCA9685_MODE1, 0b10100000); // enable restart, use internal clock, enable auto-increment, normal mode (not sleeping) + writeRegister(PCA9685_MODE2, 0b00000100); // (these are defaults) output logic state not inverted, outputs change on STOP, output are totem-pole + + // set the default internal frequency + setOscillatorFrequency(FREQUENCY_OSCILLATOR); + + setPWMFreq(50); + + return; +} + +/// @brief Write to a value to a specified register. +/// @param reg where to write +/// @param value what to write +void PCA9685_servo_driver::writeRegister(uint8_t reg, uint8_t value) +{ + uint8_t D[2]; + + D[0] = reg; + D[1] = value; + + i2c_write_blocking(_i2c, _i2c_address, D, 2, false); // write register then value + + return; +} + +/// @brief Read one byte from a register. +/// @param reg where to read from +/// @return uint8_t return value from register +uint8_t PCA9685_servo_driver::readRegister(uint8_t reg) +{ + uint8_t D[1] = {reg}; + + i2c_write_blocking(_i2c, _i2c_address, D, 1, false); // write register + i2c_read_blocking(_i2c, _i2c_address, D, 1, false); // read value + + return D[0]; +} + +// set a position based on a supplied PWM value (assumes "on" is at zero, offset applied here) + +/// @brief Set a positon based on a supplied PWM value (assumes "on" is at zero, offset applied here) +/// @param channel servo channel on the PCA9685 +/// @param PWM PWM value reflecting the angle +void PCA9685_servo_driver::setPWM(uint8_t channel, int16_t off, int16_t on) +{ + uint8_t D[5]; + + uint16_t ChannelOffset = channel * 10; // adds 0-160 to the counter values + + uint16_t ChannelOn = 0 + ChannelOffset; + uint16_t ChannelOff = off + ChannelOffset; + + // divide the PWM into 2, 8bit words + D[0] = 0x06 + (4 * channel); + D[1] = (0x00FF & ChannelOn); + D[2] = (0xFF00 & ChannelOn) >> 8; + D[3] = (0x00FF & ChannelOff); + D[4] = (0xFF00 & ChannelOff) >> 8; + + i2c_write_blocking(_i2c, _i2c_address, D, 5, false); + + return; +} + +/// @brief Utility function +/// @param x +/// @param in_min +/// @param in_max +/// @param out_min +/// @param out_max +/// @return +long PCA9685_servo_driver::map(long x, long in_min, long in_max, long out_min, long out_max) +{ + return (x - in_min) * (out_max - out_min)/(in_max - in_min) + out_min; +} + +/// @brief Sends a reset command to the PCA9685 chip over I2C +void PCA9685_servo_driver::reset() +{ + writeRegister(PCA9685_MODE1, MODE1_RESTART); + sleep_ms(10); +} + +/// @brief Puts board into sleep mode +void PCA9685_servo_driver::sleep() +{ + uint8_t awake = readRegister(PCA9685_MODE1); + uint8_t sleep = awake | MODE1_SLEEP; // set sleep bit high + writeRegister(PCA9685_MODE1, sleep); + sleep_ms(5); // wait until cycle ends for sleep to be active +} + +/// @brief Wakes board from sleep +void PCA9685_servo_driver::wakeup() +{ + uint8_t sleep = readRegister(PCA9685_MODE1); + uint8_t wakeup = sleep & ~MODE1_SLEEP; // set sleep bit low + writeRegister(PCA9685_MODE1, wakeup); +} + +/// @brief Sets EXTCLK pin to use the external clock +/// @param prescale Configures the prescale value to be used by the external clock +void PCA9685_servo_driver::setExtClk(uint8_t prescale) +{ + uint8_t oldmode = readRegister(PCA9685_MODE1); + uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep + writeRegister(PCA9685_MODE1, newmode); // go to sleep, turn off internal oscillator + + // This sets both the SLEEP and EXTCLK bits of the MODE1 register to switch to + // use the external clock. + writeRegister(PCA9685_MODE1, (newmode |= MODE1_EXTCLK)); + + writeRegister(PCA9685_PRESCALE, prescale); // set the prescaler + + sleep_ms(5); + // clear the SLEEP bit to start + writeRegister(PCA9685_MODE1, (newmode & ~MODE1_SLEEP) | MODE1_RESTART | MODE1_AI); +} + +/// @brief Sets the PWM frequency for the entire chip, up to ~1.6 KHz +/// @param freq Floating point frequency that we will attempt to match +void PCA9685_servo_driver::setPWMFreq(float freq) +{ + // Range output modulation frequency is dependant on oscillator + if (freq < 1) + freq = 1; + if (freq > 3500) + freq = 3500; // Datasheet limit is 3052=50MHz/(4*4096) + + float prescaleval = ((_oscillator_freq / (freq * 4096.0)) + 0.5) - 1; + if (prescaleval < PCA9685_PRESCALE_MIN) + prescaleval = PCA9685_PRESCALE_MIN; + if (prescaleval > PCA9685_PRESCALE_MAX) + prescaleval = PCA9685_PRESCALE_MAX; + uint8_t prescale = (uint8_t)prescaleval; + + uint8_t oldmode = readRegister(PCA9685_MODE1); + uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep + writeRegister(PCA9685_MODE1, newmode); // go to sleep + writeRegister(PCA9685_PRESCALE, prescale); // set the prescaler + writeRegister(PCA9685_MODE1, oldmode); + sleep_us(500); + // This sets the MODE1 register to turn on auto increment. + writeRegister(PCA9685_MODE1, oldmode | MODE1_RESTART | MODE1_AI); +} + +/// @brief Sets the output mode of the PCA9685 to either +/// open drain or push pull / totempole. +/// Warning: LEDs with integrated zener diodes should +/// only be driven in open drain mode. +/// @param totempole Totempole if true, open drain if false. +void PCA9685_servo_driver::setOutputMode(bool totempole) +{ + uint8_t oldmode = readRegister(PCA9685_MODE2); + uint8_t newmode; + if (totempole) { + newmode = oldmode | MODE2_OUTDRV; + } else { + newmode = oldmode & ~MODE2_OUTDRV; + } + writeRegister(PCA9685_MODE2, newmode); +} + +/// @brief Reads set Prescale from PCA9685 +/// @return prescale value +uint8_t PCA9685_servo_driver::readPrescale(void) +{ + return readRegister(PCA9685_PRESCALE); +} + +/// @brief Gets the PWM output of one of the PCA9685 pins +/// @param num One of the PWM output pins, from 0 to 15 +/// @param off If true, returns PWM OFF value, otherwise PWM ON +/// @return requested PWM output value +uint16_t PCA9685_servo_driver::getPWM(uint8_t num, bool off) +{ + uint8_t buffer[2] = {uint8_t(PCA9685_LED0_ON_L + 4 * num), 0}; + if(off) + buffer[0] += 2; + i2c_write_blocking(_i2c, _i2c_address, buffer, 1, false); + i2c_read_blocking(_i2c, _i2c_address, buffer, 2, false); // read value + return uint16_t(buffer[0]) | (uint16_t(buffer[1]) << 8); +} + +/// @brief Helper to set pin PWM output. Sets pin without having to deal with +/// on/off tick placement and properly handles a zero value as completely off and +/// 4095 as completely on. Optional invert parameter supports inverting the +/// pulse for sinking to ground. +/// @param num One of the PWM output pins, from 0 to 15 +/// @param val The number of ticks out of 4096 to be active, should be a value +/// from 0 to 4095 inclusive. +/// @param invert If true, inverts the output, defaults to 'false' +void PCA9685_servo_driver::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + val = std::min(val, (uint16_t)4095); + if (invert) { + if (val == 0) { + // Special value for signal fully on. + setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + setPWM(num, 0, 4096); + } else { + setPWM(num, 0, 4095 - val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + setPWM(num, 0, 4096); + } else { + setPWM(num, 0, val); + } + } +} + +/// @brief Getter for the internally tracked oscillator used for freq calculations +/// @return The frequency the PCA9685 thinks it is running at (it cannot introspect) +uint32_t PCA9685_servo_driver::getOscillatorFrequency(void) +{ + return _oscillator_freq; +} + +/// @brief Setter for the internally tracked oscillator used for freq calculations +/// @param freq The frequency the PCA9685 should use for frequency calculations + +void PCA9685_servo_driver::setOscillatorFrequency(uint32_t freq) +{ + _oscillator_freq = freq; +} \ No newline at end of file diff --git a/PCA9685_servo_driver.h b/PCA9685_servo_driver.h new file mode 100644 index 0000000..b91eb2c --- /dev/null +++ b/PCA9685_servo_driver.h @@ -0,0 +1,84 @@ +#ifndef PCA9685_servo_driver_h +#define PCA9685_servo_driver_h + +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include +#include + +// REGISTER ADDRESSES +#define PCA9685_MODE1 0x00 /**< Mode Register 1 */ +#define PCA9685_MODE2 0x01 /**< Mode Register 2 */ +#define PCA9685_SUBADR1 0x02 /**< I2C-bus subaddress 1 */ +#define PCA9685_SUBADR2 0x03 /**< I2C-bus subaddress 2 */ +#define PCA9685_SUBADR3 0x04 /**< I2C-bus subaddress 3 */ +#define PCA9685_ALLCALLADR 0x05 /**< LED All Call I2C-bus address */ +#define PCA9685_LED0_ON_L 0x06 /**< LED0 on tick, low byte*/ +#define PCA9685_LED0_ON_H 0x07 /**< LED0 on tick, high byte*/ +#define PCA9685_LED0_OFF_L 0x08 /**< LED0 off tick, low byte */ +#define PCA9685_LED0_OFF_H 0x09 /**< LED0 off tick, high byte */ +// etc all 16: LED15_OFF_H 0x45 +#define PCA9685_ALLLED_ON_L 0xFA /**< load all the LEDn_ON registers, low */ +#define PCA9685_ALLLED_ON_H 0xFB /**< load all the LEDn_ON registers, high */ +#define PCA9685_ALLLED_OFF_L 0xFC /**< load all the LEDn_OFF registers, low */ +#define PCA9685_ALLLED_OFF_H 0xFD /**< load all the LEDn_OFF registers,high */ +#define PCA9685_PRESCALE 0xFE /**< Prescaler for PWM output frequency */ +#define PCA9685_TESTMODE 0xFF /**< defines the test mode to be entered */ + +// MODE1 bits +#define MODE1_ALLCAL 0x01 /**< respond to LED All Call I2C-bus address */ +#define MODE1_SUB3 0x02 /**< respond to I2C-bus subaddress 3 */ +#define MODE1_SUB2 0x04 /**< respond to I2C-bus subaddress 2 */ +#define MODE1_SUB1 0x08 /**< respond to I2C-bus subaddress 1 */ +#define MODE1_SLEEP 0x10 /**< Low power mode. Oscillator off */ +#define MODE1_AI 0x20 /**< Auto-Increment enabled */ +#define MODE1_EXTCLK 0x40 /**< Use EXTCLK pin clock */ +#define MODE1_RESTART 0x80 /**< Restart enabled */ +// MODE2 bits +#define MODE2_OUTNE_0 0x01 /**< Active LOW output enable input */ +#define MODE2_OUTNE_1 0x02 /**< Active LOW output enable input - high impedience */ +#define MODE2_OUTDRV 0x04 /**< totem pole structure vs open-drain */ +#define MODE2_OCH 0x08 /**< Outputs change on ACK vs STOP */ +#define MODE2_INVRT 0x10 /**< Output logic state inverted */ + +#define PCA9685_I2C_ADDRESS 0x40 /**< Default PCA9685 I2C Slave Address */ +#define FREQUENCY_OSCILLATOR 25000000 /**< Int. osc. frequency in datasheet */ + +#define PCA9685_PRESCALE_MIN 3 /**< minimum prescale value */ +#define PCA9685_PRESCALE_MAX 255 /**< maximum prescale value */ + + +class PCA9685_servo_driver +{ +public: + PCA9685_servo_driver(i2c_inst_t *i2c=i2c0, uint SDA=0, uint SCL=1, uint8_t i2c_address=PCA9685_I2C_ADDRESS); + // PCA9685 config functions + void begin(uint baudrate = 100000); + void reset(); + void sleep(); + void wakeup(); + void setExtClk(uint8_t prescale); + void setPWMFreq(float freq); + void setOutputMode(bool totempole); + uint16_t getPWM(uint8_t num, bool off = false); + void setPWM(uint8_t channel, int16_t off, int16_t on=0); + void setPin(uint8_t num, uint16_t val, bool invert=false); + uint8_t readPrescale(void); + + void setOscillatorFrequency(uint32_t freq); + uint32_t getOscillatorFrequency(void); + + long map(long x, long in_min, long in_max, long out_min, long out_max); + +private: + i2c_inst_t *_i2c; + uint _SDA; + uint _SCL; + uint8_t _i2c_address; + uint32_t _oscillator_freq; + + void writeRegister(uint8_t reg, uint8_t value); + uint8_t readRegister(uint8_t reg); +}; + +#endif diff --git a/README.md b/README.md new file mode 100644 index 0000000..8e148ff --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# Adafruit-Servo-Driver-Library-Pi-Pico diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..87eec9f --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,40 @@ +# Set minimum required version of CMake +cmake_minimum_required(VERSION 3.14) + +# Include build functions from Pico SDK +include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake) + +set(PICO_BOARD pico_w) + +# Set name of project (as PROJECT_NAME) and C/CXX standards +project(servo_driver C CXX ASM) +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Creates a pico-sdk subdirectory in our project for the libraries +pico_sdk_init() + +# Tell CMake where to find the executable source file +add_executable(${PROJECT_NAME} + simple_servo.cpp + PCA9685_servo_driver.cpp + PCA9685_servo.cpp +) + +# Create map/bin/hex/uf2 files +pico_add_extra_outputs(${PROJECT_NAME}) + +target_include_directories(${PROJECT_NAME} + PRIVATE pca9685 +) + +# Link to pico_stdlib (gpio, time, etc. functions) +target_link_libraries(${PROJECT_NAME} + pico_stdlib + pico_cyw43_arch_none + hardware_i2c +) + +# Enable usb output, disable uart output +pico_enable_stdio_usb(${PROJECT_NAME} 1) +pico_enable_stdio_uart(${PROJECT_NAME} 0) diff --git a/examples/simple_servo.cpp b/examples/simple_servo.cpp new file mode 100644 index 0000000..6ba9afc --- /dev/null +++ b/examples/simple_servo.cpp @@ -0,0 +1,83 @@ +#include "PCA9685_servo_driver.h" +#include "PCA9685_servo.h" +#include +#include +#include +#include +#include "pico/cyw43_arch.h" +#include + + +void StartMoveHandler(uint16_t Address); // Servo callback +void StopMoveHandler(uint16_t Address); // Servo callback +void init_servo(PCA9685_servo& servo, uint8_t mode, int16_t minRange, int16_t maxRange, int16_t position, uint8_t address, uint64_t TConstDur); + +// create the controller and servos +PCA9685_servo_driver myController(i2c0, 0, 1, 0x40); +std::vector myServo = {PCA9685_servo(&myController, 0, 100, 540)}; // define a vector of servos, note this could extend to use multiple controller on the i2c bus +// necessary variables to drive the servo with constant speed or time +uint64_t TNow = 0; +uint64_t TPrevious = 0; +uint64_t TEllapsed = 0; + + +int main(void) +{ + // initialize Pi Pico peripherals + stdio_init_all(); + + // create connection to the PCA9685 and initialize it + myController.begin(100000); + // initialize the servos created + int i{0}; + for(auto& servo : myServo){ + init_servo(servo, MODE_SCONSTANT, -90, 90, servo.getMinAngle(), i++, 1000000); + } + + myServo[0].setAngularVelocity(40); + + bool changed = false; + uint64_t start_t = time_us_64(); + sleep_ms(1000); + while(1) + { + TNow = time_us_64(); // time now in microseconds + TEllapsed = TNow - TPrevious; // time, in microseconds, since the last loop + TPrevious = TNow; // store this ready for the next loop + + // loop through the servos calling their loop function so they can do their thing + for(auto& servo : myServo){ + servo.loop(TEllapsed); + } + // after 6 seconds change position to relative and move + if(TNow - start_t > 6 *1e6 && !changed) + { + myServo.at(0).setAngularVelocity(90); + myServo.at(0).setRelativePosition(-90); + changed = true; + } + } + + return 0; +} + +void StartMoveHandler(uint16_t Address) +{ + // called when a servo starts to move + return; +} + +void StopMoveHandler(uint16_t Address) +{ + // called when a servo stops moving + return; +} + +void init_servo(PCA9685_servo& servo, uint8_t mode, int16_t minRange, int16_t maxRange, int16_t position, uint8_t address, uint64_t TConstDur) +{ + servo.setRange(minRange, maxRange); + servo.setMode(mode); + servo.setPosition(position); // move to mid point + servo.setAddress(address); + servo.setTConstantDuration(TConstDur); +} diff --git a/license.txt b/license.txt new file mode 100644 index 0000000..f6a0f22 --- /dev/null +++ b/license.txt @@ -0,0 +1,26 @@ +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.