121 lines
2.8 KiB
C
121 lines
2.8 KiB
C
#include "Driver_GPIO.h"
|
|
#include "MyTimer.h"
|
|
#include "MyADC.h"
|
|
#include "stm32f10x.h"
|
|
#include "bordage.h"
|
|
|
|
/********** PWM **********/
|
|
#define TIMER_PWM (TIM3)
|
|
#define CANAL_PWM (4)
|
|
#define GPIO_PWM (GPIOB)
|
|
#define GPIO_PIN_PWM (1)
|
|
/*************************/
|
|
|
|
/********** Codeur Incrémental **********/
|
|
#define TIMER_CI (TIM4)
|
|
#define GPIO_GIROUETTE (GPIOB)
|
|
#define GIROUETTE_PHA (6) //PB6
|
|
#define GIROUETTE_PHB (7) //PB7
|
|
#define GIROUETTE_INDEX (5) //PB5
|
|
/****************************************/
|
|
|
|
int tempo_chavirement = 0;
|
|
|
|
void IT_bordage_auto ( void) {
|
|
|
|
int angle_girouette;
|
|
|
|
if (tempo_chavirement > 0){
|
|
tempo_chavirement--;
|
|
return;
|
|
}
|
|
|
|
angle_girouette = TIMER_CI->CNT/4;
|
|
bordage(calcul_angle_voile(angle_girouette));
|
|
}
|
|
|
|
void init_bordage ( void )
|
|
{
|
|
MyGPIO_Struct_TypeDef GPIO_Struct;
|
|
|
|
// Configuration du timer avec une période de 20ms
|
|
MyTimer_Struct_TypeDef TIM;
|
|
TIM.Timer = TIMER_PWM;
|
|
TIM.ARR = 59999;
|
|
TIM.PSC = 23;
|
|
MyTimer_Base_Init(&TIM);
|
|
|
|
// Configuration du GPIO sur lequel sort la PWM
|
|
GPIO_Struct.GPIO = GPIO_PWM;
|
|
GPIO_Struct.GPIO_Pin = GPIO_PIN_PWM;
|
|
GPIO_Struct.GPIO_Conf = AltOut_Ppull;
|
|
MyGPIO_Init(&GPIO_Struct);
|
|
|
|
MyTimer_PWM (TIMER_PWM, CANAL_PWM);
|
|
|
|
}
|
|
|
|
int bordage ( int angle ) {
|
|
|
|
float angle_servo = 90.0 - angle;
|
|
|
|
float duty_cycle = angle_servo/18.0 + 5.0; // convertit l'angle en rapport cyclique pour la commande du servo moteur
|
|
|
|
// Génération de la PWM
|
|
Set_Duty_Cycle(TIMER_PWM, CANAL_PWM, duty_cycle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void init_codeur_incr ( void ) {
|
|
// Configuration GPIOS
|
|
MyGPIO_Struct_TypeDef GPIO_Struct;
|
|
GPIO_Struct.GPIO = GPIO_GIROUETTE;
|
|
GPIO_Struct.GPIO_Pin = GIROUETTE_PHA;
|
|
GPIO_Struct.GPIO_Conf = In_Floating;
|
|
MyGPIO_Init(&GPIO_Struct);
|
|
|
|
GPIO_Struct.GPIO_Pin = GIROUETTE_PHB;
|
|
GPIO_Struct.GPIO_Conf = In_Floating;
|
|
MyGPIO_Init(&GPIO_Struct);
|
|
|
|
GPIO_Struct.GPIO_Pin = GIROUETTE_INDEX;
|
|
GPIO_Struct.GPIO_Conf = In_Floating;
|
|
MyGPIO_Init(&GPIO_Struct);
|
|
|
|
init_exti_interrupt();
|
|
MyTimer_EncoderMode_Conf(TIMER_CI);
|
|
|
|
}
|
|
void init_exti_interrupt ( void ) {
|
|
RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
|
|
RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
|
|
AFIO->EXTICR[1] |= AFIO_EXTICR2_EXTI5_PB;
|
|
EXTI->RTSR |= EXTI_RTSR_TR5; // Rising trigger enabled
|
|
EXTI->IMR |= EXTI_IMR_MR5; // Interrupt request from Line 5 is not masked
|
|
NVIC->IP[23] |= (1 << 4); // Fixe la priorité de l'interruption dans le NVIC
|
|
NVIC->ISER[0] |= (1 << 23); // Autorise la prise en compte de l'interruption dans le NVIC
|
|
}
|
|
|
|
void EXTI9_5_IRQHandler ( void ) {
|
|
EXTI->PR |= EXTI_PR_PR5; // Clear the pending bit
|
|
TIMER_CI->CNT = 0;
|
|
}
|
|
|
|
void Roulis_Handler ( void )
|
|
{
|
|
tempo_chavirement = 50; // 50 appels à 40ms = 2000 ms = 2 sec.
|
|
bordage(0);
|
|
}
|
|
|
|
int calcul_angle_voile ( int angle ) {
|
|
if (angle <= 45 || angle >= 360 - 45){
|
|
return 0;
|
|
}
|
|
else if ( angle > 45 && angle <= 180) {
|
|
return (angle - 45) * 90.0/135.0;
|
|
}
|
|
else {
|
|
return (315 - angle) * 90.0/135.0;
|
|
}
|
|
}
|