Semua tahap membuat robot untuk mengikuti garis, atau cara mengumpulkan semua penggaruk dengan STM32

1. ;

void RCC_Init(void) {						// Quartz 16 MHz

	RCC->CFGR |= RCC_CFGR_PLLXTPRE;	// PLLXTPRE set divider (/2) = 8 MHz

	RCC->CR |= RCC_CR_HSEON;			// On HSE
	while (!(RCC->CR & RCC_CR_HSERDY)) {
	};

	RCC->CFGR |= (RCC_CFGR_PLLMULL6);     // Setup PLLMULL set multiplier (x6) = 48 MHz

	RCC->CFGR |= RCC_CFGR_PLLSRC;		// PLLSRC set HSE

	RCC->CR |= RCC_CR_PLLON;			// ON PLL
	while (!(RCC->CR & RCC_CR_PLLRDY)) {
	};

	FLASH->ACR &= ~FLASH_ACR_LATENCY;	// Setup FLASH
	FLASH->ACR |= FLASH_ACR_LATENCY_1;

	RCC->CFGR |= RCC_CFGR_HPRE_DIV1; 	// Set not divider (AHB) = 48 MHz
	RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; 	// Set divider (/2) (APB1) = 24 MHz
	RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; 	// Set not divider (APB2) = 48 MHz

	RCC->CFGR &= ~RCC_CFGR_SW; 		// Setup SW, select PLL
	RCC->CFGR |= RCC_CFGR_SW_PLL;

//	RCC->CFGR |= RCC_CFGR_MCO_SYSCLK;

}

2. , , , Push Pull;

