多线程之Task(任务)

一、介绍下Task

对于多线程,我们经常使用的是Thread。在我们了解Task之前,如果我们要使用多核的功能可能就会自己来开线程,然而这种线程模型在.net 4.0之后被一种称为基于“任务的编程模型”所冲击,因为task会比thread具有更小的性能开销,不过大家肯定会有疑惑,任务和线程到底有什么区别呢?

任务和线程的区别:
1、任务是架构在线程之上的,也就是说任务最终还是要抛给线程去执行。

2、任务跟线程不是一对一的关系,比如开10个任务并不是说会开10个线程,这一点任务有点类似线程池,但是任务相比线程池有很小的开销和精确的控制。
3、Task的优势
  ThreadPool相比Thread来说具备了很多优势,但是ThreadPool却又存在一些使用上的不方便。比如:
  ◆ ThreadPool不支持线程的取消、完成、失败通知等交互性操作;
  ◆ ThreadPool不支持线程执行的先后次序;
  以往,如果开发者要实现上述功能,需要完成很多额外的工作,现在,微软提供了一个功能更强大的概念:Task。Task在线程池的基础上进行了优化,并提供了更多的API。在Framework 4.0中,如果我们要编写多线程程序,Task显然已经优于传统的方式。
  以下是一个简单的任务示例:

using System;
using System.Threading;
using System.Threading.Tasks;

namespace ConsoleApp1
{
    class Program
    {
        static void Main(string[] args)
        {
            Task t = new Task(() =>
            {
                Console.WriteLine("任务开始工作……");
                //模拟工作过程
                Thread.Sleep(5000);
            });
            t.Start();
            t.ContinueWith((task) =>
            {
                Console.WriteLine("任务完成,完成时候的状态为:");
                Console.WriteLine("IsCanceled={0}\tIsCompleted={1}\tIsFaulted={2}", task.IsCanceled, task.IsCompleted, task.IsFaulted);
            });
            Console.ReadKey();
        }
    }
}

二、创建Task

创建Task的方法有两种,一种是直接创建——new一个出来,一种是通过工厂创建。下面来看一下这两种创建方法:

    //第一种创建方式,直接实例化
    Task task1 = new Task(() =>
     {
        //To Do you code  也可以在这直接调用方法,直接传递参数,也比较方便
     });

这是最简单的创建方法,可以看到其构造函数是一个Action,其构造函数有如下几种,比较常用的是前两种。

/第二种创建方式,工厂创建
         var task2 = Task.Factory.StartNew(() =>
         {
            //TODO you code
         });
这种方式通过静态工厂,创建一个Task并运行。

构造函数创建的task,必须手动Start,而通过工厂创建的Task直接就启动了。

下面我们来看一下Task的声明周期,编写如下代码:

var task1 = new Task(() =>
      {
        Console.WriteLine("Begin");
        System.Threading.Thread.Sleep(2000);
        Console.WriteLine("Finish");
      });
      Console.WriteLine("Before start:" + task1.Status);
      task1.Start();
      Console.WriteLine("After start:" + task1.Status);
      task1.Wait();
      Console.WriteLine("After Finish:" + task1.Status);

      Console.Read();

ask1.Status就是输出task的当前状态,其输出结果如下:
在这里插入图片描述
可以看到调用Start前的状态是Created,然后等待分配线程去执行,到最后执行完成。
从我们可以得出Task的简略生命周期:
Created:表示默认初始化任务,但是“工厂创建的”实例直接跳过。

WaitingToRun: 这种状态表示等待任务调度器分配线程给任务执行。

RanToCompletion:任务执行完毕。

三、Task的任务控制

Task最吸引人的地方就是他的任务控制了,你可以很好的控制task的执行顺序,让多个task有序的工作。下面来详细说一下:

1、Task.Wait
在上个例子中,我们已经使用过了,task1.Wait();就是等待任务执行完成,我们可以看到最后task1的状态变为Completed。

2、Task.WaitAll
看字面意思就知道,就是等待所有的任务都执行完成,下面我们来写一段代码演示一下:

 static void Main(string[] args)
    {
      var task1 = new Task(() =>
      {
        Console.WriteLine("Task 1 Begin");
        System.Threading.Thread.Sleep(2000);
        Console.WriteLine("Task 1 Finish");
      });
      var task2 = new Task(() =>
      {
        Console.WriteLine("Task 2 Begin");
        System.Threading.Thread.Sleep(3000);
        Console.WriteLine("Task 2 Finish");
      });
      
      task1.Start();
      task2.Start();
      Task.WaitAll(task1, task2);
      Console.WriteLine("All task finished!");

      Console.Read();
    }

