void main()
{
clock_init(SYSTEM_CLOCK_40M); // 务必保留
debug_init(); // 务必保留
system_delay_init();
interrupt_global_disable();
Laser_Init(); // 激光器初始化
Beep_Init(); // 蜂鸣器初始化
Beep_On(); // 蜂鸣器打开
pit_ms_init(TIM0_PIT,1);
pit_us_init(TIM1_PIT, 100);
IP &= ~(1 << 3); // 将 IP 寄存器的 B3 清零
IPH &= ~(1 << 3); // 将 IPH 寄存器的 B3 清零
// 此处编写用户代码 例如外设初始化代码等
Key_Init();
ZDT_Motor_Init();
gyroscope_init();
Init_OLED(); // 初始化OLED
Menu_init();
PID_Param_Init(); // PID参数初始化
MyUart_Init();
Servo_Init();
Beep_Off(); // 蜂鸣器关闭
interrupt_global_enable();
while(1)
{
uint8 i = 0;
if(FLAG.T_1ms == 1) // 1ms
{
FLAG.T_1ms = 0; // 清除标志位
}
if(FLAG.T_2ms == 1)
{
FLAG.T_2ms = 0;
Menu_Key_Event(); // 扫描按键
}
if(FLAG.T_10ms == 1)
{
Update_GyroData();
FLAG.T_10ms = 0;
Problem_Process();
Gimbal_Ctrl();
}
if(FLAG.T_50ms)
{
FLAG.T_50ms = 0; // 清除标志位
Menu_Display();
K230_Update(); // 更新K230数据
}
if(FLAG.T_100ms)
{
FLAG.T_100ms = 0; // 清除标志位
if(sys_status.gimbal_status == open_status)
sys_status.start_time_ms+=100;
else
sys_status.start_time_ms = 0; // 如果云台状态为关闭,则清除启动时间
}
if(FLAG.T_200ms)
{
FLAG.T_200ms = 0; // 清除标志位
}
Data_Receive(); // 此处编写需要循环执行的代码
}
}
2.k230串口通讯代码
MY_UART_T my_uart = {0}; // 定义 MY_UART_T 结构体实例
MY_UART_VARIABLE_T my_uart_variable[NUM_LEN] = {
{"DONOTHING"},
{"Test1"},
{"Test"},
{"B_x" },
{"B_y" },
{"C_x" },
{"C_y" },
{"X_Step" },
{"Y_Step" },
{"stable" },
}; // 定义 MY_UART_VARIABLE_T 结构体实例
#define UART_PACKET_MAX_LEN 6128
// 查找FIFO中第一个'$'的位置,返回下标(0为第一个字节),未找到返回-1
int fifo_find_dollar(fifo_struct *fifo)
{
uint32 used = fifo_used(fifo);
uint8 tmp;
uint32 i = 0;
for(i = 0; i < used; i++)
{
// 只读不取出
uint32 idx = (fifo->end + i) % fifo->max;
switch(fifo->type)
{
case FIFO_DATA_8BIT:
tmp = ((uint8*)fifo->buffer)[idx];
break;
default:
return -1;
}
if(tmp == '$') return i;
}
return -1;
}
// 每次调用只取出一个完整包(含$),返回实际长度(含$),未找到返回0
uint32 fifo_read_one_packet(fifo_struct *fifo, uint8 *out_buf, uint32 buf_size)
{
int pos = fifo_find_dollar(fifo);
uint32 len = 0;
if(pos < 0) return 0; // 没有完整包
len = pos + 1;
if(len > buf_size) len = buf_size; // 防止溢出
fifo_read_buffer(fifo, out_buf, &len, FIFO_READ_AND_CLEAN);
return len;
}
void uart_rx_interrupt_handler (uint8 dat)
{
// get_data = uart_read_byte(UART_INDEX); // 接收数据 while 等待式 不建议在中断使用
uart_query_byte(MY_UART, &dat); // 查询式接收
fifo_write_buffer(&my_uart.uart_data_fifo, &dat, 1); // 写入FIFO
}
void MyUart_Init(void)
{
uart_init(MY_UART, MY_UART_BAUDRATE, MY_UART_TX_PIN, MY_UART_RX_PIN); // 初始化 UART 模块与引脚
fifo_init(&my_uart.uart_data_fifo, FIFO_DATA_8BIT, my_uart.uart_get_data, 128); // 初始化 fifo 挂载缓冲区
uart_init(MY_UART, MY_UART_BAUDRATE, MY_UART_TX_PIN, MY_UART_RX_PIN); // 初始化编码器模块与引脚 正交解码编码器模式
// UART1的中断优先级不能设置,为最低优先级值0
// UART1的中断优先级不能设置,为最低优先级值0
// UART1的中断优先级不能设置,为最低优先级值0
uart_rx_interrupt(MY_UART, ZF_ENABLE); // 开启 UART_INDEX 的接收中断
// 设置中断回调函数
uart3_irq_handler = uart_rx_interrupt_handler;
}
void MyUart_PrintStr( char *format, ...)
{
char String[64]; // 定义字符数组
uint8 String_length = 0;
va_list arg; // 定义可变参数列表数据类型的变量arg
va_start(arg, format); // 从format开始,接收参数列表到arg变量
vsprintf(String, format, arg); // 使用vsprintf打印格式化字符串和参数列表到字符数组中
va_end(arg); // 结束变量arg
String_length = strlen(String);
String[String_length] = '$';
String[String_length + 1] = '\0';
String_length++;
MyUart_SendString(String);
}
void Get_Str_Between(const char *input, char start_char, char end_char, char *out_buf, int out_buf_size)
{
const char *start;
const char *end;
int len = end - start;
// 处理首字符
if(start_char == ' ')
start = input;
else
{
start = strchr(input, start_char);
if(!start) { out_buf[0] = '\0'; return; }
start++; // 跳过分割符
}
// 处理末字符
if(end_char == ' ')
end = input + strlen(input);
else
{
end = strchr(start, end_char);
if(!end) { out_buf[0] = '\0'; return; }
}
if(len >= out_buf_size) len = out_buf_size - 1;
strncpy(out_buf, start, len);
out_buf[len] = '\0';
}
// 查找变量名在 Variable_Check 中的索引
int find_variable_index(const char *name)
{
int j = 0;
for(j = 0; j < NUM_LEN; j++)
{
if(strcmp(my_uart_variable[j].DataBuff, name) == 0)
return j;
}
return -1;
}
// 处理参数调整
void ParamAdjust_OneWord(void)
{
char temp[64];
char *eq;
int idx;
float data_return;
char TEMP_SHOW[25];
strncpy(temp, (char *)my_uart.fifo_get_data, my_uart.fifo_data_count);
temp[my_uart.fifo_data_count] = '\0'; // 确保字符串结束
eq = strchr(temp, '=');
if(!eq) return;
*eq = '\0'; // 分割变量名和数值
idx = find_variable_index(temp);
if(idx < 0) return;
data_return = atof(eq + 1);
switch(idx)
{
case DONOTHING:
// 不处理
break;
case Test1:
break;
case B_x:
blue_x = (uint16)data_return;
break;
case B_y:
blue_y = (uint16)data_return;
break;
case C_x:
rectangle_center_x = (uint16)data_return;
break;
case C_y:
rectangle_center_y = (uint16)data_return;
break;
case X_Step:
ZDT_Motor_Set_Step(down, (int16)data_return);
break;
case Y_Step:
ZDT_Motor_Set_Step(up, (int16)data_return);
break;
case stable:
sys_status.camera_stable = 1;
// default:
// // 未知变量名,忽略
// return;
}
// menu.Refresh_Flag = 1;
// sprintf(TEMP_SHOW,"%s:%.4f",my_uart_variable[idx].DataBuff,data_return);
// Menu_Show_String(0,0,TEMP_SHOW);
}
void Data_Receive(void)
{
uint8 uart_packet[UART_PACKET_MAX_LEN];
uint32 pkt_len;
while((pkt_len = fifo_read_one_packet(&my_uart.uart_data_fifo, uart_packet, UART_PACKET_MAX_LEN)) > 0)
{
Beep_On();
// uart_packet[0..pkt_len-1]为一个完整包(含$)
// 复制到my_uart.fifo_get_data,兼容原有处理流程
if(pkt_len > sizeof(my_uart.fifo_get_data)) pkt_len = sizeof(my_uart.fifo_get_data);
memcpy(my_uart.fifo_get_data, uart_packet, pkt_len);
my_uart.fifo_data_count = pkt_len - 1; // 不含结尾$
my_uart.fifo_get_data[my_uart.fifo_data_count] = '\0'; // 字符串结束符
my_uart.Data_Receive_Flag = 1;
ParamAdjust_OneWord();
}
}
2.瞄准任务代码
Sys_Status sys_status = {0}; // 系统状态初始化
Problem3_Value problem3_value;
Element_Status element_status = 0;
float debug_temp = 0;
float yaw_offset_kp = 80;
void Gimbal_Start(void)
{
if(sys_status.problem_num > 0 && sys_status.problem_num < 7)
{
sys_status.camera_stable = 0;
sys_status.gimbal_status = open_status;
element_status = 0;
MyUart_PrintStr("gimbal_status=1");
}
}
//具体的来说,此处应该是sys_stop
void Gimbal_Stop(void)
{
sys_status.gimbal_status = close_status;
// sys_status.problem_num = 0; // 问题处理完成,重置问题编号
sys_status.start_time_ms = 0; // 重置启动时间
Laser_Off(); // 确保激光器关闭
element_status = 0;
MyUart_PrintStr("gimbal_status=0");
}
void Problem3_SetYawZero(void)
{
problem3_value.yaw_zero = MY_Yaw_TOTAL; // 设置问题3的偏航角零点
}
void Problem_Process(void)
{
if(sys_status.gimbal_status == open_status)
{
// printf("%d\r\n", sys_status.start_time_ms);
if(sys_status.problem_num == 0);
else if(sys_status.problem_num == 1); //前一题和0不做处理
else if(sys_status.problem_num == 2)
{
if (sys_status.start_time_ms > project_2_dT)
{
Laser_Off(); // 激光器关闭
Gimbal_Stop(); // 停止系统
}
else if(sys_status.start_time_ms > 200)
{
Beep_On();
Laser_On(); // 激光器打开
}
}
else if(sys_status.problem_num == 3)
{
if(element_status < problem3_1_status || element_status > problem3_num_status)
{
if(sys_status.start_time_ms > 2000)
{
sys_status.camera_stable = 0;
problem3_value.yaw_err = ((int)MY_Yaw_TOTAL - (int)problem3_value.yaw_zero) % 360;
if(problem3_value.yaw_err > 180)
problem3_value.yaw_err -= 360;
else if(problem3_value.yaw_err < -180)
problem3_value.yaw_err += 360;
problem3_value.down_motor_step = problem3_value.yaw_err *100.0;// 142.2;
problem3_value.down_motor_step = LIMIT_VAL(problem3_value.down_motor_step, -51200, 51200);
// printf("%f,%d,%d,%d\r\n", problem3_value.yaw_err, problem3_value.down_motor_step, problem3_value.down_motor_step>0?rotation_forward:rotation_reverse, problem3_value.down_motor_step>0?problem3_value.down_motor_step:-problem3_value.down_motor_step);
ZDT_Motor_Step(down, problem3_value.down_motor_step>0?rotation_forward:rotation_reverse, problem3_value.down_motor_step>0?problem3_value.down_motor_step:-problem3_value.down_motor_step); // 下电机正转
MyUart_PrintStr("problem3_status=2");
element_status = problem3_2_status;
}
}
if(element_status == problem3_2_status)
{
if(sys_status.camera_stable == 1)
{
element_status = problem3_3_status; // 处理问题3的第三阶段
sys_status.past_timer_start_ms = sys_status.start_time_ms; // 记录开始时间
}
// 处理问题3的第二阶段
}
if(element_status == problem3_3_status)
{
Laser_On(); // 激光器打开
if(sys_status.start_time_ms - sys_status.past_timer_start_ms > 3000)
{
Laser_Off(); // 激光器关闭
Gimbal_Stop(); // 停止系统
}
}
}
else if(sys_status.problem_num == 4)
{
// if(gimbal_data.receive_flag == 1)
// {
Beep_On();
// printf("%f, %f, %f\r\n", yaw_total, gimbal_data.receive_yaw, yaw_total - gimbal_data.receive_yaw);
debug_temp = -(yaw_total - gimbal_data.receive_yaw) * yaw_offset_kp;
// debug_temp = LIMIT_VAL(debug_temp, -20000.0, 20000.0);
// debug_temp = (int16)debug_temp;
gimbal_data.receive_yaw = yaw_total;
// printf("%f, %f, %f\r\n", debug_temp, yaw_total, gimbal_data.receive_yaw);
ZDT_Motor_Set_Step(down, (int16)debug_temp); // 下电机转动,补偿偏航角
// }
}
}
}