diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go index 9139c9315d3..c721eee27cd 100644 --- a/components/base/sensorcontrolled/sensorcontrolled.go +++ b/components/base/sensorcontrolled/sensorcontrolled.go @@ -280,7 +280,7 @@ func (sb *sensorBase) DoCommand(ctx context.Context, req map[string]interface{}) 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) + respStr += pidConf.String() } } resp[getPID] = respStr @@ -352,11 +352,25 @@ 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()) { + done := true + needsTuning := false + + for i := range sb.configPIDVals { + // check if the current signal needed tuning + if sb.configPIDVals[i].NeedsAutoTuning() { + // return true if either signal needed tuning + needsTuning = needsTuning || true + // if the tunedVals have not been updated, then tuning is still in progress + done = done && !(*sb.tunedVals)[i].NeedsAutoTuning() + } + } + + if needsTuning { + if done { + return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals) + } 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 e7f7ec49cb7..263a9dac719 100644 --- a/components/base/sensorcontrolled/sensorcontrolled_test.go +++ b/components/base/sensorcontrolled/sensorcontrolled_test.go @@ -3,7 +3,6 @@ package sensorcontrolled import ( "context" "errors" - "fmt" "strings" "sync" "testing" @@ -535,8 +534,7 @@ func TestSensorBaseDoCommand(t *testing.T) { 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)) + expectedeMap["get_tuned_pid"] = (expectedPID.String()) req := make(map[string]interface{}) req["get_tuned_pid"] = true diff --git a/components/motor/gpio/controlled.go b/components/motor/gpio/controlled.go index 690d81f2117..62ef6b3e66b 100644 --- a/components/motor/gpio/controlled.go +++ b/components/motor/gpio/controlled.go @@ -2,7 +2,6 @@ package gpio import ( "context" - "fmt" "math" "sync" "time" @@ -396,8 +395,7 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa 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) + respStr += (*cm.tunedVals)[0].String() } resp[getPID] = respStr } @@ -408,10 +406,25 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa // 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()) { + done := true + needsTuning := false + + for i := range cm.configPIDVals { + // check if the current signal needed tuning + if cm.configPIDVals[i].NeedsAutoTuning() { + // return true if either signal needed tuning + needsTuning = needsTuning || true + // if the tunedVals have not been updated, then tuning is still in progress + done = done && !(*cm.tunedVals)[i].NeedsAutoTuning() + } + } + + if needsTuning { + if done { + return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals) + } 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 9b40a805175..bcfb542ea88 100644 --- a/components/motor/gpio/controlled_test.go +++ b/components/motor/gpio/controlled_test.go @@ -2,7 +2,6 @@ package gpio import ( "context" - "fmt" "testing" "go.viam.com/test" @@ -106,8 +105,7 @@ func TestControlledMotorCreation(t *testing.T) { 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)) + expectedeMap["get_tuned_pid"] = expectedPID.String() req := make(map[string]interface{}) req["get_tuned_pid"] = true diff --git a/control/control_loop.go b/control/control_loop.go index 68529db99ba..8d5637f069d 100644 --- a/control/control_loop.go +++ b/control/control_loop.go @@ -209,12 +209,9 @@ func (l *Loop) BlockList(ctx context.Context) ([]string, error) { } // GetPIDVals returns the tuned PID values. +// TODO: update this when MIMO fully supported. func (l *Loop) GetPIDVals(pidIndex int) PIDConfig { - return PIDConfig{ - P: l.pidBlocks[pidIndex].kP, - I: l.pidBlocks[pidIndex].kI, - D: l.pidBlocks[pidIndex].kD, - } + return *l.pidBlocks[pidIndex].PIDSets[0] } // Frequency returns the loop's frequency. @@ -227,7 +224,7 @@ func (l *Loop) Start() error { if len(l.ts) == 0 { return errors.New("cannot start the control loop if there are no blocks depending on an impulse") } - l.logger.Infof("Running loop on %1.4f %+v\r\n", l.cfg.Frequency, l.dt) + l.logger.Infof("Running control loop at %1.4f Hz, %+v\r\n", l.cfg.Frequency, l.dt) l.ct = controlTicker{ ticker: time.NewTicker(l.dt), stop: make(chan bool, 1), @@ -339,17 +336,29 @@ func (l *Loop) GetConfig(ctx context.Context) Config { func (l *Loop) MonitorTuning(ctx context.Context) { // wait until tuning has started for { - tuning := l.GetTuning(ctx) - if tuning { - break + // 100 Hz is probably faster than we need, but we needed at least a small delay because + // GetTuning will lock the PID block + if utils.SelectContextOrWait(ctx, 10*time.Millisecond) { + tuning := l.GetTuning(ctx) + if tuning { + break + } + continue } + l.logger.Error("error starting tuner") + return } // wait until tuning is done for { - tuning := l.GetTuning(ctx) - if !tuning { - break + if utils.SelectContextOrWait(ctx, 10*time.Millisecond) { + tuning := l.GetTuning(ctx) + if !tuning { + break + } + continue } + l.logger.Error("error waiting for tuner") + return } } diff --git a/control/control_loop_test.go b/control/control_loop_test.go index a3fbdfce83f..26e3c941f18 100644 --- a/control/control_loop_test.go +++ b/control/control_loop_test.go @@ -347,9 +347,7 @@ func TestMultiSignalLoop(t *testing.T) { Name: "pid_block", Type: "PID", Attribute: utils.AttributeMap{ - "kP": expectedPIDVals.P, // random for now - "kD": expectedPIDVals.D, - "kI": expectedPIDVals.I, + "PIDSets": []*PIDConfig{&expectedPIDVals}, }, DependsOn: []string{"gain_block"}, }, diff --git a/control/control_signal.go b/control/control_signal.go index 297ea41128f..8febd8d46bf 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -1,6 +1,8 @@ package control -import "sync" +import ( + "sync" +) // Signal holds any data passed between blocks. type Signal struct { @@ -23,11 +25,23 @@ func makeSignal(name string, blockType controlBlockType) *Signal { return &s } +// makeSignals returns a Signal object where the length of its signal[] array is dependent +// on the number of PIDSets from the config. +func makeSignals(name string, blockType controlBlockType, dimension int) *Signal { + var s Signal + s.dimension = dimension + s.signal = make([]float64, dimension) + s.time = make([]int, dimension) + s.name = name + s.blockType = blockType + return &s +} + // GetSignalValueAt returns the value of the signal at an index, threadsafe. func (s *Signal) GetSignalValueAt(i int) float64 { s.mu.Lock() defer s.mu.Unlock() - if i > len(s.signal)-1 { + if !(i < len(s.signal)) { return 0.0 } return s.signal[i] @@ -37,7 +51,7 @@ func (s *Signal) GetSignalValueAt(i int) float64 { func (s *Signal) SetSignalValueAt(i int, val float64) { s.mu.Lock() defer s.mu.Unlock() - if i > len(s.signal)-1 { + if !(i < len(s.signal)) { return } s.signal[i] = val diff --git a/control/pid.go b/control/pid.go index d11e42bb111..3f268735f0b 100644 --- a/control/pid.go +++ b/control/pid.go @@ -22,25 +22,39 @@ func (l *Loop) newPID(config BlockConfig, logger logging.Logger) (Block, error) // BasicPID is the standard implementation of a PID controller. type basicPID struct { - mu sync.Mutex - cfg BlockConfig - error float64 - kI float64 - kD float64 - kP float64 - int float64 + mu sync.Mutex + cfg BlockConfig + logger logging.Logger + + // MIMO gains + state + PIDSets []*PIDConfig + tuners []*pidTuner + + // used by both y []*Signal satLimUp float64 `default:"255.0"` limUp float64 `default:"255.0"` satLimLo float64 limLo float64 - tuner pidTuner - tuning bool - logger logging.Logger } +// GetTuning returns whether the PID block is currently tuning any signals. func (p *basicPID) GetTuning() bool { - return p.tuning + // using locks to prevent reading from tuners while the object is being modified + p.mu.Lock() + defer p.mu.Unlock() + return p.getTuning() +} + +func (p *basicPID) getTuning() bool { + multiTune := false + for _, tuner := range p.tuners { + // the tuners for MIMO are only initialized when we want to tune + if tuner != nil { + multiTune = tuner.tuning || multiTune + } + } + return multiTune } // Output returns the discrete step of the PID controller, dt is the delta time between two subsequent call, @@ -49,57 +63,89 @@ func (p *basicPID) GetTuning() bool { func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]*Signal, bool) { p.mu.Lock() defer p.mu.Unlock() - if p.tuning { - out, done := p.tuner.pidTunerStep(math.Abs(x[0].GetSignalValueAt(0)), p.logger) - if done { - p.kD = p.tuner.kD - p.kI = p.tuner.kI - p.kP = p.tuner.kP - p.logger.Info("\n\n-------- ***** PID GAINS CALCULATED **** --------") - p.logger.CInfof(ctx, "Calculated gains are p: %1.6f, i: %1.6f, d: %1.6f", p.kP, p.kI, p.kD) - p.logger.CInfof(ctx, "You must MANUALLY ADD p, i and d gains to the robot config to use the values after tuning\n\n") - p.tuning = false + if p.getTuning() { + // Multi Input/Output Implementation + + // For each PID Set and its respective Tuner Object, Step through an iteration of tuning until done. + for i := 0; i < len(p.PIDSets); i++ { + // if we do not need to tune this signal, skip to the next signal + if !p.tuners[i].tuning { + continue + } + out, done := p.tuners[i].pidTunerStep(math.Abs(x[0].GetSignalValueAt(i)), p.logger) + if done { + p.PIDSets[i].D = p.tuners[i].kD + p.PIDSets[i].I = p.tuners[i].kI + p.PIDSets[i].P = p.tuners[i].kP + p.logger.Info("\n\n-------- ***** PID GAINS CALCULATED **** --------") + p.logger.CInfof(ctx, "Calculated gains for signal %v are p: %1.6f, i: %1.6f, d: %1.6f", + i, p.PIDSets[i].P, p.PIDSets[i].I, p.PIDSets[i].D) + p.logger.CInfof(ctx, "You must MANUALLY ADD p, i and d gains to the robot config to use the values after tuning\n\n") + p.tuners[i].tuning = false + } + p.y[0].SetSignalValueAt(i, out) + // return early to only step this signal + return p.y, true } - p.y[0].SetSignalValueAt(0, out) } else { - dtS := dt.Seconds() - pvError := x[0].GetSignalValueAt(0) - p.int += p.kI * pvError * dtS - switch { - case p.int >= p.satLimUp: - p.int = p.satLimUp - case p.int <= p.satLimLo: - p.int = p.satLimLo - default: - } - deriv := (pvError - p.error) / dtS - output := p.kP*pvError + p.int + p.kD*deriv - p.error = pvError - if output > p.limUp { - output = p.limUp - } else if output < p.limLo { - output = p.limLo + for i := 0; i < len(p.PIDSets); i++ { + output := calculateSignalValue(p, x, dt, i) + p.y[0].SetSignalValueAt(i, output) } - p.y[0].SetSignalValueAt(0, output) } return p.y, true } +// For a given signal, compute new signal value based on current signal value, & its respective error. +func calculateSignalValue(p *basicPID, x []*Signal, dt time.Duration, sIndex int) float64 { + dtS := dt.Seconds() + pvError := x[0].GetSignalValueAt(sIndex) + p.PIDSets[sIndex].int += p.PIDSets[sIndex].I * pvError * dtS + + switch { + case p.PIDSets[sIndex].int >= p.satLimUp: + p.PIDSets[sIndex].int = p.satLimUp + case p.PIDSets[sIndex].int <= p.satLimLo: + p.PIDSets[sIndex].int = p.satLimLo + default: + } + deriv := (pvError - p.PIDSets[sIndex].signalErr) / dtS + output := p.PIDSets[sIndex].P*pvError + p.PIDSets[sIndex].int + p.PIDSets[sIndex].D*deriv + p.PIDSets[sIndex].signalErr = pvError + if output > p.limUp { + output = p.limUp + } else if output < p.limLo { + output = p.limLo + } + + return output +} + func (p *basicPID) reset() error { - p.int = 0 - p.error = 0 + var ok bool - if !p.cfg.Attribute.Has("kI") && - !p.cfg.Attribute.Has("kD") && - !p.cfg.Attribute.Has("kP") { - return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name) + // Each PIDSet is taken from the config, if the attribute exists (it's optional). + // If PID Sets was given as an attribute, we know we're in 'multi' mode. For each + // set of PIDs we initialize its values to 0 and create a tuner object. + if p.cfg.Attribute.Has("PIDSets") { + p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig) + if !ok { + return errors.New("PIDSet did not initialize correctly") + } + if len(p.PIDSets) > 0 { + p.tuners = make([]*pidTuner, len(p.PIDSets)) + for i := 0; i < len(p.PIDSets); i++ { + p.PIDSets[i].int = 0 + p.PIDSets[i].signalErr = 0 + } + } + } else { + return errors.Errorf("pid block %s does not have a PID configured", p.cfg.Name) } - if len(p.cfg.DependsOn) != 1 { - return errors.Errorf("pid block %s should have 1 input got %d", p.cfg.Name, len(p.cfg.DependsOn)) + + if len(p.cfg.DependsOn) != len(p.PIDSets) { + return errors.Errorf("pid block %s should have %d inputs got %d", p.cfg.Name, len(p.PIDSets), len(p.cfg.DependsOn)) } - p.kI = p.cfg.Attribute["kI"].(float64) - p.kD = p.cfg.Attribute["kD"].(float64) - p.kP = p.cfg.Attribute["kP"].(float64) // ensure a default of 255 p.satLimUp = 255 @@ -125,41 +171,52 @@ func (p *basicPID) reset() error { p.limLo = p.cfg.Attribute["limit_lo"].(float64) } - p.tuning = false - if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 { - var ssrVal float64 - if p.cfg.Attribute["tune_ssr_value"] != nil { - ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64) - } + for i := 0; i < len(p.PIDSets); i++ { + // Create a Tuner object for our PID set. Across all Tuner objects, they share global + // values (limUp, limLo, ssR, tuneMethod, stepPct). The only values that differ are P,I,D. + if p.PIDSets[i].NeedsAutoTuning() { + var ssrVal float64 + if p.cfg.Attribute["tune_ssr_value"] != nil { + ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64) + } - tuneStepPct := 0.35 - if p.cfg.Attribute.Has("tune_step_pct") { - tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64) - } + tuneStepPct := 0.35 + if p.cfg.Attribute.Has("tune_step_pct") { + tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64) + } - tuneMethod := tuneMethodZiegerNicholsPID - if p.cfg.Attribute.Has("tune_method") { - tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) - } + tuneMethod := tuneMethodZiegerNicholsPID + if p.cfg.Attribute.Has("tune_method") { + tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) + } - p.tuner = pidTuner{ - limUp: p.limUp, - limLo: p.limLo, - ssRValue: ssrVal, - tuneMethod: tuneMethod, - stepPct: tuneStepPct, - } - err := p.tuner.reset() - if err != nil { - return err - } - if p.tuner.stepPct > 1 || p.tuner.stepPct < 0 { - return errors.Errorf("tuner pid block %s should have a percentage value between 0-1 for TuneStepPct", p.cfg.Name) + p.tuners[i] = &pidTuner{ + limUp: p.limUp, + limLo: p.limLo, + ssRValue: ssrVal, + tuneMethod: tuneMethod, + stepPct: tuneStepPct, + kP: p.PIDSets[i].P, + kI: p.PIDSets[i].I, + kD: p.PIDSets[i].D, + tuning: true, + } + + err := p.tuners[i].reset() + if err != nil { + return err + } + + if p.tuners[i].stepPct > 1 || p.tuners[i].stepPct < 0 { + return errors.Errorf("tuner pid block %s should have a percentage value between 0-1 for TuneStepPct", p.cfg.Name) + } } - p.tuning = true } + // Note: In our Signal[] array, we only have one element. For MIMO, within Signal[0], + // the length of the signal[] array is lengthened to accommodate multiple outputs. p.y = make([]*Signal, 1) - p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type) + p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) + return nil } @@ -231,6 +288,7 @@ type pidTuner struct { ccT2 time.Duration ccT3 time.Duration out float64 + tuning bool } // reference for computation: https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method#cite_note-1 @@ -393,5 +451,7 @@ func (p *pidTuner) reset() error { p.kI = 0.0 p.kD = 0.0 p.kP = 0.0 + p.pPeakH = []float64{} + p.pPeakL = []float64{} return nil } diff --git a/control/pid_test.go b/control/pid_test.go index c24d1dd2ebe..6c045bc02d1 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -14,7 +14,7 @@ import ( var loop = Loop{} -func TestPIDConfig(t *testing.T) { +func TestPIDMultiConfig(t *testing.T) { logger := logging.NewTestLogger(t) for i, tc := range []struct { conf BlockConfig @@ -22,30 +22,33 @@ func TestPIDConfig(t *testing.T) { }{ { BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{"kD": 0.11, "kP": 0.12, "kI": 0.22}, + Name: "PID1", + Attribute: utils.AttributeMap{ + "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, + }, Type: "PID", DependsOn: []string{"A", "B"}, }, - "pid block PID1 should have 1 input got 2", + "", }, { BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{"kD": 0.11, "kP": 0.12, "kI": 0.22}, + Name: "PID1", + Attribute: utils.AttributeMap{ + "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, + }, Type: "PID", DependsOn: []string{"A"}, }, - "", + "pid block PID1 should have 2 inputs got 1", }, { BlockConfig{ Name: "PID1", - Attribute: utils.AttributeMap{"Kdd": 0.11}, Type: "PID", DependsOn: []string{"A"}, }, - "pid block PID1 should have at least one kI, kP or kD field", + "pid block PID1 does not have a PID configured", }, } { t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { @@ -60,22 +63,20 @@ func TestPIDConfig(t *testing.T) { } } -func TestPIDBasicIntegralWindup(t *testing.T) { +func TestPIDMultiIntegralWindup(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) cfg := BlockConfig{ Name: "PID1", Attribute: utils.AttributeMap{ - "kD": 0.11, - "kP": 0.12, - "kI": 0.22, + "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .33, I: .33, D: .10}}, "limit_up": 100.0, "limit_lo": 0.0, "int_sat_lim_up": 100.0, "int_sat_lim_lo": 0.0, }, Type: "PID", - DependsOn: []string{"A"}, + DependsOn: []string{"A", "B"}, } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) @@ -83,47 +84,77 @@ func TestPIDBasicIntegralWindup(t *testing.T) { s := []*Signal{ { name: "A", - signal: make([]float64, 1), + signal: make([]float64, 2), time: make([]int, 1), }, } + for i := 0; i < 50; i++ { dt := time.Duration(1000000 * 10) s[0].SetSignalValueAt(0, 1000.0) + s[0].SetSignalValueAt(1, 1000.0) + out, ok := pid.Next(ctx, s, dt) if i < 46 { test.That(t, ok, test.ShouldBeTrue) - test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 100.0) + test.That(t, out[0].signal[0], test.ShouldEqual, 100.0) + test.That(t, out[0].signal[1], test.ShouldEqual, 100.0) } else { - test.That(t, pid.int, test.ShouldBeGreaterThanOrEqualTo, 100) + // Multi Input Signal Testing s[0] s[0].SetSignalValueAt(0, 0.0) - out, ok := pid.Next(ctx, s, dt) + out, ok = pid.Next(ctx, s, dt) test.That(t, ok, test.ShouldBeTrue) - test.That(t, pid.int, test.ShouldBeGreaterThanOrEqualTo, 100) + test.That(t, pid.PIDSets[0].int, test.ShouldBeGreaterThanOrEqualTo, 100) test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 0.0) s[0].SetSignalValueAt(0, -1.0) out, ok = pid.Next(ctx, s, dt) test.That(t, ok, test.ShouldBeTrue) - test.That(t, pid.int, test.ShouldBeLessThanOrEqualTo, 100) + test.That(t, pid.PIDSets[0].int, test.ShouldBeLessThanOrEqualTo, 100) test.That(t, out[0].GetSignalValueAt(0), test.ShouldAlmostEqual, 88.8778) + + // Multi Input Signal Testing s[1] + s[0].SetSignalValueAt(1, 0.0) + out, ok = pid.Next(ctx, s, dt) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, pid.PIDSets[1].int, test.ShouldBeGreaterThanOrEqualTo, 100) + test.That(t, out[0].GetSignalValueAt(1), test.ShouldEqual, 0.0) + s[0].SetSignalValueAt(1, -1.0) + out, ok = pid.Next(ctx, s, dt) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, pid.PIDSets[1].int, test.ShouldBeLessThanOrEqualTo, 100) + test.That(t, out[0].GetSignalValueAt(1), test.ShouldAlmostEqual, 89.6667) + break } } err = pid.Reset(ctx) test.That(t, err, test.ShouldBeNil) - test.That(t, pid.int, test.ShouldEqual, 0) - test.That(t, pid.error, test.ShouldEqual, 0) + + test.That(t, pid.PIDSets[0].int, test.ShouldEqual, 0) + test.That(t, pid.PIDSets[0].signalErr, test.ShouldEqual, 0) + test.That(t, pid.PIDSets[0].P, test.ShouldEqual, .12) + test.That(t, pid.PIDSets[0].I, test.ShouldEqual, .22) + test.That(t, pid.PIDSets[0].D, test.ShouldEqual, .11) + + test.That(t, pid.PIDSets[1].int, test.ShouldEqual, 0) + test.That(t, pid.PIDSets[1].signalErr, test.ShouldEqual, 0) + test.That(t, pid.PIDSets[1].P, test.ShouldEqual, .33) + test.That(t, pid.PIDSets[1].I, test.ShouldEqual, .33) + test.That(t, pid.PIDSets[1].D, test.ShouldEqual, .10) } -func TestPIDTuner(t *testing.T) { +func TestPIDMultiTuner(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) + + // define N PID gains to tune + pidConfigs := []*PIDConfig{{P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}} + dependsOnNames := []string{"A", "B", "C"} + cfg := BlockConfig{ - Name: "PID1", + Name: "3 PID Set", Attribute: utils.AttributeMap{ - "kD": 0.0, - "kP": 0.0, - "kI": 0.0, + "PIDSets": pidConfigs, // N PID Sets defined here "limit_up": 255.0, "limit_lo": 0.0, "int_sat_lim_up": 255.0, @@ -132,34 +163,53 @@ func TestPIDTuner(t *testing.T) { "tune_step_pct": 0.45, }, Type: "PID", - DependsOn: []string{"A"}, + DependsOn: dependsOnNames, } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) test.That(t, err, test.ShouldBeNil) - test.That(t, pid.tuning, test.ShouldBeTrue) - test.That(t, pid.tuner.currentPhase, test.ShouldEqual, begin) + test.That(t, pid.GetTuning(), test.ShouldBeTrue) + test.That(t, pid.tuners[0].currentPhase, test.ShouldEqual, begin) s := []*Signal{ { name: "A", - signal: make([]float64, 1), + signal: make([]float64, len(pidConfigs)), // Make N signals here time: make([]int, 1), }, } dt := time.Millisecond * 10 - for i := 0; i < 22; i++ { - s[0].SetSignalValueAt(0, s[0].GetSignalValueAt(0)+2) - out, hold := pid.Next(ctx, s, dt) - test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) - } - for i := 0; i < 15; i++ { - s[0].SetSignalValueAt(0, 100.0) + + // we want to test the tuning behavior for each signal that we defined above + for signalIndex := range s[0].signal { + // This loop tests each PID controller's response to increasing input values, + // verifying that it reaches a steady state such that the output remains constant. + for i := 0; i < 22; i++ { + s[0].SetSignalValueAt(signalIndex, s[0].GetSignalValueAt(signalIndex)+2) + out, hold := pid.Next(ctx, s, dt) + test.That(t, out[0].GetSignalValueAt(signalIndex), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + } + + // This loop tests each PID controller's response to constant input values, verifying + // that it reaches a steady state such that the output remains constant. + for i := 0; i < 15; i++ { + // Set the signal to a constant value + s[0].SetSignalValueAt(signalIndex, 100.0) + test.That(t, s[0].GetSignalValueAt(signalIndex), test.ShouldEqual, 100) + + out, hold := pid.Next(ctx, s, dt) + + // Verify that each signal remained the correct output value after call to Next() + test.That(t, out[0].GetSignalValueAt(signalIndex), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + } + // After reaching steady state, these tests verify that each signal responds correctly to + // 1 call to Next(). Each Signal should oscillate, out, hold := pid.Next(ctx, s, dt) - test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 255.0*0.45) + test.That(t, out[0].GetSignalValueAt(signalIndex), test.ShouldEqual, 255.0*0.45+0.5*255.0*0.45) test.That(t, hold, test.ShouldBeTrue) + + // disable the tuner to test the next signal + pid.tuners[signalIndex].tuning = false } - out, hold := pid.Next(ctx, s, dt) - test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 255.0*0.45+0.5*255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) } diff --git a/control/setup_control.go b/control/setup_control.go index eba74b25c9d..ceede97be8d 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -52,6 +52,11 @@ type PIDConfig struct { P float64 `json:"p"` I float64 `json:"i"` D float64 `json:"d"` + + // PID block specific values + // these are integral sum and signalErr for the pid signal + int float64 + signalErr float64 } // NeedsAutoTuning checks if the PIDConfig values require auto tuning. @@ -161,6 +166,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc p.logger.Debug("tuning trapz PID") p.ControlConf.Blocks[sumIndex].DependsOn[0] = p.BlockNames[BlockNameConstant][0] if err := p.StartControlLoop(); err != nil { + p.logger.Error(err) errs = multierr.Combine(errs, err) } @@ -177,7 +183,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, 0); err != nil { + if err := p.tuneSinglePIDBlock(ctx, angularPIDIndex, 0); err != nil { errs = multierr.Combine(errs, err) } } @@ -185,7 +191,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, 1); err != nil { + if err := p.tuneSinglePIDBlock(ctx, linearPIDIndex, 1); err != nil { errs = multierr.Combine(errs, err) } } @@ -194,13 +200,16 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc return errs } -func (p *PIDLoop) tuneSinglePID(ctx context.Context, blockIndex, pidIndex int) error { +// tunes a single PID block assuming there are two PID blocks in the loop. +func (p *PIDLoop) tuneSinglePIDBlock(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"] + pidOld := p.ControlConf.Blocks[blockIndex].Attribute["PIDSets"].([]*PIDConfig) // to tune one set of PID values, the other PI values must be non-zero - p.ControlConf.Blocks[blockIndex].Attribute["kP"] = 0.0001 - p.ControlConf.Blocks[blockIndex].Attribute["kI"] = 0.0001 + tempPIDConfigs := make([]*PIDConfig, len(pidOld)) + for index := range pidOld { + tempPIDConfigs[index] = &PIDConfig{P: .001, I: .001} + } + p.ControlConf.Blocks[blockIndex].Attribute["PIDSets"] = tempPIDConfigs if err := p.StartControlLoop(); err != nil { return err } @@ -214,8 +223,7 @@ func (p *PIDLoop) tuneSinglePID(ctx context.Context, blockIndex, pidIndex int) e p.ControlLoop = nil // reset PI values - p.ControlConf.Blocks[blockIndex].Attribute["kP"] = pOld - p.ControlConf.Blocks[blockIndex].Attribute["kI"] = iOld + p.ControlConf.Blocks[blockIndex].Attribute["PIDSets"] = pidOld return nil } @@ -279,9 +287,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con Attribute: rdkutils.AttributeMap{ "int_sat_lim_lo": -255.0, "int_sat_lim_up": 255.0, - "kD": pidVals.D, - "kI": pidVals.I, - "kP": pidVals.P, + "PIDSets": []*PIDConfig{&pidVals}, "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", @@ -383,9 +389,7 @@ func (p *PIDLoop) addSensorFeedbackVelocityControl(angularPIDVals PIDConfig) { Name: "angular_PID", Type: blockPID, Attribute: rdkutils.AttributeMap{ - "kD": angularPIDVals.D, - "kI": angularPIDVals.I, - "kP": angularPIDVals.P, + "PIDSets": []*PIDConfig{&angularPIDVals}, "int_sat_lim_lo": -255.0, "int_sat_lim_up": 255.0, "limit_lo": -255.0, @@ -484,12 +488,19 @@ 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) + if tunedStr != "" { + tunedStr += "," + } + tunedStr += pid.String() } } return fmt.Errorf(`%v has been tuned, please copy the following control values into your config: %v`, name, tunedStr) } +func (conf PIDConfig) String() string { + return fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"}`, conf.P, conf.I, conf.D, conf.Type) +} + // 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)