其输出结果如下:
在这里插入图片描述
3、Task.WaitAny
这个用发同Task.WaitAll,就是等待任何一个任务完成就继续向下执行,将上面的代码WaitAll替换为WaitAny,输出结果如下:
在这里插入图片描述
4、Task.ContinueWith
就是在第一个Task完成后自动启动下一个Task,实现Task的延续,下面我们来看下他的用法,编写如下代码:

static void Main(string[] args)
    {
      var task1 = new Task(() =>
      {
        Console.WriteLine("Task 1 Begin");
        System.Threading.Thread.Sleep(2000);
        Console.WriteLine("Task 1 Finish");
      });
      var task2 = new Task(() =>
      {
        Console.WriteLine("Task 2 Begin");
        System.Threading.Thread.Sleep(3000);
        Console.WriteLine("Task 2 Finish");
      });

      task1.Start();
      task2.Start();
      var result = task1.ContinueWith<string>(task =>
      {
        Console.WriteLine("task1 finished!");
        return "This is task result!";
      });
      
      Console.WriteLine(result.Result.ToString());
      Console.Read();
    }

在这里插入图片描述
可以看到,task1完成之后,开始执行后面的内容,并且这里我们取得task的返回值。

在每次调用ContinueWith方法时,每次会把上次Task的引用传入进来,以便检测上次Task的状态,比如我们可以使用上次Task的Result属性来获取返回值。我们还可以这么写:

Task.Factory.StartNew<string>(() => {return "One";}).ContinueWith(ss => { Console.WriteLine(ss.Result);});

输出One
要写可伸缩的软件,一定不能使你的线程阻塞。这意味着如果调用Wait或者在任务未完成时查询Result属性,极有可能造成线程池创建一个新线程,这增大了资源的消耗,并损害了伸缩性。

注意下面代码中TaskContinuationOptions 枚举,挺有意思的:

static void Main(string[] args)
        {
            Task<Int32> t = new Task<Int32>(i => Sum((Int32)i),10000);

            t.Start();

            t.ContinueWith(task=>Console.WriteLine("The sum is:{0}",task.Result),
                TaskContinuationOptions.OnlyOnRanToCompletion);
            
            t.ContinueWith(task=>Console.WriteLine("Sum throw:"+task.Exception),
                TaskContinuationOptions.OnlyOnFaulted);
           
            t.ContinueWith(task=>Console.WriteLine("Sum was cancel:"+task.IsCanceled),
                TaskContinuationOptions.OnlyOnCanceled);
            try
            {
                t.Wait();  // 测试用
            }
            catch (AggregateException)
            {
                Console.WriteLine("出错");
            }
        }

        private static Int32 Sum(Int32 i)
        {
            Int32 sum = 0;
            for (; i > 0; i--)
            {
                checked { sum += i; }
            }
            return sum;
        }
    }

AttachedToParnt枚举类型(父任务)也不能放过!看看怎么用,写法有点新奇,看看:

static void Main(string[] args)
        {
            Task<Int32[]> parent = new Task<Int32[]>(() => {
                var results = new Int32[3];
                //
                new Task(() => results[0] = Sum(10000), TaskCreationOptions.AttachedToParent).Start();
                new Task(() => results[1] = Sum(20000), TaskCreationOptions.AttachedToParent).Start();
                new Task(() => results[2] = Sum(30000), TaskCreationOptions.AttachedToParent).Start();
                return results;
            });

            var cwt = parent.ContinueWith( parentTask=>Array.ForEach(parentTask.Result,Console.WriteLine));
                   

            parent.Start();
            cwt.Wait();
        }

        private static Int32 Sum(Int32 i)
        {
            Int32 sum = 0;
            for (; i > 0; i--)
            {
                checked { sum += i; }
            }
            return sum;
        }
    }

例子中,父任务创建启动3个Task对象。默认情况下,一个任务创建的Task对象是顶级任务,这些任务跟创建它们的那个任务没有关系。

TaskCreationOptions.AttachedToParent标志将一个Task和创建它的那个Task关联起来,除非所有子任务(子任务的子任务)结束运行,否则创建任务(父任务)不会认为已经结束。调用ContinueWith方法创建一个Task时,可以指定TaskContinuationOptions.AttachedToParent标志将延续任务置顶为一个子任务。
在这里插入图片描述
  看了这么多任务的方法操作示例了,现在来挖挖任务内部构造:

每个Task对象都有一组构成任务状态的字段。

