From a8171144f200486e9971485e070a9e3f3aab4c2e Mon Sep 17 00:00:00 2001 From: martha-johnston <106617924+martha-johnston@users.noreply.github.com> Date: Thu, 25 Jan 2024 09:08:29 -0500 Subject: [PATCH] RSDK-6282 [part 1]: add PID values to the gpio motor config (#3435) --- components/motor/gpio/loop.go | 67 +++++++++++------- components/motor/gpio/motor_encoder.go | 36 +++++++--- components/motor/gpio/motor_encoder_test.go | 75 +-------------------- components/motor/gpio/setup.go | 30 +++++---- control/control_loop.go | 5 +- control/pid.go | 2 +- 6 files changed, 92 insertions(+), 123 deletions(-) diff --git a/components/motor/gpio/loop.go b/components/motor/gpio/loop.go index 46e209159fa..a5af2f2d695 100644 --- a/components/motor/gpio/loop.go +++ b/components/motor/gpio/loop.go @@ -93,14 +93,14 @@ 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, }, }, { @@ -108,33 +108,25 @@ func (m *EncodedMotor) createControlLoopConfig() control.Config { 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 } diff --git a/components/motor/gpio/motor_encoder.go b/components/motor/gpio/motor_encoder.go index 64ded2c2efd..078d3a4bbaf 100644 --- a/components/motor/gpio/motor_encoder.go +++ b/components/motor/gpio/motor_encoder.go @@ -102,9 +102,15 @@ 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) } @@ -112,16 +118,24 @@ func newEncodedMotor( 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) } diff --git a/components/motor/gpio/motor_encoder_test.go b/components/motor/gpio/motor_encoder_test.go index 323a84560e4..f462f29f317 100644 --- a/components/motor/gpio/motor_encoder_test.go +++ b/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, diff --git a/components/motor/gpio/setup.go b/components/motor/gpio/setup.go index eee824fabbc..60379326cfd 100644 --- a/components/motor/gpio/setup.go +++ b/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. diff --git a/control/control_loop.go b/control/control_loop.go index dc9dda8f7df..4dce325e93b 100644 --- a/control/control_loop.go +++ b/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. diff --git a/control/pid.go b/control/pid.go index e4f2fb978c4..b1cd3830865 100644 --- a/control/pid.go +++ b/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)