Skip to content

Commit

Permalink
[RSDK-8272] use the current power to determine the starting power of …
Browse files Browse the repository at this point in the history
…goForInternal (viamrobotics#4214)
  • Loading branch information
JohnN193 authored Jul 23, 2024
1 parent f301d32 commit 7ee5b7e
Show file tree
Hide file tree
Showing 8 changed files with 63 additions and 14 deletions.
4 changes: 4 additions & 0 deletions components/base/sensorcontrolled/sensorcontrolled.go
Original file line number Diff line number Diff line change
Expand Up @@ -227,6 +227,10 @@ func (sb *sensorBase) Stop(ctx context.Context, extra map[string]interface{}) er
sb.opMgr.CancelRunning(ctx)
if sb.loop != nil {
sb.loop.Pause()
// update pid controllers to be an at rest state
if err := sb.updateControlConfig(ctx, 0, 0); err != nil {
return err
}
}
return sb.controlledBase.Stop(ctx, extra)
}
Expand Down
2 changes: 1 addition & 1 deletion components/base/sensorcontrolled/velocities.go
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,12 @@ func (sb *sensorBase) SetVelocity(
return err
}
}
sb.loop.Resume()

// convert linear.Y mmPerSec to mPerSec, angular.Z is degPerSec
if err := sb.updateControlConfig(ctx, linear.Y/1000.0, angular.Z); err != nil {
return err
}
sb.loop.Resume()

return nil
}
Expand Down
13 changes: 11 additions & 2 deletions components/motor/gpio/controlled.go
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,15 @@ func (cm *controlledMotor) Stop(ctx context.Context, extra map[string]interface{
// is auto-tuning, the loop needs to keep running
if cm.loop != nil && !cm.loop.GetTuning(ctx) {
cm.loop.Pause()

// update pid controller to use the current state as the desired state
currentTicks, _, err := cm.enc.Position(ctx, encoder.PositionTypeTicks, extra)
if err != nil {
return err
}
if err := cm.updateControlBlock(ctx, currentTicks, cm.real.maxRPM*cm.ticksPerRotation/60); err != nil {
return err
}
}
return cm.real.Stop(ctx, nil)
}
Expand Down Expand Up @@ -279,14 +288,14 @@ func (cm *controlledMotor) SetRPM(ctx context.Context, rpm float64, extra map[st
}
}

cm.loop.Resume()
// set control loop values
velVal := math.Abs(rpm * cm.ticksPerRotation / 60)
goalPos := math.Inf(int(rpm))
// setPoint is +/- infinity, maxVel is calculated velVal
if err := cm.updateControlBlock(ctx, goalPos, velVal); err != nil {
return err
}
cm.loop.Resume()

return nil
}
Expand Down Expand Up @@ -321,7 +330,6 @@ func (cm *controlledMotor) GoFor(ctx context.Context, rpm, revolutions float64,
return err
}
}
cm.loop.Resume()

goalPos, _, _ := encodedGoForMath(rpm, revolutions, currentTicks, cm.ticksPerRotation)

