feat: asymetric throttle

cmd/rc-arduino/rc-arduino.go

@@ -15,6 +15,10 @@ const (
 
 	SteeringLeftPWM  = 1004
 	SteeringRightPWM = 1986
+
+	ThrottleZeroPWM = 1260
+	ThrottleMinPWM  = 972.0
+	ThrottleMaxPWM  = 1954.0
 )
 
 var (
@@ -34,6 +38,7 @@ func main() {
 	cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
 
 	var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int
+	var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int
 	if err := cli.SetIntDefaultValueFromEnv(&steeringLeftPWM, "STEERING_LEFT_PWM", SteeringLeftPWM); err != nil {
 		zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err)
 	}
@@ -43,6 +48,15 @@ func main() {
 	if err := cli.SetIntDefaultValueFromEnv(&steeringCenterPWM, "STEERING_CENTER_PWM", SteeringCenterPWM); err != nil {
 		zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
 	}
+	if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", ThrottleMinPWM); err != nil {
+		zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err)
+	}
+	if err := cli.SetIntDefaultValueFromEnv(&throttleMaxPWM, "THROTTLE_MAX_PWM", ThrottleMaxPWM); err != nil {
+		zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
+	}
+	if err := cli.SetIntDefaultValueFromEnv(&throttleZeroPWM, "THROTTLE_ZERO_PWM", ThrottleZeroPWM); 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")
@@ -54,6 +68,9 @@ func main() {
 	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.IntVar(&throttleMinPWM, "throttle-min-pwm", throttleMinPWM, "Min value for throttle PWM, THROTTLE_MIN_PWM env if args not set")
+	flag.IntVar(&steeringRightPWM, "throttle-max-pwm", throttleMaxPWM, "Throttle max value for throttle PWM, THROTTLE_MAX_PWM env if args not set")
+	flag.IntVar(&steeringCenterPWM, "throttle-zero-pwm", throttleZeroPWM, "Zero value for throttle PWM, THROTTLE_ZERO_PWM env if args not set")
 
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 	flag.Parse()
@@ -83,7 +100,8 @@ func main() {
 	defer client.Disconnect(10)
 
 	sc := arduino.NewAsymetricPWMSteeringConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM)
-	a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency, sc)
+	tc := arduino.PWMThrottleConfig{Min: throttleMinPWM, Max: throttleMaxPWM, Zero: throttleZeroPWM}
+	a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency, sc, tc)
 
 	cli.HandleExit(a)
 

pkg/arduino/arduino.go

@@ -37,6 +37,13 @@ type Part struct {
 	cancel                                                          chan interface{}
 
 	pwmSteeringConfig PWMSteeringConfig
+	pwmThrottleConfig PWMThrottleConfig
+}
+
+type PWMThrottleConfig struct {
+	Min  int
+	Max  int
+	Zero int
 }
 
 type PWMSteeringConfig struct {
@@ -52,6 +59,7 @@ func NewPWMSteeringConfig(min, max int) PWMSteeringConfig {
 		Center: min + (max-min)/2,
 	}
 }
+
 func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
 	c := NewPWMSteeringConfig(min, max)
 	c.Center = middle
@@ -59,7 +67,7 @@ func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
 }
 
 func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic,
-	switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig) *Part {
+	switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig, throttleConfig PWMThrottleConfig) *Part {
 	c := &serial.Config{Name: name, Baud: baud}
 	s, err := serial.OpenPort(c)
 	if err != nil {
@@ -77,6 +85,7 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
 		cancel:            make(chan interface{}),
 
 		pwmSteeringConfig: steeringConfig,
+		pwmThrottleConfig: throttleConfig,
 	}
 }
 
