From a07d04226952fec1380ea72a4aea3ec99666eed1 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Wed, 10 Aug 2022 16:49:58 +0200 Subject: [PATCH] feat: allow to control car with 2nd radio-command --- cmd/rc-arduino/rc-arduino.go | 50 +++++++--- pkg/arduino/arduino.go | 183 ++++++++++++++++++++++++++--------- pkg/arduino/arduino_test.go | 163 +++++++++++++++++-------------- 3 files changed, 267 insertions(+), 129 deletions(-) diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index b306d80..461317c 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/cmd/rc-arduino/rc-arduino.go @@ -38,7 +38,7 @@ func main() { cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain) var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int - var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int + var secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM int if err := cli.SetIntDefaultValueFromEnv(&steeringLeftPWM, "STEERING_LEFT_PWM", SteeringLeftPWM); err != nil { zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err) } @@ -46,7 +46,19 @@ func main() { 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) + zap.S().Warnf("unable to init steeringCenterPWM arg: %v", err) + } + + var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int + var secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleCenterPWM int + if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", arduino.DefaultPwmThrottle.Min); err != nil { + zap.S().Warnf("unable to init throttleMinPWM arg: %v", err) + } + if err := cli.SetIntDefaultValueFromEnv(&throttleMaxPWM, "THROTTLE_MAX_PWM", arduino.DefaultPwmThrottle.Max); err != nil { + zap.S().Warnf("unable to init throttleMaxPWM arg: %v", err) + } + if err := cli.SetIntDefaultValueFromEnv(&throttleZeroPWM, "THROTTLE_CENTER_PWM", arduino.DefaultPwmThrottle.Middle); err != nil { + zap.S().Warnf("unable to init throttleZeroPWM arg: %v", err) } if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", ThrottleMinPWM); err != nil { zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err) @@ -65,12 +77,20 @@ 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.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") + + flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set") + flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set") + flag.IntVar(&steeringCenterPWM, "steering-center-pwm", steeringCenterPWM, "middlePwm value for steering PWM, STEERING_CENTER_PWM env if args not set") + flag.IntVar(&secondarySteeringLeftPWM, "steering-secondary-left-pwm", steeringLeftPWM, "maxPwm left value for secondary radio controller steering PWM, STEERING_LEFT_PWM env if args not set") + flag.IntVar(&secondarySteeringRightPWM, "steering-secondary-right-pwm", steeringRightPWM, "maxPwm right value for secondary radio controller steering PWM, STEERING_RIGHT_PWM env if args not set") + flag.IntVar(&secondarySteeringCenterPWM, "steering-secondary-center-pwm", steeringCenterPWM, "middlePwm value for secondary radio controller steering PWM, STEERING_CENTER_PWM env if args not set") + + flag.IntVar(&throttleMinPWM, "throttle-min-pwm", throttleMinPWM, "maxPwm min value for throttle PWM, THROTTLE_MIN_PWM env if args not set") + flag.IntVar(&throttleMaxPWM, "throttle-max-pwm", throttleMaxPWM, "maxPwm max value for throttle PWM, THROTTLE_MAX_PWM env if args not set") + flag.IntVar(&throttleZeroPWM, "throttle-center-pwm", throttleZeroPWM, "middlePwm value for throttle PWM, THROTTLE_CENTER_PWM env if args not set") + flag.IntVar(&secondaryThrottleMinPWM, "throttle-secondary-min-pwm", throttleMinPWM, "maxPwm min value for secondary radio controller throttle PWM, THROTTLE_MIN_PWM env if args not set") + flag.IntVar(&secondaryThrottleMaxPWM, "throttle-secondary-max-pwm", throttleMaxPWM, "maxPwm max value for secondary radio controller throttle PWM, THROTTLE_MAX_PWM env if args not set") + flag.IntVar(&secondaryThrottleCenterPWM, "throttle-secondary-center-pwm", throttleZeroPWM, "middlePwm value for secondary radio controller throttle PWM, THROTTLE_CENTER_PWM env if args not set") logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level") flag.Parse() @@ -99,9 +119,17 @@ func main() { } defer client.Disconnect(10) - sc := arduino.NewAsymetricPWMSteeringConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM) - tc := arduino.PWMThrottleConfig{Min: throttleMinPWM, Max: throttleMaxPWM, Zero: throttleZeroPWM} - a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency, sc, tc) + sc := arduino.NewAsymetricPWMConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM) + secondarySc := arduino.NewAsymetricPWMConfig(secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM) + tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM) + secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM) + + a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, + pubFrequency, + arduino.WithThrottleConfig(tc), + arduino.WithSteeringConfig(sc), + arduino.WithSecondaryRC(secondaryTc, secondarySc), + ) cli.HandleExit(a) diff --git a/pkg/arduino/arduino.go b/pkg/arduino/arduino.go index 2c2b610..4d12768 100644 --- a/pkg/arduino/arduino.go +++ b/pkg/arduino/arduino.go @@ -21,7 +21,12 @@ const ( ) var ( - serialLineRegex = regexp.MustCompile(`(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+)`) + serialLineRegex = regexp.MustCompile(`(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+)`) + DefaultPwmThrottle = PWMConfig{ + Min: MinPwmThrottle, + Max: MaxPwmThrottle, + Middle: MinPwmThrottle + (MaxPwmThrottle-MinPwmThrottle)/2, + } ) type Part struct { @@ -30,50 +35,68 @@ type Part struct { pubFrequency float64 serial io.Reader mutex sync.Mutex - steering float32 - throttle float32 + steering, secondarySteering float32 + throttle, secondaryThrottle float32 ctrlRecord bool driveMode events.DriveMode cancel chan interface{} - pwmSteeringConfig PWMSteeringConfig - pwmThrottleConfig PWMThrottleConfig + useSecondaryRc bool + pwmSteeringConfig *PWMConfig + pwmSecondarySteeringConfig *PWMConfig + pwmThrottleConfig *PWMConfig + pwmSecondaryThrottleConfig *PWMConfig } -type PWMThrottleConfig struct { - Min int - Max int - Zero int +type PWMConfig struct { + Min int + Max int + Middle int } -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 NewPWMConfig(min, max int) *PWMConfig { + return &PWMConfig{ + Min: min, + Max: max, + Middle: min + (max-min)/2, } } -func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig { - c := NewPWMSteeringConfig(min, max) - c.Center = middle +func NewAsymetricPWMConfig(min, max, middle int) *PWMConfig { + c := NewPWMConfig(min, max) + c.Middle = middle return c } +type Option func(p *Part) + +func WithThrottleConfig(throttleConfig *PWMConfig) Option { + return func(p *Part) { + p.pwmThrottleConfig = throttleConfig + } +} + +func WithSteeringConfig(steeringConfig *PWMConfig) Option { + return func(p *Part) { + p.pwmSteeringConfig = steeringConfig + } +} + +func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option { + return func(p *Part) { + p.pwmSecondaryThrottleConfig = throttleConfig + p.pwmSecondarySteeringConfig = steeringConfig + } +} + func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, - switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig, throttleConfig PWMThrottleConfig) *Part { + switchRecordTopic string, pubFrequency float64, options ...Option) *Part { c := &serial.Config{Name: name, Baud: baud} s, err := serial.OpenPort(c) if err != nil { zap.S().Panicw("unable to open serial port: %v", err) } - return &Part{ + p := &Part{ client: client, serial: s, throttleTopic: throttleTopic, @@ -84,9 +107,16 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT driveMode: events.DriveMode_INVALID, cancel: make(chan interface{}), - pwmSteeringConfig: steeringConfig, - pwmThrottleConfig: throttleConfig, + pwmSteeringConfig: &DefaultPwmThrottle, + pwmSecondarySteeringConfig: &DefaultPwmThrottle, + pwmThrottleConfig: &DefaultPwmThrottle, + pwmSecondaryThrottleConfig: &DefaultPwmThrottle, } + + for _, o := range options { + o(p) + } + return p } func (a *Part) Start() error { @@ -121,6 +151,8 @@ func (a *Part) updateValues(values []string) { a.processChannel4(values[4]) a.processChannel5(values[5]) a.processChannel6(values[6]) + a.processChannel7(values[7]) + a.processChannel8(values[8]) } func (a *Part) Stop() { @@ -140,23 +172,23 @@ func (a *Part) processChannel1(v string) { if err != nil { zap.S().Errorf("invalid steering value for channel1, should be an int: %v", err) } - a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center) + a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig) } -func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 { - if value < minPwm { - value = minPwm - } else if value > maxPwm { - value = maxPwm +func convertPwmSteeringToPercent(value int, c *PWMConfig) float32 { + if value < c.Min { + value = c.Min + } else if value > c.Max { + value = c.Max } - if value == middlePwm { + if value == c.Middle { return 0. } - if value < middlePwm { - return (float32(value) - float32(middlePwm)) / float32(middlePwm-minPwm) + if value < c.Middle { + return (float32(value) - float32(c.Middle)) / float32(c.Middle-c.Min) } // middle < value < max - return (float32(value) - float32(middlePwm)) / float32(maxPwm-middlePwm) + return (float32(value) - float32(c.Middle)) / float32(c.Max-c.Middle) } func (a *Part) processChannel2(v string) { @@ -172,11 +204,11 @@ func (a *Part) processChannel2(v string) { } 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.Middle { + throttle = (float64(value) - float64(a.pwmThrottleConfig.Middle)) / float64(a.pwmThrottleConfig.Max-a.pwmThrottleConfig.Middle) } - if value < a.pwmThrottleConfig.Zero { - throttle = -1. * (float64(a.pwmThrottleConfig.Zero) - float64(value)) / (float64(a.pwmThrottleConfig.Zero - a.pwmThrottleConfig.Min)) + if value < a.pwmThrottleConfig.Middle { + throttle = -1. * (float64(a.pwmThrottleConfig.Middle) - float64(value)) / (float64(a.pwmThrottleConfig.Middle - a.pwmThrottleConfig.Min)) } a.throttle = float32(throttle) @@ -184,6 +216,11 @@ func (a *Part) processChannel2(v string) { func (a *Part) processChannel3(v string) { zap.L().Debug("process new value for channel3", zap.String("value", v)) + value, err := strconv.Atoi(v) + if err != nil { + zap.S().Errorf("invalid throttle value for channel2, should be an int: %v", err) + } + a.useSecondaryRc = value > 1900 } func (a *Part) processChannel4(v string) { @@ -231,6 +268,59 @@ func (a *Part) processChannel6(v string) { } } +func (a *Part) processChannel7(v string) { + zap.L().Debug("process new value for secondary steering on channel7", zap.String("value", v)) + value, err := strconv.Atoi(v) + if err != nil { + zap.S().Errorf("invalid steering value for channel7, should be an int: %v", err) + } + a.secondarySteering = convertPwmSteeringToPercent(value, a.pwmSecondarySteeringConfig) +} + +func (a *Part) processChannel8(v string) { + zap.L().Debug("process new throttle value on channel8", zap.String("value", v)) + value, err := strconv.Atoi(v) + if err != nil { + zap.S().Errorf("invalid throttle value for channel8, should be an int: %v", err) + } + if value < a.pwmSecondaryThrottleConfig.Min { + value = a.pwmSecondaryThrottleConfig.Min + } else if value > a.pwmSecondaryThrottleConfig.Max { + value = a.pwmSecondaryThrottleConfig.Max + } + a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0 +} + +func (a *Part) Throttle() float32 { + a.mutex.Lock() + defer a.mutex.Unlock() + if a.useSecondaryRc { + return a.secondaryThrottle + } + return a.throttle +} + +func (a *Part) Steering() float32 { + a.mutex.Lock() + defer a.mutex.Unlock() + if a.useSecondaryRc { + return a.secondarySteering + } + return a.steering +} + +func (a *Part) DriveMode() events.DriveMode { + a.mutex.Lock() + defer a.mutex.Unlock() + return a.driveMode +} + +func (a *Part) SwitchRecord() bool { + a.mutex.Lock() + defer a.mutex.Unlock() + return a.ctrlRecord +} + func (a *Part) publishLoop() { ticker := time.NewTicker(time.Second / time.Duration(int(a.pubFrequency))) @@ -246,9 +336,6 @@ func (a *Part) publishLoop() { } func (a *Part) publishValues() { - a.mutex.Lock() - defer a.mutex.Unlock() - a.publishThrottle() a.publishSteering() a.publishDriveMode() @@ -257,7 +344,7 @@ func (a *Part) publishValues() { func (a *Part) publishThrottle() { throttle := events.ThrottleMessage{ - Throttle: a.throttle, + Throttle: a.Throttle(), Confidence: 1.0, } throttleMessage, err := proto.Marshal(&throttle) @@ -271,7 +358,7 @@ func (a *Part) publishThrottle() { func (a *Part) publishSteering() { steering := events.SteeringMessage{ - Steering: a.steering, + Steering: a.Steering(), Confidence: 1.0, } steeringMessage, err := proto.Marshal(&steering) @@ -285,7 +372,7 @@ func (a *Part) publishSteering() { func (a *Part) publishDriveMode() { dm := events.DriveModeMessage{ - DriveMode: a.driveMode, + DriveMode: a.DriveMode(), } driveModeMessage, err := proto.Marshal(&dm) if err != nil { @@ -297,7 +384,7 @@ func (a *Part) publishDriveMode() { func (a *Part) publishSwitchRecord() { sr := events.SwitchRecordMessage{ - Enabled: !a.ctrlRecord, + Enabled: !a.SwitchRecord(), } switchRecordMessage, err := proto.Marshal(&sr) if err != nil { diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index cee747c..ba9c1c8 100644 --- a/pkg/arduino/arduino_test.go +++ b/pkg/arduino/arduino_test.go @@ -58,11 +58,9 @@ func TestArduinoPart_Update(t *testing.T) { } }() - defaultPwmThrottleConfig := PWMThrottleConfig{MinPwmThrottle, MaxPwmThrottle, MinPwmThrottle + (MaxPwmThrottle-MinPwmAngle)/int(2)} - a := Part{client: nil, serial: conn, pubFrequency: 100, - pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), - } - + defaultPwmThrottleConfig := NewPWMConfig(MinPwmThrottle, MaxPwmThrottle) + a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), + pwmThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)} go func() { err := a.Start() if err != nil { @@ -71,10 +69,10 @@ func TestArduinoPart_Update(t *testing.T) { } }() - channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1112, 1678, 1910, 112, 0, 0 + channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1012, 1678, 1910, 112, 0, 0 cases := []struct { name, content string - throttlePwmConfig PWMThrottleConfig + throttlePwmConfig *PWMConfig expectedThrottle, expectedSteering float32 expectedDriveMode events.DriveMode expectedSwitchRecord bool @@ -138,7 +136,7 @@ func TestArduinoPart_Update(t *testing.T) { defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false}, {"Throttle: stop", fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8), - defaultPwmThrottleConfig, 0.0, -1., events.DriveMode_USER, false}, + NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false}, {"Throttle: up", fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8), defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false}, @@ -147,8 +145,11 @@ func TestArduinoPart_Update(t *testing.T) { defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false}, {"Throttle: zero not middle", fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8), - PWMThrottleConfig{1000, 1700, 1500}, + &PWMConfig{1000, 1700, 1500}, 0.5, -1., events.DriveMode_USER, false}, + {"Use 2nd rc: use channels 7 and 8", + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008), + defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false}, } for _, c := range cases { @@ -168,17 +169,17 @@ func TestArduinoPart_Update(t *testing.T) { time.Sleep(10 * time.Millisecond) a.mutex.Lock() a.mutex.Unlock() - if fmt.Sprintf("%0.2f", a.throttle) != fmt.Sprintf("%0.2f", c.expectedThrottle) { - t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.throttle) + if fmt.Sprintf("%0.2f", a.Throttle()) != fmt.Sprintf("%0.2f", c.expectedThrottle) { + t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.Throttle()) } - if fmt.Sprintf("%0.2f", a.steering) != fmt.Sprintf("%0.2f", c.expectedSteering) { - t.Errorf("%s: bad steering value, expected: %0.2f, actual: %.2f", c.name, c.expectedSteering, a.steering) + if fmt.Sprintf("%0.2f", a.Steering()) != fmt.Sprintf("%0.2f", c.expectedSteering) { + t.Errorf("%s: bad steering value, expected: %0.2f, actual: %.2f", c.name, c.expectedSteering, a.Steering()) } - if a.driveMode != c.expectedDriveMode { - t.Errorf("%s: bad drive mode, expected: %v, actual:%v", c.name, c.expectedDriveMode, a.driveMode) + if a.DriveMode() != c.expectedDriveMode { + t.Errorf("%s: bad drive mode, expected: %v, actual:%v", c.name, c.expectedDriveMode, a.DriveMode()) } - if a.ctrlRecord != c.expectedSwitchRecord { - t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.ctrlRecord) + if a.SwitchRecord() != c.expectedSwitchRecord { + t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.SwitchRecord()) } }) } @@ -311,10 +312,8 @@ func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) { func Test_convertPwmSteeringToPercent(t *testing.T) { type args struct { - value int - middle int - min int - max int + value int + steeringConfig *PWMConfig } tests := []struct { name string @@ -324,127 +323,151 @@ func Test_convertPwmSteeringToPercent(t *testing.T) { { name: "middle", args: args{ - value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle, - middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle, - min: MinPwmAngle, - max: MaxPwmAngle, + value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle, + steeringConfig: &PWMConfig{ + 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, + value: MinPwmAngle, + steeringConfig: &PWMConfig{ + 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, + value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle - (MaxPwmAngle-MinPwmAngle)/4)), + steeringConfig: &PWMConfig{ + 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, + value: MinPwmAngle - 100, + steeringConfig: &PWMConfig{ + 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, + value: MaxPwmAngle, + steeringConfig: &PWMConfig{ + 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, + value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + (MaxPwmAngle-MinPwmAngle)/4)), + steeringConfig: &PWMConfig{ + 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, + value: MaxPwmAngle + 100, + steeringConfig: &PWMConfig{ + 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, + value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100, + steeringConfig: &PWMConfig{ + 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, + value: int(math.Round(((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100-MinPwmAngle)/2) + MinPwmAngle), + steeringConfig: &PWMConfig{ + 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, + value: MinPwmAngle, + steeringConfig: &PWMConfig{ + 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, + value: int(math.Round((MaxPwmAngle - (MaxPwmAngle-((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100))/2))), + steeringConfig: &PWMConfig{ + 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, + value: MaxPwmAngle, + steeringConfig: &PWMConfig{ + 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 { + if got := convertPwmSteeringToPercent(tt.args.value, tt.args.steeringConfig); got != tt.want { t.Errorf("convertPwmSteeringToPercent() = %v, want %v", got, tt.want) } })