[RSDK-6164] Mimo pid updates

control/control_block.go

@@ -22,6 +22,7 @@ const (
 	blockEncoderToRPM               controlBlockType = "encoderToRpm"
 	blockEndpoint                   controlBlockType = "endpoint"
 	blockFilter                     controlBlockType = "filter"
+	blockMIMOPID                    controlBlockType = "MIMOPID"
 )
 
 // BlockConfig configuration of a given block.

control/control_signal.go

@@ -1,6 +1,8 @@
 package control
 
-import "sync"
+import (
+	"sync"
+)
 
 // Signal holds any data passed between blocks.
 type Signal struct {
@@ -23,11 +25,24 @@ func makeSignal(name string, blockType controlBlockType) *Signal {
 	return &s
 }
 
+// Make Signals 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]
@@ -37,7 +52,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

control/pid.go

@@ -24,17 +24,20 @@ func (l *Loop) newPID(config BlockConfig, logger logging.Logger) (Block, error)
 type basicPID struct {
 	mu       sync.Mutex
 	cfg      BlockConfig
-	error    float64
-	kI       float64
-	kD       float64
-	kP       float64
-	int      float64
+	PIDSets  []*PIDConfig
+	error    float64 // MIMO
+	kI       float64 //
+	kD       float64 //
+	kP       float64 //
+	int      float64 // MIMO
 	y        []*Signal
+	useMulti bool
 	satLimUp float64 `default:"255.0"`
 	limUp    float64 `default:"255.0"`
 	satLimLo float64
 	limLo    float64
 	tuner    pidTuner
+	tuners   []*pidTuner
 	tuning   bool
 	logger   logging.Logger
 }
@@ -50,41 +53,102 @@ func (p *basicPID) Next(ctx context.Context, x []*Signal, dt time.Duration) ([]*
 	p.mu.Lock()
 	defer p.mu.Unlock()
 	if p.tuning {
-		out, done := p.tuner.pidTunerStep(math.Abs(x[0].GetSignalValueAt(0)), p.logger)
-		if done {
-			p.kD = p.tuner.kD
-			p.kI = p.tuner.kI
-			p.kP = p.tuner.kP
-			p.logger.Info("\n\n-------- ***** PID GAINS CALCULATED **** --------")
-			p.logger.CInfof(ctx, "Calculated gains are p: %1.6f, i: %1.6f, d: %1.6f", p.kP, p.kI, p.kD)
-			p.logger.CInfof(ctx, "You must MANUALLY ADD p, i and d gains to the robot config to use the values after tuning\n\n")
-			p.tuning = false
+		// Multi Input/Output Implementation
+		if p.useMulti {
+
+			// For each PID Set and its respective Tuner Object, Step through an iteration of tuning until done.
+			for i := 0; i < len(p.PIDSets); i++ {
+
+				out, done := p.tuners[i].pidTunerStep(math.Abs(x[0].GetSignalValueAt(i)), p.logger)
+				if done {
+
+					p.PIDSets[i].D = p.tuners[i].kD
+					p.PIDSets[i].I = p.tuners[i].kI
+					p.PIDSets[i].P = p.tuners[i].kP
+					p.logger.Info("\n\n-------- ***** PID GAINS CALCULATED **** --------")
+					p.logger.CInfof(ctx, "Calculated gains are p: %1.6f, i: %1.6f, d: %1.6f", p.PIDSets[i].P, p.PIDSets[i].I, p.PIDSets[i].D)
+					p.logger.CInfof(ctx, "You must MANUALLY ADD p, i and d gains to the robot config to use the values after tuning\n\n")
+					p.tuning = false
+				}
+				p.y[0].SetSignalValueAt(i, out)
+			}
+		} else {
+			// Single Input/Output Implementation
+			out, done := p.tuner.pidTunerStep(math.Abs(x[0].GetSignalValueAt(0)), p.logger)
+			if done {
+				p.kD = p.tuner.kD
+				p.kI = p.tuner.kI
+				p.kP = p.tuner.kP
+				p.logger.Info("\n\n-------- ***** PID GAINS CALCULATED **** --------")
+				p.logger.CInfof(ctx, "Calculated gains are p: %1.6f, i: %1.6f, d: %1.6f", p.kP, p.kI, p.kD)
+				p.logger.CInfof(ctx, "You must MANUALLY ADD p, i and d gains to the robot config to use the values after tuning\n\n")
+				p.tuning = false
+			}
+			p.y[0].SetSignalValueAt(0, out)
 		}
-		p.y[0].SetSignalValueAt(0, out)
+
 	} else {
-		dtS := dt.Seconds()
-		pvError := x[0].GetSignalValueAt(0)
-		p.int += p.kI * pvError * dtS
-		switch {
-		case p.int >= p.satLimUp:
-			p.int = p.satLimUp
-		case p.int <= p.satLimLo:
-			p.int = p.satLimLo
-		default:
-		}
-		deriv := (pvError - p.error) / dtS
-		output := p.kP*pvError + p.int + p.kD*deriv
-		p.error = pvError
-		if output > p.limUp {
-			output = p.limUp
-		} else if output < p.limLo {
-			output = p.limLo
+
+		// Multi Input/Output Implementation
+		if p.useMulti {
+
+			for i := 0; i < len(p.PIDSets); i++ {
+				output := calculateSignalValue(p, x, dt, i)
+				p.y[0].SetSignalValueAt(i, output)
+			}
+
+			// Single Input/Output Implementation
+		} else {
+
+			dtS := dt.Seconds()
+			pvError := x[0].GetSignalValueAt(0)
+			p.int += p.kI * pvError * dtS
+			switch {
+			case p.int >= p.satLimUp:
+				p.int = p.satLimUp
+			case p.int <= p.satLimLo:
+				p.int = p.satLimLo
+			default:
+			}
+			deriv := (pvError - p.error) / dtS
+			output := p.kP*pvError + p.int + p.kD*deriv
+			p.error = pvError
+			if output > p.limUp {
+				output = p.limUp
+			} else if output < p.limLo {
+				output = p.limLo
+			}
+			p.y[0].SetSignalValueAt(0, output)
 		}
-		p.y[0].SetSignalValueAt(0, output)
 	}
 	return p.y, true
 }
 
+// For a given signal, compute new signal value based on current signal value, & its respective error
+func calculateSignalValue(p *basicPID, x []*Signal, dt time.Duration, sIndex int) float64 {
+	dtS := dt.Seconds()
+	pvError := x[0].GetSignalValueAt(sIndex)
+	p.PIDSets[sIndex].int += p.PIDSets[sIndex].I * pvError * dtS
+
+	switch {
+	case p.PIDSets[sIndex].int >= p.satLimUp:
+		p.PIDSets[sIndex].int = p.satLimUp
+	case p.PIDSets[sIndex].int <= p.satLimLo:
+		p.PIDSets[sIndex].int = p.satLimLo
+	default:
+	}
+	deriv := (pvError - p.PIDSets[sIndex].error) / dtS
+	output := p.PIDSets[sIndex].P*pvError + p.PIDSets[sIndex].int + p.PIDSets[sIndex].D*deriv
+	p.PIDSets[sIndex].error = pvError
+	if output > p.limUp {
+		output = p.limUp
+	} else if output < p.limLo {
+		output = p.limLo
+	}
+
+	return output
+}
+
 func (p *basicPID) reset() error {
 	p.int = 0
 	p.error = 0
@@ -94,13 +158,33 @@ func (p *basicPID) reset() error {
 		!p.cfg.Attribute.Has("kP") {
 		return errors.Errorf("pid block %s should have at least one kI, kP or kD field", p.cfg.Name)
 	}
-	if len(p.cfg.DependsOn) != 1 {
+	if len(p.cfg.DependsOn) != 1 && !p.useMulti {
 		return errors.Errorf("pid block %s should have 1 input got %d", p.cfg.Name, len(p.cfg.DependsOn))
 	}
 	p.kI = p.cfg.Attribute["kI"].(float64)
 	p.kD = p.cfg.Attribute["kD"].(float64)
 	p.kP = p.cfg.Attribute["kP"].(float64)
 
+	// Each PIDSet is taken from the config, if the attribute exists (it's optional).
+	// If PID Sets was given as an attribute, we know we're in 'multi' mode. For each
+	// set of PIDs we initialize its values to 0 and create a tuner object.
+	if p.cfg.Attribute.Has("PIDSets") {
+		ok := true
+		p.PIDSets, ok = p.cfg.Attribute["PIDSets"].([]*PIDConfig)
+		if !ok {
+			return errors.New("PIDSet did not initalize correctly")
+		}
+		if len(p.PIDSets) > 0 {
+			p.useMulti = true
+			p.tuners = make([]*pidTuner, len(p.PIDSets))
+
+			for i := 0; i < len(p.PIDSets); i++ {
+				p.PIDSets[i].int = 0
+				p.PIDSets[i].error = 0
+			}
+		}
+	}
+
 	// ensure a default of 255
 	p.satLimUp = 255
 	if satLimUp, ok := p.cfg.Attribute["int_sat_lim_up"].(float64); ok {
@@ -126,40 +210,93 @@ func (p *basicPID) reset() error {
 	}
 
 	p.tuning = false
-	if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 {
-		var ssrVal float64
-		if p.cfg.Attribute["tune_ssr_value"] != nil {
-			ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64)
-		}
+	if p.useMulti {
+		for i := 0; i < len(p.PIDSets); i++ {
 
-		tuneStepPct := 0.35
-		if p.cfg.Attribute.Has("tune_step_pct") {
-			tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64)
-		}
+			if p.PIDSets[i].I == 0.0 && p.PIDSets[i].D == 0.0 && p.PIDSets[i].P == 0.0 {
+				var ssrVal float64
+				if p.cfg.Attribute["tune_ssr_value"] != nil {
+					ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64)
+				}
 
-		tuneMethod := tuneMethodZiegerNicholsPID
-		if p.cfg.Attribute.Has("tune_method") {
-			tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string))
-		}
+				tuneStepPct := 0.35
+				if p.cfg.Attribute.Has("tune_step_pct") {
+					tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64)
+				}
 
-		p.tuner = pidTuner{
-			limUp:      p.limUp,
-			limLo:      p.limLo,
-			ssRValue:   ssrVal,
-			tuneMethod: tuneMethod,
-			stepPct:    tuneStepPct,
-		}
-		err := p.tuner.reset()
-		if err != nil {
-			return err
+				tuneMethod := tuneMethodZiegerNicholsPID
+				if p.cfg.Attribute.Has("tune_method") {
+					tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string))
+				}
+
+				// Create a Tuner object for our PID set. Across all Tuner objects, they share global
+				// values (limUp, limLo, ssR, tuneMethod, stepPct). The only values that differ are P,I,D.
+				p.tuners[i] = &pidTuner{
+					limUp:      p.limUp,
+					limLo:      p.limLo,
+					ssRValue:   ssrVal,
+					tuneMethod: tuneMethod,
+					stepPct:    tuneStepPct,
+					kP:         p.PIDSets[i].P,
+					kI:         p.PIDSets[i].I,
+					kD:         p.PIDSets[i].D,
+				}
+
+				err := p.tuners[i].reset()
+				if err != nil {
+					return err
+				}
+
+				if p.tuners[i].stepPct > 1 || p.tuners[i].stepPct < 0 {
+					return errors.Errorf("tuner pid block %s should have a percentage value between 0-1 for TuneStepPct", p.cfg.Name)
+				}
+				p.tuning = true
+			}
 		}
-		if p.tuner.stepPct > 1 || p.tuner.stepPct < 0 {
-			return errors.Errorf("tuner pid block %s should have a percentage value between 0-1 for TuneStepPct", p.cfg.Name)
+		// Note: In our Signal[] array, we only have one element. For MIMO, within Signal[0],
+		// the length of the signal[] array is lengthened to accommodate multiple outputs.
+		p.y = make([]*Signal, 1)
+		p.y[0] = makeSignals(p.cfg.Name, p.cfg.Type, len(p.PIDSets))
+	} else {
+
+		if p.kI == 0.0 && p.kD == 0.0 && p.kP == 0.0 {
+			var ssrVal float64
+			if p.cfg.Attribute["tune_ssr_value"] != nil {
+				ssrVal = p.cfg.Attribute["tune_ssr_value"].(float64)
+			}
+
+			tuneStepPct := 0.35
+			if p.cfg.Attribute.Has("tune_step_pct") {
+				tuneStepPct = p.cfg.Attribute["tune_step_pct"].(float64)
+			}
+
+			tuneMethod := tuneMethodZiegerNicholsPID
+			if p.cfg.Attribute.Has("tune_method") {
+				tuneMethod = tuneCalcMethod(p.cfg.Attribute["tune_method"].(string))
+			}
+
+			p.tuner = pidTuner{
+				limUp:      p.limUp,
+				limLo:      p.limLo,
+				ssRValue:   ssrVal,
+				tuneMethod: tuneMethod,
+				stepPct:    tuneStepPct,
+			}
+			err := p.tuner.reset()
+			if err != nil {
+				return err
+			}
+
+			if p.tuner.stepPct > 1 || p.tuner.stepPct < 0 {
+				return errors.Errorf("tuner pid block %s should have a percentage value between 0-1 for TuneStepPct", p.cfg.Name)
+			}
+			p.tuning = true
 		}
-		p.tuning = true
+
+		p.y = make([]*Signal, 1)
+		p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type)
 	}
-	p.y = make([]*Signal, 1)
-	p.y[0] = makeSignal(p.cfg.Name, p.cfg.Type)
+
 	return nil
 }
 
@@ -393,5 +530,7 @@ func (p *pidTuner) reset() error {
 	p.kI = 0.0
 	p.kD = 0.0
 	p.kP = 0.0
+	p.pPeakH = []float64{}
+	p.pPeakL = []float64{}
 	return nil
 }