/* USER CODE BEGIN Header */

/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2024 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"

/* Private includes ----------------------------------------------------------*/

/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stddef.h>
#include <stdlib.h>
#include <math.h>
/* 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 ---------------------------------------------------------*/

ADC_HandleTypeDef hadc1;
DMA_HandleTypeDef hdma_adc1;

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;

/* USER CODE BEGIN PV */

//Khai bao bien toan cuc (Private Variables)có thể truy cập ở bất cứ hàm nào
//Doc gia tri khoi luong
#define DT_PIN GPIO_PIN_8
 #define DT_PORT GPIOB
#define SCK_PIN GPIO_PIN_9
#define SCK_PORT GPIOB
#define PI 3.1416
uint32_t tare = 8285512;
float knownOriginal = 1770000; // in milli gram
float knownHX711 = 766082 ; // Giá trị doc từ HX711 cho trong lượng đã biết
int weight; // Biến để lưu trong lượng tính toán.
int current_weight =0;
int weight_updated = 0;
int check_good =0;
int cho=0;
////Loadcell
//bien loadcell xem di ve huong nao
int top_road = 0, below_road = 0;
//-----------------------------SENSOR
LINE-------------------------------------------------------
//biến sensorline
uint16_t AdcValues[5];
uint16_t ADCMinValue[5] = {180, 205, 185, 230, 180};
uint16_t ADCMaxValue[5] = {2600, 2500, 2350, 2400, 2450};
uint16_t TB[5]={1380,1595,1382,1438,1565};
uint16_t Ymax = 2460, Ymin = 196, sumADCmax = 0, sumADCmin = 0;
uint16_t AdcValuesAfterCalib[5];
float A[5], e2 = 0, pre_e2 = 0, e3 = 0, pre_e3 = 0;
float sum_Adcvalues = 0;
int a=0,m=0,bk=0;

//Motor---------------------------

int enc_right=0,enc_pre_right=0,delta_enc_right=0;
int enc_left=0,enc_pre_left=0,delta_enc_left;
float rpm_left =0,rpm_right=0;

//-----------Var for PI---------------

//----------Right motor v2---------------
float err_right=0, err_right_pre =0,err_right_pre_pre=0; //Sai so
float P_right =0, I_right =0, D_right=0;
float kp_right =0.8545, ki_right =51.8236;
volatile float duty_right =0, duty_left=0, duty_right_pre=0, duty_left_pre=0;
//---------Left motor v1-----------------
float err_left=0, err_left_pre =0,err_left_pre_pre=0; //Sai so
float P_left =0, I_left =0, D_left=0;
float kp_left =1.094, ki_left =60.974;
//------------cotrol motor-------//
float PWM_right=0;
float PWM_left=0;

//---------------------------SYSTEM
VARIABLES-----------------------------------------------------
//He thong
float Ts = 0.01, flag =0, re=0;
//float v_control, w_control, v_reponse, w_reponse;//* ta_control,
tb_control
float k1 = 0, k2 = 150, k3 =9.9;//*
float v, w, r = 0.0425, b = 0.196;
float vr = 0.4, vtb = 0, wr = 0,v_A=0, v_B=0;
float e1_line = 0, e2_line = 0, e3_line = 0;
 int empty = 1, outline = 0; //Ban dau empty de kiem tra cho dung line
den //Khai bao emty ,outline
int counter = 0;
int stop = 0;
float setpoint_r = 0, setpoint_l = 0; //duty_cycle cua he thong
int check =0;
int top=0, below=0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM3_Init(void);
static void MX_ADC1_Init(void);
static void MX_TIM4_Init(void);
static void MX_TIM2_Init(void);
/* USER CODE BEGIN PFP */
////Khai bao ham (fuction prototypes) co the goi bat cu đâu trong chương trình
//-------Loadcell---
//ham tri tuyet doi
float abs_(float a) {
if(a < 0) return -a;
else return a;
}
//---------------
void Set_PWM_DutyCycle_right(uint16_t duty_cycle) {
__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,100-duty_cycle);
}
void Set_PWM_DutyCycle_left(uint16_t duty_cycle) {
__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,100-duty_cycle);
}

