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

refactor(pwm steering): asymetric center

Browse Source
This commit is contained in:
Cyrille Nofficial
2022-01-17 18:40:25 +01:00
parent 7360b2bfa4
commit fcfc60ea38
3 changed files with 225 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
pkg/arduino/arduino.go
View File

@ -16,9 +16,6 @@ import (
)
const (
MinPwmAngle = 999.0
MaxPwmAngle = 1985.0
MinPwmThrottle = 972.0
MaxPwmThrottle = 1954.0
)
@ -38,9 +35,31 @@ type Part struct {
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
pwmSteeringConfig PWMSteeringConfig
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string, pubFrequency float64) *Part {
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 NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
c := NewPWMSteeringConfig(min, max)
c.Center = middle
return c
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic,
switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig) *Part {
c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c)
if err != nil {
@ -56,6 +75,8 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
pwmSteeringConfig: steeringConfig,
}
}
@ -109,12 +130,23 @@ func (a *Part) processChannel1(v string) {
if err != nil {
zap.S().Errorf("invalid value for channel1, should be an int: %v", err)
}
if value < MinPwmAngle {
value = MinPwmAngle
} else if value > MaxPwmAngle {
value = MaxPwmAngle
a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center)
}
func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 {
if value < minPwm {
value = minPwm
} else if value > maxPwm {
value = maxPwm
}
a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
if value == middlePwm {
return 0.
}
if value < middlePwm {
return (float32(value) - float32(middlePwm)) / float32(middlePwm-minPwm)
}
// middle < value < max
return (float32(value) - float32(middlePwm)) / float32(maxPwm-middlePwm)
}
func (a *Part) processChannel2(v string) {

162
pkg/arduino/arduino_test.go
View File

@ -6,12 +6,22 @@ import (
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
"math"
"net"
"sync"
"testing"
"time"
)
const (
MinPwmAngle = 999.0
MaxPwmAngle = 1985.0
)
var (
MiddlePwmAngle = int((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle)
)
func TestArduinoPart_Update(t *testing.T) {
oldPublish := publish
defer func() { publish = oldPublish }()
@ -48,7 +58,7 @@ func TestArduinoPart_Update(t *testing.T) {
}
}()
a := Part{client: nil, serial: conn, pubFrequency: 100}
a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle)}
go func() {
err := a.Start()
if err != nil {
@ -103,14 +113,14 @@ func TestArduinoPart_Update(t *testing.T) {
fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,50,%d\n", 99, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -1., events.DriveMode_USER, false},
{"Sterring: left",
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", 998, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.93, events.DriveMode_USER, false},
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, distanceCm),
-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),
-1., -0.04, events.DriveMode_USER, false},
-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),
-1., 0.96, events.DriveMode_USER, false},
-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),
-1., 1., events.DriveMode_USER, false},
@ -285,3 +295,145 @@ func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) {
t.Errorf("unable to unmarshal protobuf content to %T: %v", msg, err)
}
}
func Test_convertPwmSteeringToPercent(t *testing.T) {
type args struct {
value int
middle int
min int
max int
}
tests := []struct {
name string
args args
want float32
}{
{
name: "middle",
args: args{
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
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,
},
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,
},
want: -0.4989858,
},
{
name: "over left",
args: args{
value: MinPwmAngle - 100,
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,
},
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,
},
want: 0.5010142,
},
{
name: "over right",
args: args{
value: MaxPwmAngle + 100,
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,
},
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,
},
want: -0.49915683,
},
{
name: "asymetric left",
args: args{
value: MinPwmAngle,
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,
},
want: 0.50127226,
},
{
name: "asymetric right",
args: args{
value: MaxPwmAngle,
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 {
t.Errorf("convertPwmSteeringToPercent() = %v, want %v", got, tt.want)
}
})
}
}