diff --git a/components/base/sensorcontrolled/movestraight.go b/components/base/sensorcontrolled/movestraight.go index ad1c3b8551c..a6307f7ac4d 100644 --- a/components/base/sensorcontrolled/movestraight.go +++ b/components/base/sensorcontrolled/movestraight.go @@ -34,7 +34,7 @@ func (sb *sensorBase) MoveStraight( // If a position movement sensor or controls are not configured, we cannot use this MoveStraight method. // Instead we need to use the MoveStraight method of the base that the sensorcontrolled base wraps. // If there is no valid velocity sensor, there won't be a controlLoopConfig. - if len(sb.controlLoopConfig.Blocks) == 0 { + if sb.controlLoopConfig == nil { sb.logger.CWarnf(ctx, "control loop not configured, using base %s's MoveStraight", sb.controlledBase.Name().ShortName()) @@ -54,6 +54,11 @@ func (sb *sensorBase) MoveStraight( } } + // check tuning status + if err := sb.checkTuningStatus(); err != nil { + return err + } + // make sure the control loop is enabled if sb.loop == nil { if err := sb.startControlLoop(); err != nil { diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go index 1e08ef3cb87..9139c9315d3 100644 --- a/components/base/sensorcontrolled/sensorcontrolled.go +++ b/components/base/sensorcontrolled/sensorcontrolled.go @@ -3,6 +3,7 @@ package sensorcontrolled import ( "context" + "fmt" "sync" "time" @@ -26,6 +27,7 @@ const ( typeLinVel = "linear_velocity" typeAngVel = "angular_velocity" defaultControlFreq = 10 // Hz + getPID = "get_tuned_pid" ) var ( @@ -48,13 +50,20 @@ func (cfg *Config) Validate(path string) ([]string, error) { if len(cfg.MovementSensor) == 0 { return nil, resource.NewConfigValidationError(path, errors.New("need at least one movement sensor for base")) } - deps = append(deps, cfg.MovementSensor...) + if cfg.Base == "" { return nil, resource.NewConfigValidationFieldRequiredError(path, "base") } - deps = append(deps, cfg.Base) + + for _, pidConf := range cfg.ControlParameters { + if pidConf.Type != typeLinVel && pidConf.Type != typeAngVel { + return nil, resource.NewConfigValidationError(path, + errors.New("control_parameters type must be 'linear_velocity' or 'angular_velocity'")) + } + } + return deps, nil } @@ -75,9 +84,11 @@ type sensorBase struct { // headingFunc returns the current angle between (-180,180) and whether Spin is supported headingFunc func(ctx context.Context) (float64, bool, error) - controlLoopConfig control.Config + controlLoopConfig *control.Config blockNames map[string][]string loop *control.Loop + configPIDVals []control.PIDConfig + tunedVals *[]control.PIDConfig controlFreq float64 } @@ -95,9 +106,11 @@ func createSensorBase( logger logging.Logger, ) (base.Base, error) { sb := &sensorBase{ - logger: logger, - Named: conf.ResourceName().AsNamed(), - opMgr: operation.NewSingleOperationManager(), + logger: logger, + tunedVals: &[]control.PIDConfig{{}, {}}, + configPIDVals: []control.PIDConfig{{}, {}}, + Named: conf.ResourceName().AsNamed(), + opMgr: operation.NewSingleOperationManager(), } if err := sb.Reconfigure(ctx, deps, conf); err != nil { @@ -195,26 +208,28 @@ func (sb *sensorBase) Reconfigure(ctx context.Context, deps resource.Dependencie if sb.velocities != nil && len(newConf.ControlParameters) != 0 { // assign linear and angular PID correctly based on the given type - var linear, angular control.PIDConfig - for _, c := range newConf.ControlParameters { - switch c.Type { + for _, pidConf := range newConf.ControlParameters { + switch pidConf.Type { case typeLinVel: - linear = c + // configPIDVals at index 0 is linear + sb.configPIDVals[0] = pidConf case typeAngVel: - angular = c + // configPIDVals at index 1 is angular + sb.configPIDVals[1] = pidConf default: - sb.logger.Warn("control_parameters type must be 'linear_velocity' or 'angular_velocity'") + return fmt.Errorf("control_parameters type '%v' not accepted, type must be 'linear_velocity' or 'angular_velocity'", + pidConf.Type) } } // unlock the mutex before setting up the control loop so that the motors // are not locked, and can run if any auto-tuning is necessary sb.mu.Unlock() - if err := sb.setupControlLoop(linear, angular); err != nil { + if err := sb.setupControlLoop(sb.configPIDVals[0], sb.configPIDVals[1]); err != nil { sb.mu.Lock() return err } - // relock the mutex after setting up the control loop since there is still a defer unlock + // relock the mutex after setting up the control loop since there is still a defer unlock sb.mu.Lock() } @@ -255,6 +270,25 @@ func (sb *sensorBase) Geometries(ctx context.Context, extra map[string]interface return sb.controlledBase.Geometries(ctx, extra) } +func (sb *sensorBase) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) { + resp := make(map[string]interface{}) + + sb.mu.Lock() + defer sb.mu.Unlock() + ok := req[getPID].(bool) + if ok { + var respStr string + for _, pidConf := range *sb.tunedVals { + if !pidConf.NeedsAutoTuning() { + respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", pidConf.P, pidConf.I, pidConf.D, pidConf.Type) + } + } + resp[getPID] = respStr + } + + return resp, nil +} + func (sb *sensorBase) Close(ctx context.Context) error { if err := sb.Stop(ctx, nil); err != nil { return err @@ -314,3 +348,15 @@ func (sb *sensorBase) determineHeadingFunc(ctx context.Context, } } } + +// if loop is tuning, return an error +// if loop has been tuned but the values haven't been added to the config, error with tuned values. +func (sb *sensorBase) checkTuningStatus() error { + if sb.loop != nil && sb.loop.GetTuning(context.Background()) { + return control.TuningInProgressErr(sb.Name().ShortName()) + } else if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) || + (sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) { + return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals) + } + return nil +} diff --git a/components/base/sensorcontrolled/sensorcontrolled_test.go b/components/base/sensorcontrolled/sensorcontrolled_test.go index 5bb30c669fc..d79e3d28c32 100644 --- a/components/base/sensorcontrolled/sensorcontrolled_test.go +++ b/components/base/sensorcontrolled/sensorcontrolled_test.go @@ -3,6 +3,7 @@ package sensorcontrolled import ( "context" "errors" + "fmt" "strings" "sync" "testing" @@ -28,6 +29,8 @@ const ( // compassValue and orientationValue should be different for tests. defaultCompassValue = 45. defaultOrientationValue = 40. + wrongTypeLinVel = "linear" + wrongTypeAngVel = "angulr_velocity" ) var ( @@ -145,16 +148,16 @@ func TestSensorBase(t *testing.T) { test.That(t, sb.Close(ctx), test.ShouldBeNil) } -func sBaseTestConfig(msNames []string, freq float64) resource.Config { +func sBaseTestConfig(msNames []string, freq float64, linType, angType string) resource.Config { controlParams := make([]control.PIDConfig, 2) controlParams[0] = control.PIDConfig{ - Type: typeLinVel, + Type: linType, P: 0.5, I: 0.5, D: 0.0, } controlParams[1] = control.PIDConfig{ - Type: typeAngVel, + Type: angType, P: 0.5, I: 0.5, D: 0.0, @@ -177,7 +180,7 @@ func msDependencies(t *testing.T, msNames []string, ) (resource.Dependencies, resource.Config) { t.Helper() - cfg := sBaseTestConfig(msNames, defaultControlFreq) + cfg := sBaseTestConfig(msNames, defaultControlFreq, typeLinVel, typeAngVel) deps := make(resource.Dependencies) @@ -279,7 +282,7 @@ func TestReconfig(t *testing.T) { deps, _ = msDependencies(t, []string{"setvel2"}) // generate a config with a non default freq - cfg = sBaseTestConfig([]string{"setvel2"}, 100) + cfg = sBaseTestConfig([]string{"setvel2"}, 100, typeLinVel, typeAngVel) err = b.Reconfigure(ctx, deps, cfg) test.That(t, err, test.ShouldBeNil) test.That(t, sb.velocities.Name().ShortName(), test.ShouldResemble, "setvel2") @@ -336,6 +339,12 @@ func TestReconfig(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, headingSupported, test.ShouldBeFalse) test.That(t, headingBad, test.ShouldEqual, 0) + + deps, _ = msDependencies(t, []string{"setvel2"}) + // generate a config with invalid pid types + cfg = sBaseTestConfig([]string{"setvel2"}, 100, wrongTypeLinVel, wrongTypeAngVel) + err = b.Reconfigure(ctx, deps, cfg) + test.That(t, err.Error(), test.ShouldContainSubstring, "type must be 'linear_velocity' or 'angular_velocity'") } func TestSensorBaseWithVelocitiesSensor(t *testing.T) { @@ -343,7 +352,7 @@ func TestSensorBaseWithVelocitiesSensor(t *testing.T) { logger := logging.NewTestLogger(t) deps, _ := msDependencies(t, []string{"setvel1"}) // generate a config with a non default freq - cfg := sBaseTestConfig([]string{"setvel1"}, 100) + cfg := sBaseTestConfig([]string{"setvel1"}, 100, typeLinVel, typeAngVel) b, err := createSensorBase(ctx, deps, cfg, logger) test.That(t, err, test.ShouldBeNil) @@ -511,3 +520,32 @@ func TestSensorBaseMoveStraight(t *testing.T) { orientationValue = defaultOrientationValue }) } + +func TestSensorBaseDoCommand(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + deps, cfg := msDependencies(t, []string{"setvel1", "position1", "orientation1"}) + b, err := createSensorBase(ctx, deps, cfg, logger) + test.That(t, err, test.ShouldBeNil) + + sb, ok := b.(*sensorBase) + test.That(t, ok, test.ShouldBeTrue) + + expectedPID := control.PIDConfig{P: 0.1, I: 2.0, D: 0.0} + sb.tunedVals = &[]control.PIDConfig{expectedPID, {}} + expectedeMap := make(map[string]interface{}) + expectedeMap["get_tuned_pid"] = (fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", + expectedPID.P, expectedPID.I, expectedPID.D, expectedPID.Type)) + + req := make(map[string]interface{}) + req["get_tuned_pid"] = true + resp, err := b.DoCommand(ctx, req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp, test.ShouldResemble, expectedeMap) + + emptyMap := make(map[string]interface{}) + req["get_tuned_pid"] = false + resp, err = b.DoCommand(ctx, req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp, test.ShouldResemble, emptyMap) +} diff --git a/components/base/sensorcontrolled/spin.go b/components/base/sensorcontrolled/spin.go index 5fc33770b26..644c0613ec4 100644 --- a/components/base/sensorcontrolled/spin.go +++ b/components/base/sensorcontrolled/spin.go @@ -27,11 +27,16 @@ func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, ex // If an orientation movement sensor or controls are not configured, we cannot use this Spin method. // Instead we need to use the Spin method of the base that the sensorBase wraps. // If there is no valid velocity sensor, there won't be a controlLoopConfig. - if len(sb.controlLoopConfig.Blocks) == 0 { + if sb.controlLoopConfig == nil { sb.logger.CWarnf(ctx, "control parameters not configured, using %v's Spin method", sb.controlledBase.Name().ShortName()) return sb.controlledBase.Spin(ctx, angleDeg, degsPerSec, extra) } + // check tuning status + if err := sb.checkTuningStatus(); err != nil { + return err + } + prevAngle, hasOrientation, err := sb.headingFunc(ctx) if err != nil { return err diff --git a/components/base/sensorcontrolled/velocities.go b/components/base/sensorcontrolled/velocities.go index c36bf09a7d4..513cdd306bf 100644 --- a/components/base/sensorcontrolled/velocities.go +++ b/components/base/sensorcontrolled/velocities.go @@ -19,11 +19,16 @@ func (sb *sensorBase) SetVelocity( ctx, done := sb.opMgr.New(ctx) defer done() - if len(sb.controlLoopConfig.Blocks) == 0 { + if sb.controlLoopConfig == nil { sb.logger.CWarnf(ctx, "control parameters not configured, using %v's SetVelocity method", sb.controlledBase.Name().ShortName()) return sb.controlledBase.SetVelocity(ctx, linear, angular, extra) } + // check tuning status + if err := sb.checkTuningStatus(); err != nil { + return err + } + // make sure the control loop is enabled if sb.loop == nil { if err := sb.startControlLoop(); err != nil { @@ -43,7 +48,7 @@ func (sb *sensorBase) SetVelocity( // startControlLoop uses the control config to initialize a control loop and store it on the sensor controlled base struct. // The sensor base is the controllable interface that implements State and GetState called from the endpoint block of the control loop. func (sb *sensorBase) startControlLoop() error { - loop, err := control.NewLoop(sb.logger, sb.controlLoopConfig, sb) + loop, err := control.NewLoop(sb.logger, *sb.controlLoopConfig, sb) if err != nil { return err } @@ -80,6 +85,7 @@ func (sb *sensorBase) setupControlLoop(linear, angular control.PIDConfig) error sb.controlLoopConfig = pl.ControlConf sb.loop = pl.ControlLoop sb.blockNames = pl.BlockNames + sb.tunedVals = pl.TunedVals return nil } diff --git a/components/motor/gpio/controlled.go b/components/motor/gpio/controlled.go index 06d74a8d77e..690d81f2117 100644 --- a/components/motor/gpio/controlled.go +++ b/components/motor/gpio/controlled.go @@ -2,6 +2,7 @@ package gpio import ( "context" + "fmt" "math" "sync" "time" @@ -18,6 +19,8 @@ import ( rdkutils "go.viam.com/rdk/utils" ) +const getPID = "get_tuned_pid" + // SetState sets the state of the motor for the built-in control loop. func (cm *controlledMotor) SetState(ctx context.Context, state []*control.Signal) error { if cm.loop != nil && !cm.loop.Running() { @@ -58,7 +61,7 @@ func (cm *controlledMotor) setupControlLoop(conf *Config) error { } // convert the motor config ControlParameters to the control.PIDConfig structure for use in setup_control.go - convertedControlParams := []control.PIDConfig{{ + cm.configPIDVals = []control.PIDConfig{{ Type: "", P: conf.ControlParameters.P, I: conf.ControlParameters.I, @@ -67,18 +70,19 @@ func (cm *controlledMotor) setupControlLoop(conf *Config) error { // auto tune motor if all ControlParameters are 0 // since there's only one set of PID values for a motor, they will always be at convertedControlParams[0] - if convertedControlParams[0].NeedsAutoTuning() { + if cm.configPIDVals[0].NeedsAutoTuning() { options.NeedsAutoTuning = true } - pl, err := control.SetupPIDControlConfig(convertedControlParams, cm.Name().ShortName(), options, cm, cm.logger) + pl, err := control.SetupPIDControlConfig(cm.configPIDVals, cm.Name().ShortName(), options, cm, cm.logger) if err != nil { return err } - cm.controlLoopConfig = pl.ControlConf + cm.controlLoopConfig = *pl.ControlConf cm.loop = pl.ControlLoop cm.blockNames = pl.BlockNames + cm.tunedVals = pl.TunedVals return nil } @@ -117,6 +121,7 @@ func setupMotorWithControls( Named: cfg.ResourceName().AsNamed(), logger: logger, opMgr: operation.NewSingleOperationManager(), + tunedVals: &[]control.PIDConfig{{}}, ticksPerRotation: tpr, real: m, enc: enc, @@ -150,6 +155,8 @@ type controlledMotor struct { controlLoopConfig control.Config blockNames map[string][]string loop *control.Loop + configPIDVals []control.PIDConfig + tunedVals *[]control.PIDConfig } // SetPower sets the percentage of power the motor should employ between -1 and 1. @@ -281,6 +288,10 @@ func (cm *controlledMotor) SetRPM(ctx context.Context, rpm float64, extra map[st return err } + if err := cm.checkTuningStatus(); err != nil { + return err + } + if cm.loop == nil { // create new control loop if err := cm.startControlLoop(); err != nil { @@ -327,6 +338,10 @@ func (cm *controlledMotor) GoFor(ctx context.Context, rpm, revolutions float64, return err } + if err := cm.checkTuningStatus(); err != nil { + return err + } + if cm.loop == nil { // create new control loop if err := cm.startControlLoop(); err != nil { @@ -371,3 +386,32 @@ func (cm *controlledMotor) GoFor(ctx context.Context, rpm, revolutions float64, return nil } + +func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interface{}) (map[string]interface{}, error) { + resp := make(map[string]interface{}) + + cm.mu.Lock() + defer cm.mu.Unlock() + ok := req[getPID].(bool) + if ok { + var respStr string + if !(*cm.tunedVals)[0].NeedsAutoTuning() { + respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", + (*cm.tunedVals)[0].P, (*cm.tunedVals)[0].I, (*cm.tunedVals)[0].D, (*cm.tunedVals)[0].Type) + } + resp[getPID] = respStr + } + + return resp, nil +} + +// if loop is tuning, return an error +// if loop has been tuned but the values haven't been added to the config, error with tuned values. +func (cm *controlledMotor) checkTuningStatus() error { + if cm.loop != nil && cm.loop.GetTuning(context.Background()) { + return control.TuningInProgressErr(cm.Name().ShortName()) + } else if cm.configPIDVals[0].NeedsAutoTuning() && !(*cm.tunedVals)[0].NeedsAutoTuning() { + return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals) + } + return nil +} diff --git a/components/motor/gpio/controlled_test.go b/components/motor/gpio/controlled_test.go index 7696c1d78c4..9b40a805175 100644 --- a/components/motor/gpio/controlled_test.go +++ b/components/motor/gpio/controlled_test.go @@ -2,6 +2,7 @@ package gpio import ( "context" + "fmt" "testing" "go.viam.com/test" @@ -9,6 +10,7 @@ import ( "go.viam.com/rdk/components/board" "go.viam.com/rdk/components/encoder" + "go.viam.com/rdk/control" "go.viam.com/rdk/logging" "go.viam.com/rdk/operation" "go.viam.com/rdk/resource" @@ -99,4 +101,23 @@ func TestControlledMotorCreation(t *testing.T) { test.That(t, cm.enc.Name().ShortName(), test.ShouldEqual, encoderName) test.That(t, cm.real.Name().ShortName(), test.ShouldEqual, motorName) + + // test DoCommand + expectedPID := control.PIDConfig{P: 0.1, I: 2.0, D: 0.0} + cm.tunedVals = &[]control.PIDConfig{expectedPID, {}} + expectedeMap := make(map[string]interface{}) + expectedeMap["get_tuned_pid"] = (fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", + expectedPID.P, expectedPID.I, expectedPID.D, expectedPID.Type)) + + req := make(map[string]interface{}) + req["get_tuned_pid"] = true + resp, err := m.DoCommand(context.Background(), req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp, test.ShouldResemble, expectedeMap) + + emptyMap := make(map[string]interface{}) + req["get_tuned_pid"] = false + resp, err = cm.DoCommand(context.Background(), req) + test.That(t, err, test.ShouldBeNil) + test.That(t, resp, test.ShouldResemble, emptyMap) } diff --git a/control/control_loop.go b/control/control_loop.go index 5724182287f..68529db99ba 100644 --- a/control/control_loop.go +++ b/control/control_loop.go @@ -208,6 +208,15 @@ func (l *Loop) BlockList(ctx context.Context) ([]string, error) { return out, nil } +// GetPIDVals returns the tuned PID values. +func (l *Loop) GetPIDVals(pidIndex int) PIDConfig { + return PIDConfig{ + P: l.pidBlocks[pidIndex].kP, + I: l.pidBlocks[pidIndex].kI, + D: l.pidBlocks[pidIndex].kD, + } +} + // Frequency returns the loop's frequency. func (l *Loop) Frequency(ctx context.Context) (float64, error) { return l.cfg.Frequency, nil diff --git a/control/control_loop_test.go b/control/control_loop_test.go index 810071c4325..a3fbdfce83f 100644 --- a/control/control_loop_test.go +++ b/control/control_loop_test.go @@ -327,6 +327,11 @@ func TestControlLoop(t *testing.T) { } func TestMultiSignalLoop(t *testing.T) { + expectedPIDVals := PIDConfig{ + P: 10.0, + I: 0.2, + D: 0.5, + } logger := logging.NewTestLogger(t) cfg := Config{ Blocks: []BlockConfig{ @@ -342,9 +347,9 @@ func TestMultiSignalLoop(t *testing.T) { Name: "pid_block", Type: "PID", Attribute: utils.AttributeMap{ - "kP": 10.0, // random for now - "kD": 0.5, - "kI": 0.2, + "kP": expectedPIDVals.P, // random for now + "kD": expectedPIDVals.D, + "kI": expectedPIDVals.I, }, DependsOn: []string{"gain_block"}, }, @@ -381,5 +386,8 @@ func TestMultiSignalLoop(t *testing.T) { cLoop.Start() test.That(t, err, test.ShouldBeNil) + pidVals := cLoop.GetPIDVals(0) + test.That(t, pidVals, test.ShouldResemble, expectedPIDVals) + cLoop.Stop() } diff --git a/control/setup_control.go b/control/setup_control.go index a5ca0b22848..eba74b25c9d 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -2,6 +2,7 @@ package control import ( "context" + "fmt" "sync" "github.com/pkg/errors" @@ -36,7 +37,8 @@ var ( type PIDLoop struct { BlockNames map[string][]string PIDVals []PIDConfig - ControlConf Config + TunedVals *[]PIDConfig + ControlConf *Config ControlLoop *Loop Options Options Controllable Controllable @@ -100,15 +102,16 @@ func SetupPIDControlConfig( pidLoop := &PIDLoop{ Controllable: c, PIDVals: pidVals, + TunedVals: &[]PIDConfig{{}, {}}, logger: logger, Options: options, - ControlConf: Config{}, + ControlConf: &Config{}, ControlLoop: nil, } // set controlConf as either an optional custom config, or as the default control config if options.UseCustomConfig { - pidLoop.ControlConf = options.CompleteCustomConfig + *pidLoop.ControlConf = options.CompleteCustomConfig for i, b := range options.CompleteCustomConfig.Blocks { if b.Type == blockSum { sumIndex = i @@ -163,6 +166,10 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc p.ControlLoop.MonitorTuning(ctx) + tunedPID := p.ControlLoop.GetPIDVals(0) + tunedPID.Type = p.PIDVals[0].Type + (*p.TunedVals)[0] = tunedPID + p.ControlLoop.Stop() p.ControlLoop = nil } @@ -170,7 +177,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc // check if linear needs to be tuned if p.PIDVals[0].NeedsAutoTuning() { p.logger.Info("tuning linear PID") - if err := p.tuneSinglePID(ctx, angularPIDIndex); err != nil { + if err := p.tuneSinglePID(ctx, angularPIDIndex, 0); err != nil { errs = multierr.Combine(errs, err) } } @@ -178,7 +185,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc // check if angular needs to be tuned if p.PIDVals[1].NeedsAutoTuning() { p.logger.Info("tuning angular PID") - if err := p.tuneSinglePID(ctx, linearPIDIndex); err != nil { + if err := p.tuneSinglePID(ctx, linearPIDIndex, 1); err != nil { errs = multierr.Combine(errs, err) } } @@ -187,7 +194,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc return errs } -func (p *PIDLoop) tuneSinglePID(ctx context.Context, blockIndex int) error { +func (p *PIDLoop) tuneSinglePID(ctx context.Context, blockIndex, pidIndex int) error { // preserve old values and set them to be non-zero pOld := p.ControlConf.Blocks[blockIndex].Attribute["kP"] iOld := p.ControlConf.Blocks[blockIndex].Attribute["kI"] @@ -199,6 +206,9 @@ func (p *PIDLoop) tuneSinglePID(ctx context.Context, blockIndex int) error { } p.ControlLoop.MonitorTuning(ctx) + tunedPID := p.ControlLoop.GetPIDVals(pidIndex) + tunedPID.Type = p.PIDVals[pidIndex].Type + (*p.TunedVals)[pidIndex] = tunedPID p.ControlLoop.Stop() p.ControlLoop = nil @@ -246,7 +256,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con if p.Options.LoopFrequency != 0.0 { loopFrequency = p.Options.LoopFrequency } - p.ControlConf = Config{ + *p.ControlConf = Config{ Blocks: []BlockConfig{ { Name: "set_point", @@ -409,7 +419,7 @@ func (p *PIDLoop) addSensorFeedbackVelocityControl(angularPIDVals PIDConfig) { // StartControlLoop starts a PID control loop. func (p *PIDLoop) StartControlLoop() error { - loop, err := NewLoop(p.logger, p.ControlConf, p.Controllable) + loop, err := NewLoop(p.logger, *p.ControlConf, p.Controllable) if err != nil { return err } @@ -468,3 +478,19 @@ func UpdateTrapzBlock(ctx context.Context, name string, maxVel float64, dependsO } return nil } + +// TunedPIDErr returns an error with the stored tuned PID values. +func TunedPIDErr(name string, tunedVals []PIDConfig) error { + var tunedStr string + for _, pid := range tunedVals { + if !pid.NeedsAutoTuning() { + tunedStr += fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"} `, pid.P, pid.I, pid.D, pid.Type) + } + } + return fmt.Errorf(`%v has been tuned, please copy the following control values into your config: %v`, name, tunedStr) +} + +// TuningInProgressErr returns an error when the loop is actively tuning. +func TuningInProgressErr(name string) error { + return fmt.Errorf(`tuning for %v is in progress`, name) +}