diff --git a/main.go b/main.go index 4fe4b56..0c0b5af 100644 --- a/main.go +++ b/main.go @@ -12,7 +12,6 @@ import ( "encoding/binary" "gen/ledd" "github.com/golang/protobuf/proto" - "github.com/sergiorb/pca9685-golang/device" "fmt" ) @@ -46,8 +45,8 @@ type LedDaemon struct { var log = logging.MustGetLogger("LedD") var ledDaemon = &LedDaemon{} -var pca9685 = &device.PCA9685{} -var pwmMap = map[int32]*device.Pwm{} +var pca9685 = &PCA9685{} +var pwmMap = map[int32]*Pwm{} func check(e error) { if e != nil { @@ -86,10 +85,10 @@ func (daemon *LedDaemon) receive() { case *ledd.BackendWrapperMessage_MSetChannel: for c, v := range msg.MSetChannel.NewChannelValues { if pwm, ok := pwmMap[c]; ok { - pwm.SetPercentage(float32(v) / RESOLUTION * 100) + pwm.setPercentage(float32(v) / RESOLUTION * 100) } else { pwmMap[c] = pca9685.NewPwm(int(c)) - pwmMap[c].SetPercentage(float32(v) / RESOLUTION * 100) + pwmMap[c].setPercentage(float32(v) / RESOLUTION * 100) } } } @@ -134,10 +133,10 @@ func main() { check(err) defer i2cDevice.Close() - pca9685 = device.NewPCA9685(i2cDevice, "PWM Controller", config.Pca9685.MinPulse, config.Pca9685.MaxPulse, logging.MustGetLogger("PCA9685")) + pca9685 = createPCA9685(i2cDevice, "PWM Controller", config.Pca9685.MinPulse, config.Pca9685.MaxPulse, logging.MustGetLogger("PCA9685")) pca9685.Init() - pwmMap = make(map[int32]*device.Pwm, 1) + pwmMap = make(map[int32]*Pwm, 1) conn, err := net.Dial("tcp4", fmt.Sprintf("%s:%d", config.Ledd.Host, config.Ledd.Port)) check(err) diff --git a/pca9685.go b/pca9685.go new file mode 100644 index 0000000..03d8f21 --- /dev/null +++ b/pca9685.go @@ -0,0 +1,202 @@ +package main + +import ( + "errors" + "fmt" + "github.com/op/go-logging" + "golang.org/x/exp/io/i2c" + "math" + "time" +) + +const ( + MODE1 byte = 0x00 + MODE2 byte = 0x01 + PRESCALE byte = 0xFE + LED0_ON_L byte = 0x06 + LED0_ON_H byte = 0x07 + LED0_OFF_L byte = 0x08 + LED0_OFF_H byte = 0x09 + ALL_LED_ON_L byte = 0xFA + ALL_LED_ON_H byte = 0xFB + ALL_LED_OFF_L byte = 0xFC + ALL_LED_OFF_H byte = 0xFD + OUTDRV byte = 0x04 + SLEEP byte = 0x10 + BYTE byte = 0xFF +) + +type PCA9685 struct { + i2cBus *i2c.Device + name string + initiated bool + minPulse int + maxPulse int + log *logging.Logger + frequency float32 +} + +type Pwm struct { + pca *PCA9685 + pin int + lastValue float32 +} + +func createPCA9685(i2cDevice *i2c.Device, name string, minPulse int, maxPulse int, log *logging.Logger) *PCA9685 { + log.Info(fmt.Sprintf("Creating a new PCA9685 device. Alias: %v", name)) + + return &PCA9685{ + i2cBus: i2cDevice, + name: name, + initiated: false, + minPulse: minPulse, + maxPulse: maxPulse, + log: log, + frequency: 1000.0, + } +} + +func (p *PCA9685) NewPwm(pin int) *Pwm { + p.log.Info(fmt.Sprintf("Creating a new Pwm controler at pin %v from %v", pin, p.name)) + + return &Pwm{ + pca: p, + pin: pin, + lastValue: 0.0, + } +} + +func (p *PCA9685) Init() { + if p.initiated { + + p.log.Warning(fmt.Sprintf("Device \"%v\" already initiated!", p.name)) + + } else { + + p.log.Info(fmt.Sprintf("Initiating \"%v\" PCA9685 device", p.name)) + + p.setAllPwm(0, 0) + p.i2cBus.WriteReg(MODE2, []byte{OUTDRV}) + + time.Sleep(5 * time.Millisecond) + + var mode1 byte + err := p.i2cBus.ReadReg(MODE1, []byte{mode1}) + + if err != nil { + + p.log.Error("Can't read!") + return + } + + mode1 &= BYTE + mode1 = mode1 & ^SLEEP + + p.i2cBus.WriteReg(MODE1, []byte{mode1 & 0xFF}) + + time.Sleep(5 * time.Millisecond) + + p.setPwmFreq(p.frequency) + + p.initiated = true + } +} + +func (p *PCA9685) SwichOn(pwm []int) error { + if !p.initiated { + + return errors.New(fmt.Sprintf("Device \"%v\"is not initiated!", p.name)) + } + + for i := 0; i < len(pwm); i++ { + + p.log.Info(fmt.Sprintf("Swiching on pwm #%v", pwm[i])) + + p.setPwm(pwm[i], p.minPulse, p.maxPulse) + } + + return nil +} + +func (p *PCA9685) SwichOff(pwm []int) error { + if !p.initiated { + + return errors.New(fmt.Sprintf("Device \"%v\"is not initiated!", p.name)) + } + + for i := 0; i < len(pwm); i++ { + + p.log.Info(fmt.Sprintf("Swiching off pwm #%v", pwm[i])) + + p.setPwm(pwm[i], 0, p.minPulse) + } + + return nil +} + +func (p *PCA9685) setPwmFreq(freqHz float32) { + var prescaleValue float32 = 25000000.0 // 25MHz + prescaleValue /= 4096.0 + prescaleValue /= freqHz + prescaleValue -= 1.0 + + p.log.Debug(fmt.Sprintf("Setting PWM frequency to %v Hz", freqHz)) + p.log.Debug(fmt.Sprintf("Esimated pre-scale: %v", prescaleValue)) + + prescale := int(math.Floor(float64(prescaleValue + 0.5))) + + p.log.Debug(fmt.Sprintf("Final pre.scale: %v", prescale)) + + var oldMode byte + err := p.i2cBus.ReadReg(MODE1, []byte{oldMode}) + + if err != nil { + + p.log.Error("Can't read!") + } + + oldMode &= BYTE + + newMode := (oldMode & 0x7F) | 0x10 + + p.i2cBus.WriteReg(MODE1, []byte{newMode & BYTE}) + p.i2cBus.WriteReg(PRESCALE, []byte{byte(prescale) & BYTE}) + p.i2cBus.WriteReg(MODE1, []byte{oldMode & BYTE}) + + time.Sleep(5 * time.Millisecond) + + p.i2cBus.WriteReg(MODE1, []byte{oldMode&BYTE | 0x80}) +} + +func (p *PCA9685) setAllPwm(on int, off int) { + onB := byte(on) & BYTE + offB := byte(off) & BYTE + + p.i2cBus.WriteReg(ALL_LED_ON_L, []byte{onB & BYTE}) + p.i2cBus.WriteReg(ALL_LED_ON_H, []byte{onB & BYTE}) + p.i2cBus.WriteReg(ALL_LED_OFF_L, []byte{offB & BYTE}) + p.i2cBus.WriteReg(ALL_LED_OFF_H, []byte{offB & BYTE}) +} + +func (p *PCA9685) setPwm(pwm int, on int, off int) { + onB := byte(on) & BYTE + offB := byte(off) & BYTE + + p.i2cBus.WriteReg(LED0_ON_L+byte(4)*byte(pwm), []byte{onB & BYTE}) + p.i2cBus.WriteReg(LED0_ON_H+byte(4)*byte(pwm), []byte{onB >> 8}) + p.i2cBus.WriteReg(LED0_OFF_L+byte(4)*byte(pwm), []byte{offB & BYTE}) + p.i2cBus.WriteReg(LED0_OFF_H+byte(4)*byte(pwm), []byte{offB >> 8}) +} + +func (pwm *Pwm) setPercentage(percentage float32) error { + if percentage < 0.0 || percentage > 100.0 || pwm.pca.maxPulse > 4095 { + + return errors.New(fmt.Sprintf("Percentage must be between 0.0 and 100.0. Got %v.", percentage)) + } + + pwm.pca.log.Info(fmt.Sprintf("Setting pwm #%v to %v%% at \"%v\" device.", pwm.pin, percentage, pwm.pca.name)) + + pwm.pca.setPwm(pwm.pin, 0, int(percentage*float32(pwm.pca.maxPulse))) + + return nil +}