void GPIO_Init(void) {

	RCC->APB2ENR |=	(RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_AFIOEN); // Enable clock portA, portB and Alternative function

	AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_JTAGDISABLE;	// Disable JTAG for pins B3,B4 and A15

	//---------------LED: (B3 to B9; A11, A12, A15); Output mode: Push Pull, max 10 MHz---------------//

	GPIOB->CRL &= ~(GPIO_CRL_CNF3 | GPIO_CRL_MODE3);	// Setup B3 pins PP, 10MHz
	GPIOB->CRL |= GPIO_CRL_MODE3_0;

	GPIOB->CRL &= ~(GPIO_CRL_CNF4 | GPIO_CRL_MODE4);	// Setup B3 pins PP, 10MHz
	GPIOB->CRL |= GPIO_CRL_MODE4_0;

	GPIOB->CRL &= ~(GPIO_CRL_CNF5 | GPIO_CRL_MODE5);	// Setup B4 pins PP, 10MHz
	GPIOB->CRL |= GPIO_CRL_MODE5_0;

	GPIOB->CRL &= ~(GPIO_CRL_CNF6 | GPIO_CRL_MODE6);	// Setup B5 pins PP, 10MHz
	GPIOB->CRL |= GPIO_CRL_MODE6_0;

	GPIOB->CRL &= ~(GPIO_CRL_CNF7 | GPIO_CRL_MODE7);	// Setup B6 pins PP, 10MHz
	GPIOB->CRL |= GPIO_CRL_MODE7_0;

	GPIOB->CRH &= ~(GPIO_CRH_CNF8 | GPIO_CRH_MODE8);	// Setup B7 pins PP, 10MHz
	GPIOB->CRH |= GPIO_CRH_MODE8_0;

	GPIOB->CRH &= ~(GPIO_CRH_CNF9 | GPIO_CRH_MODE9);	// Setup B8 pins PP, 10MHz
	GPIOB->CRH |= GPIO_CRH_MODE9_0;

	GPIOA->CRH &= ~(GPIO_CRH_CNF11 | GPIO_CRH_MODE11);	// Setup A11 pins PP, 10MHz
	GPIOA->CRH |= GPIO_CRH_MODE11_0;

	GPIOA->CRH &= ~(GPIO_CRH_CNF12 | GPIO_CRH_MODE12);	// Setup A12 pins PP, 10MHz
	GPIOA->CRH |= GPIO_CRH_MODE12_0;

	GPIOA->CRH &= ~(GPIO_CRH_CNF15 | GPIO_CRH_MODE15);	// Setup A15 pins PP, 10MHz
	GPIOA->CRH |= GPIO_CRH_MODE15_0;

	//--Motors: (A8 to A10; B12 to B15); Output and input (B12, B13) mode: Alternative function, PWM - Push Pull, max 10 MHz--//

	GPIOB->CRH &= ~(GPIO_CRH_CNF12 | GPIO_CRH_MODE12);		// Setup B12 pins analog input

	GPIOB->CRH &= ~(GPIO_CRH_CNF13 | GPIO_CRH_MODE13);		// Setup B13 pins analog input

	GPIOB->CRH &= ~(GPIO_CRH_CNF14   | GPIO_CRH_MODE14);	// Setup B14 pins PP, 10MHz
	GPIOB->CRH |=  GPIO_CRH_MODE14_0;

	GPIOB->CRH &= ~(GPIO_CRH_CNF15   | GPIO_CRH_MODE15);	// Setup B15 pins PP, AF, 10MHz
	GPIOB->CRH |=  (GPIO_CRH_CNF15_1 | GPIO_CRH_MODE15_0);

	GPIOA->CRH &= ~(GPIO_CRH_CNF8 | GPIO_CRH_MODE8);		// Setup A10 pins PP, 10MHz
	GPIOA->CRH |= GPIO_CRH_MODE8_0;

	GPIOA->CRH &= ~(GPIO_CRH_CNF9    | GPIO_CRH_MODE9);		// Setup A9 pins PP, AF, 10MHz
	GPIOA->CRH |=  (GPIO_CRH_CNF9_1  | GPIO_CRH_MODE9_0);

	GPIOA->CRH &= ~(GPIO_CRH_CNF10 | GPIO_CRH_MODE10);		// Setup A10 pins PP, 10MHz
	GPIOA->CRH |= GPIO_CRH_MODE10_0;

	//--UART3: (B10 - TX; B11 - RX); Output mode: Alternative function, UART - Push Pull, max 10 MHz--//

	GPIOB->CRH &= ~(GPIO_CRH_CNF10   | GPIO_CRH_MODE10);	// Setup B10 pins PP, AF, 10MHz
	GPIOB->CRH |=  (GPIO_CRH_CNF10_1 | GPIO_CRH_MODE10_0);

	GPIOB->CRH &= ~(GPIO_CRH_CNF11   | GPIO_CRH_MODE11);	// Setup B11 pins PP, AF, 10MHz
	GPIOB->CRH |=  (GPIO_CRH_CNF11_1 | GPIO_CRH_MODE11_0);

	//--Optical sensors: (B0, B1, A0 - A7); Input mode: Analog--//

	GPIOB->CRL &= ~(GPIO_CRL_CNF0 | GPIO_CRL_MODE0);	// Setup B0 pins analog input
	GPIOB->CRL &= ~(GPIO_CRL_CNF1 | GPIO_CRL_MODE1);	// Setup B1 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF0 | GPIO_CRL_MODE0);	// Setup A0 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF1 | GPIO_CRL_MODE1);	// Setup A1 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF2 | GPIO_CRL_MODE2);	// Setup A2 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF3 | GPIO_CRL_MODE3);	// Setup A3 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF4 | GPIO_CRL_MODE4);	// Setup A4 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF5 | GPIO_CRL_MODE5);	// Setup A5 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF6 | GPIO_CRL_MODE6);	// Setup A6 pins analog input
	GPIOA->CRL &= ~(GPIO_CRL_CNF7 | GPIO_CRL_MODE7);	// Setup A7 pins analog input

}

3. DMA, ;

extern uint16_t adc_buf[10];

