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

feat: publish pwm feedback

Browse Source
This commit is contained in:
cyrilix committed 2022-09-03 18:37:53 +02:00
1 parent 14d5c605e7
commit e788abd5a9
7 files changed
+423 -75
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

pkg/arduino/arduino_test.go
+135 -52
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)
}
})
}
}