/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "adc.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "readpin.h"
#include "callback.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "variables.h"
/* USER CODE END Includes */





void car_move(unsigned char ctrl,uint16_t speed[2])
{
	speed[0] = 0x00;
	speed[1] = 0x00;
	ctrl = 0;
	switch (motor_ctrl_buffer[0]){
		case 1 :
		speed[0] = 8500;
		break;
		case 2 :
		speed[0] = 8500 * 2;
		break;
		case 3 :
		speed[0] = 8500 * 3;
		break;
		case 4 :
		speed[0] = 33999;
		break;
	}
	switch (motor_ctrl_buffer[1]){
		case 1 :
		speed[1] = 8500;
		break;
		case 2 :
		speed[1] = 8500 * 2;
		break;
		case 3 :
		speed[1] = 8500 * 3;
		break;
		case 4 :
		speed[1] = 33999;
		break;
	}
	ctrl = pin_value;
	if (ctrl == 16) // left front,right brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	else if (ctrl == 8)  //left back,right brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	else if (ctrl == 4)  //left brake,right front
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, speed[1]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

	}
	else if (ctrl == 2)  //left brake,right back
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, speed[1]);

	}
	else if (ctrl == 24)  //all brake,right hang
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	else if (ctrl == 20)  //all front
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, speed[1]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

	}
	else if (ctrl == 18)  //left front,right back
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, speed[1]);

	}
	else if (ctrl == 12)  //left back,right front
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, speed[1]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

	}
	else if (ctrl == 10)  //all back
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, speed[1]);
		
	}
	else if (ctrl == 6)  //left brake,right brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	
	else if (ctrl == 28)  //left brake,right front
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, speed[1]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

	}
	
	else if (ctrl == 26)  //left brake,right brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, speed[1]);

	}
	
	else if (ctrl == 22)  //left front,right brake
	{
		
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	
	else if (ctrl == 14)  //left back,right brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed[0]);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}

	else        					//all brake
	{
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

	}
	

}


/*
//easiest control logic


void car_move(unsigned char ctrl,uint16_t speed[2])
{
	ctrl = ReadPin();
		if (ctrl == 16) // left front,right brake
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else if (ctrl == 8)  //left back,right brake
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else if (ctrl == 4)  //left brake,right front
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

		}
		else if (ctrl == 2)  //left brake,right back
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else if (ctrl == 24)  //all brake
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else if (ctrl == 20)  //all front
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

		}
		else if (ctrl == 18)  //left front,right back
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else if (ctrl == 12)  //left back,right front
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

		}
		else if (ctrl == 10)  //all back
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}
		else        					//all brake
		{
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 33999);
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 33999);

		}

}
*/


