From fa2a3ce53b90a30e039f9badfe9e3411eea4c0b2 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Tue, 24 May 2022 20:02:26 +0200 Subject: [PATCH] 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 | 46 +++++++++++++++-- pkg/actuator/pca9685.go | 95 +++++++++++++++++++++++++++++++----- pkg/actuator/pca9685_test.go | 81 ++++++++++++++++++++++++++++++ pkg/actuator/steering.go | 52 -------------------- pkg/actuator/throttle.go | 54 -------------------- pkg/part/part.go | 38 ++++++++++++--- 6 files changed, 238 insertions(+), 128 deletions(-) create mode 100644 pkg/actuator/pca9685_test.go delete mode 100644 pkg/actuator/steering.go delete mode 100644 pkg/actuator/throttle.go diff --git a/cmd/rc-pca9685/rc-pca9685.go b/cmd/rc-pca9685/rc-pca9685.go index 443b587..3554c22 100644 --- a/cmd/rc-pca9685/rc-pca9685.go +++ b/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) - s := actuator2.NewSteering(steeringChannel, steeringLeftPWM, steeringRightPWM, steeringCenterPWM) + freq := DefaultFrequency + + 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) diff --git a/pkg/actuator/pca9685.go b/pkg/actuator/pca9685.go index 08732c5..d61c706 100644 --- a/pkg/actuator/pca9685.go +++ b/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 ( - device *pca9685.Dev +const ( + MinPercent = -1. + MaxPercent = 1. ) -func init() { - zap.S().Info("init pca9685 controller") +type PWM int + +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 } diff --git a/pkg/actuator/pca9685_test.go b/pkg/actuator/pca9685_test.go new file mode 100644 index 0000000..9808fe5 --- /dev/null +++ b/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) + } + }) + } +} diff --git a/pkg/actuator/steering.go b/pkg/actuator/steering.go deleted file mode 100644 index 9f8e7be..0000000 --- a/pkg/actuator/steering.go +++ /dev/null @@ -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 -} diff --git a/pkg/actuator/throttle.go b/pkg/actuator/throttle.go deleted file mode 100644 index 4a4e10f..0000000 --- a/pkg/actuator/throttle.go +++ /dev/null @@ -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 -} diff --git a/pkg/part/part.go b/pkg/part/part.go index ceeb9e4..6586e1d 100644 --- a/pkg/part/part.go +++ b/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 - steeringCtrl *actuator.Steering + throttleCtrl ActuatorController + 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) - p.throttleCtrl.SetPercentValue(0) + if err := p.steeringCtrl.SetPercentValue(0); err != nil { + 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 {