diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index b431ac6..d3e45ec 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/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() @@ -52,7 +75,7 @@ func main() { } defer func() { if err := lgr.Sync(); err != nil { - log.Printf("unable to Sync logger: %v\n", err) + log.Printf("unable to Sync logger: %v\n", err) } }() zap.ReplaceGlobals(lgr) @@ -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) diff --git a/pkg/arduino/arduino.go b/pkg/arduino/arduino.go index b6f51b8..9aa9485 100644 --- a/pkg/arduino/arduino.go +++ b/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 { - 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) { diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index 427d491..269e1e2 100644 --- a/pkg/arduino/arduino_test.go +++ b/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), - -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) + } + }) + } +}