/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "tim.h"
#include "gpio.h"
#include "math.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* 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 */

/* USER CODE END PV */

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

int32_t WH_CAN_ID;

int32_t CAN_Buf[8];


int32_t Clean_Swi=0;
int32_t Scan_Hor1_Ver2=0;
int32_t Rise_down_XX=0;
int32_t Turn_Left_Right_YY=0;
int32_t Hz_Adjust=0;

int32_t Forword_Limit=0;
int32_t Backword_Limit=0;
int32_t Left_Limit=0;
int32_t Right_Limit=0;

int Hori_Flag=0; int Time_Sum_TH=0; int Vert_Flag=0;int Time_Sum_TV=0;

void Manual_Oper_Func();
void MsetHz_1(uint16_t _PWM_Hz_1);
void MsetHz_2(uint16_t _PWM_Hz_2);
void Horizontal_Scan_func(int32_t Scan_Value);
void Vertical_Scan_func(int32_t Scan_Value);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void MsetHz(uint16_t _PWM_Hz)
{
	static uint8_t Mstate = 0;
	if(_PWM_Hz==0)
	{
		Mstate=0;
		HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_1);
		HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_2);
	}
	else
	{
		if(Mstate==0)
		{
			HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
			HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
			Mstate=1;
		}
		_PWM_Hz = 3200000/_PWM_Hz;
		if(_PWM_Hz>60000)
		{
			_PWM_Hz=60000;
		}
		__HAL_TIM_SET_AUTORELOAD(&htim4,_PWM_Hz);
		__HAL_TIM_SET_AUTORELOAD(&htim4,_PWM_Hz);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, _PWM_Hz/2);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, _PWM_Hz/2);
	}
}
/* 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_CAN_Init();
  MX_TIM4_Init();
  /* USER CODE BEGIN 2 */
  HAL_TIM_Base_Start(&htim4);
  HAL_GPIO_WritePin(M_ST_GPIO_Port,M_ST_Pin,GPIO_PIN_SET);
  /* USER CODE END 2 */

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

//	  HAL_GPIO_WritePin(M2_FR_GPIO_Port, M2_FR_Pin, GPIO_PIN_RESET);//Ver_motor   Down_Motion
//	//  HAL_GPIO_WritePin(M1_FR_GPIO_Port, M1_FR_Pin, GPIO_PIN_RESET);
//	  HAL_Delay(2000);
//	//  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
//	  MsetHz(0);
//	  HAL_Delay(2000);
//	//  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
//	  MsetHz(1000);
//	  CAN_senddata(&hcan);

//??????

	 // GPIO_ReadInputDataBit(GPIOB,13);//Read Input State
	Forword_Limit=HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_9);      //Read Input State
	Backword_Limit=HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0);      //Read Input State
	Left_Limit=HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_8);      //Read Input Stat
	Right_Limit=HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1);      //Read Input State


	switch(Scan_Hor1_Ver2)
	{
		case 0:
			Vert_Flag=0;
			Time_Sum_TV=0;
			Hori_Flag=0;
			Time_Sum_TH=0;
			Manual_Oper_Func();
			break;
		case 1:
			Vert_Flag=0;
			Time_Sum_TV=0;
			Horizontal_Scan_func(Clean_Swi);
			break;
		case 2:
			Hori_Flag=0;
			Time_Sum_TH=0;
			Vertical_Scan_func(Clean_Swi);
			break;
		default:
			MsetHz_1(0);//Set Speed
			MsetHz_2(0);//Set Speed
			break;
	}


//	  HAL_Delay(2000);
//	  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
//	  MsetHz(0);
//	  HAL_Delay(2000);
//	  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
//	  MsetHz(1000);
//	  CAN_senddata(&hcan);
//	  HAL_GPIO_TogglePin(M2_FR_GPIO_Port, M2_FR_Pin);
//	  HAL_GPIO_TogglePin(M1_FR_GPIO_Port, M1_FR_Pin);


    /* 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};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL8;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses 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_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != 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 */
  __disable_irq();
  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,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */


