本帖最后由 zero949079783 于 2024-4-14 13:40 编辑
上位机使用：正点原子ATK_PID硬件：正点原子电机开发板
直流有刷电机 (电流环):链接：https://pan.baidu.com/s/1cpPnogMKNhTAeAOBJlFAxA?pwd=n8ut
提取码：n8ut
直流有刷电机 (速度环):链接：https://pan.baidu.com/s/1bd6SHPedWeDGvbiKfD88Vg?pwd=0vvt
提取码：0vvt
直流有刷电机 (位置环):
链接：https://pan.baidu.com/s/1Cy3HBHYqmDLjFAGRBZYWVQ?pwd=20dn
提取码：20dn
电流环：
#include "main.h"
#include "math.h"
/************************************* 第二部分 电压电流温度采集 **********************************************/
uint16_t g_adc_val[ADC_CH_NUM]; /*ADC平均值存放数组*/
PID_TypeDef g_current_pid; /* 电流环PID参数结构体 */
/*
Rt = Rp *exp(B*(1/T1-1/T2))
Rt 是热敏电阻在T1温度下的阻值；
Rp是热敏电阻在T2常温下的标称阻值；
exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
T1就是所求的温度
*/
const float Rp = 10000.0f; /* 10K */
const float T2 = (273.15f + 25.0f); /* T2 */
const float Bx = 3380.0f; /* B */
const float Ka = 273.15f;
Motor_TypeDef g_motor_data; /*电机参数变量*/
ENCODE_TypeDef g_encode; /*编码器参数变量*/
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] 计算温度值
* @param para: 温度采集对应ADC通道的值（已滤波）
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 计算温度分为两步：
1.根据ADC采集到的值计算当前对应的Rt
2.根据Rt计算对应的温度值
* @retval 温度值
*/
float get_temp(uint16_t para)
{
float Rt;
float temp;
/*
第一步：
Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;
*/
Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; /* 根据当前ADC值计算出Rt的值 */
/*
第二步：
根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2))
*/
temp = Rt / Rp; /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
temp = log(temp); /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
temp /= Bx; /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
temp += (1.0f / T2); /* 解出1/T1 ，即temp = 1/T1 */
temp = 1.0f / (temp); /* 解出T1 ，即temp = T1 */
temp -= Ka; /* 计算T1对应的摄氏度 */
return temp; /* 返回温度值 */
}
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] 计算ADC的平均值（滤波）
* @param * p ：存放ADC值的指针地址
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 此函数对电压、温度、电流对应的ADC值进行滤波，
* p[0]-p[2]对应的分别是电压、温度和电流
* @retval 无
*/
void calc_adc_val(uint16_t * adcdata)
{
uint32_t temp[3] = {0,0,0};
uint16_t i;
for(i=0;i<ADC_COLL;i++) /* 循环ADC_COLL次取值，累加 */
{
temp[0] += adcdata[0+i*ADC_CH_NUM];
temp[1] += adcdata[1+i*ADC_CH_NUM];
temp[2] += adcdata[2+i*ADC_CH_NUM];
}
temp[0] /= ADC_COLL; /* 取平均值 */
temp[1] /= ADC_COLL;
temp[2] /= ADC_COLL;
g_adc_val[0] = temp[0]; /* 存入电压ADC通道平均值 */
g_adc_val[1] = temp[1]; /* 存入温度ADC通道平均值 */
g_adc_val[2] = temp[2]; /* 存入电流ADC通道平均值 */
}
uint8_t current_flag =0;
void get_Motor_value(uint16_t * adcdata)
{
#define CURRENT_NUM 4
#define INIT_CURRENT_NUM 5
float temp_current=0.0;
static float adc_current = 0;
static float init_adc_current = 0;
static uint8_t current_count1 =0,current_count2 =0;
current_count1++;
calc_adc_val(adcdata);
adc_current += g_adc_val[2]; /* 取出电流通道对应的ADC值进行累计 */
if(current_count1 >= CURRENT_NUM)/* 累计15次 */
{
adc_current = (float)(adc_current / current_count1); /* 取平均值 */
if(current_count2 <= INIT_CURRENT_NUM)
{
current_count2++;
init_adc_current += adc_current; /* 对平均值累计求和 */
if(current_count2 == INIT_CURRENT_NUM)
{
init_adc_current = (float)(init_adc_current/current_count2); /* 存储初始ADC值 */
current_count2 = INIT_CURRENT_NUM+1;
current_flag =1;
}
}
if(current_count2 >= (INIT_CURRENT_NUM+1))
{
temp_current= ((adc_current - init_adc_current) *ADC2CURT);
// g_motor_data.current = temp_current;
FirstOrderRC_LPF(g_motor_data.current,temp_current,0.40);
//g_motor_data.current = (float)((g_motor_data.current * (float)0.60) + ((float)0.40 * temp_current)); /* 一阶低通滤波 */
if(g_motor_data.current <=20)
{
g_motor_data.current=0.0;
}
}
adc_current=0;
current_count1=0;
}
}
/************************************* 第三部分 编码器测速 ****************************************************/
void speed_computer(int32_t encode_now, uint8_t ms)
{
uint8_t i = 0, j = 0;
float temp = 0.0;
static uint8_t sp_count = 0, k = 0;
static float speed_arr[10] = {0.0}; /* 存储速度进行滤波运算 */
if (sp_count == ms) /* 计算一次速度 */
{
/* 计算电机转速
第一步 ：计算ms毫秒内计数变化量
第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
第四步 ：除以减速比即可得出电机转速
*/
g_encode.encode_now = encode_now; /* 取出编码器当前计数值 */
g_encode.speed = (g_encode.encode_now - g_encode.encode_old); /* 计算编码器计数值的变化量 */
speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO ); /* 保存电机转速 */
g_encode.encode_old = g_encode.encode_now; /* 保存当前编码器的值 */
/* 累计10次速度值，后续进行滤波*/
if (k == 10)
{
for (i = 10; i >= 1; i--) /* 冒泡排序*/
{
for (j = 0; j < (i - 1); j++)
{
if (speed_arr[j] > speed_arr[j + 1]) /* 数值比较 */
{
temp = speed_arr[j]; /* 数值换位 */
speed_arr[j] = speed_arr[j + 1];
speed_arr[j + 1] = temp;
}
}
}
temp = 0.0;
for (i = 2; i < 8; i++) /* 去除两边高低数据 */
{
temp += speed_arr[i]; /* 将中间数值累加 */
}
temp = (float)(temp / 6); /*求速度平均值*/
/* 一阶低通滤波
* 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
* 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
* q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
*/
g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
k = 0;
}
sp_count = 0;
}
sp_count ++;
}
uint8_t g_run_flag = 0;
int32_t motor_pwm = 0;
atk_debug_data_rev get_atk_data;
void DCMotor_App(void)
{
static uint8_t key=0;
get_atkdebug(&get_atk_data);
key=key_scan(0);
if(key == KEY0_PRES)
{
TurnTogLed(LED1);
g_current_pid.SetPoint +=50;
g_run_flag = 1;
if (g_current_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
DC_Motor_Start(MOTOR_1);
if(g_current_pid.SetPoint >= 250)
{
g_current_pid.SetPoint =250;
}
atk_debug_send_motorstate(RUN_STATE); /* 上传电机状态（运行） */
//Motor_PWM_Set(g_current_pid.SetPoint,MOTOR_1);
}
}
else if(key == KEY1_PRES) {
TurnTogLed(LED1);
if (g_current_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
}
}
else if(key == KEY2_PRES)
{
DC_Motor_Stop(MOTOR_1);
g_run_flag = 0; /* 标记电机停止 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转向、速度 */
pid_init();
atk_debug_send_motorstate(BREAKED_STATE); /* 上传电机状态（刹车） */
atk_debug_send_initdata(TYPE_PID1, (float *)(&g_current_pid.SetPoint), KP, KI, KD); /* 同步数据到上位机 */
}
}
void DCMotor_Value(void)
{
static uint16_t g_time =0;
int32_t motor_pwm_temp = 0;
if(current_flag == 0 )
return;
/* 获取编码器值，用于计算速度 */
get_EncodeValue(5); /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
if(g_time % 499 ==0)
{
g_motor_data.volatage = g_adc_val[0]*ADC2VBUS;
atk_debug_send_valtage(g_motor_data.volatage );
__NOP();__NOP();__NOP();
g_motor_data.board_temp = get_temp(g_adc_val[1]);
__NOP();__NOP();__NOP();
atk_debug_send_temp(0,get_temp(g_adc_val[1]));
__NOP();__NOP();__NOP();
//g_motor_data.current = (g_adc_val[2]*ADC2CURT)*0.001;
atk_debug_send_current(g_motor_data.current*0.001,0,0);
__NOP();__NOP();__NOP();
atk_debug_send_speed(g_motor_data.speed);
__NOP();__NOP();__NOP();
// printf("\r\n******************************\r\n");
// printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
// printf("Valtage:%.2fV \r\n", g_motor_data.volatage); /* 打印电压值 */
// printf("Temp:%.2fC \r\n", g_motor_data.board_temp ); /* 打印温度值 */
// printf("Current:%.2fmA \r\n", g_motor_data.current); /* 打印电流值 */
// printf("电机速度：%.1f RPM\r\n\r\n", g_motor_data.speed);
// printf("\r\n******************************\r\n");
}
else if(g_time % SMAPLSE_PID_SPEED == 0) /* 电机速度 50ms进行一次pid计算 */
{
if(g_run_flag)
{
/* PID计算，输出比较值（占空比） */
motor_pwm_temp = increment_pid_ctrl (&g_current_pid,g_motor_data.current);
g_motor_data.motor_pwm = (int32_t)((g_motor_data.motor_pwm * 0.5) + (motor_pwm_temp * 0.5));
if(g_motor_data.motor_pwm >= 8200)
{
g_motor_data.motor_pwm = 8200;
}
else if (g_motor_data.motor_pwm <= 0)
{
g_motor_data.motor_pwm = 0;
}
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
atk_debug_send_wave_data( 1 ,g_motor_data.current); /* 选择通道1发送实际电流（波形显示）*/
atk_debug_send_wave_data( 2 ,g_current_pid.SetPoint); /* 选择通道2，发送目标电流（波形显示）*/
atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400); /* 选择通道3，发送占空比（波形显示）*/
}
g_time ++;
}
void get_EncodeValue(uint8_t ms)
{
int encode_now = gtim_get_encode();
speed_computer(encode_now, ms);
}
PID_TypeDef g_speed_pid; /* 速度环PID参数结构体 */
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] pid初始化
* @param 无
* @retval 无
*/
void pid_init(void)
{
memset(&g_motor_data,0,sizeof(g_motor_data));
memset(&g_current_pid,0,sizeof(g_current_pid));
g_current_pid.SetPoint = 0; /* 设定目标值 */
g_current_pid.ActualValue = 0.0; /* 期望输出值 */
g_current_pid.SumError = 0.0; /* 积分值 */
g_current_pid.Error = 0.0; /* Error[1] */
g_current_pid.LastError = 0.0; /* Error[-1] */
g_current_pid.PrevError = 0.0; /* Error[-2] */
g_current_pid.Proportion = KP; /* 比例常数 Proportional Const */
g_current_pid.Integral = KI; /* 积分常数 Integral Const */
g_current_pid.Derivative = KD; /* 微分常数 Derivative Const */
}
void DCMotorsample_Init(void)
{
pid_init();
ADC1CbReg(get_Motor_value);
}
速度环：
#include "main.h"
#include "math.h"
/************************************* 第二部分 电压电流温度采集 **********************************************/
uint16_t g_adc_val[ADC_CH_NUM]; /*ADC平均值存放数组*/
PID_TypeDef g_speed_pid; /*速度环PID参数结构体*/
/*
Rt = Rp *exp(B*(1/T1-1/T2))
Rt 是热敏电阻在T1温度下的阻值；
Rp是热敏电阻在T2常温下的标称阻值；
exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
T1就是所求的温度
*/
const float Rp = 10000.0f; /* 10K */
const float T2 = (273.15f + 25.0f); /* T2 */
const float Bx = 3380.0f; /* B */
const float Ka = 273.15f;
Motor_TypeDef g_motor_data; /*电机参数变量*/
ENCODE_TypeDef g_encode; /*编码器参数变量*/
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] 计算温度值
* @param para: 温度采集对应ADC通道的值（已滤波）
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 计算温度分为两步：
1.根据ADC采集到的值计算当前对应的Rt
2.根据Rt计算对应的温度值
* @retval 温度值
*/
float get_temp(uint16_t para)
{
float Rt;
float temp;
/*
第一步：
Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;
*/
Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; /* 根据当前ADC值计算出Rt的值 */
/*
第二步：
根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2))
*/
temp = Rt / Rp; /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
temp = log(temp); /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
temp /= Bx; /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
temp += (1.0f / T2); /* 解出1/T1 ，即temp = 1/T1 */
temp = 1.0f / (temp); /* 解出T1 ，即temp = T1 */
temp -= Ka; /* 计算T1对应的摄氏度 */
return temp; /* 返回温度值 */
}
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] 计算ADC的平均值（滤波）
* @param * p ：存放ADC值的指针地址
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 此函数对电压、温度、电流对应的ADC值进行滤波，
* p[0]-p[2]对应的分别是电压、温度和电流
* @retval 无
*/
void calc_adc_val(uint16_t * adcdata)
{
uint32_t temp[3] = {0,0,0};
uint16_t i;
for(i=0;i<ADC_COLL;i++) /* 循环ADC_COLL次取值，累加 */
{
temp[0] += adcdata[0+i*ADC_CH_NUM];
temp[1] += adcdata[1+i*ADC_CH_NUM];
temp[2] += adcdata[2+i*ADC_CH_NUM];
}
temp[0] /= ADC_COLL; /* 取平均值 */
temp[1] /= ADC_COLL;
temp[2] /= ADC_COLL;
g_adc_val[0] = temp[0]; /* 存入电压ADC通道平均值 */
g_adc_val[1] = temp[1]; /* 存入温度ADC通道平均值 */
g_adc_val[2] = temp[2]; /* 存入电流ADC通道平均值 */
}
void get_Motor_value(uint16_t * adcdata)
{
calc_adc_val(adcdata);
}
/************************************* 第三部分 编码器测速 ****************************************************/
void speed_computer(int32_t encode_now, uint8_t ms)
{
uint8_t i = 0, j = 0;
float temp = 0.0;
static uint8_t sp_count = 0, k = 0;
static float speed_arr[10] = {0.0}; /* 存储速度进行滤波运算 */
if (sp_count == ms) /* 计算一次速度 */
{
/* 计算电机转速
第一步 ：计算ms毫秒内计数变化量
第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
第四步 ：除以减速比即可得出电机转速
*/
g_encode.encode_now = encode_now; /* 取出编码器当前计数值 */
g_encode.speed = (g_encode.encode_now - g_encode.encode_old); /* 计算编码器计数值的变化量 */
speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO ); /* 保存电机转速 */
g_encode.encode_old = g_encode.encode_now; /* 保存当前编码器的值 */
/* 累计10次速度值，后续进行滤波*/
if (k == 10)
{
for (i = 10; i >= 1; i--) /* 冒泡排序*/
{
for (j = 0; j < (i - 1); j++)
{
if (speed_arr[j] > speed_arr[j + 1]) /* 数值比较 */
{
temp = speed_arr[j]; /* 数值换位 */
speed_arr[j] = speed_arr[j + 1];
speed_arr[j + 1] = temp;
}
}
}
temp = 0.0;
for (i = 2; i < 8; i++) /* 去除两边高低数据 */
{
temp += speed_arr[i]; /* 将中间数值累加 */
}
temp = (float)(temp / 6); /*求速度平均值*/
/* 一阶低通滤波
* 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
* 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
* q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
*/
g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
k = 0;
}
sp_count = 0;
}
sp_count ++;
}
uint8_t g_run_flag = 0;
int32_t motor_pwm = 0;
void DCMotor_App(void)
{
static uint8_t key=0;
key=key_scan(0);
if(key == KEY0_PRES)
{
TurnTogLed(LED1);
g_speed_pid.SetPoint +=30;
g_run_flag = 1;
if (g_speed_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
DC_Motor_Start(MOTOR_1);
if(g_speed_pid.SetPoint >= 300)
{
g_speed_pid.SetPoint =300;
}
atk_debug_send_motorstate(RUN_STATE); /* 上传电机状态（运行） */
DC_Motor_Speed(g_speed_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
}
}
else if(key == KEY1_PRES) {
TurnTogLed(LED1);
g_speed_pid.SetPoint -= 30;
g_run_flag = 1;
if (g_speed_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
DC_Motor_Start(MOTOR_1);
if(g_speed_pid.SetPoint <= -300)
{
g_speed_pid.SetPoint =-300;
}
DC_Motor_Speed(g_speed_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
atk_debug_send_motorstate(RUN_STATE); /* 上传电机状态（运行） */
}
}
else if(key == KEY2_PRES)
{
DC_Motor_Stop(MOTOR_1);
g_run_flag = 0; /* 标记电机停止 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转向、速度 */
g_speed_pid.SetPoint = 0;
pid_init();
atk_debug_send_motorstate(BREAKED_STATE); /* 上传电机状态（刹车） */
atk_debug_send_initdata(TYPE_PID1, (float *)(&g_speed_pid.SetPoint), KP, KI, KD); /* 同步数据到上位机 */
}
}
void DCMotor_Value(void)
{
static uint16_t init_adc_val = 0;
static uint16_t g_time =0;
static uint8_t num = 0;
if(num < 2)
{
num ++;
atk_debug_send_motorcode(DC_MOTOR); /* 上传电机类型（直流有刷电机） */
atk_debug_send_motorstate(IDLE_STATE); /* 上传电机状态（空闲） */
init_adc_val = g_adc_val[2];
init_adc_val += g_adc_val[2]; /* 现在的值和上一次存储的值相加 */
init_adc_val /= 2;
}else
{
/* 获取编码器值，用于计算速度 */
get_EncodeValue(5); /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
if(g_time % 1000 ==0)
{
__NOP();__NOP();__NOP();
g_motor_data.board_temp = get_temp(g_adc_val[1]);
//
printf("\r\n******************************\r\n");
printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
printf("Valtage:%.2fV \r\n", g_motor_data.volatage); /* 打印电压值 */
printf("Temp:%.2fC \r\n", g_motor_data.board_temp ); /* 打印温度值 */
printf("Current:%.2fmA \r\n", g_motor_data.current); /* 打印电流值 */
printf("电机速度：%.1f RPM\r\n\r\n", g_motor_data.speed);
printf("\r\n******************************\r\n");
}
else if(g_time %300 == 0)
{
atk_debug_send_temp(0,get_temp(g_adc_val[1]));
}
else if(g_time %200 == 0)
{
g_motor_data.volatage = g_adc_val[0]*ADC2VBUS;
atk_debug_send_valtage(g_motor_data.volatage );
}
else if(g_time % SMAPLSE_PID_SPEED == 0) /* 电机速度 50ms进行一次pid计算 */
{
atk_debug_send_speed(g_motor_data.speed);
if(g_run_flag)
{
/* PID计算，输出比较值（占空比） */
g_motor_data.motor_pwm = increment_pid_ctrl (&g_speed_pid,g_motor_data.speed);
if(g_motor_data.motor_pwm >= 8200)
{
g_motor_data.motor_pwm = 8200;
}
else if (g_motor_data.motor_pwm <= -8200)
{
g_motor_data.motor_pwm = -8200;
}
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
atk_debug_send_wave_data( 1 ,g_motor_data.speed); /* 选择通道1，发送实际速度（波形显示）*/
atk_debug_send_wave_data( 2 ,g_speed_pid.SetPoint); /* 选择通道2，发送目标速度（波形显示）*/
atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400); /* 选择通道3，发送占空比（波形显示）*/
}
else if(g_time % 20 == 0)
{
g_motor_data.current = (abs(g_adc_val[2]-init_adc_val)*ADC2CURT)*0.001;
atk_debug_send_current(g_motor_data.current,0,0);
}
g_time ++;
}
}
void get_EncodeValue(uint8_t ms)
{
int encode_now = gtim_get_encode();
speed_computer(encode_now, ms);
}
PID_TypeDef g_speed_pid; /* 速度环PID参数结构体 */
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] pid初始化
* @param 无
* @retval 无
*/
void pid_init(void)
{
memset(&g_motor_data,0,sizeof(g_motor_data));
memset(&g_speed_pid,0,sizeof(g_speed_pid));
/* 初始化速度环PID参数 */
g_speed_pid.SetPoint = 0; /* 设定目标值 */
g_speed_pid.ActualValue = 0.0; /* 期望输出值 */
g_speed_pid.SumError = 0.0; /* 积分值 */
g_speed_pid.Error = 0.0; /* Error[1] */
g_speed_pid.LastError = 0.0; /* Error[-1] */
g_speed_pid.PrevError = 0.0; /* Error[-2] */
g_speed_pid.Proportion = KP; /* 比例常数 Proportional Const */
g_speed_pid.Integral = KI; /* 积分常数 Integral Const */
g_speed_pid.Derivative = KD; /* 微分常数 Derivative Const */
atk_debug_send_wave_data( 1 ,g_motor_data.speed); /* 选择通道1，发送实际速度（波形显示）*/
atk_debug_send_wave_data( 2 ,g_speed_pid.SetPoint); /* 选择通道2，发送目标速度（波形显示）*/
atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400); /* 选择通道3，发送占空比（波形显示）*/
}
void DCMotorsample_Init(void)
{
pid_init();
ADC1CbReg(get_Motor_value);
}
位置环:#include "main.h"
#include "math.h"
/************************************* 第二部分 电压电流温度采集 **********************************************/
uint16_t g_adc_val[ADC_CH_NUM]; /*ADC平均值存放数组*/
PID_TypeDef g_location_pid; /*位置环PID参数结构体*/
/*
Rt = Rp *exp(B*(1/T1-1/T2))
Rt 是热敏电阻在T1温度下的阻值；
Rp是热敏电阻在T2常温下的标称阻值；
exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
T1就是所求的温度
*/
const float Rp = 10000.0f; /* 10K */
const float T2 = (273.15f + 25.0f); /* T2 */
const float Bx = 3380.0f; /* B */
const float Ka = 273.15f;
Motor_TypeDef g_motor_data; /*电机参数变量*/
ENCODE_TypeDef g_encode; /*编码器参数变量*/
/**
* [url=home.php?mod=space&uid=247401]@brief[/url] 计算温度值
* @param para: 温度采集对应ADC通道的值（已滤波）
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 计算温度分为两步：
1.根据ADC采集到的值计算当前对应的Rt
2.根据Rt计算对应的温度值
* @retval 温度值
*/
float get_temp(uint16_t para)
{
float Rt;
float temp;
/*
第一步：
Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;
*/
Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; /* 根据当前ADC值计算出Rt的值 */
/*
第二步：
根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2))
*/
temp = Rt / Rp; /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
temp = log(temp); /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
temp /= Bx; /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
temp += (1.0f / T2); /* 解出1/T1 ，即temp = 1/T1 */
temp = 1.0f / (temp); /* 解出T1 ，即temp = T1 */
temp -= Ka; /* 计算T1对应的摄氏度 */
return temp; /* 返回温度值 */
}
/**
* @brief 计算ADC的平均值（滤波）
* @param * p ：存放ADC值的指针地址
* [url=home.php?mod=space&uid=536309]@NOTE[/url] 此函数对电压、温度、电流对应的ADC值进行滤波，
* p[0]-p[2]对应的分别是电压、温度和电流
* @retval 无
*/
void calc_adc_val(uint16_t * adcdata)
{
uint32_t temp[3] = {0,0,0};
uint16_t i;
for(i=0;i<ADC_COLL;i++) /* 循环ADC_COLL次取值，累加 */
{
temp[0] += adcdata[0+i*ADC_CH_NUM];
temp[1] += adcdata[1+i*ADC_CH_NUM];
temp[2] += adcdata[2+i*ADC_CH_NUM];
}
temp[0] /= ADC_COLL; /* 取平均值 */
temp[1] /= ADC_COLL;
temp[2] /= ADC_COLL;
g_adc_val[0] = temp[0]; /* 存入电压ADC通道平均值 */
g_adc_val[1] = temp[1]; /* 存入温度ADC通道平均值 */
g_adc_val[2] = temp[2]; /* 存入电流ADC通道平均值 */
}
void get_Motor_value(uint16_t * adcdata)
{
calc_adc_val(adcdata);
}
/************************************* 第三部分 编码器测速 ****************************************************/
void speed_computer(int32_t encode_now, uint8_t ms)
{
uint8_t i = 0, j = 0;
float temp = 0.0;
static uint8_t sp_count = 0, k = 0;
static float speed_arr[10] = {0.0}; /* 存储速度进行滤波运算 */
if (sp_count == ms) /* 计算一次速度 */
{
/* 计算电机转速
第一步 ：计算ms毫秒内计数变化量
第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
第四步 ：除以减速比即可得出电机转速
*/
g_encode.encode_now = encode_now; /* 取出编码器当前计数值 */
g_encode.speed = (g_encode.encode_now - g_encode.encode_old); /* 计算编码器计数值的变化量 */
speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO ); /* 保存电机转速 */
g_encode.encode_old = g_encode.encode_now; /* 保存当前编码器的值 */
/* 累计10次速度值，后续进行滤波*/
if (k == 10)
{
for (i = 10; i >= 1; i--) /* 冒泡排序*/
{
for (j = 0; j < (i - 1); j++)
{
if (speed_arr[j] > speed_arr[j + 1]) /* 数值比较 */
{
temp = speed_arr[j]; /* 数值换位 */
speed_arr[j] = speed_arr[j + 1];
speed_arr[j + 1] = temp;
}
}
}
temp = 0.0;
for (i = 2; i < 8; i++) /* 去除两边高低数据 */
{
temp += speed_arr[i]; /* 将中间数值累加 */
}
temp = (float)(temp / 6); /*求速度平均值*/
/* 一阶低通滤波
* 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
* 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
* q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
*/
g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
k = 0;
}
sp_count = 0;
}
sp_count ++;
}
uint8_t g_run_flag = 0;
int32_t motor_pwm = 0;
void DCMotor_App(void)
{
static uint8_t key=0;
key=key_scan(0);
if(key == KEY0_PRES)
{
TurnTogLed(LED1);
g_location_pid.SetPoint +=1320; /* 正转一圈，电机旋转圈数 = 计数值变化量 / 44 / 30 */
g_run_flag = 1;
if (g_location_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
DC_Motor_Start(MOTOR_1);
if(g_location_pid.SetPoint >= 6600)
{
g_location_pid.SetPoint =6600;
}
atk_debug_send_motorstate(RUN_STATE); /* 上传电机状态（运行） */
DC_Motor_Speed(g_location_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
}
}
else if(key == KEY1_PRES) {
TurnTogLed(LED1);
g_location_pid.SetPoint -= 1320; /* 正转一圈，电机旋转圈数 = 计数值变化量 / 44 / 30 */
g_run_flag = 1;
if (g_location_pid.SetPoint == 0)
{
DC_Motor_Stop(MOTOR_1); /* 停止则立刻响应 */
g_motor_data.motor_pwm = 0;
DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
}
else
{
DC_Motor_Start(MOTOR_1);
if(g_location_pid.SetPoint <= -6600)
{
g_location_pid.SetPoint =-6600;
}
DC_Motor_Speed(g_location_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
atk_debug_send_motorstate(RUN_STATE); /* 上传电机状态（运行） */
}
}
else if(key == KEY2_PRES)
{
DC_Motor_Stop(MOTOR_1);
g_run_flag = 0; /* 标记电机停止 */
g_motor_data.motor_pwm = 0;
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转向、速度 */
g_location_pid.SetPoint = 0;
pid_init();
atk_debug_send_motorstate(BREAKED_STATE); /* 上传电机状态（刹车） */
atk_debug_send_initdata(TYPE_PID1, (float *)(&g_location_pid.SetPoint), KP, KI, KD); /* 同步数据到上位机 */
}
}
void DCMotor_Value(void)
{
int32_t motor_pwm_temp = 0;
static uint16_t init_adc_val = 0;
static uint16_t g_time =0;
static uint8_t num = 0;
if(num < 2)
{
num ++;
atk_debug_send_motorcode(DC_MOTOR); /* 上传电机类型（直流有刷电机） */
atk_debug_send_motorstate(IDLE_STATE); /* 上传电机状态（空闲） */
init_adc_val = g_adc_val[2];
init_adc_val += g_adc_val[2]; /* 现在的值和上一次存储的值相加 */
init_adc_val /= 2;
}else
{
/* 获取编码器值，用于计算速度 */
get_EncodeValue(5); /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
if(g_time % 1000 ==0)
{
g_motor_data.volatage = g_adc_val[0]*ADC2VBUS;
atk_debug_send_valtage(g_motor_data.volatage );
__NOP();__NOP();__NOP();
g_motor_data.board_temp = get_temp(g_adc_val[1]);
atk_debug_send_temp(0,g_motor_data.board_temp);
__NOP();__NOP();__NOP();
g_motor_data.current = (abs(g_adc_val[2]-init_adc_val)*ADC2CURT)*0.001;
atk_debug_send_current(g_motor_data.current,0,0);
__NOP();__NOP();__NOP();
// printf("\r\n******************************\r\n");
// printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
// printf("Valtage:%.2fV \r\n", g_motor_data.volatage); /* 打印电压值 */
// printf("Temp:%.2fC \r\n", g_motor_data.board_temp ); /* 打印温度值 */
// printf("Current:%.2fmA \r\n", g_motor_data.current); /* 打印电流值 */
// printf("电机速度：%.1f RPM\r\n\r\n", g_motor_data.speed);
// printf("\r\n******************************\r\n");
}
else if(g_time % SMAPLSE_PID_SPEED == 0) /* 电机速度 50ms进行一次pid计算 */
{
if(g_run_flag == 1)
{
/* PID计算，输出比较值（占空比），再进行一阶低通滤波 */
motor_pwm_temp = increment_pid_ctrl(&g_location_pid,g_motor_data.location);
g_motor_data.motor_pwm = (int32_t)((g_motor_data.motor_pwm * 0.5) + (motor_pwm_temp * 0.5));
if (g_motor_data.motor_pwm >= 3000) /* 限制占空比 */
{
g_motor_data.motor_pwm = 3000;
}
else if (g_motor_data.motor_pwm <= -3000)
{
g_motor_data.motor_pwm = -3000;
}
atk_debug_send_wave_data( 1 ,g_motor_data.location); /* 选择通道1，发送实际位置（波形显示）*/
atk_debug_send_wave_data( 2 ,g_location_pid.SetPoint); /* 选择通道2，发送目标位置（波形显示）*/
Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);
}
}
g_time ++;
}
}
void get_EncodeValue(uint8_t ms)
{
int encode_now = gtim_get_encode();
g_motor_data.location = encode_now; /* 获取当前计数总值，用于位置闭环控制 */
speed_computer(encode_now, ms);
}
/**
* @brief pid初始化
* @param 无
* @retval 无
*/
void pid_init(void)
{
memset(&g_motor_data,0,sizeof(g_motor_data));
memset(&g_location_pid,0,sizeof(g_location_pid));
g_location_pid.SetPoint = 0.0; /* 设定目标值 */
g_location_pid.ActualValue = 0.0; /* 期望输出值 */
g_location_pid.SumError = 0.0; /* 积分值 */
g_location_pid.Error = 0.0; /* Error[1] */
g_location_pid.LastError = 0.0; /* Error[-1] */
g_location_pid.PrevError = 0.0; /* Error[-2] */
g_location_pid.Proportion = KP; /* 比例常数 Proportional Const */
g_location_pid.Integral = KI; /* 积分常数 Integral Const */
g_location_pid.Derivative = KD; /* 微分常数 Derivative Const */
atk_debug_send_wave_data( 1 ,g_motor_data.speed); /* 选择通道1，发送实际速度（波形显示）*/
atk_debug_send_wave_data( 2 ,g_location_pid.SetPoint); /* 选择通道2，发送目标速度（波形显示）*/
atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400); /* 选择通道3，发送占空比（波形显示）*/
}
void DCMotorsample_Init(void)
{
pid_init();
ADC1CbReg(get_Motor_value);
}
|
|
楼主
标题置顶
标题高亮
