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

feat: allow to control car with 2nd radio-command

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-08-10 16:49:58 +02:00
parent e64c7ff559
commit a07d042269
3 changed files with 267 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
cmd/rc-arduino/rc-arduino.go
View File

@ -38,7 +38,7 @@ func main() {
cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int
var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int
var secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM int
if err := cli.SetIntDefaultValueFromEnv(&steeringLeftPWM, "STEERING_LEFT_PWM", SteeringLeftPWM); err != nil {
zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err)
}
@ -46,7 +46,19 @@ func main() {
zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&steeringCenterPWM, "STEERING_CENTER_PWM", SteeringCenterPWM); err != nil {
zap.S().Warnf("unable to init steeringRightPWM arg: %v", err)
zap.S().Warnf("unable to init steeringCenterPWM arg: %v", err)
}
var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int
var secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleCenterPWM int
if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", arduino.DefaultPwmThrottle.Min); err != nil {
zap.S().Warnf("unable to init throttleMinPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&throttleMaxPWM, "THROTTLE_MAX_PWM", arduino.DefaultPwmThrottle.Max); err != nil {
zap.S().Warnf("unable to init throttleMaxPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&throttleZeroPWM, "THROTTLE_CENTER_PWM", arduino.DefaultPwmThrottle.Middle); err != nil {
zap.S().Warnf("unable to init throttleZeroPWM arg: %v", err)
}
if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", ThrottleMinPWM); err != nil {
zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err)
@ -65,12 +77,20 @@ func main() {
flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic where to publish switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set")
flag.StringVar(&device, "device", "/dev/serial0", "Serial device")
flag.IntVar(&baud, "baud", 115200, "Serial baud")
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")
flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set")
flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set")
flag.IntVar(&steeringCenterPWM, "steering-center-pwm", steeringCenterPWM, "middlePwm value for steering PWM, STEERING_CENTER_PWM env if args not set")
flag.IntVar(&secondarySteeringLeftPWM, "steering-secondary-left-pwm", steeringLeftPWM, "maxPwm left value for secondary radio controller steering PWM, STEERING_LEFT_PWM env if args not set")
flag.IntVar(&secondarySteeringRightPWM, "steering-secondary-right-pwm", steeringRightPWM, "maxPwm right value for secondary radio controller steering PWM, STEERING_RIGHT_PWM env if args not set")
flag.IntVar(&secondarySteeringCenterPWM, "steering-secondary-center-pwm", steeringCenterPWM, "middlePwm value for secondary radio controller steering PWM, STEERING_CENTER_PWM env if args not set")
flag.IntVar(&throttleMinPWM, "throttle-min-pwm", throttleMinPWM, "maxPwm min value for throttle PWM, THROTTLE_MIN_PWM env if args not set")
flag.IntVar(&throttleMaxPWM, "throttle-max-pwm", throttleMaxPWM, "maxPwm max value for throttle PWM, THROTTLE_MAX_PWM env if args not set")
flag.IntVar(&throttleZeroPWM, "throttle-center-pwm", throttleZeroPWM, "middlePwm value for throttle PWM, THROTTLE_CENTER_PWM env if args not set")
flag.IntVar(&secondaryThrottleMinPWM, "throttle-secondary-min-pwm", throttleMinPWM, "maxPwm min value for secondary radio controller throttle PWM, THROTTLE_MIN_PWM env if args not set")
flag.IntVar(&secondaryThrottleMaxPWM, "throttle-secondary-max-pwm", throttleMaxPWM, "maxPwm max value for secondary radio controller throttle PWM, THROTTLE_MAX_PWM env if args not set")
flag.IntVar(&secondaryThrottleCenterPWM, "throttle-secondary-center-pwm", throttleZeroPWM, "middlePwm value for secondary radio controller throttle PWM, THROTTLE_CENTER_PWM env if args not set")
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
flag.Parse()
@ -99,9 +119,17 @@ 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)
sc := arduino.NewAsymetricPWMConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM)
secondarySc := arduino.NewAsymetricPWMConfig(secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM)
tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM)
secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM)
a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic,
pubFrequency,
arduino.WithThrottleConfig(tc),
arduino.WithSteeringConfig(sc),
arduino.WithSecondaryRC(secondaryTc, secondarySc),
)
cli.HandleExit(a)