////-------------------------------------------------------------------------------
----------------------------------
// Kiem tra co line den khong de chay
void no_line_no_run() {
//AdcValue nho hon 1000 thi dang o line trang
//Tay ca sensor
if((AdcValues[0] <= 600 && AdcValues[1] <= 600 && AdcValues[2] <= 600 &&
AdcValues[3] <= 600 && AdcValues[4] <= 600))
{
stop = 1;
outline = 1;
}
else outline =0;
if(empty ==1 && flag ==0 &&((AdcValues[0] >= 1000 && AdcValues[1] >= 1000 &&
AdcValues[4]<1000)
||(AdcValues[1] >= 1000 && AdcValues[2] >= 1000 &&
AdcValues[4]<1000)
||(AdcValues[2] >= 1000 && AdcValues[3] >= 1000 &&
AdcValues[0]<1000)
||(AdcValues[2] >= 1000 && AdcValues[0] <= 1000 &&
AdcValues[4] <= 1000 && AdcValues[3] <= 1000 && AdcValues[1] <= 1000)
|| (AdcValues[3]>1000 && AdcValues[4] >= 1000 &&
AdcValues[0]<1000)))
{
stop=0;
 cho=1;
flag =1;
}

}
////-------------------------------------------------------------------------------
-----------------------------------
//-------------------------Giam toc--------------------------
//void Speed_down()
//{
// if(below ==1 && below_road ==0 && m==1)
// {
// m=2;
// setpoint_l =96;
// setpoint_r = 78;
// }
// if(m ==2)
// {
// m=3;
// for(int q =0; q<=100; q++)
// {
// setpoint_l=84;
// setpoint_r =92;
// }
// if(m ==3 || vtb > 0.41)
// {
// for (int w=0; w<= 20; w++)
// {
// setpoint_l =54;
// setpoint_r = 109;
// }
// m=0;
// }
// }
//}
//--------------------------------------------
////Ham kiem tra nen chay hay dung khi den noi nhan hang
void stop_or_run()
{
//Xem mấy cảm biến nhận
if(empty==1 &&((AdcValues[0]>=1000 &&AdcValues[1] >= 1000&& AdcValues[2] >= 1000 &&
AdcValues[4]<=1000 ) //111_0
||(AdcValues[0]<=1000 && AdcValues[1] >= 1000 && AdcValues[2] >= 1000
&& AdcValues[3] >= 1000 && AdcValues[4]<=1000)//1110
||(AdcValues[0]<=1000 && AdcValues[2] >= 1000 && AdcValues[3] >= 1000
&& AdcValues[4] >= 1000) //00111
||(AdcValues[0]<=1000 && AdcValues[1] >= 1000 && AdcValues[2] >= 1000
&& AdcValues[3] >= 1000 && AdcValues[4] >= 1000)
||(AdcValues[0]>=1000 && AdcValues[1] >= 1000 && AdcValues[2] >= 1000
&& AdcValues[3] >= 1000 && AdcValues[4] <= 1000)
)) //01111)
{
stop = 1;
}
//Nen de xe doc gia tri thoi gian khoang 1s khi gia tri ADC lien tuc phu hop
stop =1;
}
//day cam bien line duoc xac dinh gia tri

