From c42086a8370f800f5ef4e82e6ddc4a0806442b99 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Mon, 17 Jan 2022 19:00:12 +0100 Subject: [PATCH] refactor(pwm steering): asymetric center --- cmd/rc-pca9685/rc-pca9685.go | 20 ++++++++++++++------ pkg/actuator/steering.go | 27 +++++++++++++++++---------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/cmd/rc-pca9685/rc-pca9685.go b/cmd/rc-pca9685/rc-pca9685.go index 45229ac..443b587 100644 --- a/cmd/rc-pca9685/rc-pca9685.go +++ b/cmd/rc-pca9685/rc-pca9685.go @@ -23,6 +23,10 @@ const ( SteeringRightPWM = 1986 ) +var ( + SteeringCenterPWM = (SteeringRightPWM-SteeringLeftPWM)/2 + SteeringLeftPWM +) + func main() { var mqttBroker, username, password, clientId, topicThrottle, topicSteering string @@ -45,7 +49,7 @@ func main() { zap.S().Warnf("unable to init throttleMaxPWM arg: %v", err) } - var steeringChannel, steeringLeftPWM, steeringRightPWM int + var steeringChannel, steeringLeftPWM, steeringRightPWM, steeringCenterPWM int if err := cli.SetIntDefaultValueFromEnv(&steeringChannel, "STEERING_CHANNEL", SteeringChannel); err != nil { zap.S().Warnf("unable to init steeringChannel arg: %v", err) } @@ -55,6 +59,9 @@ func main() { if err := cli.SetIntDefaultValueFromEnv(&steeringRightPWM, "STEERING_RIGHT_PWM", SteeringRightPWM); err != nil { 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) + } var updatePWMFrequency int if err := cli.SetIntDefaultValueFromEnv(&updatePWMFrequency, "UPDATE_PWM_FREQUENCY", 25); err != nil { @@ -66,10 +73,11 @@ func main() { flag.IntVar(&throttleChannel, "throttle-channel", throttleChannel, "I2C channel to use to control throttle, THROTTLE_CHANNEL env if args not set") flag.IntVar(&steeringChannel, "steering-channel", steeringChannel, "I2C channel to use to control steering, STEERING_CHANNEL env if args not set") flag.IntVar(&throttleStoppedPWM, "throttle-zero-pwm", throttleStoppedPWM, "Zero value for throttle PWM, THROTTLE_STOPPED_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(&throttleMaxPWM, "throttle-max-pwm", throttleMaxPWM, "Max value for throttle PWM, THROTTLE_MAX_PWM env if args not set") - flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "Max left value for steering PWM, STEERING_STOPPED_PWM env if args not set") - flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "Max right value for steering PWM, STEERING_MIN_PWM env if args not set") + flag.IntVar(&throttleMinPWM, "throttle-min-pwm", throttleMinPWM, "Left value for throttle PWM, THROTTLE_MIN_PWM env if args not set") + flag.IntVar(&throttleMaxPWM, "throttle-max-pwm", throttleMaxPWM, "Right value for throttle PWM, THROTTLE_MAX_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(&updatePWMFrequency, "update-pwm-frequency", updatePWMFrequency, "Number of update values per seconds, UPDATE_PWM_FREQUENCY env if args not set") logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level") @@ -99,7 +107,7 @@ func main() { defer client.Disconnect(50) t := actuator2.NewThrottle(throttleChannel, throttleStoppedPWM, throttleMinPWM, throttleMaxPWM) - s := actuator2.NewSteering(steeringChannel, steeringLeftPWM, steeringRightPWM) + s := actuator2.NewSteering(steeringChannel, steeringLeftPWM, steeringRightPWM, steeringCenterPWM) p := part.NewPca9685Part(client, t, s, updatePWMFrequency, topicThrottle, topicSteering) err = p.Start() diff --git a/pkg/actuator/steering.go b/pkg/actuator/steering.go index de3bd7f..5d9c395 100644 --- a/pkg/actuator/steering.go +++ b/pkg/actuator/steering.go @@ -13,9 +13,9 @@ const ( ) type Steering struct { - channel int - leftPWM, rightPWM int - dev *pca9685.Dev + channel int + leftPWM, rightPWM, centerPWM int + dev *pca9685.Dev } func (s *Steering) SetPulse(pulse int) { @@ -26,19 +26,26 @@ func (s *Steering) SetPulse(pulse int) { } -// Set percent value steering +// SetPercentValue Set percent value steering func (s *Steering) SetPercentValue(p float32) { // map absolute angle to angle that vehicle can implement. - pulse := util.MapRange(float64(p), LeftAngle, RightAngle, float64(s.leftPWM), float64(s.rightPWM)) + + pulse := s.centerPWM + if p > 0 { + pulse = util.MapRange(float64(p), 0, RightAngle, float64(s.centerPWM), float64(s.rightPWM)) + } else if p < 0 { + pulse = util.MapRange(float64(p), LeftAngle, 0, float64(s.leftPWM), float64(s.centerPWM)) + } s.SetPulse(pulse) } -func NewSteering(channel, leftPWM, rightPWM int) *Steering { +func NewSteering(channel, leftPWM, rightPWM, centerPWM int) *Steering { s := Steering{ - channel: channel, - dev: device, - leftPWM: leftPWM, - rightPWM: rightPWM, + channel: channel, + dev: device, + leftPWM: leftPWM, + rightPWM: rightPWM, + centerPWM: centerPWM, } return &s }