[系统运维] 大疆A型板使用经验分享（八）—

开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 系统运维 -> 大疆A型板使用经验分享（八）——FreeRTOS操作系统的使用 -> 正文阅读

[系统运维]大疆A型板使用经验分享（八）——FreeRTOS操作系统的使用

一. freeRTOS操作系统

操作系统（operating system） 本质上是一个帮助用户进行功能管理的软件，操作系统运行在硬件之上，为其他工作的软件执行资源分配等管理工作。一般称呼不使用操作系统的单片机开发方式为“裸机开发”，当进行裸机开发时，需要自己设计循环，中断，定时等功能来控制各个任务的执行顺序。而使用操作系统进行开发时，只需要创建任务，操作系统会自动按照一些特定的机制自动进行任务的运行和切换。除了任务管理之外，操作系统还可以提供许多功能，比如各个任务之间的通信，同步，任务的堆栈管理，控制任务对重要资源的互斥访问等。

在操作系统，每一个要执行的任务，也就是一段程序的运行过程被称为一个进程。进程包含
着动态的概念，它是一个程序的运行过程，而不是一个静态的程序。进程体现在程序中形式
实际上就是一段循环执行的代码，例如，led_green 是一个让绿色 led 灯闪烁的
进程，使用操作系统的任务创建函数创建了这个进程之后，操作系统就自动找到这段代码并
执行。

一段程序执行时，一般划分成三个阶段，开始执行—>执行中—>执行完成。这也恰好对应了
进程的工作状态：就绪态（Ready）—>运行态（Running）—>终止态（Blocked）。如果在
一个进程执行的过程中，调用了将进程挂起的功能函数，或者是进程执行时有更高优先级的
任务就绪了，则进程会进入挂起状态（Suspended）。

操作系统的一个重要工作就是执行各个进程的状态切换，因为实际上单片机每次只能运行一
个进程，而操作系统通过适当的管理，让每一个进程都可以得到及时的响应，让多个进程呈
现出一种同时运行的“并发”感。

操作系统执行任务切换时必须得要遵循一定的调度算法，操作系统会根据能否将正在运行的
进程打断分为抢占式操作系统和合作式操作系统。

合作式操作系统不能够打断正在运行的进程，当多个进程就绪时，必须等待目前正在执行的
进程结束，实现起来更加简单，但是降低了进程执行的实时性。抢占式操作系统可以将正在
运行的进程打断，因此有着更加复杂的调度机制，但是也有更好的实时性。

二. STM32CubeMX 配置

1.开启freeRTOS：
在这里插入图片描述
2. 在任务栏添加任务，并对相应的任务栏进行命名：

设置任务函数优先级：

按照上述进行配置后便可生成相应的代码，在freertos.c文件中便可看到这里主要使用两个函数：

在这里插入图片描述

函数	osThreadDef(name, thread, priority, instances, stacksz)
功能	对要创建的任务进行设置
参数 1	name，要创建的任务的名称
参数 2	thread，要创建的任务代码的入口名称
参数 3	priority，要创建的任务的优先级
参数 4	instances，任务下可以创建的线程的数量
参数 5	stacksz，任务栈大小

函数	osThreadId osThreadCreate (const osThreadDef_t thread_def, void argument)
功能	创建一个任务
返回值	osThreadId，任务 ID，ID 是一个任务的重要标识，当在创建完任务后需要执行修改这个任务的优先级，或者销毁该任务时，就需要调用任务 ID，需要提前声明一个类型为 osThreadId 的变量在此处存储返回值。
参数 1	const osThreadDef_t *thread_def，我们通过 osThreadDef 所设置的任务参数，采用强制转换+任务名的方式进行输入，比如在 osThreadDef 中设置任务名为 LED_RED，则在此处输入 osThread(LED_RED)
参数 2	void *argument，任务需要的初始化参数，一般填为 NULL

三. 部分代码和注释

直接在main.c初始化过程中中运行freertos，这样所有的函数直接在freertos.c文件中运行，不需要在while循环中重复，从而实现操作系统的并行过程。

