From 88a02bce17e9f355b8bb33111f397a000b6b5235 Mon Sep 17 00:00:00 2001 From: kortschak Date: Sun, 17 Sep 2017 09:05:08 +0930 Subject: [PATCH] motorutil: add steering block equivalent --- README.md | 2 +- examples/znap/znap.go | 90 ++-------------- motorutil/steer.go | 232 ++++++++++++++++++++++++++++++++++++++++ motorutil/steer_test.go | 94 ++++++++++++++++ 4 files changed, 337 insertions(+), 81 deletions(-) create mode 100644 motorutil/steer.go create mode 100644 motorutil/steer_test.go diff --git a/README.md b/README.md index 9f8985b..900b599 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,6 @@ For device-specific functions see [EV3](https://github.com/ev3go/ev3) and [Brick ### Common tasks -None yet. +- [x] Steering helper similar to EV-G steering block LEGO® is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this software. diff --git a/examples/znap/znap.go b/examples/znap/znap.go index 0e55bab..6ce31d2 100644 --- a/examples/znap/znap.go +++ b/examples/znap/znap.go @@ -11,7 +11,6 @@ package main import ( - "fmt" "log" "math" "math/rand" @@ -20,6 +19,7 @@ import ( "time" "github.com/ev3go/ev3dev" + "github.com/ev3go/ev3dev/motorutil" ) func main() { @@ -229,7 +229,7 @@ func wander() { } max /= 2 - s := steering{left, right} + s := motorutil.Steering{Left: left, Right: right, Timeout: 5 * time.Second} for { rol: for { @@ -239,13 +239,17 @@ func wander() { } for _, move := range []struct { speed int - dir float64 + dir int }{ - {speed: max, dir: 1}, - {speed: max, dir: -1}, + {speed: max, dir: 100}, + {speed: max, dir: -100}, {speed: max, dir: 0}, } { - err = s.steer(move.speed, (rand.Intn(3)+1)*360, move.dir) + err = s.SteerCounts(move.speed, move.dir, (rand.Intn(3)+1)*360) + if err != nil { + log.Fatalf("failed to steer %v/%v: %v", move.speed, move.dir, err) + } + err = s.Wait() if err != nil { log.Fatalf("failed to steer %v/%v: %v", move.speed, move.dir, err) } @@ -270,77 +274,3 @@ func wander() { time.Sleep(2 * time.Second) } } - -type steering struct { - left, right *ev3dev.TachoMotor -} - -func (s steering) steer(speed, counts int, dir float64) error { - if dir < -1 || 1 < dir || math.IsNaN(dir) { - return fmt.Errorf("direction out of range: %v", dir) - } - - var ls, rs, lcounts, rcounts int - switch { - case dir == 0: - ls = speed - rs = speed - lcounts = counts - rcounts = counts - case dir < 0: - rs = speed - rcounts = counts - dir = (dir + 0.5) * 2 - ls = int(math.Abs(dir * float64(speed))) - lcounts = int(float64(rcounts) * dir) - case dir > 0: - ls = speed - lcounts = counts - dir = (0.5 - dir) * 2 - rs = int(math.Abs(dir * float64(speed))) - rcounts = int(float64(lcounts) * dir) - } - - var err error - err = s.left. - SetSpeedSetpoint(ls). - SetPositionSetpoint(lcounts). - Err() - if err != nil { - return err - } - err = s.right. - SetSpeedSetpoint(rs). - SetPositionSetpoint(rcounts). - Err() - if err != nil { - return err - } - - err = s.left.Command("run-to-rel-pos").Err() - if err != nil { - return err - } - err = s.right.Command("run-to-rel-pos").Err() - if err != nil { - s.left.Command("stop").Err() - return err - } - var stat ev3dev.MotorState - var ok bool - stat, ok, err = ev3dev.Wait(s.left, ev3dev.Running, 0, 0, false, 5*time.Second) - if err != nil { - log.Fatalf("failed to wait for left motor to stop: %v", err) - } - if !ok { - log.Fatalf("failed to wait for left motor to stop: %v", stat) - } - stat, ok, err = ev3dev.Wait(s.right, ev3dev.Running, 0, 0, false, 5*time.Second) - if err != nil { - log.Fatalf("failed to wait for right motor to stop: %v", err) - } - if !ok { - log.Fatalf("failed to wait for right motor to stop: %v", stat) - } - return nil -} diff --git a/motorutil/steer.go b/motorutil/steer.go new file mode 100644 index 0000000..a0251f4 --- /dev/null +++ b/motorutil/steer.go @@ -0,0 +1,232 @@ +// Copyright ©2016 The ev3go Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package motorutil + +import ( + "fmt" + "math" + "sync" + "time" + + "github.com/ev3go/ev3dev" +) + +// Steering implements a paired-motor steering unit similar to an EV-G steering block. +type Steering struct { + // Left and Right are the left and right motors to be + // used by the steering module. + Left, Right *ev3dev.TachoMotor + + // Timeout is the timeout for waiting for motors to + // return to a non-driving state. + // + // See ev3dev.Wait documentation for timeout behaviour. + Timeout time.Duration +} + +// SteerCounts steers in the given turn for the given tacho counts and at the +// specified speed. The valid range of turn is -100 (hard left) to +100 (hard right). +// If counts is negative, the turn will be made in reverse. The sign of speed is +// ignored. +// +// See the ev3dev.SetSpeedSetPoint and ev3dev.SetPositionSetPoint documentation for +// speed and count behaviour. +func (s Steering) SteerCounts(speed, turn, counts int) error { + if turn < -100 || 100 < turn { + return directionError(turn) + } + + // leftSpeed and rightSpeed may be signed here, + // but ev3dev ignores speed_sp for run-to-*-pos. + leftSpeed, leftCounts, rightSpeed, rightCounts := motorRates(speed, turn, counts) + + var err error + err = s.Left. + SetSpeedSetpoint(leftSpeed). + SetPositionSetpoint(leftCounts). + Err() + if err != nil { + return err + } + err = s.Right. + SetSpeedSetpoint(rightSpeed). + SetPositionSetpoint(rightCounts). + Err() + if err != nil { + return err + } + + err = s.Left.Command("run-to-rel-pos").Err() + if err != nil { + return err + } + err = s.Right.Command("run-to-rel-pos").Err() + if err != nil { + s.Left.Command("stop").Err() + return err + } + return nil +} + +// SteerDuration steers in the given turn for the given duration, d, and at the +// specified speed. The valid range of turn is -100 (hard left) to +100 (hard right). +// If speed is negative, the turn will be made in reverse. +// +// See the ev3dev.SetSpeedSetpoint and ev3dev.SetTimeSetpoint documentation for speed +// and duration behaviour. +func (s Steering) SteerDuration(speed, turn int, d time.Duration) error { + if turn < -100 || 100 < turn { + return directionError(turn) + } + if d < 0 { + return durationError(d) + } + + leftSpeed, _, rightSpeed, _ := motorRates(speed, turn, 0) + + var err error + err = s.Left. + SetSpeedSetpoint(leftSpeed). + SetTimeSetpoint(d). + Err() + if err != nil { + return err + } + err = s.Right. + SetSpeedSetpoint(rightSpeed). + SetTimeSetpoint(d). + Err() + if err != nil { + return err + } + + err = s.Left.Command("run-timed").Err() + if err != nil { + return err + } + err = s.Right.Command("run-timed").Err() + if err != nil { + s.Left.Command("stop").Err() + return err + } + return nil +} + +func motorRates(speed, turn, counts int) (leftSpeed, leftCounts, rightSpeed, rightCounts int) { + switch { + case turn == 0: + leftSpeed = speed + rightSpeed = speed + leftCounts = counts + rightCounts = counts + case turn < 0: + rightSpeed = speed + rightCounts = counts + turn = (turn + 50) * 2 + leftSpeed = (speed * turn) / 100 + leftCounts = (rightCounts * turn) / 100 + case turn > 0: + leftSpeed = speed + leftCounts = counts + turn = (50 - turn) * 2 + rightSpeed = (speed * turn) / 100 + rightCounts = (leftCounts * turn) / 100 + } + return leftSpeed, leftCounts, rightSpeed, rightCounts +} + +// Wait waits for the last steering operation to complete. A non-nil error will either +// implements the Cause method, which may be used to determine the underlying cause, or +// be an Errors holding errors that implement the Cause method. +func (s Steering) Wait() error { + var errors [2]error + + var wg sync.WaitGroup + for i, motor := range []struct { + side string + device *ev3dev.TachoMotor + }{ + {side: "left", device: s.Left}, + {side: "right", device: s.Right}, + } { + i := i + side := motor.side + device := motor.device + wg.Add(1) + go func() { + defer wg.Done() + stat, ok, err := ev3dev.Wait(s.Left, ev3dev.Running, 0, 0, false, s.Timeout) + if err != nil { + errors[i] = waitError{side: side, motor: device, cause: err} + } + if !ok { + errors[i] = waitError{side: side, motor: device, cause: timeoutError(s.Timeout), stat: stat} + } + }() + } + wg.Wait() + + switch { + case errors[0] != nil && errors[1] != nil: + return Errors(errors[:]) + case errors[0] != nil: + return errors[0] + case errors[1] != nil: + return errors[1] + } + return nil +} + +// directionError is a ev3dev.ValidFloat64Ranger error. +type directionError int + +var _ ev3dev.ValidRanger = directionError(0) + +func (e directionError) Error() string { + return fmt.Sprintf("motorutil: invalid turn: %d (must be in within -100 to 100)", e) +} + +func (e directionError) Range() (value, min, max int) { + return int(e), -100, 100 +} + +// durationError is a ev3dev.ValidDurationRanger error. +type durationError time.Duration + +var _ ev3dev.ValidDurationRanger = durationError(0) + +func (e durationError) Error() string { + return fmt.Sprintf("motorutil: invalid duration: %v (must be positive)", time.Duration(e)) +} + +func (e durationError) DurationRange() (value, min, max time.Duration) { + return time.Duration(e), 0, math.MaxInt64 +} + +// waitError is a Causer error. +type waitError struct { + side string + motor *ev3dev.TachoMotor + stat ev3dev.MotorState + cause error +} + +func (e waitError) Error() string { + if _, ok := e.cause.(timeoutError); ok { + return fmt.Sprintf("motorutil: failed to wait for %s motor (%v) to stop (state=%v): %v", e.side, e.motor, e.stat, e.cause) + } + return fmt.Sprintf("motorutil: failed to wait for %s motor (%v) to stop: %v", e.side, e.motor, e.cause) +} + +func (e waitError) Cause() error { return e.cause } + +// timeoutError is a timeout failure. +type timeoutError time.Duration + +func (e timeoutError) Error() string { + return fmt.Sprintf("motorutil: wait timed out: longer than %v", time.Duration(e)) +} + +func (e timeoutError) Timeout() bool { return true } diff --git a/motorutil/steer_test.go b/motorutil/steer_test.go new file mode 100644 index 0000000..a10097d --- /dev/null +++ b/motorutil/steer_test.go @@ -0,0 +1,94 @@ +// Copyright ©2017 The ev3go Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package motorutil + +import "testing" + +var motorRatesTests = []struct { + speed, turn, counts int + + wantLeftSpeed, wantLeftCounts int + wantRightSpeed, wantRightCounts int +}{ + { + speed: 100, turn: 0, counts: 10, + wantLeftSpeed: 100, wantLeftCounts: 10, + wantRightSpeed: 100, wantRightCounts: 10, + }, + { + speed: 100, turn: -25, counts: 10, + wantLeftSpeed: 50, wantLeftCounts: 5, + wantRightSpeed: 100, wantRightCounts: 10, + }, + { + speed: 100, turn: +25, counts: 10, + wantLeftSpeed: 100, wantLeftCounts: 10, + wantRightSpeed: 50, wantRightCounts: 5, + }, + { + speed: 100, turn: -50, counts: 10, + wantLeftSpeed: 0, wantLeftCounts: 0, + wantRightSpeed: 100, wantRightCounts: 10, + }, + { + speed: 100, turn: +50, counts: 10, + wantLeftSpeed: 100, wantLeftCounts: 10, + wantRightSpeed: 0, wantRightCounts: 0, + }, + { + speed: 100, turn: -75, counts: 10, + wantLeftSpeed: -50, wantLeftCounts: -5, + wantRightSpeed: 100, wantRightCounts: 10, + }, + { + speed: 100, turn: +75, counts: 10, + wantLeftSpeed: 100, wantLeftCounts: 10, + wantRightSpeed: -50, wantRightCounts: -5, + }, + { + speed: 100, turn: -100, counts: 10, + wantLeftSpeed: -100, wantLeftCounts: -10, + wantRightSpeed: 100, wantRightCounts: 10, + }, + { + speed: 100, turn: +100, counts: 10, + wantLeftSpeed: 100, wantLeftCounts: 10, + wantRightSpeed: -100, wantRightCounts: -10, + }, +} + +func TestMotorRates(t *testing.T) { + for _, speedDirection := range []int{1, -1} { + for _, countDirection := range []int{1, 0, -1} { + for _, test := range motorRatesTests { + test.counts *= countDirection + test.speed *= speedDirection + + leftSpeed, leftCounts, rightSpeed, rightCounts := motorRates(test.speed, test.turn, test.counts) + + if leftSpeed != test.wantLeftSpeed*speedDirection { + t.Errorf("unexpected left motor speed for speed=%d turn=%d counts=%d: got:%d want:%d", + test.speed, test.turn, test.counts, + leftSpeed, test.wantLeftSpeed*speedDirection) + } + if leftCounts != test.wantLeftCounts*countDirection { + t.Errorf("unexpected left motor counts for speed=%d turn=%d counts=%d: got:%d want:%d", + test.speed, test.turn, test.counts, + leftCounts, test.wantLeftCounts*countDirection) + } + if rightSpeed != test.wantRightSpeed*speedDirection { + t.Errorf("unexpected right motor speed for speed=%d turn=%d counts=%d: got:%d want:%d", + test.speed, test.turn, test.counts, + rightSpeed, test.wantRightSpeed*speedDirection) + } + if rightCounts != test.wantRightCounts*countDirection { + t.Errorf("unexpected right motor counts for speed=%d turn=%d counts=%d: got:%d want:%d", + test.speed, test.turn, test.counts, + rightCounts, test.wantRightCounts*countDirection) + } + } + } + } +}