@@ -156,12 +165,21 @@ func (a *Part) processChannel2(v string) {
 	if err != nil {
 		zap.S().Errorf("invalid throttle value for channel2, should be an int: %v", err)
 	}
-	if value < MinPwmThrottle {
+	if value < a.pwmThrottleConfig.Min {
-		value = MinPwmThrottle
+		value = a.pwmThrottleConfig.Min
-	} else if value > MaxPwmThrottle {
+	} else if value > a.pwmThrottleConfig.Max {
-		value = MaxPwmThrottle
+		value = a.pwmThrottleConfig.Max
 	}
-	a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
+
+	throttle := 0.
+	if value > a.pwmThrottleConfig.Zero {
+		throttle = (float64(value) - float64(a.pwmThrottleConfig.Zero)) / float64(a.pwmThrottleConfig.Max-a.pwmThrottleConfig.Zero)
+	}
+	if value < a.pwmThrottleConfig.Zero {
+		throttle = -1. * (float64(a.pwmThrottleConfig.Zero) - float64(value)) / (float64(a.pwmThrottleConfig.Zero - a.pwmThrottleConfig.Min))
+	}
+
+	a.throttle = float32(throttle)
 }
 
 func (a *Part) processChannel3(v string) {
@@ -248,7 +266,7 @@ func (a *Part) publishThrottle() {
 		return
 	}
 	zap.L().Debug("throttle channel", zap.Float32("throttle", a.throttle))
-	publish(a.client, a.throttleTopic, &throttleMessage)
+	publish(a.client, a.throttleTopic, throttleMessage)
 }
 
 func (a *Part) publishSteering() {
@@ -262,7 +280,7 @@ func (a *Part) publishSteering() {
 		return
 	}
 	zap.L().Debug("steering channel", zap.Float32("steering", a.steering))
-	publish(a.client, a.steeringTopic, &steeringMessage)
+	publish(a.client, a.steeringTopic, steeringMessage)
 }
 
 func (a *Part) publishDriveMode() {
@@ -274,7 +292,7 @@ func (a *Part) publishDriveMode() {
 		zap.S().Errorf("unable to marshal protobuf driveMode message: %v", err)
 		return
 	}
-	publish(a.client, a.driveModeTopic, &driveModeMessage)
+	publish(a.client, a.driveModeTopic, driveModeMessage)
 }
 
 func (a *Part) publishSwitchRecord() {
@@ -286,9 +304,9 @@ func (a *Part) publishSwitchRecord() {
 		zap.S().Errorf("unable to marshal protobuf SwitchRecord message: %v", err)
 		return
 	}
-	publish(a.client, a.switchRecordTopic, &switchRecordMessage)
+	publish(a.client, a.switchRecordTopic, switchRecordMessage)
 }
 
-var publish = func(client mqtt.Client, topic string, payload *[]byte) {
+var publish = func(client mqtt.Client, topic string, payload []byte) {
-	client.Publish(topic, 0, false, *payload)
+	client.Publish(topic, 0, false, payload)
 }

pkg/arduino/arduino_test.go

@@ -26,7 +26,7 @@ func TestArduinoPart_Update(t *testing.T) {
 	oldPublish := publish
 	defer func() { publish = oldPublish }()
 
-	publish = func(client mqtt.Client, topic string, payload *[]byte) {}
+	publish = func(client mqtt.Client, topic string, payload []byte) {}
 
 	ln, err := net.Listen("tcp", ":8080")
 	if err != nil {
@@ -58,7 +58,11 @@ func TestArduinoPart_Update(t *testing.T) {
 		}
 	}()
 
-	a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle)}
+	defaultPwmThrottleConfig := PWMThrottleConfig{MinPwmThrottle, MaxPwmThrottle, MinPwmThrottle + (MaxPwmThrottle-MinPwmAngle)/int(2)}
+	a := Part{client: nil, serial: conn, pubFrequency: 100,
+		pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
+	}
+
 	go func() {
 		err := a.Start()
 		if err != nil {
@@ -70,76 +74,93 @@ func TestArduinoPart_Update(t *testing.T) {
 	channel1, channel2, channel3, channel4, channel5, channel6, distanceCm := 678, 910, 1112, 1678, 1910, 112, 128
 	cases := []struct {
 		name, content                      string
+		throttlePwmConfig                  PWMThrottleConfig
 		expectedThrottle, expectedSteering float32
 		expectedDriveMode                  events.DriveMode
 		expectedSwitchRecord               bool
 	}{
 		{"Good value",
 			fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, channel6, distanceCm),
-			-1., -1., events.DriveMode_USER, false},
+			defaultPwmThrottleConfig, -1., -1.,
+			events.DriveMode_USER, false},
 		{"Unparsable line",
-			"12350,invalid line\n",
+			"12350,invalid line\n", defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"Switch record on",
-			fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 998, channel6, distanceCm),
+			fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 998, channel6, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, true},
 
 		{"Switch record off",
-			fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1987, channel6, distanceCm),
+			fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1987, channel6, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"Switch record off",
-			fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1850, channel6, distanceCm),
+			fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1850, channel6, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"Switch record on",
-			fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm),
+			fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, true},
 
 		{"DriveMode: user",
-			fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm),
+			fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"DriveMode: pilot",
-			fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1987, distanceCm),
+			fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1987, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_PILOT, false},
 		{"DriveMode: pilot",
-			fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1850, distanceCm),
+			fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1850, distanceCm), defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_PILOT, false},
 
 		// DriveMode: user
 		{"DriveMode: user",
 			fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1003, distanceCm),
