-
Notifications
You must be signed in to change notification settings - Fork 110
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.
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-6164] Mimo pid updates #4290
Changes from all commits
358fe3e
252a42a
21c3e33
31e3453
1cbb1a1
00644c8
b7900ee
63eeb2a
8e60a2d
9b2916a
9f80288
311e05c
8bf5301
343544f
5b86ac8
c2506d6
598e704
22be53f
0ca4594
d2cba49
756e5b8
372f540
12303b2
4528d58
a983cfa
87b1283
7956825
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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 | ||
|
@@ -352,11 +352,25 @@ func (sb *sensorBase) determineHeadingFunc(ctx context.Context, | |
// if loop is tuning, return an error | ||
// if loop has been tuned but the values haven't been added to the config, error with tuned values. | ||
func (sb *sensorBase) checkTuningStatus() error { | ||
if sb.loop != nil && sb.loop.GetTuning(context.Background()) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. turns out the loop is always nil at this point because we do not set the loop until tuning completes. |
||
done := true | ||
needsTuning := false | ||
|
||
for i := range sb.configPIDVals { | ||
// check if the current signal needed tuning | ||
if sb.configPIDVals[i].NeedsAutoTuning() { | ||
// return true if either signal needed tuning | ||
needsTuning = needsTuning || true | ||
// if the tunedVals have not been updated, then tuning is still in progress | ||
done = done && !(*sb.tunedVals)[i].NeedsAutoTuning() | ||
} | ||
} | ||
|
||
if needsTuning { | ||
if done { | ||
return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals) | ||
} | ||
return control.TuningInProgressErr(sb.Name().ShortName()) | ||
} else if (sb.configPIDVals[0].NeedsAutoTuning() && !(*sb.tunedVals)[0].NeedsAutoTuning()) || | ||
(sb.configPIDVals[1].NeedsAutoTuning() && !(*sb.tunedVals)[1].NeedsAutoTuning()) { | ||
return control.TunedPIDErr(sb.Name().ShortName(), *sb.tunedVals) | ||
} | ||
|
||
return nil | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -209,12 +209,9 @@ func (l *Loop) BlockList(ctx context.Context) ([]string, error) { | |
} | ||
|
||
// GetPIDVals returns the tuned PID values. | ||
// TODO: update this when MIMO fully supported. | ||
func (l *Loop) GetPIDVals(pidIndex int) PIDConfig { | ||
return PIDConfig{ | ||
P: l.pidBlocks[pidIndex].kP, | ||
I: l.pidBlocks[pidIndex].kI, | ||
D: l.pidBlocks[pidIndex].kD, | ||
} | ||
return *l.pidBlocks[pidIndex].PIDSets[0] | ||
} | ||
|
||
// Frequency returns the loop's frequency. | ||
|
@@ -227,7 +224,7 @@ func (l *Loop) Start() error { | |
if len(l.ts) == 0 { | ||
return errors.New("cannot start the control loop if there are no blocks depending on an impulse") | ||
} | ||
l.logger.Infof("Running loop on %1.4f %+v\r\n", l.cfg.Frequency, l.dt) | ||
l.logger.Infof("Running control loop at %1.4f Hz, %+v\r\n", l.cfg.Frequency, l.dt) | ||
l.ct = controlTicker{ | ||
ticker: time.NewTicker(l.dt), | ||
stop: make(chan bool, 1), | ||
|
@@ -339,17 +336,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) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I can try to remove these if ya want. Was worried that calling GetTuning was affecting things at the time when I added this. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. this is fine and probably safer. happy with leaving it. |
||
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 | ||
} | ||
} | ||
|
||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
this function is now effectively the same for both controlled motor and sensorbase, but going to wait until the refactor ticket to turn them into one function in case there are any other optimizations we can find wrt determining if tuning was completed. @martha-johnston