robocars/robocar-arduino
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat: publish pwm feedback

Browse Source
This commit is contained in:
Cyrille Nofficial
2022-09-02 11:44:14 +02:00
parent 14d5c605e7
commit e788abd5a9
7 changed files with 423 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
pkg/arduino/arduino.go
View File

@ -2,6 +2,7 @@ package arduino
import (
"bufio"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"github.com/tarm/serial"
@ -21,7 +22,7 @@ const (
)
var (
serialLineRegex = regexp.MustCompile(`(?P<timestamp>\d+),(?P<channel_1>\d+),(?P<channel_2>\d+),(?P<channel_3>\d+),(?P<channel_4>\d+),(?P<channel_5>\d+),(?P<channel_6>-?\d+),(?P<channel_7>\d+),(?P<channel_8>\d+),(?P<frequency>\d+)`)
serialLineRegex = regexp.MustCompile(`(?P<timestamp>\d+),(?P<channel_1>\d+),(?P<channel_2>\d+),(?P<channel_3>\d+),(?P<channel_4>\d+),(?P<channel_5>\d+),(?P<channel_6>-?\d+),(?P<channel_7>\d+),(?P<channel_8>\d+),(?P<channel_9>\d+),(?P<frequency>\d+)`)
DefaultPwmThrottle = PWMConfig{
Min: MinPwmThrottle,
Max: MaxPwmThrottle,
@ -30,22 +31,25 @@ var (
)
type Part struct {
client mqtt.Client
throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering, secondarySteering float32
throttle, secondaryThrottle float32
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
client mqtt.Client
throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering, secondarySteering float32
throttle, secondaryThrottle float32
throttleFeedback float32
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
useSecondaryRc bool
pwmSteeringConfig *PWMConfig
pwmSecondarySteeringConfig *PWMConfig
pwmThrottleConfig *PWMConfig
pwmSecondaryThrottleConfig *PWMConfig
throttleFeedbackThresholds *tools.ThresholdConfig
}
type PWMConfig struct {
@ -89,28 +93,46 @@ func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option {
}
}
func WithThrottleFeedbackConfig(filename string) Option {
return func(p *Part) {
if filename == "" {
p.throttleFeedbackThresholds = tools.NewThresholdConfig()
return
}
tc, err := tools.NewThresholdConfigFromJson(filename)
if err != nil {
zap.S().Panicf("unable to load ThresholdConfig from file %v: %v", filename, err)
}
p.throttleFeedbackThresholds = tc
}
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic,
switchRecordTopic string, pubFrequency float64, options ...Option) *Part {
switchRecordTopic, throttleFeedbackTopic 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)
}
p := &Part{
client: client,
serial: s,
throttleTopic: throttleTopic,
steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic,
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
client: client,
serial: s,
throttleTopic: throttleTopic,
steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic,
throttleFeedbackTopic: throttleFeedbackTopic,
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
pwmSteeringConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: &DefaultPwmThrottle,
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
throttleFeedbackThresholds: tools.NewThresholdConfig(),
}
for _, o := range options {
@ -153,6 +175,7 @@ func (a *Part) updateValues(values []string) {
a.processChannel6(values[6])
a.processChannel7(values[7])
a.processChannel8(values[8])
a.processChannel9(values[9])
}
func (a *Part) Stop() {
@ -225,6 +248,11 @@ func (a *Part) processChannel3(v string) {
func (a *Part) processChannel4(v string) {
zap.L().Debug("process new value for channel4", 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.throttleFeedback = a.convertPwmFeedBackToPercent(value)
}
func (a *Part) processChannel5(v string) {
@ -295,6 +323,10 @@ func (a *Part) processChannel8(v string) {
a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0
}
func (a *Part) processChannel9(v string) {
zap.L().Debug("process new value for channel9", zap.String("value", v))
}
func (a *Part) Throttle() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
@ -304,6 +336,12 @@ func (a *Part) Throttle() float32 {
return a.throttle
}
func (a *Part) ThrottleFeedback() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
return a.throttleFeedback
}
func (a *Part) Steering() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
@ -341,6 +379,7 @@ func (a *Part) publishLoop() {
func (a *Part) publishValues() {
a.publishThrottle()
a.publishThrottleFeedback()
a.publishSteering()
a.publishDriveMode()
a.publishSwitchRecord()
@ -374,6 +413,19 @@ func (a *Part) publishSteering() {
publish(a.client, a.steeringTopic, steeringMessage)
}
func (a *Part) publishThrottleFeedback() {
tm := events.ThrottleMessage{
Throttle: a.ThrottleFeedback(),
Confidence: 1.,
}
tfMessage, err := proto.Marshal(&tm)
if err != nil {
zap.S().Errorf("unable to marshal protobuf throttleFeedback message: %v", err)
return
}
publish(a.client, a.throttleFeedbackTopic, tfMessage)
}
func (a *Part) publishDriveMode() {
dm := events.DriveModeMessage{
DriveMode: a.DriveMode(),
@ -398,6 +450,10 @@ func (a *Part) publishSwitchRecord() {
publish(a.client, a.switchRecordTopic, switchRecordMessage)
}
func (a *Part) convertPwmFeedBackToPercent(value int) float32 {
return float32(a.throttleFeedbackThresholds.ValueOf(value))
}
var publish = func(client mqtt.Client, topic string, payload []byte) {
client.Publish(topic, 0, false, payload)
}

187
pkg/arduino/arduino_test.go
View File

@ -3,6 +3,7 @@ package arduino
import (
"bufio"
"fmt"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
@ -59,8 +60,13 @@ func TestArduinoPart_Update(t *testing.T) {
}()
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)}
a := Part{client: nil, serial: conn, pubFrequency: 100,
pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle),
throttleFeedbackThresholds: tools.NewThresholdConfig(),
}
go func() {
err := a.Start()
if err != nil {
@ -69,7 +75,7 @@ func TestArduinoPart_Update(t *testing.T) {
}
}()
channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1012, 1678, 1910, 112, 0, 0
channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9 := 678, 910, 1012, 1678, 1910, 112, 0, 0, 0
cases := []struct {
name, content string
throttlePwmConfig *PWMConfig
@ -78,86 +84,86 @@ func TestArduinoPart_Update(t *testing.T) {
expectedSwitchRecord bool
}{
{"Good value",
fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1.,
events.DriveMode_USER, false},
{"Invalid line",
"12350,invalid line\n", defaultPwmThrottleConfig,
-1., -1., events.DriveMode_INVALID, false},
{"Switch record on",
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8),
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
{"Switch record off",
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8),
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Switch record off",
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8),
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Switch record on",
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8),
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
{"DriveMode: user",
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8),
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"DriveMode: pilot",
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8),
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
{"DriveMode: pilot",
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8),
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
// DriveMode: user
{"DriveMode: user",
fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8),
fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
{"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Sterring: left",
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -0.92, events.DriveMode_USER, false},
{"Sterring: middle",
fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false},
{"Sterring: right",
fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false},
{"Sterring: over right",
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false},
{"Throttle: over down",
fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Throttle: down",
fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
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),
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
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),
fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false},
{"Throttle: over up",
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8),
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
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),
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
&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),
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008, channel9),
defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false},
{"Drive Mode: user",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8),
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Drive Mode: pilot",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8),
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
{"Drive Mode: no value",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8),
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false},
}
@ -227,44 +233,50 @@ func TestPublish(t *testing.T) {
pubFrequency := 100.
a := Part{
client: nil,
serial: conn,
pubFrequency: pubFrequency,
throttleTopic: "car/part/arduino/throttle",
steeringTopic: "car/part/arduino/steering",
driveModeTopic: "car/part/arduino/drive_mode",
switchRecordTopic: "car/part/arduino/switch_record",
cancel: make(chan interface{}),
client: nil,
serial: conn,
pubFrequency: pubFrequency,
throttleTopic: "car/part/arduino/throttle/target",
steeringTopic: "car/part/arduino/steering",
driveModeTopic: "car/part/arduino/drive_mode",
switchRecordTopic: "car/part/arduino/switch_record",
throttleFeedbackTopic: "car/part/arduino/throttle/feedback",
cancel: make(chan interface{}),
}
go a.Start()
defer a.Stop()
cases := []struct {
throttle, steering float32
driveMode events.DriveMode
switchRecord bool
expectedThrottle events.ThrottleMessage
expectedSteering events.SteeringMessage
expectedDriveMode events.DriveModeMessage
expectedSwitchRecord events.SwitchRecordMessage
throttle, steering float32
driveMode events.DriveMode
throttleFeedback float32
switchRecord bool
expectedThrottle events.ThrottleMessage
expectedSteering events.SteeringMessage
expectedDriveMode events.DriveModeMessage
expectedSwitchRecord events.SwitchRecordMessage
expectedThrottleFeedback events.ThrottleMessage
}{
{-1, 1, events.DriveMode_USER, true,
{-1, 1, events.DriveMode_USER, 0.3, true,
events.ThrottleMessage{Throttle: -1., Confidence: 1.},
events.SteeringMessage{Steering: 1.0, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SwitchRecordMessage{Enabled: false},
events.ThrottleMessage{Throttle: 0.3, Confidence: 1.},
},
{0, 0, events.DriveMode_PILOT, false,
{0, 0, events.DriveMode_PILOT, 0.4, false,
events.ThrottleMessage{Throttle: 0., Confidence: 1.},
events.SteeringMessage{Steering: 0., Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true},
events.ThrottleMessage{Throttle: 0.4, Confidence: 1.},
},
{0.87, -0.58, events.DriveMode_PILOT, false,
{0.87, -0.58, events.DriveMode_PILOT, 0.5, false,
events.ThrottleMessage{Throttle: 0.87, Confidence: 1.},
events.SteeringMessage{Steering: -0.58, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true},
events.ThrottleMessage{Throttle: 0.5, Confidence: 1.},
},
}
@ -274,17 +286,17 @@ func TestPublish(t *testing.T) {
a.steering = c.steering
a.driveMode = c.driveMode
a.ctrlRecord = c.switchRecord
a.throttleFeedback = c.throttleFeedback
a.mutex.Unlock()
time.Sleep(time.Second / time.Duration(int(pubFrequency)))
time.Sleep(500 * time.Millisecond)
time.Sleep(time.Second / time.Duration(int(pubFrequency)) * 2)
var throttleMsg events.ThrottleMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle"], &throttleMsg)
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/target"], &throttleMsg)
muPublishedEvents.Unlock()
if throttleMsg.String() != c.expectedThrottle.String() {
t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", throttleMsg, c.expectedThrottle)
t.Errorf("msg(car/part/arduino/throttle/target): %v, wants %v", throttleMsg.String(), c.expectedThrottle.String())
}
var steeringMsg events.SteeringMessage
@ -292,7 +304,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg)
muPublishedEvents.Unlock()
if steeringMsg.String() != c.expectedSteering.String() {
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg, c.expectedSteering)
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg.String(), c.expectedSteering.String())
}
var driveModeMsg events.DriveModeMessage
@ -300,7 +312,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg)
muPublishedEvents.Unlock()
if driveModeMsg.String() != c.expectedDriveMode.String() {
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg, c.expectedDriveMode)
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg.String(), c.expectedDriveMode.String())
}
var switchRecordMsg events.SwitchRecordMessage
@ -308,7 +320,15 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg)
muPublishedEvents.Unlock()
if switchRecordMsg.String() != c.expectedSwitchRecord.String() {
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg, c.expectedSwitchRecord)
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg.String(), c.expectedSwitchRecord.String())
}
var throttleFeedbackMsg events.ThrottleMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/feedback"], &throttleFeedbackMsg)
muPublishedEvents.Unlock()
if throttleFeedbackMsg.String() != c.expectedThrottleFeedback.String() {
t.Errorf("msg(car/part/arduino/throttle/feedback): %v, wants %v", throttleFeedbackMsg.String(), c.expectedThrottleFeedback.String())
}
}
}
@ -483,3 +503,66 @@ func Test_convertPwmSteeringToPercent(t *testing.T) {
})
}
}
func TestPart_convertPwmFeedBackToPercent(t *testing.T) {
type fields struct {
}
type args struct {
value int
}
tests := []struct {
name string
fields fields
args args
want float32
}{
{
name: "big value -> 0%",
args: args{value: 1234567},
want: 0.,
},
{
name: "very slow",
args: args{10000},
want: 0.,
},
{
name: "0.07 limit",
args: args{8700},
want: 0.07,
},
{
name: "0.075",
args: args{value: 8700 - (8700-4800)/2},
want: 0.075,
},
{
name: "0.08",
args: args{value: 4800},
want: 0.08,
},
{
name: "1.0",
args: args{value: 548},
want: 1.,
},
{
name: "under lower limit",
args: args{value: 520},
want: 1.,
},
{
name: "invalid value",
args: args{value: 499},
want: 0.,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
a := &Part{throttleFeedbackThresholds: tools.NewThresholdConfig()}
if got := a.convertPwmFeedBackToPercent(tt.args.value); got != tt.want {
t.Errorf("convertPwmFeedBackToPercent() = %v, want %v", got, tt.want)
}
})
}
}