From 358fe3e8f38d804ae18bca6aed9a6116fbbef159 Mon Sep 17 00:00:00 2001 From: Maria Patni <40074554+mariapatni@users.noreply.github.com> Date: Fri, 16 Aug 2024 13:15:58 -0400 Subject: [PATCH 01/24] MIMO on PID Block done, need to clean up code --- control/control_block.go | 1 + control/control_signal.go | 25 +++- control/pid.go | 240 ++++++++++++++++++++++++++++++++------ control/pid_test.go | 181 +++++++++++++++++++++++++--- control/setup_control.go | 11 +- 5 files changed, 404 insertions(+), 54 deletions(-) diff --git a/control/control_block.go b/control/control_block.go index b79cd28af1b..97f44c43fc6 100644 --- a/control/control_block.go +++ b/control/control_block.go @@ -22,6 +22,7 @@ const ( blockEncoderToRPM controlBlockType = "encoderToRpm" blockEndpoint controlBlockType = "endpoint" blockFilter controlBlockType = "filter" + blockMIMOPID controlBlockType = "MIMOPID" ) // BlockConfig configuration of a given block. diff --git a/control/control_signal.go b/control/control_signal.go index 297ea41128f..ecdf3d38f99 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -1,6 +1,9 @@ package control -import "sync" +import ( + "fmt" + "sync" +) // Signal holds any data passed between blocks. type Signal struct { @@ -20,6 +23,20 @@ func makeSignal(name string, blockType controlBlockType) *Signal { s.time = make([]int, dimension) s.name = name s.blockType = blockType + fmt.Printf("made signal %s\n", s.name) + return &s +} + +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 + fmt.Printf("made signal of length %d \n", dimension) + fmt.Printf("made signals %s\n", s.name) return &s } @@ -27,7 +44,8 @@ func makeSignal(name string, blockType controlBlockType) *Signal { 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)) { + fmt.Print("erring here\n") return 0.0 } return s.signal[i] @@ -37,7 +55,8 @@ 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)) { + fmt.Printf("errin here\n") return } s.signal[i] = val diff --git a/control/pid.go b/control/pid.go index d11e42bb111..ffff381d9cb 100644 --- a/control/pid.go +++ b/control/pid.go @@ -2,6 +2,7 @@ package control import ( "context" + "fmt" "math" "sync" "time" @@ -20,21 +21,32 @@ func (l *Loop) newPID(config BlockConfig, logger logging.Logger) (Block, error) return p, nil } +// type PIDSet struct { +// // error float64 +// kI float64 +// kD float64 +// kP float64 +// // int float64 +// } + // 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 + PIDSets []*PIDConfig + error float64 // MIMO + kI float64 // + kD float64 // + kP float64 // + int float64 // MIMO y []*Signal + useMulti bool satLimUp float64 `default:"255.0"` limUp float64 `default:"255.0"` satLimLo float64 limLo float64 tuner pidTuner + tuners []*pidTuner tuning bool logger logging.Logger } @@ -61,7 +73,26 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* p.tuning = false } p.y[0].SetSignalValueAt(0, out) + + if p.useMulti { + for i := 0; i < len(p.PIDSets); i++ { + + 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 are p: %1.6f, i: %1.6f, d: %1.6f", 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.tuning = false + } + p.y[0].SetSignalValueAt(i, out) + } + } } else { + dtS := dt.Seconds() pvError := x[0].GetSignalValueAt(0) p.int += p.kI * pvError * dtS @@ -81,7 +112,41 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* output = p.limLo } p.y[0].SetSignalValueAt(0, output) + + if p.useMulti { + + for i := 0; i < len(p.PIDSets); i++ { + dtS := dt.Seconds() + pvError := x[0].GetSignalValueAt(i) + p.PIDSets[i].int += p.PIDSets[i].I * pvError * dtS + // + fmt.Printf("\ndts %f pv error 1%f 2 %f.int %f \n", dtS, pvError, x[0].signal[i], p.PIDSets[i].int) + + switch { + case p.PIDSets[i].int >= p.satLimUp: + p.PIDSets[i].int = p.satLimUp + case p.PIDSets[i].int <= p.satLimLo: + p.PIDSets[i].int = p.satLimLo + default: + } + deriv := (pvError - p.PIDSets[i].error) / dtS + output := p.PIDSets[i].P*pvError + p.PIDSets[i].int + p.PIDSets[i].D*deriv + p.PIDSets[i].error = pvError + if output > p.limUp { + output = p.limUp + } else if output < p.limLo { + output = p.limLo + } + + fmt.Printf("deriv %f output %f .error %f \n\n", deriv, output, p.PIDSets[i].error) + + p.y[0].SetSignalValueAt(i, output) // i + } + } + } + + fmt.Printf("done with next. \n") return p.y, true } @@ -94,13 +159,31 @@ func (p *basicPID) reset() error { !p.cfg.Attribute.Has("kP") { return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name) } - if len(p.cfg.DependsOn) != 1 { + if len(p.cfg.DependsOn) != 1 && !p.useMulti { return errors.Errorf("pid block %s should have 1 input got %d", p.cfg.Name, 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) + if p.cfg.Attribute.Has("PIDSets") { + ok := true + p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig) + if !ok { + return errors.Errorf("PIDSet did not initalize correctly") + } + if len(p.PIDSets) > 0 { + p.useMulti = true + p.tuners = make([]*pidTuner, len(p.PIDSets)) + + for i := 0; i < len(p.PIDSets); i++ { + p.PIDSets[i].int = 0 + p.PIDSets[i].error = 0 + } + } + + } + // ensure a default of 255 p.satLimUp = 255 if satLimUp, ok := p.cfg.Attribute["int_sat_lim_up"].(float64); ok { @@ -126,40 +209,91 @@ func (p *basicPID) reset() error { } 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) - } + if p.useMulti { + for i := 0; i < len(p.PIDSets); i++ { - tuneStepPct := 0.35 - if p.cfg.Attribute.Has("tune_step_pct") { - tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64) - } + if p.PIDSets[i].I == 0.0 && p.PIDSets[i].D == 0.0 && p.PIDSets[i].P == 0.0 { + var ssrVal float64 + if p.cfg.Attribute["tune_ssr_value"] != nil { + ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64) + } - tuneMethod := tuneMethodZiegerNicholsPID - if p.cfg.Attribute.Has("tune_method") { - tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) - } + tuneStepPct := 0.35 + if p.cfg.Attribute.Has("tune_step_pct") { + tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64) + } - p.tuner = pidTuner{ - limUp: p.limUp, - limLo: p.limLo, - ssRValue: ssrVal, - tuneMethod: tuneMethod, - stepPct: tuneStepPct, - } - err := p.tuner.reset() - if err != nil { - return err + tuneMethod := tuneMethodZiegerNicholsPID + if p.cfg.Attribute.Has("tune_method") { + tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) + } + + 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, + } + + 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 + } } - 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) + } else { + + 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) + } + + 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)) + } + + 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.tuning = true } - p.tuning = true } + p.y = make([]*Signal, 1) - p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type) + if p.useMulti { + p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) + } else { + p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type) + } + return nil } @@ -296,6 +430,7 @@ func (p *pidTuner) computeGains() { p.kI = 0.5454 * (kU / pU) p.kD = 0.0 } + } func pidTunerFindTCat(speeds []float64, times []time.Time, speed float64) time.Duration { @@ -393,5 +528,44 @@ func (p *pidTuner) reset() error { p.kI = 0.0 p.kD = 0.0 p.kP = 0.0 + + // p.currentPhase = 0.0 + // p.stepRsp = []float64{} + // p.stepRespT = []time.Time{} + // // p.tS. + // p.xF = 0.0 + // p.vF = 0.0 + // p.dF = 0.0 + // p.pPv = 0.0 + + // //lastR time.Time + // p.avgSpeedSS = 0.0 + // //tC time.Duration + + p.pPeakH = []float64{} + p.pPeakL = []float64{} + + // p.pFindDir = 0.0 + // //tuneMethod tuneCalcMethod + // //stepPct float64 `default:".35"` + // //limUp float64 + // //limLo float64 + // //ssRValue float64 `default:"2.0"` + // //ccT2 time.Duration + // //ccT3 time.Duration + // p.out = 0.0 + return nil } + +/* + kI float64 + kD float64 + kP float64 + + tS time.Time + + + + +*/ diff --git a/control/pid_test.go b/control/pid_test.go index c24d1dd2ebe..c8fe35e7bec 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,8 +22,13 @@ 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{ + "kD": 0.11, + "kP": 0.12, + "kI": 0.22, + "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, + }, Type: "PID", DependsOn: []string{"A", "B"}, }, @@ -31,8 +36,13 @@ 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{ + "kD": 0.11, + "kP": 0.12, + "kI": 0.22, + "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, + }, Type: "PID", DependsOn: []string{"A"}, }, @@ -60,15 +70,16 @@ 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, + "kD": 0.11, + "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, @@ -79,33 +90,51 @@ func TestPIDBasicIntegralWindup(t *testing.T) { } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) + pid.useMulti = true test.That(t, err, test.ShouldBeNil) 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 } } @@ -113,8 +142,20 @@ func TestPIDBasicIntegralWindup(t *testing.T) { 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].error, 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].error, 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) { ctx := context.Background() logger := logging.NewTestLogger(t) @@ -163,3 +204,115 @@ func TestPIDTuner(t *testing.T) { 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) } + +func TestPIDMultiTuner(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + cfg := BlockConfig{ + Name: "3 PID Set", + Attribute: utils.AttributeMap{ + "kD": 0.0, + "kP": 0.0, + "kI": 0.0, + "PIDSets": []*PIDConfig{{P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}}, // 3 PID Sets defined here + "limit_up": 255.0, + "limit_lo": 0.0, + "int_sat_lim_up": 255.0, + "int_sat_lim_lo": 0.0, + "tune_ssr_value": 2.0, + "tune_step_pct": 0.45, + }, + Type: "PID", + DependsOn: []string{"A"}, + } + 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) + s := []*Signal{ + { + name: "A", + signal: make([]float64, 3), // Make 3 signals here + time: make([]int, 1), + }, + } + dt := time.Millisecond * 10 + + // We make a set of 3 Signals. 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(0, s[0].GetSignalValueAt(0)+2) + s[0].SetSignalValueAt(1, s[0].GetSignalValueAt(1)+2) + s[0].SetSignalValueAt(2, s[0].GetSignalValueAt(2)+2) + out, hold := pid.Next(ctx, s, dt) + + test.That(t, out[0].GetSignalValueAt(2), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + test.That(t, out[0].GetSignalValueAt(1), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + test.That(t, out[0].GetSignalValueAt(0), 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 all 3 signals to constant value + s[0].SetSignalValueAt(0, 100.0) + test.That(t, s[0].GetSignalValueAt(0), test.ShouldEqual, 100) + s[0].SetSignalValueAt(1, 100.0) + test.That(t, s[0].GetSignalValueAt(0), test.ShouldEqual, 100) + s[0].SetSignalValueAt(2, 100.0) + test.That(t, s[0].GetSignalValueAt(0), 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(2), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + test.That(t, out[0].GetSignalValueAt(1), 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+0.5*255.0*0.45) + test.That(t, out[0].GetSignalValueAt(1), test.ShouldEqual, 255.0*0.45+0.5*255.0*0.45) + test.That(t, out[0].GetSignalValueAt(2), test.ShouldEqual, 255.0*0.45+0.5*255.0*0.45) + test.That(t, hold, test.ShouldBeTrue) + +} + +func TestMIMOPIDConfig(t *testing.T) { + logger := logging.NewTestLogger(t) + for i, tc := range []struct { + conf BlockConfig + err string + }{ + { + BlockConfig{ + Name: "PID1", + Attribute: utils.AttributeMap{"kD": 0.11, "kP": 0.12, "kI": 0.22, "PIDSets": []*PIDConfig{{P: .12, I: .13, D: .14}, {P: .22, I: .23, D: .24}}}, + Type: "PID", + DependsOn: []string{"A", "B"}, + }, + "pid block PID1 should have 1 input got 2", + }, + } { + t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { + _, err := loop.newPID(tc.conf, logger) + if tc.err == "" { + test.That(t, err, test.ShouldBeNil) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldEqual, tc.err) + } + }) + } +} diff --git a/control/setup_control.go b/control/setup_control.go index a5ca0b22848..1c2c01d3ae6 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -46,10 +46,12 @@ type PIDLoop struct { // PIDConfig is values needed to configure a PID control loop. type PIDConfig struct { - Type string `json:"type,omitempty"` - P float64 `json:"p"` - I float64 `json:"i"` - D float64 `json:"d"` + Type string `json:"type,omitempty"` + P float64 `json:"p"` + I float64 `json:"i"` + D float64 `json:"d"` + int float64 + error float64 } // NeedsAutoTuning checks if the PIDConfig values require auto tuning. @@ -272,6 +274,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con "kD": pidVals.D, "kI": pidVals.I, "kP": pidVals.P, + "PIDSets": []*PIDConfig{&pidVals}, "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", From 252a42a7ba9249d8210125be191edde1e5ddca81 Mon Sep 17 00:00:00 2001 From: Maria Patni <40074554+mariapatni@users.noreply.github.com> Date: Fri, 16 Aug 2024 14:21:35 -0400 Subject: [PATCH 02/24] added some comments and cleaned up some code --- control/control_signal.go | 2 + control/pid.go | 187 ++++++++++++++++---------------------- 2 files changed, 78 insertions(+), 111 deletions(-) diff --git a/control/control_signal.go b/control/control_signal.go index ecdf3d38f99..4f2559a02b8 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -27,6 +27,8 @@ func makeSignal(name string, blockType controlBlockType) *Signal { return &s } +// Make Signals 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 diff --git a/control/pid.go b/control/pid.go index ffff381d9cb..d68002b173a 100644 --- a/control/pid.go +++ b/control/pid.go @@ -2,7 +2,6 @@ package control import ( "context" - "fmt" "math" "sync" "time" @@ -21,14 +20,6 @@ func (l *Loop) newPID(config BlockConfig, logger logging.Logger) (Block, error) return p, nil } -// type PIDSet struct { -// // error float64 -// kI float64 -// kD float64 -// kP float64 -// // int float64 -// } - // BasicPID is the standard implementation of a PID controller. type basicPID struct { mu sync.Mutex @@ -62,19 +53,10 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* 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 - } - p.y[0].SetSignalValueAt(0, out) - + // Multi Input/Output Implementation if p.useMulti { + + // 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++ { out, done := p.tuners[i].pidTunerStep(math.Abs(x[0].GetSignalValueAt(i)), p.logger) @@ -90,64 +72,81 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* } p.y[0].SetSignalValueAt(i, out) } + } else { + // Single Input/Output Implementation + 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 + } + 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 - } - p.y[0].SetSignalValueAt(0, output) + } else { + // Multi Input/Output Implementation if p.useMulti { for i := 0; i < len(p.PIDSets); i++ { - dtS := dt.Seconds() - pvError := x[0].GetSignalValueAt(i) - p.PIDSets[i].int += p.PIDSets[i].I * pvError * dtS - // - fmt.Printf("\ndts %f pv error 1%f 2 %f.int %f \n", dtS, pvError, x[0].signal[i], p.PIDSets[i].int) - - switch { - case p.PIDSets[i].int >= p.satLimUp: - p.PIDSets[i].int = p.satLimUp - case p.PIDSets[i].int <= p.satLimLo: - p.PIDSets[i].int = p.satLimLo - default: - } - deriv := (pvError - p.PIDSets[i].error) / dtS - output := p.PIDSets[i].P*pvError + p.PIDSets[i].int + p.PIDSets[i].D*deriv - p.PIDSets[i].error = pvError - if output > p.limUp { - output = p.limUp - } else if output < p.limLo { - output = p.limLo - } - - fmt.Printf("deriv %f output %f .error %f \n\n", deriv, output, p.PIDSets[i].error) + output := calculateSignalValue(p, x, dt, i) + p.y[0].SetSignalValueAt(i, output) + } - p.y[0].SetSignalValueAt(i, output) // i + // Single Input/Output Implementation + } 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 } + 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].error) / dtS + output := p.PIDSets[sIndex].P*pvError + p.PIDSets[sIndex].int + p.PIDSets[sIndex].D*deriv + p.PIDSets[sIndex].error = pvError + if output > p.limUp { + output = p.limUp + } else if output < p.limLo { + output = p.limLo } - fmt.Printf("done with next. \n") - return p.y, true + return output } func (p *basicPID) reset() error { @@ -166,6 +165,9 @@ func (p *basicPID) reset() error { p.kD = p.cfg.Attribute["kD"].(float64) p.kP = p.cfg.Attribute["kP"].(float64) + // 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") { ok := true p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig) @@ -181,7 +183,6 @@ func (p *basicPID) reset() error { p.PIDSets[i].error = 0 } } - } // ensure a default of 255 @@ -228,6 +229,8 @@ func (p *basicPID) reset() error { tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) } + // 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. p.tuners[i] = &pidTuner{ limUp: p.limUp, limLo: p.limLo, @@ -250,6 +253,10 @@ func (p *basicPID) reset() error { 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] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) } else { if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 { @@ -285,12 +292,8 @@ func (p *basicPID) reset() error { } p.tuning = true } - } - p.y = make([]*Signal, 1) - if p.useMulti { - p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) - } else { + p.y = make([]*Signal, 1) p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type) } @@ -430,7 +433,6 @@ func (p *pidTuner) computeGains() { p.kI = 0.5454 * (kU / pU) p.kD = 0.0 } - } func pidTunerFindTCat(speeds []float64, times []time.Time, speed float64) time.Duration { @@ -528,44 +530,7 @@ func (p *pidTuner) reset() error { p.kI = 0.0 p.kD = 0.0 p.kP = 0.0 - - // p.currentPhase = 0.0 - // p.stepRsp = []float64{} - // p.stepRespT = []time.Time{} - // // p.tS. - // p.xF = 0.0 - // p.vF = 0.0 - // p.dF = 0.0 - // p.pPv = 0.0 - - // //lastR time.Time - // p.avgSpeedSS = 0.0 - // //tC time.Duration - p.pPeakH = []float64{} p.pPeakL = []float64{} - - // p.pFindDir = 0.0 - // //tuneMethod tuneCalcMethod - // //stepPct float64 `default:".35"` - // //limUp float64 - // //limLo float64 - // //ssRValue float64 `default:"2.0"` - // //ccT2 time.Duration - // //ccT3 time.Duration - // p.out = 0.0 - return nil } - -/* - kI float64 - kD float64 - kP float64 - - tS time.Time - - - - -*/ From 21c3e336e37ddc67631a0dbf0c236e84d12d8b14 Mon Sep 17 00:00:00 2001 From: Maria Patni <40074554+mariapatni@users.noreply.github.com> Date: Fri, 16 Aug 2024 14:42:53 -0400 Subject: [PATCH 03/24] ran linter --- control/control_signal.go | 6 ------ control/pid.go | 2 +- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/control/control_signal.go b/control/control_signal.go index 4f2559a02b8..02cf396d069 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -1,7 +1,6 @@ package control import ( - "fmt" "sync" ) @@ -23,7 +22,6 @@ func makeSignal(name string, blockType controlBlockType) *Signal { s.time = make([]int, dimension) s.name = name s.blockType = blockType - fmt.Printf("made signal %s\n", s.name) return &s } @@ -37,8 +35,6 @@ func makeSignals(name string, blockType controlBlockType, dimension int) *Signal s.time = make([]int, dimension) s.name = name s.blockType = blockType - fmt.Printf("made signal of length %d \n", dimension) - fmt.Printf("made signals %s\n", s.name) return &s } @@ -47,7 +43,6 @@ func (s *Signal) GetSignalValueAt(i int) float64 { s.mu.Lock() defer s.mu.Unlock() if !(i < len(s.signal)) { - fmt.Print("erring here\n") return 0.0 } return s.signal[i] @@ -58,7 +53,6 @@ func (s *Signal) SetSignalValueAt(i int, val float64) { s.mu.Lock() defer s.mu.Unlock() if !(i < len(s.signal)) { - fmt.Printf("errin here\n") return } s.signal[i] = val diff --git a/control/pid.go b/control/pid.go index d68002b173a..e01e9859894 100644 --- a/control/pid.go +++ b/control/pid.go @@ -172,7 +172,7 @@ func (p *basicPID) reset() error { ok := true p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig) if !ok { - return errors.Errorf("PIDSet did not initalize correctly") + return errors.New("PIDSet did not initalize correctly") } if len(p.PIDSets) > 0 { p.useMulti = true From 31e34536f99407fc5cf0d866042cf92f51d32dc9 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 15:41:48 -0400 Subject: [PATCH 04/24] tune signals one at a time --- control/control_block.go | 1 - control/control_signal.go | 1 - control/pid.go | 43 ++++++++++--------- control/pid_test.go | 89 ++++++++++++++++++--------------------- 4 files changed, 63 insertions(+), 71 deletions(-) diff --git a/control/control_block.go b/control/control_block.go index 97f44c43fc6..b79cd28af1b 100644 --- a/control/control_block.go +++ b/control/control_block.go @@ -22,7 +22,6 @@ const ( blockEncoderToRPM controlBlockType = "encoderToRpm" blockEndpoint controlBlockType = "endpoint" blockFilter controlBlockType = "filter" - blockMIMOPID controlBlockType = "MIMOPID" ) // BlockConfig configuration of a given block. diff --git a/control/control_signal.go b/control/control_signal.go index 02cf396d069..4f8e8eba8a4 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -28,7 +28,6 @@ func makeSignal(name string, blockType controlBlockType) *Signal { // Make Signals 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) diff --git a/control/pid.go b/control/pid.go index e01e9859894..7a29909abc3 100644 --- a/control/pid.go +++ b/control/pid.go @@ -38,12 +38,18 @@ type basicPID struct { limLo float64 tuner pidTuner tuners []*pidTuner - tuning bool logger logging.Logger } func (p *basicPID) GetTuning() bool { - return p.tuning + multiTune := false + if p.useMulti { + for _, tuner := range p.tuners { + multiTune = tuner.tuning || multiTune + } + return multiTune + } + return p.tuner.tuning } // Output returns the discrete step of the PID controller, dt is the delta time between two subsequent call, @@ -52,25 +58,28 @@ 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 { + if p.GetTuning() { // Multi Input/Output Implementation if p.useMulti { - // 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 are p: %1.6f, i: %1.6f, d: %1.6f", 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.tuning = false + p.tuners[i].tuning = false } p.y[0].SetSignalValueAt(i, out) + // return early to only step this signal + return p.y, true } } else { // Single Input/Output Implementation @@ -82,16 +91,13 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* 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 + p.tuner.tuning = false } p.y[0].SetSignalValueAt(0, out) } - } else { - // Multi Input/Output Implementation if p.useMulti { - for i := 0; i < len(p.PIDSets); i++ { output := calculateSignalValue(p, x, dt, i) p.y[0].SetSignalValueAt(i, output) @@ -99,7 +105,6 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* // Single Input/Output Implementation } else { - dtS := dt.Seconds() pvError := x[0].GetSignalValueAt(0) p.int += p.kI * pvError * dtS @@ -124,7 +129,7 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* return p.y, true } -// For a given signal, compute new signal value based on current signal value, & its respective error +// 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) @@ -150,6 +155,7 @@ func calculateSignalValue(p *basicPID, x []*Signal, dt time.Duration, sIndex int } func (p *basicPID) reset() error { + var ok bool p.int = 0 p.error = 0 @@ -169,10 +175,9 @@ func (p *basicPID) reset() error { // 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") { - ok := true p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig) if !ok { - return errors.New("PIDSet did not initalize correctly") + return errors.New("PIDSet did not initialize correctly") } if len(p.PIDSets) > 0 { p.useMulti = true @@ -209,10 +214,8 @@ func (p *basicPID) reset() error { p.limLo = p.cfg.Attribute["limit_lo"].(float64) } - p.tuning = false if p.useMulti { for i := 0; i < len(p.PIDSets); i++ { - if p.PIDSets[i].I == 0.0 && p.PIDSets[i].D == 0.0 && p.PIDSets[i].P == 0.0 { var ssrVal float64 if p.cfg.Attribute["tune_ssr_value"] != nil { @@ -240,6 +243,7 @@ func (p *basicPID) reset() error { kP: p.PIDSets[i].P, kI: p.PIDSets[i].I, kD: p.PIDSets[i].D, + tuning: true, } err := p.tuners[i].reset() @@ -250,7 +254,6 @@ func (p *basicPID) reset() error { 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], @@ -258,7 +261,6 @@ func (p *basicPID) reset() error { p.y = make([]*Signal, 1) p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) } else { - if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 { var ssrVal float64 if p.cfg.Attribute["tune_ssr_value"] != nil { @@ -281,6 +283,7 @@ func (p *basicPID) reset() error { ssRValue: ssrVal, tuneMethod: tuneMethod, stepPct: tuneStepPct, + tuning: true, } err := p.tuner.reset() if err != nil { @@ -290,7 +293,6 @@ func (p *basicPID) reset() error { 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.tuning = true } p.y = make([]*Signal, 1) @@ -368,6 +370,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 diff --git a/control/pid_test.go b/control/pid_test.go index c8fe35e7bec..924accb3aa8 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -154,8 +154,8 @@ func TestPIDMultiIntegralWindup(t *testing.T) { 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) { ctx := context.Background() logger := logging.NewTestLogger(t) @@ -178,7 +178,7 @@ func TestPIDTuner(t *testing.T) { 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.GetTuning(), test.ShouldBeTrue) test.That(t, pid.tuner.currentPhase, test.ShouldEqual, begin) s := []*Signal{ { @@ -208,13 +208,16 @@ 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}} cfg := BlockConfig{ Name: "3 PID Set", Attribute: utils.AttributeMap{ "kD": 0.0, "kP": 0.0, "kI": 0.0, - "PIDSets": []*PIDConfig{{P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}, {P: .0, I: .0, D: .0}}, // 3 PID Sets defined here + "PIDSets": pidConfigs, // N PID Sets defined here "limit_up": 255.0, "limit_lo": 0.0, "int_sat_lim_up": 255.0, @@ -228,65 +231,50 @@ func TestPIDMultiTuner(t *testing.T) { 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.GetTuning(), test.ShouldBeTrue) test.That(t, pid.tuner.currentPhase, test.ShouldEqual, begin) s := []*Signal{ { name: "A", - signal: make([]float64, 3), // Make 3 signals here + signal: make([]float64, len(pidConfigs)), // Make N signals here time: make([]int, 1), }, } dt := time.Millisecond * 10 - // We make a set of 3 Signals. 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(0, s[0].GetSignalValueAt(0)+2) - s[0].SetSignalValueAt(1, s[0].GetSignalValueAt(1)+2) - s[0].SetSignalValueAt(2, s[0].GetSignalValueAt(2)+2) - out, hold := pid.Next(ctx, s, dt) - - test.That(t, out[0].GetSignalValueAt(2), test.ShouldEqual, 255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) - test.That(t, out[0].GetSignalValueAt(1), test.ShouldEqual, 255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) - test.That(t, out[0].GetSignalValueAt(0), test.ShouldEqual, 255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) - } + // 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++ { + // 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) - // Set all 3 signals to constant value - s[0].SetSignalValueAt(0, 100.0) - test.That(t, s[0].GetSignalValueAt(0), test.ShouldEqual, 100) - s[0].SetSignalValueAt(1, 100.0) - test.That(t, s[0].GetSignalValueAt(0), test.ShouldEqual, 100) - s[0].SetSignalValueAt(2, 100.0) - test.That(t, s[0].GetSignalValueAt(0), 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) - - // Verify that each signal remained the correct output value after call to Next() - test.That(t, out[0].GetSignalValueAt(2), test.ShouldEqual, 255.0*0.45) - test.That(t, hold, test.ShouldBeTrue) - 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) - test.That(t, out[0].GetSignalValueAt(1), 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+0.5*255.0*0.45) - test.That(t, out[0].GetSignalValueAt(1), test.ShouldEqual, 255.0*0.45+0.5*255.0*0.45) - test.That(t, out[0].GetSignalValueAt(2), 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 + } } func TestMIMOPIDConfig(t *testing.T) { @@ -297,8 +285,11 @@ func TestMIMOPIDConfig(t *testing.T) { }{ { BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{"kD": 0.11, "kP": 0.12, "kI": 0.22, "PIDSets": []*PIDConfig{{P: .12, I: .13, D: .14}, {P: .22, I: .23, D: .24}}}, + Name: "PID1", + Attribute: utils.AttributeMap{ + "kD": 0.11, "kP": 0.12, "kI": 0.22, + "PIDSets": []*PIDConfig{{P: .12, I: .13, D: .14}, {P: .22, I: .23, D: .24}}, + }, Type: "PID", DependsOn: []string{"A", "B"}, }, From 1cbb1a1a37daed212ea85237333a358b2bb31155 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 16:12:31 -0400 Subject: [PATCH 05/24] feedback from john review --- control/pid.go | 10 +++---- control/pid_test.go | 58 ++++++++++++++++++++++++++++++++++++++-- control/setup_control.go | 17 +++++++----- 3 files changed, 71 insertions(+), 14 deletions(-) diff --git a/control/pid.go b/control/pid.go index 7a29909abc3..ebd82a3f2c1 100644 --- a/control/pid.go +++ b/control/pid.go @@ -73,7 +73,7 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* 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 are p: %1.6f, i: %1.6f, d: %1.6f", p.PIDSets[i].P, p.PIDSets[i].I, p.PIDSets[i].D) + 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 } @@ -142,9 +142,9 @@ func calculateSignalValue(p *basicPID, x []*Signal, dt time.Duration, sIndex int p.PIDSets[sIndex].int = p.satLimLo default: } - deriv := (pvError - p.PIDSets[sIndex].error) / dtS + 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].error = pvError + p.PIDSets[sIndex].signalErr = pvError if output > p.limUp { output = p.limUp } else if output < p.limLo { @@ -185,7 +185,7 @@ func (p *basicPID) reset() error { for i := 0; i < len(p.PIDSets); i++ { p.PIDSets[i].int = 0 - p.PIDSets[i].error = 0 + p.PIDSets[i].signalErr = 0 } } } @@ -216,7 +216,7 @@ func (p *basicPID) reset() error { if p.useMulti { for i := 0; i < len(p.PIDSets); i++ { - if p.PIDSets[i].I == 0.0 && p.PIDSets[i].D == 0.0 && p.PIDSets[i].P == 0.0 { + if p.PIDSets[i].NeedsAutoTuning() { var ssrVal float64 if p.cfg.Attribute["tune_ssr_value"] != nil { ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64) diff --git a/control/pid_test.go b/control/pid_test.go index 924accb3aa8..5f0cff38e8e 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -14,6 +14,60 @@ import ( var loop = Loop{} +func TestPIDConfig(t *testing.T) { + logger := logging.NewTestLogger(t) + for i, tc := range []struct { + conf BlockConfig + err string + }{ + { + BlockConfig{ + Name: "PID1", + Attribute: utils.AttributeMap{ + "kD": 0.11, + "kP": 0.12, + "kI": 0.22, + }, + 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, + }, + Type: "PID", + DependsOn: []string{"A"}, + }, + "", + }, + { + 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", + }, + } { + t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { + _, err := loop.newPID(tc.conf, logger) + if tc.err == "" { + test.That(t, err, test.ShouldBeNil) + } else { + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldEqual, tc.err) + } + }) + } +} + func TestPIDMultiConfig(t *testing.T) { logger := logging.NewTestLogger(t) for i, tc := range []struct { @@ -144,13 +198,13 @@ func TestPIDMultiIntegralWindup(t *testing.T) { test.That(t, pid.error, test.ShouldEqual, 0) test.That(t, pid.PIDSets[0].int, test.ShouldEqual, 0) - test.That(t, pid.PIDSets[0].error, 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].error, 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) diff --git a/control/setup_control.go b/control/setup_control.go index 1c2c01d3ae6..96b0cb4afdd 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -46,12 +46,15 @@ type PIDLoop struct { // PIDConfig is values needed to configure a PID control loop. type PIDConfig struct { - Type string `json:"type,omitempty"` - P float64 `json:"p"` - I float64 `json:"i"` - D float64 `json:"d"` - int float64 - error float64 + Type string `json:"type,omitempty"` + 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. @@ -274,7 +277,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con "kD": pidVals.D, "kI": pidVals.I, "kP": pidVals.P, - "PIDSets": []*PIDConfig{&pidVals}, + // "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", From 00644c87c2e33369cc96916465cde7c983d6601e Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 17:26:27 -0400 Subject: [PATCH 06/24] test MIMO PID works for single signal case --- control/control_loop.go | 24 ++++++++++++++++++------ control/pid.go | 40 ++++++++++++++++++++++++++-------------- control/setup_control.go | 2 +- 3 files changed, 45 insertions(+), 21 deletions(-) diff --git a/control/control_loop.go b/control/control_loop.go index 5724182287f..d004fdfd7dc 100644 --- a/control/control_loop.go +++ b/control/control_loop.go @@ -330,17 +330,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/pid.go b/control/pid.go index ebd82a3f2c1..910f08a0f5f 100644 --- a/control/pid.go +++ b/control/pid.go @@ -41,11 +41,23 @@ type basicPID struct { logger logging.Logger } +// GetTuning returns whether the PID block is currently tuning any signals func (p *basicPID) GetTuning() bool { + // using locks so we do not check for tuning mid reconfigure or mid tune + p.mu.Lock() + defer p.mu.Unlock() + return p.getTuning() +} + +func (p *basicPID) getTuning() bool { multiTune := false + if p.useMulti { for _, tuner := range p.tuners { - multiTune = tuner.tuning || multiTune + // the tuners for MIMO only get created if we want to tune + if tuner != nil { + multiTune = tuner.tuning || multiTune + } } return multiTune } @@ -58,7 +70,7 @@ 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.GetTuning() { + if p.getTuning() { // Multi Input/Output Implementation if p.useMulti { // For each PID Set and its respective Tuner Object, Step through an iteration of tuning until done. @@ -159,18 +171,6 @@ func (p *basicPID) reset() error { p.int = 0 p.error = 0 - 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) - } - if len(p.cfg.DependsOn) != 1 && !p.useMulti { - return errors.Errorf("pid block %s should have 1 input got %d", p.cfg.Name, 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) - // 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. @@ -190,6 +190,18 @@ func (p *basicPID) reset() error { } } + 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) + } + if len(p.cfg.DependsOn) != 1 && !p.useMulti { + return errors.Errorf("pid block %s should have 1 input got %d", p.cfg.Name, 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 if satLimUp, ok := p.cfg.Attribute["int_sat_lim_up"].(float64); ok { diff --git a/control/setup_control.go b/control/setup_control.go index 96b0cb4afdd..dc1d5f78bde 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -277,7 +277,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con "kD": pidVals.D, "kI": pidVals.I, "kP": pidVals.P, - // "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it + "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", From b7900ee030ef53cd67cc73e62c58c58445427c8a Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 17:27:13 -0400 Subject: [PATCH 07/24] disable mimo single case --- control/setup_control.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/setup_control.go b/control/setup_control.go index dc1d5f78bde..96b0cb4afdd 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -277,7 +277,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con "kD": pidVals.D, "kI": pidVals.I, "kP": pidVals.P, - "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it + // "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", From 63eeb2a22791357f675392219a17ac92336162c9 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 17:40:03 -0400 Subject: [PATCH 08/24] small test cleanup --- control/pid_test.go | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/control/pid_test.go b/control/pid_test.go index 5f0cff38e8e..e52443cbe6d 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -22,12 +22,8 @@ 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{"kD": 0.11, "kP": 0.12, "kI": 0.22}, Type: "PID", DependsOn: []string{"A", "B"}, }, @@ -35,12 +31,8 @@ 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{"kD": 0.11, "kP": 0.12, "kI": 0.22}, Type: "PID", DependsOn: []string{"A"}, }, From 8e60a2dc494ec0ccec1bc7c0ad6fde20c1458b04 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 17:42:02 -0400 Subject: [PATCH 09/24] add back old test --- control/pid_test.go | 55 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/control/pid_test.go b/control/pid_test.go index e52443cbe6d..075c2d93487 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -60,6 +60,61 @@ func TestPIDConfig(t *testing.T) { } } +func TestPIDBasicIntegralWindup(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, + "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"}, + } + b, err := loop.newPID(cfg, logger) + pid := b.(*basicPID) + test.That(t, err, test.ShouldBeNil) + s := []*Signal{ + { + name: "A", + signal: make([]float64, 1), + time: make([]int, 1), + }, + } + for i := 0; i < 50; i++ { + dt := time.Duration(1000000 * 10) + s[0].SetSignalValueAt(0, 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) + } else { + test.That(t, pid.int, test.ShouldBeGreaterThanOrEqualTo, 100) + s[0].SetSignalValueAt(0, 0.0) + out, ok := pid.Next(ctx, s, dt) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, pid.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, out[0].GetSignalValueAt(0), test.ShouldAlmostEqual, 88.8778) + 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) +} + func TestPIDMultiConfig(t *testing.T) { logger := logging.NewTestLogger(t) for i, tc := range []struct { From 9b2916a4260e5f7ea265c7acc9f2cf49d6b83b35 Mon Sep 17 00:00:00 2001 From: John Date: Tue, 20 Aug 2024 17:50:25 -0400 Subject: [PATCH 10/24] tweak pid struct comments --- control/pid.go | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/control/pid.go b/control/pid.go index 910f08a0f5f..9e0524ada12 100644 --- a/control/pid.go +++ b/control/pid.go @@ -22,23 +22,28 @@ 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 + mu sync.Mutex + cfg BlockConfig + logger logging.Logger + // used by the single input/output controller + error float64 + kI float64 + kD float64 + kP float64 + int float64 + tuner pidTuner + + // MIMO gains + state PIDSets []*PIDConfig - error float64 // MIMO - kI float64 // - kD float64 // - kP float64 // - int float64 // MIMO - y []*Signal + tuners []*pidTuner useMulti bool + + // used by both + y []*Signal satLimUp float64 `default:"255.0"` limUp float64 `default:"255.0"` satLimLo float64 limLo float64 - tuner pidTuner - tuners []*pidTuner - logger logging.Logger } // GetTuning returns whether the PID block is currently tuning any signals From 9f80288002540e3ce7bf358266387ae68419470c Mon Sep 17 00:00:00 2001 From: John Date: Wed, 21 Aug 2024 10:35:34 -0400 Subject: [PATCH 11/24] lint --- control/pid.go | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/control/pid.go b/control/pid.go index 9e0524ada12..0e8d29faef8 100644 --- a/control/pid.go +++ b/control/pid.go @@ -46,7 +46,7 @@ type basicPID struct { limLo float64 } -// GetTuning returns whether the PID block is currently tuning any signals +// GetTuning returns whether the PID block is currently tuning any signals. func (p *basicPID) GetTuning() bool { // using locks so we do not check for tuning mid reconfigure or mid tune p.mu.Lock() @@ -90,7 +90,8 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* 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, "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 } From 311e05c5acd536cfccc0beb8321fee988e57ce66 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 21 Aug 2024 11:37:56 -0400 Subject: [PATCH 12/24] fix pid test --- control/pid.go | 4 ++++ control/pid_test.go | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/control/pid.go b/control/pid.go index 0e8d29faef8..aaae4daf211 100644 --- a/control/pid.go +++ b/control/pid.go @@ -201,9 +201,13 @@ func (p *basicPID) reset() error { !p.cfg.Attribute.Has("kP") { return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name) } + if len(p.cfg.DependsOn) != 1 && !p.useMulti { 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) && p.useMulti { + 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) diff --git a/control/pid_test.go b/control/pid_test.go index 075c2d93487..e5c9046fc74 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -133,7 +133,7 @@ func TestPIDMultiConfig(t *testing.T) { Type: "PID", DependsOn: []string{"A", "B"}, }, - "pid block PID1 should have 1 input got 2", + "", }, { BlockConfig{ @@ -147,7 +147,7 @@ func TestPIDMultiConfig(t *testing.T) { Type: "PID", DependsOn: []string{"A"}, }, - "", + "pid block PID1 should have 2 inputs got 1", }, { BlockConfig{ From 8bf5301177ce2385869c24ab850b76d96d988ad7 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 21 Aug 2024 11:41:52 -0400 Subject: [PATCH 13/24] fix other tests and remove dupe test --- control/pid_test.go | 37 ++++--------------------------------- 1 file changed, 4 insertions(+), 33 deletions(-) diff --git a/control/pid_test.go b/control/pid_test.go index e5c9046fc74..ad1082f6865 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -187,7 +187,7 @@ func TestPIDMultiIntegralWindup(t *testing.T) { "int_sat_lim_lo": 0.0, }, Type: "PID", - DependsOn: []string{"A"}, + DependsOn: []string{"A", "B"}, } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) @@ -312,6 +312,8 @@ func TestPIDMultiTuner(t *testing.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: "3 PID Set", Attribute: utils.AttributeMap{ @@ -327,7 +329,7 @@ func TestPIDMultiTuner(t *testing.T) { "tune_step_pct": 0.45, }, Type: "PID", - DependsOn: []string{"A"}, + DependsOn: dependsOnNames, } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) @@ -377,34 +379,3 @@ func TestPIDMultiTuner(t *testing.T) { pid.tuners[signalIndex].tuning = false } } - -func TestMIMOPIDConfig(t *testing.T) { - logger := logging.NewTestLogger(t) - for i, tc := range []struct { - conf BlockConfig - err string - }{ - { - BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{ - "kD": 0.11, "kP": 0.12, "kI": 0.22, - "PIDSets": []*PIDConfig{{P: .12, I: .13, D: .14}, {P: .22, I: .23, D: .24}}, - }, - Type: "PID", - DependsOn: []string{"A", "B"}, - }, - "pid block PID1 should have 1 input got 2", - }, - } { - t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { - _, err := loop.newPID(tc.conf, logger) - if tc.err == "" { - test.That(t, err, test.ShouldBeNil) - } else { - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldEqual, tc.err) - } - }) - } -} From 343544f8011247ccd6d037e872656494c4b14756 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 4 Sep 2024 14:54:48 -0400 Subject: [PATCH 14/24] tweak comment --- control/pid.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/pid.go b/control/pid.go index aaae4daf211..242a0179f4e 100644 --- a/control/pid.go +++ b/control/pid.go @@ -59,7 +59,7 @@ func (p *basicPID) getTuning() bool { if p.useMulti { for _, tuner := range p.tuners { - // the tuners for MIMO only get created if we want to tune + // the tuners for MIMO are only initialized when we want to tune if tuner != nil { multiTune = tuner.tuning || multiTune } From 5b86ac88abbe5dd22297fbe9abbe3a8f5cb6d4db Mon Sep 17 00:00:00 2001 From: John Date: Wed, 4 Sep 2024 16:03:55 -0400 Subject: [PATCH 15/24] only confirm pid values are set when not using multi and add error logs during tuning --- control/pid.go | 9 +++++---- control/setup_control.go | 3 +++ 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/control/pid.go b/control/pid.go index 242a0179f4e..d88d5fc666d 100644 --- a/control/pid.go +++ b/control/pid.go @@ -198,7 +198,7 @@ func (p *basicPID) reset() error { if !p.cfg.Attribute.Has("kI") && !p.cfg.Attribute.Has("kD") && - !p.cfg.Attribute.Has("kP") { + !p.cfg.Attribute.Has("kP") && !p.useMulti { return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name) } @@ -208,9 +208,6 @@ func (p *basicPID) reset() error { if len(p.cfg.DependsOn) != len(p.PIDSets) && p.useMulti { 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 @@ -283,6 +280,10 @@ func (p *basicPID) reset() error { p.y = make([]*Signal, 1) p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) } else { + p.kI = p.cfg.Attribute["kI"].(float64) + p.kD = p.cfg.Attribute["kD"].(float64) + p.kP = p.cfg.Attribute["kP"].(float64) + if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 { var ssrVal float64 if p.cfg.Attribute["tune_ssr_value"] != nil { diff --git a/control/setup_control.go b/control/setup_control.go index 96b0cb4afdd..2727d8f9a8f 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -163,6 +163,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) } @@ -176,6 +177,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc if p.PIDVals[0].NeedsAutoTuning() { p.logger.Info("tuning linear PID") if err := p.tuneSinglePID(ctx, angularPIDIndex); err != nil { + p.logger.Error(err) errs = multierr.Combine(errs, err) } } @@ -184,6 +186,7 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc if p.PIDVals[1].NeedsAutoTuning() { p.logger.Info("tuning angular PID") if err := p.tuneSinglePID(ctx, linearPIDIndex); err != nil { + p.logger.Error(err) errs = multierr.Combine(errs, err) } } From 0ca459414c5fb72af575f8a12cdc46fe9ed1254a Mon Sep 17 00:00:00 2001 From: John Date: Wed, 2 Oct 2024 16:29:32 -0400 Subject: [PATCH 16/24] add string method to pidconfig --- components/base/sensorcontrolled/sensorcontrolled.go | 2 +- components/base/sensorcontrolled/sensorcontrolled_test.go | 4 +--- components/motor/gpio/controlled.go | 4 +--- control/setup_control.go | 6 +++++- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go index 9139c9315d3..899deeaa060 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 += fmt.Sprintf(`%s`, pidConf.String()) } } resp[getPID] = respStr 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..71e002642f1 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 } diff --git a/control/setup_control.go b/control/setup_control.go index c150acfa5dd..1e84b976b7f 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -491,12 +491,16 @@ 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) + tunedStr += pid.String() } } return fmt.Errorf(`%v has been tuned, please copy the following control values into your config: %v`, name, tunedStr) } +func (pid PIDConfig) String() string { + return fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"} `, pid.P, pid.I, pid.D, pid.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) From d2cba49de75eba33f2bafd867d198c6a12d86288 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 11:44:58 -0400 Subject: [PATCH 17/24] lint --- components/base/sensorcontrolled/sensorcontrolled.go | 2 +- control/setup_control.go | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go index 899deeaa060..14c0f246bdc 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(`%s`, pidConf.String()) + respStr += pidConf.String() } } resp[getPID] = respStr diff --git a/control/setup_control.go b/control/setup_control.go index 1e84b976b7f..a1338323ce3 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -497,8 +497,8 @@ func TunedPIDErr(name string, tunedVals []PIDConfig) error { return fmt.Errorf(`%v has been tuned, please copy the following control values into your config: %v`, name, tunedStr) } -func (pid PIDConfig) String() string { - return fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"} `, pid.P, pid.I, pid.D, pid.Type) +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. From 756e5b87b7fa6bd1061fe50bd278535cd6b8ea59 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 15:24:30 -0400 Subject: [PATCH 18/24] write string differently --- control/setup_control.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/setup_control.go b/control/setup_control.go index a1338323ce3..a7d39f5bfbd 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -288,7 +288,7 @@ func (p *PIDLoop) basicControlConfig(endpointName string, pidVals PIDConfig, con "kD": pidVals.D, "kI": pidVals.I, "kP": pidVals.P, - // "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it + "PIDSets": []*PIDConfig{&pidVals}, // commenting out until we use it "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", @@ -498,7 +498,7 @@ func TunedPIDErr(name string, tunedVals []PIDConfig) error { } func (conf PIDConfig) String() string { - return fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"} `, conf.P, conf.I, conf.D, conf.Type) + 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. From 372f5402dca37648f28ccb1f245deac35a989ef9 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 16:09:59 -0400 Subject: [PATCH 19/24] fix test --- components/motor/gpio/controlled_test.go | 4 +--- control/setup_control.go | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) 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/setup_control.go b/control/setup_control.go index a7d39f5bfbd..de2e2238740 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -498,7 +498,7 @@ func TunedPIDErr(name string, tunedVals []PIDConfig) error { } func (conf PIDConfig) String() string { - return fmt.Sprintf("{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"%v\"} ", conf.P, conf.I, conf.D, conf.Type) + 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. From 12303b2b10f3061d1962b6ff57f2106b4136907d Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 17:25:13 -0400 Subject: [PATCH 20/24] remove single input output pid --- control/control_loop.go | 9 +- control/control_loop_test.go | 4 +- control/pid.go | 194 ++++++++--------------------------- control/pid_test.go | 170 +----------------------------- control/setup_control.go | 29 +++--- 5 files changed, 64 insertions(+), 342 deletions(-) diff --git a/control/control_loop.go b/control/control_loop.go index a11565b1279..efe54ce8330 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), 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/pid.go b/control/pid.go index d88d5fc666d..205352119ab 100644 --- a/control/pid.go +++ b/control/pid.go @@ -25,18 +25,10 @@ type basicPID struct { mu sync.Mutex cfg BlockConfig logger logging.Logger - // used by the single input/output controller - error float64 - kI float64 - kD float64 - kP float64 - int float64 - tuner pidTuner // MIMO gains + state - PIDSets []*PIDConfig - tuners []*pidTuner - useMulti bool + PIDSets []*PIDConfig + tuners []*pidTuner // used by both y []*Signal @@ -56,17 +48,13 @@ func (p *basicPID) GetTuning() bool { func (p *basicPID) getTuning() bool { multiTune := false - - if p.useMulti { - for _, tuner := range p.tuners { - // the tuners for MIMO are only initialized when we want to tune - if tuner != nil { - multiTune = tuner.tuning || multiTune - } + 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 } - return p.tuner.tuning + return multiTune } // Output returns the discrete step of the PID controller, dt is the delta time between two subsequent call, @@ -77,71 +65,33 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* defer p.mu.Unlock() if p.getTuning() { // Multi Input/Output Implementation - if p.useMulti { - // 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 + + // 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 } - } else { - // Single Input/Output Implementation - out, done := p.tuner.pidTunerStep(math.Abs(x[0].GetSignalValueAt(0)), p.logger) + out, done := p.tuners[i].pidTunerStep(math.Abs(x[0].GetSignalValueAt(i)), p.logger) if done { - p.kD = p.tuner.kD - p.kI = p.tuner.kI - p.kP = p.tuner.kP + 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 are p: %1.6f, i: %1.6f, d: %1.6f", p.kP, p.kI, p.kD) + 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.tuner.tuning = false + p.tuners[i].tuning = false } - p.y[0].SetSignalValueAt(0, out) + p.y[0].SetSignalValueAt(i, out) + // return early to only step this signal + return p.y, true + } } else { - // Multi Input/Output Implementation - if p.useMulti { - for i := 0; i < len(p.PIDSets); i++ { - output := calculateSignalValue(p, x, dt, i) - p.y[0].SetSignalValueAt(i, output) - } - - // Single Input/Output Implementation - } 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 - } - p.y[0].SetSignalValueAt(0, output) + for i := 0; i < len(p.PIDSets); i++ { + output := calculateSignalValue(p, x, dt, i) + p.y[0].SetSignalValueAt(i, output) } } return p.y, true @@ -174,8 +124,6 @@ func calculateSignalValue(p *basicPID, x []*Signal, dt time.Duration, sIndex int func (p *basicPID) reset() error { var ok bool - p.int = 0 - p.error = 0 // 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 @@ -186,26 +134,17 @@ func (p *basicPID) reset() error { return errors.New("PIDSet did not initialize correctly") } if len(p.PIDSets) > 0 { - p.useMulti = true 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 !p.cfg.Attribute.Has("kI") && - !p.cfg.Attribute.Has("kD") && - !p.cfg.Attribute.Has("kP") && !p.useMulti { - return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name) - } - - if len(p.cfg.DependsOn) != 1 && !p.useMulti { - 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) && p.useMulti { + 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)) } @@ -233,58 +172,8 @@ func (p *basicPID) reset() error { p.limLo = p.cfg.Attribute["limit_lo"].(float64) } - if p.useMulti { - for i := 0; i < len(p.PIDSets); i++ { - 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) - } - - tuneMethod := tuneMethodZiegerNicholsPID - if p.cfg.Attribute.Has("tune_method") { - tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) - } - - // 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. - 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) - } - } - } - // 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] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) - } else { - p.kI = p.cfg.Attribute["kI"].(float64) - p.kD = p.cfg.Attribute["kD"].(float64) - p.kP = p.cfg.Attribute["kP"].(float64) - - if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 { + for i := 0; i < len(p.PIDSets); i++ { + if p.PIDSets[i].NeedsAutoTuning() { var ssrVal float64 if p.cfg.Attribute["tune_ssr_value"] != nil { ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64) @@ -300,27 +189,34 @@ func (p *basicPID) reset() error { tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) } - p.tuner = pidTuner{ + // 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. + 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.tuner.reset() + + err := p.tuners[i].reset() if err != nil { return err } - if p.tuner.stepPct > 1 || p.tuner.stepPct < 0 { + 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.y = make([]*Signal, 1) - p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type) } + // 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] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets)) return nil } diff --git a/control/pid_test.go b/control/pid_test.go index ad1082f6865..6c045bc02d1 100644 --- a/control/pid_test.go +++ b/control/pid_test.go @@ -14,107 +14,6 @@ import ( var loop = Loop{} -func TestPIDConfig(t *testing.T) { - logger := logging.NewTestLogger(t) - for i, tc := range []struct { - conf BlockConfig - err string - }{ - { - BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{"kD": 0.11, "kP": 0.12, "kI": 0.22}, - 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}, - Type: "PID", - DependsOn: []string{"A"}, - }, - "", - }, - { - 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", - }, - } { - t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { - _, err := loop.newPID(tc.conf, logger) - if tc.err == "" { - test.That(t, err, test.ShouldBeNil) - } else { - test.That(t, err, test.ShouldNotBeNil) - test.That(t, err.Error(), test.ShouldEqual, tc.err) - } - }) - } -} - -func TestPIDBasicIntegralWindup(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, - "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"}, - } - b, err := loop.newPID(cfg, logger) - pid := b.(*basicPID) - test.That(t, err, test.ShouldBeNil) - s := []*Signal{ - { - name: "A", - signal: make([]float64, 1), - time: make([]int, 1), - }, - } - for i := 0; i < 50; i++ { - dt := time.Duration(1000000 * 10) - s[0].SetSignalValueAt(0, 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) - } else { - test.That(t, pid.int, test.ShouldBeGreaterThanOrEqualTo, 100) - s[0].SetSignalValueAt(0, 0.0) - out, ok := pid.Next(ctx, s, dt) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, pid.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, out[0].GetSignalValueAt(0), test.ShouldAlmostEqual, 88.8778) - 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) -} - func TestPIDMultiConfig(t *testing.T) { logger := logging.NewTestLogger(t) for i, tc := range []struct { @@ -125,9 +24,6 @@ func TestPIDMultiConfig(t *testing.T) { BlockConfig{ Name: "PID1", Attribute: utils.AttributeMap{ - "kD": 0.11, - "kP": 0.12, - "kI": 0.22, "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, }, Type: "PID", @@ -139,9 +35,6 @@ func TestPIDMultiConfig(t *testing.T) { BlockConfig{ Name: "PID1", Attribute: utils.AttributeMap{ - "kD": 0.11, - "kP": 0.12, - "kI": 0.22, "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .12, I: .22, D: .11}}, }, Type: "PID", @@ -152,11 +45,10 @@ func TestPIDMultiConfig(t *testing.T) { { 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) { @@ -177,9 +69,6 @@ func TestPIDMultiIntegralWindup(t *testing.T) { cfg := BlockConfig{ Name: "PID1", Attribute: utils.AttributeMap{ - "kP": 0.12, - "kI": 0.22, - "kD": 0.11, "PIDSets": []*PIDConfig{{P: .12, I: .22, D: .11}, {P: .33, I: .33, D: .10}}, "limit_up": 100.0, "limit_lo": 0.0, @@ -191,7 +80,6 @@ func TestPIDMultiIntegralWindup(t *testing.T) { } b, err := loop.newPID(cfg, logger) pid := b.(*basicPID) - pid.useMulti = true test.That(t, err, test.ShouldBeNil) s := []*Signal{ { @@ -241,8 +129,6 @@ func TestPIDMultiIntegralWindup(t *testing.T) { } 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) @@ -257,55 +143,6 @@ func TestPIDMultiIntegralWindup(t *testing.T) { test.That(t, pid.PIDSets[1].D, test.ShouldEqual, .10) } -func TestPIDTuner(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - cfg := BlockConfig{ - Name: "PID1", - Attribute: utils.AttributeMap{ - "kD": 0.0, - "kP": 0.0, - "kI": 0.0, - "limit_up": 255.0, - "limit_lo": 0.0, - "int_sat_lim_up": 255.0, - "int_sat_lim_lo": 0.0, - "tune_ssr_value": 2.0, - "tune_step_pct": 0.45, - }, - Type: "PID", - DependsOn: []string{"A"}, - } - b, err := loop.newPID(cfg, logger) - pid := b.(*basicPID) - test.That(t, err, test.ShouldBeNil) - test.That(t, pid.GetTuning(), test.ShouldBeTrue) - test.That(t, pid.tuner.currentPhase, test.ShouldEqual, begin) - s := []*Signal{ - { - name: "A", - signal: make([]float64, 1), - 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) - 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) - } - 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) -} - func TestPIDMultiTuner(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) @@ -317,9 +154,6 @@ func TestPIDMultiTuner(t *testing.T) { cfg := BlockConfig{ 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, @@ -335,7 +169,7 @@ func TestPIDMultiTuner(t *testing.T) { pid := b.(*basicPID) test.That(t, err, test.ShouldBeNil) test.That(t, pid.GetTuning(), test.ShouldBeTrue) - test.That(t, pid.tuner.currentPhase, test.ShouldEqual, begin) + test.That(t, pid.tuners[0].currentPhase, test.ShouldEqual, begin) s := []*Signal{ { name: "A", diff --git a/control/setup_control.go b/control/setup_control.go index de2e2238740..404ff3605ed 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -183,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) } } @@ -191,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) } } @@ -200,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 } @@ -220,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 } @@ -285,10 +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}, // commenting out until we use it + "PIDSets": []*PIDConfig{&pidVals}, "limit_lo": -255.0, "limit_up": 255.0, "tune_method": "ziegerNicholsPI", @@ -390,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, From 4528d58ca3638a66ec68930279cc11c541d434f8 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 17:29:28 -0400 Subject: [PATCH 21/24] address comments --- control/control_signal.go | 2 +- control/pid.go | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/control/control_signal.go b/control/control_signal.go index 4f8e8eba8a4..8febd8d46bf 100644 --- a/control/control_signal.go +++ b/control/control_signal.go @@ -25,7 +25,7 @@ func makeSignal(name string, blockType controlBlockType) *Signal { return &s } -// Make Signals returns a Signal object where the length of its signal[] array is dependent +// 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 diff --git a/control/pid.go b/control/pid.go index 205352119ab..163abd59b63 100644 --- a/control/pid.go +++ b/control/pid.go @@ -40,7 +40,7 @@ type basicPID struct { // GetTuning returns whether the PID block is currently tuning any signals. func (p *basicPID) GetTuning() bool { - // using locks so we do not check for tuning mid reconfigure or mid tune + // using locks to prevent reading from tuners while the object is being modified p.mu.Lock() defer p.mu.Unlock() return p.getTuning() @@ -173,6 +173,8 @@ func (p *basicPID) reset() error { } 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 { @@ -189,8 +191,6 @@ func (p *basicPID) reset() error { tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string)) } - // 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. p.tuners[i] = &pidTuner{ limUp: p.limUp, limLo: p.limLo, From a983cfae59c6b68c791a9c18b55d9e054ce7817f Mon Sep 17 00:00:00 2001 From: John Date: Thu, 3 Oct 2024 17:31:31 -0400 Subject: [PATCH 22/24] lint --- control/control_loop.go | 2 +- control/pid.go | 1 - control/setup_control.go | 4 ++-- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/control/control_loop.go b/control/control_loop.go index efe54ce8330..8d5637f069d 100644 --- a/control/control_loop.go +++ b/control/control_loop.go @@ -209,7 +209,7 @@ func (l *Loop) BlockList(ctx context.Context) ([]string, error) { } // GetPIDVals returns the tuned PID values. -// TODO: update this when MIMO fully supported +// TODO: update this when MIMO fully supported. func (l *Loop) GetPIDVals(pidIndex int) PIDConfig { return *l.pidBlocks[pidIndex].PIDSets[0] } diff --git a/control/pid.go b/control/pid.go index 163abd59b63..3f268735f0b 100644 --- a/control/pid.go +++ b/control/pid.go @@ -86,7 +86,6 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]* p.y[0].SetSignalValueAt(i, out) // return early to only step this signal return p.y, true - } } else { for i := 0; i < len(p.PIDSets); i++ { diff --git a/control/setup_control.go b/control/setup_control.go index 404ff3605ed..16511deeda2 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -200,13 +200,13 @@ func (p *PIDLoop) TunePIDLoop(ctx context.Context, cancelFunc context.CancelFunc return errs } -// tunes a single PID block assuming there are two PID blocks in the loop +// 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 pidOld := p.ControlConf.Blocks[blockIndex].Attribute["PIDSets"].([]*PIDConfig) // to tune one set of PID values, the other PI values must be non-zero tempPIDConfigs := make([]*PIDConfig, len(pidOld)) - for index, _ := range pidOld { + for index := range pidOld { tempPIDConfigs[index] = &PIDConfig{P: .001, I: .001} } p.ControlConf.Blocks[blockIndex].Attribute["PIDSets"] = tempPIDConfigs From 87b128304b7564dfa751ff662bde92ccfbaafa60 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 9 Oct 2024 12:32:49 -0400 Subject: [PATCH 23/24] check for in progress tuning --- .../base/sensorcontrolled/sensorcontrolled.go | 22 +++++++++++++++---- components/motor/gpio/controlled.go | 21 +++++++++++++++--- control/setup_control.go | 3 +++ 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go index 14c0f246bdc..c721eee27cd 100644 --- a/components/base/sensorcontrolled/sensorcontrolled.go +++ b/components/base/sensorcontrolled/sensorcontrolled.go @@ -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/motor/gpio/controlled.go b/components/motor/gpio/controlled.go index 71e002642f1..62ef6b3e66b 100644 --- a/components/motor/gpio/controlled.go +++ b/components/motor/gpio/controlled.go @@ -406,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/control/setup_control.go b/control/setup_control.go index 16511deeda2..61e979b97d9 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -488,6 +488,9 @@ func TunedPIDErr(name string, tunedVals []PIDConfig) error { var tunedStr string for _, pid := range tunedVals { if !pid.NeedsAutoTuning() { + if tunedStr != "" { + tunedStr += "," + } tunedStr += pid.String() } } From 7956825e0e0e9084ef5f7bad76329ac3b3ac2a84 Mon Sep 17 00:00:00 2001 From: John Date: Wed, 9 Oct 2024 13:19:31 -0400 Subject: [PATCH 24/24] tweak string --- control/setup_control.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/setup_control.go b/control/setup_control.go index 61e979b97d9..ceede97be8d 100644 --- a/control/setup_control.go +++ b/control/setup_control.go @@ -498,7 +498,7 @@ func TunedPIDErr(name string, tunedVals []PIDConfig) error { } func (conf PIDConfig) String() string { - return fmt.Sprintf(`{"p": %v, "i": %v, "d": %v, "type": "%v"} `, conf.P, conf.I, conf.D, conf.Type) + 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.