rgv-kinco/applications/kinco.c

/*
 * @Description: 
 该协议一问一答上传，问在task_can中进行
对外3个接口：
数据解析，存在结构体
对外提供结构体查询
在线计时
底层 处理完毕
先给工作模式，再给控制字86 ，再给F，再给转速
 * @version: 
 * @Author: Joe
 * @Date: 2021-11-13 21:55:10
 * @LastEditTime: 2021-11-13 21:55:11
 */

#include "kinco.h"
#include "chassis_bus.h"

#define DBG_TAG                        "kinco"
#define DBG_LVL                        DBG_INFO
#include <rtdbg.h>


static MOTOR_TypeDef   motor = {0};


#if defined SEND_ADCC_EN
static uint16_t ACC_TIME = 500;
static uint16_t DCC_TIME = 500;
uint8_t get_motor_acc(void)
{	
	return	motor.acc;
}
/****************************************
 *        设置加减速    
 *函数功能 : 
 *参数描述 : param = 0x00,0xb8：加速度
			 param = 0x00,0xb9：减速度	
			 value：时间，单位ms
//00 24 00 b9 03 e8 00 00默认1000
 *返回值   : 返回发送的can结构体
 ****************************************/
struct rt_can_msg send_motor_acc(void)   
{	
	struct rt_can_msg tx_msg;
	tx_msg.id = motor.id+0x100;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 8;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x00;		   /* 源地址 */
	tx_msg.data[1] = 0x24;		   /* 功能码 */
	tx_msg.data[2] = 0x00;     /* 寄存器地址 */	
	tx_msg.data[3] = 0xb8;	       /* 寄存器地址 */	
	tx_msg.data[4] = ACC_TIME>>8;	   /* 时间值 */
	tx_msg.data[5] = ACC_TIME;		   /* 时间值 */
	return	tx_msg;
}

uint8_t get_motor_dcc(void)
{	
	return	motor.dcc;
}
/****************************************
 *        设置加减速    
 *函数功能 : 
 *参数描述 : param = 0x00,0xb8：加速度
			 param = 0x00,0xb9：减速度	
			 value：时间，单位ms
//00 24 00 b9 03 e8 00 00默认1000
 *返回值   : 返回发送的can结构体
 ****************************************/
struct rt_can_msg send_motor_dcc(void)   
{	
	struct rt_can_msg tx_msg;
	tx_msg.id = motor.id+0x100;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 8;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x00;		   /* 源地址 */
	tx_msg.data[1] = 0x24;		   /* 功能码 */
	tx_msg.data[2] = 0x00;     	   /* 寄存器地址 */	
	tx_msg.data[3] = 0xb9;	       /* 寄存器地址 */	
	tx_msg.data[4] = DCC_TIME>>8;	   	   /* 时间值 */
	tx_msg.data[5] = DCC_TIME;		   /* 时间值 */
	return	tx_msg;
}
#endif /* SEND_ADCC_EN */ 


#if defined SEND_MODE_EN
/****************************************
 *        设置 位置/速度 模式   
 *函数功能 : 
 *参数描述 : 	
	value：0x00,0x06:位置模式
	0x00,0x02：速度模式
	//00 1e 00 03 00 06 00 00
	//00 1e 00 03 00 02 00 00
 *返回值   : 返回发送的can结构体
 ****************************************/
uint8_t get_motor_mode(void)
{	
	return	motor.mode;
}
struct rt_can_msg send_motor_speed_mode(void)   
{	
	struct rt_can_msg tx_msg;
	tx_msg.id = motor.id + 0x600;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 6;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x2F;		   /* 源地址 */
	tx_msg.data[1] = 0x60;		   /* 功能码 */
	tx_msg.data[2] = 0x60;     	   /* 寄存器地址 */	
	tx_msg.data[3] = 0x00;	       /* 寄存器地址 */	
	tx_msg.data[4] = 0x03;	   /* 值 */
	tx_msg.data[5] = 0x00;		   /* 值 */
	return	tx_msg;
}

struct rt_can_msg send_motor_control_mode(void)   
{	
	struct rt_can_msg tx_msg;
	tx_msg.id = motor.id + 0x600;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 6;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x2B;		   /* 源地址 */
	tx_msg.data[1] = 0x40;		   /* 功能码 */
	tx_msg.data[2] = 0x60;     	   /* 寄存器地址 */	
	tx_msg.data[3] = 0x00;	       /* 寄存器地址 */	
	tx_msg.data[4] = 0x0F;	   /* 值 */
	tx_msg.data[5] = 0x00;		   /* 值 */
	return	tx_msg;
}




#endif /* SEND_MODE_EN */ 


