[RSDK-6164] 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

ticket: https://viam.atlassian.net/browse/RSDK-6164

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

eventually I think yes and just index 0 any time it's single input. but that does seem like a problem for a future PR

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-10-03T19:13:04Z

control/pid.go

+		for i := 0; i < len(p.PIDSets); i++ {
+			if p.PIDSets[i].NeedsAutoTuning() {
+				var ssrVal float64
+				if p.cfg.Attribute["tune_ssr_value"] != nil {


so in this case all pid's in a MIMO block need to use the same values? (excluding P, I, and D). otherwise how would it know which "tune_ssr_value" to use?

jk I see it in a comment below

optional nit to move that comment from line 254 to line ~239, but nbd

martha-johnston · 2024-10-03T19:17:46Z

control/setup_control.go

+	// PID block specific values
+	// these are integral sum and signalErr for the pid signal
+	int       float64
+	signalErr float64


what are these used for in setup?

they are not used in setup, they are used within the PID loop. we made them private members so implementers will not try to use them

ah my bad I thought this was part of the PIDLoop struct so I was confused what their purpose was but this makes more sense

martha-johnston

left a couple minor questions and comments, but lgtm! excited to have this!

JohnN193 · 2024-10-03T21:36:25Z

code is updated to use the MIMO configuration for the PID block everywhere!

martha-johnston · 2024-10-04T13:24:30Z

still lgtm but I do have one test question. can you check the case that you tune your pid and then you try and move it without copying the values? make sure that error looks right and doesn't move the motors and everything. for 1 and for multiple signals? (if that's supported right now)

JohnN193 · 2024-10-04T13:59:31Z

yeah i tested on app. the errors look good and you can copy/paste them directly into the config. the doCommand looks a lil weird but its obvious what to modify (need to remove backslashes on the quotes")

martha-johnston · 2024-10-04T14:02:02Z

Yeah I couldn’t figure out how to get rid of those ticks when it was a string. Fine with leaving for now, I think external users are probably less likely to use DoCommand rather than just see the error and copy from there.

JohnN193 · 2024-10-09T18:13:17Z

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 {


this function is now effectively the same for both controlled motor and sensorbase, but going to wait until the refactor ticket to turn them into one function in case there are any other optimizations we can find wrt determining if tuning was completed. @martha-johnston

JohnN193 · 2024-10-09T18:14:01Z

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


turns out the loop is always nil at this point because we do not set the loop until tuning completes.

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