[RSDK-6164] Mimo pid updates

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

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
 	}

control/control_loop.go

@@ -339,17 +339,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_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
 }
 
+// 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)
+	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