Compare commits
No commits in common. "14d5c605e7686e8fa5283e07624c3360dcbb9b3b" and "973ebb9df9e10ec5e0ad32a581f8355549ab6e93" have entirely different histories.
14d5c605e7
...
973ebb9df9
@ -1,4 +1,4 @@
|
||||
FROM --platform=$BUILDPLATFORM golang:1.19-alpine AS builder
|
||||
FROM --platform=$BUILDPLATFORM golang:1.17-alpine AS builder
|
||||
|
||||
ARG TARGETPLATFORM
|
||||
ARG BUILDPLATFORM
|
||||
|
||||
@ -38,7 +38,7 @@ func main() {
|
||||
cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
|
||||
|
||||
var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int
|
||||
var secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM 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)
|
||||
}
|
||||
@ -46,19 +46,7 @@ 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 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)
|
||||
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)
|
||||
@ -77,20 +65,12 @@ 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, "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")
|
||||
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()
|
||||
@ -119,17 +99,9 @@ func main() {
|
||||
}
|
||||
defer client.Disconnect(10)
|
||||
|
||||
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),
|
||||
)
|
||||
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)
|
||||
|
||||
cli.HandleExit(a)
|
||||
|
||||
|
||||
2
go.mod
2
go.mod
@ -1,6 +1,6 @@
|
||||
module github.com/cyrilix/robocar-arduino
|
||||
|
||||
go 1.19
|
||||
go 1.18
|
||||
|
||||
require (
|
||||
github.com/cyrilix/robocar-base v0.1.7
|
||||
|
||||
@ -21,12 +21,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+)`)
|
||||
DefaultPwmThrottle = PWMConfig{
|
||||
Min: MinPwmThrottle,
|
||||
Max: MaxPwmThrottle,
|
||||
Middle: MinPwmThrottle + (MaxPwmThrottle-MinPwmThrottle)/2,
|
||||
}
|
||||
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<frequency>\d+),(?P<distance_cm>\d+)?`)
|
||||
)
|
||||
|
||||
type Part struct {
|
||||
@ -35,68 +30,50 @@ type Part struct {
|
||||
pubFrequency float64
|
||||
serial io.Reader
|
||||
mutex sync.Mutex
|
||||
steering, secondarySteering float32
|
||||
throttle, secondaryThrottle float32
|
||||
steering float32
|
||||
throttle float32
|
||||
ctrlRecord bool
|
||||
driveMode events.DriveMode
|
||||
cancel chan interface{}
|
||||
|
||||
useSecondaryRc bool
|
||||
pwmSteeringConfig *PWMConfig
|
||||
pwmSecondarySteeringConfig *PWMConfig
|
||||
pwmThrottleConfig *PWMConfig
|
||||
pwmSecondaryThrottleConfig *PWMConfig
|
||||
pwmSteeringConfig PWMSteeringConfig
|
||||
pwmThrottleConfig PWMThrottleConfig
|
||||
}
|
||||
|
||||
type PWMConfig struct {
|
||||
Min int
|
||||
Max int
|
||||
Middle int
|
||||
type PWMThrottleConfig struct {
|
||||
Min int
|
||||
Max int
|
||||
Zero int
|
||||
}
|
||||
|
||||
func NewPWMConfig(min, max int) *PWMConfig {
|
||||
return &PWMConfig{
|
||||
Min: min,
|
||||
Max: max,
|
||||
Middle: min + (max-min)/2,
|
||||
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 NewAsymetricPWMConfig(min, max, middle int) *PWMConfig {
|
||||
c := NewPWMConfig(min, max)
|
||||
c.Middle = middle
|
||||
func NewAsymetricPWMSteeringConfig(min, max, middle int) PWMSteeringConfig {
|
||||
c := NewPWMSteeringConfig(min, max)
|
||||
c.Center = 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, options ...Option) *Part {
|
||||
switchRecordTopic string, pubFrequency float64, steeringConfig PWMSteeringConfig, throttleConfig PWMThrottleConfig) *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{
|
||||
return &Part{
|
||||
client: client,
|
||||
serial: s,
|
||||
throttleTopic: throttleTopic,
|
||||
@ -107,16 +84,9 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
|
||||
driveMode: events.DriveMode_INVALID,
|
||||
cancel: make(chan interface{}),
|
||||
|
||||
pwmSteeringConfig: &DefaultPwmThrottle,
|
||||
pwmSecondarySteeringConfig: &DefaultPwmThrottle,
|
||||
pwmThrottleConfig: &DefaultPwmThrottle,
|
||||
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
|
||||
pwmSteeringConfig: steeringConfig,
|
||||
pwmThrottleConfig: throttleConfig,
|
||||
}
|
||||
|
||||
for _, o := range options {
|
||||
o(p)
|
||||
}
|
||||
return p
|
||||
}
|
||||
|
||||
func (a *Part) Start() error {
|
||||
@ -151,8 +121,6 @@ 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() {
|
||||
@ -172,23 +140,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)
|
||||
a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig.Left, a.pwmSteeringConfig.Right, a.pwmSteeringConfig.Center)
|
||||
}
|
||||
|
||||
func convertPwmSteeringToPercent(value int, c *PWMConfig) float32 {
|
||||
if value < c.Min {
|
||||
value = c.Min
|
||||
} else if value > c.Max {
|
||||
value = c.Max
|
||||
func convertPwmSteeringToPercent(value int, minPwm int, maxPwm int, middlePwm int) float32 {
|
||||
if value < minPwm {
|
||||
value = minPwm
|
||||
} else if value > maxPwm {
|
||||
value = maxPwm
|
||||
}
|
||||
if value == c.Middle {
|
||||
if value == middlePwm {
|
||||
return 0.
|
||||
}
|
||||
if value < c.Middle {
|
||||
return (float32(value) - float32(c.Middle)) / float32(c.Middle-c.Min)
|
||||
if value < middlePwm {
|
||||
return (float32(value) - float32(middlePwm)) / float32(middlePwm-minPwm)
|
||||
}
|
||||
// middle < value < max
|
||||
return (float32(value) - float32(c.Middle)) / float32(c.Max-c.Middle)
|
||||
return (float32(value) - float32(middlePwm)) / float32(maxPwm-middlePwm)
|
||||
}
|
||||
|
||||
func (a *Part) processChannel2(v string) {
|
||||
@ -204,11 +172,11 @@ func (a *Part) processChannel2(v string) {
|
||||
}
|
||||
|
||||
throttle := 0.
|
||||
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 = (float64(value) - float64(a.pwmThrottleConfig.Zero)) / float64(a.pwmThrottleConfig.Max-a.pwmThrottleConfig.Zero)
|
||||
}
|
||||
if value < a.pwmThrottleConfig.Middle {
|
||||
throttle = -1. * (float64(a.pwmThrottleConfig.Middle) - float64(value)) / (float64(a.pwmThrottleConfig.Middle - a.pwmThrottleConfig.Min))
|
||||
if value < a.pwmThrottleConfig.Zero {
|
||||
throttle = -1. * (float64(a.pwmThrottleConfig.Zero) - float64(value)) / (float64(a.pwmThrottleConfig.Zero - a.pwmThrottleConfig.Min))
|
||||
}
|
||||
|
||||
a.throttle = float32(throttle)
|
||||
@ -216,11 +184,6 @@ 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) {
|
||||
@ -255,10 +218,6 @@ func (a *Part) processChannel6(v string) {
|
||||
zap.S().Errorf("invalid value for channel6 'drive-mode', should be an int: %v", err)
|
||||
return
|
||||
}
|
||||
if value < 0 {
|
||||
// No value, ignore it
|
||||
return
|
||||
}
|
||||
if value > 1800 {
|
||||
if a.driveMode != events.DriveMode_PILOT {
|
||||
zap.S().Infof("Update channel 6 'drive-mode' with value %v, new user_mode: %v", value, events.DriveMode_PILOT)
|
||||
@ -272,59 +231,6 @@ 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)))
|
||||
|
||||
@ -340,6 +246,9 @@ func (a *Part) publishLoop() {
|
||||
}
|
||||
|
||||
func (a *Part) publishValues() {
|
||||
a.mutex.Lock()
|
||||
defer a.mutex.Unlock()
|
||||
|
||||
a.publishThrottle()
|
||||
a.publishSteering()
|
||||
a.publishDriveMode()
|
||||
@ -348,7 +257,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)
|
||||
@ -362,7 +271,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)
|
||||
@ -376,7 +285,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 {
|
||||
@ -388,7 +297,7 @@ func (a *Part) publishDriveMode() {
|
||||
|
||||
func (a *Part) publishSwitchRecord() {
|
||||
sr := events.SwitchRecordMessage{
|
||||
Enabled: !a.SwitchRecord(),
|
||||
Enabled: !a.ctrlRecord,
|
||||
}
|
||||
switchRecordMessage, err := proto.Marshal(&sr)
|
||||
if err != nil {
|
||||
|
||||
@ -58,9 +58,11 @@ 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)}
|
||||
defaultPwmThrottleConfig := PWMThrottleConfig{MinPwmThrottle, MaxPwmThrottle, MinPwmThrottle + (MaxPwmThrottle-MinPwmAngle)/int(2)}
|
||||
a := Part{client: nil, serial: conn, pubFrequency: 100,
|
||||
pwmSteeringConfig: NewAsymetricPWMSteeringConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
|
||||
}
|
||||
|
||||
go func() {
|
||||
err := a.Start()
|
||||
if err != nil {
|
||||
@ -69,129 +71,126 @@ 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, distanceCm := 678, 910, 1112, 1678, 1910, 112, 128
|
||||
cases := []struct {
|
||||
name, content string
|
||||
throttlePwmConfig *PWMConfig
|
||||
throttlePwmConfig PWMThrottleConfig
|
||||
expectedThrottle, expectedSteering float32
|
||||
expectedDriveMode events.DriveMode
|
||||
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,50,%d\n", channel1, channel2, channel3, channel4, channel5, channel6, distanceCm),
|
||||
defaultPwmThrottleConfig, -1., -1.,
|
||||
events.DriveMode_USER, false},
|
||||
{"Invalid line",
|
||||
{"Unparsable line",
|
||||
"12350,invalid line\n", defaultPwmThrottleConfig,
|
||||
-1., -1., events.DriveMode_INVALID, false},
|
||||
-1., -1., events.DriveMode_USER, 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
|
||||
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 998, channel6, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
|
||||
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1987, channel6, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
|
||||
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1850, channel6, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
|
||||
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
|
||||
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
|
||||
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1987, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
|
||||
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1850, distanceCm), 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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
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,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, -1., -0.92, 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),
|
||||
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),
|
||||
defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false},
|
||||
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},
|
||||
{"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},
|
||||
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,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false},
|
||||
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,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8),
|
||||
&PWMConfig{1000, 1700, 1500},
|
||||
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},
|
||||
{"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},
|
||||
{"Drive Mode: user",
|
||||
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8),
|
||||
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),
|
||||
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),
|
||||
defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false},
|
||||
}
|
||||
|
||||
for _, c := range cases {
|
||||
t.Run(c.name, func(t *testing.T) {
|
||||
w := bufio.NewWriter(serialClient)
|
||||
_, err := w.WriteString(c.content)
|
||||
if err != nil {
|
||||
t.Errorf("unable to send test content: %v", c.content)
|
||||
}
|
||||
err = w.Flush()
|
||||
if err != nil {
|
||||
t.Error("unable to flush content")
|
||||
}
|
||||
w := bufio.NewWriter(serialClient)
|
||||
_, err := w.WriteString(c.content)
|
||||
if err != nil {
|
||||
t.Errorf("unable to send test content: %v", c.content)
|
||||
}
|
||||
err = w.Flush()
|
||||
if err != nil {
|
||||
t.Error("unable to flush content")
|
||||
}
|
||||
|
||||
a.pwmThrottleConfig = c.throttlePwmConfig
|
||||
a.driveMode = events.DriveMode_INVALID
|
||||
a.pwmThrottleConfig = c.throttlePwmConfig
|
||||
|
||||
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.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.SwitchRecord() != c.expectedSwitchRecord {
|
||||
t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.SwitchRecord())
|
||||
}
|
||||
})
|
||||
time.Sleep(10 * time.Millisecond)
|
||||
a.mutex.Lock()
|
||||
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 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)
|
||||
}
|
||||
a.mutex.Unlock()
|
||||
}
|
||||
}
|
||||
|
||||
@ -322,8 +321,10 @@ func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) {
|
||||
|
||||
func Test_convertPwmSteeringToPercent(t *testing.T) {
|
||||
type args struct {
|
||||
value int
|
||||
steeringConfig *PWMConfig
|
||||
value int
|
||||
middle int
|
||||
min int
|
||||
max int
|
||||
}
|
||||
tests := []struct {
|
||||
name string
|
||||
@ -333,151 +334,127 @@ func Test_convertPwmSteeringToPercent(t *testing.T) {
|
||||
{
|
||||
name: "middle",
|
||||
args: args{
|
||||
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
value: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
min: MinPwmAngle,
|
||||
max: MaxPwmAngle,
|
||||
},
|
||||
want: 0.,
|
||||
},
|
||||
{
|
||||
name: "left",
|
||||
args: args{
|
||||
value: MinPwmAngle,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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)),
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
value: MinPwmAngle - 100,
|
||||
middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
min: MinPwmAngle,
|
||||
max: MaxPwmAngle,
|
||||
},
|
||||
want: -1.,
|
||||
},
|
||||
{
|
||||
name: "right",
|
||||
args: args{
|
||||
value: MaxPwmAngle,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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)),
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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),
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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))),
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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,
|
||||
steeringConfig: &PWMConfig{
|
||||
Middle: (MaxPwmAngle-MinPwmAngle)/2 + MinPwmAngle + 100,
|
||||
Min: MinPwmAngle,
|
||||
Max: MaxPwmAngle,
|
||||
},
|
||||
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.steeringConfig); got != tt.want {
|
||||
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)
|
||||
}
|
||||
})
|
||||
|
||||
Loading…
Reference in New Issue
Block a user