[RSDK-6164] update PID block to support MIMO (#4290)

components/base/sensorcontrolled/sensorcontrolled.go

@@ -280,7 +280,7 @@ func (sb *sensorBase) DoCommand(ctx context.Context, req map[string]interface{})
 		var respStr string
 		for _, pidConf := range *sb.tunedVals {
 			if !pidConf.NeedsAutoTuning() {
-				respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ", pidConf.P, pidConf.I, pidConf.D, pidConf.Type)
+				respStr += pidConf.String()
 			}
 		}
 		resp[getPID] = respStr
@@ -352,11 +352,25 @@ func (sb *sensorBase) determineHeadingFunc(ctx context.Context,
 // if loop is tuning, return an error
 // if loop has been tuned but the values haven't been added to the config, error with tuned values.
 func (sb *sensorBase) checkTuningStatus() error {
-	if sb.loop != nil && sb.loop.GetTuning(context.Background()) {
+	done := true
+	needsTuning := false
+
+	for i := range sb.configPIDVals {
+		// check if the current signal needed tuning
+		if sb.configPIDVals[i].NeedsAutoTuning() {
+			// return true if either signal needed tuning
+			needsTuning = needsTuning || true
+			// if the tunedVals have not been updated, then tuning is still in progress
+			done = done && !(*sb.tunedVals)[i].NeedsAutoTuning()
+		}
+	}
+
+	if needsTuning {
+		if done {
+			return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals)
+		}
 		return control.TuningInProgressErr(sb.Name().ShortName())
-	} else if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) ||
-		(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) {
-		return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals)
 	}
+
 	return nil
 }

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

components/motor/gpio/controlled.go

@@ -2,7 +2,6 @@ package gpio
 
 import (
 	"context"
-	"fmt"
 	"math"
 	"sync"
 	"time"
@@ -396,8 +395,7 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa
 	if ok {
 		var respStr string
 		if !(*cm.tunedVals)[0].NeedsAutoTuning() {
-			respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ",
-				(*cm.tunedVals)[0].P, (*cm.tunedVals)[0].I, (*cm.tunedVals)[0].D, (*cm.tunedVals)[0].Type)
+			respStr += (*cm.tunedVals)[0].String()
 		}
 		resp[getPID] = respStr
 	}
@@ -408,10 +406,25 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa
 // if loop is tuning, return an error
 // if loop has been tuned but the values haven't been added to the config, error with tuned values.
 func (cm *controlledMotor) checkTuningStatus() error {
-	if cm.loop != nil && cm.loop.GetTuning(context.Background()) {
+	done := true
+	needsTuning := false
+
+	for i := range cm.configPIDVals {
+		// check if the current signal needed tuning
+		if cm.configPIDVals[i].NeedsAutoTuning() {
+			// return true if either signal needed tuning
+			needsTuning = needsTuning || true
+			// if the tunedVals have not been updated, then tuning is still in progress
+			done = done && !(*cm.tunedVals)[i].NeedsAutoTuning()
+		}
+	}
+
+	if needsTuning {
+		if done {
+			return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals)
+		}
 		return control.TuningInProgressErr(cm.Name().ShortName())
-	} else if cm.configPIDVals[0].NeedsAutoTuning() && !(*cm.tunedVals)[0].NeedsAutoTuning() {
-		return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals)
 	}
+
 	return nil
 }

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

control/control_loop.go

@@ -209,12 +209,9 @@ func (l *Loop) BlockList(ctx context.Context) ([]string, error) {
 }
 
 // GetPIDVals returns the tuned PID values.
+// TODO: update this when MIMO fully supported.
 func (l *Loop) GetPIDVals(pidIndex int) PIDConfig {
-	return PIDConfig{
-		P: l.pidBlocks[pidIndex].kP,
-		I: l.pidBlocks[pidIndex].kI,
-		D: l.pidBlocks[pidIndex].kD,
-	}
+	return *l.pidBlocks[pidIndex].PIDSets[0]
 }
 
 // Frequency returns the loop's frequency.
@@ -227,7 +224,7 @@ func (l *Loop) Start() error {
 	if len(l.ts) == 0 {
 		return errors.New("cannot start the control loop if there are no blocks depending on an impulse")
 	}
-	l.logger.Infof("Running loop on %1.4f %+v\r\n", l.cfg.Frequency, l.dt)
+	l.logger.Infof("Running control loop at %1.4f Hz, %+v\r\n", l.cfg.Frequency, l.dt)
 	l.ct = controlTicker{
 		ticker: time.NewTicker(l.dt),
 		stop:   make(chan bool, 1),
@@ -339,17 +336,29 @@ func (l *Loop) GetConfig(ctx context.Context) Config {
 func (l *Loop) MonitorTuning(ctx context.Context) {
 	// wait until tuning has started
 	for {
-		tuning := l.GetTuning(ctx)
-		if tuning {
-			break
+		// 100 Hz is probably faster than we need, but we needed at least a small delay because
+		// GetTuning will lock the PID block
+		if utils.SelectContextOrWait(ctx, 10*time.Millisecond) {
+			tuning := l.GetTuning(ctx)
+			if tuning {
+				break
+			}
+			continue
 		}
+		l.logger.Error("error starting tuner")
+		return
 	}
 	// wait until tuning is done
 	for {
-		tuning := l.GetTuning(ctx)
-		if !tuning {
-			break
+		if utils.SelectContextOrWait(ctx, 10*time.Millisecond) {
+			tuning := l.GetTuning(ctx)
+			if !tuning {
+				break
+			}
+			continue
 		}
+		l.logger.Error("error waiting for tuner")
+		return
 	}
 }
 

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"},
 			},

control/control_signal.go

@@ -1,6 +1,8 @@
 package control
 
-import "sync"
+import (
+	"sync"
+)
 
 // Signal holds any data passed between blocks.
 type Signal struct {
@@ -23,11 +25,23 @@ func makeSignal(name string, blockType controlBlockType) *Signal {
 	return &s
 }
 
+// makeSignals returns a Signal object where the length of its signal[] array is dependent
+// on the number of PIDSets from the config.
+func makeSignals(name string, blockType controlBlockType, dimension int) *Signal {
+	var s Signal
+	s.dimension = dimension
+	s.signal = make([]float64, dimension)
+	s.time = make([]int, dimension)
+	s.name = name
+	s.blockType = blockType
+	return &s
+}
+
 // GetSignalValueAt returns the value of the signal at an index, threadsafe.
 func (s *Signal) GetSignalValueAt(i int) float64 {
 	s.mu.Lock()
 	defer s.mu.Unlock()
-	if i > len(s.signal)-1 {
+	if !(i < len(s.signal)) {
 		return 0.0
 	}
 	return s.signal[i]
@@ -37,7 +51,7 @@ func (s *Signal) GetSignalValueAt(i int) float64 {
 func (s *Signal) SetSignalValueAt(i int, val float64) {
 	s.mu.Lock()
 	defer s.mu.Unlock()
-	if i > len(s.signal)-1 {
+	if !(i < len(s.signal)) {
 		return
 	}
 	s.signal[i] = val