- 打卡等级：偶尔看看I
- 打卡总天数：15
- 最近打卡：2025-11-10 13:36:40
中级会员
- 积分
- 276
|
发表于 2025-11-10 13:36:40
|
显示全部楼层
主程序
#include "bsp_system.h"
#define time_limit_gyro 80
#define time_limit_speed 150
// UI_task UI_alter
int32 cnt_test=0;
int32 speed_LL=0,speed_RR=0;
float v_next; //测量电压
void main_init(void)
{
distance_target = St_2;
BT_state =1;
UI = UI_alter;
task = task_xunji;
READY = 0;
}
void task_run(void);
void task_trace(void);
void state_run(void);
void Pit_fuwu(void);
void key_long_excute(void);
void main(void)
{
/***********初始化************/
clock_init(SYSTEM_CLOCK_40M);
debug_init();
gpio_init(IO_P32,GPO,0,GPO_PUSH_PULL);
main_init();
task_init();
system_delay_init();
//初始化使用
//按键 跳过读取
if(key_D == 1){iap_scan();ips200_string(19,4,"scan");}
else{ips200_string(19,4,"no scan");}
/*中断服务*/
//调参or启动
if(key_E == 0)
{
system_delay_ms(50);
ips200_string(19,3,"OK");
while(key_E==0);
system_delay_ms(50);
UI = UI_alter;
ips200_string(19,4," ");
}
else{UI = UI_task;}
//中断时间选择
if(UI == UI_task) {pit_ms_init(TIM_1,5);}
else {UI_ips200_init();pit_ms_init(TIM_1,50);}
interrupt_set_priority(TIMER1_IRQn, 0); // 设置 PIT1 对周期中断的中断优先级为 0，0为最低优先级
tim1_irq_handler = Pit_fuwu;
/************循环*************/
while(1)
{
// if(UI == UI_task && adc_v<=5.3 && distance > 10000)
// {
// UI = UI_alter;
// motor_L(0);
// motor_R(0);
// pit_ms_init(TIM_1,50);
//// aoi_bee(1);
// }
}//while
}
void Pit_fuwu(void)
{
//0-获取数据
get_data();
distance = abs_int32(distance_text);
//电压检测
v_next = (adc_sample(ADC_V));
adc_v = adc_v_sample(v_next);
if(UI == UI_task)
{
state_run();
task_run();
if(READY ==3){time_run+=5;}
}
//3-串口信息打印
my_debug_excute();
//0-调参
if(UI == UI_alter )
{
siai_adc_all_sample(); //中间平均滤波
adc_normalizing(); //电磁归一化
key_loop();
key_long_excute();
UI_key_excute();
UI_ips200_proc();
speed_target = speed_straight;
}
}
void task_run(void)
{
switch(READY)
{
case 0: //零漂解算
READY = null_drift_calculate();
//#if chongdian == 0
// if(READY == 1){READY=3;}
//#endif
// if(READY == 1){system_delay_ms(4000);READY=3;}
break;
case 1: //等待电压
if(adc_v>adc_start)
{
angle_clear();
READY =2;
}
break;
case 2: //起跑
if(turn_direction == 1)
{
speed_target = 195;
gyro_loop(-gyro_adv_out);
if(out_gyro>0)
{
loop_speed_LR(speed_target + small_t*out_gyro,speed_target - large_t*out_gyro);
}
else
{
loop_speed_LR(speed_target + large_t*out_gyro,speed_target - small_t*out_gyro);
}
motor_L(out_L);
motor_R(out_R);
/**********记得改大小于号********/
if(angle_ringR<-angle_start)
{
READY = 3;
}
}
else if(turn_direction == 2)
{
speed_target = 195;
gyro_loop(gyro_adv_out);
if(out_gyro>0)
{
loop_speed_LR(speed_target + small_t*out_gyro,speed_target - large_t*out_gyro);
}
else
{
loop_speed_LR(speed_target + large_t*out_gyro,speed_target - small_t*out_gyro);
}
motor_L(out_L);
motor_R(out_R);
/**********记得改大小于号********/
if(angle_ringR>angle_start)
{
READY = 3;
}
}
else if(turn_direction == 3)
{
speed_target = 190;
gyro_loop(0);
if(out_gyro>0)
{
loop_speed_LR(speed_target + small_t*out_gyro,speed_target - large_t*out_gyro);
}
else
{
loop_speed_LR(speed_target + large_t*out_gyro,speed_target - small_t*out_gyro);
}
motor_L(out_L);
motor_R(out_R);
if(distance > (pid_motor_ringR.Kd*100))
{
READY = 3;
}
}
else
{
READY = 3;
}
break;
case 3:
task_trace();
break;
default:break;
}
}
void task_trace(void)
{
if(distance < distance_target) //循迹
{
task_excute(task);
}
else //停车
{
pid_loop_speed = pid_loop_speed_start;
speed_target = 0;
loop_speed();
turn_flag = 2;
motor_L(out_L);
motor_R(out_L);
}
speed_LL = abs_int32(speed_L);
speed_RR = abs_int32(speed_R);
if( distance > 10000 && distance <distance_target && speed_LL<50 && speed_RR<50) //堵转停车
{
motor_L(0);
motor_R(0);
UI = UI_alter;
aoi_bee(1);
turn_flag = 3;
}
}
void state_run(void)
{
if(time_run > time_limit_gyro)
{
limit_gyro =limit_gyro_t;
}
else
{
limit_gyro =300;
}
if(time_run > time_limit_speed)
{
pid_loop_speed = pid_loop_speed_run;
}
else
{
pid_loop_speed = pid_loop_speed_start;
}
}
|
|
|手机版|深圳国芯人工智能有限公司
( 粤ICP备2022108929号-2 )
已绑定手机