RSDK-6282 [part 1]: add PID values to the gpio motor config (viamrobo…

…tics#3435)

components/motor/gpio/loop.go

@@ -93,48 +93,40 @@ func (m *EncodedMotor) validateControlConfig(ctx context.Context) error {
 // trapezoidalVelocityProfile-> sum -> PID -> gain -> endpoint -> derivative back to sum, and endpoint
 // back to trapezoidalVelocityProfile structure. The gain is 0.0039 (1/255) to account for the PID range,
 // the PID values are experimental this structure can change as hardware experiments with an encoded motor require.
-func (m *EncodedMotor) createControlLoopConfig() control.Config {
-	return control.Config{
+func (m *EncodedMotor) createControlLoopConfig(p, i, d float64) control.Config {
+	conf := control.Config{
 		Blocks: []control.BlockConfig{
 			{
 				Name: "set_point",
 				Type: "constant",
 				Attribute: rdkutils.AttributeMap{
-					"constant_val": 0,
+					"constant_val": 0.0,
 				},
 			},
 			{
 				Name: "trapz",
 				Type: "trapezoidalVelocityProfile",
 				Attribute: rdkutils.AttributeMap{
 					"kpp_gain":   0.45,
-					"max_acc":    30000,
-					"max_vel":    4000,
-					"pos_window": 0,
+					"max_acc":    30000.0,
+					"max_vel":    4000.0,
+					"pos_window": 0.0,
 				},
 				DependsOn: []string{"set_point", "endpoint"},
 			},
-			{
-				Name: "sum",
-				Type: "sum",
-				Attribute: rdkutils.AttributeMap{
-					"sum_string": "+-",
-				},
-				DependsOn: []string{"trapz", "derivative"},
-			},
 			{
 				Name: "PID",
 				Type: "PID",
 				Attribute: rdkutils.AttributeMap{
-					"int_sat_lim_lo": -255,
-					"int_sat_lim_up": 255,
-					"kD":             0,
-					"kI":             0.53977,
-					"kP":             0.048401,
-					"limit_lo":       -255,
-					"limit_up":       255,
-					"tune_method":    "ziegerNicholsSomeOvershoot",
-					"tune_ssr_value": 2,
+					"int_sat_lim_lo": -255.0,
+					"int_sat_lim_up": 255.0,
+					"kD":             d,
+					"kI":             i,
+					"kP":             p,
+					"limit_lo":       -255.0,
+					"limit_up":       255.0,
+					"tune_method":    "ziegerNichlsPI",
+					"tune_ssr_value": 2.0,
 					"tune_step_pct":  0.35,
 				},
 				DependsOn: []string{"sum"},
@@ -151,7 +143,7 @@ func (m *EncodedMotor) createControlLoopConfig() control.Config {
 				Name: "endpoint",
 				Type: "endpoint",
 				Attribute: rdkutils.AttributeMap{
-					"motor_name": "motor",
+					"motor_name": m.Name().ShortName(),
 				},
 				DependsOn: []string{"gain"},
 			},
@@ -164,6 +156,31 @@ func (m *EncodedMotor) createControlLoopConfig() control.Config {
 				DependsOn: []string{"endpoint"},
 			},
 		},
-		Frequency: 100,
+		Frequency: 100.0,
+	}
+
+	if p == 0.0 && i == 0.0 && d == 0.0 {
+		// when tuning (all PID are zero), the loop has to exclude the trapz block, so the sum block is
+		// temporarily changed to depend on the constant block instead of the trapz block to pass over trapz
+		conf = setSumBlock(conf, "set_point")
+	} else {
+		// if not tuning, set the sum block to once again depend
+		// on the trapz block instead of the constant bloc
+		conf = setSumBlock(conf, "trapz")
+	}
+
+	return conf
+}
+
+func setSumBlock(conf control.Config, firstDependency string) control.Config {
+	sumBlock := control.BlockConfig{
+		Name: "sum",
+		Type: "sum",
+		Attribute: rdkutils.AttributeMap{
+			"sum_string": "+-",
+		},
+		DependsOn: []string{firstDependency, "derivative"},
 	}
+	conf.Blocks = append(conf.Blocks, sumBlock)
+	return conf
 }

components/motor/gpio/motor_encoder.go

@@ -102,26 +102,40 @@ func newEncodedMotor(
 	em.encoder = realEncoder
 
 	// setup control loop
-	if len(motorConfig.ControlLoop.Blocks) != 0 {
-		em.controlLoopConfig = em.createControlLoopConfig()
-		cLoop, err := control.NewLoop(em.logger, em.cfg.ControlLoop, em)
+	if motorConfig.ControlParameters != nil {
+		// create control loop config with PID values from config
+		em.controlLoopConfig = em.createControlLoopConfig(
+			motorConfig.ControlParameters.P,
+			motorConfig.ControlParameters.I,
+			motorConfig.ControlParameters.D,
+		)
+
+		cLoop, err := control.NewLoop(em.logger, em.controlLoopConfig, em)
 		if err != nil {
 			em.logger.Error(err)
 		}
 		if err = cLoop.Start(); err != nil {
 			em.logger.Error(err)
 		}
 		em.loop = cLoop
+
+		// validate control loop config
 		if err = em.validateControlConfig(cancelCtx); err != nil {
 			return nil, err
 		}
-		pidBlock, err := em.loop.ConfigAt(cancelCtx, "PID")
-		if err != nil {
-			return nil, err
-		}
-		if pidBlock.Attribute["kP"].(float64) == 0.0 && pidBlock.Attribute["kI"].(float64) == 0.0 && pidBlock.Attribute["kD"].(float64) == 0.0 {
-			em.loop.SetTuning(cancelCtx, true)
+
+		// SetTuning to true if all PID values are 0
+		if motorConfig.ControlParameters.P == 0.0 &&
+			motorConfig.ControlParameters.I == 0.0 &&
+			motorConfig.ControlParameters.D == 0.0 {
+			em.loop.SetTuning(context.Background(), true)
+		} else {
+			em.loop.SetTuning(context.Background(), false)
 		}
+	} else {
+		// TODO DOCS-1524: link to docs that explain control parameters
+		em.logger.Warn(
+			"recommended: for more accurate motor control, configure 'control_parameters' in the motor config")
 	}
 
 	if em.rampRate < 0 || em.rampRate > 1 {
@@ -195,8 +209,8 @@ func (m *EncodedMotor) rpmMonitorStart() {
 		}
 	}
 	// create new control loop if control config exists
-	if len(m.cfg.ControlLoop.Blocks) != 0 {
-		cLoop, err := control.NewLoop(m.logger, m.cfg.ControlLoop, m)
+	if m.cfg.ControlParameters != nil {
+		cLoop, err := control.NewLoop(m.logger, m.controlLoopConfig, m)
 		if err != nil {
 			m.logger.Error(err)
 		}

components/motor/gpio/motor_encoder_test.go

@@ -16,11 +16,9 @@ import (
 	"go.viam.com/rdk/components/encoder/single"
 	"go.viam.com/rdk/components/motor"
 	fakemotor "go.viam.com/rdk/components/motor/fake"
-	"go.viam.com/rdk/control"
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/operation"
 	"go.viam.com/rdk/resource"
-	"go.viam.com/rdk/utils"
 )
 
 func nowNanosTest() uint64 {
@@ -77,79 +75,10 @@ func MakeIncrementalBoard(t *testing.T) *fakeboard.Board {
 	return &b
 }
 
-func MakeControlConfig(t *testing.T) control.Config {
-	cfg := control.Config{
-		Blocks: []control.BlockConfig{
-			{
-				Name: "endpoint",
-				Type: "endpoint",
-				Attribute: utils.AttributeMap{
-					"motor_name": "MotorFake",
-				},
-				DependsOn: []string{"PID"},
-			},
-			{
-				Name: "trapz",
-				Type: "trapezoidalVelocityProfile",
-				Attribute: utils.AttributeMap{
-					"max_acc":    30000.0,
-					"max_vel":    4000.0,
-					"pos_window": 0.0,
-					"kpp_gain":   0.45,
-				},
-				DependsOn: []string{"set_point", "endpoint"},
-			},
-			{
-				Name: "sum",
-				Type: "sum",
-				Attribute: utils.AttributeMap{
-					"sum_string": "+-",
-				},
-				DependsOn: []string{"derivative", "trapz"},
-			},
-			{
-				Name: "set_point",
-				Type: "constant",
-				Attribute: utils.AttributeMap{
-					"constant_val": 0.0,
-				},
-				DependsOn: []string{},
-			},
-			{
-				Name: "derivative",
-				Type: "derivative",
-				Attribute: utils.AttributeMap{
-					"derive_type": "backward1st1",
-				},
-				DependsOn: []string{"endpoint"},
-			},
-			{
-				Name: "PID",
-				Type: "PID",
-				Attribute: utils.AttributeMap{
-					"tune_ssr_value": 2.0,
-					"kI":             0.53977,
-					"int_sat_lim_lo": -255.0,
-					"int_sat_lim_up": 255.0,
-					"limit_up":       255.0,
-					"tune_method":    "ziegerNicholsSomeOvershoot",
-					"limit_lo":       -255.0,
-					"tune_step_pct":  0.35,
-					"kD":             0.0,
-					"kP":             0.048401,
-				},
-				DependsOn: []string{"sum"},
-			},
-		},
-		Frequency: 100.0,
-	}
-	return cfg
-}
-
 func TestCreateMotorWithControls(t *testing.T) {
 	logger := logging.NewTestLogger(t)
-	controlCfg := MakeControlConfig(t)
-	cfg := Config{TicksPerRotation: 100, MaxRPM: 100, ControlLoop: controlCfg}
+	controlParams := motorPIDConfig{P: 0.2, I: 0.2, D: 0.0}
+	cfg := Config{TicksPerRotation: 100, MaxRPM: 100, ControlParameters: &controlParams}
 	fakeMotor := &fakemotor.Motor{
 		MaxRPM:           100,
 		Logger:           logger,

components/motor/gpio/setup.go

@@ -8,7 +8,6 @@ import (
 	"go.viam.com/rdk/components/board"
 	"go.viam.com/rdk/components/encoder"
 	"go.viam.com/rdk/components/motor"
-	"go.viam.com/rdk/control"
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/resource"
 )
@@ -101,19 +100,26 @@ func (conf *PinConfig) MotorType(path string) (MotorType, error) {
 	return motorType, nil
 }
 
+// PIDConfig contains the PID values that are accessible from controlled component configs.
+type motorPIDConfig struct {
+	P float64 `json:"p"`
+	I float64 `json:"i"`
+	D float64 `json:"d"`
+}
+
 // Config describes the configuration of a motor.
 type Config struct {
-	Pins             PinConfig      `json:"pins"`
-	BoardName        string         `json:"board"`
-	MinPowerPct      float64        `json:"min_power_pct,omitempty"` // min power percentage to allow for this motor default is 0.0
-	MaxPowerPct      float64        `json:"max_power_pct,omitempty"` // max power percentage to allow for this motor (0.06 - 1.0)
-	PWMFreq          uint           `json:"pwm_freq,omitempty"`
-	DirectionFlip    bool           `json:"dir_flip,omitempty"`       // Flip the direction of the signal sent if there is a Dir pin
-	ControlLoop      control.Config `json:"control_config,omitempty"` // Optional control loop
-	Encoder          string         `json:"encoder,omitempty"`        // name of encoder
-	RampRate         float64        `json:"ramp_rate,omitempty"`      // how fast to ramp power to motor when using rpm control
-	MaxRPM           float64        `json:"max_rpm,omitempty"`
-	TicksPerRotation int            `json:"ticks_per_rotation,omitempty"`
+	Pins              PinConfig       `json:"pins"`
+	BoardName         string          `json:"board"`
+	MinPowerPct       float64         `json:"min_power_pct,omitempty"` // min power percentage to allow for this motor default is 0.0
+	MaxPowerPct       float64         `json:"max_power_pct,omitempty"` // max power percentage to allow for this motor (0.06 - 1.0)
+	PWMFreq           uint            `json:"pwm_freq,omitempty"`
+	DirectionFlip     bool            `json:"dir_flip,omitempty"`  // Flip the direction of the signal sent if there is a Dir pin
+	Encoder           string          `json:"encoder,omitempty"`   // name of encoder
+	RampRate          float64         `json:"ramp_rate,omitempty"` // how fast to ramp power to motor when using rpm control
+	MaxRPM            float64         `json:"max_rpm,omitempty"`
+	TicksPerRotation  int             `json:"ticks_per_rotation,omitempty"`
+	ControlParameters *motorPIDConfig `json:"control_parameters,omitempty"`
 }
 
 // Validate ensures all parts of the config are valid.

control/control_loop.go

@@ -195,7 +195,10 @@ func (l *Loop) SetConfigAt(ctx context.Context, name string, config BlockConfig)
 	if !ok {
 		return errors.Errorf("cannot return Config for nonexistent %s", name)
 	}
-	return blk.blk.UpdateConfig(ctx, config)
+	if err := blk.blk.UpdateConfig(ctx, config); err != nil {
+		return err
+	}
+	return nil
 }
 
 // BlockList returns the list of blocks in a control loop error when the list is empty.

control/pid.go

@@ -51,7 +51,7 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]*
 			p.kD = p.tuner.kD
 			p.kI = p.tuner.kI
 			p.kP = p.tuner.kP
-			p.logger.CInfof(ctx, "Calculated gains are Kp %1.6f, Ki: %1.6f, Kd: %1.6f", p.kP, p.kI, p.kD)
+			p.logger.CInfof(ctx, "Calculated gains are p: %1.6f, i: %1.6f, d: %1.6f", p.kP, p.kI, p.kD)
 			p.tuning = false
 		}
 		p.y[0].SetSignalValueAt(0, out)