void ADC_Init(void) {

	 RCC->AHBENR |= RCC_AHBENR_DMA1EN;
	 DMA1_Channel1->CPAR = (uint32_t) &ADC1->DR;		//   
	 DMA1_Channel1->CMAR = (unsigned int) adc_buf;   	//    

	 DMA1_Channel1->CNDTR = 10;			  				//    
	 DMA1_Channel1->CCR &= ~DMA_CCR_EN;			   		//   
	 DMA1_Channel1->CCR |= DMA_CCR_MSIZE_0;		  		//   16 bit
	 DMA1_Channel1->CCR |= DMA_CCR_PSIZE_0;		 		//   16 bit
	 DMA1_Channel1->CCR |= DMA_CCR_MINC;			  	// memory increment mode
	 DMA1_Channel1->CCR |= DMA_CCR_CIRC;
	 DMA1_Channel1->CCR |= DMA_CCR_TCIE;				//    
	 DMA1_Channel1->CCR |= DMA_CCR_EN;				  	//  
	 NVIC_SetPriority(DMA1_Channel1_IRQn, 10);
	 NVIC_EnableIRQ(DMA1_Channel1_IRQn);

	 RCC->CFGR &= ~RCC_CFGR_ADCPRE;
	 RCC->CFGR |= RCC_CFGR_ADCPRE_DIV8;
	 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;

	 ADC1->SQR3 = 0;			// 1
	 ADC1->SQR3 |= 1 << 5;		// 2     
	 ADC1->SQR3 |= 2 << 10;		// 3
	 ADC1->SQR3 |= 3 << 15;		// 4
	 ADC1->SQR3 |= 4 << 20;		// 5
	 ADC1->SQR3 |= 5 << 25;		// 6
	 ADC1->SQR2 = 6;			// 7
	 ADC1->SQR2 |= 7 << 5;		// 8
	 ADC1->SQR2 |= 8 << 10;		// 9
	 ADC1->SQR2 |= 9 << 15;		// 10

	 ADC1->CR2 = ADC_CR2_EXTSEL_0 | ADC_CR2_EXTSEL_1 | ADC_CR2_EXTSEL_2
	 | ADC_CR2_EXTTRIG;
	 ADC1->SMPR1 = 0;					 		//   
	 ADC1->SMPR2 = 0;					 		//
	 ADC1->SMPR2 |= (uint32_t) (6 << (0 * 3)); 	// 0,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (1 * 3)); 	// 1,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (2 * 3)); 	// 2,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (3 * 3)); 	// 3,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (4 * 3)); 	// 4,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (5 * 3)); 	// 5,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (6 * 3)); 	// 6,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (7 * 3)); 	// 7,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (8 * 3)); 	// 8,   6 
	 ADC1->SMPR2 |= (uint32_t) (6 << (9 * 3)); 	// 9,   6 
	 ADC1->SMPR1 |= (uint32_t) (6 << (0 * 3)); 	// 10,   6 

	 ADC1->CR2 |= ADC_CR2_ADON;

	 ADC1->CR2 |= ADC_CR2_RSTCAL;
	 while ((ADC1->CR2 & ADC_CR2_RSTCAL) == ADC_CR2_RSTCAL) {
	 }

	 ADC1->CR2 |= ADC_CR2_CAL;

	 while ((ADC1->CR2 & ADC_CR2_RSTCAL) == ADC_CR2_CAL) {
	 }

	 ADC1->SQR1 |= 9 << 20;						//  
	 ADC1->CR1 |= ADC_CR1_SCAN;					//  
	 ADC1->CR2 |= ADC_CR2_DMA;				  	// DMA on
	 // ADC1->CR2 |= ADC_CR2_CONT;

	 //  ADC1->CR2 |= ADC_CR2_SWSTART;
	 ADC1->CR2 |= ADC_CR2_ADON;

}

4. , ;

void TIM1_Init(void) {

	/*************** Enable TIM1 (CH1) ***************/
	RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
	TIM1->CCER = 0;										//  CCER ( )
	TIM1->ARR = 1000; 									//  ,     
	TIM1->PSC = 48 - 1;                					// 
	TIM1->BDTR |= TIM_BDTR_MOE;     					//     

	TIM1->CCR2 = 0; 									//       (  0  TIM1->ARR)
	TIM1->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2; //     
	TIM1->CCER |= (TIM_CCER_CC2P |TIM_CCER_CC2E); 		//        
														//   -   3
	TIM1->CCR3 = 0; 									//       (  0  TIM1->ARR)
	TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2; //     
	TIM1->CCER |= TIM_CCER_CC3NE; 						//        
	TIM1->CCER &= ~TIM_CCER_CC3NP;

	TIM1->CR1 |= TIM_CR1_CEN;
}

5. UART , ;