////----------------------------------------SENSOR
CALIB--------------------------------------------------------------------------
//sensorline
void CaculatorFuntionSensor(void)
{
A[0] = (float) ( Ymax - Ymin ) / ( ADCMaxValue[0] - ADCMinValue[0] );
A[1] = (float) ( Ymax - Ymin ) / ( ADCMaxValue[1] - ADCMinValue[1] );
A[2] = (float) ( Ymax - Ymin ) / ( ADCMaxValue[2] - ADCMinValue[2] );
A[3] = (float) ( Ymax - Ymin ) / ( ADCMaxValue[3] - ADCMinValue[3] );
A[4] = (float) ( Ymax - Ymin ) / ( ADCMaxValue[4] - ADCMinValue[4] );
}
void CalibrationSenrsorValue(uint16_t *ADCValue)
{
AdcValuesAfterCalib[0] = (uint16_t) (Ymin + A[0]*( ADCValue[0] -
ADCMinValue[0]));
AdcValuesAfterCalib[1] = (uint16_t) (Ymin + A[1]*( ADCValue[1] -
ADCMinValue[1]));
AdcValuesAfterCalib[2] = (uint16_t) (Ymin + A[2]*( ADCValue[2] -
ADCMinValue[2]));
AdcValuesAfterCalib[3] = (uint16_t) (Ymin + A[3]*( ADCValue[3] -
ADCMinValue[3]));
AdcValuesAfterCalib[4] = (uint16_t) (Ymin + A[4]*( ADCValue[4] -
ADCMinValue[4]));
}
void CalibSensorDependOnEnvironment(void)
{
for(int i = 0; i < 5; i++)
{
if(AdcValues[i] != 0)
{
if(AdcValues[i] > ADCMaxValue[i])
ADCMaxValue[i] = AdcValues[i];
if(AdcValues[i] < ADCMinValue[i])
ADCMinValue[i] = AdcValues[i];
}
sumADCmax += ADCMaxValue[i];
sumADCmin += ADCMinValue[i];
}
Ymax = sumADCmax/5;
Ymin = sumADCmin/5;
sumADCmax = 0;
sumADCmin = 0;
}
float ErrorLine(void)
{
CalibrationSenrsorValue(AdcValues);
sum_Adcvalues = (AdcValuesAfterCalib[0] + AdcValuesAfterCalib[1] +
AdcValuesAfterCalib[2] + AdcValuesAfterCalib[3] + AdcValuesAfterCalib[4]);
e2_line = (2*(AdcValuesAfterCalib[4] - AdcValuesAfterCalib[0]) +
(AdcValuesAfterCalib[3] - AdcValuesAfterCalib[1]))*17/sum_Adcvalues;
e2_line = (e2_line + 1.25977)/1.241049;
return e2_line;
//Sai so cua line
}
//-----------------------------------------------------------------------
//-----------------Speed-------------------//
void find_speed(void)
{
enc_right =__HAL_TIM_GET_COUNTER(&htim4); //v2 banh right
if (enc_right < enc_pre_right) {
delta_enc_right = 65535 -enc_pre_right +enc_right ;
}
else {
delta_enc_right = enc_right - enc_pre_right;
}
enc_left =__HAL_TIM_GET_COUNTER(&htim3); //v1 banh left
if (enc_left < enc_pre_left) {
delta_enc_left = 65535 -enc_pre_left +enc_left ;
}
else {
delta_enc_left = enc_left - enc_pre_left;
}
rpm_right = 60 * (delta_enc_right) / (1980 * Ts);
rpm_left = 60 * (delta_enc_left) / (1980 * Ts);
enc_pre_left =enc_left;
enc_pre_right =enc_right;
v_A = abs_(rpm_left*2*PI*r/60); //m/s
v_B = abs_(rpm_right*2*PI*r/60); //m/s
vtb = (v_A+v_B)/2; //m/s

}
//-------------Control motor right-----//
float PI_Motor_Right(float right, float exp_right)
{
err_right_pre = err_right;

err_right_pre_pre = err_right_pre;

err_right = exp_right-right;
duty_right_pre=duty_right;

P_right = kp_right*(err_right-err_right_pre);
I_right = 0.5*ki_right*(err_right + err_right_pre)*Ts;
duty_right= duty_right_pre + P_right+ I_right;

if(duty_right>100) return 99;

if(duty_right<0) return 0;
return duty_right;
}
//-------------Control motor left-----//
float PI_Motor_Left(float left, float exp_left)
{
err_left_pre = err_left;

err_left_pre_pre = err_left_pre;

err_left = exp_left-left;

duty_left_pre = duty_left;

P_left = kp_left*(err_left-err_left_pre);
I_left = 0.5*ki_left*(err_left + err_left_pre)*Ts;
duty_left= duty_left_pre+ P_left+I_left;

if(duty_left>100) return 99;

if(duty_left<0) return 0;
 return duty_left;
}

