diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.Designer.cs b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.Designer.cs index ae96538e7..fb609b619 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.Designer.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.Designer.cs @@ -88,7 +88,7 @@ this.chbSonarAtivado = new System.Windows.Forms.CheckBox(); this.lblPressao = new System.Windows.Forms.Label(); this.label1 = new System.Windows.Forms.Label(); - this.nudPotenciaBomba = new System.Windows.Forms.NumericUpDown(); + this.nudPotenciaBombaMin = new System.Windows.Forms.NumericUpDown(); this.groupBox2 = new System.Windows.Forms.GroupBox(); this.groupBox3 = new System.Windows.Forms.GroupBox(); this.cmbModoAgitador = new System.Windows.Forms.ComboBox(); @@ -101,6 +101,8 @@ this.txtKi = new System.Windows.Forms.TextBox(); this.label6 = new System.Windows.Forms.Label(); this.txtKd = new System.Windows.Forms.TextBox(); + this.label7 = new System.Windows.Forms.Label(); + this.nudPotenciaBombaMax = new System.Windows.Forms.NumericUpDown(); ((System.ComponentModel.ISupportInitialize)(this.chartGraficos)).BeginInit(); this.tabControl1.SuspendLayout(); this.tabMapa.SuspendLayout(); @@ -114,9 +116,10 @@ this.pnlDir_ET.SuspendLayout(); this.pnlDir_DT.SuspendLayout(); this.pnlDir_DF.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBomba)).BeginInit(); + ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBombaMin)).BeginInit(); this.groupBox2.SuspendLayout(); this.groupBox3.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBombaMax)).BeginInit(); this.SuspendLayout(); // // pnlAtuadores @@ -751,27 +754,29 @@ this.label1.Location = new System.Drawing.Point(6, 94); this.label1.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0); this.label1.Name = "label1"; - this.label1.Size = new System.Drawing.Size(77, 13); + this.label1.Size = new System.Drawing.Size(50, 13); this.label1.TabIndex = 35; - this.label1.Text = "Potencia SP"; + this.label1.Text = "Pot Min"; // - // nudPotenciaBomba + // nudPotenciaBombaMin // - this.nudPotenciaBomba.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F, System.Drawing.FontStyle.Bold, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - this.nudPotenciaBomba.Location = new System.Drawing.Point(6, 111); - this.nudPotenciaBomba.Margin = new System.Windows.Forms.Padding(2); - this.nudPotenciaBomba.Maximum = new decimal(new int[] { + this.nudPotenciaBombaMin.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F, System.Drawing.FontStyle.Bold, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.nudPotenciaBombaMin.Location = new System.Drawing.Point(6, 111); + this.nudPotenciaBombaMin.Margin = new System.Windows.Forms.Padding(2); + this.nudPotenciaBombaMin.Maximum = new decimal(new int[] { 200, 0, 0, 0}); - this.nudPotenciaBomba.Name = "nudPotenciaBomba"; - this.nudPotenciaBomba.Size = new System.Drawing.Size(121, 19); - this.nudPotenciaBomba.TabIndex = 34; - this.nudPotenciaBomba.TextAlign = System.Windows.Forms.HorizontalAlignment.Center; + this.nudPotenciaBombaMin.Name = "nudPotenciaBombaMin"; + this.nudPotenciaBombaMin.Size = new System.Drawing.Size(55, 19); + this.nudPotenciaBombaMin.TabIndex = 34; + this.nudPotenciaBombaMin.TextAlign = System.Windows.Forms.HorizontalAlignment.Center; // // groupBox2 // + this.groupBox2.Controls.Add(this.label7); + this.groupBox2.Controls.Add(this.nudPotenciaBombaMax); this.groupBox2.Controls.Add(this.label6); this.groupBox2.Controls.Add(this.txtKd); this.groupBox2.Controls.Add(this.label5); @@ -783,7 +788,7 @@ this.groupBox2.Controls.Add(this.btnLigarBomba); this.groupBox2.Controls.Add(this.label1); this.groupBox2.Controls.Add(this.btnDesligarBomba); - this.groupBox2.Controls.Add(this.nudPotenciaBomba); + this.groupBox2.Controls.Add(this.nudPotenciaBombaMin); this.groupBox2.Controls.Add(this.nudPressaoSP); this.groupBox2.Controls.Add(this.lblPressaoSP); this.groupBox2.Location = new System.Drawing.Point(5, 6); @@ -905,6 +910,32 @@ this.txtKd.TabIndex = 44; this.txtKd.TextAlign = System.Windows.Forms.HorizontalAlignment.Center; // + // label7 + // + this.label7.AutoSize = true; + this.label7.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F, System.Drawing.FontStyle.Bold, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.label7.Location = new System.Drawing.Point(78, 94); + this.label7.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0); + this.label7.Name = "label7"; + this.label7.Size = new System.Drawing.Size(53, 13); + this.label7.TabIndex = 47; + this.label7.Text = "Pot Max"; + // + // nudPotenciaBombaMax + // + this.nudPotenciaBombaMax.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F, System.Drawing.FontStyle.Bold, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.nudPotenciaBombaMax.Location = new System.Drawing.Point(72, 111); + this.nudPotenciaBombaMax.Margin = new System.Windows.Forms.Padding(2); + this.nudPotenciaBombaMax.Maximum = new decimal(new int[] { + 200, + 0, + 0, + 0}); + this.nudPotenciaBombaMax.Name = "nudPotenciaBombaMax"; + this.nudPotenciaBombaMax.Size = new System.Drawing.Size(55, 19); + this.nudPotenciaBombaMax.TabIndex = 46; + this.nudPotenciaBombaMax.TextAlign = System.Windows.Forms.HorizontalAlignment.Center; + // // frmOperacaoManual // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); @@ -959,11 +990,12 @@ this.pnlDir_DT.PerformLayout(); this.pnlDir_DF.ResumeLayout(false); this.pnlDir_DF.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBomba)).EndInit(); + ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBombaMin)).EndInit(); this.groupBox2.ResumeLayout(false); this.groupBox2.PerformLayout(); this.groupBox3.ResumeLayout(false); this.groupBox3.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)(this.nudPotenciaBombaMax)).EndInit(); this.ResumeLayout(false); this.PerformLayout(); @@ -1026,7 +1058,7 @@ private System.Windows.Forms.Label lblPressao; private System.Windows.Forms.Label lblStatusCarroMapa; private System.Windows.Forms.Label label1; - private System.Windows.Forms.NumericUpDown nudPotenciaBomba; + private System.Windows.Forms.NumericUpDown nudPotenciaBombaMin; private System.Windows.Forms.GroupBox groupBox3; private System.Windows.Forms.GroupBox groupBox2; private System.Windows.Forms.Label label2; @@ -1039,5 +1071,7 @@ private System.Windows.Forms.TextBox txtKi; private System.Windows.Forms.Label label4; private System.Windows.Forms.TextBox txtKp; + private System.Windows.Forms.Label label7; + private System.Windows.Forms.NumericUpDown nudPotenciaBombaMax; } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs index df9da0598..0359954ed 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs @@ -60,10 +60,11 @@ namespace AgroBase.Forms.Operacoes txtKp.Text = b.Controle.Kp.ToString(); txtKi.Text = b.Controle.Ki.ToString(); txtKd.Text = b.Controle.Kd.ToString(); + nudPotenciaBombaMin.Value = Convert.ToInt32(b.Controle.PotenciaMinPID); } nudPressaoSP.Value = Convert.ToInt32(pC.AtuPressaoLinha); - nudPotenciaBomba.Value = 100; + nudPotenciaBombaMax.Value = 100; nudPotenciaAgitador.Value = 100; flwCamerasSolo.Controls.Clear(); @@ -670,14 +671,14 @@ namespace AgroBase.Forms.Operacoes { Variaveis.OperacaoEmAndamento.Parametros.Controle.AtuModoControle = (ModoControleBomba)cmbModoBomba.SelectedIndex; - double potencia = Convert.ToInt32(nudPotenciaBomba.Value); + double pressao = Convert.ToInt32(nudPressaoSP.Value); + double potMin = Convert.ToInt32(nudPotenciaBombaMin.Value); + double potMax = Convert.ToInt32(nudPotenciaBombaMax.Value); double Kp = Convert.ToDouble(txtKp.Text); double Ki = Convert.ToDouble(txtKi.Text); double Kd = Convert.ToDouble(txtKd.Text); - GeneralJoystick.EnviarComandoAtuador(Bomba.Componente, Bomba.ID, Status: true, Valores: new System.Collections.Generic.List() { cmbModoBomba.SelectedIndex, potencia, Kp, Ki, Kd }, Comando: CanMessagePosicaoDados.Command2, Forcar: true); - - double pressao = Convert.ToInt32(nudPressaoSP.Value); - GeneralJoystick.EnviarComandoAtuador(Bomba.Componente, Bomba.ID, Status: true, Valores: new System.Collections.Generic.List() { potencia, pressao }, Comando: CanMessagePosicaoDados.Command1, Forcar: true); + GeneralJoystick.EnviarComandoAtuador(Bomba.Componente, Bomba.ID, Status: true, Valores: new System.Collections.Generic.List() { cmbModoBomba.SelectedIndex, potMin, Kp, Ki, Kd }, Comando: CanMessagePosicaoDados.Command2, Forcar: true); + GeneralJoystick.EnviarComandoAtuador(Bomba.Componente, Bomba.ID, Status: true, Valores: new System.Collections.Generic.List() { potMax, pressao }, Comando: CanMessagePosicaoDados.Command1, Forcar: true); } } diff --git a/Firmware/Modulos/BombaPressurizadoraModel_v2.h b/Firmware/Modulos/BombaPressurizadoraModel_v2.h index 5e6335a43..16fc15a34 100644 --- a/Firmware/Modulos/BombaPressurizadoraModel_v2.h +++ b/Firmware/Modulos/BombaPressurizadoraModel_v2.h @@ -80,6 +80,7 @@ class BombaPressurizadora_v2 : public ComponenteCAN { double Kd = 0.0; double erroAnterior = 0; + double pressaoAnterior = 0.0; double integralErro = 0; unsigned long ultimoPID_ms = 0; @@ -341,56 +342,107 @@ class BombaPressurizadora_v2 : public ComponenteCAN { } void ResetPID() { - erroAnterior = 0; - integralErro = 0; + erroAnterior = 0.0; + integralErro = 0.0; + pressaoAnterior = 0.0; ultimoPID_ms = millis(); } double CalcularPotenciaPID(double pressaoAtual) { - unsigned long agora = millis(); + const unsigned long agora = millis(); if (ultimoPID_ms == 0) { ultimoPID_ms = agora; + erroAnterior = _PressaoSP - pressaoAtual; + pressaoAnterior = pressaoAtual; + return 0.0; } double dt = (agora - ultimoPID_ms) / 1000.0; ultimoPID_ms = agora; - if (dt <= 0.001) { - dt = 0.001; + if (dt < 0.01) { + dt = 0.01; + } + + if (dt > 0.5) { + dt = 0.5; } double erro = _PressaoSP - pressaoAtual; - // Zona morta: se está perto do SP, evita ficar oscilando demais - if (abs(erro) <= DeadbandPressao) { - erro = 0; + if (fabs(erro) <= DeadbandPressao) { + erro = 0.0; } - integralErro += erro * dt; - integralErro = constrain(integralErro, IntegralMin, IntegralMax); + /* + Derivada feita sobre a medição, e não sobre o erro. + Isso reduz o tranco quando o SP muda. + */ + double derivadaMedicao = + (pressaoAtual - pressaoAnterior) / dt; - double derivada = (erro - erroAnterior) / dt; - erroAnterior = erro; + pressaoAnterior = pressaoAtual; - double saida = (Kp * erro) + (Ki * integralErro) + (Kd * derivada); + /* + Calcula primeiro sem atualizar a integral. + */ + double saidaSemIntegral = + (Kp * erro) + - (Kd * derivadaMedicao); - // Se passou da pressão desejada, deixa cortar a bomba - if (pressaoAtual >= _PressaoSP) { - saida = 0; + double integralCandidata = + integralErro + (erro * dt); + + integralCandidata = constrain( + integralCandidata, + IntegralMin, + IntegralMax + ); + + double saidaCandidata = + saidaSemIntegral + + (Ki * integralCandidata); + + double teto = constrain( + _PotenciaSP, + 0.0, + PotenciaMaxPID + ); + + /* + Anti-windup: + só integra se não estiver saturado, + ou se o erro estiver ajudando a sair da saturação. + */ + bool saturadoSuperior = saidaCandidata >= teto; + bool saturadoInferior = saidaCandidata <= 0.0; + + bool podeIntegrar = + (!saturadoSuperior && !saturadoInferior) + || (saturadoSuperior && erro < 0) + || (saturadoInferior && erro > 0); + + if (podeIntegrar) { + integralErro = integralCandidata; } - // Limite máximo vindo do comando - double teto = _PotenciaSP > 0 ? _PotenciaSP : PotenciaMaxPID; - teto = constrain(teto, PotenciaMinPID, PotenciaMaxPID); + double saida = + saidaSemIntegral + + (Ki * integralErro); - saida = constrain(saida, 0, teto); + saida = constrain(saida, 0.0, teto); - // Degrau mínimo: se o PID quer ligar, liga com potência útil - if (saida > 0 && saida < PotenciaMinPID) { + /* + Só aplica o mínimo quando existe demanda real + para ligar a bomba. + */ + if (saida > 0.0 && saida < PotenciaMinPID) { saida = PotenciaMinPID; } + erroAnterior = erro; + return saida; }