#if defined SEND_RPM_EN
/****************************************
 *        设置转速   
 *函数功能 : 
 *参数描述 :
	value：转速值，带符号，最大3000转速
	//00 28 00 00 00 64 00 00
 *返回值   : 返回发送的can结构体
 ****************************************/
void set_motor_rpm(int16_t rpm)
{
	 motor.rpm = rpm;		
}

struct rt_can_msg send_motor_rpm(void)   
{	
	struct rt_can_msg tx_msg;
	int32_t dec = 0;
	dec = motor.rpm*2730;	//编码器的值
	tx_msg.id = motor.id+0x600;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 8;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x23;		   /* 发送命令 */
	tx_msg.data[1] = 0xFF;		   /* 对象索引 */
	tx_msg.data[2] = 0x60;     	   /* 对象索引 */	
	tx_msg.data[3] = 0x00;	       /* 对象子索引 */	
	tx_msg.data[4] = dec;	   /* 值 */
	tx_msg.data[5] = dec>>8;	   /* 值 */
	tx_msg.data[6] = dec>>16;	   /* 值 */
	tx_msg.data[7] = dec>>24;		   /* 值 */
	return	tx_msg;
}
#endif /* SEND_RPM_EN */ 

uint8_t motor_init_ok(void)
{
	struct rt_can_msg msg;	
	if(motor.mode != 3)
	{
		msg = send_motor_speed_mode();
		chassis_bus_write(&msg, sizeof(msg));
		return 1;
	}
	else
	{
		if(motor.control != 0X0F)
		{
			msg = send_motor_control_mode();
			chassis_bus_write(&msg, sizeof(msg));
			return 1;
		}
		else
		return 0;		
	}
}



uint8_t motor_msg_parse(struct rt_can_msg *msg)
{
    uint8_t temp = 1;
	uint16_t svd;
	if(msg->ide!=RT_CAN_STDID)
	return temp;
	if(msg->id == motor.id + 0x580)	/* 定时上传 */
	{
		temp = 0;
		svd = (msg->data[1]<<8) + msg->data[2];
		switch(svd)/* 功能码 */
		{
			case 0x6060:	//工作模式
				motor.mode = msg->data[4];	//
			break;
            case 0x4060: //控制字
				motor.control = msg->data[4];	//
			break;
			default:
			break;
		}		
	}
//	else
//	if(msg->id == 0x0700)	/*即发即回:主站发送，从站回复，以0x700为id,功能码+1，进行状态和参数信息交换*/
//	{
//		temp = 0;
//		if(msg->data[0] == motor.id) /* 源地址 */
//		{
//			if(msg->data[1] ==0x1F)/* 功能码 */
//			{
//				if(msg->data[2]==0x00 && msg.data[3]==0x03)   /* 速度模式地址 */
//				{	
//					if(msg.data[5] == mode_pos)
//					motor.mode = mode_pos;
//					else
//					if(msg.data[5] == mode_speed)
//					motor.mode = mode_speed;
//				}		
//			}
//			else
//			if(msg.data[1] ==0x25)/* 功能码 */
//			{
//				if(msg.data[2]==0x00 && msg.data[3]==0xb8)   /* 加速度地址 */
//				{	
//					motor.acc = 1;
//				}
//				else
//				if(msg.data[2]==0x00 && msg.data[3]==0xb9)   /* 减速度地址 */
//				{	
//					motor.dcc = 1;
//				}
//			}
//		}	
//	}
	//数据解析 
    return temp;    	
}
uint8_t get_motor_err(void)
{
	return motor.err;
}
struct rt_can_msg send_motor_reset(void)   
{	
	struct rt_can_msg tx_msg;
	tx_msg.id = motor.id+0x100;
	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
	tx_msg.len = 8;                /* 数据长度为 8 */
	tx_msg.data[0] = 0x00;		   /* 源地址 */
	tx_msg.data[1] = 0x21;		   /* 功能码 */
	tx_msg.data[2] = 0x00;     	   /* 寄存器地址 */	
	tx_msg.data[3] = 0x11;	       /* 寄存器地址 */	
	tx_msg.data[4] = 0x00;	   		/* 值 */
	tx_msg.data[5] = 0x01;		   /* 值 */
	tx_msg.data[6] = 0x00;		   
	tx_msg.data[7] = 0x01;		  
	return	tx_msg;
}

static void motor_param_init(void)
{  
	motor.mode = 0;
	motor.id = 0x7a;   
    motor.err = 0;
    motor.rpm = 0;
}

/****************************************
 *        motor_init
*函数功能 : 配置初始化
 *参数描述 : 无
 *返回值   : 无
 ****************************************/
int motor_init(void)
{
    
    motor_param_init();	
    return RT_EOK;
}
INIT_APP_EXPORT(motor_init);