image.png

freertos初始化

  1. void MX_FREERTOS_Init(void) {
  2.   /* USER CODE BEGIN Init */
  3.     xTaskCreate(OpenLoopPosControlTask, "OpenPos", 64, NULL, 6, &openLoopPosControlTaskHandle);
  4.     vTaskSuspend(openLoopPosControlTaskHandle);
  6.     xTaskCreate(OpenLoopVelControlTask, "OpenVel", 64, NULL, 6, &openLoopVelControlTaskHandle);
  7.     vTaskSuspend(openLoopVelControlTaskHandle);
  9.     xTaskCreate(TorqueControlTask, "TorCtrl", 128, NULL, 6, &torqueControlTaskHandle);
  10.     vTaskSuspend(torqueControlTaskHandle);
  12.     xTaskCreate(VelocityControlTask, "VelCtrl", 64, NULL, 6, &velocityControlTaskHandle);
  13.     vTaskSuspend(velocityControlTaskHandle);
  15.     xTaskCreate(PositionControlTask, "PosCtrl", 64, NULL, 6, &positionControlTaskHandle);
  16.     vTaskSuspend(positionControlTaskHandle);
  18.     xTaskCreate(SpringTask, "Spring", 64, NULL, 6, &springTaskHandle);
  19.     vTaskSuspend(springTaskHandle);
  21.     xTaskCreate(KnobTask, "Knob", 64, NULL, 6, &knobTaskHandle);
  22.     vTaskSuspend(knobTaskHandle);
  24.     xTaskCreate(TaskSelectTask, "Select", 64, NULL, 6, &taskSelectTaskHandle);
  26.   /* USER CODE END Init */
  28.   /* USER CODE BEGIN RTOS_MUTEX */
  29.     /* add mutexes, ... */
  30.   /* USER CODE END RTOS_MUTEX */
  32.   /* USER CODE BEGIN RTOS_SEMAPHORES */
  33.     /* add semaphores, ... */
  34.   /* USER CODE END RTOS_SEMAPHORES */
  36.   /* USER CODE BEGIN RTOS_TIMERS */
  37.     /* start timers, add new ones, ... */
  38.   /* USER CODE END RTOS_TIMERS */
  40.   /* USER CODE BEGIN RTOS_QUEUES */
  41.     /* add queues, ... */
  42.   /* USER CODE END RTOS_QUEUES */
  44.   /* Create the thread(s) */
  45.   /* definition and creation of defaultTask */
  46.   osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 64);
  47.   defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
  49.   /* USER CODE BEGIN RTOS_THREADS */
  50.     /* add threads, ... */
  51.   /* USER CODE END RTOS_THREADS */
  53. }

多任务

  1. void TaskSelectTask(void *argument) {
  2.     FOC_CONTROL_MODE mode = OPEN_LOOP_POSITION_CONTROL;
  3.     portENTER_CRITICAL();
  4.     gui_draw_mode_selection(mode);
  5.     portEXIT_CRITICAL();
  6.     TickType_t xLastWakeTime = xTaskGetTickCount();
  8.     HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
  9.     while (1) {
  10.         if (button_press_pending_flag) {
  11.             if (button_left_press_pending_flag) {
  12.                 if (mode) mode--;
  13.                 else mode = FOC_CONTROL_MODE_NUM - 1;
  14.                 gui_draw_mode_selection(mode);
  16.             } else if (button_right_press_pending_flag) {
  17.                 mode++;
  18.                 if (mode > FOC_CONTROL_MODE_NUM - 1) mode = 0;
  19.                 gui_draw_mode_selection(mode);
  21.             } else if (button_confirm_press_pending_flag) {
  22.                 switch (mode) {
  23.                     case OPEN_LOOP_POSITION_CONTROL:
  24.                         SuspendToRunOtherTask(openLoopPosControlTaskHandle);
  25.                         break;
  26.                     case OPEN_LOOP_SPEED_CONTROL:
  27.                         SuspendToRunOtherTask(openLoopVelControlTaskHandle);
  28.                         break;
  29.                     case TORQUE_CONTROL:
  30.                         SuspendToRunOtherTask(torqueControlTaskHandle);
  31.                         break;
  32.                     case SPEED_CONTROL:
  33.                         SuspendToRunOtherTask(velocityControlTaskHandle);
  34.                         break;
  35.                     case POSITION_CONTROL:;
  36.                         SuspendToRunOtherTask(positionControlTaskHandle);
  37.                         break;
  38.                     case SPRING:
  39.                         spring_pid = &pid_spring;
  40.                         SuspendToRunOtherTask(springTaskHandle);
  41.                         break;
  42.                     case SPRING_WITH_DAMP:
  43.                         spring_pid = &pid_spring_with_damp;
  44.                         SuspendToRunOtherTask(springTaskHandle);
  45.                         break;
  46.                     case DAMP:
  47.                         spring_pid = &pid_damp;
  48.                         SuspendToRunOtherTask(springTaskHandle);
  49.                         break;
  50.                     case KNOB:
  51.                         SuspendToRunOtherTask(knobTaskHandle);
  52.                         break;
  53.                     case ZERO_RESISTANCE:
  54.                         spring_pid = &pid_zero_resistance;
  55.                         SuspendToRunOtherTask(springTaskHandle);
  56.                         break;
  57.                 }
  58.                 gui_draw_mode_selection(mode);
  59.                 continue;
  60.             }
  62.             button_reset_all_flags();
  63.         }
  66.         vTaskDelayUntil(&xLastWakeTime,
  67.                         10);
  68.     }
  69. }

