Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
h"
#include <stdio.h>
#define FORWARD 1
#define BACKWARD 2
#define STOP 3
//#define LEFT_SPEED TIM3->CCR2
#define RIGHT_SPEED TIM3->CCR1
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
volatile char received_string[12];
volatile char data;
int vitri=0;
int32_t NumInPulse;
int32_t pulse;
int32_t pulse1;
void Delay(__IO uint32_t nCount);
void PWM_MOTOR_Config();
void GPIO_Configuration();
void configureEncoder();
void motor(int direction, int pwm, int time);
void pid_controller(int vitri);
void INIT_USART1(uint32_t baudrate);
void LED_Config();
__IO int32_t xung_encoder;
float KP = 1;
float KI = 0.6;
float KD = 2;
int lastError = 0;
int sumError = 0;
int iMax = 1000;
int iMin = -1000;
int setpoint = 6;
int posMin = 100;
int posMax = 6000;
int pwmMin = 50;
int pwmMax = 1000;
//int errorMax = posMax - posMin;
int errorMax = 5900;
float maxSetPointError = 0.3;
double distance = 0;
void EXTILine4_Config(void)
{
/* Enable GPIOB clock */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
/* Enable SYSCFG clock */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
while(soxung>0){
GPIO_SetBits(GPIOA, GPIO_Pin_5);
Delay(f);
GPIO_ResetBits(GPIOA, GPIO_Pin_5);
Delay(f);
soxung--;
}
void PWM_MOTOR_Config()
{
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
/*Configuration DIR MOTOR LEFT & MOTOR RIGHT */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
if(direction == BACKWARD)
{
GPIO_SetBits(GPIOC, GPIO_Pin_9);
GPIO_ResetBits(GPIOC, GPIO_Pin_8);
}
if(direction == STOP)
{
GPIO_ResetBits(GPIOC, GPIO_Pin_8);
GPIO_ResetBits(GPIOC, GPIO_Pin_9);
pwm = 0;
}
// analogWrite(motor1PWM, pwm);
RIGHT_SPEED = pwm;
// if(time > 0) delay(time);
}
void GPIO_Configuration(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
void main()
{
EXTILine4_Config();
GPIO_Configuration();
configureEncoder();
PWM_MOTOR_Config();
INIT_USART1(9600);
LED_Config();
//motor(2, 500, 0);
while(1)
{
// GPIO_SetBits(GPIOD, GPIO_Pin_15);
if(data == '1'){
GPIO_SetBits(GPIOD, GPIO_Pin_15);
vitri = 2000;
}
if(data== '2'){
GPIO_ResetBits(GPIOD, GPIO_Pin_15);
vitri = 0;
}
xung_encoder = TIM_GetCounter(TIM4)+NumInPulse;
pid_controller(vitri);
// Delay(10000000);
// pid_controller(0);
// Delay(10000000);
}
}
*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
void LED_Config()
{
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
/* Configure B0->B15 in output pushpull mode LED LED HIEN THI*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|
GPIO_Pin_14|GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}
void pid_controller(int vitri)
{
setpoint = vitri;
distance = xung_encoder;
float error = distance - setpoint;
int outputMax = KP * errorMax + KI * iMax;
int output = KP * error + KI * (sumError) + KD * (error - lastError);
float ref = 0;
if(error <= 0.6)
ref = error * -1;
else
ref = error;
int direction = 0;
int pwm = 0;
else
{
lastError = error;
sumError += error;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
TIM_SetCounter(TIM2,1000);
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
}
*/
void configureEncoder()
{
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void Delay(__IO uint32_t nCount)
{
while(nCount--)
{
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t* file, uint32_t line)
{
/* 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) */
/* Infinite loop */
while (1)
{
}
}
#endif