一个Int32 ID(只读属性)
1.代表Task执行状态的一个Int32
2.对父任务的一个引用
3.对Task创建时置顶TaskSchedule的一个引用
4.对回调方法的一个引用
5.对要传给回调方法的对象的一个引用(通过Task只读AsyncState属性查询)
6.对一个ExceptionContext的引用
7.对一个ManualResetEventSlim对象的引用
8.还有没个Task对象都有对根据需要创建的一些补充状态的一个引用,补充状态包含这些:
(1)一个CancellationToken
(2) 一个ContinueWithTask对象集合
(3)为抛出未处理异常的子任务,所准备的一个Task对象集合
  ContinueWith便是一个更好的方式,一个任务完成时它可以启动另一个任务。
更多ContinueWith用法参见:http://technet.microsoft.com/zh-CN/library/dd321405

5、Task的取消和异常处理
前面说了那么多Task的用法,下面来说下Task的取消,比如我们启动了一个task,出现异常或者用户点击取消等等,我们可以取消这个任务
 在一个线程调用Wait方法时,系统会检查线程要等待的Task是否已经开始执行,如果任务正在执行,那么这个Wait方法会使线程阻塞,直到Task运行结束为止。

在一个任务抛出一个未处理的异常时,这个异常会被“包含”不并存储到一个集合中,而线程池线程是允许返回到线程池中的,在调用Wait方法或者Result属性时,这个成员会抛出一个System.AggregateException对象。

现在你会问,为什么要调用Wait或者Result?或者一直不查询Task的Exception属性?你的代码就永远注意不到这个异常的发生,如果不能捕捉到这个异常,垃圾回收时,抛出AggregateException,进程就会立即终止,这就是“牵一发动全身”,莫名其妙程序就自己关掉了,谁也不知道这是什么情况。所以,必须调用前面提到的某个成员,确保代码注意到异常,并从异常中恢复。悄悄告诉你,其实在用Result的时候,内部会调用Wait。

怎么恢复?

为了帮助你检测没有注意到的异常,可以向TaskScheduler的静态UnobservedTaskException时间等级一个回调方法,当Task被垃圾回收时,如果出现一个没有被注意到的异常,CLR终结器会引发这个事件。
  
  一旦引发,就会向你的时间处理器方法传递一个UnobservedTaskExceptionEvenArgs对象,其中包含了你没有注意的AggregateException。然后再调用UnobservedTasExceptionEvenArgs的SetObserved方法来指出你的异常已经处理好了,从而阻止CLR终止进程。这是个图省事的做法,要少做这些,宁愿终止进程,也不要呆着已经损坏的状态而继续运行。

除了单个等待任务,Task 还提供了两个静态方法:WaitAny和WaitAll,他们允许线程等待一个Task对象数组。

WaitAny方法会阻塞调用线程,知道数组中的任何一个Task对象完成,这个方法会返回一个索引值,指明完成的是哪一个Task对象。如果发生超时,方法将返回-1。它可以通过一个CancellationToken取消,会抛出一个OperationCanceledException。

WaitAll方法也会阻塞调用线程,知道数组中的所有Task对象都完成,如果全部完成就返回true,如果超时就返回false。当然它也能取消,同样会抛出OperationCanceledException。

说了取消任务的方法,现在来试试这个方法,加深下印象,修改先前例子代码,完整代码如下:

static void Main(string[] args)
        {
            CancellationTokenSource cts = new CancellationTokenSource();
            
            Task<Int32> t = new Task<Int32>(() => Sum(cts.Token,10000), cts.Token);

            //可以现在开始,也可以以后开始 
            
            t.Start();

            //在之后的某个时间,取消CancellationTokenSource 以取消Task

            cts.Cancel();//这是个异步请求,Task可能已经完成了。我是双核机器,Task没有完成过

            //注释这个为了测试抛出的异常
            //Console.WriteLine("This sum is:" + t.Result);
            try
            {
                //如果任务已经取消了,Result会抛出AggregateException
                Console.WriteLine("This sum is:" + t.Result);
            }
            catch (AggregateException x)
            {
                //将任何OperationCanceledException对象都视为已处理。
                //其他任何异常都造成抛出一个AggregateException,其中
                //只包含未处理的异常

                x.Handle(e => e is OperationCanceledException);
                Console.WriteLine("Sum was Canceled");
            }
         
        }

        private static Int32 Sum(CancellationToken ct ,Int32 i)
        {
            Int32 sum = 0;
            for (; i > 0; i--)
            {
                //在取消标志引用的CancellationTokenSource上如果调用
                //Cancel,下面这一行就会抛出OperationCanceledException

                ct.ThrowIfCancellationRequested();

                checked { sum += i; }
            }
            
            return sum;
        }
    }

整理 自:https://www.cnblogs.com/tuyile006/p/7154924.html
https://www.e-learn.cn/content/net/1114080
https://www.cnblogs.com/wangchuang/p/5737188.html

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. 与现有状态机架构兼容
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值