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

refactor(pwm steering): asymetric center

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-01-17 18:40:25 +01:00
parent 7360b2bfa4
commit fcfc60ea38
3 changed files with 225 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cmd/rc-arduino/rc-arduino.go
View File

@ -10,7 +10,16 @@ import (
"os"
)
const DefaultClientId = "robocar-arduino"
const (
DefaultClientId = "robocar-arduino"
SteeringLeftPWM = 1004
SteeringRightPWM = 1986
)
var (
SteeringCenterPWM = (SteeringRightPWM-SteeringLeftPWM)/2 + SteeringLeftPWM
)
func main() {
var mqttBroker, username, password, clientId string
@ -25,6 +34,17 @@ func main() {
cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int
if err := cli.SetIntDefaultValueFromEnv(&steeringLeftPWM, "STEERING_LEFT_PWM", SteeringLeftPWM); err != nil {
zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&steeringRightPWM, "STEERING_RIGHT_PWM", SteeringRightPWM); err != nil {
zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&steeringCenterPWM, "STEERING_CENTER_PWM", SteeringCenterPWM); err != nil {
zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
}
flag.Float64Var(&pubFrequency, "mqtt-pub-frequency", 25., "Number of messages to publish per second")
flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic where to publish throttle values, use MQTT_TOPIC_THROTTLE if args not set")
flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic where to publish steering values, use MQTT_TOPIC_STEERING if args not set")
@ -32,6 +52,9 @@ func main() {
flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic where to publish switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set")
flag.StringVar(&device, "device", "/dev/serial0", "Serial device")
flag.IntVar(&baud, "baud", 115200, "Serial baud")
flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "Right left value for steering PWM, STEERING_LEFT_PWM env if args not set")
flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "Right right value for steering PWM, STEERING_RIGHT_PWM env if args not set")
flag.IntVar(&steeringCenterPWM, "steering-center-pwm", steeringCenterPWM, "Center value for steering PWM, STEERING_CENTER_PWM env if args not set")
flag.BoolVar(&debug, "debug", false, "Display raw value to debug")
flag.Parse()
@ -63,7 +86,8 @@ func main() {
}
defer client.Disconnect(10)
a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency)
sc := arduino.NewAsymetricPWMSteeringConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM)
a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency, sc)
err = a.Start()
if err != nil {
zap.S().Errorw("unable to start service", "error", err)

50
pkg/arduino/arduino.go
View File

@ -16,9 +16,6 @@ import (
)
const (
MinPwmAngle = 999.0
MaxPwmAngle = 1985.0
MinPwmThrottle = 972.0
MaxPwmThrottle = 1954.0
)
@ -38,9 +35,31 @@ type Part struct {
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
pwmSteeringConfig PWMSteeringConfig
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string, pubFrequency float64) *Part {
type PWMSteeringConfig struct {
Left int
Right int
Center int
}
func NewPWMSteeringConfig(min, max int) PWMSteeringConfig {
return PWMSteeringConfig{
Left: min,
Right: max,
Center: min + (max-min)/2,
}
}
func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
c := NewPWMSteeringConfig(min, max)
c.Center = middle
return c
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic,
switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig) *Part {
c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c)
if err != nil {
@ -56,6 +75,8 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
pwmSteeringConfig: steeringConfig,
}
}
@ -109,12 +130,23 @@ func (a *Part) processChannel1(v string) {
if err != nil {
zap.S().Errorf("invalid value for channel1, should be an int: %v", err)
}
if value < MinPwmAngle {
value = MinPwmAngle
} else if value > MaxPwmAngle {
value = MaxPwmAngle
a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center)
}
func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 {
if value < minPwm {
value = minPwm
} else if value > maxPwm {
value = maxPwm
}
a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
if value == middlePwm {
return 0.
}
if value < middlePwm {
return (float32(value) - float32(middlePwm)) / float32(middlePwm-minPwm)
}
// middle < value < max
return (float32(value) - float32(middlePwm)) / float32(maxPwm-middlePwm)
}
func (a *Part) processChannel2(v string) {

162
pkg/arduino/arduino_test.go
View File

@ -6,12 +6,22 @@ import (
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
"math"
"net"
"sync"
"testing"
"time"
)
const (
MinPwmAngle = 999.0
MaxPwmAngle = 1985.0
)
var (
MiddlePwmAngle = int((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle)
)
func TestArduinoPart_Update(t *testing.T) {
oldPublish := publish
defer func() { publish = oldPublish }()
@ -48,7 +58,7 @@ func TestArduinoPart_Update(t *testing.T) {
}
}()
a := Part{client: nil, serial: conn, pubFrequency: 100}
a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle)}
go func() {
err := a.Start()
if err != nil {
@ -103,14 +113,14 @@ func TestArduinoPart_Update(t *testing.T) {
fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,50,%d\n", 99, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -1., events.DriveMode_USER, false},
{"Sterring: left",
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", 998, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.93, events.DriveMode_USER, false},
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.92, events.DriveMode_USER, false},
{"Sterring: middle",
fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,50,%d\n", 1450, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.04, events.DriveMode_USER, false},
-1., -0.09, events.DriveMode_USER, false},
{"Sterring: right",
fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,50,%d\n", 1958, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., 0.96, events.DriveMode_USER, false},
-1., 0.95, events.DriveMode_USER, false},
{"Sterring: over right",
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,50,%d\n", 2998, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., 1., events.DriveMode_USER, false},
@ -285,3 +295,145 @@ func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) {
t.Errorf("unable to unmarshal protobuf content to %T: %v", msg, err)
}
}
func Test_convertPwmSteeringToPercent(t *testing.T) {
type args struct {
value int
middle int
min int
max int
}
tests := []struct {
name string
args args
want float32
}{
{
name: "middle",
args: args{
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 0.,
},
{
name: "left",
args: args{
value: MinPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: -1.,
},
{
name: "mid-left",
args: args{
value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle - (MaxPwmAngle-MinPwmAngle)/4)),
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: -0.4989858,
},
{
name: "over left",
args: args{
value: MinPwmAngle - 100,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: -1.,
},
{
name: "right",
args: args{
value: MaxPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 1.,
},
{
name: "mid-right",
args: args{
value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + (MaxPwmAngle-MinPwmAngle)/4)),
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 0.5010142,
},
{
name: "over right",
args: args{
value: MaxPwmAngle + 100,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 1.,
},
{
name: "asymetric middle",
args: args{
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 0.,
},
{
name: "asymetric mid-left",
args: args{
value: int(math.Round(((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100-MinPwmAngle)/2) + MinPwmAngle),
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: -0.49915683,
},
{
name: "asymetric left",
args: args{
value: MinPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: -1.,
},
{
name: "asymetric mid-right",
args: args{
value: int(math.Round((MaxPwmAngle - (MaxPwmAngle-((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100))/2))),
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 0.50127226,
},
{
name: "asymetric right",
args: args{
value: MaxPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
min: MinPwmAngle,
max: MaxPwmAngle,
},
want: 1.,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
if got := convertPwmSteeringToPercent(tt.args.value, tt.args.min, tt.args.max, tt.args.middle); got != tt.want {
t.Errorf("convertPwmSteeringToPercent() = %v, want %v", got, tt.want)
}
})
}
}