refactor(pwm steering): asymetric center

cmd/rc-arduino/rc-arduino.go

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

pkg/arduino/arduino.go

@@ -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 {
+	a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center)
-		value = MinPwmAngle
-	} else if value > MaxPwmAngle {
-		value = MaxPwmAngle
 }
-	a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
+
+func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 {
+	if value < minPwm {
+		value = minPwm
+	} else if value > maxPwm {
+		value = maxPwm
+	}
+	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) {

pkg/arduino/arduino_test.go

@@ -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),
+			fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, distanceCm),
-			-1., -0.93, events.DriveMode_USER, false},
+			-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)
+			}
+		})
+	}
+}