Mimo pid updates #4290

mariapatni · 2024-08-16T18:27:05Z

Added Multi Input Output Functionality to the PID Controller Block of the Controls Package.

Tested on hardware that the MIMO code will work with tuning single signals, and unit tests cover the expected behavior for multi signals

CLAassistant · 2024-08-16T18:27:16Z

All committers have signed the CLA.

CLAassistant · 2024-08-16T18:27:17Z

Thank you for your submission! We really appreciate it. Like many open source projects, we ask that you sign our Contributor License Agreement before we can accept your contribution.
_{You have signed the CLA already but the status is still pending? Let us recheck it.}

JohnN193

heres some thoughts/notes I had for the first half, looking thru the rest of the code now!

control/control_block.go

JohnN193 · 2024-08-16T18:30:04Z

control/control_signal.go

@@ -20,14 +23,31 @@ 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)


remove before merging. running make lint-go should also help catch these

JohnN193 · 2024-08-16T18:56:58Z

control/pid.go

+		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++ {


add a comment that this attempts to tune multiple signals simultaneously

just as an fyi I don't think tuning simultaneously would work for the base. or if you wanted to tune motors and a base? although maybe this change will allow it to do so

i kinda agree with you. I think I might try changing this to tune signals one at a time

updated the code to tune one signal at a time. the tests are alittle weird with this change(have to turn off tuning after we reach the "end" of the test) but otherwise i think this should work the way we want

control/pid.go

JohnN193 · 2024-08-16T19:00:20Z

control/pid.go

+		if p.useMulti {
+
+			for i := 0; i < len(p.PIDSets); i++ {
+				output := calculateSignalValue(p, x, dt, i)


can this function be used for the single input case?

Not going to expand this for the single input case. will hold off on making large changes to the single input case

control/pid.go

JohnN193

a few other notes. wondering if we should add a constructor funciton or anything to PIDConfig since we added a new function to it.

Also just noticed the change in setup_control, so if ya want to test this on hardware then we can

JohnN193 · 2024-08-16T19:08:08Z

control/pid.go

+				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))
+				}
+
+				// 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,
+				}


would it be worth making a helper function for this code? since both the mimo and single input/output code use it. no is an okay answer since we eventually only want one code path

leaving this code as is, as we will eventually remove the single input/output path once MIMO is ready

control/pid_test.go

control/setup_control.go

JohnN193 · 2024-08-20T21:33:48Z

control/pid.go

 func (p *basicPID) GetTuning() bool {
-	return p.tuning
+	// using locks so we do not check for tuning mid reconfigure or mid tune
+	p.mu.Lock()
+	defer p.mu.Unlock()
+	return p.getTuning()
+}


might be able to remove the locks from this. I added them because I was worried a reconfigure was breaking some logic, but i also don't hate keeping it?

I think we can leave it if it's not causing issues. better to be safe

okay thinking about this more, if we are locking here and then waiting one level up because of the locking, maybe we can safely get rid of both? if not it's definitely fine but something to maybe try? because the motor is always rebuild so it seems less important to lock in case of reconfigure issues but that might be a bad opinion. we can discuss it further but if you feel strongly towards keeping both, fine by me

thinking about it more I want to keep the locks. since both the PID block and whoever is using Get/MonitorTuning are trying to access the same variables at the same, we need the lock to prevent race conditions.

JohnN193 · 2024-08-20T21:44:50Z

control/control_loop.go

-			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) {


I can try to remove these if ya want. Was worried that calling GetTuning was affecting things at the time when I added this.

this is fine and probably safer. happy with leaving it.

martha-johnston · 2024-09-03T20:24:01Z

control/pid.go

+
+	if p.useMulti {
+		for _, tuner := range p.tuners {
+			// the tuners  for MIMO only get created if we want to tune


this might become clear later on in my review, but if we only create tuners for PID blocks that need to be tuned, will there be any issues with the tuners being associated with the wrong blocks in a list or anything like that?

martha-johnston · 2024-09-03T20:29:28Z

control/pid.go

+			for i := 0; i < len(p.PIDSets); i++ {
+				p.PIDSets[i].int = 0
+				p.PIDSets[i].signalErr = 0
+			}


okay so in reference to my above comment about mismatch in number of pids and number of tuners, it seems like that comment is just wrong, because we're making a tuner for every pid block right?

yeah I guess the comment is abit misleading. we make a structure that holds up to N tuners, based on the number of pid sets/gains/signals configured.

a tuner only gets initialized if a pid set needs tuning, otherwise the tuning boolean will be set to false

I'll update the comment in getTuning to reflect this behavior

martha-johnston · 2024-09-03T20:34:21Z

@JohnN193 did a first pass and left a couple comments. looks pretty great, so exciting! very interested to talk in more detail and see some hardware testing when you're back

…gs during tuning

mariapatni added 2 commits August 16, 2024 13:15

MIMO on PID Block done, need to clean up code

358fe3e

added some comments and cleaned up some code

252a42a

mariapatni requested review from JohnN193 and martha-johnston August 16, 2024 18:27

viambot added the safe to test This pull request is marked safe to test from a trusted zone label Aug 16, 2024

ran linter

21c3e33

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Aug 16, 2024

JohnN193 reviewed Aug 16, 2024

View reviewed changes

tune signals one at a time

31e3453