//-------------------------------------
void find_e2_e3(void) {
CalibSensorDependOnEnvironment();
CaculatorFuntionSensor();
CalibrationSenrsorValue(AdcValues);
e1_line = 0;

e2=ErrorLine();//m
if(vtb != 0){
e3 = atan((e2 - pre_e2)/ (vtb*0.01*1000));//rad
}
else e3 = 0;
}
void stepping1(void)//*
{
wr = -e3;
v = vr * cos(e3) + k1 * e1_line; //m/s vr gia tri mong muon
w = wr + vr * k2 * e2/1000 + k3 * vr * sin(e3); //rad/s wr là gì
setpoint_r = (1 / r) * (v - b*w/2) * (60 / (2 * PI)); //rpm
setpoint_l = (1 / r) * (v + b*w/2) * (60 / (2 * PI)); //rpm
if(setpoint_l<0){
setpoint_l = 0;
}
if(setpoint_r < 0) {
setpoint_r = 0;
}
if(setpoint_r > 133) {
setpoint_r = 133;
}
if(setpoint_l>133){
setpoint_l = 133;
}
}

//Sai so cua he thong

void yes_or_no_good()
{

if( current_weight> 750000 && current_weight < 1400000)

{
//mass1== 1&& mass2==0
top_road = 1;
below_road = 0;
empty = 0;
//11.5v
cho=0;
k2=450;
k3=9;
// k3=8.842;
// k3=7;
// vr=0.38;

}
else
{
if(current_weight > 1600000 )
 {

top_road = 0;
below_road = 1;
empty = 0;
// stop=0;
cho=0;
k2 =550;
k3 = 7;
// k2 =210;
// k3=9.9;
vr=0.38;
}
}

}
//--------------Loadcell
int32_t getHX711(void)
{
uint32_t data = 0;
uint32_t startTime = HAL_GetTick();
while(HAL_GPIO_ReadPin(DT_PORT, DT_PIN) == GPIO_PIN_SET)
{
if(HAL_GetTick() - startTime > 200)
return 0;
}
for(int8_t len=0; len<24 ; len++)
{
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_SET);
// microDelay(1);
data = data << 1;
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_RESET);
//microDelay(1);
if(HAL_GPIO_ReadPin(DT_PORT, DT_PIN) == GPIO_PIN_SET)
data ++;
}
data = data ^ 0x800000;
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_SET);
// microDelay(1);
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_RESET);
// microDelay(1);
return data;
}

int weigh()
{
int32_t total = 0;
int32_t samples = 15;
int milligram;
float coefficient;
for(uint16_t i=0 ; i<samples ; i++)
{
total += getHX711();
}
int32_t average = (int32_t)(total / samples);
coefficient = knownOriginal / knownHX711;
milligram = (int)(average-tare)*coefficient;
return milligram;
}
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/

/* USER CODE BEGIN 0 */

//---------------------------------------------Main ngat ra thuc hien chuong

