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.

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

Merged
merged 27 commits into from
Oct 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
358fe3e
MIMO on PID Block done, need to clean up code
mariapatni Aug 16, 2024
252a42a
added some comments and cleaned up some code
mariapatni Aug 16, 2024
21c3e33
ran linter
mariapatni Aug 16, 2024
31e3453
tune signals one at a time
JohnN193 Aug 20, 2024
1cbb1a1
feedback from john review
JohnN193 Aug 20, 2024
00644c8
test MIMO PID works for single signal case
JohnN193 Aug 20, 2024
b7900ee
disable mimo single case
JohnN193 Aug 20, 2024
63eeb2a
small test cleanup
JohnN193 Aug 20, 2024
8e60a2d
add back old test
JohnN193 Aug 20, 2024
9b2916a
tweak pid struct comments
JohnN193 Aug 20, 2024
9f80288
lint
JohnN193 Aug 21, 2024
311e05c
fix pid test
JohnN193 Aug 21, 2024
8bf5301
fix other tests and remove dupe test
JohnN193 Aug 21, 2024
343544f
tweak comment
JohnN193 Sep 4, 2024
5b86ac8
only confirm pid values are set when not using multi and add error lo…
JohnN193 Sep 4, 2024
c2506d6
Merge branch 'main' of github.com:viamrobotics/rdk into mimoPIDUpdates
JohnN193 Sep 4, 2024
598e704
Merge branch 'main' of github.com:viamrobotics/rdk into mimoPIDUpdates
JohnN193 Sep 26, 2024
22be53f
Merge branch 'main' of github.com:viamrobotics/rdk into mimoPIDUpdates
JohnN193 Oct 2, 2024
0ca4594
add string method to pidconfig
JohnN193 Oct 2, 2024
d2cba49
lint
JohnN193 Oct 3, 2024
756e5b8
write string differently
JohnN193 Oct 3, 2024
372f540
fix test
JohnN193 Oct 3, 2024
12303b2
remove single input output pid
JohnN193 Oct 3, 2024
4528d58
address comments
JohnN193 Oct 3, 2024
a983cfa
lint
JohnN193 Oct 3, 2024
87b1283
check for in progress tuning
JohnN193 Oct 9, 2024
7956825
tweak string
JohnN193 Oct 9, 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
24 changes: 19 additions & 5 deletions components/base/sensorcontrolled/sensorcontrolled.go
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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 {
Copy link
Member

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

if sb.loop != nil && sb.loop.GetTuning(context.Background()) {
Copy link
Member

Choose a reason for hiding this comment

The 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
}
4 changes: 1 addition & 3 deletions components/base/sensorcontrolled/sensorcontrolled_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@ package sensorcontrolled
import (
"context"
"errors"
"fmt"
"strings"
"sync"
"testing"
Expand Down Expand Up @@ -535,8 +534,7 @@ func TestSensorBaseDoCommand(t *testing.T) {
expectedPID := control.PIDConfig{P: 0.1, I: 2.0, D: 0.0}
sb.tunedVals = &[]control.PIDConfig{expectedPID, {}}
expectedeMap := make(map[string]interface{})
expectedeMap["get_tuned_pid"] = (fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ",
expectedPID.P, expectedPID.I, expectedPID.D, expectedPID.Type))
expectedeMap["get_tuned_pid"] = (expectedPID.String())

req := make(map[string]interface{})
req["get_tuned_pid"] = true
Expand Down
25 changes: 19 additions & 6 deletions components/motor/gpio/controlled.go
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@ package gpio

import (
"context"
"fmt"
"math"
"sync"
"time"
Expand Down Expand Up @@ -396,8 +395,7 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa
if ok {
var respStr string
if !(*cm.tunedVals)[0].NeedsAutoTuning() {
respStr += fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ",
(*cm.tunedVals)[0].P, (*cm.tunedVals)[0].I, (*cm.tunedVals)[0].D, (*cm.tunedVals)[0].Type)
respStr += (*cm.tunedVals)[0].String()
}
resp[getPID] = respStr
}
Expand All @@ -408,10 +406,25 @@ func (cm *controlledMotor) DoCommand(ctx context.Context, req map[string]interfa
// 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 (cm *controlledMotor) checkTuningStatus() error {
if cm.loop != nil && cm.loop.GetTuning(context.Background()) {
done := true
needsTuning := false

for i := range cm.configPIDVals {
// check if the current signal needed tuning
if cm.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 && !(*cm.tunedVals)[i].NeedsAutoTuning()
}
}

if needsTuning {
if done {
return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals)
}
return control.TuningInProgressErr(cm.Name().ShortName())
} else if cm.configPIDVals[0].NeedsAutoTuning() && !(*cm.tunedVals)[0].NeedsAutoTuning() {
return control.TunedPIDErr(cm.Name().ShortName(), *cm.tunedVals)
}

return nil
}
4 changes: 1 addition & 3 deletions components/motor/gpio/controlled_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@ package gpio

import (
"context"
"fmt"
"testing"

"go.viam.com/test"
Expand Down Expand Up @@ -106,8 +105,7 @@ func TestControlledMotorCreation(t *testing.T) {
expectedPID := control.PIDConfig{P: 0.1, I: 2.0, D: 0.0}
cm.tunedVals = &[]control.PIDConfig{expectedPID, {}}
expectedeMap := make(map[string]interface{})
expectedeMap["get_tuned_pid"] = (fmt.Sprintf("{p: %v, i: %v, d: %v, type: %v} ",
expectedPID.P, expectedPID.I, expectedPID.D, expectedPID.Type))
expectedeMap["get_tuned_pid"] = expectedPID.String()

req := make(map[string]interface{})
req["get_tuned_pid"] = true
Expand Down
33 changes: 21 additions & 12 deletions control/control_loop.go
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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),
Expand Down Expand Up @@ -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) {
Copy link
Member

Choose a reason for hiding this comment

The 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.

Copy link
Contributor

@martha-johnston martha-johnston Sep 3, 2024

Choose a reason for hiding this comment

The 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
}
}