void UART3_Init(void) {

	RCC->APB1ENR |= RCC_APB1ENR_USART3EN;

	USART3->BRR = 0xD0;						// Speed = 115200; (24 000 000 + (115200 / 2)) / 115200 = 208 -> 0xD0

	USART3->CR1 |= USART_CR1_TE | USART_CR1_RE | USART_CR1_UE;

//	USART3->CR1 |= USART_CR1_RXNEIE;
//	NVIC_EnableIRQ(USART3_IRQn);
}

void UART3_Send(char chr) {
	while (!(USART3->SR & USART_SR_TC))
		;
	USART3->DR = chr;
}

void UART3_Send_String(char* str) {
	uint8_t i = 0;

	while (str[i])
		UART3_Send(str[i++]);
}

void UART3_Send_Number_Float(float data){

	char str[100];

	char *tmpSign = (data < 0) ? "-" : "";
	float tmpVal = (data < 0) ? -data : data;

	int tmpInt1 = tmpVal;                  // Get the integer (678).
	float tmpFrac = tmpVal - tmpInt1;      // Get fraction (0.0123).
	int tmpInt2 = trunc(tmpFrac * 10);  // Turn into integer (123).	int tmpInt2 = trunc(tmpFrac * 10000)

	// Print as parts, note that you need 0-padding for fractional bit.

	sprintf (str, "%s%d.%01d", tmpSign, tmpInt1, tmpInt2);

	UART3_Send_String(str);
}

6. FreeRtos config

/*
 * FreeRTOS Kernel V10.3.1
 * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://www.FreeRTOS.org
 * http://aws.amazon.com/freertos
 *
 * 1 tab == 4 spaces!
 */

#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H

/* Library includes. */
//#include "stm32f10x_lib.h"
/*-----------------------------------------------------------
 * Application specific definitions.
 *
 * These definitions should be adjusted for your particular hardware and
 * application requirements.
 *
 * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
 * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. 
 *
 * See http://www.freertos.org/a00110.html
 *----------------------------------------------------------*/

#define vPortSVCHandler SVC_Handler		// fix problem
#define xPortPendSVHandler PendSV_Handler
#define vPortSVCHandler SVC_Handler
#define xPortSysTickHandler SysTick_Handler


#define configUSE_PREEMPTION		1
#define configUSE_IDLE_HOOK			0
#define configUSE_TICK_HOOK			1
#define configCPU_CLOCK_HZ			( ( unsigned long ) 48000000 )
#define configTICK_RATE_HZ			( ( TickType_t ) 1000 )
#define configMAX_PRIORITIES		( 5 )
#define configMINIMAL_STACK_SIZE	( ( unsigned short ) 32 )
#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 17 * 1024 ) )
#define configMAX_TASK_NAME_LEN		( 16 )
#define configUSE_TRACE_FACILITY	0
#define configUSE_16_BIT_TICKS		0
#define configIDLE_SHOULD_YIELD		1
#define configUSE_MUTEXES			1

/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 		0
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )

/* Set the following definitions to 1 to include the API function, or zero
 to exclude the API function. */

#define INCLUDE_vTaskPrioritySet		1
#define INCLUDE_uxTaskPriorityGet		1
#define INCLUDE_vTaskDelete				1
#define INCLUDE_vTaskCleanUpResources	0
#define INCLUDE_vTaskSuspend			1
#define INCLUDE_vTaskDelayUntil			1
#define INCLUDE_vTaskDelay				1

/* This is the raw value as per the Cortex-M3 NVIC.  Values can be 255
 (lowest) to 0 (1?) (highest). */
#define configKERNEL_INTERRUPT_PRIORITY 		255
/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!!
 See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY 	191 /* equivalent to 0xb0, or priority 11. */

/* This is the value being used as per the ST library which permits 16
 priority values, 0 to 15.  This must correspond to the
 configKERNEL_INTERRUPT_PRIORITY setting.  Here 15 corresponds to the lowest
 NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY	15

#endif /* FREERTOS_CONFIG_H */

7. main;

#include "main.h"

void vTaskUART2(void *argument);
void vTaskConvADC(void *argument);

uint32_t Motor(int32_t which, int32_t speed, int32_t turn);

