#include "pca9685.h" extern I2C_HandleTypeDef hi2c1; // hendl for i2c #define PCA9685_ADRESS 0x40 // i2c slave adress pca9685 uint8_t PRESCALE_REG[2] = {0xFE,0x79}; // prescale = 121 (50 Hz) uint8_t MODE1_REG[2] = {0x00,0x20}; // Auto increment mode, sleep out void initPCA9685(void) { HAL_I2C_Master_Transmit(&hi2c1, (PCA9685_ADRESS << 1), PRESCALE_REG, 2, 10); HAL_Delay(1); HAL_I2C_Master_Transmit(&hi2c1, (PCA9685_ADRESS << 1), MODE1_REG, 2, 10); HAL_Delay(1); } void SetServo(uint8_t channel, uint8_t angel) { uint8_t bufer[5]; uint16_t A; if (angel > 180) { angel = 180;} //check for scale 0..180 A = 150 + ((angel*15)>>3); //get puls time for servo from min to max // min 150/4095 // max 488/4095 bufer[0] = 6 + channel*4; //channel register adress bufer[1] = 0; //start time for "on" Lowbyte (0..4095) bufer[2] = 0; //start time for "on" HIbyte bufer[3] = A&0xFF; //start time for "off" Lowbyte (0..4095) bufer[4] = A>>8;; //start time for "off" HIbyte HAL_I2C_Master_Transmit(&hi2c1, (PCA9685_ADRESS << 1), bufer, 5, 10); } void SetServoOFF(uint8_t channel) { uint8_t bufer[5]; bufer[0] = 6 + channel*4; //channel register adress bufer[1] = 0; //start time for "on" Lowbyte (0..4095) bufer[2] = 0; //start time for "on" HIbyte bufer[3] = 0&0xFF; //start time for "off" Lowbyte (0..4095) bufer[4] = 0>>8;; //start time for "off" HIbyte HAL_I2C_Master_Transmit(&hi2c1, (PCA9685_ADRESS << 1), bufer, 5, 10); }