Expand Down
4 changes: 1 addition & 3 deletions control/control_loop_test.go
Original file line number Diff line number Diff line change
Expand Up @@ -347,9 +347,7 @@ func TestMultiSignalLoop(t *testing.T) {
Name: "pid_block",
Type: "PID",
Attribute: utils.AttributeMap{
"kP": expectedPIDVals.P, // random for now
"kD": expectedPIDVals.D,
"kI": expectedPIDVals.I,
"PIDSets": []*PIDConfig{&expectedPIDVals},
},
DependsOn: []string{"gain_block"},
},
Expand Down
20 changes: 17 additions & 3 deletions control/control_signal.go
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package control

import "sync"
import (
"sync"
)

// Signal holds any data passed between blocks.
type Signal struct {
Expand All @@ -23,11 +25,23 @@ func makeSignal(name string, blockType controlBlockType) *Signal {
return &s
}

// makeSignals returns a Signal object where the length of its signal[] array is dependent
// on the number of PIDSets from the config.
func makeSignals(name string, blockType controlBlockType, dimension int) *Signal {
var s Signal
s.dimension = dimension
s.signal = make([]float64, dimension)
s.time = make([]int, dimension)
s.name = name
s.blockType = blockType
return &s
}

// GetSignalValueAt returns the value of the signal at an index, threadsafe.
func (s *Signal) GetSignalValueAt(i int) float64 {
s.mu.Lock()
defer s.mu.Unlock()
if i > len(s.signal)-1 {
if !(i < len(s.signal)) {
return 0.0
}
return s.signal[i]
Expand All @@ -37,7 +51,7 @@ func (s *Signal) GetSignalValueAt(i int) float64 {
func (s *Signal) SetSignalValueAt(i int, val float64) {
s.mu.Lock()
defer s.mu.Unlock()
if i > len(s.signal)-1 {
if !(i < len(s.signal)) {
return
}
s.signal[i] = val
Expand Down
Loading
Loading