+			defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 
 		{"Sterring: over left",
 			fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,50,%d\n", 99, channel2, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"Sterring: left",
 			fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			-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),
+			defaultPwmThrottleConfig,
 			-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),
+			defaultPwmThrottleConfig,
 			-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),
+			defaultPwmThrottleConfig,
 			-1., 1., events.DriveMode_USER, false},
 
 		{"Throttle: over down",
 			fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 99, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			-1., -1., events.DriveMode_USER, false},
 		{"Throttle: down",
 			fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 998, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			-0.95, -1., events.DriveMode_USER, false},
 		{"Throttle: stop",
 			fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 1450, channel3, channel4, channel5, channel6, distanceCm),
-			-0.03, -1., events.DriveMode_USER, false},
+			defaultPwmThrottleConfig,
+			0.0, -1., events.DriveMode_USER, false},
 		{"Throttle: up",
 			fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 1948, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			0.99, -1., events.DriveMode_USER, false},
 		{"Throttle: over up",
 			fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 2998, channel3, channel4, channel5, channel6, distanceCm),
+			defaultPwmThrottleConfig,
 			1., -1., events.DriveMode_USER, false},
+		{"Throttle: zero not middle",
+			fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 1600, channel3, channel4, channel5, channel6, distanceCm),
+			PWMThrottleConfig{1000, 1700, 1500},
+			0.5, -1., events.DriveMode_USER, false},
 	}
 
 	for _, c := range cases {
@@ -153,6 +174,8 @@ func TestArduinoPart_Update(t *testing.T) {
 			t.Error("unable to flush content")
 		}
 
+		a.pwmThrottleConfig = c.throttlePwmConfig
+
 		time.Sleep(10 * time.Millisecond)
 		a.mutex.Lock()
 		if fmt.Sprintf("%0.2f", a.throttle) != fmt.Sprintf("%0.2f", c.expectedThrottle) {
@@ -177,10 +200,10 @@ func TestPublish(t *testing.T) {
 
 	var muPublishedEvents sync.Mutex
 	pulishedEvents := make(map[string][]byte)
-	publish = func(client mqtt.Client, topic string, payload *[]byte) {
+	publish = func(client mqtt.Client, topic string, payload []byte) {
 		muPublishedEvents.Lock()
 		defer muPublishedEvents.Unlock()
-		pulishedEvents[topic] = *payload
+		pulishedEvents[topic] = payload
 	}
 
 	ln, err := net.Listen("tcp", ":8080")