792 lines
20 KiB
C
792 lines
20 KiB
C
#include "motor.h"
|
||
#include "app.h"
|
||
|
||
#define STEP_LIN 16 //直行程步长单位数值（脉冲数）
|
||
#define STEP_ROT 20 //角行程步长单位数值（脉冲数）
|
||
#define DATA_LEN 10 //角行程步长单位数值（脉冲数）
|
||
|
||
#define MOTOR_STOP 0 //电机停止
|
||
#define MOTOR_POS 1 //电机正向运动
|
||
#define MOTOR_REV 2 //电机反向运动
|
||
#define MOTOR_RETURN 3 //电机返回
|
||
|
||
int magnet_middle = 1240; //磁条中点
|
||
int magnet_start = 800; //磁条范围-起点mv
|
||
int magnet_end = 1200; //磁条范围-终点mv
|
||
#define motor_start 150 //电机运动范围起点mv
|
||
#define motor_end 1850 //电机运动范围终点mv
|
||
|
||
int Travle_Flag = 0; //0 直 1 角
|
||
char Motor_Run = 2; //0 停止 1 运行 2 运行到起始点 3 运行到结束点
|
||
char Run_Mode = 0; //0 点动 1 方案一 2 方案二
|
||
unsigned int Run_Step = 0; //电机运行步长
|
||
unsigned int Run_Inter = 0; //电机运行间隔时长
|
||
unsigned int Run_Stop = 0; //到“结束点”后，停止时间
|
||
unsigned int Run_mm = 1; //行进长度（mm）/转动角度（°）
|
||
unsigned int Run_num = 0; //角行程电机转动圈数
|
||
unsigned int ct_num = 0; //磁条长度
|
||
|
||
int mov_flag = 0; //脉冲标志
|
||
int send_flag = 0; //发送标志
|
||
int seat_flag = 0; //位置标志
|
||
int motor_dire = 1; //电机转动方向
|
||
int flag = 0;
|
||
|
||
int motor_direc = 1;//电机转动方向
|
||
|
||
char motor_control = 0;
|
||
|
||
|
||
//步骤
|
||
unsigned char Runmotor_step = 0;
|
||
|
||
//次数
|
||
unsigned int Runmotor_Nums = 0;
|
||
|
||
//清除电机标记
|
||
void ClrRunmotorStep(void)
|
||
{
|
||
//步骤
|
||
Runmotor_step = 0;
|
||
//次数
|
||
Runmotor_Nums = 0;
|
||
//脉冲标记清0
|
||
mov_flag = 0;
|
||
//发送标记清0
|
||
send_flag = 0;
|
||
//位置标记清0
|
||
seat_flag = 0;
|
||
}
|
||
|
||
//处理马达运行
|
||
#define SPEED_MIN 0x00003A98 //最大速度，用于复位&远距离接近
|
||
#define SPEED_NORMAL 0x00007530 //常规速度，用于步进
|
||
#define SPEED_MAX 0x0001D4C0 //最小速度，用于近距离接近
|
||
int first_flag = 1;
|
||
void Deal_Motor(void)
|
||
{
|
||
//判断直行程还是角行程
|
||
if(Travle_Flag == 0)//直行程——电机旋转一圈，磁条水平位移5mm
|
||
{
|
||
motor_data[0] = 0x00;//00 直行程 01 角行程
|
||
//判断电机停止还是运行，运行到起始位还是结束位
|
||
if( Motor_Run == 0) //停止
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
if( speed_max != SPEED_NORMAL) //速度调整至常规速度，调整完成后不再进入此处
|
||
{
|
||
speed_max = SPEED_NORMAL; //VMAX
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
|
||
}
|
||
else if((Motor_Run == 1) && (first_flag == 0)) //运行，上电复位完成后才生效
|
||
{
|
||
if(Motor_Run >= 1 && Runmotor_step == 0)
|
||
{
|
||
Runmotor_step = 1;
|
||
}
|
||
|
||
//判断电机运行方式是点动还是连续，点动为方案三，方案一和方案二为连续
|
||
if(Run_Mode == 0)//点动（方案三）
|
||
{
|
||
|
||
mov_step();//点动
|
||
}
|
||
else if(Run_Mode == 1)//连续（方案一）“步长过大可能会越过限位开关”
|
||
{
|
||
|
||
mov_loop1();//方案一
|
||
}
|
||
// else if(Run_Mode == 2)//连续（方案二）“步长过大可能会越过限位开关”
|
||
// {
|
||
|
||
// //磁条循环“起始点-结束点-起始点”，一定次数后停在起始点
|
||
// mov_loop2();//方案二
|
||
// }
|
||
else//初始化
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
}
|
||
}
|
||
else if(Motor_Run == 2)//运行到起始位
|
||
{
|
||
int rt2_stop_cnt = 0; //停止计数，防止while卡死
|
||
if( ocin1 == 0 ) //判断是否到达始限位1
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
rt2_stop_cnt++;
|
||
if(rt2_stop_cnt > 10000)
|
||
{
|
||
rt2_stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) rt2_stop_cnt = 0;
|
||
}
|
||
|
||
if(first_flag == 1) //停止后记录第一次复位时电机位置
|
||
{
|
||
TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //读取实际位置XACTUAL
|
||
XA_32 = Raw_32(XA);
|
||
|
||
first_xa_32 = XA_32;
|
||
first_flag = 0;
|
||
}
|
||
|
||
motor_direc = MOTOR_POS; //在限位1处仅允许正向运动
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
return;
|
||
}
|
||
|
||
if( (X_ads1220 <= (motor_start) ) && (X_ads1220 != 0) ) //电机起点限位，上电初期读取到的X值为0
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
rt2_stop_cnt++;
|
||
if(rt2_stop_cnt > 10000)
|
||
{
|
||
rt2_stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) rt2_stop_cnt = 0;
|
||
}
|
||
|
||
motor_direc = MOTOR_POS;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
|
||
if(first_flag) //记录第一次复位时电机位置
|
||
{
|
||
TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //读取实际位置XACTUAL
|
||
XA_32 = Raw_32(XA);
|
||
|
||
first_xa_32 = XA_32;
|
||
first_flag = 0;
|
||
}
|
||
return;
|
||
|
||
}
|
||
|
||
if((ocin2 == 1) && (ocin2 == 1) && (X_ads1220 != 0)) //未到达限位时，快速大步长运行，上电初期：ocin1 == 1，ocin2 == 1，Xads == 0
|
||
{
|
||
if( speed_max != SPEED_MAX ) //目标速度调整为最大速度，调整完成后不再进入此处
|
||
{
|
||
speed_max = SPEED_MAX;
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); // VMAX
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
|
||
tmc5160_operate(MOTOR_REV,51200*30);
|
||
}
|
||
|
||
}
|
||
else if((Motor_Run == 3) && (first_flag == 0)) //运行到结束位
|
||
{
|
||
int rt3_stop_cnt = 0;
|
||
if( ocin2 == 0 ) //判断是否到达终限位
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
rt3_stop_cnt++;
|
||
if(rt3_stop_cnt > 10000)
|
||
{
|
||
rt3_stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) rt3_stop_cnt = 0;
|
||
}
|
||
|
||
motor_direc = MOTOR_REV;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
return;
|
||
}
|
||
|
||
if(X_ads1220 >= (motor_end)) //达到电机终点限位后停止
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
rt3_stop_cnt++;
|
||
if(rt3_stop_cnt > 10000)
|
||
{
|
||
rt3_stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) rt3_stop_cnt = 0;
|
||
}
|
||
|
||
motor_direc = MOTOR_REV;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
return;
|
||
}
|
||
|
||
if((ocin2 == 1) && (ocin2 == 1) && (X_ads1220 != 0)) //上电初期Xads值为零
|
||
{
|
||
if( speed_max != SPEED_MAX)
|
||
{
|
||
speed_max = SPEED_MAX;
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); // VMAX
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
|
||
tmc5160_operate(MOTOR_POS,51200*30);
|
||
}
|
||
}
|
||
else//数据错误
|
||
{
|
||
//SC_Init();
|
||
}
|
||
}
|
||
else //角行程——电机旋转一圈，磁条旋转4°
|
||
{
|
||
// motor_data[0] = 0x01;//00 直行程 01 角行程
|
||
// //判断电机停止还是运行，运行到起始位还是结束位
|
||
// if( Motor_Run == 0)//停止
|
||
// {
|
||
// tmc5160_operate(0,0);
|
||
// }
|
||
// else if(Motor_Run == 1)//运行
|
||
// {
|
||
// if(Motor_Run >= 1 && Runmotor_step == 0)
|
||
// {
|
||
// Runmotor_step = 1;
|
||
// }
|
||
//
|
||
// //判断电机运行方式是点动还是连续，点动为方案三，方案一和方案二为连续
|
||
// if(Run_Mode == 0)//点动（方案三）
|
||
// {
|
||
// motor_data[2] = 0x00;//发送时反馈的数据
|
||
// mov_step_ang();//电机点动运行
|
||
// }
|
||
// else if(Run_Mode == 1)//连续（方案一）
|
||
// {
|
||
// motor_data[2] = 0x01;//发送时反馈的数据
|
||
// mov_loop1_ang();//磁条旋转一圈
|
||
// }
|
||
// else if(Run_Mode == 2)//连续（方案二）
|
||
// {
|
||
// motor_data[2] = 0x02;//发送时反馈的数据
|
||
// if(seat_flag == 0)
|
||
// {
|
||
// if(GPIO_ReadPin(GPIO1,GPIO_PIN_4) == 0) //判断是否到达限位处
|
||
// {
|
||
// motor_stop(); //电机停止
|
||
// Run_mm = 0;
|
||
// seat_flag = 1;//位置标记
|
||
// }
|
||
// else
|
||
// {
|
||
// REV(); //反转
|
||
// motor_start(); //运行
|
||
// motor_mov(1); //提供脉冲信号
|
||
// }
|
||
// }
|
||
// else
|
||
// {
|
||
// mov_loop2_ang();//磁条旋转一圈
|
||
// }
|
||
// }
|
||
// else//初始化
|
||
// {
|
||
// motor_stop();
|
||
// }
|
||
// }
|
||
// else if(Motor_Run == 2 || Motor_Run == 3)//运行到起始位
|
||
// {
|
||
// mov_begin();//回到起始位
|
||
// }
|
||
// else//数据错误
|
||
// SC_Init();
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//电机点动运行，方案三（直行程）
|
||
void mov_step(void)
|
||
{
|
||
if( (ocin1 == 0) && (motor_direc != MOTOR_POS) )//判断是否到达始限位，考虑到再次启动的问题，判断条件增加了方向
|
||
{
|
||
motor_direc = MOTOR_POS; //到达限位后反向
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0; //跳转至静止模式
|
||
return;
|
||
}
|
||
if( (ocin2 == 0) && (motor_direc != MOTOR_REV) )//判断是否到达终限位
|
||
{
|
||
motor_direc = MOTOR_REV; //到达限位后反向
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0; //跳转至静止模式
|
||
return;
|
||
}
|
||
|
||
switch(Runmotor_step)
|
||
{
|
||
case 1 : //电机运行准备
|
||
{
|
||
|
||
Runmotor_Nums = 0; //运行次数
|
||
mov_flag = 0;
|
||
Runmotor_step++;
|
||
}
|
||
break;
|
||
case 2 : //电机运行过程
|
||
{
|
||
if(Run_Step == 0) //上位机无消息时使用下位机的控制参数Run_mm
|
||
{
|
||
tmc5160_operate(motor_direc,Run_mm * 10240); //1 Run_mm = 1 mm
|
||
|
||
}else
|
||
{
|
||
tmc5160_operate(motor_direc,Run_Step * 1024); //1 Runstep = 10 mm
|
||
}
|
||
if(busy_flag == 0) //电机到达目标位置后busy_flag == 0
|
||
{
|
||
Runmotor_step++;
|
||
}
|
||
}
|
||
break;
|
||
case 3 :
|
||
{
|
||
Runmotor_step++;
|
||
}
|
||
break;
|
||
case 4 :
|
||
{
|
||
if(it_50ms_flag) //检查50ms定时标志
|
||
{
|
||
it_50ms_flag = 0; //定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= 10) //时间
|
||
{
|
||
Runmotor_Nums = 0;
|
||
Runmotor_step++;
|
||
|
||
get_state(); //读取电机当前状态
|
||
send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data); //数据发送
|
||
}
|
||
}
|
||
break;
|
||
case 5 :
|
||
{
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0; //运行标记清除，等待下一次上位机发送命令
|
||
}
|
||
break;
|
||
default :
|
||
{
|
||
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
//电机连续运行，方案一（直行程）
|
||
int stop_cnt = 0;
|
||
signed int motor_x_start = 0,motor_x_end = 0;
|
||
uint8_t loop_flag = 0; //阶段标志，0阶段：接近磁条，1阶段：磁条范围内正向步进，2阶段：磁条范围内反向步进，3阶段：离开磁条返回并复位。
|
||
void mov_loop1(void)
|
||
{
|
||
switch(Runmotor_step)
|
||
{
|
||
case 1 : //电机运行准备
|
||
{
|
||
|
||
// motor_direc = MOTOR_POS; //由上位机控制
|
||
Run_num = 0;
|
||
motor_data[2] = 0x00; //发送时反馈的数据
|
||
Runmotor_Nums = 0; //运行次数
|
||
mov_flag = 0;
|
||
busy_flag = 0;
|
||
Runmotor_step++;
|
||
|
||
}
|
||
break;
|
||
case 2 : //电机运行过程，运行至磁条所在范围
|
||
{
|
||
if( X_ads1220 >= magnet_start ) //第一次到达磁条范围的起点,以电阻尺为准
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
stop_cnt++;
|
||
if(stop_cnt > 10000)
|
||
{
|
||
stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) stop_cnt = 0;
|
||
}
|
||
|
||
loop_flag = 1; //0阶段结束，进入1阶段
|
||
|
||
TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //停止后读取实际位置XACTUAL
|
||
XA_32 = Raw_32(XA);
|
||
|
||
motor_x_start = XA_32; //记录起点时的电机位置
|
||
|
||
switch(magnet_type) //根据不同规格，计算电机运动的终点
|
||
{
|
||
case 25:
|
||
{
|
||
motor_x_end = motor_x_start + 10240*(25+6);
|
||
}
|
||
break;
|
||
case 50:
|
||
{
|
||
motor_x_end = motor_x_start + 10240*(50+6);
|
||
}
|
||
break;
|
||
case 110:
|
||
{
|
||
motor_x_end = motor_x_start + 10240*(110+6);
|
||
}
|
||
break;
|
||
case 210:
|
||
{
|
||
motor_x_end = motor_x_start + 10240*(210+5);
|
||
}
|
||
break;
|
||
default:
|
||
{
|
||
motor_x_end = 0;
|
||
}
|
||
break;
|
||
}
|
||
|
||
if(it_1000ms_flag) //检查1000ms定时标志
|
||
{
|
||
it_1000ms_flag = 0; //定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= Run_Stop) //时间
|
||
{
|
||
Runmotor_step++;
|
||
Runmotor_Nums = 0;
|
||
get_state(); //读取电机当前状态
|
||
send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data); //数据发送
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(send_flag == 0)
|
||
{
|
||
send_flag = 1;
|
||
// get_state(); //读取电机当前状态
|
||
// send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data);//数据发送
|
||
}
|
||
|
||
if( X_ads1220 <= magnet_start - 150) //与磁条起点距离超过150时，快速运动，以电阻尺读数为准
|
||
{
|
||
if( speed_max != SPEED_MAX)
|
||
{
|
||
speed_max = SPEED_MAX;
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1); //VMAX
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
}
|
||
else //与磁条起点距离小于等于150时，减速至最小速度
|
||
{
|
||
if( speed_max != SPEED_MIN)
|
||
{
|
||
speed_max = SPEED_MIN; //VMAX
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
}
|
||
|
||
tmc5160_operate( motor_direc, 51200*30 ); //以150mm的大步长循环运动
|
||
|
||
Runmotor_step = 6; //在case 2 - 6 之间循环，2运动，6限位
|
||
}
|
||
}
|
||
break;
|
||
case 3 :
|
||
{
|
||
//进入磁条范围后，以小步长进行运动，单位为mm
|
||
if( speed_max != SPEED_NORMAL) //速度降低至常规速度
|
||
{
|
||
speed_max = SPEED_NORMAL; //VMAX
|
||
TMC5160_SPIWriteInt(VMAX_ADDR, speed_max,1);
|
||
|
||
speed_1 = speed_max / 2; //V1
|
||
TMC5160_SPIWriteInt(V1_ADDR, speed_1,1);
|
||
}
|
||
|
||
if(loop_flag == 1) motor_direc = MOTOR_POS;
|
||
if(loop_flag == 2) motor_direc = MOTOR_REV;
|
||
|
||
if(Run_Step == 0)
|
||
{
|
||
tmc5160_operate(motor_direc,Run_mm*10240); //下位机固定为1mm步长，OLED2.c→OLED_MenuTest()→case5
|
||
}else
|
||
{
|
||
tmc5160_operate(motor_direc,Run_Step*1024); //上位机给定步长
|
||
}
|
||
if(busy_flag == 0) //到达指定位置后释放busy_flag
|
||
{
|
||
Runmotor_step++;
|
||
}
|
||
}
|
||
break;
|
||
case 4 : //延时
|
||
{
|
||
if(it_50ms_flag) //检查50ms定时标志
|
||
{
|
||
it_50ms_flag = 0; //定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if(Runmotor_Nums >= (Run_Inter / 50)) //时间
|
||
{
|
||
|
||
Runmotor_Nums = 0;
|
||
Runmotor_step++;
|
||
send_flag = 0;
|
||
|
||
get_state(); //读取电机当前位置
|
||
send_set_resp(0xF001, OBJ_DEVICE_ADDR, DATA_LEN, motor_data); //数据发送
|
||
}
|
||
|
||
}
|
||
break;
|
||
case 5 :
|
||
{
|
||
|
||
if( (ocin1 == 0) && (motor_direc != MOTOR_POS) ) //判断是否到达始限位
|
||
{
|
||
motor_direc = MOTOR_POS;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
break;
|
||
}
|
||
if( (ocin2 == 0) && (motor_direc != MOTOR_REV) ) //判断是否到达终限位
|
||
{
|
||
motor_direc = MOTOR_REV;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
break;
|
||
}
|
||
|
||
Runmotor_step = 3; //超出磁条范围之前，在3-4-5之间循环
|
||
|
||
TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //读取实际位置XACTUAL
|
||
XA_32 = Raw_32(XA);
|
||
|
||
if(motor_x_end != 0) //未出现异常时以电机为准
|
||
{
|
||
if((XA_32 >= motor_x_end) && (motor_direc != MOTOR_REV)) //到达磁条范围终点后，停止并反向
|
||
{
|
||
loop_flag = 2; //1阶段结束，进入2阶段
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
stop_cnt++;
|
||
if(stop_cnt > 10000)
|
||
{
|
||
stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) stop_cnt = 0;
|
||
}
|
||
|
||
if(busy_flag == 0)
|
||
{
|
||
motor_direc = MOTOR_REV; //停止后反向
|
||
Runmotor_Nums = 0;
|
||
Runmotor_step = 7;
|
||
}
|
||
}
|
||
}else //出现异常时以电阻尺为准
|
||
{
|
||
if((X_ads1220 >= magnet_end) && (motor_direc != MOTOR_REV)) //到达磁条范围终点后，停止并反向
|
||
{
|
||
loop_flag = 2; //1阶段结束，进入2阶段
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
stop_cnt++;
|
||
if(stop_cnt > 10000)
|
||
{
|
||
stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) stop_cnt = 0;
|
||
}
|
||
|
||
if(busy_flag == 0)
|
||
{
|
||
motor_direc = MOTOR_REV; //停止后反转
|
||
Runmotor_Nums = 0;
|
||
Runmotor_step = 7;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
if((XA_32 <= motor_x_start) && (motor_direc == MOTOR_REV)) //反向回到磁条范围起点后，复位
|
||
{
|
||
loop_flag = 3; //2阶段结束，进入3阶段
|
||
tmc5160_operate(MOTOR_STOP,0); //立即停止
|
||
while(busy_flag == 1) //加速停止动作
|
||
{
|
||
tmc5160_operate(MOTOR_STOP,0);
|
||
|
||
stop_cnt++;
|
||
if(stop_cnt > 10000)
|
||
{
|
||
stop_cnt = 0;
|
||
break; //防止卡死
|
||
}
|
||
|
||
if(busy_flag == 0) stop_cnt = 0;
|
||
}
|
||
|
||
Motor_Run = 2; //跳转至复位模式
|
||
Runmotor_step = 0;
|
||
}
|
||
|
||
|
||
}
|
||
break;
|
||
case 6 :
|
||
{
|
||
if( (ocin1 == 0) && (motor_direc != MOTOR_POS) ) //判断是否到达始限位
|
||
{
|
||
motor_direc = MOTOR_POS;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
break;
|
||
}
|
||
if( (ocin2 == 0) && (motor_direc != MOTOR_REV) ) //判断是否到达终限位
|
||
{
|
||
motor_direc = MOTOR_REV;
|
||
Runmotor_step = 0;
|
||
Motor_Run = 0;
|
||
break;
|
||
}
|
||
|
||
Runmotor_step = 2; //在case 2 - 6 之间循环，2运动，6限位
|
||
}
|
||
break;
|
||
case 7 :
|
||
{
|
||
if(it_10ms_flag) //检查1ms定时标志
|
||
{
|
||
it_10ms_flag = 0; //定时标志清零
|
||
Runmotor_Nums++;
|
||
}
|
||
if((Runmotor_Nums/100) >= Run_Stop) //时间
|
||
{
|
||
Runmotor_step = 3;
|
||
Runmotor_Nums = 0;
|
||
}
|
||
}
|
||
break;
|
||
default :
|
||
{
|
||
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
uint32_t V_data = 0;
|
||
signed int _data = 0,vol_data = 0; //用于OLED显示
|
||
char start_check = 0; //用于定位起始帧的下标
|
||
uint16_t send_xa = 0; //用于发送当前位置
|
||
void get_state(void)
|
||
{
|
||
motor_data[1] = Motor_Run;
|
||
motor_data[2] = Run_Mode;
|
||
|
||
if(rx_data2[start_check] != 0x05)
|
||
{
|
||
start_check = (start_check + 1)*(start_check <5); //定位起始帧
|
||
}
|
||
else
|
||
{
|
||
motor_data[4] = rx_data2[ (start_check + 1) - 6*( (start_check + 1) > 5) ]; //根据起始帧所在位置，记录磁感应模块的反馈值
|
||
motor_data[5] = rx_data2[ (start_check + 2) - 6*( (start_check + 2) > 5) ];
|
||
motor_data[6] = rx_data2[ (start_check + 3) - 6*( (start_check + 3) > 5) ];
|
||
motor_data[7] = rx_data2[ (start_check + 4) - 6*( (start_check + 4) > 5) ];
|
||
}
|
||
|
||
|
||
/*电阻尺读数*/
|
||
// motor_data[8] = ((uint16_t)(X_ads1220*10) & 0xff00)>> 8; //磁条长度高8位
|
||
// motor_data[9] = (uint16_t)(X_ads1220*10) & 0x00ff; //磁条长度低8位
|
||
|
||
/*电机内部编码*/
|
||
TMC5160_SPIReadInt(XACTUAL_ADDR,XA); //读取实际位置XACTUAL
|
||
XA_32 = Raw_32(XA);
|
||
send_xa = ((XA_32 - first_xa_32) * 100) / 10240;
|
||
motor_data[8] = ((uint16_t)send_xa & 0xff00)>> 8; //磁条长度高8位
|
||
motor_data[9] = ((uint16_t)send_xa & 0x00ff); //磁条长度低8位
|
||
|
||
if( ocin1 == 0 )//判断是否到达始限位
|
||
motor_data[3] = 1;
|
||
else if(ocin2 == 0)
|
||
motor_data[3] = 2;
|
||
else
|
||
motor_data[3] = motor_direc + 2;
|
||
}
|
||
|
||
void get_magnetv(void)
|
||
{
|
||
V_data = motor_data[4]*0xffffff + motor_data[5]*0xffff + motor_data[6]*0xff + motor_data[7];
|
||
//ADS数据转换为实际电压值
|
||
if(V_data & 0x00800000)
|
||
{
|
||
_data = 0xffffffff - V_data;
|
||
vol_data = (-2.5 / 0x7fffff) * _data*1000;
|
||
}
|
||
else
|
||
{
|
||
_data = V_data;
|
||
vol_data = (2.5 / 0x7fffff) * _data*1000;
|
||
}
|
||
}
|