Skip to content

Commit

Permalink
[RSDK-869] Allow for servo to turn more than 180 (#1624)
Browse files Browse the repository at this point in the history
  • Loading branch information
@npmenard
npmenard authored Nov 29, 2022
1 parent 1760bfb commit 9637534
Show file tree
Hide file tree
Showing 13 changed files with 70 additions and 74 deletions.
12 changes: 6 additions & 6 deletions components/board/pi/impl/servo.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,10 @@ func init() {

theServo := &piPigpioServo{pin: C.uint(bcom)}
if attr.Min > 0 {
theServo.min = uint8(attr.Min)
theServo.min = uint32(attr.Min)
}
if attr.Max > 0 {
theServo.max = uint8(attr.Max)
theServo.max = uint32(attr.Max)
}

theServo.pinname = attr.Pin
Expand Down Expand Up @@ -90,15 +90,15 @@ type piPigpioServo struct {
pin C.uint
pinname string
res C.int
min, max uint8
min, max uint32
opMgr operation.SingleOperationManager
pulseWidth int // pulsewidth value, 500-2500us is 0-180 degrees, 0 is off
holdPos bool
}

// Move moves the servo to the given angle (0-180 degrees)
// This will block until done or a new operation cancels this one
func (s *piPigpioServo) Move(ctx context.Context, angle uint8, extra map[string]interface{}) error {
func (s *piPigpioServo) Move(ctx context.Context, angle uint32, extra map[string]interface{}) error {
ctx, done := s.opMgr.New(ctx)
defer done()

Expand Down Expand Up @@ -148,7 +148,7 @@ func (s *piPigpioServo) pigpioErrors(res int) error {
}

// Position returns the current set angle (degrees) of the servo.
func (s *piPigpioServo) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (s *piPigpioServo) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
res := C.gpioGetServoPulsewidth(s.pin)
err := s.pigpioErrors(int(res))
if int(res) != 0 {
Expand All @@ -157,7 +157,7 @@ func (s *piPigpioServo) Position(ctx context.Context, extra map[string]interface
if err != nil {
return 0, err
}
return uint8(pulseWidthToAngle(int(s.res))), nil
return uint32(pulseWidthToAngle(int(s.res))), nil
}

// angleToPulseWidth changes the input angle in degrees
Expand Down
8 changes: 4 additions & 4 deletions components/servo/client.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,19 +31,19 @@ func NewClientFromConn(ctx context.Context, conn rpc.ClientConn, name string, lo
}
}

func (c *client) Move(ctx context.Context, angleDeg uint8, extra map[string]interface{}) error {
func (c *client) Move(ctx context.Context, angleDeg uint32, extra map[string]interface{}) error {
ext, err := protoutils.StructToStructPb(extra)
if err != nil {
return err
}
req := &pb.MoveRequest{AngleDeg: uint32(angleDeg), Name: c.name, Extra: ext}
req := &pb.MoveRequest{AngleDeg: angleDeg, Name: c.name, Extra: ext}
if _, err := c.client.Move(ctx, req); err != nil {
return err
}
return nil
}

func (c *client) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (c *client) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
ext, err := protoutils.StructToStructPb(extra)
if err != nil {
return 0, err
Expand All @@ -53,7 +53,7 @@ func (c *client) Position(ctx context.Context, extra map[string]interface{}) (ui
if err != nil {
return 0, err
}
return uint8(resp.PositionDeg), nil
return resp.PositionDeg, nil
}

func (c *client) Stop(ctx context.Context, extra map[string]interface{}) error {
Expand Down
8 changes: 4 additions & 4 deletions components/servo/client_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,11 @@ func TestClient(t *testing.T) {
workingServo := &inject.Servo{}
failingServo := &inject.Servo{}

workingServo.MoveFunc = func(ctx context.Context, angle uint8, extra map[string]interface{}) error {
workingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error {
actualExtra = extra
return nil
}
workingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
workingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
actualExtra = extra
return 20, nil
}
Expand All @@ -48,10 +48,10 @@ func TestClient(t *testing.T) {
return nil
}

failingServo.MoveFunc = func(ctx context.Context, angle uint8, extra map[string]interface{}) error {
failingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error {
return errors.New("move failed")
}
failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
return 0, errors.New("current angle not readable")
}
failingServo.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
Expand Down
2 changes: 1 addition & 1 deletion components/servo/collectors.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ func (m method) String() string {

// Position wraps the returned set angle (degrees) value.
type Position struct {
Position uint8
Position uint32
}

func newPositionCollector(resource interface{}, params data.CollectorParams) (data.Collector, error) {
Expand Down
6 changes: 3 additions & 3 deletions components/servo/fake/servo.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,18 +24,18 @@ func init() {
// A Servo allows setting and reading a single angle.
type Servo struct {
Name string
angle uint8
angle uint32
generic.Echo
}

// Move sets the given angle.
func (s *Servo) Move(ctx context.Context, angleDeg uint8, extra map[string]interface{}) error {
func (s *Servo) Move(ctx context.Context, angleDeg uint32, extra map[string]interface{}) error {
s.angle = angleDeg
return nil
}

// Position returns the set angle.
func (s *Servo) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (s *Servo) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
return s.angle, nil
}

Expand Down
50 changes: 25 additions & 25 deletions components/servo/gpio/servo.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,10 @@ import (
)

const (
minDeg float64 = 0.0
maxDeg float64 = 180.0
minWidthUs uint = 500 // absolute minimum pwm width
maxWidthUs uint = 2500 // absolute maximum pwm width
defaultMinDeg float64 = 0.0
defaultMaxDeg float64 = 180.0
minWidthUs uint = 500 // absolute minimum pwm width
maxWidthUs uint = 2500 // absolute maximum pwm width
)

type servoConfig struct {
Expand Down Expand Up @@ -57,21 +57,21 @@ func (config *servoConfig) Validate(path string) ([]string, error) {
return nil, viamutils.NewConfigValidationFieldRequiredError(path, "pin")
}
if config.StartPos != nil {
if config.MinDeg != nil && *config.StartPos < *config.MinDeg {
return nil, viamutils.NewConfigValidationError(path, errors.New("starting_position_degs cannot be lower than min_angle_deg"))
minDeg := defaultMinDeg
maxDeg := defaultMaxDeg
if config.MinDeg != nil {
minDeg = *config.MinDeg
}
if config.MaxDeg != nil && *config.StartPos > *config.MaxDeg {
return nil, viamutils.NewConfigValidationError(path, errors.New("starting_position_degs cannot be higher than max_angle_deg"))
if config.MaxDeg != nil {
maxDeg = *config.MaxDeg
}
if *config.StartPos < minDeg || *config.StartPos > maxDeg {
return nil, viamutils.NewConfigValidationError(path, errors.New("starting_position_degs should be between 0 and 180"))
return nil, viamutils.NewConfigValidationError(path,
errors.Errorf("starting_position_degs should be between %.1f and %.1f", minDeg, maxDeg))
}
}
if config.MinDeg != nil && *config.MinDeg < minDeg {
return nil, viamutils.NewConfigValidationError(path, errors.Errorf("min_angle_deg cannot be lower than %f", minDeg))
}
if config.MaxDeg != nil && *config.MaxDeg > maxDeg {
return nil, viamutils.NewConfigValidationError(path, errors.Errorf("max_angle_deg cannot be higher than %f", maxDeg))
if config.MinDeg != nil && *config.MinDeg < 0 {
return nil, viamutils.NewConfigValidationError(path, errors.New("min_angle_deg cannot be lower than 0"))
}
if config.MinWidthUS != nil && *config.MinWidthUS < minWidthUs {
return nil, viamutils.NewConfigValidationError(path, errors.Errorf("min_width_us cannot be lower than %d", minWidthUs))
Expand Down Expand Up @@ -133,7 +133,7 @@ func newGPIOServo(ctx context.Context, deps registry.Dependencies, cfg config.Co
return nil, errors.Wrap(err, "couldn't get servo pin pwm frequency")
}
if attr.Frequency != nil {
if frequency > 450 || frequency == 0 {
if *attr.Frequency > 450 || *attr.Frequency == 0 {
return nil, errors.Errorf("PWM frequencies should not be above 450Hz or 0, have %d", frequency)
}

Expand All @@ -144,8 +144,8 @@ func newGPIOServo(ctx context.Context, deps registry.Dependencies, cfg config.Co
frequency = *attr.Frequency
}

minDeg := minDeg
maxDeg := maxDeg
minDeg := defaultMinDeg
maxDeg := defaultMaxDeg
if attr.MinDeg != nil {
minDeg = *attr.MinDeg
}
Expand Down Expand Up @@ -176,14 +176,14 @@ func newGPIOServo(ctx context.Context, deps registry.Dependencies, cfg config.Co
currPct: 0,
}

if err := servo.Move(ctx, uint8(startPos), nil); err != nil {
if err := servo.Move(ctx, uint32(startPos), nil); err != nil {
return nil, errors.Wrap(err, "couldn't move servo to start position")
}
if servo.pwmRes == 0 {
if err := servo.findPWMResolution(ctx); err != nil {
return nil, errors.Wrap(err, "failed to guess the pwm resolution")
}
if err := servo.Move(ctx, uint8(startPos), nil); err != nil {
if err := servo.Move(ctx, uint32(startPos), nil); err != nil {
return nil, errors.Wrap(err, "couldn't move servo to start position")
}
}
Expand All @@ -193,7 +193,7 @@ func newGPIOServo(ctx context.Context, deps registry.Dependencies, cfg config.Co
var _ = servo.LocalServo(&servoGPIO{})

// Given minUs, maxUs, deg and frequency attempt to calculate the corresponding duty cycle pct.
func mapDegToDutyCylePct(minUs, maxUs uint, deg float64, frequency uint) float64 {
func mapDegToDutyCylePct(minUs, maxUs uint, minDeg, maxDeg, deg float64, frequency uint) float64 {
period := 1.0 / float64(frequency) // dutyCycle in s
degRange := maxDeg - minDeg // servo moves from minDeg to maxDeg
uSRange := float64(maxUs - minUs) // pulse width between minUs to maxUs
Expand All @@ -205,7 +205,7 @@ func mapDegToDutyCylePct(minUs, maxUs uint, deg float64, frequency uint) float64
}

// Given minUs, maxUs, deg and frequency returns the corresponding duty cycle pct.
func mapDutyCylePctToDeg(minUs, maxUs uint, pct float64, frequency uint) float64 {
func mapDutyCylePctToDeg(minUs, maxUs uint, minDeg, maxDeg, pct float64, frequency uint) float64 {
period := 1.0 / float64(frequency) // dutyCycle in s
pwmWidthUs := pct * period * 1000 * 1000
degRange := maxDeg - minDeg // servo moves from minDeg to maxDeg
Expand Down Expand Up @@ -289,7 +289,7 @@ func (s *servoGPIO) findPWMResolution(ctx context.Context) error {

// Move moves the servo to the given angle (0-180 degrees)
// This will block until done or a new operation cancels this one.
func (s *servoGPIO) Move(ctx context.Context, ang uint8, extra map[string]interface{}) error {
func (s *servoGPIO) Move(ctx context.Context, ang uint32, extra map[string]interface{}) error {
ctx, done := s.opMgr.New(ctx)
defer done()
angle := float64(ang)
Expand All @@ -299,7 +299,7 @@ func (s *servoGPIO) Move(ctx context.Context, ang uint8, extra map[string]interf
if angle > s.max {
angle = s.max
}
pct := mapDegToDutyCylePct(s.minUs, s.maxUs, angle, s.frequency)
pct := mapDegToDutyCylePct(s.minUs, s.maxUs, s.min, s.max, angle, s.frequency)
if s.pwmRes != 0 {
realTick := math.Round(pct * float64(s.pwmRes))
pct = realTick / float64(s.pwmRes)
Expand All @@ -312,12 +312,12 @@ func (s *servoGPIO) Move(ctx context.Context, ang uint8, extra map[string]interf
}

// Position returns the current set angle (degrees) of the servo.
func (s *servoGPIO) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (s *servoGPIO) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
pct, err := s.pin.PWM(ctx, nil)
if err != nil {
return 0, errors.Wrap(err, "couldn't get servo pin duty cycle")
}
return uint8(mapDutyCylePctToDeg(s.minUs, s.maxUs, pct, s.frequency)), nil
return uint32(mapDutyCylePctToDeg(s.minUs, s.maxUs, s.min, s.max, pct, s.frequency)), nil
}

// Stop stops the servo. It is assumed the servo stops immediately.
Expand Down
12 changes: 4 additions & 8 deletions components/servo/gpio/servo_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,13 +37,9 @@ func TestValidate(t *testing.T) {
cfg.MinDeg = Ptr(-1.5)
_, err = cfg.Validate("test")
test.That(t, err, test.ShouldNotBeNil)
test.That(t, err.Error(), test.ShouldContainSubstring, "error validating \"test\": min_angle_deg cannot be lower than 0.0")
test.That(t, err.Error(), test.ShouldContainSubstring, "error validating \"test\": min_angle_deg cannot be lower than 0")
cfg.MinDeg = Ptr(1.5)

cfg.MaxDeg = Ptr(181.0)
_, err = cfg.Validate("test")
test.That(t, err, test.ShouldNotBeNil)
test.That(t, err.Error(), test.ShouldContainSubstring, "error validating \"test\": max_angle_deg cannot be higher than 180.0")
cfg.MaxDeg = Ptr(90.0)

cfg.MinWidthUS = Ptr(uint(450))
Expand All @@ -63,14 +59,14 @@ func TestValidate(t *testing.T) {
test.That(t, err, test.ShouldNotBeNil)
test.That(t, err.Error(),
test.ShouldContainSubstring,
"error validating \"test\": starting_position_degs cannot be higher than max_angle_deg")
"error validating \"test\": starting_position_degs should be between 1.5 and 90.0")

cfg.StartPos = Ptr(1.0)
_, err = cfg.Validate("test")
test.That(t, err, test.ShouldNotBeNil)
test.That(t, err.Error(),
test.ShouldContainSubstring,
"error validating \"test\": starting_position_degs cannot be lower than min_angle_deg")
"error validating \"test\": starting_position_degs should be between 1.5 and 90.0")

cfg.StartPos = Ptr(199.0)
cfg.MaxDeg = nil
Expand All @@ -79,7 +75,7 @@ func TestValidate(t *testing.T) {
test.That(t, err, test.ShouldNotBeNil)
test.That(t, err.Error(),
test.ShouldContainSubstring,
"error validating \"test\": starting_position_degs should be between 0 and 180")
"error validating \"test\": starting_position_degs should be between 0.0 and 180.0")

cfg.StartPos = Ptr(0.0)
_, err = cfg.Validate("test")
Expand Down
4 changes: 2 additions & 2 deletions components/servo/server.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ func (server *subtypeServer) Move(ctx context.Context, req *pb.MoveRequest) (*pb
if err != nil {
return nil, err
}
return &pb.MoveResponse{}, servo.Move(ctx, uint8(req.GetAngleDeg()), req.Extra.AsMap())
return &pb.MoveResponse{}, servo.Move(ctx, req.GetAngleDeg(), req.Extra.AsMap())
}

func (server *subtypeServer) GetPosition(
Expand All @@ -55,7 +55,7 @@ func (server *subtypeServer) GetPosition(
if err != nil {
return nil, err
}
return &pb.GetPositionResponse{PositionDeg: uint32(angleDeg)}, nil
return &pb.GetPositionResponse{PositionDeg: angleDeg}, nil
}

func (server *subtypeServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) {
Expand Down
8 changes: 4 additions & 4 deletions components/servo/server_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,11 @@ func TestServoMove(t *testing.T) {

var actualExtra map[string]interface{}

workingServo.MoveFunc = func(ctx context.Context, angle uint8, extra map[string]interface{}) error {
workingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error {
actualExtra = extra
return nil
}
failingServo.MoveFunc = func(ctx context.Context, angle uint8, extra map[string]interface{}) error {
failingServo.MoveFunc = func(ctx context.Context, angle uint32, extra map[string]interface{}) error {
return errors.New("move failed")
}

Expand Down Expand Up @@ -70,11 +70,11 @@ func TestServoGetPosition(t *testing.T) {

var actualExtra map[string]interface{}

workingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
workingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
actualExtra = extra
return 20, nil
}
failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
failingServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
return 0, errors.New("current angle not readable")
}

Expand Down
10 changes: 5 additions & 5 deletions components/servo/servo.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,10 @@ var Subtype = resource.NewSubtype(
type Servo interface {
// Move moves the servo to the given angle (0-180 degrees)
// This will block until done or a new operation cancels this one
Move(ctx context.Context, angleDeg uint8, extra map[string]interface{}) error
Move(ctx context.Context, angleDeg uint32, extra map[string]interface{}) error

// Position returns the current set angle (degrees) of the servo.
Position(ctx context.Context, extra map[string]interface{}) (uint8, error)
Position(ctx context.Context, extra map[string]interface{}) (uint32, error)

// Stop stops the servo. It is assumed the servo stops immediately.
Stop(ctx context.Context, extra map[string]interface{}) error
Expand Down Expand Up @@ -129,7 +129,7 @@ func CreateStatus(ctx context.Context, resource interface{}) (*pb.Status, error)
if err != nil {
return nil, err
}
return &pb.Status{PositionDeg: uint32(position), IsMoving: isMoving}, nil
return &pb.Status{PositionDeg: position, IsMoving: isMoving}, nil
}

type reconfigurableServo struct {
Expand All @@ -155,13 +155,13 @@ func (r *reconfigurableServo) DoCommand(ctx context.Context, cmd map[string]inte
return r.actual.DoCommand(ctx, cmd)
}

func (r *reconfigurableServo) Move(ctx context.Context, angleDeg uint8, extra map[string]interface{}) error {
func (r *reconfigurableServo) Move(ctx context.Context, angleDeg uint32, extra map[string]interface{}) error {
r.mu.RLock()
defer r.mu.RUnlock()
return r.actual.Move(ctx, angleDeg, extra)
}

func (r *reconfigurableServo) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (r *reconfigurableServo) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
r.mu.RLock()
defer r.mu.RUnlock()
return r.actual.Position(ctx, extra)
Expand Down
Loading

0 comments on commit 9637534

Please sign in to comment.