apply pr feedback

viamrobotics · Sep 5, 2024 · b4db62f · b4db62f
1 parent 4eabe04
commit b4db62f
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 20 deletions.
diff --git a/components/base/sensorcontrolled/movestraight.go b/components/base/sensorcontrolled/movestraight.go
@@ -4,11 +4,12 @@ package sensorcontrolled
 import (
 	"context"
 	"errors"
-	"fmt"
 	"math"
 	"time"
 
 	geo "github.com/kellydunn/golang-geo"
+
+	"go.viam.com/rdk/control"
 )
 
 const (
@@ -58,11 +59,7 @@ func (sb *sensorBase) MoveStraight(
 	// if loop has been tuned but the values haven't been added to the config, error with tuned values
 	if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) ||
 		(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) {
-		return fmt.Errorf("%v has been tuned, copy these control parameters into the config to enable movement: "+
-			"\"control_parameters\": [{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"linear_velocity\"}, "+
-			"{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"angular_velocity\"}]",
-			sb.Name().ShortName(), (*sb.tunedVals)[0].P, (*sb.tunedVals)[0].I, (*sb.tunedVals)[0].D,
-			(*sb.tunedVals)[1].P, (*sb.tunedVals)[1].I, (*sb.tunedVals)[1].D)
+		return control.TunedPIDErr(sb.tunedVals, sb.Name().ShortName())
 	}
 
 	// make sure the control loop is enabled

diff --git a/components/base/sensorcontrolled/sensorcontrolled.go b/components/base/sensorcontrolled/sensorcontrolled.go
@@ -210,7 +210,7 @@ func (sb *sensorBase) Reconfigure(ctx context.Context, deps resource.Dependencie
 				// configPIDVals at index 1 is angular
 				sb.configPIDVals[1] = c
 			default:
-				sb.logger.Warn("control_parameters type must be 'linear_velocity' or 'angular_velocity'")
+				sb.logger.Error("control_parameters type must be 'linear_velocity' or 'angular_velocity'")
 			}
 		}
 
@@ -269,7 +269,7 @@ func (sb *sensorBase) DoCommand(ctx context.Context, req map[string]interface{})
 	defer sb.mu.Unlock()
 	ok := req[getPID].(bool)
 	if ok {
-		resp[getPID] = fmt.Sprintf(`tuned PID vals: {p: %v, i: %v, d: %v, type: linear_velocity}, {p: %v, i: %v, d: %v, type: angular_velocity}`,
+		resp[getPID] = fmt.Sprintf(`"control_parameters": [{p: %v, i: %v, d: %v, type: linear_velocity}, {p: %v, i: %v, d: %v, type: angular_velocity}]`,
 			(*sb.tunedVals)[0].P, (*sb.tunedVals)[0].I, (*sb.tunedVals)[0].D, (*sb.tunedVals)[1].P, (*sb.tunedVals)[1].I, (*sb.tunedVals)[1].D)
 	}
 

diff --git a/components/base/sensorcontrolled/spin.go b/components/base/sensorcontrolled/spin.go
@@ -3,9 +3,10 @@ package sensorcontrolled
 import (
 	"context"
 	"errors"
-	"fmt"
 	"math"
 	"time"
+
+	"go.viam.com/rdk/control"
 )
 
 const (
@@ -36,11 +37,7 @@ func (sb *sensorBase) Spin(ctx context.Context, angleDeg, degsPerSec float64, ex
 	// if loop has been tuned but the values haven't been added to the config, error with tuned values
 	if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) ||
 		(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) {
-		return fmt.Errorf("%v has been tuned, copy these control parameters into the config to enable movement: "+
-			"\"control_parameters\": [{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"linear_velocity\"}, "+
-			"{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"angular_velocity\"}]",
-			sb.Name().ShortName(), (*sb.tunedVals)[0].P, (*sb.tunedVals)[0].I, (*sb.tunedVals)[0].D,
-			(*sb.tunedVals)[1].P, (*sb.tunedVals)[1].I, (*sb.tunedVals)[1].D)
+		return control.TunedPIDErr(sb.tunedVals, sb.Name().ShortName())
 	}
 
 	prevAngle, hasOrientation, err := sb.headingFunc(ctx)

diff --git a/components/base/sensorcontrolled/velocities.go b/components/base/sensorcontrolled/velocities.go
@@ -2,7 +2,6 @@ package sensorcontrolled
 
 import (
 	"context"
-	"fmt"
 	"math"
 
 	"github.com/golang/geo/r3"
@@ -28,11 +27,7 @@ func (sb *sensorBase) SetVelocity(
 	// if loop has been tuned but the values haven't been added to the config, error with tuned values
 	if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) ||
 		(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) {
-		return fmt.Errorf("%v has been tuned, copy these control parameters into the config to enable movement: "+
-			"\"control_parameters\": [{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"linear_velocity\"}, "+
-			"{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"angular_velocity\"}]",
-			sb.Name().ShortName(), (*sb.tunedVals)[0].P, (*sb.tunedVals)[0].I, (*sb.tunedVals)[0].D,
-			(*sb.tunedVals)[1].P, (*sb.tunedVals)[1].I, (*sb.tunedVals)[1].D)
+		return control.TunedPIDErr(sb.tunedVals, sb.Name().ShortName())
 	}
 
 	// make sure the control loop is enabled

diff --git a/control/setup_control.go b/control/setup_control.go
@@ -2,6 +2,7 @@ package control
 
 import (
 	"context"
+	"fmt"
 	"sync"
 
 	"github.com/pkg/errors"
@@ -473,3 +474,12 @@ func UpdateTrapzBlock(ctx context.Context, name string, maxVel float64, dependsO
 	}
 	return nil
 }
+
+// TunedPIDErr returns an error with the stored tuned PID values.
+func TunedPIDErr(tunedVals *[]PIDConfig, name string) error {
+	return fmt.Errorf("%v has been tuned, copy these control parameters into the config to enable movement: "+
+		"\"control_parameters\": [{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"linear_velocity\"}, "+
+		"{\"p\": %v, \"i\": %v, \"d\": %v, \"type\": \"angular_velocity\"}]",
+		name, (*tunedVals)[0].P, (*tunedVals)[0].I, (*tunedVals)[0].D,
+		(*tunedVals)[1].P, (*tunedVals)[1].I, (*tunedVals)[1].D)
+}