Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[RSDK-869] Allow for servo to turn more than 180 #1624

Merged
merged 4 commits into from
Nov 29, 2022
Merged

[RSDK-869] Allow for servo to turn more than 180 #1624

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
84a13e4
Allow arbitrary range for servo
npmenard Nov 28, 2022
5b2c04c
remove log
npmenard Nov 28, 2022
00b8366
addressed comments
npmenard Nov 28, 2022
19824f8
Merge remote-tracking branch 'upstream/main' into RSDK-869
npmenard Nov 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions components/board/pi/impl/servo.go
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
39 changes: 18 additions & 21 deletions components/servo/gpio/servo.go
View file
Open in desktop
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 @@ -63,15 +63,12 @@ func (config *servoConfig) Validate(path string) ([]string, error) {
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.StartPos < minDeg || *config.StartPos > maxDeg {
if *config.StartPos < defaultMinDeg || *config.StartPos > defaultMaxDeg {
return nil, viamutils.NewConfigValidationError(path, errors.New("starting_position_degs should be between 0 and 180"))
}
}
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 {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There used to be a corresponding check for MaxDeg, but it's now gone. Shouldn't it still be here, modified in a similar way to the MinDeg check?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we don't enforce an upper limit for maxDeg since it's dependent on the hardware. in theory we should also ignore the test minDeg and let a user set a negative minimum deg

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

...so why not do that, too? (i.e., why not get rid of this check and allow a negative minimum value?)

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 +130,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 +141,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 +173,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 +190,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 {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should these also be functions on the servo object and use the stored servo variables? Would it be less/more boiler plate? Otherwise looks good to me.

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 +202,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 +286,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 +296,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 +309,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
6 changes: 1 addition & 5 deletions components/servo/gpio/servo_test.go
View file
Open in desktop
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 Down
4 changes: 2 additions & 2 deletions components/servo/server.go
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
10 changes: 5 additions & 5 deletions components/servo/servo_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,8 +107,8 @@ func TestCreateStatus(t *testing.T) {
status := &pb.Status{PositionDeg: uint32(8), IsMoving: true}

injectServo := &inject.Servo{}
injectServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
return uint8(status.PositionDeg), nil
injectServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
return status.PositionDeg, nil
}
injectServo.IsMovingFunc = func(context.Context) (bool, error) {
return true, nil
Expand Down Expand Up @@ -138,7 +138,7 @@ func TestCreateStatus(t *testing.T) {

t.Run("fail on Position", func(t *testing.T) {
errFail := errors.New("can't get position")
injectServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint8, error) {
injectServo.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (uint32, error) {
return 0, errFail
}
_, err = servo.CreateStatus(context.Background(), injectServo)
Expand Down Expand Up @@ -314,12 +314,12 @@ type mockLocal struct {
extra map[string]interface{}
}

func (mServo *mockLocal) Move(ctx context.Context, angleDegs uint8, extra map[string]interface{}) error {
func (mServo *mockLocal) Move(ctx context.Context, angleDegs uint32, extra map[string]interface{}) error {
mServo.extra = extra
return nil
}

func (mServo *mockLocal) Position(ctx context.Context, extra map[string]interface{}) (uint8, error) {
func (mServo *mockLocal) Position(ctx context.Context, extra map[string]interface{}) (uint32, error) {
mServo.posCount++
mServo.extra = extra
return pos, nil
Expand Down
Loading