refactor: move packages to pkg directory

pkg/actuator/pca9685.go

@@ -0,0 +1,39 @@
+package actuator
+
+import (
+	"go.uber.org/zap"
+	"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 (
+	device *pca9685.Dev
+)
+
+func init() {
+	zap.S().Info("init pca9685 controller")
+	_, err := host.Init()
+	if err != nil {
+		zap.S().Fatalf("unable to init 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().Info("i2c bus opened")
+
+	device, err = pca9685.NewI2C(bus, pca9685.I2CAddr)
+	if err != nil {
+		zap.S().Fatalf("unable to init pca9685 bus: %v", err)
+	}
+	zap.S().Infof("set pwm frequency to %d", 60)
+	err = device.SetPwmFreq(60 * physic.Hertz)
+	if err != nil {
+		zap.S().Panicf("unable to set pwm frequency: %v", err)
+	}
+	zap.S().Info("init done")
+}

pkg/actuator/steering.go

@@ -0,0 +1,44 @@
+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 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)
+	}
+
+}
+
+// Set percent value steering
+func (s *Steering) SetPercentValue(p float32) {
+	// map absolute angle to angle that vehicle can implement.
+	pulse := util.MapRange(float64(p), LeftAngle, RightAngle, float64(s.leftPWM), float64(s.rightPWM))
+	s.SetPulse(pulse)
+}
+
+func NewSteering(channel, leftPWM, rightPWM int) *Steering {
+	s := Steering{
+		channel:  channel,
+		dev:      device,
+		leftPWM:  leftPWM,
+		rightPWM: rightPWM,
+	}
+	return &s
+}

pkg/actuator/throttle.go

@@ -0,0 +1,54 @@
+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

@@ -0,0 +1,107 @@
+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"
+	"github.com/golang/protobuf/proto"
+	"go.uber.org/zap"
+	"sync"
+)
+
+type Pca9685Part struct {
+	client       MQTT.Client
+	throttleCtrl *actuator.Throttle
+	steeringCtrl *actuator.Steering
+
+	muSteering    sync.Mutex
+	steeringValue float32
+	muThrottle    sync.Mutex
+	throttleValue float32
+
+	updateFrequency int
+
+	throttleTopic string
+	steeringTopic string
+
+	cancel chan interface{}
+}
+
+func NewPca9685Part(client MQTT.Client, throttleCtrl *actuator.Throttle, steeringCtrl *actuator.Steering, updateFrequency int, throttleTopic, steeringTopic string) *Pca9685Part {
+	return &Pca9685Part{
+		client:          client,
+		throttleCtrl:    throttleCtrl,
+		steeringCtrl:    steeringCtrl,
+		updateFrequency: updateFrequency,
+		throttleTopic:   throttleTopic,
+		steeringTopic:   steeringTopic,
+		cancel:          make(chan interface{}),
+	}
+}
+
+func (p *Pca9685Part) Start() error {
+	if err := p.registerCallbacks(); err != nil {
+		return fmt.Errorf("unable to start service: %v", err)
+	}
+
+	p.steeringCtrl.SetPercentValue(0)
+	p.throttleCtrl.SetPercentValue(0)
+
+	for {
+		select {
+		case <-p.cancel:
+			return nil
+		}
+	}
+}
+
+func (p *Pca9685Part) Stop() {
+	close(p.cancel)
+	service.StopService("pca9685", p.client, p.throttleTopic, p.steeringTopic)
+}
+
+func (p *Pca9685Part) onThrottleChange(_ MQTT.Client, message MQTT.Message) {
+	var throttle events.ThrottleMessage
+	err := proto.Unmarshal(message.Payload(), &throttle)
+	if err != nil {
+		zap.S().Warnw("unable to unmarshall throttle msg", "topic",
+			message.Topic(),
+			"error", err)
+		return
+	}
+	zap.S().Debugf("new throttle value: %v", throttle.GetThrottle())
+	p.muThrottle.Lock()
+	defer p.muThrottle.Unlock()
+	p.throttleCtrl.SetPercentValue(throttle.GetThrottle())
+}
+
+func (p *Pca9685Part) onSteeringChange(_ MQTT.Client, message MQTT.Message) {
+	var steering events.SteeringMessage
+	err := proto.Unmarshal(message.Payload(), &steering)
+	if err != nil {
+		zap.S().Warnw("unable to unmarshal steering msg",
+			"topic", message.Topic(),
+			"error", err,
+		)
+		return
+	}
+	p.muSteering.Lock()
+	defer p.muSteering.Unlock()
+	p.steeringCtrl.SetPercentValue(steering.GetSteering())
+}
+
+func (p *Pca9685Part) registerCallbacks() error {
+	err := service.RegisterCallback(p.client, p.throttleTopic, p.onThrottleChange)
+	if err != nil {
+		return fmt.Errorf("unable to register throttle callback: %v", err)
+	}
+
+	err = service.RegisterCallback(p.client, p.steeringTopic, p.onSteeringChange)
+	if err != nil {
+		return fmt.Errorf("unable to register steering callback: %v", err)
+	}
+
+	return nil
+}

pkg/util/util.go

@@ -0,0 +1,12 @@
+package util
+
+// MapRange Linear mapping between two ranges of values
+func MapRange(x, xmin, xmax, ymin, ymax float64) int {
+	Xrange := xmax - xmin
+	Yrange := ymax - ymin
+	XYratio := Xrange / Yrange
+
+	y := (x-xmin)/XYratio + ymin
+
+	return int(y)
+}

pkg/util/util_test.go

@@ -0,0 +1,32 @@
+package util
+
+import "testing"
+
+func TestRangeMapping(t *testing.T) {
+	cases := []struct {
+		x, xmin, xmax, ymin, ymax float64
+		expected                  int
+	}{
+		// Test positive
+		{-100, -100, 100, 0, 1000, 0},
+		{0, -100, 100, 0, 1000, 500},
+		{100, -100, 100, 0, 1000, 1000},
+
+		// Test negative
+		{0, 0, 100, 0, 1000, 0},
+		{50, 0, 100, 0, 1000, 500},
+		{100, 0, 100, 0, 1000, 1000},
+
+		// Reverse
+		{0, 100, 0, 0, 1000, 1000},
+		{50, 100, 0, 0, 1000, 500},
+		{100, 100, 0, 0, 1000, 0},
+	}
+
+	for _, c := range cases {
+		val := MapRange(c.x, c.xmin, c.xmax, c.ymin, c.ymax)
+		if val != c.expected {
+			t.Errorf("MapRange(%.0f, %.0f, %.0f, %.0f, %.0f): %d, wants %d", c.x, c.xmin, c.xmax, c.ymin, c.ymax, val, c.expected)
+		}
+	}
+}