[release] 发布8DOF串联足中期检查版本
This commit is contained in:
@@ -0,0 +1,236 @@
|
||||
#include "main.h"
|
||||
#include "can.h"
|
||||
#include "app_can.h"
|
||||
motor_measure_t m3508[8];
|
||||
motor_measure_t m2006[8];
|
||||
static CAN_TxHeaderTypeDef can1_tx_message;
|
||||
static uint8_t can1_send_data[8];
|
||||
static CAN_TxHeaderTypeDef can2_tx_message;
|
||||
static uint8_t can2_send_data[8];
|
||||
|
||||
extern CAN_HandleTypeDef hcan1;
|
||||
extern CAN_HandleTypeDef hcan2;
|
||||
void can_filter_init(void)
|
||||
{
|
||||
|
||||
CAN_FilterTypeDef can_filter_st;
|
||||
can_filter_st.FilterActivation = ENABLE;
|
||||
can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
|
||||
can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
|
||||
can_filter_st.FilterIdHigh = 0x0000;
|
||||
can_filter_st.FilterIdLow = 0x0000;
|
||||
can_filter_st.FilterMaskIdHigh = 0x0000;
|
||||
can_filter_st.FilterMaskIdLow = 0x0000;
|
||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||
|
||||
can_filter_st.FilterBank = 0;
|
||||
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
|
||||
HAL_CAN_Start(&hcan1);
|
||||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||||
|
||||
can_filter_st.SlaveStartFilterBank = 14;
|
||||
can_filter_st.FilterBank = 14;
|
||||
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
||||
HAL_CAN_Start(&hcan2);
|
||||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||||
}
|
||||
/*******************************************************************************************
|
||||
* @Func void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
|
||||
* @Brief 接收3508电机通过CAN发过来的信息
|
||||
* @Param
|
||||
* @Retval None
|
||||
* @Date 2015/11/24
|
||||
*******************************************************************************************/
|
||||
void get_moto_measure(motor_measure_t *ptr, uint8_t Data[])
|
||||
{
|
||||
ptr->last_angle = ptr->angle;
|
||||
ptr->angle = (uint16_t)(Data[0] << 8 | Data[1]) ;
|
||||
ptr->speed = (int16_t)(Data[2] << 8 | Data[3]);
|
||||
ptr->troque = (Data[4] << 8 | Data[5]);
|
||||
ptr->temperate = Data[6];
|
||||
|
||||
if(ptr->angle - ptr->last_angle > 4096)
|
||||
ptr->round_cnt --;
|
||||
else if (ptr->angle - ptr->last_angle < -4096)
|
||||
ptr->round_cnt ++;
|
||||
|
||||
ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
|
||||
ptr->real_angle = ptr->total_angle / 22.75555556f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief hal CAN fifo call back, receive motor data
|
||||
* @param[in] hcan, the point to CAN handle
|
||||
* @retval none
|
||||
*/
|
||||
/**
|
||||
* @brief hal库CAN回调函数,接收电机数据
|
||||
* @param[in] hcan:CAN句柄指针
|
||||
* @retval none
|
||||
*/
|
||||
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
|
||||
{
|
||||
CAN_RxHeaderTypeDef rx1_header;
|
||||
CAN_RxHeaderTypeDef rx2_header;
|
||||
uint8_t rx1_data[8];
|
||||
uint8_t rx2_data[8];
|
||||
/* if(hcan==(&hcan1))另一种方式*/
|
||||
if(hcan->Instance == CAN1)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &rx1_header, rx1_data);
|
||||
switch (rx1_header.StdId)//3508接收
|
||||
{
|
||||
case 0x201:
|
||||
case 0x202:
|
||||
case 0x203:
|
||||
case 0x204:
|
||||
case 0x205:
|
||||
case 0x206:
|
||||
case 0x207:
|
||||
case 0x208:
|
||||
{
|
||||
static u8 i = 0;
|
||||
static u8 offset_sign[8] = {0}; //八个电机上电角度标志位
|
||||
//get motor id
|
||||
i = rx1_header.StdId - 0x201;
|
||||
get_moto_measure(&m3508[i], rx1_data);//获取电机信息
|
||||
|
||||
if(offset_sign[i] == 0) //只记一次上电角度
|
||||
{
|
||||
get_moto_offset(&m3508[i]);//获取上电角度
|
||||
offset_sign[i]++;
|
||||
}
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(hcan->Instance == CAN2)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO0, &rx2_header, rx2_data);
|
||||
switch (rx2_header.StdId)
|
||||
{
|
||||
case 0x201:
|
||||
case 0x202:
|
||||
case 0x203:
|
||||
case 0x204:
|
||||
case 0x205:
|
||||
case 0x206:
|
||||
case 0x207:
|
||||
case 0x208:
|
||||
{
|
||||
static u8 i = 0;
|
||||
static u8 offset_sign[8] = {0}; //八个电机上电角度标志位
|
||||
//get motor id
|
||||
i = rx2_header.StdId - 0x201;
|
||||
get_moto_measure(&m2006[i],rx2_data);//获取电机信息
|
||||
|
||||
if(offset_sign[i] == 0) //只记一次上电角度
|
||||
{
|
||||
get_moto_offset(&m2006[i]);//获取上电角度
|
||||
offset_sign[i]++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
|
||||
* @param[in] motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
|
||||
* @retval none
|
||||
*/
|
||||
void can1_cmd1(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
can1_tx_message.StdId = 0x200;
|
||||
can1_tx_message.IDE = CAN_ID_STD;
|
||||
can1_tx_message.RTR = CAN_RTR_DATA;
|
||||
can1_tx_message.DLC = 0x08;
|
||||
can1_send_data[0] = motor1 >> 8;
|
||||
can1_send_data[1] = motor1;
|
||||
can1_send_data[2] = motor2 >> 8;
|
||||
can1_send_data[3] = motor2;
|
||||
can1_send_data[4] = motor3 >> 8;
|
||||
can1_send_data[5] = motor3;
|
||||
can1_send_data[6] = motor4 >> 8;
|
||||
can1_send_data[7] = motor4;
|
||||
|
||||
HAL_CAN_AddTxMessage(&hcan1, &can1_tx_message, can1_send_data, &send_mail_box);
|
||||
}
|
||||
void can1_cmd2(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
can1_tx_message.StdId = 0x1FF;
|
||||
can1_tx_message.IDE = CAN_ID_STD;
|
||||
can1_tx_message.RTR = CAN_RTR_DATA;
|
||||
can1_tx_message.DLC = 0x08;
|
||||
can1_send_data[0] = motor1 >> 8;
|
||||
can1_send_data[1] = motor1;
|
||||
can1_send_data[2] = motor2 >> 8;
|
||||
can1_send_data[3] = motor2;
|
||||
can1_send_data[4] = motor3 >> 8;
|
||||
can1_send_data[5] = motor3;
|
||||
can1_send_data[6] = motor4 >> 8;
|
||||
can1_send_data[7] = motor4;
|
||||
|
||||
HAL_CAN_AddTxMessage(&hcan1, &can1_tx_message, can1_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
|
||||
* @param[in] motor1: (0x201) 2006电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor2: (0x202) 2006电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor3: (0x203) 2006电机控制电流, 范围 [-16384,16384]
|
||||
* @param[in] motor4: (0x204) 2006电机控制电流, 范围 [-16384,16384]
|
||||
* @retval none
|
||||
*/
|
||||
void can2_cmd1(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
can2_tx_message.StdId = 0x200;
|
||||
can2_tx_message.IDE = CAN_ID_STD;
|
||||
can2_tx_message.RTR = CAN_RTR_DATA;
|
||||
can2_tx_message.DLC = 0x08;
|
||||
can2_send_data[0] = (motor1 >> 8);
|
||||
can2_send_data[1] = motor1;
|
||||
can2_send_data[2] = (motor2 >> 8);
|
||||
can2_send_data[3] = motor2;
|
||||
can2_send_data[4] = (motor3 >> 8);
|
||||
can2_send_data[5] = motor3;
|
||||
can2_send_data[6] = (motor4 >> 8);
|
||||
can2_send_data[7] = motor4;
|
||||
HAL_CAN_AddTxMessage(&hcan2, &can2_tx_message, can2_send_data, &send_mail_box);
|
||||
}
|
||||
void can2_cmd2(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
can2_tx_message.StdId = 0x1FF;
|
||||
can2_tx_message.IDE = CAN_ID_STD;
|
||||
can2_tx_message.RTR = CAN_RTR_DATA;
|
||||
can2_tx_message.DLC = 0x08;
|
||||
can2_send_data[0] = (motor1 >> 8);
|
||||
can2_send_data[1] = motor1;
|
||||
can2_send_data[2] = (motor2 >> 8);
|
||||
can2_send_data[3] = motor2;
|
||||
can2_send_data[4] = (motor3 >> 8);
|
||||
can2_send_data[5] = motor3;
|
||||
can2_send_data[6] = (motor4 >> 8);
|
||||
can2_send_data[7] = motor4;
|
||||
HAL_CAN_AddTxMessage(&hcan2, &can2_tx_message, can2_send_data, &send_mail_box);
|
||||
}
|
||||
/*this function should be called after system+can init */
|
||||
void get_moto_offset(motor_measure_t *ptr)
|
||||
{
|
||||
ptr->offset_angle = ptr->angle;
|
||||
ptr->round_cnt = 0;//防止角度差8192即一圈
|
||||
}
|
||||
Reference in New Issue
Block a user