trinh------------
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
if (htim->Instance == TIM2) {
counter++;
if( below==1 && below_road ==0)
{
for(int cham =0; cham<=1; cham++ ){
setpoint_l = 75;
setpoint_r=93;
}

}
// Speed_down();
//--------------no line no run and stop to get goods-------------
no_line_no_run();
stop_or_run(); //den vach tai hang hoac ra khoi line se dung
'stop = 1'
//-----------------------
// if((below_road==1 || top_road ==1) && vtb >0.475)
// {
// setpoint_l =85;
// setpoint_r =85;
// }

//-------------------Nga 3 re huong
---------------------------------------------------
if((top_road == 1 || below_road ==1) && outline == 0)
{
stop = 0;
////Tại ngã 3 cb rẽ nhánh
if(empty == 0 && (
(AdcValues[0] < 1000 &&
AdcValues[1] > 1000 && AdcValues[2] < 1000 && AdcValues[3] > 1000 )
//0101_ ok
|| (AdcValues[0] < 1000
&& AdcValues[1] > 1100 && AdcValues[2] > 1100 && AdcValues[3] < 1000 &&
AdcValues[4] > 1000) //01101 ok
|| (AdcValues[0] < 1000
&& AdcValues[1] > 1100 && AdcValues[2] > 1100 && AdcValues[3] < 1000 &&
AdcValues[4] > 1000) //01101 ok
// || (AdcValues[0] < 1000
&& AdcValues[1] > 1000 && AdcValues[2] < 1000 && AdcValues[3] > 1000 &&
AdcValues[4] > 1000) //01011 ok
|| (AdcValues[0] < 1000
&& AdcValues[1] < 1000 && AdcValues[3] > 1000 && AdcValues[4] > 1000)
//00_11 ok

|| (AdcValues[1] > 1100 && AdcValues[0] < 1000 && AdcValues[4] < 1000
&& AdcValues[3] > 1100) //01_10

||(AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3] > 1100 &&
AdcValues[4] > 1000) //_0011 ok
 || (AdcValues[0] > 1100
&& AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3] < 1000 &&
AdcValues[4] > 1000) //10001 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3] < 1000 &&
AdcValues[4] > 1000) //10001 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3] > 1000 &&
AdcValues[4] > 1000) //10011 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] > 1100 && AdcValues[2] < 1000 && AdcValues[4] < 1000)
//110_0 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] > 1100 && AdcValues[2] < 1100 && AdcValues[3] > 1100 &&
AdcValues[4] > 1000) //11011 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] < 1000 && AdcValues[2] > 1100 && AdcValues[3] > 1100 &&
AdcValues[4] < 1000) //10110 ok
|| (AdcValues[0] < 1000
&& AdcValues[1] > 1100 && AdcValues[2] > 1100 && AdcValues[3] < 1000 &&
AdcValues[4] > 1000) //11001 ok
|| (AdcValues[0] > 1100
&& AdcValues[1] > 1000 && AdcValues[2] > 1000 && AdcValues[3] < 1000 )
//1110_ ok
|| (AdcValues[0] > 1100
&& AdcValues[1] < 1000 && AdcValues[3] < 1000 && AdcValues[4] > 1100)
//10_01
|| (AdcValues[0] > 1100
&& AdcValues[1] > 1100 && AdcValues[3] > 1100 && AdcValues[4] > 1100 &&
AdcValues[2] < 1000))) //11110 ok

{
if(top_road == 1)
{
if((AdcValues[0] <
1000 && AdcValues[1] > 1000 && AdcValues[2] > 1000 && AdcValues[3] < 1000 &&
AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] > 1000 && AdcValues[3]
> 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] > 1000 && AdcValues[2] < 1000 && AdcValues[3]
< 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] > 1000 && AdcValues[3]
< 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3]
> 1000 && AdcValues[4] < 1000))
{top_road=0;}
for(int z =0;
z<=7; z++ ){
setpoint_r=109;
setpoint_l = 48;
}
}
if(below_road == 1)
{
 if((AdcValues[0] <
1000 && AdcValues[1] > 1000 && AdcValues[2] > 1000 && AdcValues[3] < 1000 &&
AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] > 1000 && AdcValues[3]
> 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] > 1000 && AdcValues[2] < 1000 && AdcValues[3]
< 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] > 1000 && AdcValues[3]
< 1000 && AdcValues[4] < 1000)
||
(AdcValues[0] < 1000 && AdcValues[1] < 1000 && AdcValues[2] < 1000 && AdcValues[3]
> 1000 && AdcValues[4] < 1000))