main.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  ** This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * COPYRIGHT(c) 2020 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "can.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motor.h"	//motor control
#include "remoter_uart.h"	//remoter control
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* Private variables ---------------------------------------------------------*/
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_USART6_UART_Init();
  MX_CAN1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM5_Init();
  MX_TIM8_Init();
  MX_TIM12_Init();
  /* USER CODE BEGIN 2 */
	/* enable PWM channel */
	HAL_TIM_Base_Start(&htim2);
	HAL_TIM_Base_Start(&htim3);
	HAL_TIM_Base_Start(&htim4);
	HAL_TIM_Base_Start(&htim5);
	HAL_TIM_Base_Start(&htim8);
	HAL_TIM_Base_Start(&htim12);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_4);
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4);
	HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_1);
	
	/* motor initial */
	can_filter_init();
	
	/* open dbus uart receive it */
	dbus_uart_init();
	
  /* USER CODE END 2 */

  /* Call init function for freertos objects (in freertos.c) */
  MX_FREERTOS_Init();

  /* Start scheduler */
  osKernelStart();
  
  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /**Configure the main internal regulator output voltage 
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /**Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 6;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /**Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while(1)
  {
		
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

freertos.c:

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2021 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */     
#include "pid.h"	//pid control
#include "motor.h"	//motor control
#include "servo.h"	//servo control
#include "sensor.h"	//sensor control
#include "buzzer.h"	//buzzer control
#include "remoter_uart.h"	//remoter control

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
extern rc_info_t rc;	//remoter control parameter
float distance_mm;	//sensor control parameter
uint16_t psc = 0;
uint16_t pwm = MIN_BUZZER_PWM; //buzzer control parameter
uint16_t set_speed;
uint16_t set_speed_x;
uint16_t set_speed_y;
PidTypeDef motor_pid;
PidTypeDef motor_pid_x;
PidTypeDef motor_pid_y;
fp32 PID[3]={3,0.1,0};
motor_measure_t* motor_data; //PID_motor param

/* USER CODE END Variables */
osThreadId LED_REDHandle;
osThreadId LED_GREENHandle;
osThreadId SONARHandle;
osThreadId CHASSISHandle;
osThreadId SERVOHandle;
osThreadId REMOTERHandle;
osThreadId UPSTAIRHandle;
osThreadId LEDHandle;
osThreadId BUZZERHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* sensor control */
void Delay_us(uint16_t time);
void soner_startrange(void);
uint16_t soner_gettime(void);
/* servo control */
void joint_first_cw(uint16_t time);
void joint_first_ccw(uint16_t time);
/* motor control */
motor_measure_t* get_chassis_motor_measure_point(uint8_t i);
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan);
void CAN_cmd_chassis(int16_t M1, int16_t M2, int16_t M3, int16_t M4);
/* pid control */
fp32 PID_Calc(PidTypeDef *pid,fp32 ref,fp32 set);

/* USER CODE END FunctionPrototypes */

