mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2024-11-23 08:06:13 +01:00
110 lines
3.9 KiB
C++
110 lines
3.9 KiB
C++
/*
|
|
* (c) 2020 Chris Harlow. All rights reserved.
|
|
*
|
|
* This file is part of CommandStation-EX
|
|
*
|
|
* This is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* It is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
|
*/
|
|
/*!
|
|
* @file PWMServoDriver.cpp
|
|
*
|
|
* @mainpage Adafruit 16-channel PWM & Servo driver, based on Adafruit_PWMServoDriver
|
|
*
|
|
* @section intro_sec Introduction
|
|
*
|
|
* This is a library for the 16-channel PWM & Servo driver.
|
|
*
|
|
* Designed specifically to work with the Adafruit PWM & Servo driver.
|
|
* This class contains a very small subset of the Adafruit version which
|
|
* is relevant to driving simple servos at 50Hz through a number of chained
|
|
* servo driver boards (ie servos 0-15 on board 0x40, 16-31 on board 0x41 etc.)
|
|
*
|
|
* @section author Author
|
|
* Chris Harlow (TPL)
|
|
*
|
|
*/
|
|
#include <Arduino.h>
|
|
#include "PWMServoDriver.h"
|
|
#include "DIAG.h"
|
|
#include "I2CManager.h"
|
|
|
|
|
|
// REGISTER ADDRESSES
|
|
const byte PCA9685_MODE1=0x00; // Mode Register
|
|
const byte PCA9685_FIRST_SERVO=0x06; /** low byte first servo register ON*/
|
|
const byte PCA9685_PRESCALE=0xFE; /** Prescale register for PWM output frequency */
|
|
// MODE1 bits
|
|
const byte MODE1_SLEEP=0x10; /**< Low power mode. Oscillator off */
|
|
const byte MODE1_AI=0x20; /**< Auto-Increment enabled */
|
|
const byte MODE1_RESTART=0x80; /**< Restart enabled */
|
|
|
|
const byte PCA9685_I2C_ADDRESS=0x40; /** First PCA9685 I2C Slave Address */
|
|
const float FREQUENCY_OSCILLATOR=25000000.0; /** Accurate enough for our purposes */
|
|
const uint8_t PRESCALE_50HZ = (uint8_t)(((FREQUENCY_OSCILLATOR / (50.0 * 4096.0)) + 0.5) - 1);
|
|
const uint32_t MAX_I2C_SPEED = 1000000L; // PCA9685 rated up to 1MHz I2C clock speed
|
|
|
|
/*!
|
|
* @brief Sets the PWM frequency for a chip to 50Hz for servos
|
|
*/
|
|
|
|
byte PWMServoDriver::setupFlags=0; // boards that have been initialised
|
|
byte PWMServoDriver::failFlags=0; // boards that have faild initialisation
|
|
|
|
bool PWMServoDriver::setup(int board) {
|
|
if (board>3 || (failFlags & (1<<board))) return false;
|
|
if (setupFlags & (1<<board)) return true;
|
|
|
|
I2CManager.begin();
|
|
I2CManager.setClock(MAX_I2C_SPEED);
|
|
|
|
uint8_t i2caddr=PCA9685_I2C_ADDRESS + board;
|
|
|
|
// Test if device is available
|
|
byte error = I2CManager.checkAddress(i2caddr);
|
|
if (error) {
|
|
DIAG(F("I2C Servo device 0x%x Not Found %d"),i2caddr, error);
|
|
failFlags|=1<<board;
|
|
return false;
|
|
}
|
|
|
|
//DIAG(F("PWMServoDriver::setup %x prescale=%d"),i2caddr,PRESCALE_50HZ);
|
|
writeRegister(i2caddr,PCA9685_MODE1, MODE1_SLEEP | MODE1_AI);
|
|
writeRegister(i2caddr,PCA9685_PRESCALE, PRESCALE_50HZ);
|
|
writeRegister(i2caddr,PCA9685_MODE1,MODE1_AI);
|
|
writeRegister(i2caddr,PCA9685_MODE1, MODE1_RESTART | MODE1_AI);
|
|
setupFlags|=1<<board;
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* @brief Sets the PWM output to a servo
|
|
*/
|
|
void PWMServoDriver::setServo(byte servoNum, uint16_t value) {
|
|
int board=servoNum/16;
|
|
int pin=servoNum%16;
|
|
|
|
if (setup(board)) {
|
|
DIAG(F("SetServo %d %d"),servoNum,value);
|
|
uint8_t buffer[] = {(uint8_t)(PCA9685_FIRST_SERVO + 4 * pin), // 4 registers per pin
|
|
0, 0, (uint8_t)(value & 0xff), (uint8_t)(value >> 8)};
|
|
if (value == 4095) buffer[2] = 0x10; // Full on
|
|
byte error=I2CManager.write(PCA9685_I2C_ADDRESS + board, buffer, sizeof(buffer));
|
|
if (error!=0) DIAG(F("SetServo error %d"),error);
|
|
}
|
|
}
|
|
|
|
void PWMServoDriver::writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d) {
|
|
I2CManager.write(i2caddr, 2, hardwareRegister, d);
|
|
}
|