{below_road=0;}
for(int z =0;
z<=5; z++ ){

setpoint_l = 107; //+-2

setpoint_r=66;

}
// for(int cham =0;
cham<=1; cham++ ){
// setpoint_l =
72;
//
setpoint_r=90;
// }
below =1;
// m=1;
// below_road=0;
//k2 = 400;
//k3 =15;
//k2=550;
//k3=15;
}// DK below
} // DK nga 3
}// Khi bat dau co hang

//-----------------------------find error------------------------------
find_e2_e3();
//Speed
find_speed();
if(counter ==10)
{
// sau 100ms thì sẽ trả tín hiệu của DK hệ thống
counter =0;
//fl tracking (lyapunov)
stepping1();
}
//Xuat PWM
PWM_right= PI_Motor_Right(rpm_right, setpoint_r);
PWM_left=PI_Motor_Left(rpm_left, setpoint_l);
pre_e2 = e2;
 if(stop ==0)
{
Set_PWM_DutyCycle_left(PWM_left);
Set_PWM_DutyCycle_right(PWM_right);
}
else{
TIM1->CCR3 = 99;
TIM1->CCR4 = 99;
duty_right_pre=0;
duty_left_pre=0;
duty_right=0;
duty_left=0;
}
} // Ngat TIM2
} // End Period...

/* 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_TIM1_Init();
MX_TIM3_Init();
MX_ADC1_Init();
MX_TIM4_Init();
MX_TIM2_Init();
/* USER CODE BEGIN 2 */
//He thong cho ngat
HAL_TIM_Base_Start_IT(&htim2);

//Set_up cho DC motor

 HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1 | TIM_CHANNEL_2); //Motor_1
HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1 | TIM_CHANNEL_2); //Motor_2
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
// HAL_TIM_Base_Start_IT(&htim1);
//Doc gia tri khoi luong
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(SCK_PORT, SCK_PIN, GPIO_PIN_RESET);
//dò line
HAL_ADC_Start_DMA(&hadc1,(uint32_t*)AdcValues,5);
CalibSensorDependOnEnvironment();
CaculatorFuntionSensor();

//CCR thay doi do rong Duty cycle (%), CNT dem den CCR thi PWM ve muc 0

/* USER CODE END 2 */

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

/* USER CODE BEGIN 3 */

//De kiem tra có Goods hay chưa

if( cho==1 && stop ==1){
if(weigh()>5000)
{
current_weight=weigh();
yes_or_no_good();
}
}

/* USER CODE END 3 */

}

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

/** Initializes the RCC Oscillators according to the specified parameters

* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.[Link] = RCC_PLL_ON;
RCC_OscInitStruct.[Link] = RCC_PLLSOURCE_HSE;
 RCC_OscInitStruct.[Link] = RCC_PLL_MUL2;
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_DIV2;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)

{
Error_Handler();
}
[Link] = RCC_PERIPHCLK_ADC;
[Link] = RCC_ADCPCLK2_DIV2;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}

/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC1_Init(void)
{

/* USER CODE BEGIN ADC1_Init 0 */

/* USER CODE END ADC1_Init 0 */

ADC_ChannelConfTypeDef sConfig = {0};

/* USER CODE BEGIN ADC1_Init 1 */

/* USER CODE END ADC1_Init 1 */

/** Common config

*/
[Link] = ADC1;
[Link] = ADC_SCAN_ENABLE;
[Link] = ENABLE;
[Link] = DISABLE;
[Link] = ADC_SOFTWARE_START;
[Link] = ADC_DATAALIGN_RIGHT;
[Link] = 5;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
Error_Handler();
}
 /** Configure Regular Channel
*/
[Link] = ADC_CHANNEL_1;
[Link] = ADC_REGULAR_RANK_1;
[Link] = ADC_SAMPLETIME_55CYCLES_5;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}

/** Configure Regular Channel

*/
[Link] = ADC_CHANNEL_2;
[Link] = ADC_REGULAR_RANK_2;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}