void led_red_task(void const * argument);
void led_green_task(void const * argument);
void sonar_task(void const * argument);
void chassis_task(void const * argument);
void servo_task(void const * argument);
void remoter_task(void const * argument);
void upstair_task(void const * argument);
void led_task(void const * argument);
void buzzer_task(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
       
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the thread(s) */
  /* definition and creation of LED_RED */
  osThreadDef(LED_RED, led_red_task, osPriorityLow, 0, 128);
  LED_REDHandle = osThreadCreate(osThread(LED_RED), NULL);

  /* definition and creation of LED_GREEN */
  osThreadDef(LED_GREEN, led_green_task, osPriorityLow, 0, 128);
  LED_GREENHandle = osThreadCreate(osThread(LED_GREEN), NULL);

  /* definition and creation of SONAR */
  osThreadDef(SONAR, sonar_task, osPriorityHigh, 0, 128);
  SONARHandle = osThreadCreate(osThread(SONAR), NULL);

  /* definition and creation of CHASSIS */
  osThreadDef(CHASSIS, chassis_task, osPriorityNormal, 0, 128);
  CHASSISHandle = osThreadCreate(osThread(CHASSIS), NULL);

  /* definition and creation of SERVO */
  osThreadDef(SERVO, servo_task, osPriorityNormal, 0, 128);
  SERVOHandle = osThreadCreate(osThread(SERVO), NULL);

  /* definition and creation of REMOTER */
  osThreadDef(REMOTER, remoter_task, osPriorityHigh, 0, 128);
  REMOTERHandle = osThreadCreate(osThread(REMOTER), NULL);

  /* definition and creation of UPSTAIR */
  osThreadDef(UPSTAIR, upstair_task, osPriorityNormal, 0, 128);
  UPSTAIRHandle = osThreadCreate(osThread(UPSTAIR), NULL);

  /* definition and creation of LED */
  osThreadDef(LED, led_task, osPriorityLow, 0, 128);
  LEDHandle = osThreadCreate(osThread(LED), NULL);

  /* definition and creation of BUZZER */
  osThreadDef(BUZZER, buzzer_task, osPriorityLow, 0, 128);
  BUZZERHandle = osThreadCreate(osThread(BUZZER), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */
}

/* USER CODE BEGIN Header_led_red_task */
/**
  * @brief  Function implementing the LED_RED thread.
  * @param  argument: Not used 
  * @retval None
  */
/* USER CODE END Header_led_red_task */
__weak void led_red_task(void const * argument)
{

  /* USER CODE BEGIN led_red_task */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END led_red_task */
}

/* USER CODE BEGIN Header_led_green_task */
/**
* @brief Function implementing the LED_GREEN thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_led_green_task */
__weak void led_green_task(void const * argument)
{
  /* USER CODE BEGIN led_green_task */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END led_green_task */
}

/* USER CODE BEGIN Header_sonar_task */
/**
* @brief Function implementing the SONAR thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_sonar_task */
__weak void sonar_task(void const * argument)
{
  /* USER CODE BEGIN sonar_task */
  /* Infinite loop */
  for(;;)
  {
		distance_mm = soner_getdistance();
    osDelay(10);
  }
  /* USER CODE END sonar_task */
}

/* USER CODE BEGIN Header_chassis_task */
/**
* @brief Function implementing the CHASSIS thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_chassis_task */
__weak void chassis_task(void const * argument)
{
  /* USER CODE BEGIN chassis_task */
	PID_init(&motor_pid,PID_POSITION,PID,16000,2000);
	motor_data = get_chassis_motor_measure_point(0);
  /* Infinite loop */
  for(;;)
  {
		for(set_speed = 0; set_speed < 2000; set_speed += 200)
		{
			PID_Calc(&motor_pid,motor_data->speed_rpm,set_speed);
			CAN_cmd_chassis(-motor_pid.out,motor_pid.out,motor_pid.out,motor_pid.out);
			osDelay(100);
		}
  }
  /* USER CODE END chassis_task */
}

/* USER CODE BEGIN Header_servo_task */
/**
* @brief Function implementing the SERVO thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_servo_task */
void servo_task(void const * argument)
{
  /* USER CODE BEGIN servo_task */
  /* Infinite loop */
  for(;;)
  {
		if(distance_mm < 20)
		{
			joint_first_cw(1000);
			HAL_Delay(1000);
			joint_first_ccw(1000);
			HAL_Delay(1000);
		}	
    osDelay(1);
  }
  /* USER CODE END servo_task */
}

/* USER CODE BEGIN Header_remoter_task */
/**
* @brief Function implementing the REMOTER thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_remoter_task */
void remoter_task(void const * argument)
{
  /* USER CODE BEGIN remoter_task */
	PID_init(&motor_pid,PID_POSITION,PID,16000,2000);
	motor_data = get_chassis_motor_measure_point(0);
  /* Infinite loop */
  for(;;)
  {
		if(rc.sw1 == 1 && rc.sw2 == 1)
		{
			set_speed_x = rc.ch2;
			set_speed_y = rc.ch3;
			PID_Calc(&motor_pid_x,motor_data->speed_rpm,set_speed_x);
			PID_Calc(&motor_pid_y,motor_data->speed_rpm,set_speed_y);
			CAN_cmd_chassis(motor_pid_x.out+motor_pid_y.out,motor_pid.out-motor_pid_y.out,motor_pid.out+motor_pid_y.out,motor_pid.out-motor_pid_y.out);
			osDelay(100);
		}
		else if(rc.sw1 == 3 && rc.sw2 == 3)
		{
			set_speed_x = rc.ch2;
			set_speed_y = rc.ch3;
			PID_Calc(&motor_pid_x,motor_data->speed_rpm,set_speed_x);
			PID_Calc(&motor_pid_y,motor_data->speed_rpm,set_speed_y);
			CAN_cmd_auxiliary(motor_pid_x.out,motor_pid_y.out);
			osDelay(100);
		}
		else if(rc.sw1 == 3 && rc.sw2 == 1)
		{
			uint16_t angle1 = -rc.ch1;
			uint16_t angle2 = rc.ch1;
			uint16_t angle3 = -rc.ch3;
			uint16_t angle4 = rc.ch3;
			HAL_Delay(10);
			if(rc.ch1 < 0)
			{
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 400);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 1100);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, 400);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, 1100);
				HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_SET);
				HAL_Delay(angle1);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, 2000);
				HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
			}
			else 
			{
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 1100);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 400);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, 1100);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, 400);
				HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_SET);
				HAL_Delay(angle2);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, 2000);
				__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, 2000);
				HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
			}
			
			if(rc.ch3 < 0)
			{
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, 400);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 1100);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, 400);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, 1100);
				HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_SET);
				HAL_Delay(angle3);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, 2000);
				HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
			}
			else 
			{
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, 1100);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 400);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, 1100);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, 400);
				HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_SET);
				HAL_Delay(angle4);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, 2000);
				__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, 2000);
				HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
			}
			
			HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_RESET);
			HAL_Delay(100);
		}
		else
		{
			HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_RESET);
			HAL_Delay(50);
			HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
			HAL_GPIO_WritePin(GPIOF, GPIO_PIN_14, GPIO_PIN_SET);
			HAL_Delay(50);
		}
  }
  /* USER CODE END remoter_task */
}

