这里是工作之余写的一些小算法，实现一些功能。不过这里仅仅是个算法，只能用来了解一下。不能直接拿过来用，
因为这里要涉及到硬件平台的问题，这里的所有算法都是要生成可执行文件在硬件平台上运行的。
这里版权所有，不能用于非法用途，否则责任自负。转载的话，请注明转载出处！！！
/// <summary>
/// 表示阻抗控制的控制算法。
/// </summary>
public static class AlgorithmImpedance
{
#region 1.成员
/// <summary>
/// 计算结果（控制量）。
/// </summary>
private static ControlResult result;
/// <summary>
/// 运行开始时间
/// </summary>
private static DateTime startTime;
/// <summary>
/// 开始治疗时间
/// </summary>
public static DateTime startCureTime;
/// <summary>
/// 首次运行标志。
/// </summary>
private static bool isFirstRun;
/// <summary>
/// 冷却时间。
/// </summary>
private static double coolingTime;
/// <summary>
/// 每次增加功率的步长。
/// </summary>
private static double powerStep;
/// <summary>
/// 每次增加功率的时间步长。
/// </summary>
private static double timeStep;
/// <summary>
/// 冷却标志，每次治疗前均需冷却一段时间。
/// </summary>
private static bool isCooling;
/// <summary>
/// 阻抗模式下检测阻抗斜率时间的开始值
/// </summary>
private static DateTime dtStart;
/// <summary>
/// 阻抗的斜率
/// </summary>
public static Slope slope;
/// <summary>
/// 阻抗差值的一阶导，阻抗差值的斜率
/// </summary>
public static Slope K1Slope;
/// <summary>
/// 斜率的一阶导，阻抗差值的两阶导
/// </summary>
public static Slope K2Slope;
//台阶法。
private static Step step = new Step();
/// <summary>
/// 通过计算得到的实时电压值
/// </summary>
private static double VoltageValue;
/// <summary>
/// 电压转换为DA后，调整功率匹配的DA值步长
/// </summary>
private static double DAStep;
/// <summary>
/// 认为匹配功率值的绝对值范围
/// </summary>
private static double PowerAbs;
/// <summary>
/// 功率下降下限
/// </summary>
private static double MinPower;
/// <summary>
/// 稳定阻抗时初始功率
/// </summary>
private static double P0;
/// <summary>
/// 当前输出的功率
/// </summary>
public static double P;
/// <summary>
/// PID 系数1
/// </summary>
private static double S1;
/// <summary>
/// PID 系数2
/// </summary>
private static double S2;
#region 1. 阻抗法治疗参数设置属性
#region 功率阶梯上升参数设置
/// <summary>
/// 阶梯功率起始值
/// </summary>
private static double PMin;
/// <summary>
/// 阶梯功率终点值
/// </summary>
public static double PMax;
/// <summary>
///阶梯功率上升时间
/// </summary>
private static double TRise;
#endregion
#region 负载阻抗曲线斜率检测参数设置
/// <summary>
/// 采样时间间隔
/// </summary>
private static double Tsample;
/// <summary>
/// 采样队列长度
/// </summary>
private static int PNum;
//斜率阈值
private static double Kthreshod;
#endregion
#region 负载阻抗超出额定范围参数设置
/// <summary>
/// 目标阻抗
/// </summary>
private static double RMax;
/// <summary>
/// 允许超出目标阻抗的范围
/// </summary>
private static double MaxOverRating;
/// <summary>
/// 超出最大阻抗的处理方法
/// </summary>
private static bool isOverLoadDealing = true;
#endregion
#region 射频消融最后时段功率快速提升参数设置
/// <summary>
/// 冲刺时间长度（单位：S）
/// </summary>
private static double EndTime;
/// <summary>
/// 冲刺的目标功率值
/// </summary>
private static double EndPullPower;
#endregion
#region 负载阻抗斜率平缓控制参数设置
//阈值斜率1
private static double K1;
//阈值斜率2
private static double K2;
//阈值斜率3
private static double K3;
//阈值斜率4
private static double K4;
private static double K5;
//阈值斜率6
private static double K6;
/// <summary>
/// DA最低值
/// </summary>
public static double DAValue;
/// <summary>
/// 开始治疗到结束治疗前实际治疗时间
/// </summary>
private static double timeTreatment;
#endregion
#endregion
#endregion
#region 2.属性
/// <summary>
/// 获取当前算法计算的结果（控制量）。
/// </summary>
public static ControlResult Result
{
private set
{
result = value;
}
get
{
return result;
}
}
/// <summary>
/// 计算一次阻抗斜率（PNum个点）的时间
/// </summary>
private static double RunTime
{
get
{
return (DateTime.Now - dtStart).TotalSeconds;
}
}
/// <summary>
/// 温度采样值。
/// </summary>
private static double AcqTemperature
{
get
{
return ControlAlgorithm.AcqTemperature;
}
}
/// <summary>
/// 阻抗采集值。
/// </summary>
private static double AcqImpedance
{
get
{
return ControlAlgorithm.AcqImpedance;
}
}
/// <summary>
/// 功率采集值。
/// </summary>
private static double AcqPower
{
get
{
return ControlAlgorithm.AcqPower;
}
}
/// <summary>
/// 阻抗模式下各个参数的调试信息
/// </summary>
private static string DebugInf
{
set
{
ControlAlgorithm.DebugInf = value;
}
get
{
return ControlAlgorithm.DebugInf;
}
}
#endregion
#region 3.函数
/// <summary>
/// 初始化。
/// </summary>
public static void Initialize()
{
result = new ControlResult();
isFirstRun = true;
startTime = new DateTime();
startCureTime = new DateTime();
isCooling = true;
isOverLoadDealing = true;
slope = new Slope(PNum);
K1Slope = new Slope(100);
K2Slope = new Slope(100);
GetRegistry();
}
/// <summary>
/// 读取注册表中阻抗模式下的参数值
/// </summary>
public static void GetRegistry()
{
PMin = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P_Min", "8"));
PMax = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P_Max", "35"));
TRise = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "T_Rise", "600"));
Tsample = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "T_sample", "1"));
PNum = Convert.ToInt32(RegistryFunctions.GetRegistry("HGCF3000", "P_Num", "20"));
Kthreshod = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K_threshod", "0.577"));
RMax = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "RMax", "200"));
MaxOverRating = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Max_OverRating", "20"));
P0 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P0", "30"));
S1 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "S1", "0"));
S2 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "S2", "0"));
EndTime = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "End_Time", "180"));
EndPullPower = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "End_PullPower", "40"));
DAStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "DAStep", "0.05"));
PowerAbs = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Abs", "2"));
MinPower = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "MinPower", "15"));
K1 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K1", "0"));
K2 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K2", "0.577"));
K3 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K3", "0.788"));
K4 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K4", "1.0"));
K5 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K5", "1.366"));
K6 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K6", "1.732"));
DAValue = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "DAValue", "0.875"));
coolingTime = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Cooling_Time", "5"));
powerStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Power_Step", "0.5"));
timeStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Time_Step", "5"));
}
/// <summary>
/// 根据当前的运行状况（采样值）进行计算。
/// </summary>
public static void Control(object sender, EventArgs e)
{
//是否开始运行
if (isFirstRun)
{
isFirstRun = false;
startTime = DateTime.Now;
dtStart = DateTime.Now;
startCureTime = DateTime.Now;
Result.PowerSwitchRFSource = false;
Result.OutputPower = PMin;
}
//每隔RunTime计算一下阻抗的斜率（20个点）
slope.Add(new double[] { RunTime, AcqImpedance });
K1Slope.Add(new double[] { RunTime, RMax-AcqImpedance });
K2Slope.Add(new double[] { RunTime, K1Slope.KValue });
//当前已运行多长时间
double time = (DateTime.Now - startTime).TotalSeconds;
//当处于冷却阶段时
if (isCooling)
{
//超过冷却时间后，重置计时，并关闭冷却标志、打开射频源。
if (time >= coolingTime)
{
startTime = DateTime.Now;
isCooling = false;
Result.PowerSwitchRFSource = true;//开射频
}
}
//并非处于冷却阶段时
else
{
//每隔Tsample2秒钟计算一下阻抗的斜率（20个点）
if (RunTime >= Tsample)
{
//如果当前时间大于时间步长，开始加功率步长
if (time >= timeStep)
{
startTime = DateTime.Now;
ControlImpedance();
}
timeTreatment = (DateTime.Now - startCureTime).TotalSeconds;
PowerSet();
Power();
}
}
Debug();
VoltageValue = Math.Pow(AcqImpedance * Result.OutputPower, 0.5);
}
/// <summary>
/// PID算法功率输出
/// </summary>
public static void PID()
{
//P0 = 30;
if (K1Slope.KValue >= 10 || K1Slope.KValue <= -10)
{
//todo
}
else if (K2Slope.KValue >= 10 || K2Slope.KValue <= -10)
{
//todo
}
else
{
P = P0 + S1 * K1Slope.KValue + S2 * K2Slope.KValue;
}
if (P >= 50)
{
P = 50;
}
else if (P<=20)
{
P = 20;
}
Result.OutputPower = P;
Power();
}
/// <summary>
/// 台阶控制阻抗
/// 吴新强 2013年10月12日15:48:45
/// </summary>
public static void ControlImpedance()
{
if (Math.Abs(RMax - AcqImpedance)<=MaxOverRating)
{
PID();
}
else if (RMax - MaxOverRating > AcqImpedance)
{
if (Result.OutputPower <= PMax - powerStep)
{
Result.OutputPower += powerStep;
}
else
{
Result.OutputPower = PMax;
}
//斜率小于K1时（默认为0）2倍的功率步长加功率
if (slope.KValue < -10)
{
Result.OutputPower +=5* powerStep;
}
else if (slope.KValue < -5)
{
Result.OutputPower += 4 * powerStep;
}
else if (slope.KValue < -1.0)
{
Result.OutputPower += 3 * powerStep;
}
else if (slope.KValue < K1)
{
Result.OutputPower += 2 * powerStep;
}
else if (slope.KValue < K2)//斜率小于K2时（默认为0.268、0.577）1倍的功率步长加功率
{
Result.OutputPower += powerStep;
}
else if (slope.KValue < K3)//斜率小于K3时（默认为0.423）1倍的功率步长降功率
{
//Result.OutputPower -= 4;
Result.OutputPower -= powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
else if (slope.KValue < K4)//斜率小于K3时（默认为0.577）1倍的功率步长降功率
{
//Result.OutputPower -= 4;
Result.OutputPower -= 2 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
else if (slope.Value < K5)
// //斜率大于K3时（默认为0.683）2倍的功率步长降功率
{
Result.OutputPower -= 3 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
//斜率小于K3时（默认0.788）1倍的功率步长降功率
else if (slope.KValue < K6)
{
Result.OutputPower -= 4 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
else
{
Result.OutputPower -= 5 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
}
else if (AcqImpedance >RMax + MaxOverRating)
{
//斜率小于K1时（默认为0）2倍的功率步长加功率
if (slope.KValue < -10)
{
Result.OutputPower +=4* powerStep;
}
else if (slope.KValue < -5)
{
Result.OutputPower += 3* powerStep;
}
else if (slope.KValue < -1.0)
{
Result.OutputPower += 2 * powerStep;
}
else if (slope.KValue < K1)
{
Result.OutputPower += powerStep;
}
else if (slope.KValue < 0.1)
{
}
//斜率小于K2时（默认为0.12、0.577）1倍的功率步长加功率
else if (slope.KValue < K2)
{
Result.OutputPower -= powerStep;
}
//斜率小于K3时（默认为0.268）1倍的功率步长降功率
else if (slope.KValue < K3)
{
//Result.OutputPower -= 4;
Result.OutputPower -=powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
//斜率小于K3时（默认为0.577）1倍的功率步长降功率
else if (slope.KValue < K4)
{
//Result.OutputPower -= 4;
Result.OutputPower -=powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
/*
* else if (slope.Value < K5)
// //斜率大于K3时（默认为0.683）2倍的功率步长降功率
{
Result.OutputPower -= 2 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
*/
//斜率小于K3时（默认0.788）1倍的功率步长降功率
else if (slope.KValue < K6)
{
Result.OutputPower -= 2 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
else
{
Result.OutputPower -= 3 * powerStep;
if (Result.OutputPower <= MinPower)
{
Result.OutputPower = MinPower;
}
}
if (Result.OutputPower >= MinPower + 2 * powerStep)
{
Result.OutputPower -= 2 * powerStep;
}
else
{
Result.OutputPower = MinPower;
}
}
else
{
//PID();
}
Power();
}
/// <summary>
/// 功率匹配(使采集的功率（AcqPower）达到计算输出的功率值（Result.OutputPower）)
/// 吴新强 2013年10月12日15:49:55
/// </summary>
public static void Power()
{
if (Math.Abs(Result.OutputPower - AcqPower) >= PowerAbs)
{
if (Result.OutputPower > AcqPower)
{
DAValue += DAStep;
if (DAValue >= 2.5)
{
DAValue = 2.5;
}
}
else
{
DAValue -= DAStep;
if (DAValue <= 0.5)
{
DAValue = 0.5;
}
}
}
else
{
//todo
}
}
/// <summary>
/// 功率函数（把U^2=P*R,计算得到的电压值转化为DA值作为射频源电压输出）
/// </summary>
public static void PowerSet()
{
double daValue = (double)0;
daValue = Math.Pow(Result.OutputPower * AcqImpedance, 0.5);
Result.OutputVoltageRFSource = ControlAlgorithm.ConvertRFVolt2DAValue(daValue);
//设置DA值的最低值
if (Result.OutputVoltageRFSource <= DAValue)
Result.OutputVoltageRFSource = DAValue;
}
/// <summary>
/// 最后阶段功率冲刺
/// </summary>
public static void PowerSprint()
{
if (AcqImpedance < MaxOverRating && AcqPower < PMax)
{
Result.OutputPower *= 0.5;
}
else if (Result.OutputPower >= PMax)
{
Result.OutputPower = PMax;
}
}
/// <summary>
/// 台阶功率控制阻抗斜率
/// </summary>
public static void AdjustPower()
{
if (AcqImpedance < RMax && AcqPower < PMax)
//if (AcqImpedance < 300 && AcqPower < 40)
{
//斜率小于K1时（默认为0）2倍的功率步长加功率
if (slope.KValue < -10)
{
Result.OutputPower *= 2;
}
else if (slope.KValue < -5)
{
Result.OutputPower += 10 * powerStep;
}
else if (slope.KValue < -1.0)
{
Result.OutputPower += 6 * powerStep;
}
else if (slope.KValue < K1)
{
Result.OutputPower += 4 * powerStep;
}
//斜率小于K2时（默认为0.268、0.577）1倍的功率步长加功率
else if (slope.KValue < K2)
{
Result.OutputPower += powerStep;
}
//斜率小于K3时（默认为0.423）1倍的功率步长降功率
else if (slope.KValue < K3)
{
//Result.OutputPower -= 4;
Result.OutputPower -= powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
//斜率小于K3时（默认为0.577）1倍的功率步长降功率
else if (slope.KValue < K4)
{
//Result.OutputPower -= 4;
Result.OutputPower -=2* powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
else if (slope.Value < K5)
// //斜率大于K3时（默认为0.683）2倍的功率步长降功率
{
Result.OutputPower -= 3 * powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
//斜率小于K3时（默认0.788）1倍的功率步长降功率
else if (slope.KValue < K6)
{
Result.OutputPower -= 4 * powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
else
{
Result.OutputPower -= 5 * powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
}
else if (AcqImpedance >= RMax + MaxOverRating)
{
if (slope.Value >= K3)
{
Result.OutputPower -=5*powerStep ;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
else if (slope.Value >= K2)
{
Result.OutputPower -=3*powerStep;
if (Result.OutputPower <= 10)
{
Result.OutputPower = 10;
}
}
else
{
//todo
}
}
else if (Result.OutputPower >= PMax)
{
Result.OutputPower = PMax;
}
Power();
}
/// <summary>
/// 界面显示参数调试信息
/// </summary>
public static void Debug()
{
double setTime = ControlAlgorithm.CureTime * 60;
DebugInf += "<AlgorithmImpedance>治疗总时间:" + setTime.ToString() + " S\r\n";
DebugInf += "<AlgorithmImpedance>已经治疗时间:" + timeTreatment.ToString("0.00") + " S\r\n";
DebugInf += "<AlgorithmImpedance>输出设置电压值:" + VoltageValue.ToString("0.000") + " V\r\n";
DebugInf += "<AlgorithmImpedance>输出电压值:" + Result.OutputVoltageRFSource.ToString("0.00") + " V\r\n";
DebugInf += "<AlgorithmImpedance>输出功率值:" + Result.OutputPower.ToString("0.0") + " W\r\n";
DebugInf += "<AlgorithmImpedance>PID功率值:" + P.ToString("0.0") + " W\r\n";
DebugInf += "<AlgorithmImpedance>功率最大值:" + PMax.ToString("0.00") + " W\r\n";
DebugInf += "<AlgorithmImpedance>功率最小值:" + PMin.ToString("0.0") + " W\r\n";
DebugInf += "<AlgorithmImpedance>功率下限值:" + MinPower.ToString("0.0") + " W\r\n";
DebugInf += "<AlgorithmImpedance>目标阻抗:" + RMax.ToString("0.0") + " Ω\r\n";
DebugInf += "<AlgorithmImpedance>采集阻抗:" + AcqImpedance.ToString("0.00") + " Ω\r\n";
DebugInf += "<AlgorithmImpedance>斜率值:" + slope.KValue.ToString("0.000") + "\r\n";
DebugInf += "<AlgorithmImpedance>斜率的一阶导:" + K1Slope.KValue.ToString("0.000") + "\r\n";
DebugInf += "<AlgorithmImpedance>斜率的二阶导:" + K2Slope.KValue.ToString("0.000") + "\r\n";
DebugInf += "<AlgorithmImpedance>S1:" + S1.ToString("0.000") + "\r\n";
DebugInf += "<AlgorithmImpedance>S2:" + S2.ToString("0.000") + "\r\n";
DebugInf += "<AlgorithmImpedance>时间步长:" + timeStep.ToString("0.0") + "S\r\n";
DebugInf += "<AlgorithmImpedance>功率步长:" + powerStep.ToString("0.0") + "W\r\n";
}
#endregion
}
|
