Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RSDK-6282 [part 1]: add PID values to the gpio motor config #3435

Merged
merged 6 commits into from
Jan 25, 2024
Merged

RSDK-6282 [part 1]: add PID values to the gpio motor config #3435

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
77f3e14
use PID values from config for encoded motor
martha-johnston Jan 12, 2024
283ec2d
set tuning to false
martha-johnston Jan 12, 2024
252959e
helper function for sum block
martha-johnston Jan 18, 2024
2575381
fix issue of using two motors at once
martha-johnston Jan 24, 2024
d99b787
remove blockIndex
martha-johnston Jan 24, 2024
3a0538d
Merge branch 'viamrobotics:main' into rsdk-6282
martha-johnston Jan 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 42 additions & 25 deletions components/motor/gpio/loop.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"},
Expand All @@ -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"},
},
Expand All @@ -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
}
36 changes: 25 additions & 11 deletions components/motor/gpio/motor_encoder.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this background context canceled by anything in the loop? Or will it continue returning nil and blocking on a done call in the controls package?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think it's canceled by anything... I'm not sure it's necessary to even have a context for this function

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this function should be removed in a future pr

} 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 {
Expand Down Expand Up @@ -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)
}
Expand Down
75 changes: 2 additions & 73 deletions components/motor/gpio/motor_encoder_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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,
Expand Down
30 changes: 18 additions & 12 deletions components/motor/gpio/setup.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
)
Expand Down Expand Up @@ -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"`
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Love this struct <3

}

// 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.
Expand Down
5 changes: 4 additions & 1 deletion control/control_loop.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
2 changes: 1 addition & 1 deletion control/pid.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
Loading