📄 force_control.c
字号:
#include "CGJ.H" // 包含项目头文件
#include <math.h> // 包含算术运算头文件
#include "control.h" // 包含控制任务所共有的头文件
#include "i2c.h" // 包含I2C头文件
#include "DataBuffer.h" // 包含对数据缓冲处理的头文件
/*********************************************************************************
* 函数名称: ForceTask()
* 功 能: 恒力
* 入口参数：无
* 出口参数: 无
* 调用模块: OSFlagPend() 等待事件标志组函数
* OSFlagAccept() 无等待获得事件标志组函数
* OSTaskDel() 删除任务函数
* OSTaskCreate() 建立任务函数
* count_parameter 计算各种参数函数
* OSTimeDly() 任务延时函数
* 全局变量: set_control_value 命令设置值
* power_set_value 功率设定值
* task_status 事件标志组变量
* kzq 可控硅的导通周期
* 设计者: 饶阳胜
* 日期: 05-6-20
* 说明:
*********************************************************************************/
#define MAX_FORCE 1400
// 定义在快速加力状态下的各个关键字
//#define ONE_PERIOD 0
//#define TWO_PERIOD 1
//#define THREE_PERIOD 2
// 定义在测台体惯量下各个关键字
#define WAIT_CHECK 0 // 准备计时态
#define CHECK 1 // 正在计时态
#define SEND_CHECK 2 // 发送结果态
#define OVER_CHECK 3 // 计时结束态
#define KSCALE (12000.0 / 12000.0)
FP32 LastError;
FP32 PrevError;
FP32 SumError;
/********************************************************************************
* 函数名称:
* 功 能:
* 入口参数:
* 出口参数:
* 调用模块:
*
* 全局变量:
*
*--------------------------------------------------------------------------------
* 设计者:
* 日 期:
* 说 明:
********************************************************************************/
FP32 PidCalc(FP32 RealValue, FP32 SetValue)
{
FP32 dError, Error;
FP32 result;
FP32 tmp;
Error = SetValue - RealValue;
SumError += Error;
dError = LastError - PrevError;
PrevError = LastError;
// LastError = Error;
LastError = RealValue;
tmp = RealValue / SetValue;
if (tmp <= G_Kc) {
result = G_Kp * KSCALE * SetValue
- G_Kd * dError * KSCALE;
} else {
if ( RealValue / SetValue > 1.0 ) {
result = G_Ki * SumError * KSCALE
- G_Kd * dError * KSCALE;
} else {
result = (G_Kp * KSCALE * SetValue * 1.00) / (1.0 - G_Kc) * (1.0 - RealValue / ((1.0 - G_Kc + G_Kp) / G_Kp * SetValue * 1.00))
+ G_Ki * SumError * KSCALE
- G_Kd * dError * KSCALE;
}
/* result = (G_Kp * KSCALE * SetValue * 0.97) / (1.0 - G_Kc) * (1.0 - RealValue / ((1.0 - G_Kc + G_Kp) / G_Kp * SetValue * 0.97))
+ G_Ki * SumError * KSCALE
- G_Kd * dError * KSCALE; */
}
// result = G_Kp * 0.25 * SetValue + G_Ki * SumError * 0.25 * SetValue + G_Kd * dError * 0.25 * SetValue;
return result;
}
/********************************************************************************
* 函数名称:
* 功 能:
* 入口参数:
* 出口参数:
* 调用模块:
*
* 全局变量:
*
*--------------------------------------------------------------------------------
* 设计者:
* 日 期:
* 说 明:
********************************************************************************/
void PidValueReset(void)
{
LastError = 0;
PrevError = 0;
SumError = 0;
}
/*********************************************************************************************************
** 函数名称: ForceTask()
** 功能描述: 恒扭距功能函数
**
** 输 入 : 无
** 输 出 : 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者:
** 日 期:
**-------------------------------------------------------------------------------------------------------
** 修 改:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void ForceTask(void *pdata)
{
INT8U l, time, err; // 出错信息变量
BOOLEAN begin, over; // 指示任务开始和结束的布尔变量
OS_FLAGS flag; // 事件标志组变量
FP32 copy_force_set_value,
copy_force_real_value,
copy_speed_real_value,
Aver_Speed;
FP32 ForceOut; // IGBT输出
FP32 FirstSpeed, CurrSpeed;
INT32U AccelTick;
FirstSpeed = 0;
CurrSpeed = 0;
AccelTick = 0;
Aver_Speed = 0;
LastError = 0;
PrevError = 0;
SumError = 0;
while(1){
begin = FALSE; // 任务开始布尔变量初始化
while(1){
flag = OSFlagPend(task_status, // 调用等事件标志组函数
FORCE_FLAG,
OS_FLAG_WAIT_SET_ANY + OS_FLAG_CONSUME,
0,
&err);
switch(flag){
case SET_FORCE_VALUE:
OS_ENTER_CRITICAL(); // 访问全局变量前,关中断
force_set_value = set_control_value; // 收到设定力命令,存储
OS_EXIT_CRITICAL();
SendProgramRunStatus(FORCE_SET_SUCCESS);
break;
case TEST_WASTE: // 收到检测台体损耗命令
break;
case START_CONTROL_FORCE: // 收到开始控制速度命令
if(force_set_value >= 20){ // 只有在设定的力大于等于100N的情况下,才能进入恒力测功状态
IGBT_SetPeriodCount(0);
LastError = 0;
PrevError = 0;
SumError = 0;
OS_ENTER_CRITICAL();
OSTaskSuspend(WAIT_TASK_PRIO); // 挂起空闲等待任务
OSTaskSuspend(POWER_TASK_PRIO); // 挂起恒功率任务
OSTaskSuspend(SPEED_TASK_PRIO); // 挂起恒速度任务
OSTaskSuspend(DEMA_TASK_PRIO); // 挂起标定任务
OSTaskSuspend(ROAD_SIMUL_TASK_PRIO); // 挂起道路模拟任务
OS_EXIT_CRITICAL();
begin = TRUE;
SendProgramRunStatus(ENTER_FORCE_CONTROL);
}else{
SendProgramRunStatus(FORCE_SET_VALUE_TOO_SMALL);
}
break;
default:
break;
}
if(begin == TRUE) break; // 如果任务正式开始,则跳出等开始命令循环
}
time = 0; // 控制计算速度频率的参数清0
l = 0; // 控制向上位机发送数据的参数清0
over = FALSE; // 任务结束布尔变量初始化
ForceOut = 0;
OS_ENTER_CRITICAL(); // 处理全局变量,关中断
sc_t1cr0 = T1TC; // 得到实时的T1TC值
sc_t1cr1 = T1TC;
OS_EXIT_CRITICAL(); // 全局变量处理完成,开中断
OSTimeDly(TASK_DELAY_TIME * 4); // 将任务挂起几个节拍,
// 这样做,是为了在第一次计算时,得到正确的
// 速度值和市电频率周期
while(1){
flag = OSFlagAccept(task_status, // 调用无等待获得事件标志组中的事件标志函数
FORCE_FLAG,
OS_FLAG_WAIT_SET_ANY + OS_FLAG_CONSUME,
&err);
switch(flag){
case SET_FORCE_VALUE: // 收到设定力命令,存储
if(set_control_value >= 20){ // 只有在新设定的力值大于20Kg时,力设定值才能被更新
force_set_value = set_control_value;
SendProgramRunStatus(FORCE_SET_SUCCESS);
}else{
SendProgramRunStatus(FORCE_SET_VALUE_TOO_SMALL);
}
break;
case STOP_CONTROL: // 收到停止控制命令
over = TRUE; // 结束布尔变量置有效
IGBT_SetPeriodCount(0); // IGBT关闭输出
IGBT_Disable();
OSTaskResume(POWER_TASK_PRIO); // 唤醒恒功率任务
OSTaskResume(SPEED_TASK_PRIO); // 唤醒恒速度任务
OSTaskResume(DEMA_TASK_PRIO); // 唤醒标定任务
OSTaskResume(ROAD_SIMUL_TASK_PRIO); // 唤醒道路模拟阻力任务
OSTaskResume(WAIT_TASK_PRIO); // 唤醒等待任务
SendProgramRunStatus(EXIT_FORCE_CONTROL);
OS_ENTER_CRITICAL();
task_status->OSFlagFlags = 0; // 在退出速度控制前,清除事件标志组
OS_EXIT_CRITICAL();
break;
default:
break;
}
if(over == TRUE) break; // 已收到结束命令,退出恒力状态
// 得到上次的实时速度值,如果上次速度值大于
copy_speed_real_value = parameter[SPEED];
// 10km/h,则每0.01秒计算一次速度,否则每0.1秒计算一次
if(copy_speed_real_value > 10){
CountSpeed();
time = 0;
}else if(++time >= COUNT_SPEED_TIME){
time = 0;
CountSpeed(); // 计算速度
}
CountForce();
copy_force_real_value = parameter[FORCE]; // 得到实时力值
copy_speed_real_value = parameter[SPEED]; // 得到实时速度值
copy_force_set_value = force_set_value; // 得到速度设定值
//=============================================================================================================
// PID控制的代码
// 如果速度回落到10Km以下,涡流机输出为0
if(Aver_Speed > 10){ // 如果速度超过门槛,开始进行控制
ForceOut = PidCalc(copy_force_real_value, copy_force_set_value);
IGBT_Enable();
IGBT_SetPeriodCount((INT32U)ForceOut);
}else{
ForceOut = 0;
LastError = 0;
PrevError = 0;
SumError = 0;
IGBT_SetPeriodCount(0);
IGBT_Disable();
}
//=============================================================================================================
InsertSpeedPoint(parameter[SPEED]); // 将当前速度值与力值插入缓冲区
InsertForcePoint(parameter[FORCE]);
// 判断是否到了向上位机送数时间
if(++l == SEND_DELAY_TIME){ // 向上位机送出检测到的数据
l = 0; // 向上位机送数据计时器清0
OS_ENTER_CRITICAL();
parameter[SPEED] = GetAdverOfSpeed(); // 得到速度的平均值
parameter[FORCE] = GetAdverOfForce(); // 得到力的平均值
parameter[POWER] = parameter[SPEED] * parameter[FORCE] / 360.0 / 10.0;
CurrSpeed = parameter[SPEED];
Aver_Speed = parameter[SPEED];
OS_EXIT_CRITICAL();
SendRealParaToComputer();
}
if(++AccelTick == 100) {
AccelTick = 0;
OS_ENTER_CRITICAL();
if(CurrSpeed > 5) {
parameter[JSD] = (CurrSpeed - FirstSpeed) * 1000.0 / 3600.0;
}else {
parameter[JSD] = 0.0;
}
OS_EXIT_CRITICAL();
FirstSpeed = CurrSpeed;
}
OSTimeDly(TASK_DELAY_TIME); // 将任务挂起2个节拍
}
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -