diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index 9256da6..b306d80 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/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) diff --git a/pkg/arduino/arduino.go b/pkg/arduino/arduino.go index 83b6de5..a9a72ea 100644 --- a/pkg/arduino/arduino.go +++ b/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,10 +165,10 @@ 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 { - value = MinPwmThrottle - } else if value > MaxPwmThrottle { - value = MaxPwmThrottle + if value < a.pwmThrottleConfig.Min { + value = a.pwmThrottleConfig.Min + } else if value > a.pwmThrottleConfig.Max { + value = a.pwmThrottleConfig.Max } a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0 } @@ -248,7 +257,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 +271,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 +283,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 +295,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) { - client.Publish(topic, 0, false, *payload) +var publish = func(client mqtt.Client, topic string, payload []byte) { + client.Publish(topic, 0, false, payload) } diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index 269e1e2..24fb546 100644 --- a/pkg/arduino/arduino_test.go +++ b/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,75 +74,88 @@ 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), + defaultPwmThrottleConfig, -0.03, -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}, } @@ -152,6 +169,8 @@ func TestArduinoPart_Update(t *testing.T) { if err != nil { t.Error("unable to flush content") } + + a.pwmThrottleConfig = c.throttlePwmConfig time.Sleep(10 * time.Millisecond) a.mutex.Lock() @@ -177,10 +196,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")