Expand All @@ -332,6 +340,7 @@ func (cm *controlledMotor) GoFor(ctx context.Context, rpm, revolutions float64,
if err := cm.updateControlBlock(ctx, goalPos, velVal); err != nil {
return err
}
cm.loop.Resume()

if revolutions == 0 {
cm.logger.Warn("Deprecated: setting revolutions == 0 will spin the motor indefinitely at the specified RPM")
Expand Down
3 changes: 2 additions & 1 deletion components/motor/gpio/controlled_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,12 @@ func TestEncodedMotorControls(t *testing.T) {
}()

t.Run("encoded motor controls test loop exists", func(t *testing.T) {
test.That(t, cm.GoFor(context.Background(), 10, 1, nil), test.ShouldBeNil)
test.That(t, cm.SetRPM(context.Background(), 10, nil), test.ShouldBeNil)
testutils.WaitForAssertion(t, func(tb testing.TB) {
tb.Helper()
test.That(tb, cm.loop, test.ShouldNotBeNil)
})
test.That(t, cm.Stop(context.Background(), nil), test.ShouldBeNil)
})
}

Expand Down
19 changes: 18 additions & 1 deletion components/motor/gpio/encoded.go
Original file line number Diff line number Diff line change
Expand Up @@ -296,7 +296,13 @@ func (m *EncodedMotor) goForInternal(rpm, goalPos, direction float64) error {
m.activeBackgroundWorkers.Add(1)
go func() {
defer m.activeBackgroundWorkers.Done()
if err := m.real.SetPower(adjustmentsCtx, 0.2*direction, nil); err != nil {
_, lastPowerPct, err := m.real.IsPowered(adjustmentsCtx, nil)
if err != nil {
m.logger.Error(err)
return
}

if err := m.real.SetPower(adjustmentsCtx, calcStartingPower(lastPowerPct, direction), nil); err != nil {
m.logger.Error(err)
return
}
Expand All @@ -309,6 +315,17 @@ func (m *EncodedMotor) goForInternal(rpm, goalPos, direction float64) error {
return nil
}

func calcStartingPower(currPower, direction float64) float64 {
defaultPower := .2
if sign(currPower) != direction {
return defaultPower * direction
}
if math.Abs(currPower) < defaultPower {
return defaultPower * direction
}
return currPower
}

// GoTo instructs the motor to go to a specific position (provided in revolutions from home/zero),
// at a specific speed. Regardless of the directionality of the RPM this function will move the motor
// towards the specified target/position
Expand Down
14 changes: 8 additions & 6 deletions control/control_loop.go
Original file line number Diff line number Diff line change
Expand Up @@ -300,17 +300,19 @@ func (l *Loop) Stop() {

// Pause sets l.running to false to pause the loop.
func (l *Loop) Pause() {
for _, b := range l.pidBlocks {
if err := b.Reset(context.Background()); err != nil {
l.logger.Error(err)
return
}
}
l.running.Store(false)
}

// Resume sets l.running to true to resume the loop.
func (l *Loop) Resume() {
if !l.running.Load() {
for _, b := range l.pidBlocks {
if err := b.Reset(context.Background()); err != nil {
l.logger.Error(err)
return
}
}
}
l.running.Store(true)
}

Expand Down
4 changes: 4 additions & 0 deletions control/setup_control.go
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ import (
"context"
"sync"

"github.com/pkg/errors"
"go.uber.org/multierr"
"go.viam.com/utils"

Expand Down Expand Up @@ -458,6 +459,9 @@ func CreateTrapzBlock(ctx context.Context, name string, maxVel float64, dependsO

// UpdateTrapzBlock creates and sets a control config trapezoidalVelocityProfile block.
func UpdateTrapzBlock(ctx context.Context, name string, maxVel float64, dependsOn []string, loop *Loop) error {
if maxVel == 0 {
return errors.New("maxVel must be a non-zero value")
}
newTrapzBlock := CreateTrapzBlock(ctx, name, maxVel, dependsOn)
if err := loop.SetConfigAt(ctx, name, newTrapzBlock); err != nil {
return err
Expand Down
18 changes: 15 additions & 3 deletions control/trapezoid_velocity_profile.go
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ func newTrapezoidVelocityProfile(config BlockConfig, logger logging.Logger) (Blo
}

func (s *trapezoidVelocityGenerator) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]*Signal, bool) {
s.mu.Lock()
defer s.mu.Unlock()
var pos float64
var setPoint float64
if len(x) == 2 {
Expand All @@ -67,7 +69,6 @@ func (s *trapezoidVelocityGenerator) Next(ctx context.Context, x []*Signal, dt t
s.dir = 1
}
s.trapDistance = math.Abs(setPoint - pos)
s.lastVelCmd = 0
s.vDec = math.Min(math.Sqrt(s.trapDistance*s.maxAcc), s.maxVel)
s.kPP0 = 2.0 * s.maxAcc / s.vDec
s.kPP = s.kppGain * s.kPP0
Expand All @@ -94,6 +95,7 @@ func (s *trapezoidVelocityGenerator) Next(ctx context.Context, x []*Signal, dt t
s.lastVelCmd = vel
s.y[0].SetSignalValueAt(0, vel*float64(s.dir))
} else {
s.lastVelCmd = 0
s.y[0].SetSignalValueAt(0, 0.0)
s.currentPhase = rest
}
Expand All @@ -107,8 +109,14 @@ func (s *trapezoidVelocityGenerator) reset() error {
if !s.cfg.Attribute.Has("max_vel") {
return errors.Errorf("trapezoidale velocity profile block %s needs max_vel field", s.cfg.Name)
}
s.maxAcc = s.cfg.Attribute["max_acc"].(float64) // default 0.0
s.maxVel = s.cfg.Attribute["max_vel"].(float64) // default 0.0
s.maxAcc = s.cfg.Attribute["max_acc"].(float64)
s.maxVel = s.cfg.Attribute["max_vel"].(float64)
if s.maxAcc == 0 { // default 1.0, the math breaks if maxAcc = 0
s.maxAcc = 1
}
if s.maxVel == 0 { // default 1.0, the math breaks if maxVel = 0
s.maxVel = 1
}

s.posWindow = 0
if s.cfg.Attribute.Has("pos_window") {
Expand All @@ -122,6 +130,10 @@ func (s *trapezoidVelocityGenerator) reset() error {
if s.kppGain == 0 {
s.kppGain = 0.45
}
s.lastVelCmd = 0
if s.y != nil {
s.lastVelCmd = s.y[0].GetSignalValueAt(0)
}

s.lastsetPoint = math.NaN()

Expand Down

0 comments on commit 7ee5b7e

Please sign in to comment.