每一个任务函数

  1. void OpenLoopPosControlTask(void *argument) {
  2.     float angle = 0, Ud = 1;
  3.     TickType_t xLastWakeTime = xTaskGetTickCount();
  5.     while (1) {
  6.         if (button_press_pending_flag) {
  7.             if (button_left_press_pending_flag) {
  8.                 angle -= _PI_2;
  9.             } else if (button_right_press_pending_flag) {
  10.                 angle += _PI_2;
  11.             } else if (button_cancel_press_pending_flag) {
  12.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  13.                 continue;
  14.             }
  15.             button_reset_all_flags();
  16.         }
  18.         FOC_SVPWM(0, Ud, angle);
  19.         vTaskDelayUntil(&xLastWakeTime,
  20.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  21.     }
  22. }
  24. void OpenLoopVelControlTask(void *argument) {
  25.     float angle = 0, Ud = 1, delta_angle = 0;
  26.     TickType_t xLastWakeTime = xTaskGetTickCount();
  27.     while (1) {
  28.         if (button_press_pending_flag) {
  29.             if (button_left_press_pending_flag) {
  30.                 delta_angle -= 0.01;
  31.                 if (delta_angle < -_PI) delta_angle = -_PI;
  32.             } else if (button_right_press_pending_flag) {
  33.                 delta_angle += 0.01;
  34.                 if (delta_angle > _PI) delta_angle = _PI;
  35.             } else if (button_cancel_press_pending_flag) {
  36.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  37.                 continue;
  38.             }
  39.             button_reset_all_flags();
  40.         }
  41.         angle += delta_angle;
  42.         FOC_SVPWM(0, Ud, angle);
  43.         vTaskDelayUntil(&xLastWakeTime,
  44.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  45.     }
  46. }
  48. #ifdef FOC_MODE_VOLTAGE_CONTROL
  49. void TorqueControlTask(void *argument) {
  50.     TickType_t xLastWakeTime = xTaskGetTickCount();
  51.     float Uq = 0;
  53.     while (1) {
  54.         if (button_press_pending_flag) {
  55.             if (button_left_press_pending_flag) {
  56.                 Uq -= 1.0;
  57.             } else if (button_right_press_pending_flag) {
  58.                 Uq += 1.0;
  59.             } else if (button_cancel_press_pending_flag) {
  60.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  61.                 continue;
  62.             }
  63.             button_reset_all_flags();
  64.         }
  65.         FOC_open_loop_voltage_control_loop(Uq);
  67.         vTaskDelayUntil(&xLastWakeTime,
  68.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  69.     }
  70. }
  71. #endif
  72. #ifdef FOC_MODE_CURRENT_CONTROL
  73. void TorqueControlTask(void *argument) {
  74.     TickType_t xLastWakeTime = xTaskGetTickCount();
  75.     float Iq = 0;
  77.     while (1) {
  78.         if (button_press_pending_flag) {
  79.             if (button_left_press_pending_flag) {
  80.                 Iq -= 100.0;
  81.             } else if (button_right_press_pending_flag) {
  82.                 Iq += 100.0;
  83.             } else if (button_cancel_press_pending_flag) {
  84.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  85.                 continue;
  86.             }
  87.             button_reset_all_flags();
  88.         }
  89.         FOC_current_control_loop(Iq);
  91.         //vTaskDelayUntil(&xLastWakeTime,
  92. //                        1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  93.     }
  94. }
  95. #endif
  97. void VelocityControlTask(void *argument) {
  98.     float velocity = 0;
  99.     TickType_t xLastWakeTime = xTaskGetTickCount();
  101.     while (1) {
  102.         if (button_press_pending_flag) {
  103.             if (button_left_press_pending_flag) {
  104.                 velocity -= 5;
  105.             } else if (button_right_press_pending_flag) {
  106.                 velocity += 5;
  107.             } else if (button_cancel_press_pending_flag) {
  108.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  109.                 continue;
  110.             }
  111.             button_reset_all_flags();
  112.         }
  115.         FOC_velocity_control_loop(velocity);
  116.         vTaskDelayUntil(&xLastWakeTime,
  117.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  118.     }
  119. }
  121. void PositionControlTask(void *argument) {
  122.     float angle = 0;
  123.     TickType_t xLastWakeTime = xTaskGetTickCount();
  124.     gui_draw_position_mode(angle, 1);
  126.     while (1) {
  127.         if (button_press_pending_flag) {
  128.             if (button_left_press_pending_flag) {
  129.                 angle -= _PI_2;
  130.             } else if (button_right_press_pending_flag) {
  131.                 angle += _PI_2;
  132.             } else if (button_cancel_press_pending_flag) {
  133.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  134.                 gui_draw_position_mode(angle, 1);
  135.                 continue;
  136.             }
  137.             button_reset_all_flags();
  138.             gui_draw_position_mode(angle, 0);
  139.         }
  141.         FOC_position_control_loop(angle);
  142.         vTaskDelayUntil(&xLastWakeTime,
  143.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  144.     }
  145. }
  147. void SpringTask(void *argument) {
  148.     TickType_t xLastWakeTime = xTaskGetTickCount();
  149.     while (1) {
  150.         if (button_press_pending_flag) {
  151.             if (button_cancel_press_pending_flag) {
  152.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  153.                 continue;
  154.             }
  155.             button_reset_all_flags();
  156.         }
  157.         FOC_spring_loop(0, spring_pid);
  158.         vTaskDelayUntil(&xLastWakeTime,
  159.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  160.     }
  161. }
  163. void DampTask(void *argument) {
  165. }
  167. void KnobTask(void *argument) {
  168.     TickType_t xLastWakeTime = xTaskGetTickCount();
  170.     // Settings
  171.     uint8_t select_param = 0;
  172.     const uint8_t max_select_param = 3;
  173.     uint8_t change_param = 0;
  175.     uint8_t values[3] = {4, 5, 4}; // [0]: sector_num [1]: k [2]: max_force
  177.     pid_knob.Kp = values[1];
  178.     pid_knob.max_u = values[2];
  179.     pid_knob.min_u = -values[2];
  180.     gui_draw_knob_mode(values[0], values[1], values[2], select_param, change_param, 1);
  182.     while (1) {
  183.         if (button_press_pending_flag) {
  184.             if (button_confirm_press_pending_flag) {
  185.                 if(select_param){
  186.                     change_param = !change_param;
  187.                 }
  188.             } else if (button_left_press_pending_flag) {
  189.                 if(change_param){
  190.                     values[select_param - 1] --;
  192.                     // reconfigure
  193.                     pid_knob.Kp = values[1];
  194.                     pid_knob.max_u = values[2];
  195.                     pid_knob.min_u = -values[2];
  196.                     gui_draw_knob_mode(values[0], values[1], values[2], select_param, change_param, 0);
  197.                 }else{
  198.                     if (select_param == 0) {
  199.                         select_param = max_select_param;
  200.                     } else select_param--;
  201.                 }
  202.             } else if (button_right_press_pending_flag) {
  203.                 if(change_param){
  204.                     values[select_param - 1] ++;
  206.                     // reconfigure
  207.                     pid_knob.Kp = values[1];
  208.                     pid_knob.max_u = values[2];
  209.                     pid_knob.min_u = -values[2];
  210.                     gui_draw_knob_mode(values[0], values[1], values[2], select_param, change_param, 0);
  211.                 }else{
  212.                     if (select_param == max_select_param) {
  213.                         select_param = 0;
  214.                     } else select_param++;
  215.                 }
  216.             } else if (button_cancel_press_pending_flag) {
  217.                 SuspendToRunOtherTask(taskSelectTaskHandle);
  218.                 gui_draw_knob_mode(values[0], values[1], values[2], select_param, change_param, 1);
  219.                 continue;
  220.             }
  221.             button_reset_all_flags();
  222.             gui_draw_knob_mode(values[0], values[1], values[2], select_param, change_param, 0);
  223.         }
  225.         FOC_knob_loop(values[0]);
  226.         vTaskDelayUntil(&xLastWakeTime,
  227.                         1); // every FOC control task need at least 1ms delay otherwise cannot detect key press normally
  228.     }
  229. }
  231. /* USER CODE END Application */