Sei sulla pagina 1di 10

#include "stm32f4xx.

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);

/* Configure PB0 pin as input floating */


GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_Init(GPIOA, &GPIO_InitStructure);

/* Connect EXTI Line0 to PB0 pin */


SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource4);

/* Configure EXTI Line0 */


EXTI_InitStructure.EXTI_Line = EXTI_Line4;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);

/* Enable and set EXTI Line0 Interrupt to the lowest priority */


NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void kich_xung(int soxung, int f){

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);

/* GPIOC Configuration: TIM3 CH1 (PC6) and TIM3 CH2 (PC7) */


GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 ;
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 ;
GPIO_Init(GPIOC, &GPIO_InitStructure);

/* Connect TIM3 pins to AF2 */


GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource7, GPIO_AF_TIM3);

/* Time base configuration */


TIM_TimeBaseStructure.TIM_Period = 250; //1us*200=0.2ms 5kz
TIM_TimeBaseStructure.TIM_Prescaler = 83; //Tan so 1MHz : 84000000/84=1Mhz
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

/* PWM1 Mode configuration: Channel 1 */


TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = 0;

TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC1Init(TIM3, &TIM_OCInitStructure);

/* PWM1 Mode configuration: Channel 2 */

TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_OC2Init(TIM3, &TIM_OCInitStructure);

/* TIM3 enable counter */


TIM_Cmd(TIM3, ENABLE);
TIM_CtrlPWMOutputs(TIM3, ENABLE);
}

void motor(int direction, int pwm, int time)


{
if(direction == FORWARD)
{
GPIO_SetBits(GPIOC, GPIO_Pin_8);
GPIO_ResetBits(GPIOC, GPIO_Pin_9);
}

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);

//Cai dat cac led


GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11|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);
//Chan xuat xung cho robot

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);

}
}

void INIT_USART1(uint32_t baudrate)


{

/* This is a concept that has to do with the libraries provided by ST


* to make development easier the have made up something similar to
* classes, called TypeDefs, which actually just define the common
* parameters that every peripheral needs to work correctly
*
* They make our life easier because we don't have to mess around with
* the low level stuff of setting bits in the correct registers
*/
GPIO_InitTypeDef GPIO_InitStruct; // this is for the GPIO pins used as TX and
RX
USART_InitTypeDef USART_InitStructure; // this is for the USART1
initilization
NVIC_InitTypeDef NVIC_InitStructure; // this is used to configure the NVIC
(nested vector interrupt controller)

/* enable APB2 peripheral clock for USART1


* note that only USART1 and USART6 are connected to APB2
* the other USARTs are connected to APB1
*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

/* enable the peripheral clock for the pins used by


* USART1, PB6 for TX and PB7 for RX

USART3, PB10 for TX and PB11 for RX

*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

/* This sequence sets up the TX and RX pins


* so they work correctly with the USART1 peripheral
*/
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; // Pins 6 (TX) and 7
(RX) are used
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; // the pins are
configured as alternate function so the USART peripheral has access to them
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; // this defines the IO
speed and has nothing to do with the baudrate!
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; // this defines
the output type as push pull mode (as opposed to open drain)
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; // this activates the
pullup resistors on the IO pins
GPIO_Init(GPIOB, &GPIO_InitStruct); // now all the
values are passed to the GPIO_Init() function which sets the GPIO registers

/* The RX and TX pins are now connected to their AF


* so that the USART1 can take over control of the
* pins
*/
GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3); //
GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_USART3);

/* Now the USART_InitStruct is used to define the


* properties of USART1
*/
USART_InitStructure.USART_BaudRate = baudrate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART3, &USART_InitStructure); // again all the properties are
passed to the USART_Init function which takes care of all the bit setting

/* Here the USART1 receive interrupt is enabled


* and the interrupt controller is configured
* to jump to the USART1_IRQHandler() function
* if the USART1 receive interrupt occurs
*/
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); // enable the USART1 receive
interrupt
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; // we want to
configure the USART1 interrupts
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;// this sets the
priority group of the USART1 interrupts
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // this sets the
subpriority inside the group
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; // the USART1
interrupts are globally enabled
NVIC_Init(&NVIC_InitStructure); //
the properties are passed to the NVIC_Init function which takes care of the low
level stuff

// finally this enables the complete USART1 peripheral


USART_Cmd(USART3, 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;

if(ref < maxSetPointError)


{
direction = STOP;
pwm = 0;
lastError = 0;
sumError = 0;
}

else
{
lastError = error;
sumError += error;

if(sumError > iMax) sumError = iMax;


if(sumError < iMin) sumError = iMin;

if(output > 0) direction = FORWARD;


if(output < 0)
{
direction = BACKWARD;
output = -1 * output;
}

//pwm = map(output, 0, outputMax, pwmMin, pwmMax);


}

motor(direction, pwm, 0);


}
/*
void GPIO_Configuration()
{
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA ,ENABLE);
// TIM2 CH1 AND CH2 pin (PA0 & PA1) configuration AS input Encoder1 Chanel A & B
GPIO_InitStructure. GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure. GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure. GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

// Time base configuration


TIM_DeInit(TIM2);
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_Cmd(TIM2, ENABLE);
//@brief TIM Input Capture Init structure definition
TIM_ICInitStructure.TIM_Channel=TIM_Channel_2|TIM_Channel_3;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; // Chon canh tin
hieu dau vao @defgroup TIM_Input_Capture_Polarity
TIM_ICInitStructure.TIM_ICFilter=15;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICSelection=TIM_ICSelection_DirectTI;
TIM_ICInit(TIM2, &TIM_ICInitStructure);

TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Falling,


TIM_ICPolarity_Falling);

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;

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6| GPIO_Pin_7;


GPIO_Init(GPIOB, &GPIO_InitStructure);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_TIM4);


GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_TIM4);

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);

/* Configure the timer */


TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising,
TIM_ICPolarity_Rising);
TIM_SetAutoreload (TIM4, 0xffff);
TIM4->CNT = 0;
TIM_Cmd(TIM4,ENABLE);
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);

/* TIM4 counter enable */


// TIM_Cmd(TIM4, ENABLE);
TIM_SetCounter(TIM4,0);

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

Potrebbero piacerti anche