void IndicatorLED(uint32_t number, uint32_t status);


//define      typedef enum :)
#define MotorLeft		1
#define MotorRight		0
#define MoveBack		0
#define MoveForward		1
#define MaxSpeedMotor	1000

uint16_t adc_buf[10];				// Buffer DMA ADC sensors
uint16_t SensWhiteOrBlack[10];		// Buffer sensors white or black

int main(void) {

	RCC_Init();
	GPIO_Init();
	TIM1_Init();
	UART3_Init();
	ADC_Init();

	xTaskCreate(vTaskUART2, "UART", 512, NULL, 1, NULL);
	xTaskCreate(vTaskConvADC, "ADC", 128, NULL, 1, NULL);

	UART3_Send_String("Start program\r\n");

	GPIOA->BSRR |= GPIO_BSRR_BS10;	// Motor enable

	vTaskStartScheduler();

	while (1) {

	}
}
/*************************************Tasks***************************************/

void vTaskUART2(void *argument) {

	while (1) {

//-------------------Read sensors from buff DMA ADC, and print to UART3-------------//

		for (uint32_t i = 0; i <= 9; i++)
		{

			if (adc_buf[i] <= 300)		// value sens is white
			{
				IndicatorLED(i, 0);
				SensWhiteOrBlack[i] = 0;
			} else						// else value sens is black
			{
				IndicatorLED(i, 1);
				SensWhiteOrBlack[i] = 1;
			}

//			UART3_Send_Number_Float(SensWhiteOrBlack[i]);
//			UART3_Send_String("\t");
		}
///		UART3_Send_String("\r\n");

//-----------------------------------------Move-----------------------------------------//


		//----Normal mode----//
		float devMotorLeft  = 1;
		float devMotorRight = 1;
		uint32_t flagSizeBuf = 0;

		for (int32_t i = 6; i <= 9; i++)
		{
			if (SensWhiteOrBlack[i])
			{
				flagSizeBuf = 1;

				if(i == 6 && SensWhiteOrBlack[i] == 1)
				{
					devMotorRight = 0.75;
				}
				else if(i == 7 && SensWhiteOrBlack[i] == 1)
				{
					devMotorRight = 0.5;
				}
				else if(i == 8 && SensWhiteOrBlack[i] == 1)
				{
					devMotorRight = 0.25;
				}
				else if(i == 9 && SensWhiteOrBlack[i] == 1)
				{
					devMotorRight = 0.0;
				}
			}
		}

		for(int32_t i = 5; i >= 0; i--)
		{
			if(SensWhiteOrBlack[i])
			{
				flagSizeBuf = 1;

				if(i == 3 && SensWhiteOrBlack[i] == 1)
				{
					devMotorLeft = 0.75;
				}
				else if(i == 2 && SensWhiteOrBlack[i] == 1)
				{
					devMotorLeft = 0.5;
				}
				else if(i == 1 && SensWhiteOrBlack[i] == 1)
				{
					devMotorLeft = 0.25;
				}
				else if(i == 0 && SensWhiteOrBlack[i] == 1)
				{
					devMotorLeft = 0.0;
				}
			}
		}

		if(!flagSizeBuf)
		{
			devMotorLeft  = 0;
			devMotorRight = 0;
		}

		Motor(MotorRight, (int32_t)(MaxSpeedMotor * (float)devMotorRight), MoveForward);
		Motor(MotorLeft,  (int32_t)(MaxSpeedMotor * (float)devMotorLeft),  MoveForward);


/*
		if (adc_buf[0] > 1000)
		{
			Motor(MotorLeft, 500, MoveBack);
		}
		else if (adc_buf[0] <= 1000)
		{
			Motor(MotorLeft, 0, MoveForward);
		}

		if (adc_buf[9] > 1000)
		{
			Motor(MotorRight, 500, MoveBack);//Motor(0, 500, 1);	MotorRight	MoveBack
		}
		else if (adc_buf[9] <= 1000)
		{
			Motor(MotorRight, 0, MoveForward);
		}
*/

		vTaskDelay(50);
	}
}
void vTaskConvADC(void *argument) {

	while (1) {

		// just void :)

		vTaskDelay(5000);
	}

}
/**********************************Function*************************************/

