Task

Problem A : Task

From:HDU, 4864

Problem Description
Today the company has m tasks to complete. The ith task need xi minutes to complete. Meanwhile, this task has a difficulty level yi. The machine whose level below this task’s level yi cannot complete this task. If the company completes this task, they will get (500*xi+2*yi) dollars.
The company has n machines. Each machine has a maximum working time and a level. If the time for the task is more than the maximum working time of the machine, the machine can not complete this task. Each machine can only complete a task one day. Each task can only be completed by one machine.
The company hopes to maximize the number of the tasks which they can complete today. If there are multiple solutions, they hopes to make the money maximum.
 

Input
The input contains several test cases.
The first line contains two integers N and M. N is the number of the machines.M is the number of tasks(1 < =N <= 100000,1<=M<=100000).
The following N lines each contains two integers xi(0<xi<1440),yi(0=<yi<=100).xi is the maximum time the machine can work.yi is the level of the machine.
The following M lines each contains two integers xi(0<xi<1440),yi(0=<yi<=100).xi is the time we need to complete the task.yi is the level of the task.
 

Output
For each test case, output two integers, the maximum number of the tasks which the company can complete today and the money they will get.
 

Sample Input
  
1 2 100 3 100 2 100 1
 

Sample Output
  
1 50004
 解题思路:一个机器只能进行一次作业,要求最终的价值最大,是贪心问题,由于0=<yi<=100,value=500*xi+2*yi;可以看出在权重方面,y对value的贡献可以忽略