/** Configure Regular Channel

*/
[Link] = ADC_CHANNEL_3;
[Link] = ADC_REGULAR_RANK_3;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}

/** Configure Regular Channel

*/
[Link] = ADC_CHANNEL_4;
[Link] = ADC_REGULAR_RANK_4;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}

/** Configure Regular Channel

*/
[Link] = ADC_CHANNEL_5;
[Link] = ADC_REGULAR_RANK_5;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC1_Init 2 */

/* USER CODE END ADC1_Init 2 */

/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
{
/* USER CODE BEGIN TIM1_Init 0 */

/* USER CODE END TIM1_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

/* USER CODE BEGIN TIM1_Init 1 */

/* USER CODE END TIM1_Init 1 */

[Link] = TIM1;
[Link] = 7;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 99;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = 0;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_OCMODE_PWM1;
[Link] = 0;
[Link] = TIM_OCPOLARITY_HIGH;
[Link] = TIM_OCNPOLARITY_HIGH;
[Link] = TIM_OCFAST_DISABLE;
[Link] = TIM_OCIDLESTATE_SET;
[Link] = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_OSSR_DISABLE;
[Link] = TIM_OSSI_DISABLE;
[Link] = TIM_LOCKLEVEL_OFF;
[Link] = 0;
[Link] = TIM_BREAK_DISABLE;
[Link] = TIM_BREAKPOLARITY_HIGH;
 [Link] = TIM_AUTOMATICOUTPUT_DISABLE;
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */

/* USER CODE END TIM1_Init 2 */

HAL_TIM_MspPostInit(&htim1);

/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{

/* USER CODE BEGIN TIM2_Init 0 */

/* USER CODE END TIM2_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM2_Init 1 */

/* USER CODE END TIM2_Init 1 */

[Link] = TIM2;
[Link] = 799;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 99;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */

/* USER CODE END TIM2_Init 2 */

/**
* @brief TIM3 Initialization Function
 * @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{

/* USER CODE BEGIN TIM3_Init 0 */

/* USER CODE END TIM3_Init 0 */

TIM_Encoder_InitTypeDef sConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM3_Init 1 */

/* USER CODE END TIM3_Init 1 */

[Link] = TIM3;
[Link] = 0;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 65535;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
[Link] = TIM_ENCODERMODE_TI12;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM3_Init 2 */

/* USER CODE END TIM3_Init 2 */

/**
* @brief TIM4 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM4_Init(void)
{

/* USER CODE BEGIN TIM4_Init 0 */

/* USER CODE END TIM4_Init 0 */

 TIM_Encoder_InitTypeDef sConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM4_Init 1 */

/* USER CODE END TIM4_Init 1 */

[Link] = TIM4;
[Link] = 0;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 65535;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
[Link] = TIM_ENCODERMODE_TI12;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK)
{
Error_Handler();
}
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM4_Init 2 */

/* USER CODE END TIM4_Init 2 */

/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{

/* DMA controller clock enable */

__HAL_RCC_DMA1_CLK_ENABLE();

/* DMA interrupt init */

/* DMA1_Channel1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);

/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */

/* GPIO Ports Clock Enable */

__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();

/*Configure GPIO pin Output Level */

HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);

/*Configure GPIO pin Output Level */

HAL_GPIO_WritePin(SCK_GPIO_Port, SCK_Pin, GPIO_PIN_RESET);

/*Configure GPIO pin : PC13 */

GPIO_InitStruct.Pin = GPIO_PIN_13;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

/*Configure GPIO pin : DT_Pin */

GPIO_InitStruct.Pin = DT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(DT_GPIO_Port, &GPIO_InitStruct);

/*Configure GPIO pin : SCK_Pin */

GPIO_InitStruct.Pin = SCK_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(SCK_GPIO_Port, &GPIO_InitStruct);

/* USER CODE BEGIN MX_GPIO_Init_2 */

/* USER CODE END MX_GPIO_Init_2 */
}

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