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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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/15] 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) } }