因此,优先考虑x,对于符合要求的机器,取y尽可能小的。由于n、m的值比较大,考虑o(nlogn),提高算法效率。
#include<iostream>
#include<algorithm>
#include<set>
#include<cstdio>
using namespace std;
#define maxn 110000
struct note{
int x;
int y;
friend bool operator <(note a,note b){//注意此处是重载小于号,返回的应该是小于号的规则 
if(a.y!=b.y)
return a.y<b.y;//此处是小于 
else
return a.x<b.x;

}
}aa[maxn],bb[maxn];
int n,m;
bool cmp(note a,note b){
if(a.x!=b.x)
return a.x>b.x;
else return a.y>b.y;
}
void solve(){
multiset<note>mp;//multiset类型可以将相同的数据放进去,此处mp为note类型,排序按照note定义的规则或默认规则 
sort(aa,aa+n,cmp);
sort(bb,bb+m,cmp);
int t=0,count=0;
long long sum=0;
for(int i=0;i<m;i++){ //注意if的判定条件  &&bb[i].y<=aa[t].y,不可加入 
while(t<n&&bb[i].x<=aa[t].x){//注意wh ile的范围,仅是将满足条件的插入,插入之后再对每个bb[i]作比较 
mp.insert(aa[t++]);}//将a[t]插入,并按照默认或改写的规则排序 
multiset<note>::iterator it=mp.lower_bound(bb[i]);//it指向大于等于bb[i]的位置 
/if(it!=mp.end()){
mp.erase(it);//将it扔掉 
count++;
sum=sum+bb[i].x*500+bb[i].y*2;
}

}
cout<<count<<" "<<sum<<endl;
}
int main(){
while(cin>>n>>m){
for(int i=0;i<n;i++) scanf("%d%d",&aa[i].x,&aa[i].y);
for(int i=0;i<m;i++) scanf("%d%d",&bb[i].x,&bb[i].y);
solve();
}
return 0;
}
void go_to_xy(float target_x, float target_y) 的函数发现执行时,只执行了第一阶段。考虑运用switch函数解决,可以使用别的方法#include <math.h> // 添加数学库用于fabs函数 #include "board.h" #include "my_key.h" #include "my_time.h" #include "ti_msp_dl_config.h" #include "oled.h" #define _USE_MATH_DEFINES #define WHEEL_BASE 80.0f #ifndef M_PI #define M_PI 3.14159265358979323846 #endif void BUZZY_OFF(void) { DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN); } void BUZZY_ON(void) { DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN); } void refresh_oled(void); void key(void); void go_straight(int dis); void go_arc_ccd(hsu_time_t); void go_brc_ccd(hsu_time_t); void turn_90_degrees(int direction); void turn_in_place(float angle); void go_to_xy(float target_x, float target_y); void sound_light_alert(void); void show_task_now(void); u8 Car_Mode = Diff_Car; int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的 u8 PID_Send; // 延时和调参相关变量 float RC_Velocity = 200, RC_Turn_Velocity, Move_X, Move_Y, Move_Z, PS2_ON_Flag; // 遥控控制的速度 float Velocity_Left, Velocity_Right; // 车轮速度(mm/s) u16 test_num, show_cnt; float Voltage = 0; extern float Yaw; // 声明外部YAW角度变量 int64_t left_encoder = 0, right_encoder = 0; void SysTick_Handler(void) { hsu_time_systick_handler(); } typedef enum { BEGIN, T1, T2, T3, T4 } TaskState; typedef enum { STOP, GO_STRAIGHT, GO_CCD, TURN_IN_PLACE, TURN_90_DEGREES, WAIT_ALERT } DoingWhat; typedef struct __TASK_NAMESPACE { uint8_t is; uint8_t is_running; uint8_t finish; uint8_t sub_finish; uint8_t running_state; // 0: 停止, 1: 运行中 TaskState state; DoingWhat doing_what; float target; float vx; float vz; // 用于复杂任务 uint8_t sub_task_stage; // 子任务阶段 uint8_t lap_count; // 圈数计数 int64_t start_encoder; // 起始编码器值 uint32_t alert_start_time; // 声光提示开始时间 float start_yaw; // 起始YAW角度 float target_yaw_diff; // 目标YAW角度差 hsu_time_t ccd_end_time; float target_x; // 目标X坐标 (毫米) float target_y; // 目标Y坐标 (毫米) float current_x; // 当前X坐标 (毫米) float current_y; // 当前Y坐标 (毫米) int64_t rotation_start_left; // 旋转开始时左轮编码器值 int64_t rotation_start_right; // 旋转开始时右轮编码器值 float target_rotation; // 目标旋转量(弧度) } TaskNamespace; void reset_task_namespace(TaskNamespace *t) { t->is_running = 0; t->finish = 0; t->sub_finish = 0; t->state = BEGIN; t->doing_what = STOP; t->vx = 0; t->vz = 0; t->sub_task_stage = 0; t->lap_count = 0; t->start_encoder = left_encoder; t->alert_start_time = 0; t->start_yaw = 0; t->target_yaw_diff = 0; t->running_state = 0; // 明确重置运行状态为0 t->ccd_end_time = 0; t->is = 0; // 初始化坐标 t->target_x = 0.0f; t->target_y = 0.0f; t->current_x = 0.0f; t->current_y = 0.0f; t->rotation_start_left = 0; t->rotation_start_right = 0; t->target_rotation = 0.0f; } void next_state(TaskNamespace *t) { TaskState last_state = t->state; reset_task_namespace(t); if (last_state < T4) { t->state = last_state + 1; } } TaskNamespace task_namespace; void show_task_now(void) { //OLED_ShowString(0, 0, "Task Now:"); switch (task_namespace.state) { case BEGIN: OLED_ShowString(1, 10,"0"); break; case T1: OLED_ShowString(1, 10,"1"); break; case T2: OLED_ShowString(1, 10,"2"); break; case T3: OLED_ShowString(1, 10,"3"); break; case T4: OLED_ShowString(1, 10,"4"); break; default: break; } } void main_task(void); int main(void) { // 系统初始化 SYSCFG_DL_init(); // 初始化系统配置 hsu_time_init(); // 时间 // 清除所有外设的中断挂起状态 NVIC_ClearPendingIRQ(ENCODERA_INT_IRQN); // 编码器A中断 NVIC_ClearPendingIRQ(ENCODERB_INT_IRQN); // 编码器B中断 NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN); // UART0串口中断 // 使能各外设的中断 NVIC_EnableIRQ(ENCODERA_INT_IRQN); // 开启编码器A中断 NVIC_EnableIRQ(ENCODERB_INT_IRQN); // 开启编码器B中断 NVIC_EnableIRQ(UART_0_INST_INT_IRQN); // 开启UART0中断 reset_task_namespace(&task_namespace); task_namespace.state = BEGIN; // 明确设置初始状态 // 定时器和ADC相关中断配置 NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); // 清除定时器0中断挂起 NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); // 开启定时器0中断 NVIC_EnableIRQ(ADC12_VOLTAGE_INST_INT_IRQN); NVIC_EnableIRQ(ADC12_CCD_INST_INT_IRQN); OLED_Init(); // 初始化OLED显示屏 OLED_ShowString(1, 1, "Task Now:"); OLED_ShowString(2, 1, "state:"); OLED_ShowString(3, 1, "yaw:"); OLED_ShowString(4, 1, "x:"); OLED_ShowString(4, 6, "y:"); //MPU6050_initialize(); //DMP_Init(); BUZZY_ON(); // 主循环 // printf("Test delay 500us\n"); // hsu_time_delay_us(500); // printf("Test delay 500us end\n"); uint8_t main_task_timer = hsu_time_timer_create(10, true, main_task); hsu_time_timer_start(main_task_timer); uint8_t refresh_oled_timer = hsu_time_timer_create(5, true, refresh_oled); hsu_time_timer_start(refresh_oled_timer); uint8_t key_timer = hsu_time_timer_create(2, true, key); hsu_time_timer_start(key_timer); while (1) { hsu_time_timer_process(); RD_TSL(); // 读取CCD数据 Find_CCD_Median(); // 计算CCD数据中值 Read_DMP(); show_task_now(); //DL_GPIO_togglePins(LED_PORT, LED_led_PIN); // printf("L=%lld R=%lld YAW=%.1f\n", left_encoder, right_encoder, Yaw); } } void task_no(void); void task_1(void); void task_2(void); void task_3(void); void task_4(void); void main_task(void) { if (!(task_namespace.is)) return; printf("main task\n"); switch (task_namespace.state) { case BEGIN: task_no(); break; case T1: task_1(); break; case T2: task_2(); break; case T3: task_3(); break; case T4: task_4(); break; default: break; } switch (task_namespace.doing_what) { case STOP: Get_Target_Encoder(0, 0); break; case GO_STRAIGHT: if ((left_encoder * 1.f) < task_namespace.target) { Get_Target_Encoder(0.6, 0); // 提高速度到600mm/s } else { Get_Target_Encoder(0, 0); task_namespace.doing_what = STOP; task_namespace.finish = 1; } break; case GO_CCD: if (task_namespace.ccd_end_time < hsu_time_get_ms()) { Get_Target_Encoder(0, 0); task_namespace.doing_what = STOP; task_namespace.finish = 1; } else { CCD_Mode(); } break; case TURN_IN_PLACE: // 原地转向控制 if (task_namespace.target_yaw_diff != 0) { float current_yaw_diff = Yaw - task_namespace.start_yaw; // 处理角度跨越±180度的情况 if (current_yaw_diff > 180) { current_yaw_diff -= 360; } else if (current_yaw_diff < -180) { current_yaw_diff += 360; } printf("Turn: Start=%.1f Current=%.1f Diff=%.1f Target=%.1f\n", task_namespace.start_yaw, Yaw, current_yaw_diff, task_namespace.target_yaw_diff); // 检查是否达到目标角度 if ((task_namespace.target_yaw_diff > 0 && current_yaw_diff >= task_namespace.target_yaw_diff) || (task_namespace.target_yaw_diff < 0 && current_yaw_diff <= task_namespace.target_yaw_diff)) { Get_Target_Encoder(0, 0); // 停止转向 task_namespace.doing_what = STOP; task_namespace.finish = 1; } else { // 继续转向 float turn_speed = (task_namespace.target_yaw_diff > 0) ? 0.1 : -0.1; Get_Target_Encoder(0, turn_speed); } } break; case TURN_90_DEGREES: { // 计算当前旋转角度(使用编码器差值) float rotation = (left_encoder - task_namespace.rotation_start_left - (right_encoder - task_namespace.rotation_start_right)) * M_PI / (2 * WHEEL_BASE); // 检查是否达到目标角度(允许±0.1弧度误差) if (fabs(rotation - task_namespace.target_rotation) < 0.1f) { Get_Target_Encoder(0, 0); // 停止旋转 task_namespace.doing_what = STOP; task_namespace.finish = 1; } else { // 控制旋转速度(根据方向调整) float turn_speed = (task_namespace.target_rotation > 0) ? 0.1f : -0.1f; Get_Target_Encoder(-turn_speed, turn_speed); // 左右轮反向运动 } break; } case WAIT_ALERT: Get_Target_Encoder(0, 0); // 停车 if (hsu_time_get_ms() - task_namespace.alert_start_time > 1000) { // 声光提示1秒 task_namespace.doing_what = STOP; task_namespace.finish = 1; } break; default: break; } } void task_no(void) { return; } // 任务1:A点到B点直线行驶 void task_1(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 开始第一阶段:A到B go_straight(3300); break; case 1: // 完成亮灯 DL_GPIO_togglePins(LED_PORT, LED_led_PIN); break; case 2: //任务结束 reset_task_namespace(&task_namespace); task_namespace.running_state = 0; // 重置为停止状态 break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务2:X Y坐标行驶 void task_2(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 运动到XY坐标 go_to_xy(3000.0f, 3000.0f); break; case 1: // 完成灯亮 DL_GPIO_togglePins(LED_PORT, LED_led_PIN); break; case 2: // 任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务3:A->C->B->D->A循环 void task_3(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: turn_in_place(-31.0f); break; case 1: go_straight(4060); break; case 2: turn_in_place(30.0f); break; case 3: // 开始C到B弧线 sound_light_alert(); //go_straight(40); go_brc_ccd(3550); break; case 4: turn_in_place(36.0f); break; case 5: sound_light_alert(); go_straight(3985); break; case 6: turn_in_place(-40.0f); break; case 7: // 开始C到B弧线 sound_light_alert(); go_arc_ccd(3600); break; case 8: // D到A弧线完成,任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } return; } // 任务4:重复任务3路径4圈 void task_4(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: turn_in_place(-30.0f); break; case 1: go_straight(4045); break; case 2: turn_in_place(29.0f); break; case 3: // 开始C到B弧线 sound_light_alert(); //go_straight(40); go_brc_ccd(5000); break; case 4: turn_in_place(34.0f); break; case 5: sound_light_alert(); go_straight(4035); break; case 6: turn_in_place(-33.0f); break; case 7: // 开始C到B弧线 sound_light_alert(); go_arc_ccd(5000); break; case 8: // D到A弧线完成,任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.finish = 0; task_namespace.sub_task_stage++; } } void TIMER_0_INST_IRQHandler(void) { if (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) { if (DL_TIMER_IIDX_ZERO) { Get_Velocity_From_Encoder(Get_Encoder_countA, Get_Encoder_countB); Get_Encoder_countA = Get_Encoder_countB = 0; MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder); MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder); if (!Flag_Stop) { Set_PWM(-MotorA.Motor_Pwm, -MotorB.Motor_Pwm); } else { Set_PWM(0, 0); } } } } uint32_t gpio_interrup1, gpio_interrup2; int64_t B1, B2, B3, B4; int64_t A1, A2, A3, A4; void GROUP1_IRQHandler(void) { // 获取中断信号 gpio_interrup1 = DL_GPIO_getEnabledInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN); gpio_interrup2 = DL_GPIO_getEnabledInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN); // encoderB if ((gpio_interrup1 & ENCODERA_E1A_PIN) == ENCODERA_E1A_PIN) { if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1B_PIN)) { right_encoder--; Get_Encoder_countB--; } else { right_encoder++; Get_Encoder_countB++; } } else if ((gpio_interrup1 & ENCODERA_E1B_PIN) == ENCODERA_E1B_PIN) { if (!DL_GPIO_readPins(ENCODERA_PORT, ENCODERA_E1A_PIN)) { right_encoder++; Get_Encoder_countB++; } else { right_encoder--; Get_Encoder_countB--; } } // encoderA if ((gpio_interrup2 & ENCODERB_E2A_PIN) == ENCODERB_E2A_PIN) { if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2B_PIN)) { left_encoder++; Get_Encoder_countA--; } else { left_encoder--; Get_Encoder_countA++; } } else if ((gpio_interrup2 & ENCODERB_E2B_PIN) == ENCODERB_E2B_PIN) { if (!DL_GPIO_readPins(ENCODERB_PORT, ENCODERB_E2A_PIN)) { left_encoder--; Get_Encoder_countA++; } else { left_encoder++; Get_Encoder_countA--; } } DL_GPIO_clearInterruptStatus(ENCODERA_PORT, ENCODERA_E1A_PIN | ENCODERA_E1B_PIN); DL_GPIO_clearInterruptStatus(ENCODERB_PORT, ENCODERB_E2A_PIN | ENCODERB_E2B_PIN); } // 直线行驶函数 void go_straight(int dis) { task_namespace.doing_what = GO_STRAIGHT; task_namespace.target = left_encoder + dis; task_namespace.finish = 0; } // 原地转向函数 void turn_in_place(float angle) { task_namespace.doing_what = TURN_IN_PLACE; task_namespace.start_yaw = Yaw; task_namespace.target_yaw_diff = angle; // 正值右转,负值左转 task_namespace.finish = 0; } // CCD巡线函数(需要外部条件结束) void go_ccd_line(void) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = left_encoder; task_namespace.finish = 0; // 设置一个安全的最大距离,防止无限巡线 // 可以根据实际场地调整这个值 static uint32_t ccd_end_time = 0; if (ccd_end_time == 0) { ccd_end_time = hsu_time_get_ms(); } // 如果巡线时间超过10秒或距离超过2000mm,强制结束 if (hsu_time_get_ms() - ccd_end_time > 10000 || (left_encoder * 1.f - task_namespace.start_encoder) > 2000) { task_namespace.finish = 1; ccd_end_time = 0; } } // 弧线CCD巡线函数 void go_arc_ccd(hsu_time_t time) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = left_encoder; task_namespace.ccd_end_time = hsu_time_get_ms() + time; } void go_brc_ccd(hsu_time_t time) { task_namespace.doing_what = GO_CCD; task_namespace.start_encoder = right_encoder; task_namespace.ccd_end_time = hsu_time_get_ms() + time; } // 旋转90度函数 /*void turn_90_degrees(int direction) { // direction: 1为顺时针,-1为逆时针 turn_in_place(90.0f * direction); }*/ // 运动到指定坐标函数 void go_to_xy(float target_x, float target_y) { task_namespace.sub_task_stage = 0; // 重置子任务阶段 // 第一阶段:运动到X坐标 float x_distance = target_x - task_namespace.current_x; go_straight((int)fabs(x_distance)); // 使用绝对值距离 // 更新当前坐标 task_namespace.current_x = target_x; // 第二阶段:旋转90度(方向根据Y坐标位置决定) int direction = (target_y > task_namespace.current_y) ? 1 : -1; turn_90_degrees(direction); // 第三阶段:运动到Y坐标 float y_distance = target_y - task_namespace.current_y; go_straight((int)fabs(y_distance)); // 使用绝对值距离 // 更新当前坐标 task_namespace.current_y = target_y; } // 旋转90度函数(使用编码器计算) void turn_90_degrees(int direction) { // 保存旋转开始时的编码器值 task_namespace.rotation_start_left = left_encoder; task_namespace.rotation_start_right = right_encoder; // 计算目标旋转量(90度 = π/2 弧度) // 假设轮距为120mm(根据实际小车尺寸调整) #define WHEEL_BASE 80.0f task_namespace.target_rotation = (direction > 0) ? (M_PI/2) : (-M_PI/2); // 设置状态为旋转 task_namespace.doing_what = TURN_90_DEGREES; task_namespace.finish = 0; } // 声光提示函数 void sound_light_alert(void) { DL_GPIO_togglePins(LED_PORT, LED_led_PIN); DL_GPIO_setPins(BUZZY_PORT, BUZZY_PIN_PIN); uint32_t start_time = hsu_time_get_ms(); while (hsu_time_get_ms() - start_time < 1000) { // 空循环等待1秒 } //hsu_time_delay_ms(200); DL_GPIO_togglePins(LED_PORT, LED_led_PIN); DL_GPIO_clearPins(BUZZY_PORT, BUZZY_PIN_PIN); } // callback void refresh_oled(void) { show_task_now(); OLED_ShowString(2, 1, "state:"); if (task_namespace.running_state) { OLED_ShowString(2, 7, "1"); // 运行中 } else { OLED_ShowString(2, 7, "0"); // 停止 } } uint32_t key_get_tick_ms(void) { return hsu_time_get_ms(); } void key(void) { key_event_t event = key_scan(); //uint8_t key_value = key_read_pin(); // 获取按键状态 //S1 switch (event) { case KEY_EVENT_SINGLE_CLICK: next_state(&task_namespace); break; case KEY_EVENT_DOUBLE_CLICK: task_namespace.is = 1; task_namespace.running_state = 1; task_namespace.is_running = 0; // 重置任务运行标志 break; } }
最新发布
07-21
<think>我们注意到在代码中,`go_to_xy` 函数被设计为执行三个阶段:直线运动到目标X坐标,旋转90度,再直线运动到目标Y坐标。但是,根据问题描述,执行时只执行了第一阶段。我们需要修改这个函数,使其能够分阶段执行,并且使用状态机(例如switch)来管理每个阶段。 问题分析: 当前 `go_to_xy` 函数试图在一个函数调用中完成三个阶段,但这样会导致它无法在非阻塞的方式下工作(即需要在一个循环中分步执行)。因此,我们需要将 `go_to_xy` 拆分成多个阶段,并在任务状态机中逐步执行。 解决方案: 1. 修改 `go_to_xy` 函数,将其拆分为多个阶段(移动到X、旋转、移动到Y),并在任务状态机中管理这些阶段。 2. 在 `TaskNamespace` 结构中添加必要的状态变量,用于存储中间状态(如当前阶段、目标坐标、当前坐标等)。 3. 在 `main_task` 中根据当前阶段调用相应的动作,并更新阶段。 然而,观察现有的代码结构,我们已经在 `TaskNamespace` 中定义了 `sub_task_stage` 用于管理子任务阶段。因此,我们可以利用这个机制。 具体修改步骤: 步骤1:在 `TaskNamespace` 中添加字段来存储目标坐标和当前坐标(已经存在,但需要正确初始化): float target_x; // 目标X坐标 (毫米) float target_y; // 目标Y坐标 (毫米) float current_x; // 当前X坐标 (毫米) float current_y; // 当前Y坐标 (毫米) 步骤2:修改 `go_to_xy` 函数,使其不直接执行动作,而是设置任务状态机的目标坐标,并启动第一个阶段(移动到X坐标)。同时,重置子任务阶段。 步骤3:在任务2(task_2)中,我们调用 `go_to_xy(3000.0f, 3000.0f)`,这应该设置目标坐标,并开始执行。 步骤4:在 `main_task` 中,当任务处于 `TURN_90_DEGREES` 状态时,我们执行旋转控制。同样,我们需要为移动到X和移动到Y分别设置状态(例如 `GO_STRAIGHT` 状态)。 但是,我们注意到现有的状态 `doing_what` 已经包含了 `GO_STRAIGHT` 和 `TURN_90_DEGREES`,因此我们可以利用这个状态。 步骤5:我们需要在 `TaskNamespace` 中记录当前是移动到X还是移动到Y,或者旋转。我们可以使用 `sub_task_stage` 来记录当前阶段: 阶段0: 移动到目标X(直线运动) 阶段1: 旋转90度(根据Y坐标方向决定顺时针还是逆时针) 阶段2: 移动到目标Y(直线运动) 阶段3: 完成 步骤6:在 `main_task` 的循环中,当 `doing_what` 为 `STOP` 且当前子任务阶段未完成时,根据子任务阶段启动下一个动作。 但是,我们现有的任务2(task_2)已经是一个状态机,它会在每个子阶段完成后触发下一个动作。因此,我们可以在任务2中按阶段调用动作。 然而,问题描述中只执行了第一阶段,可能是因为在 `go_to_xy` 函数中连续调用了多个动作,而每个动作都是非阻塞的,需要等待完成。但我们的代码结构是:在 `task_2` 中,我们一次设置了 `go_to_xy`,然后等待任务完成标志,然后进入下一个阶段(亮灯)。但是,`go_to_xy` 函数内部并没有分阶段执行,而是一次性设置了多个动作,但我们的系统设计是每次只能执行一个动作(通过 `doing_what` 状态)。 因此,我们需要重新设计 `go_to_xy` 函数,使其只启动第一阶段(移动到X),然后当第一阶段完成后,再启动第二阶段(旋转),以此类推。 修改后的计划: 在 `task_2` 中,我们不再直接调用 `go_to_xy`,而是将目标坐标保存到任务命名空间,然后设置子任务阶段为0(移动到X)。然后,在 `main_task` 中,当任务2运行时,根据子任务阶段执行相应的动作,并在每个动作完成后进入下一阶段。 具体修改: 1. 修改 `task_2` 函数,在子任务阶段0时,设置目标坐标,并启动第一阶段(移动到X坐标)。 2. 在 `main_task` 中,当检测到任务2的当前动作完成(`task_namespace.finish` 为1)时,进入下一阶段。 由于时间关系,我们重新设计任务2的执行流程: 在任务2中: - 子任务阶段0:设置目标坐标(3000,3000),然后启动移动到X坐标(直线运动),设置 `doing_what` 为 `GO_STRAIGHT`,并设置目标距离(绝对值)。然后等待完成(`finish` 由动作函数设置)。 - 当第一阶段完成(`finish`=1),进入子任务阶段1:旋转90度(根据Y坐标的方向,如果目标Y大于当前Y则顺时针,否则逆时针)。设置 `doing_what` 为 `TURN_90_DEGREES`,并等待完成。 - 完成旋转后,进入子任务阶段2:移动到Y坐标(直线运动),设置 `doing_what` 为 `GO_STRAIGHT`,目标距离为Y方向的距离(绝对值)。 - 完成后,进入阶段3:亮灯,然后任务完成。 注意:在每次设置动作后,需要将 `finish` 重置为0,以等待动作完成。 但是,我们现有的代码中,任务2的状态机已经是在每个子任务阶段完成后才进入下一个阶段。因此,我们可以在每个阶段设置相应的动作,并等待动作完成。 修改 `task_2` 函数如下: ```c void task_2(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; // 让状态机立即进入阶段0 // 设置目标坐标 task_namespace.target_x = 3000.0f; task_namespace.target_y = 3000.0f; // 初始化当前坐标(假设从(0,0)开始) task_namespace.current_x = 0.0f; task_namespace.current_y = 0.0f; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 运动到X坐标 { float x_distance = task_namespace.target_x - task_namespace.current_x; go_straight((int)fabs(x_distance)); // 使用绝对值距离 task_namespace.finish = 0; // 等待动作完成 } break; case 1: // 旋转90度 { int direction = (task_namespace.target_y > task_namespace.current_y) ? 1 : -1; turn_90_degrees(direction); task_namespace.finish = 0; // 等待动作完成 } break; case 2: // 运动到Y坐标 { float y_distance = task_namespace.target_y - task_namespace.current_y; go_straight((int)fabs(y_distance)); task_namespace.finish = 0; // 等待动作完成 } break; case 3: // 完成灯亮 DL_GPIO_togglePins(LED_PORT, LED_led_PIN); task_namespace.finish = 0; // 立即完成 break; case 4: // 任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } // 每次处理完一个阶段,子任务阶段增加 task_namespace.sub_task_stage++; } } ``` 同时,我们需要修改 `go_straight` 和 `turn_90_degrees` 函数,使它们在动作完成时设置 `task_namespace.finish = 1`。 在 `main_task` 中,当 `doing_what` 为 `GO_STRAIGHT` 时,当直线运动完成(编码器达到目标值)时,设置 `task_namespace.finish = 1`。同样,当 `doing_what` 为 `TURN_90_DEGREES` 时,旋转完成后设置 `task_namespace.finish = 1`。 在 `main_task` 的 `GO_STRAIGHT` 分支中,当完成时: task_namespace.doing_what = STOP; task_namespace.finish = 1; // 通知任务2的当前子阶段完成 在 `TURN_90_DEGREES` 分支中,当完成时同样设置。 但是,注意:在 `main_task` 中,当动作完成后,我们将 `doing_what` 设置为 `STOP`,并设置 `task_namespace.finish = 1`。这样,任务2的状态机就会进入下一个子阶段。 因此,我们只需要确保在动作函数中正确设置了目标,并在 `main_task` 中检测到动作完成时设置 `finish=1`。 我们已经在 `main_task` 的 `GO_STRAIGHT` 分支中,当完成时设置了 `task_namespace.finish = 1`。同样,在 `TURN_90_DEGREES` 分支中,完成时也设置 `task_namespace.finish = 1`。 但是,我们注意到在 `main_task` 中,当 `GO_STRAIGHT` 完成时,我们设置了: task_namespace.doing_what = STOP; task_namespace.finish = 1; 在 `TURN_IN_PLACE` 中(目前 `turn_90_degrees` 使用的是 `TURN_IN_PLACE` 状态?)我们并没有看到设置 `task_namespace.finish=1` 的代码。实际上,我们之前实现的 `turn_in_place` 函数使用的是 `TURN_IN_PLACE` 状态,而新实现的 `turn_90_degrees` 使用的是 `TURN_90_DEGREES` 状态。我们需要在 `main_task` 中添加对 `TURN_90_DEGREES` 的处理,并在完成时设置 `finish=1`。 然而,在现有的 `main_task` 中,我们有一个 `TURN_90_DEGREES` 的分支,在完成时已经设置了: task_namespace.doing_what = STOP; task_namespace.finish = 1; 因此,我们只需要确保 `turn_90_degrees` 函数设置 `doing_what` 为 `TURN_90_DEGREES` 即可。 但是,我们注意到 `turn_90_degrees` 函数已经设置了: task_namespace.doing_what = TURN_90_DEGREES; 所以,按照上述修改,任务2应该可以分阶段执行。 但是,问题描述中只执行了第一阶段,可能是因为在 `task_2` 中,阶段0调用 `go_straight` 后立即将 `sub_task_stage` 增加到了1,而没有等待动作完成。这是因为我们在 `task_2` 中,在调用 `go_straight` 之后,立即将 `task_namespace.finish` 设置为0(表示动作未完成),然后等待 `main_task` 中动作完成时设置 `finish=1`。当 `task_2` 再次运行(因为 `finish` 为0,所以不会进入下一个阶段)直到动作完成。 因此,修改后的 `task_2` 应该可以工作。 但是,我们还需要注意坐标的更新:在移动到X坐标后,当前X坐标应该更新为目标X坐标;旋转90度后,方向改变,但不改变坐标;移动到Y坐标后,更新当前Y坐标。 所以,在 `task_2` 的阶段0完成后,我们应该更新当前X坐标: task_namespace.current_x = task_namespace.target_x; 同样,在阶段2完成后,更新当前Y坐标: task_namespace.current_y = task_namespace.target_y; 修改后的 `task_2` 如下: ```c void task_2(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; // 让状态机立即进入阶段0 // 设置目标坐标 task_namespace.target_x = 3000.0f; task_namespace.target_y = 3000.0f; // 初始化当前坐标(假设从(0,0)开始) task_namespace.current_x = 0.0f; task_namespace.current_y = 0.0f; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: // 运动到X坐标 { float x_distance = task_namespace.target_x - task_namespace.current_x; go_straight((int)fabs(x_distance)); // 使用绝对值距离 task_namespace.finish = 0; // 等待动作完成 } break; case 1: // 旋转90度 { int direction = (task_namespace.target_y > task_namespace.current_y) ? 1 : -1; turn_90_degrees(direction); task_namespace.finish = 0; // 等待动作完成 } break; case 2: // 运动到Y坐标 { float y_distance = task_namespace.target_y - task_namespace.current_y; go_straight((int)fabs(y_distance)); task_namespace.finish = 0; // 等待动作完成 } break; case 3: // 更新坐标并亮灯 task_namespace.current_x = task_namespace.target_x; task_namespace.current_y = task_namespace.target_y; DL_GPIO_togglePins(LED_PORT, LED_led_PIN); task_namespace.finish = 0; // 立即进入下一阶段 break; case 4: // 任务结束 sound_light_alert(); reset_task_namespace(&task_namespace); break; } // 每次处理完一个阶段,子任务阶段增加 task_namespace.sub_task_stage++; } } ``` 注意:在阶段3,我们更新坐标,然后亮灯,并立即进入阶段4(任务结束)。 这样,任务2就可以分阶段执行了。 但是,我们还需要确保在 `main_task` 中,当 `GO_STRAIGHT` 完成时,更新编码器位置,并设置 `finish=1`,这样 `task_2` 才能进入下一阶段。 我们已经实现了这一点。 因此,按照上述修改,`go_to_xy` 应该能够执行全部三个阶段。 由于代码较长,我们只修改了 `task_2` 函数,并确保 `main_task` 中已经正确处理了各个动作状态的完成标志。 注意:在 `go_straight` 函数中,我们设置的是绝对编码器目标值(`task_namespace.target = left_encoder + dis;`),所以是相对于当前位置的增量。 另外,旋转90度函数 `turn_90_degrees` 的实现我们使用编码器差值计算旋转角度,并设置目标旋转量(弧度),然后在 `main_task` 的 `TURN_90_DEGREES` 分支中控制直到完成。 最后,由于问题中要求使用别的方法,但我们已经使用了状态机方法,符合要求。 如果问题仍然存在,请检查: - 在 `main_task` 的 `TURN_90_DEGREES` 分支中,是否在完成时设置了 `task_namespace.finish=1`。 - 在 `main_task` 的 `GO_STRAIGHT` 分支中,是否在完成时设置了 `task_namespace.finish=1`。 我们已经在代码中看到这些设置。 因此,修改后的代码应该可以工作。 由于我们无法直接运行硬件,请根据上述分析修改代码并测试。 注意:以上代码修改是基于您提供的代码框架,如果有其他细节问题,请根据实际情况调整。 由于篇幅限制,这里只给出了 `task_2` 函数的修改代码,其他部分(如 `main_task` 中的状态处理)已经存在,无需修改。 如果问题解决,请确认;如果仍有问题,请提供更多信息。</think>您的代码中 `go_to_xy` 函数只执行了第一阶段的问题在于该函数尝试一次性完成所有动作(移动X、旋转、移动Y),但您的任务系统设计是基于状态机的,需要分阶段执行。以下是完整的解决方案: ### 解决方案: 我们需要将 `go_to_xy` 函数拆分为多个阶段,并使用 `TaskNamespace` 的状态管理每个阶段。以下是修改后的代码: ```c // 在 TaskNamespace 结构体中添加新字段 typedef struct __TASK_NAMESPACE { // ... 其他字段保持不变 ... float target_x; // 目标X坐标 (毫米) float target_y; // 目标Y坐标 (毫米) float current_x; // 当前X坐标 (毫米) float current_y; // 当前Y坐标 (毫米) int go_to_xy_stage; // XY移动阶段 (0: 未开始, 1: 移动到X, 2: 旋转, 3: 移动到Y) } TaskNamespace; // 初始化时重置坐标 void reset_task_namespace(TaskNamespace *t) { // ... 其他初始化 ... t->target_x = 0.0f; t->target_y = 0.0f; t->current_x = 0.0f; t->current_y = 0.0f; t->go_to_xy_stage = 0; } // 修改后的 go_to_xy 函数 void go_to_xy(float target_x, float target_y) { task_namespace.target_x = target_x; task_namespace.target_y = target_y; task_namespace.go_to_xy_stage = 1; // 开始第一阶段 task_namespace.doing_what = GO_STRAIGHT; // 计算X方向距离 float x_distance = target_x - task_namespace.current_x; task_namespace.target = left_encoder + (int)fabs(x_distance); } // 在 main_task 中添加对XY移动阶段的处理 void main_task(void) { // ... 其他代码 ... // 处理XY移动的不同阶段 if (task_namespace.go_to_xy_stage > 0) { switch (task_namespace.go_to_xy_stage) { case 1: // 移动到X坐标 if (task_namespace.finish) { // 更新当前坐标 task_namespace.current_x = task_namespace.target_x; // 准备旋转 int direction = (task_namespace.target_y > task_namespace.current_y) ? 1 : -1; turn_90_degrees(direction); task_namespace.go_to_xy_stage = 2; task_namespace.finish = 0; } break; case 2: // 旋转90度 if (task_namespace.finish) { // 准备移动到Y坐标 float y_distance = task_namespace.target_y - task_namespace.current_y; go_straight((int)fabs(y_distance)); task_namespace.go_to_xy_stage = 3; task_namespace.finish = 0; } break; case 3: // 移动到Y坐标 if (task_namespace.finish) { // 更新当前坐标 task_namespace.current_y = task_namespace.target_y; task_namespace.go_to_xy_stage = 0; // 完成所有阶段 task_namespace.finish = 1; // 通知任务完成 } break; } } // ... 其他代码 ... } // 修改任务2的实现 void task_2(void) { if (!task_namespace.is_running) { task_namespace.is_running = 1; task_namespace.sub_task_stage = 0; task_namespace.finish = 1; // 初始化坐标 task_namespace.current_x = 0.0f; task_namespace.current_y = 0.0f; return; } if (task_namespace.finish) { switch (task_namespace.sub_task_stage) { case 0: go_to_xy(3000.0f, 3000.0f); task_namespace.finish = 0; // 等待完成 break; case 1: DL_GPIO_togglePins(LED_PORT, LED_led_PIN); task_namespace.finish = 0; break; case 2: sound_light_alert(); reset_task_namespace(&task_namespace); break; } task_namespace.sub_task_stage++; } } ``` ### 代码解释: 1. **状态跟踪**: - 添加了 `go_to_xy_stage` 跟踪XY移动的3个阶段 - 添加了 `current_x/y` 和 `target_x/y` 存储坐标信息 2. **分阶段执行**: - **阶段1**:沿X轴移动到目标位置 - **阶段2**:旋转90度(方向由Y坐标决定) - **阶段3**:沿Y轴移动到目标位置 3. **任务协调**: - 每个阶段完成后设置 `finish=1` 通知主任务 - 主任务检测到完成标志后启动下一阶段 - 所有阶段完成后设置全局 `finish=1` 4. **坐标管理**: - 移动完成后更新当前坐标 - 确保每次移动都是相对于当前位置 ### 使用说明: 1. 调用 `go_to_xy(target_x, target_y)` 启动过程 2. 系统会自动分三个阶段执行 3. 任务2的调用方式保持不变 ### 改进后的 `turn_90_degrees` 函数: ```c void turn_90_degrees(int direction) { task_namespace.rotation_start_left = left_encoder; task_namespace.rotation_start_right = right_encoder; task_namespace.target_rotation = (direction > 0) ? (M_PI/2) : (-M_PI/2); task_namespace.doing_what = TURN_90_DEGREES; task_namespace.finish = 0; } ``` ### 在OLED显示坐标信息: ```c void refresh_oled(void) { char buf[16]; // 显示当前坐标 sprintf(buf, "X:%.0f", task_namespace.current_x); OLED_ShowString(4, 1, buf); sprintf(buf, "Y:%.0f", task_namespace.current_y); OLED_ShowString(4, 6, buf); // 显示移动阶段 sprintf(buf, "Stage:%d", task_namespace.go_to_xy_stage); OLED_ShowString(5, 1, buf); } ``` 这个解决方案确保: 1. XY移动分三个阶段顺序执行 2. 每个阶段完成后才启动下一阶段 3. 坐标系统正确跟踪当前位置 4. 与现有状态机架构兼容
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值