void Manual_Oper_Func()
{
	uint16_t Speed_X=0;uint16_t Speed_Y=0;
	Speed_X=(uint16_t) (fabs(Rise_down_XX))*10;
	Speed_Y=(uint16_t) (fabs(Turn_Left_Right_YY))*10;
	uint16_t threshold=50;
	if((Rise_down_XX>threshold)&&(Speed_Y<Speed_X)&&(Forword_Limit==1))
	{
		HAL_GPIO_WritePin(M2_FR_GPIO_Port, M2_FR_Pin, GPIO_PIN_SET);	//Set Direction
		MsetHz_1(Speed_X);//Set Speed
	}
	else if((Rise_down_XX<-threshold)&&(Speed_Y<Speed_X)&&(Backword_Limit==1))
	{
		HAL_GPIO_WritePin(M2_FR_GPIO_Port, M2_FR_Pin, GPIO_PIN_RESET);	//Set Direction
		MsetHz_1(Speed_X);
	}
	else if((Turn_Left_Right_YY>threshold)&&(Speed_Y>Speed_X)&&(Left_Limit==1))//
	{
		HAL_GPIO_WritePin(M1_FR_GPIO_Port, M1_FR_Pin, GPIO_PIN_RESET );	//Set Direction
		MsetHz_2(Speed_Y);//Set Speed
	}
	else if((Turn_Left_Right_YY<-threshold)&&(Speed_Y>Speed_X)&&(Right_Limit==1) )
	{
		HAL_GPIO_WritePin(M1_FR_GPIO_Port, M1_FR_Pin, GPIO_PIN_SET);	//Set Direction
		MsetHz_2(Speed_Y);//Set Speed
	}
	else
	{
		MsetHz_1(0);//Set Speed
		MsetHz_2(0);//Set Speed
	}
}


int32_t Scan_Time;
void Horizontal_Scan_func(int32_t Scan_Value)
{
	Scan_Time=Scan_Value*50;
	switch(Hori_Flag)
	{
		case 0:
			if((Time_Sum_TH<Scan_Time)&&(Right_Limit==1))
			{
				Hori_Flag=0;
				HAL_Delay(9);// Delay 2000ms
				HAL_GPIO_WritePin(M1_FR_GPIO_Port, M1_FR_Pin, GPIO_PIN_SET);
				MsetHz_2(1000);
				Time_Sum_TH=Time_Sum_TH+10;
			}
			else
			{
				Hori_Flag=1;
				Time_Sum_TH=0;
			}
			break;
		case 1:
			if((Time_Sum_TH<Scan_Time)&&(Left_Limit==1))
			{
				Hori_Flag=1;
				HAL_Delay(9);// Delay 2000ms
				HAL_GPIO_WritePin(M1_FR_GPIO_Port, M1_FR_Pin,  GPIO_PIN_RESET);
				MsetHz_2(1000);
				Time_Sum_TH=Time_Sum_TH+10;
			}
			else
			{
				Hori_Flag=0;
				Time_Sum_TH=0;
			}
			break;
		default:
			MsetHz_1(0);//Set Speed
			MsetHz_2(0);//Set Speed
			break;
	}

}

void Vertical_Scan_func(int32_t Scan_Value)
{
	//int32_t
	Scan_Time=Scan_Value*50;
	switch(Vert_Flag)
	{
		case 0:
			if((Time_Sum_TV<Scan_Time)&&(Backword_Limit==1))
			{
				Vert_Flag=0;
				HAL_Delay(9);// Delay 2000ms
				HAL_GPIO_WritePin(M2_FR_GPIO_Port, M2_FR_Pin, GPIO_PIN_RESET);	//Set Direction
				MsetHz_1(1000);
				Time_Sum_TV=Time_Sum_TV+10;
			}
			else
			{
				Vert_Flag=1;
				Time_Sum_TV=0;
			}

			break;
		case 1:

			if((Time_Sum_TV<Scan_Time)&&(Forword_Limit==1))
			{
				Vert_Flag=1;
				HAL_Delay(9);// Delay 2000ms
				HAL_GPIO_WritePin(M2_FR_GPIO_Port, M2_FR_Pin,  GPIO_PIN_SET);	//Set Direction
				MsetHz_1(1000);
				Time_Sum_TV=Time_Sum_TV+10;
			}
			else
			{
				Vert_Flag=0;
				Time_Sum_TV=0;
			}
			break;
		default:
			MsetHz_1(0);//Set Speed
			MsetHz_2(0);//Set Speed
			break;
	}
}

void MsetHz_1(uint16_t _PWM_Hz_1)
{
	static uint8_t Mstate_1 = 0;
	if(_PWM_Hz_1==0)
	{
		Mstate_1=0;
		HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_1);
	}
	else
	{
		if(Mstate_1==0)
		{
			HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
			Mstate_1=1;
		}
		_PWM_Hz_1 = 3200000/_PWM_Hz_1;
		if(_PWM_Hz_1>60000)
		{
			_PWM_Hz_1=60000;
		}
		__HAL_TIM_SET_AUTORELOAD(&htim4,_PWM_Hz_1);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, _PWM_Hz_1/2);
	}
}


void MsetHz_2(uint16_t _PWM_Hz_2)
{
	static uint8_t Mstate_2 = 0;
	if(_PWM_Hz_2==0)
	{
		Mstate_2=0;
		HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_2);
	}
	else
	{
		if(Mstate_2==0)
		{
			HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
			Mstate_2=1;
		}
		_PWM_Hz_2 = 3200000/_PWM_Hz_2;
		if(_PWM_Hz_2>60000)
		{
			_PWM_Hz_2=60000;
		}
		__HAL_TIM_SET_AUTORELOAD(&htim4,_PWM_Hz_2);
		__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, _PWM_Hz_2/2);
	}
}