/* USER CODE BEGIN Header_upstair_task */
/**
* @brief Function implementing the UPSTAIR thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_upstair_task */
void upstair_task(void const * argument)
{
  /* USER CODE BEGIN chassis_task */
	PID_init(&motor_pid,PID_POSITION,PID,16000,2000);
	motor_data = get_chassis_motor_measure_point(0);
  /* Infinite loop */
  for(;;)
  {
		for(set_speed = 0; set_speed < 2000; set_speed += 200)
		{
			PID_Calc(&motor_pid,motor_data->speed_rpm,set_speed);
			CAN_cmd_auxiliary(motor_pid.out,motor_pid.out);
			osDelay(100);
		}
  }
  /* USER CODE END chassis_task */
}

/* USER CODE BEGIN Header_led_task */
/**
* @brief Function implementing the LED thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_led_task */
__weak void led_task(void const * argument)
{
  /* USER CODE BEGIN led_task */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END led_task */
}

/* USER CODE BEGIN Header_buzzer_task */
/**
* @brief Function implementing the BUZZER thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_buzzer_task */
__weak void buzzer_task(void const * argument)
{
  /* USER CODE BEGIN buzzer_task */
  /* Infinite loop */
  for(;;)
  {
		psc++;
		//pwm++;
		if(pwm > MAX_BUZZER_PWM)
    {
      pwm = MIN_BUZZER_PWM;
    }
    if(psc > MAX_PSC)
    {
      psc = 0;
    }
    buzzer_on(psc, pwm);
        
    osDelay(100);
  }
  /* USER CODE END buzzer_task */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
     
/* USER CODE END Application */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/