183
pkg/arduino/arduino.go
View File

@ -21,7 +21,12 @@ 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<frequency>\d+)`)
DefaultPwmThrottle = PWMConfig{
Min: MinPwmThrottle,
Max: MaxPwmThrottle,
Middle: MinPwmThrottle + (MaxPwmThrottle-MinPwmThrottle)/2,
}
)
type Part struct {
@ -30,50 +35,68 @@ type Part struct {
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering float32
throttle float32
steering, secondarySteering float32
throttle, secondaryThrottle float32
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
pwmSteeringConfig PWMSteeringConfig
pwmThrottleConfig PWMThrottleConfig
useSecondaryRc bool
pwmSteeringConfig *PWMConfig
pwmSecondarySteeringConfig *PWMConfig
pwmThrottleConfig *PWMConfig
pwmSecondaryThrottleConfig *PWMConfig
}
type PWMThrottleConfig struct {
Min int
Max int
Zero int
type PWMConfig struct {
Min int
Max int
Middle int
}
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 NewPWMConfig(min, max int) *PWMConfig {
return &PWMConfig{
Min: min,
Max: max,
Middle: min + (max-min)/2,
}
}
func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
c := NewPWMSteeringConfig(min, max)
c.Center = middle
func NewAsymetricPWMConfig(min, max, middle int) *PWMConfig {
c := NewPWMConfig(min, max)
c.Middle = middle
return c
}
type Option func(p *Part)
func WithThrottleConfig(throttleConfig *PWMConfig) Option {
return func(p *Part) {
p.pwmThrottleConfig = throttleConfig
}
}
func WithSteeringConfig(steeringConfig *PWMConfig) Option {
return func(p *Part) {
p.pwmSteeringConfig = steeringConfig
}
}
func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option {
return func(p *Part) {
p.pwmSecondaryThrottleConfig = throttleConfig
p.pwmSecondarySteeringConfig = steeringConfig
}
}
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, 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)
}
return &Part{
p := &Part{
client: client,
serial: s,
throttleTopic: throttleTopic,
@ -84,9 +107,16 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
pwmSteeringConfig: steeringConfig,
pwmThrottleConfig: throttleConfig,
pwmSteeringConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: &DefaultPwmThrottle,
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
}
for _, o := range options {
o(p)
}
return p
}
func (a *Part) Start() error {
@ -121,6 +151,8 @@ func (a *Part) updateValues(values []string) {
a.processChannel4(values[4])
a.processChannel5(values[5])
a.processChannel6(values[6])
a.processChannel7(values[7])
a.processChannel8(values[8])
}
func (a *Part) Stop() {
@ -140,23 +172,23 @@ func (a *Part) processChannel1(v string) {
if err != nil {
zap.S().Errorf("invalid steering value for channel1, should be an int: %v", err)
}
a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center)
a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig)
}
func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 {
if value < minPwm {
value = minPwm
} else if value > maxPwm {
value = maxPwm
func convertPwmSteeringToPercent(value int, c *PWMConfig) float32 {
if value < c.Min {
value = c.Min
} else if value > c.Max {
value = c.Max
}
if value == middlePwm {
if value == c.Middle {
return 0.
}
if value < middlePwm {
return (float32(value) - float32(middlePwm)) / float32(middlePwm-minPwm)
if value < c.Middle {
return (float32(value) - float32(c.Middle)) / float32(c.Middle-c.Min)
}
// middle < value < max
return (float32(value) - float32(middlePwm)) / float32(maxPwm-middlePwm)
return (float32(value) - float32(c.Middle)) / float32(c.Max-c.Middle)
}
func (a *Part) processChannel2(v string) {
@ -172,11 +204,11 @@ func (a *Part) processChannel2(v string) {
}
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.Middle {
throttle = (float64(value) - float64(a.pwmThrottleConfig.Middle)) / float64(a.pwmThrottleConfig.Max-a.pwmThrottleConfig.Middle)
}
if value < a.pwmThrottleConfig.Zero {
throttle = -1. * (float64(a.pwmThrottleConfig.Zero) - float64(value)) / (float64(a.pwmThrottleConfig.Zero - a.pwmThrottleConfig.Min))
if value < a.pwmThrottleConfig.Middle {
throttle = -1. * (float64(a.pwmThrottleConfig.Middle) - float64(value)) / (float64(a.pwmThrottleConfig.Middle - a.pwmThrottleConfig.Min))
}
a.throttle = float32(throttle)
@ -184,6 +216,11 @@ func (a *Part) processChannel2(v string) {
func (a *Part) processChannel3(v string) {
zap.L().Debug("process new value for channel3", 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.useSecondaryRc = value > 1900
}
func (a *Part) processChannel4(v string) {
@ -231,6 +268,59 @@ func (a *Part) processChannel6(v string) {
}
}
func (a *Part) processChannel7(v string) {
zap.L().Debug("process new value for secondary steering on channel7", zap.String("value", v))
value, err := strconv.Atoi(v)
if err != nil {
zap.S().Errorf("invalid steering value for channel7, should be an int: %v", err)
}
a.secondarySteering = convertPwmSteeringToPercent(value, a.pwmSecondarySteeringConfig)
}
func (a *Part) processChannel8(v string) {
zap.L().Debug("process new throttle value on channel8", zap.String("value", v))
value, err := strconv.Atoi(v)
if err != nil {
zap.S().Errorf("invalid throttle value for channel8, should be an int: %v", err)
}
if value < a.pwmSecondaryThrottleConfig.Min {
value = a.pwmSecondaryThrottleConfig.Min
} else if value > a.pwmSecondaryThrottleConfig.Max {
value = a.pwmSecondaryThrottleConfig.Max
}
a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0
}
func (a *Part) Throttle() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
if a.useSecondaryRc {
return a.secondaryThrottle
}
return a.throttle
}
func (a *Part) Steering() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
if a.useSecondaryRc {
return a.secondarySteering
}
return a.steering
}
func (a *Part) DriveMode() events.DriveMode {
a.mutex.Lock()
defer a.mutex.Unlock()
return a.driveMode
}
func (a *Part) SwitchRecord() bool {
a.mutex.Lock()
defer a.mutex.Unlock()
return a.ctrlRecord
}
func (a *Part) publishLoop() {
ticker := time.NewTicker(time.Second / time.Duration(int(a.pubFrequency)))
@ -246,9 +336,6 @@ func (a *Part) publishLoop() {
}
func (a *Part) publishValues() {
a.mutex.Lock()
defer a.mutex.Unlock()
a.publishThrottle()
a.publishSteering()
a.publishDriveMode()
@ -257,7 +344,7 @@ func (a *Part) publishValues() {
func (a *Part) publishThrottle() {
throttle := events.ThrottleMessage{
Throttle: a.throttle,
Throttle: a.Throttle(),
Confidence: 1.0,
}
throttleMessage, err := proto.Marshal(&throttle)
@ -271,7 +358,7 @@ func (a *Part) publishThrottle() {
func (a *Part) publishSteering() {
steering := events.SteeringMessage{
Steering: a.steering,
Steering: a.Steering(),
Confidence: 1.0,
}
steeringMessage, err := proto.Marshal(&steering)
@ -285,7 +372,7 @@ func (a *Part) publishSteering() {
func (a *Part) publishDriveMode() {
dm := events.DriveModeMessage{
DriveMode: a.driveMode,
DriveMode: a.DriveMode(),
}
driveModeMessage, err := proto.Marshal(&dm)
if err != nil {
@ -297,7 +384,7 @@ func (a *Part) publishDriveMode() {
func (a *Part) publishSwitchRecord() {
sr := events.SwitchRecordMessage{
Enabled: !a.ctrlRecord,
Enabled: !a.SwitchRecord(),
}
switchRecordMessage, err := proto.Marshal(&sr)
if err != nil {

163
pkg/arduino/arduino_test.go
View File

@ -58,11 +58,9 @@ 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),
}
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)}
go func() {
err := a.Start()
if err != nil {
@ -71,10 +69,10 @@ func TestArduinoPart_Update(t *testing.T) {
}
}()
channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1112, 1678, 1910, 112, 0, 0
channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1012, 1678, 1910, 112, 0, 0
cases := []struct {
name, content string
throttlePwmConfig PWMThrottleConfig
throttlePwmConfig *PWMConfig
expectedThrottle, expectedSteering float32
expectedDriveMode events.DriveMode
expectedSwitchRecord bool
@ -138,7 +136,7 @@ func TestArduinoPart_Update(t *testing.T) {
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),
defaultPwmThrottleConfig, 0.0, -1., events.DriveMode_USER, false},
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),
defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false},
@ -147,8 +145,11 @@ func TestArduinoPart_Update(t *testing.T) {
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),
PWMThrottleConfig{1000, 1700, 1500},
&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),
defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false},
}
for _, c := range cases {
@ -168,17 +169,17 @@ func TestArduinoPart_Update(t *testing.T) {
time.Sleep(10 * time.Millisecond)
a.mutex.Lock()
a.mutex.Unlock()
if fmt.Sprintf("%0.2f", a.throttle) != fmt.Sprintf("%0.2f", c.expectedThrottle) {
t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.throttle)
if fmt.Sprintf("%0.2f", a.Throttle()) != fmt.Sprintf("%0.2f", c.expectedThrottle) {
t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.Throttle())
}
if fmt.Sprintf("%0.2f", a.steering) != fmt.Sprintf("%0.2f", c.expectedSteering) {
t.Errorf("%s: bad steering value, expected: %0.2f, actual: %.2f", c.name, c.expectedSteering, a.steering)
if fmt.Sprintf("%0.2f", a.Steering()) != fmt.Sprintf("%0.2f", c.expectedSteering) {
t.Errorf("%s: bad steering value, expected: %0.2f, actual: %.2f", c.name, c.expectedSteering, a.Steering())
}
if a.driveMode != c.expectedDriveMode {
t.Errorf("%s: bad drive mode, expected: %v, actual:%v", c.name, c.expectedDriveMode, a.driveMode)
if a.DriveMode() != c.expectedDriveMode {
t.Errorf("%s: bad drive mode, expected: %v, actual:%v", c.name, c.expectedDriveMode, a.DriveMode())
}
if a.ctrlRecord != c.expectedSwitchRecord {
t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.ctrlRecord)
if a.SwitchRecord() != c.expectedSwitchRecord {
t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.SwitchRecord())
}
})
}
@ -311,10 +312,8 @@ func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) {
func Test_convertPwmSteeringToPercent(t *testing.T) {
type args struct {
value int
middle int
min int
max int
value int
steeringConfig *PWMConfig
}
tests := []struct {
name string
@ -324,127 +323,151 @@ func Test_convertPwmSteeringToPercent(t *testing.T) {
{
name: "middle",
args: args{
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
min: MinPwmAngle,
max: MaxPwmAngle,
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
steeringConfig: &PWMConfig{
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,
value: MinPwmAngle,
steeringConfig: &PWMConfig{
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,
value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle - (MaxPwmAngle-MinPwmAngle)/4)),
steeringConfig: &PWMConfig{
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,
value: MinPwmAngle - 100,
steeringConfig: &PWMConfig{
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,
value: MaxPwmAngle,
steeringConfig: &PWMConfig{
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,
value: int(math.Round((MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + (MaxPwmAngle-MinPwmAngle)/4)),
steeringConfig: &PWMConfig{
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,
value: MaxPwmAngle + 100,
steeringConfig: &PWMConfig{
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,
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
steeringConfig: &PWMConfig{
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,
value: int(math.Round(((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100-MinPwmAngle)/2) + MinPwmAngle),
steeringConfig: &PWMConfig{
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,
value: MinPwmAngle,
steeringConfig: &PWMConfig{
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,
value: int(math.Round((MaxPwmAngle - (MaxPwmAngle-((MaxPwmAngle-MinPwmAngle)/2+MinPwmAngle+100))/2))),
steeringConfig: &PWMConfig{
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,
value: MaxPwmAngle,
steeringConfig: &PWMConfig{
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 {
if got := convertPwmSteeringToPercent(tt.args.value, tt.args.steeringConfig); got != tt.want {
t.Errorf("convertPwmSteeringToPercent() = %v, want %v", got, tt.want)
}
})