void IndicatorLED(uint32_t number, uint32_t status) {

	if (number == 0 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS9;
	}

	else if (number == 0 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR9;
	}

	if (number == 1 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS8;
	}

	else if (number == 1 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR8;
	}

	if (number == 2 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS7;
	}

	else if (number == 2 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR7;
	}

	if (number == 3 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS6;
	}

	else if (number == 3 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR6;
	}

	if (number == 4 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS5;
	}

	else if (number == 4 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR5;
	}

	if (number == 5 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS4;
	}

	else if (number == 5 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR4;
	}

	if (number == 6 && status == 0) {
		GPIOB->BSRR |= GPIO_BSRR_BS3;
	}

	else if (number == 6 && status == 1) {
		GPIOB->BSRR |= GPIO_BSRR_BR3;
	}

	if (number == 7 && status == 0) {
		GPIOA->BSRR |= GPIO_BSRR_BS15;
	}

	else if (number == 7 && status == 1) {
		GPIOA->BSRR |= GPIO_BSRR_BR15;
	}

	if (number == 8 && status == 0) {
		GPIOA->BSRR |= GPIO_BSRR_BS12;
	}

	else if (number == 8 && status == 1) {
		GPIOA->BSRR |= GPIO_BSRR_BR12;
	}

	if (number == 9 && status == 0) {
		GPIOA->BSRR |= GPIO_BSRR_BS11;
	}

	else if (number == 9 && status == 1) {
		GPIOA->BSRR |= GPIO_BSRR_BR11;
	}

}

uint32_t Motor(int32_t which, int32_t speed, int32_t turn) {

	if (which == 0)	//Right motor
	{
		UART3_Send_Number_Float(speed);
		UART3_Send_String("\t");

		if (speed > 0 && speed <= 1000)
		{
			TIM1->CCR3 = speed;
			if (turn == 0) 	// back
			{
				GPIOB->BSRR |= GPIO_BSRR_BS14;
			}
			else			//forward
			{
				GPIOB->BSRR |= GPIO_BSRR_BR14;
			}
		}
		else		//disable motor
		{
			TIM1->CCR3 = 0;
			GPIOB->BSRR |= GPIO_BSRR_BR14;

		}

	}

	else if (which == 1)	//left motor
	{
		UART3_Send_Number_Float(speed);
		UART3_Send_String("\r\n");

		if (speed > 0 && speed <= 1000)
		{
			TIM1->CCR2 = speed;
			if (turn == 1) 	// back
			{
				GPIOA->BSRR |= GPIO_BSRR_BS8;
			}
			else			//forward
			{
				GPIOA->BSRR |= GPIO_BSRR_BR8;
			}
		}
		else		//disable motor
		{
			TIM1->CCR2 = 0;
			GPIOA->BSRR |= GPIO_BSRR_BS8;

		}

	}



	return 0;
}

/*************************************IRQ***************************************/
/*
 void USART2_IRQHandler(void) {

 if (USART2->SR & USART_CR1_RXNEIE) {

 USART2->SR &= ~USART_CR1_RXNEIE;

 if (USART2->DR == '0') {
 UART2_Send_String("OFF\r\n");
 GPIOC->BSRR = GPIO_BSRR_BS13;
 } else if (USART2->DR == '1') {
 UART2_Send_String("ON\r\n");
 GPIOC->BSRR = GPIO_BSRR_BR13;
 } else if (USART2->DR == '2') {
 uint16_t d0 = ADC1->DR;
 float Vdd = 1.2 * 4069 / d0;
 UART2_Send(Vdd);
 GPIOC->BSRR = GPIO_BSRR_BR13;
 }
 }
 }
 */
void DMA1_Channel1_IRQHandler(void) {

	DMA1->IFCR = DMA_IFCR_CGIF1 | DMA_IFCR_CTCIF1;	// clear DMA interrupt flags
	DMA1_Channel1->CCR &= ~DMA_CCR_EN;					//  
	/*
	 * Code
	 */
	DMA1_Channel1->CCR |= DMA_CCR_EN;			   //  
	ADC1->CR2 |= ADC_CR2_ADON;
}