Jag har en 12-bit PWM servostyrare vid namn PCA9685 + en liten blå 5V servo. Kina så klart. Men det sägs att att dom ska fungera.
Jag har STM32 F401RE som jag vill exprimentera på. Men jag fick inte PCA9685 fungera med STM32 bibliotek. Så därför tog jag friheten att skriva om Adafruit's PCA9685 C++ bibliotek till STM32 C kod.
Men det fungerar ändå inte. Nu misstänker jag att min servo's PWM kabel inte får någon signal. Jag menar, jag har skrivit av detta
https://github.com/adafruit/Adafruit-PW ... Driver.cpp
https://github.com/adafruit/Adafruit-PW ... voDriver.h
Till rent C för just STM32 men jag får det inte att fungera.
Jag har tyvärr inget oscilloskop i närheten för tillfället. Min fråga till er om man kan efterfråga om någon kodgranskning eller test och verifiering av er? Fungerar denna kod för er, så då vet jag att antingen är min servo sönder eller så är min PCA9685 sönder.
Exempelkod i STM32:
Kod: Markera allt
#include "main.h"
#include "PCA9685/PCA9685.h"
#define SERVOMIN 125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 575 // this is the 'maximum' pulse length count (out of 4096)
PCA9685_I2C i2c;
long 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;
}
int angleToPulse(int ang){
int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX);// map angle of 0 to 180 to Servo min and Servo max
return pulse;
}
void main(){
// Find I2C address
uint8_t address;
for(uint8_t i = 0; i < 128; i++){
if(HAL_I2C_IsDeviceReady(&hi2c1, i, 5, 10) == HAL_OK){
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, 1);
address = i;
break;
}
}
PCA9685_begin(&i2c, &hi2c1, address, 0);
PCA9685_setPWMFreq(&i2c, 50); // Analog servos run at ~50 Hz updates
while (1)
{
for( int angle = 0; angle<181; angle++){
HAL_Delay(1);
PCA9685_setPWM(&i2c, 0, 0, angleToPulse(angle) );
}
}
}
Kod: Markera allt
#include "PCA9685.h"
static uint8_t read8(PCA9685_I2C *i2c, uint8_t addr) {
uint8_t pData[] = { addr };
if(HAL_I2C_Master_Transmit(i2c->hi2c, i2c->address, pData, 1, 10) != HAL_OK)
return 0;
if(HAL_I2C_Master_Receive(i2c->hi2c, i2c->address, pData, 1, 10) != HAL_OK)
return 0;
return pData[0];
}
static void write8(PCA9685_I2C *i2c, uint8_t addr, uint8_t d) {
uint8_t pData[2] = { addr, d };
if(HAL_I2C_Master_Transmit(i2c->hi2c, i2c->address, pData, 2, 10) != HAL_OK)
return; // For debugging tool in STM
}
void PCA9685_setOscillatorFrequency(PCA9685_I2C *i2c, uint32_t freq) {
i2c->oscillator_freq = freq;
}
uint32_t PCA9685_getOscillatorFrequency(PCA9685_I2C *i2c) {
return i2c->oscillator_freq;
}
void PCA9685_writeMicroseconds(PCA9685_I2C *i2c, uint8_t num, uint16_t Microseconds) {
float pulse = Microseconds;
float pulselength = 1000000; // 1,000,000 us per second
// Read prescale
uint16_t prescale = PCA9685_readPrescale(i2c);
// Calculate the pulse for PWM based on Equation 1 from the datasheet section
// 7.3.5
prescale += 1;
pulselength *= prescale;
pulselength /= i2c->oscillator_freq;
pulse /= pulselength;
PCA9685_setPWM(i2c, num, 0, pulse);
}
void PCA9685_setPin(PCA9685_I2C *i2c, uint8_t num, uint16_t val, bool invert) {
// Clamp value between 0 and 4095 inclusive.
if(val > 4095)
val = 4095;
if (invert) {
if (val == 0) {
// Special value for signal fully on.
PCA9685_setPWM(i2c, num, 4096, 0);
} else if (val == 4095) {
// Special value for signal fully off.
PCA9685_setPWM(i2c, num, 0, 4096);
} else {
PCA9685_setPWM(i2c, num, 0, 4095 - val);
}
} else {
if (val == 4095) {
// Special value for signal fully on.
PCA9685_setPWM(i2c, num, 4096, 0);
} else if (val == 0) {
// Special value for signal fully off.
PCA9685_setPWM(i2c, num, 0, 4096);
} else {
PCA9685_setPWM(i2c, num, 0, val);
}
}
}
void PCA9685_setPWM(PCA9685_I2C *i2c, uint8_t num, uint16_t on, uint16_t off) {
uint8_t pData[5] = { PCA9685_LED0_ON_L + 4 * num, on, on >> 8, off, off >> 8 };
if(HAL_I2C_Master_Transmit(i2c->hi2c, i2c->address, pData, 5, 10) != HAL_OK)
return; // For debugging tool in STM
}
uint8_t PCA9685_getPWM(PCA9685_I2C *i2c, uint8_t num) {
uint8_t Size = PCA9685_LED0_ON_L + 4 * num;
uint8_t pData[Size];
if(HAL_I2C_Master_Receive(i2c->hi2c, i2c->address, pData, Size, 10) != HAL_OK)
return 0;
return pData[0];
}
uint8_t PCA9685_readPrescale(PCA9685_I2C *i2c) {
return read8(i2c, PCA9685_PRESCALE);
}
void PCA9685_setOutputMode(PCA9685_I2C *i2c, bool totempole) {
uint8_t oldmode = read8(i2c, PCA9685_MODE2);
uint8_t newmode;
if (totempole) {
newmode = oldmode | MODE2_OUTDRV;
} else {
newmode = oldmode & ~MODE2_OUTDRV;
}
write8(i2c, PCA9685_MODE2, newmode);
}
void PCA9685_setPWMFreq(PCA9685_I2C *i2c, 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 = ((i2c->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 = read8(i2c, PCA9685_MODE1);
uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep
write8(i2c, PCA9685_MODE1, newmode); // go to sleep
write8(i2c, PCA9685_PRESCALE, prescale); // set the prescaler
write8(i2c, PCA9685_MODE1, oldmode);
HAL_Delay(5);
// This sets the MODE1 register to turn on auto increment.
write8(i2c, PCA9685_MODE1, oldmode | MODE1_RESTART | MODE1_AI);
}
void PCA9685_setExtClk(PCA9685_I2C *i2c, uint8_t prescale) {
uint8_t oldmode = read8(i2c, PCA9685_MODE1);
uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep
write8(i2c, 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.
write8(i2c, PCA9685_MODE1, (newmode |= MODE1_EXTCLK));
write8(i2c, PCA9685_PRESCALE, prescale); // set the prescaler
HAL_Delay(5);
// clear the SLEEP bit to start
write8(i2c, PCA9685_MODE1, (newmode & ~MODE1_SLEEP) | MODE1_RESTART | MODE1_AI);
}
void PCA9685_wakeup(PCA9685_I2C *i2c) {
uint8_t sleep = read8(i2c, PCA9685_MODE1);
uint8_t wakeup = sleep & ~MODE1_SLEEP; // set sleep bit low
write8(i2c, PCA9685_MODE1, wakeup);
}
void PCA9685_sleep(PCA9685_I2C *i2c) {
uint8_t awake = read8(i2c, PCA9685_MODE1);
uint8_t sleep = awake | MODE1_SLEEP; // set sleep bit high
write8(i2c, PCA9685_MODE1, sleep);
HAL_Delay(5); // wait until cycle ends for sleep to be active
}
void PCA9685_reset(PCA9685_I2C *i2c) {
write8(i2c, PCA9685_MODE1, MODE1_RESTART);
HAL_Delay(10);
}
void PCA9685_begin(PCA9685_I2C *i2c, I2C_HandleTypeDef *hi2c, uint8_t address, uint8_t prescale) {
i2c->hi2c = hi2c;
i2c->address = address;
PCA9685_reset(i2c);
if (prescale) {
PCA9685_setExtClk(i2c, prescale);
} else {
// set a default frequency
PCA9685_setPWMFreq(i2c, 1000);
}
// set the default internal frequency
PCA9685_setOscillatorFrequency(i2c, FREQUENCY_OSCILLATOR);
}
Kod: Markera allt
#ifndef SRC_PCA9685_PCA9685_H_
#define SRC_PCA9685_PCA9685_H_
#include "main.h"
#include "stdbool.h"
// 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 */
typedef struct{
I2C_HandleTypeDef *hi2c;
uint8_t address;
uint32_t oscillator_freq;
}PCA9685_I2C;
void PCA9685_begin(PCA9685_I2C* i2c, I2C_HandleTypeDef *hi2c, uint8_t address, uint8_t prescale);
void PCA9685_reset(PCA9685_I2C* i2c);
void PCA9685_sleep(PCA9685_I2C* i2c);
void PCA9685_wakeup(PCA9685_I2C* i2c);
void PCA9685_setExtClk(PCA9685_I2C* i2c, uint8_t prescale);
void PCA9685_setPWMFreq(PCA9685_I2C* i2c, float freq);
void PCA9685_setOutputMode(PCA9685_I2C* i2c, bool totempole);
uint8_t PCA9685_readPrescale(PCA9685_I2C* i2c);
uint8_t PCA9685_getPWM(PCA9685_I2C* i2c, uint8_t num);
void PCA9685_setPWM(PCA9685_I2C* i2c, uint8_t num, uint16_t on, uint16_t off);
void PCA9685_setPin(PCA9685_I2C* i2c, uint8_t num, uint16_t val, bool invert);
void PCA9685_writeMicroseconds(PCA9685_I2C* i2c, uint8_t num, uint16_t Microseconds);
uint32_t PCA9685_getOscillatorFrequency(PCA9685_I2C* i2c);
void PCA9685_setOscillatorFrequency(PCA9685_I2C* i2c, uint32_t freq);
#endif /* SRC_PCA9685_PCA9685_H_ */
