主要运行逻辑如下:
需要源码私聊
if(number_change == 0){
display_6_8_string(0,7,"RUN_RUN_RUN_RUN");
if(K1_Press()){
task_run = 1;
menu = 0;
step = 1;
real_time=0;
task_4_round = 0;
delay_ms(1500);
last_remember_er = 0;
}
if(K2_Press()){
task_num++;
if(task_num>=5){
task_num=1;
}
}
}
if(number_change == 1){
if(K1_Press()){
task_num4_jay1++;
}
if(K2_Press()){
task_num4_jay1--;
}
}
if(number_change == 2){
if(K1_Press()){
task_num4_jay2++;
}
if(K2_Press()){
task_num4_jay2--;
}
}
if(number_change == 3){
if(K1_Press()){
task_num4_KP = task_num4_KP + 0.1;
}
if(K2_Press()){
task_num4_KP = task_num4_KP - 0.1;
}
}
if(K3_Press()){
number_change ++;
if(number_change > 3){
number_change = 0;
}
}
}
while(task_run)
{
// OLED_Fill(0x00);
//
// display_6_8_number(0,0,real_val[0]);
// display_6_8_number(10,0,real_val[1]);
// display_6_8_number(20,0,real_val[2]);
// display_6_8_number(30,0,real_val[3]);
// display_6_8_number(40,0,real_val[4]);
// display_6_8_number(50,0,real_val[5]);
// display_6_8_number(60,0,real_val[6]);
// display_6_8_number(70,0,real_val[7]);
// display_6_8_number(80,0,real_val[8]);
// display_6_8_string(0,3,"Yaw");
// display_6_8_number_pro(50,3,JY60_Yaw);//显示计数器值
//
// display_6_8_number(0,5,speed_0);//显示计数器值
// display_6_8_number(0,6,speed_1);//显示计数器值
//
// display_6_8_number_pro(0,7,step);//显示计数器值
// display_6_8_number(0,1,real_io_val);
if(task_num == 1){
Kp = 8;
Kp_task_2_jy60 = 2;
Kp_jy60 = 0.5;
speed = 100;
// Kd = 0;
task_3(JY60_Yaw,0);
read_io();
if(real_time > 200 && (get_led_num > 0 || get_led_lianxu > 0)){
task_run = 0;
menu = 1;
Emm_V5_Stop_Now(1,0,5);
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(1500);
}
}
if(task_num == 2){
Kp = 8;
Kp_task_2_jy60 = 2;
Kp_jy60 = 0.5;
speed = 100;
// Kd = 0;
if(step == 1){
task_3(JY60_Yaw,0);
read_io();
if(real_time > 300 && (get_led_num > 0 || get_led_lianxu > 0)){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(500);
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 2){
task_1(turn_right,0);
if(real_time > 300 && get_led_num == 0 && JY60_Yaw > 160 && JY60_Yaw < 200){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(500);
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 3){
task_3(JY60_Yaw,180);
read_io();
if(real_time > 300 && (get_led_num > 0 || get_led_lianxu > 0)){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(500);
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 4){
task_1(turn_left,0);
if(real_time > 300 && get_led_num == 0 && (JY60_Yaw > 340 || JY60_Yaw < 20)){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
task_run = 0;
menu = 1;
Emm_V5_Stop_Now(1,0,5);
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(1500);
}
}
}
if(task_num == 3){
Kp = 8;
Kp_task_2_jy60 = 2;
Kp_jy60 = 0.5;
speed = 100;
// Kd = 0;
if(step == 1){
while(!task_2(JY60_Yaw,360 - 35));
step++;
real_time = 0;
}
if(step == 2){
task_3(JY60_Yaw,360 - 35);
read_io();
if(real_time > 300 && (get_led_num > 0 || get_led_lianxu > 0)){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(180);
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 3){
while(!task_2(JY60_Yaw,0));
step++;
real_time = 0;
}
if(step == 4){
task_1(turn_right,0);
if(real_time > 300 && get_led_num == 0 && JY60_Yaw > 160 && JY60_Yaw < 200){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(200);
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 5){
while(!task_2(JY60_Yaw,180 + 38));
step++;
real_time = 0;
}
if(step == 6){
task_3(JY60_Yaw,180 + 38);
read_io();
if(real_time > 300 && (get_led_num > 0 || get_led_lianxu > 0)){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(200);
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 7){
while(!task_2(JY60_Yaw,180));
step++;
real_time = 0;
}
if(step == 8){
task_1(turn_left,0);
if(real_time > 300 && get_led_num == 0 && (JY60_Yaw > 340 || JY60_Yaw < 20)){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
task_run = 0;
menu = 1;
Emm_V5_Stop_Now(1,0,5);
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(1500);
}
}
}
if(task_num == 4){
display_6_8_string(0,1,"Yaw");
display_6_8_number_pro(50,1,JY60_Yaw);//显示计数器值
OLED_Fill(0x00);
//
// display_6_8_number(0,0,real_val[0]);
// display_6_8_number(10,0,real_val[1]);
// display_6_8_number(20,0,real_val[2]);
// display_6_8_number(30,0,real_val[3]);
// display_6_8_number(40,0,real_val[4]);
// display_6_8_number(50,0,real_val[5]);
// display_6_8_number(60,0,real_val[6]);
// display_6_8_number(70,0,real_val[7]);
// display_6_8_number(80,0,real_val[8]);
// Kp = task_num4_KP;
// Kp_task_2_jy60 = 2;
// Kp_jy60 = 0.5;
// Kd = 5;
speed = 150;
if(step == 1){
while(!task_2(JY60_Yaw,360 - task_num4_jay1));
step++;
real_time = 0;
}
if(step == 2){
task_3(JY60_Yaw,360 - task_num4_jay1);
read_io();
if(real_time > 200 && (get_led_num > 0 || get_led_lianxu > 0)){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(150);
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 3){
while(!task_2(JY60_Yaw,0));
step++;
real_time = 0;
delay_ms(100);
}
if(step == 4){
task_1(turn_left,8);
if(real_time > 200 && get_led_num == 0 && JY60_Yaw > 160 && JY60_Yaw < 200){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
Emm_V5_Stop_Now(1,0,5);
delay_ms(100);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 5){
while(!task_2(JY60_Yaw,180 + task_num4_jay2));
step++;
real_time = 0;
delay_ms(100);
}
if(step == 6){
task_3(JY60_Yaw,180 + task_num4_jay2);
read_io();
if(real_time > 200 && (get_led_num > 0 || get_led_lianxu > 0)){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(150);
Emm_V5_Stop_Now(1,0,5);
delay_ms(100);
step++;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
}
}
if(step == 7){
while(!task_2(JY60_Yaw,180));
step++;
real_time = 0;
delay_ms(100);
}
if(step == 8){
task_1(turn_right,8);
if(real_time > 200 && get_led_num == 0 && (JY60_Yaw > 340 || JY60_Yaw < 20)){
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
Emm_V5_Stop_Now(1,0,5);
delay_ms(100);
step=1;
real_time = 0;
DL_GPIO_clearPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN|RGB_PORTB_RGB_G_PIN|RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_clearPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
task_4_round++;
}
}
if(task_4_round > 1){
task_num4_jay1 = 33;
task_num4_jay2 = 35;
}
if(task_4_round > 3){
Emm_V5_Stop_Now(1,0,5);
step++;
real_time = 0;
task_run = 0;
menu = 1;
Emm_V5_Stop_Now(1,0,5);
DL_GPIO_setPins(RGB_PORTB_PORT,RGB_PORTB_RGB_R_PIN | RGB_PORTB_RGB_G_PIN | RGB_PORTB_RGB_B_PIN );//电平翻转驱动RGB
DL_GPIO_setPins(USER_GPIO_BEEP_PORT,USER_GPIO_BEEP_PIN );//电平翻转驱动RGB
delay_ms(1500);
}
}
DL_GPIO_togglePins(USER_GPIO_LED_PA0_PORT,USER_GPIO_LED_PA0_PIN);//电平翻转驱动LED
}