视觉测试可以
This commit is contained in:
parent
14cb35351f
commit
cf66cc255f
@ -18,18 +18,18 @@ void can_filter_init(void)
|
||||
can_filter_st.FilterMaskIdHigh = 0x0000;
|
||||
can_filter_st.FilterMaskIdLow = 0x0000;
|
||||
can_filter_st.FilterBank = 0;
|
||||
can_filter_st.SlaveStartFilterBank = 14;
|
||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||
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;
|
||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
|
||||
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
||||
HAL_CAN_Start(&hcan2);
|
||||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -93,6 +93,8 @@ void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float
|
||||
return;
|
||||
}
|
||||
|
||||
Pos = Pos * (3.14159f / 180.0f); // 角度转弧度
|
||||
|
||||
// assign motor target goal to the buffer
|
||||
motor->motor_send_data.head[0]=0xFE;
|
||||
motor->motor_send_data.head[1]=0xEE;
|
||||
@ -160,6 +162,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
|
||||
int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
|
||||
motor->Pos = Pos;
|
||||
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
|
||||
motor->Pos = motor->Pos * (180.0f / 3.14159f); // 弧度转角度
|
||||
int8_t Temp = Temp_buffer[11] & 0xFF;
|
||||
motor->Temp = Temp;
|
||||
motor->MError = Temp_buffer[12] & 0x7;
|
||||
|
||||
@ -22,6 +22,7 @@
|
||||
#include "TopDefine.h"
|
||||
#include<cmsis_os2.h>
|
||||
#include "FreeRTOS.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "odrive_can.h"
|
||||
|
||||
@ -78,7 +79,12 @@
|
||||
|
||||
|
||||
#if DEBUG == 1
|
||||
|
||||
//电机回传数据结构体
|
||||
motor_measure_t motor_chassis[5];
|
||||
motor_measure_t GM6020_motor[2];
|
||||
motor_measure_t motor_2006[2];
|
||||
CAN_MotorFeedback_t motor_5065[2];
|
||||
#else
|
||||
static motor_measure_t motor_chassis[5];
|
||||
#endif
|
||||
@ -92,6 +98,41 @@ static uint8_t can_send_data_200[8];
|
||||
CAN_RxHeaderTypeDef dji_rx_header;
|
||||
uint8_t dji_rx_data[8];
|
||||
|
||||
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||||
static uint8_t vesc_send_data[4];
|
||||
|
||||
|
||||
/**
|
||||
* @brief 解析vesc电机反馈数据
|
||||
* @param[in,out] feedback : Vesc电机反馈数据结构体指针
|
||||
* @param[in] raw : Vesc电机反馈数据的原始数据
|
||||
* @retval none
|
||||
*/
|
||||
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
|
||||
const uint8_t *raw)
|
||||
{
|
||||
if (feedback == NULL || raw == NULL) return;
|
||||
union
|
||||
{
|
||||
int x;
|
||||
uint8_t data[4];
|
||||
}speed;
|
||||
|
||||
speed.data[0]= raw[3];
|
||||
speed.data[1]= raw[2];
|
||||
speed.data[2]= raw[1];
|
||||
speed.data[3]= raw[0];
|
||||
feedback->rotor_speed = speed.x;
|
||||
union
|
||||
{
|
||||
int16_t y;
|
||||
uint8_t dat[2];
|
||||
}current;
|
||||
current.dat[0]= raw[5];
|
||||
current.dat[1]= raw[4];
|
||||
feedback->torque_current =(fp32)current.y/10;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
* @param[in] none
|
||||
@ -101,13 +142,10 @@ void djiMotorEncode()
|
||||
{
|
||||
switch (dji_rx_header.StdId)
|
||||
{
|
||||
case 0x201:
|
||||
case 0x202:
|
||||
case 0x203:
|
||||
case 0x204:
|
||||
case 0x205:
|
||||
case 0x206:
|
||||
case 0x207:
|
||||
case M3508_1:
|
||||
case M3508_2:
|
||||
case M3508_3:
|
||||
case M3508_4:
|
||||
{
|
||||
static uint8_t i = 0;
|
||||
//get motor id
|
||||
@ -121,6 +159,36 @@ void djiMotorEncode()
|
||||
}
|
||||
break;
|
||||
}
|
||||
case M2006_1:
|
||||
case M2006_2:
|
||||
{
|
||||
static uint8_t i = 0;
|
||||
//get motor id
|
||||
i = dji_rx_header.StdId - 0x205;
|
||||
if(motor_chassis[i].msg_cnt<=50)
|
||||
{
|
||||
motor_chassis[i].msg_cnt++;
|
||||
get_motor_offset(&motor_chassis[i], dji_rx_data);
|
||||
}else{
|
||||
get_2006_motor_measure(&motor_chassis[i], dji_rx_data);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case GM6020_1:
|
||||
case GM6020_2:
|
||||
{
|
||||
static uint8_t i = 0;
|
||||
//get motor id
|
||||
i = dji_rx_header.StdId - 0x207;
|
||||
if(GM6020_motor[i].msg_cnt<=50)
|
||||
{
|
||||
GM6020_motor[i].msg_cnt++;
|
||||
get_motor_offset(&GM6020_motor[i], dji_rx_data);
|
||||
}else{
|
||||
get_6020_motor_measure(&GM6020_motor[i], dji_rx_data);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
default:
|
||||
@ -129,6 +197,26 @@ void djiMotorEncode()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void vescMotorEncode()
|
||||
{
|
||||
switch (dji_rx_header.ExtId) {
|
||||
|
||||
case CAN_VESC5065_M1_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
CAN_VescMotor_Decode_1(&motor_5065[0], dji_rx_data);
|
||||
break;
|
||||
|
||||
case CAN_VESC5065_M2_MSG1:
|
||||
// 存储消息到对应的电机结构体中
|
||||
CAN_VescMotor_Decode_1(&motor_5065[1], dji_rx_data);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
#if FREERTOS_DJI == 0
|
||||
/**
|
||||
* @brief hal库CAN回调函数,接收电机数据
|
||||
@ -232,7 +320,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
|
||||
* @brief 发送电机控制电流(0x209,0x20A,0x20B)
|
||||
* @param[in] motor1: (0x209) 电机控制电流
|
||||
* @param[in] motor2: (0x20A) 电机控制电流
|
||||
* @param[in] motor3: (0x20B) 电机控制电流
|
||||
@ -241,7 +329,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
|
||||
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
tx_meessage_2ff.StdId = 0x1FF;
|
||||
tx_meessage_2ff.StdId = 0x2FF;
|
||||
tx_meessage_2ff.IDE = CAN_ID_STD;
|
||||
tx_meessage_2ff.RTR = CAN_RTR_DATA;
|
||||
tx_meessage_2ff.DLC = 0x08;
|
||||
@ -265,4 +353,83 @@ motor_measure_t *get_motor_point(uint8_t i)
|
||||
{
|
||||
return &motor_chassis[i];
|
||||
}
|
||||
/**
|
||||
* @brief 返回GM6020电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_6020_motor_point(uint8_t i)
|
||||
{
|
||||
return &GM6020_motor[i];
|
||||
}
|
||||
/**
|
||||
* @brief 返回M2006电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_2006_motor_point(uint8_t i)
|
||||
{
|
||||
return &motor_2006[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 限制vesc电机转速
|
||||
* @param[in/out] rpm: vesce电机转速
|
||||
* @retval none
|
||||
*
|
||||
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
|
||||
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
|
||||
*/
|
||||
void assert_param_rpm(float *rpm){
|
||||
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
|
||||
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使vesc电机进入转速模式
|
||||
* @param[in] controller_id: vesc电机控制器id
|
||||
* @param[in] RPM: 电机转速
|
||||
* @retval RPM（1000-50000之间的数）
|
||||
*/
|
||||
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
|
||||
{
|
||||
uint32_t id;
|
||||
int32_t data;
|
||||
uint32_t send_mail_box;
|
||||
|
||||
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
|
||||
assert_param_rpm(&RPM);
|
||||
data = (int32_t)(RPM);
|
||||
|
||||
vesc_tx_message.ExtId = id;
|
||||
vesc_tx_message.IDE = CAN_ID_EXT;
|
||||
vesc_tx_message.RTR = CAN_RTR_DATA;
|
||||
vesc_tx_message.DLC = 0x04;
|
||||
vesc_send_data[0] = data >> 24 ;
|
||||
vesc_send_data[1] = data >> 16 ;
|
||||
vesc_send_data[2] = data >> 8 ;
|
||||
vesc_send_data[3] = data ;
|
||||
|
||||
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使vesc电机进入制动模式
|
||||
* @param[in] controller_id: vesc电机控制器id
|
||||
* @retval none
|
||||
*/
|
||||
void CAN_VESC_HEAD(uint8_t controller_id)
|
||||
{
|
||||
uint32_t id;
|
||||
uint32_t send_mail_box;
|
||||
|
||||
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
|
||||
|
||||
vesc_tx_message.ExtId = id;
|
||||
vesc_tx_message.IDE = CAN_ID_EXT;
|
||||
vesc_tx_message.RTR = CAN_RTR_DATA;
|
||||
vesc_tx_message.DLC = 0x04;
|
||||
|
||||
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
@ -16,6 +16,18 @@ typedef enum{
|
||||
M2006 = 2
|
||||
}motor_type_e;
|
||||
|
||||
enum{
|
||||
M3508_1 = 0x201,
|
||||
M3508_2 = 0x202,
|
||||
M3508_3 = 0x203,
|
||||
M3508_4 = 0x204,
|
||||
M2006_1 = 0x205,
|
||||
M2006_2 = 0x206,
|
||||
GM6020_1 = 0x207,
|
||||
GM6020_2 = 0x208,
|
||||
CAN_VESC5065_M1_MSG1 =0x90a, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
|
||||
CAN_VESC5065_M2_MSG1 =0x90b,
|
||||
};
|
||||
|
||||
//rm motor data
|
||||
typedef struct
|
||||
@ -45,13 +57,40 @@ typedef struct
|
||||
} odrive_measure_t;
|
||||
|
||||
|
||||
typedef enum {
|
||||
CAN_PACKET_SET_DUTY = 0,
|
||||
CAN_PACKET_SET_CURRENT = 1,
|
||||
CAN_PACKET_SET_CURRENT_BRAKE =2,
|
||||
CAN_PACKET_SET_RPM = 3,
|
||||
CAN_PACKET_SET_POS = 4,
|
||||
CAN_PACKET_FILL_RX_BUFFER = 5,
|
||||
CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
|
||||
CAN_PACKET_PROCESS_RX_BUFFER = 7,
|
||||
CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
|
||||
CAN_PACKET_STATUS = 9,
|
||||
CAN_PACKET_SET_CURRENT_REL = 10,
|
||||
CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
|
||||
CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
|
||||
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
|
||||
} CAN_PACKET_ID;
|
||||
//motor calc ecd to angle
|
||||
|
||||
/* 电机反馈信息 */
|
||||
typedef struct {
|
||||
float rotor_ecd;
|
||||
float rotor_speed;
|
||||
float torque_current;
|
||||
float temp;
|
||||
} CAN_MotorFeedback_t;
|
||||
|
||||
#define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0))
|
||||
#define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36)
|
||||
#define MOTOR_ECD_TO_RAD 0.000766990394f
|
||||
#define MOTOR_ECD_TO_ANGLE_6020 (360.0 / 8191.0 )
|
||||
|
||||
#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
|
||||
#define CAN_VESC_CTRL_ID_BASE (0x300)
|
||||
|
||||
#if FREERTOS_DJI == 1
|
||||
|
||||
/**
|
||||
@ -102,6 +141,19 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
|
||||
*/
|
||||
extern motor_measure_t *get_motor_point(uint8_t i);
|
||||
|
||||
/**
|
||||
* @brief 返回GM6020电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_6020_motor_point(uint8_t i);
|
||||
|
||||
/**
|
||||
* @brief 返回M2006电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_2006_motor_point(uint8_t i);
|
||||
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
@ -110,6 +162,12 @@ extern motor_measure_t *get_motor_point(uint8_t i);
|
||||
*/
|
||||
void djiMotorEncode(void);
|
||||
|
||||
void vescMotorEncode(void);
|
||||
|
||||
void CAN_VESC_RPM(uint8_t controller_id, float RPM);
|
||||
|
||||
void CAN_VESC_HEAD(uint8_t controller_id);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "nuc.h"
|
||||
#include <string.h>
|
||||
//#include "crc16.h"
|
||||
#include "crc_ccitt.h"
|
||||
|
||||
uint8_t nucbuf[32];
|
||||
|
||||
@ -32,29 +32,38 @@ int8_t NUC_Restart(void) {
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
|
||||
bool_t NUC_WaitDmaCplt(void) {
|
||||
// return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
|
||||
// SIGNAL_NUC_RAW_REDY);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int8_t NUC_RawParse(NUC_t *n) {
|
||||
if (n == NULL) return DEVICE_ERR_NULL;
|
||||
union
|
||||
{
|
||||
union {
|
||||
float x[3];
|
||||
char data[12];
|
||||
} instance; // 方便在浮点数和字符数组之间进行数据转换
|
||||
|
||||
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
|
||||
else{
|
||||
// 校验数据包头
|
||||
if (nucbuf[0] != HEAD) {
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
// 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
|
||||
uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
|
||||
uint16_t calculated_crc = do_crc_table(nucbuf, 16); // 计算前 16 字节的 CRC
|
||||
// 校验 CRC
|
||||
if (received_crc == calculated_crc) {
|
||||
|
||||
n->status_fromnuc = nucbuf[1];
|
||||
n->ctrl_status = nucbuf[2];
|
||||
switch (n->status_fromnuc)
|
||||
{
|
||||
|
||||
switch (n->status_fromnuc) {
|
||||
case VISION:
|
||||
/* 协议格式
|
||||
0xFF HEAD
|
||||
0x0X 控制帧
|
||||
0x02 控制帧
|
||||
0x01 相机帧
|
||||
x fp32
|
||||
y fp32
|
||||
@ -62,29 +71,37 @@ int8_t NUC_RawParse(NUC_t *n){
|
||||
0xFE TAIL
|
||||
*/
|
||||
if (nucbuf[15] != TAIL) goto error;
|
||||
instance.data[3] = nucbuf[6];
|
||||
instance.data[2] = nucbuf[5];
|
||||
instance.data[1] = nucbuf[4];
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->vision.x= instance.x[0]; //
|
||||
instance.data[7] = nucbuf[10];
|
||||
instance.data[6] = nucbuf[9];
|
||||
instance.data[5] = nucbuf[8];
|
||||
instance.data[4] = nucbuf[7];
|
||||
n->vision.y = instance.x[1];//
|
||||
instance.data[11] = nucbuf[14];
|
||||
instance.data[10] = nucbuf[13];
|
||||
instance.data[9] = nucbuf[12];
|
||||
instance.data[8] = nucbuf[11];
|
||||
n->vision.z = instance.x[2];//
|
||||
break;
|
||||
case PICK:
|
||||
|
||||
instance.data[3] = nucbuf[3];
|
||||
instance.data[2] = nucbuf[4];
|
||||
instance.data[1] = nucbuf[5];
|
||||
instance.data[0] = nucbuf[6];
|
||||
n->vision.x = instance.x[0];
|
||||
|
||||
instance.data[7] = nucbuf[7];
|
||||
instance.data[6] = nucbuf[8];
|
||||
instance.data[5] = nucbuf[9];
|
||||
instance.data[4] = nucbuf[10];
|
||||
n->vision.y = instance.x[1];
|
||||
|
||||
instance.data[11] = nucbuf[11];
|
||||
instance.data[10] = nucbuf[12];
|
||||
instance.data[9] = nucbuf[13];
|
||||
instance.data[8] = nucbuf[14];
|
||||
n->vision.z = instance.x[2];
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
error:
|
||||
else
|
||||
{
|
||||
return DEVICE_ERR;
|
||||
|
||||
}
|
||||
|
||||
error:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
|
||||
@ -13,7 +13,7 @@
|
||||
#define ODOM (0x04)
|
||||
#define PICK (0x06)
|
||||
|
||||
#define VISION (0x01)
|
||||
#define VISION (0x09)
|
||||
|
||||
|
||||
//写结构体存入视觉信息
|
||||
@ -27,9 +27,9 @@ typedef struct{
|
||||
|
||||
struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
fp32 x;
|
||||
fp32 y;
|
||||
fp32 z;
|
||||
}vision;
|
||||
|
||||
}NUC_t;
|
||||
|
||||
@ -3,10 +3,18 @@
|
||||
#include "cmsis_os.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "bsp_delay.h"
|
||||
#include "go.h"
|
||||
|
||||
// PC6 ball_up_Pin读运球
|
||||
// PE13 down_Pin 下推
|
||||
// PE14 paw_Pin 爪子
|
||||
// PI6 ball_in1_Pin 读接球
|
||||
// PI7 ball_in2_Pin 读接球
|
||||
// PE11 key_Pin 收网
|
||||
#define ballcome 1
|
||||
#define balldown 0
|
||||
|
||||
extern GO go;
|
||||
//光电识别
|
||||
int ball_in(void)
|
||||
{
|
||||
@ -39,32 +47,32 @@ int pass_ball(void)
|
||||
|
||||
|
||||
//运球
|
||||
void handling_ball(int hand, int paw)
|
||||
void handling_ball(int hand, int paw,int angle)
|
||||
{
|
||||
paw =ball_in();
|
||||
GO_TURN_ball(TURN,go,angle);//停止运球
|
||||
paw =ball_in();//获取爪子有无球状态
|
||||
if (hand == 1 && paw == 1)
|
||||
{
|
||||
//爪子
|
||||
delay_us(100);
|
||||
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
|
||||
osDelay(2);
|
||||
delay_us(100);
|
||||
//上推
|
||||
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
|
||||
hand=0;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
//爪子
|
||||
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
|
||||
osDelay(2);
|
||||
delay_us(100);
|
||||
//下推
|
||||
//下推 推下立马收回
|
||||
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
|
||||
osDelay(2);
|
||||
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
|
||||
|
||||
hand=0;//清除手指状态
|
||||
}
|
||||
else if(hand == 0 && paw == 1)
|
||||
{
|
||||
|
||||
//闭合爪子
|
||||
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
|
||||
osDelay(2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -77,10 +85,13 @@ int catch_ball(int inball)
|
||||
inball =pass_ball();
|
||||
if(inball==1)
|
||||
{
|
||||
//接球
|
||||
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
|
||||
return ballcome;
|
||||
}
|
||||
else
|
||||
{
|
||||
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
|
||||
return balldown;
|
||||
}
|
||||
|
||||
|
||||
@ -12,7 +12,7 @@ typedef struct ball
|
||||
|
||||
int ball_in(void);
|
||||
int pass_ball(void);
|
||||
void handling_ball(int hand, int paw);
|
||||
void handling_ball(int hand, int paw,int angle);
|
||||
int catch_ball(int inball);
|
||||
|
||||
|
||||
|
||||
@ -15,8 +15,6 @@ pid_type_def speed_pid;//3508速度环pid结构体
|
||||
pid_type_def angle_pid;//3508位置环pid结构体
|
||||
|
||||
//pid
|
||||
//fp32 M3508_speed_PID[3]={10.0,0.3,0.0}; //P,I,D(速度环)
|
||||
//fp32 M3508_angle_PID[3]={10.0,0.0,1.5}; //P,I,D(角度环)
|
||||
|
||||
fp32 M3508_speed_PID[3]={5.0,0.3,0.0}; //P,I,D(速度环)
|
||||
fp32 M3508_angle_PID[3]={25.0,0.0,1.5}; //P,I,D(角度环)
|
||||
@ -30,6 +28,7 @@ float angleSet[MOTOR_NUM]; //位置环
|
||||
//编码器数据
|
||||
const motor_measure_t *trigger_motor_data;//3508电机数据
|
||||
|
||||
|
||||
pid_type_def t_speed_pid;//2006速度环pid结构体
|
||||
pid_type_def t_angle_pid;//2006位置环pid结构体
|
||||
|
||||
@ -41,14 +40,24 @@ fp32 M2006_PID[3]={4.9,0.01,0.0};
|
||||
int16_t t_result; //速度环 //can最后发送的数据
|
||||
float t_angleSet[1]; //位置环
|
||||
|
||||
//GM6020电机数据
|
||||
const motor_measure_t *GM6020_motor_data;//3508电机数据
|
||||
|
||||
//6020pid
|
||||
pid_type_def speed_6020_pid;
|
||||
pid_type_def angle_6020_pid;
|
||||
|
||||
const fp32 PID_6020_angle[3] = {50, 0.1, 0};
|
||||
const fp32 PID_6020_speed[3]={5,0.01,0};
|
||||
|
||||
|
||||
|
||||
void motor_init(void)
|
||||
{
|
||||
// motor_3508_data=get_motor_point(0);
|
||||
trigger_motor_data=get_motor_point(0);
|
||||
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000);
|
||||
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000);
|
||||
GM6020_motor_data=get_6020_motor_point(0);
|
||||
|
||||
|
||||
// PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
|
||||
// PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
|
||||
@ -56,6 +65,11 @@ void motor_init(void)
|
||||
PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000);
|
||||
PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000);
|
||||
|
||||
|
||||
PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,15000,15000);//6020 pid初始化
|
||||
PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,600,500);
|
||||
|
||||
|
||||
}
|
||||
|
||||
float trigger_angle = 0;
|
||||
@ -67,6 +81,9 @@ void trigger_pos(fp32 angle)
|
||||
delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle);
|
||||
t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]);
|
||||
|
||||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||||
osDelay(2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -86,3 +103,24 @@ void motor_pos(fp32 angle)
|
||||
|
||||
}
|
||||
|
||||
//6020控制
|
||||
void my_GM6020_control(fp32 angle)
|
||||
{
|
||||
|
||||
fp32 angle_get;
|
||||
fp32 my_angle;
|
||||
int32_t M6020_speed;
|
||||
|
||||
//6020串级
|
||||
angle_get= angle;
|
||||
|
||||
//外环角度环
|
||||
my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环
|
||||
//内环速度环
|
||||
M6020_speed=PID_calc(&speed_6020_pid,GM6020_motor_data->speed_rpm, my_angle);//速度环
|
||||
|
||||
CAN_cmd_1FF(M6020_speed, M6020_speed, M6020_speed, M6020_speed,&hcan1);
|
||||
osDelay(2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -20,5 +20,6 @@ typedef enum
|
||||
void motor_pos(fp32 angle);
|
||||
|
||||
void trigger_pos(fp32 angle);
|
||||
void my_GM6020_control(fp32 angle);
|
||||
|
||||
#endif
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
#include "calc_lib.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include <cmsis_os2.h>
|
||||
#define KP 0.12
|
||||
#define KP 0.9
|
||||
#define KD 0.008
|
||||
//可活动角度
|
||||
#define ANGLE_ALLOW 0.3f
|
||||
@ -13,7 +13,7 @@ extern RC_mess_t RC_mess;
|
||||
GO go;
|
||||
|
||||
//初始化
|
||||
void gimbalInit(void)
|
||||
void GO_Init(void)
|
||||
{
|
||||
int i;
|
||||
GO_M8010_init();
|
||||
@ -38,22 +38,30 @@ void gimbalInit(void)
|
||||
|
||||
void gimbalFlow(void)
|
||||
{
|
||||
//用485模块发送数据有问题
|
||||
GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
go.angleSetgo[0]=1;
|
||||
|
||||
//串口6发送数据没有问题
|
||||
GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
// GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
GO_M8010_send_data(&huart6, 1,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
// GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
|
||||
osDelay(1);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void gimbalZero(void)
|
||||
{
|
||||
GO_M8010_send_data(&huart1, 0,0,0,0,0,0,0);
|
||||
GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
|
||||
osDelay(1);
|
||||
GO_M8010_send_data(&huart1, 1,0,0,0,0,0,0);
|
||||
GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
|
||||
}
|
||||
|
||||
void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle)
|
||||
{
|
||||
|
||||
go_set.angleSetgo[go_id] = angle;
|
||||
go_set.Kp = KP;
|
||||
go_set.Kd = KD;
|
||||
GO_M8010_send_data(&huart6, go_id,0,0,go_set.angleSetgo[go_id],1,go_set.Kp,go_set.Kd);
|
||||
|
||||
}
|
||||
|
||||
@ -3,6 +3,12 @@
|
||||
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
|
||||
typedef enum{
|
||||
TURN = 0,
|
||||
GIMBAL = 1,
|
||||
OTHER = 2
|
||||
}GO_ID_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* data */
|
||||
@ -16,9 +22,10 @@ typedef struct
|
||||
float allowRange;
|
||||
}GO;
|
||||
|
||||
void gimbalInit(void);
|
||||
void GO_Init(void);
|
||||
void gimbalFlow(void);
|
||||
void gimbalZero(void);
|
||||
void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -9,6 +9,8 @@
|
||||
extern RC_mess_t RC_mess;
|
||||
|
||||
BASKETBALL basketball;
|
||||
int bb=0;
|
||||
int aa=8;
|
||||
void Task_Ball(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
@ -20,7 +22,16 @@ void Task_Ball(void *argument)
|
||||
while(1)
|
||||
{
|
||||
|
||||
handling_ball(basketball.hand, basketball.paw);
|
||||
// handling_ball(basketball.hand, basketball.paw,aa);
|
||||
if(bb==1)
|
||||
{
|
||||
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
|
||||
}
|
||||
else
|
||||
{
|
||||
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
|
||||
}
|
||||
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||
|
||||
@ -20,6 +20,7 @@ int speed=0;
|
||||
float m=0;
|
||||
float trigger_angle_o=0;
|
||||
int mode=0;
|
||||
int angle_6020=0;
|
||||
/**
|
||||
* \brief 电机任务
|
||||
*
|
||||
@ -38,39 +39,40 @@ void Task_Motor(void *argument)
|
||||
|
||||
while(1)
|
||||
{
|
||||
//收到消息队列新数据
|
||||
// //收到消息队列新数据
|
||||
|
||||
// 更新编码器数据
|
||||
Update_Encoder(&encoder);
|
||||
m=trigger_angle_o*(8191/360);
|
||||
trigger_angle_o=-2.5;//
|
||||
if( mode == THREE )
|
||||
{
|
||||
// // 更新编码器数据
|
||||
// Update_Encoder(&encoder);
|
||||
// m=trigger_angle_o*(8191/360);
|
||||
// trigger_angle_o=-2.5;//
|
||||
// if( mode == THREE )
|
||||
// {
|
||||
|
||||
//当最高点时进入离合
|
||||
if(encoder.round_cnt>=6)
|
||||
{
|
||||
trigger_angle_o=-2.5;
|
||||
trigger_pos(m);
|
||||
}
|
||||
else
|
||||
{
|
||||
trigger_angle_o=0;
|
||||
trigger_pos(m);
|
||||
}
|
||||
// //当最高点时进入离合
|
||||
// if(encoder.round_cnt>=6)
|
||||
// {
|
||||
// trigger_angle_o=-2.5;
|
||||
// trigger_pos(m);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// trigger_angle_o=0;
|
||||
// trigger_pos(m);
|
||||
// }
|
||||
|
||||
}
|
||||
else if( mode == DZ )
|
||||
{
|
||||
//退出离合
|
||||
trigger_angle_o=0;
|
||||
trigger_pos(m);
|
||||
// }
|
||||
// else if( mode == DZ )
|
||||
// {
|
||||
// //退出离合
|
||||
// trigger_angle_o=0;
|
||||
// trigger_pos(m);
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
// my_GM6020_control(angle_6020);
|
||||
|
||||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||||
osDelay(2);
|
||||
// CAN_cmd_200(t_result,0,0,0,&hcan1);//在module写好
|
||||
// osDelay(2);
|
||||
|
||||
// vofa[0]=motor_3508_data->speed_rpm;
|
||||
// vofa[1]=speed;
|
||||
|
||||
@ -16,7 +16,7 @@ void Task_Go(void *argument)
|
||||
#if GOUSE==1
|
||||
|
||||
HAL_Delay(2000);
|
||||
gimbalInit();
|
||||
GO_Init();
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin,GPIO_PIN_RESET);
|
||||
|
||||
while(1)
|
||||
@ -24,7 +24,7 @@ void Task_Go(void *argument)
|
||||
|
||||
//LED_green();
|
||||
//LED_bule();
|
||||
gimbalFlow();
|
||||
// gimbalFlow();
|
||||
osDelay(1);
|
||||
}
|
||||
#else
|
||||
|
||||
@ -7,15 +7,22 @@
|
||||
#include "shoot.h"
|
||||
#include "odrive_shoot.h"
|
||||
#include "read_spi.h"
|
||||
#include "dji_task.h"
|
||||
#include "dji.h"
|
||||
#include "go.h"
|
||||
|
||||
extern RC_mess_t RC_mess;
|
||||
extern Encoder_t encoder;
|
||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
||||
extern int mode;
|
||||
extern GO go;
|
||||
int shoot_flag = 0;
|
||||
int trigger_flag = 0;
|
||||
int pos=0;
|
||||
int gopos=0;
|
||||
|
||||
#define GO1_SHOOT 0
|
||||
#define ODRIVE_SHOOT 1
|
||||
#define ODRIVE_SHOOT 0
|
||||
#define VESC_SHOOT 1
|
||||
|
||||
//odrive发射
|
||||
#define SHOOT3 12
|
||||
@ -34,6 +41,9 @@ void Task_Shoot(void *argument)
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||
|
||||
go.angleSetgo[0] = 0; //id暂时未知
|
||||
GO_TURN_ball(TURN,go,gopos); //发射舵机位置
|
||||
osDelay(1000);
|
||||
while(1)
|
||||
{
|
||||
#if GO1_SHOOT == 1
|
||||
@ -55,6 +65,31 @@ void Task_Shoot(void *argument)
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
#if VESC_SHOOT == 1
|
||||
gopos=35;
|
||||
GO_TURN_ball(TURN,go,gopos); //发射舵机位置
|
||||
if(shoot_flag==1)
|
||||
{
|
||||
CAN_VESC_RPM(0, 10000); //发射电机转速
|
||||
CAN_VESC_RPM(1, 10000); //发射电机转速
|
||||
if(trigger_flag == 1)
|
||||
{
|
||||
trigger_pos(pos); //扳机电机转速
|
||||
trigger_flag = 0;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else if(shoot_flag == 0)
|
||||
{
|
||||
CAN_VESC_RPM(0, 0); //发射电机转速
|
||||
CAN_VESC_RPM(1, 0); //发射电机转速
|
||||
trigger_pos(0); //扳机电机转速
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user