Compare commits
No commits in common. "973ebb9df9e10ec5e0ad32a581f8355549ab6e93" and "9c914d28e798568f094b1465d3e1a2d955346688" have entirely different histories.
973ebb9df9
...
9c914d28e7
@ -15,10 +15,6 @@ const (
|
||||
|
||||
SteeringLeftPWM = 1004
|
||||
SteeringRightPWM = 1986
|
||||
|
||||
ThrottleZeroPWM = 1260
|
||||
ThrottleMinPWM = 972.0
|
||||
ThrottleMaxPWM = 1954.0
|
||||
)
|
||||
|
||||
var (
|
||||
@ -38,7 +34,6 @@ 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)
|
||||
}
|
||||
@ -48,15 +43,6 @@ 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")
|
||||
@ -68,9 +54,6 @@ 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()
|
||||
@ -100,8 +83,7 @@ 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)
|
||||
a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, pubFrequency, sc)
|
||||
|
||||
cli.HandleExit(a)
|
||||
|
||||
|
||||
@ -37,13 +37,6 @@ type Part struct {
|
||||
cancel chan interface{}
|
||||
|
||||
pwmSteeringConfig PWMSteeringConfig
|
||||
pwmThrottleConfig PWMThrottleConfig
|
||||
}
|
||||
|
||||
type PWMThrottleConfig struct {
|
||||
Min int
|
||||
Max int
|
||||
Zero int
|
||||
}
|
||||
|
||||
type PWMSteeringConfig struct {
|
||||
@ -59,7 +52,6 @@ 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
|
||||
@ -67,7 +59,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, throttleConfig PWMThrottleConfig) *Part {
|
||||
switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig) *Part {
|
||||
c := &serial.Config{Name: name, Baud: baud}
|
||||
s, err := serial.OpenPort(c)
|
||||
if err != nil {
|
||||
@ -85,7 +77,6 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
|
||||
cancel: make(chan interface{}),
|
||||
|
||||
pwmSteeringConfig: steeringConfig,
|
||||
pwmThrottleConfig: throttleConfig,
|
||||
}
|
||||
}
|
||||
|
||||
@ -165,21 +156,12 @@ 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 < a.pwmThrottleConfig.Min {
|
||||
value = a.pwmThrottleConfig.Min
|
||||
} else if value > a.pwmThrottleConfig.Max {
|
||||
value = a.pwmThrottleConfig.Max
|
||||
if value < MinPwmThrottle {
|
||||
value = MinPwmThrottle
|
||||
} else if value > MaxPwmThrottle {
|
||||
value = MaxPwmThrottle
|
||||
}
|
||||
|
||||
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)
|
||||
a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
|
||||
}
|
||||
|
||||
func (a *Part) processChannel3(v string) {
|
||||
@ -266,7 +248,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() {
|
||||
@ -280,7 +262,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() {
|
||||
@ -292,7 +274,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() {
|
||||
@ -304,9 +286,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)
|
||||
}
|
||||
|
||||
@ -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,11 +58,7 @@ 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),
|
||||
}
|
||||
|
||||
a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle)}
|
||||
go func() {
|
||||
err := a.Start()
|
||||
if err != nil {
|
||||
@ -74,93 +70,76 @@ 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),
|
||||
defaultPwmThrottleConfig, -1., -1.,
|
||||
events.DriveMode_USER, false},
|
||||
-1., -1., events.DriveMode_USER, false},
|
||||
{"Unparsable line",
|
||||
"12350,invalid line\n", defaultPwmThrottleConfig,
|
||||
"12350,invalid line\n",
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 998, channel6, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1987, channel6, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1850, channel6, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1987, distanceCm),
|
||||
-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), defaultPwmThrottleConfig,
|
||||
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1850, distanceCm),
|
||||
-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.0, -1., events.DriveMode_USER, false},
|
||||
-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},
|
||||
{"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 {
|
||||
@ -174,8 +153,6 @@ 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) {
|
||||
@ -200,10 +177,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")
|
||||
|
||||
Loading…
Reference in New Issue
Block a user