refactor: rewrite and fix pca9685 controllers

* implement throttle controller directly with pca9685 pin
 * close actuator controllers on exit
 * add trace logs
 * handle TERM signals to properly exit
 * add info logs at startup

cmd/rc-pca9685/rc-pca9685.go

@@ -3,11 +3,12 @@ package main
 import (
 	"flag"
 	"github.com/cyrilix/robocar-base/cli"
-	actuator2 "github.com/cyrilix/robocar-pca9685/pkg/actuator"
+	"github.com/cyrilix/robocar-pca9685/pkg/actuator"
 	"github.com/cyrilix/robocar-pca9685/pkg/part"
 	"go.uber.org/zap"
 	"log"
 	"os"
+	"periph.io/x/conn/v3/physic"
 )
 
 const (
@@ -21,6 +22,8 @@ const (
 
 	SteeringLeftPWM  = 1004
 	SteeringRightPWM = 1986
+
+	DefaultFrequency = 60 * physic.Hertz
 )
 
 var (
@@ -106,10 +109,47 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	t := actuator2.NewThrottle(throttleChannel, throttleStoppedPWM, throttleMinPWM, throttleMaxPWM)
+	freq := DefaultFrequency
-	s := actuator2.NewSteering(steeringChannel, steeringLeftPWM, steeringRightPWM, steeringCenterPWM)
+
+	zap.S().Infof("throttle channel  : %v", throttleChannel)
+	zap.S().Infof("throttle frequency: %v", freq)
+	zap.S().Infof("throttle zero     : %v", throttleStoppedPWM)
+	zap.S().Infof("throttle min      : %v", throttleMinPWM)
+	zap.S().Infof("throttle max      : %v", throttleMaxPWM)
+
+	zap.S().Infof("steering channel  : %v", steeringChannel)
+	zap.S().Infof("steering frequency: %v", freq)
+	zap.S().Infof("steering center   : %v", steeringCenterPWM)
+	zap.S().Infof("steering left     : %v", steeringLeftPWM)
+	zap.S().Infof("steering right    : %v", steeringRightPWM)
+
+	dev := actuator.NewDevice(freq)
+
+	t, err := actuator.NewPca9685Controller(
+		dev,
+		throttleChannel,
+		actuator.PWM(throttleMinPWM), actuator.PWM(throttleMaxPWM), actuator.PWM(throttleStoppedPWM),
+		freq,
+	)
+	if err != nil {
+		zap.S().Panicf("unable to init throttle controller: %v", err)
+	}
+
+	s, err := actuator.NewPca9685Controller(
+		dev,
+		steeringChannel,
+		actuator.PWM(steeringLeftPWM), actuator.PWM(steeringRightPWM), actuator.PWM(steeringCenterPWM),
+		freq,
+	)
+	if err != nil {
+		zap.S().Panicf("unable to init steering controller: %v", err)
+	}
 
 	p := part.NewPca9685Part(client, t, s, updatePWMFrequency, topicThrottle, topicSteering)
+
+	cli.HandleExit(p)
+
+	zap.S().Info("devices ready, start event listener")
 	err = p.Start()
 	if err != nil {
 		zap.S().Fatalf("unable to start service: %v", err)

pkg/actuator/pca9685.go

@@ -1,39 +1,112 @@
 package actuator
 
 import (
+	"fmt"
+	"github.com/cyrilix/robocar-pca9685/pkg/util"
 	"go.uber.org/zap"
+	"periph.io/x/conn/v3/gpio"
 	"periph.io/x/conn/v3/i2c/i2creg"
 	"periph.io/x/conn/v3/physic"
 	"periph.io/x/devices/v3/pca9685"
 	"periph.io/x/host/v3"
 )
 
-var (
+const (
-	device *pca9685.Dev
+	MinPercent = -1.
+	MaxPercent = 1.
 )
 
-func init() {
+type PWM int
-	zap.S().Info("init pca9685 controller")
+
+func NewDevice(freq physic.Frequency) *pca9685.Dev {
+	zap.S().Info("NewDevice pca9685 controller")
 	_, err := host.Init()
 	if err != nil {
-		zap.S().Fatalf("unable to init host: %v", err)
+		zap.S().Fatalf("unable to NewDevice host: %v", err)
 	}
 
 	zap.S().Info("open i2c bus")
 	bus, err := i2creg.Open("")
 	if err != nil {
-		zap.S().Fatalf("unable to init i2c bus: %v", err)
+		zap.S().Fatalf("unable to NewDevice i2c bus: %v", err)
 	}
-	zap.S().Info("i2c bus opened")
+	zap.S().Infof("i2c bus opened: %v", bus)
 
-	device, err = pca9685.NewI2C(bus, pca9685.I2CAddr)
+	device, err := pca9685.NewI2C(bus, pca9685.I2CAddr)
 	if err != nil {
-		zap.S().Fatalf("unable to init pca9685 bus: %v", err)
+		zap.S().Fatalf("unable to NewDevice pca9685 bus: %v", err)
 	}
 	zap.S().Infof("set pwm frequency to %d", 60)
-	err = device.SetPwmFreq(60 * physic.Hertz)
+	err = device.SetPwmFreq(freq)
 	if err != nil {
 		zap.S().Panicf("unable to set pwm frequency: %v", err)
 	}
-	zap.S().Info("init done")
+	zap.S().Info("NewDevice done")
+	return device
+}
+
+func convertToDuty(percent float32, freq physic.Frequency, centerPWM, minPWM, maxPWM PWM) (gpio.Duty, error) {
+	// map absolute angle to angle that vehicle can implement.
+	pw := int(centerPWM)
+	if percent > 0 {
+		pw = util.MapRange(float64(percent), 0, MaxPercent, float64(centerPWM), float64(maxPWM))
+	} else if percent < 0 {
+		pw = util.MapRange(float64(percent), MinPercent, 0, float64(minPWM), float64(centerPWM))
+	}
+	zap.S().Debugf("convert value %v to pw: %v", percent, pw)
+
+	per := freq.Period().Microseconds()
+
+	draw := float64(pw) / float64(per)
+	d, err := gpio.ParseDuty(fmt.Sprintf("%.f%%", draw*100))
+	if err != nil {
+		return 0, fmt.Errorf("unable to parse duty, probably bad compute: %w", err)
+	}
+	return d, nil
+}
+
+type Pca9685Controller struct {
+	pin                        gpio.PinIO
+	minPWM, maxPWM, neutralPWM PWM
+	freq                       physic.Frequency
+}
+
+func (c *Pca9685Controller) Close() error {
+	return c.pin.Halt()
+}
+
+func (c *Pca9685Controller) SetDuty(d gpio.Duty) error {
+	err := c.pin.PWM(d, c.freq)
+	if err != nil {
+		return fmt.Errorf("unable to set pwm value: %v", err)
+	}
+	return nil
+}
+
+// SetPercentValue Set percent value
+func (c *Pca9685Controller) SetPercentValue(p float32) error {
+	d, err := convertToDuty(p, c.freq, c.neutralPWM, c.minPWM, c.maxPWM)
+	if err != nil {
+		return fmt.Errorf("unable to compute Duty value for steering: %w", err)
+	}
+	err = c.SetDuty(d)
+	if err != nil {
+		return fmt.Errorf("unable to apply duty value '%v' for pin '%v': '%w' ", d, c.pin.Name(), err)
+	}
+	return nil
+}
+
+func NewPca9685Controller(device *pca9685.Dev, channel int, minPWM, maxPWM, neutralPWM PWM, freq physic.Frequency) (*Pca9685Controller, error) {
+	p, err := device.CreatePin(channel)
+	if err != nil {
+		return nil, fmt.Errorf("unable to create pin for channel %v: %w", channel, err)
+	}
+	s := Pca9685Controller{
+		pin:        p,
+		minPWM:     minPWM,
+		maxPWM:     maxPWM,
+		neutralPWM: neutralPWM,
+		freq:       freq,
+	}
+	return &s, nil
 }

pkg/actuator/pca9685_test.go

@@ -0,0 +1,81 @@
+package actuator
+
+import (
+	"go.uber.org/zap"
+	"periph.io/x/conn/v3/physic"
+	"testing"
+)
+
+func init() {
+	l, _ := zap.NewDevelopment()
+	zap.ReplaceGlobals(l)
+}
+
+func Test_convertToDuty(t *testing.T) {
+	type fields struct {
+	}
+	type args struct {
+		percent   float32
+		leftPWM   PWM
+		rightPWM  PWM
+		centerPWM PWM
+		freq      physic.Frequency
+	}
+	tests := []struct {
+		name    string
+		fields  fields
+		args    args
+		want    string
+		wantErr bool
+	}{
+		{
+			name: "center",
+			args: args{
+				percent:   0.,
+				leftPWM:   1000,
+				rightPWM:  2000,
+				centerPWM: 1500,
+				freq:      60 * physic.Hertz,
+			},
+			want: "9%",
+		},
+		{
+			name: "left",
+			args: args{
+				percent:   -1.,
+				leftPWM:   1000,
+				rightPWM:  2000,
+				centerPWM: 1500,
+				freq:      60 * physic.Hertz,
+			},
+			want: "6%",
+		},
+		{
+			name: "right",
+			args: args{
+				percent:   1.,
+				leftPWM:   1000,
+				rightPWM:  2000,
+				centerPWM: 1500,
+				freq:      60 * physic.Hertz,
+			},
+			want: "12%",
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+
+			if got, err := convertToDuty(tt.args.percent, tt.args.freq, tt.args.centerPWM, tt.args.leftPWM, tt.args.rightPWM); got.String() != tt.want {
+				if tt.wantErr && err == nil {
+					t.Errorf("an error is expected")
+					return
+				}
+				if err != nil {
+					t.Errorf("unexpected error: %v", err)
+					return
+				}
+				t.Errorf("convertToDuty() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}

pkg/actuator/steering.go

@@ -1,52 +0,0 @@
-package actuator
-
-import (
-	"github.com/cyrilix/robocar-pca9685/pkg/util"
-	"go.uber.org/zap"
-	"periph.io/x/conn/v3/gpio"
-	"periph.io/x/devices/v3/pca9685"
-)
-
-const (
-	LeftAngle  = -1.
-	RightAngle = 1.
-)
-
-type Steering struct {
-	channel                      int
-	leftPWM, rightPWM, centerPWM int
-	dev                          *pca9685.Dev
-}
-
-func (s *Steering) SetPulse(pulse int) {
-	err := s.dev.SetPwm(s.channel, 0, gpio.Duty(pulse))
-	if err != nil {
-		zap.S().Warnf("unable to set steering pwm value: %v", err)
-	}
-
-}
-
-// SetPercentValue Set percent value steering
-func (s *Steering) SetPercentValue(p float32) {
-	// map absolute angle to angle that vehicle can implement.
-
-	pulse := 0
-	if p > 0 {
-		pulse = util.MapRange(float64(p), 0, RightAngle, float64(s.centerPWM), float64(s.rightPWM))
-	} else if p < 0 {
-		pulse = util.MapRange(float64(p), LeftAngle, 0, float64(s.leftPWM), float64(s.centerPWM))
-	}
-	zap.S().Debugf("convert steering %v to %v", p, pulse)
-	s.SetPulse(pulse)
-}
-
-func NewSteering(channel, leftPWM, rightPWM, centerPWM int) *Steering {
-	s := Steering{
-		channel:   channel,
-		dev:       device,
-		leftPWM:   leftPWM,
-		rightPWM:  rightPWM,
-		centerPWM: centerPWM,
-	}
-	return &s
-}

pkg/actuator/throttle.go

@@ -1,54 +0,0 @@
-package actuator
-
-import (
-	"github.com/cyrilix/robocar-pca9685/pkg/util"
-	"go.uber.org/zap"
-	"periph.io/x/conn/v3/gpio"
-	"periph.io/x/devices/v3/pca9685"
-)
-
-const (
-	MinThrottle = -1
-	MaxThrottle = 1
-)
-
-type Throttle struct {
-	channel                       int
-	zeroPulse, minPulse, maxPulse int
-	dev                           *pca9685.Dev
-}
-
-func (t *Throttle) SetPulse(pulse int) {
-	err := t.dev.SetPwm(t.channel, 0, gpio.Duty(pulse))
-	if err != nil {
-		zap.S().Errorf("unable to set throttle pwm value: %v", err)
-	}
-
-}
-
-// Set percent value throttle
-func (t *Throttle) SetPercentValue(p float32) {
-	var pulse int
-	if p > 0 {
-		pulse = util.MapRange(float64(p), 0, MaxThrottle, float64(t.zeroPulse), float64(t.maxPulse))
-	} else {
-		pulse = util.MapRange(float64(p), MinThrottle, 0, float64(t.minPulse), float64(t.zeroPulse))
-	}
-	zap.S().Debugf("set throttle to %v-> %v (%v, %v, %v, %v, %v)", p, pulse, LeftAngle, RightAngle, t.minPulse, t.maxPulse, t.zeroPulse)
-	t.SetPulse(pulse)
-}
-
-func NewThrottle(channel, zeroPulse, minPulse, maxPulse int) *Throttle {
-	t := Throttle{
-		channel:   channel,
-		dev:       device,
-		zeroPulse: zeroPulse,
-		minPulse:  minPulse,
-		maxPulse:  maxPulse,
-	}
-
-	zap.S().Info("send zero pulse to calibrate ESC")
-	t.SetPercentValue(0)
-
-	return &t
-}

pkg/part/part.go

@@ -3,18 +3,24 @@ package part
 import (
 	"fmt"
 	"github.com/cyrilix/robocar-base/service"
-	"github.com/cyrilix/robocar-pca9685/pkg/actuator"
 	"github.com/cyrilix/robocar-protobuf/go/events"
 	MQTT "github.com/eclipse/paho.mqtt.golang"
 	"go.uber.org/zap"
 	"google.golang.org/protobuf/proto"
+	"io"
 	"sync"
 )
 
+type ActuatorController interface {
+	// SetPercentValue Set percent value
+	SetPercentValue(p float32) error
+	io.Closer
+}
+
 type Pca9685Part struct {
 	client       MQTT.Client
-	throttleCtrl *actuator.Throttle
+	throttleCtrl ActuatorController
-	steeringCtrl *actuator.Steering
+	steeringCtrl ActuatorController
 
 	muSteering    sync.Mutex
 	steeringValue float32
@@ -29,7 +35,7 @@ type Pca9685Part struct {
 	cancel chan interface{}
 }
 
-func NewPca9685Part(client MQTT.Client, throttleCtrl *actuator.Throttle, steeringCtrl *actuator.Steering, updateFrequency int, throttleTopic, steeringTopic string) *Pca9685Part {
+func NewPca9685Part(client MQTT.Client, throttleCtrl ActuatorController, steeringCtrl ActuatorController, updateFrequency int, throttleTopic, steeringTopic string) *Pca9685Part {
 	return &Pca9685Part{
 		client:          client,
 		throttleCtrl:    throttleCtrl,
@@ -46,8 +52,12 @@ func (p *Pca9685Part) Start() error {
 		return fmt.Errorf("unable to start service: %v", err)
 	}
 
-	p.steeringCtrl.SetPercentValue(0)
+	if err := p.steeringCtrl.SetPercentValue(0); err != nil {
-	p.throttleCtrl.SetPercentValue(0)
+		return fmt.Errorf("unable to start steering controller: %w", err)
+	}
+	if err := p.throttleCtrl.SetPercentValue(0); err != nil {
+		return fmt.Errorf("unable to set init value '0%%': %w", err)
+	}
 
 	for {
 		select {
@@ -59,6 +69,12 @@ func (p *Pca9685Part) Start() error {
 
 func (p *Pca9685Part) Stop() {
 	close(p.cancel)
+	if err := p.steeringCtrl.Close(); err != nil {
+		zap.S().Errorf("unable to close steering controller: %v", err)
+	}
+	if err := p.throttleCtrl.Close(); err != nil {
+		zap.S().Errorf("unable to close throttle controller: %v", err)
+	}
 	service.StopService("pca9685", p.client, p.throttleTopic, p.steeringTopic)
 }
 
@@ -74,7 +90,10 @@ func (p *Pca9685Part) onThrottleChange(_ MQTT.Client, message MQTT.Message) {
 	zap.S().Debugf("new throttle value: %v", throttle.GetThrottle())
 	p.muThrottle.Lock()
 	defer p.muThrottle.Unlock()
-	p.throttleCtrl.SetPercentValue(throttle.GetThrottle())
+	err = p.throttleCtrl.SetPercentValue(throttle.GetThrottle())
+	if err != nil {
+		zap.S().Errorf("unable to set steering value '%v': %w", throttle.GetThrottle(), err)
+	}
 }
 
 func (p *Pca9685Part) onSteeringChange(_ MQTT.Client, message MQTT.Message) {
@@ -90,7 +109,10 @@ func (p *Pca9685Part) onSteeringChange(_ MQTT.Client, message MQTT.Message) {
 	zap.S().Debugf("new steering value: %v", steering.GetSteering())
 	p.muSteering.Lock()
 	defer p.muSteering.Unlock()
-	p.steeringCtrl.SetPercentValue(steering.GetSteering())
+	err = p.steeringCtrl.SetPercentValue(steering.GetSteering())
+	if err != nil {
+		zap.S().Errorf("unable to set steering value '%v': %w", steering.GetSteering(), err)
+	}
 }
 
 func (p *Pca9685Part) registerCallbacks() error {