robocars/robocar-pca9685
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

refactor: rewrite and fix pca9685 controllers

Browse Source 
* 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
This commit is contained in:
Cyrille Nofficial
2022-05-24 20:02:26 +02:00
parent 8effe3b576
commit fa2a3ce53b
6 changed files with 238 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
cmd/rc-pca9685/rc-pca9685.go
View File

@@ -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)

95
pkg/actuator/pca9685.go
View File

@@ -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
}

81
pkg/actuator/pca9685_test.go Normal file
View File

@@ -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)
}
})
}
}

52
pkg/actuator/steering.go
View File

@@ -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
}

54
pkg/actuator/throttle.go
View File

@@ -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
}

38
pkg/part/part.go
View File

@@ -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 {