diff --git a/MDK-ARM/Project.uvoptx b/MDK-ARM/Project.uvoptx
index afd2490..906c3fb 100644
--- a/MDK-ARM/Project.uvoptx
+++ b/MDK-ARM/Project.uvoptx
@@ -75,7 +75,7 @@
1
0
- 1
+ 0
18
@@ -352,7 +352,7 @@
1
0
- 0
+ 1
18
@@ -397,7 +397,7 @@
0
DLGDARM
- (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=1554,213,1920,450,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=129,555,687,1080,0)(121=924,444,1482,969,1)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=-1,-1,-1,-1,0)(131=-1,-1,-1,-1,0)(132=150,46,938,969,0)(133=-13,526,775,1449,1)(160=1046,190,1640,699,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=-1,-1,-1,-1,0)(230=-1,-1,-1,-1,0)(234=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=130,96,930,1019,0)(151=127,38,927,961,0)
+ (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=1554,213,1920,450,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=129,555,687,1080,0)(121=924,444,1482,969,0)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=-1,-1,-1,-1,0)(131=-1,-1,-1,-1,0)(132=150,46,938,969,0)(133=0,157,788,1080,0)(160=1046,190,1640,699,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=-1,-1,-1,-1,0)(230=-1,-1,-1,-1,0)(234=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=130,96,930,1019,0)(151=127,38,927,961,0)
0
@@ -430,24 +430,7 @@
-U-O142 -O2254 -S0 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128.FLM -FS08000000 -FL020000 -FP0($$Device:STM32F103RB$Flash\STM32F10x_128.FLM)
-
-
- 0
- 0
- 51
- 1
- 134221038
- 0
- 0
- 0
- 0
- 0
- 1
- ..\Src\Display.c
-
- \\NUCLEO_F103RB\../Src/Display.c\51
-
-
+
0
@@ -503,8 +486,8 @@
1
0
0
- 1
- 1
+ 0
+ 0
0
1
0
@@ -513,12 +496,12 @@
0
0
0
- 1
+ 0
0
- 1
+ 0
0
0
- 1
+ 0
0
0
0
diff --git a/MyDrivers/ADC.c b/MyDrivers/ADC.c
index 08d4387..59559a6 100644
--- a/MyDrivers/ADC.c
+++ b/MyDrivers/ADC.c
@@ -4,23 +4,23 @@
const float MAX_VOLTS = 3.3;
const float MAX_CONVERTED_VALUE = 4095.0;
-void ADC_conf(ADC_TypeDef *adc)
+void ADC_conf(ADC_TypeDef *adc)
{
if (adc == ADC1) {
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC1);
} else if (adc == ADC2) {
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC2);
}
-
+
// Division de la frequence
RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6;
-
- // Fixe le nombre de conversion à 1
- adc->SQR1 &= ADC_SQR1_L;
-
- // Calibration
+
+ // Fixe le nombre de conversion � 1
+ adc->SQR1 &= ADC_SQR1_L;
+
+ // Calibration (ne fonctionne pas, mais pas necessaire donc ok)
// adc->CR2 |= ADC_CR2_CAL_Msk;
-// while ((adc->CR2 & ADC_CR2_CAL_Msk));
+// while ((adc->CR2 & ADC_CR2_CAL_Msk));
}
@@ -32,9 +32,9 @@ void ADC_start(ADC_TypeDef *adc)
uint16_t ADC_readRaw(ADC_TypeDef *adc, int channel)
{
// Indique la voie a convertir
- adc->SQR3 = channel;
+ adc->SQR3 = channel;
// Lancement de la conversion
- adc->CR2 |= ADC_CR2_ADON;
+ adc->CR2 |= ADC_CR2_ADON;
while(!(adc->SR & ADC_SR_EOC)) {}
return adc->DR & ADC_DR_DATA_Msk;
diff --git a/MyDrivers/Timer.c b/MyDrivers/Timer.c
index 915f895..d5b6e71 100644
--- a/MyDrivers/Timer.c
+++ b/MyDrivers/Timer.c
@@ -10,10 +10,10 @@
// variable pointeur de fonction permettant de m�moriser le callback � appeler depuis
// le handler d'IT
-void (*it_callback_TIM1)(void);
-void (*it_callback_TIM2)(void);
-void (*it_callback_TIM3)(void);
-void (*it_callback_TIM4)(void);
+void (*it_callback_TIM1)(void);
+void (*it_callback_TIM2)(void);
+void (*it_callback_TIM3)(void);
+void (*it_callback_TIM4)(void);
void TIM1_UP_IRQHandler(void)
@@ -21,61 +21,42 @@ void TIM1_UP_IRQHandler(void)
// rabaisser le flag d'IT
LL_TIM_ClearFlag_UPDATE(TIM1);
(*it_callback_TIM1)();
-}
+}
void TIM2_IRQHandler(void)
{
// rabaisser le flag d'IT
LL_TIM_ClearFlag_UPDATE(TIM2);
(*it_callback_TIM2)();
-}
+}
void TIM3_IRQHandler(void)
{
// rabaisser le flag d'IT
LL_TIM_ClearFlag_UPDATE(TIM3);
(*it_callback_TIM3)();
-}
+}
void TIM4_IRQHandler(void)
{
// rabaisser le flag d'IT
LL_TIM_ClearFlag_UPDATE(TIM4);
(*it_callback_TIM4)();
-}
+}
-/**
- * @brief Autorise les interruptions
- * @note
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * @retval None
- */
void Timer_IT_enable(TIM_TypeDef * timer)
{
LL_TIM_EnableIT_UPDATE(timer);
}
-/**
- * @brief Interdit les interruptions
- * @note
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * @retval None
- */
void Timer_IT_disable(TIM_TypeDef * timer)
{
- LL_TIM_DisableIT_UPDATE(timer);
+ LL_TIM_DisableIT_UPDATE(timer);
}
-/**
- * @brief Configure le Timer consid�r� en interruption sur d�bordement.
- * @note A ce stade, les interruptions ne sont pas valid�s (voir MyTimer_IT_Enable )
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * void (*IT_function) (void) : nom (adresse) de la fonction � lancer sur interruption
- * int Prio : priorit� associ�e � l'interruption
- * @retval None
- */
+
void Timer_IT_conf(TIM_TypeDef * timer, void (*it_callback) (void), int prio)
{
// affectation de la fonction
@@ -84,80 +65,61 @@ void Timer_IT_conf(TIM_TypeDef * timer, void (*it_callback) (void), int prio)
else if (timer == TIM3) it_callback_TIM3 = it_callback;
else it_callback_TIM4 = it_callback;
-
- // Blocage IT (il faudra la d�bloquer voir fct suivante)
+
+ // Blocage IT (il faudra la debloquer voir fct suivante)
LL_TIM_DisableIT_UPDATE(timer);
-
+
// validation du canal NVIC
IRQn_Type TIM_irq;
-
+
if (timer == TIM1) TIM_irq = TIM1_UP_IRQn;
else if (timer == TIM2) TIM_irq = TIM2_IRQn;
else if (timer == TIM3) TIM_irq = TIM3_IRQn;
else TIM_irq = TIM4_IRQn;
-
+
NVIC_SetPriority(TIM_irq, prio);
NVIC_EnableIRQ(TIM_irq);
-
+
}
/****************************************************************************
* TIMER
***************************************************************************/
-/**
- * @brief D�marre le timer consid�r� et active les interruptions
- * @note
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * @retval None
- */
void Timer_start(TIM_TypeDef * timer)
{
LL_TIM_EnableCounter(timer);
}
-/**
- * @brief Arr�t le timer consid�r� et d�sactive les interruptions
- * @note
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * @retval None
- */
void Timer_stop(TIM_TypeDef * timer)
{
LL_TIM_DisableCounter(timer);
}
-/**
- * @brief Active l'horloge et r�gle l'ARR et le PSC du timer vis�.
- * @note Fonction � lancer avant toute autre. Le timer n'est pas encore lanc� (voir Timer_start)
- * @param TIM_TypeDef Timer : indique le timer � utiliser par le chronom�tre, TIM1, TIM2, TIM3 ou TIM4
- * int Arr : valeur � placer dans ARR
- * int Psc : valeur � placer dans PSC
- * @retval None
- */
+
void Timer_conf(TIM_TypeDef * timer, int arr, int psc)
{
LL_TIM_InitTypeDef init_struct;
-
+
// Validation horloge locale
if (timer == TIM1) LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
else if (timer == TIM2) LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
else if (timer == TIM3) LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
else LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM4);
-
+
// chargement structure Arr, Psc, Up Count
init_struct.Autoreload = arr;
init_struct.Prescaler = psc;
init_struct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
init_struct.CounterMode = LL_TIM_COUNTERMODE_UP;
init_struct.RepetitionCounter = 0;
-
+
LL_TIM_Init(timer,&init_struct);
// Blocage interruptions
Timer_IT_disable(timer);
-
+
// Blocage Timer
Timer_stop(timer);
}
@@ -169,16 +131,16 @@ void Timer_conf(TIM_TypeDef * timer, int arr, int psc)
void Timer_pwmi_conf(TIM_TypeDef * TIMx, int channel)
{
// Periode à recuperer dans TIMx_CCR1, duty cycle dans TIMx_CCR2. Seul les 2 premiers channels peuvent être utilisés (cf 315).
-
+
// Validation horloge locale
if (TIMx == TIM1) LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
else if (TIMx == TIM2) LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
else if (TIMx == TIM3) LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
else LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM4);
-
-
+
+
if (channel == 1) {
- //
+
TIMx->CCMR1 |= TIM_CCMR1_CC1S_0;
TIMx->CCMR1 |= TIM_CCMR1_CC2S_1;
@@ -187,28 +149,28 @@ void Timer_pwmi_conf(TIM_TypeDef * TIMx, int channel)
// On met le channel principal en rising edge, le secondaire en falling edge
TIMx->CCER &= ~TIM_CCER_CC1P;
TIMx->CCER |= TIM_CCER_CC2P;
-
+
TIMx->SMCR |= TIM_SMCR_TS_0 | TIM_SMCR_TS_2; //101
TIMx->SMCR |= TIM_SMCR_SMS_2; //100
}
-
+
else {
TIMx->CCMR1 |= TIM_CCMR1_CC1S_1;
TIMx->CCMR1 |= TIM_CCMR1_CC2S_0;
-
+
TIMx->CCER |= TIM_CCER_CC1P;
TIMx->CCER &= ~TIM_CCER_CC2P;
TIMx->SMCR |= TIM_SMCR_TS_1 | TIM_SMCR_TS_2; //110
TIMx->SMCR |= TIM_SMCR_SMS_2; //100
}
-
+
// On met les prescalers à 0, on veut compter chaque transition
TIMx -> CCMR1 &= ~TIM_CCMR1_IC1PSC;
TIMx -> CCMR1 &= ~TIM_CCMR1_IC2PSC;
-
+
TIMx -> CCER |= TIM_CCER_CC1E | TIM_CCER_CC2E;
-
+
// TIMx -> DIER |= TIM_DIER_CC1IE; gestion des interrupts, probablement pas utile
// TIM_DIER_CC1DE_Pos; Probablement pas utile
}
@@ -239,11 +201,11 @@ void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq, float dutyCyc
Timer_conf(timer, arr, 1000);
LL_TIM_OC_InitTypeDef init_struct;
LL_TIM_OC_StructInit(&init_struct);
-
+
init_struct.OCMode = LL_TIM_OCMODE_PWM1;
init_struct.OCState = LL_TIM_OCSTATE_ENABLE;
init_struct.CompareValue= dutyCycle * arr;
-
+
LL_TIM_OC_Init(timer, channel, &init_struct);
}
@@ -280,17 +242,21 @@ float Timer_pwmo_getDutyCycle(TIM_TypeDef * timer, int channel)
void Timer_encoder_conf(TIM_TypeDef * timer)
{
- Timer_conf(timer, 719, 0); // 360 * 2 - 1
+ // L'encodeur compte 2 tours quand on en fait qu'un, donc on divise l'angle
+ // par deux avant de le renvoyer. Il faut donc un autoreload de 360 * 2 = 720
+ Timer_conf(timer, 719, 0);
LL_TIM_ENCODER_InitTypeDef init_struct;
LL_TIM_ENCODER_StructInit(&init_struct);
-
+
init_struct.EncoderMode = LL_TIM_ENCODERMODE_X2_TI1;
-
+
LL_TIM_ENCODER_Init(timer, &init_struct);
}
int Timer_encoder_getAngle(TIM_TypeDef * timer)
{
+ // L'encodeur compte 2 tours quand on en fait qu'un, donc on divise l'angle
+ // par deux avant de le renvoyer
return LL_TIM_GetCounter(timer)/2;
}
diff --git a/Services/DCMotor.c b/Services/DCMotor.c
index 87f1846..a2d2373 100644
--- a/Services/DCMotor.c
+++ b/Services/DCMotor.c
@@ -1,19 +1,21 @@
#include "DCMotor.h"
#include "math.h"
-void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin)
-{
- //On règle la vitesse en valeur absolue, ici à 0
- Timer_pwmo_conf(timer, channel, 50, 0);
-
- // Configuration du GPIO
- GPIO_conf(GPIOA, LL_GPIO_PIN_1, LL_GPIO_MODE_ALTERNATE, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
+const int MOTOR_PWM_FREQ = 50;
+
+void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpioPwm, int pinPwm, GPIO_TypeDef * gpioDirection, int pinDirection)
+{
+ // On regle la vitesse en valeur absolue, ici a 0
+ Timer_pwmo_conf(timer, channel, MOTOR_PWM_FREQ, 0);
+
+ // Configuration du GPIO
+ GPIO_conf(gpioPwm, pinPwm, LL_GPIO_MODE_ALTERNATE, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
+
+
+ // On regle le sens du moteur
+ GPIO_conf(gpioDirection, pinDirection, LL_GPIO_MODE_OUTPUT, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_DOWN);
+ GPIO_setPin(gpioDirection, pinDirection, 0);
-
- //On règle le sens du moteur, ici sens direct (?)
- GPIO_conf(gpio, pin, LL_GPIO_MODE_OUTPUT, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_DOWN);
- GPIO_setPin(gpio, pin, 0);
-
Timer_start(timer);
}
@@ -21,8 +23,7 @@ void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin
void DCMotor_setSpeed(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin, float speed)
{
- const int dir = (speed > 0.) ? 1 : 0;
-
+ const int dir = speed > 0.0;
Timer_pwmo_setDutyCycle(timer, channel, fabs(speed));
GPIO_setPin(gpio, pin, dir);
}
@@ -31,6 +32,5 @@ float DCMotor_getSpeed(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, in
{
const float speedAbs = Timer_pwmo_getDutyCycle(timer, channel);
const int dir = GPIO_readPin(gpio, pin);
-
return dir ? speedAbs : -speedAbs;
}
diff --git a/Services/DCMotor.h b/Services/DCMotor.h
index bdfa188..739a5b6 100644
--- a/Services/DCMotor.h
+++ b/Services/DCMotor.h
@@ -6,21 +6,21 @@
/**
* @brief Configure le Timer et le GPIO du DCmotor
- * @note
- * @param timer : Timer utilisé pour controler le moteur
+ * @note
+ * @param timer : Timer utilise pour controler le moteur
* channel : channel du timer
- * gpio : GPIO utilisé pour régler le sens du moteur
+ * gpio : GPIO utilise pour regler le sens du moteur
* pin : Pin pour l'axe Y
* @retval None
*/
-void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin);
+void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpioPwm, int pinPwm, GPIO_TypeDef * gpioDirection, int pinDirection);
/**
* @brief Configure le Timer et le GPIO du DCmotor
- * @note
- * @param timer : Timer utilisé pour controler le moteur
+ * @note
+ * @param timer : Timer utilise pour controler le moteur
* channel : channel du timer
- * gpio : GPIO utilisé pour régler le sens du moteur
+ * gpio : GPIO utilise pour regler le sens du moteur
* pin : Pin pour l'axe Y
* speed : vitesse voulue
* @retval None
@@ -28,11 +28,11 @@ void DCMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin
void DCMotor_setSpeed(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin, float speed);
/**
- * @brief Récupère la vitesse actuelle du moteur
- * @note
- * @param timer : Timer utilisé pour controler le moteur
+ * @brief Recupere la vitesse actuelle du moteur
+ * @note
+ * @param timer : Timer utilise pour controler le moteur
* channel : channel du timer
- * gpio : GPIO utilisé pour régler le sens du moteur
+ * gpio : GPIO utilise pour regler le sens du moteur
* pin : Pin pour l'axe Y
* speed : vitesse voulue
* @retval None
diff --git a/Services/IncrementalEncoder.h b/Services/IncrementalEncoder.h
index efa5fe5..66e9dca 100644
--- a/Services/IncrementalEncoder.h
+++ b/Services/IncrementalEncoder.h
@@ -4,35 +4,35 @@
#include "stm32f103xb.h"
/**
- * @brief Configure le timer et le pin du gpio pour le codeur incrémental
- * @note
- * @param TIM_TypeDef timer : le timer à utiliser
- * GPIO_TypeDef gpio : le gpio à utiliser pour le zero
- * int pin : le pin associé au GPIO pour le zero
+ * @brief Configure le timer et le pin du gpio pour le codeur incremental
+ * @note
+ * @param TIM_TypeDef timer : le timer a utiliser
+ * GPIO_TypeDef gpio : le gpio a utiliser pour le zero
+ * int pin : le pin associe au GPIO pour le zero
* @retval None
*/
void IncrementalEncoder_conf(TIM_TypeDef * timer, GPIO_TypeDef * gpio, int pin);
/**
- * @brief Démarre le timer pour le codeur incrémental
- * @note
- * @param TIM_TypeDef timer : le timer à utiliser
+ * @brief Demarre le timer pour le codeur incremental
+ * @note
+ * @param TIM_TypeDef timer : le timer a utiliser
* @retval None
*/
void IncrementalEncoder_start(TIM_TypeDef * timer);
/**
- * @brief Récupère l'angle du codeur incrémental associé au timer donné
- * @note
- * @param TIM_TypeDef timer : le timer à utiliser
- * @retval L'angle en degrès
+ * @brief Recupere l'angle du codeur incremental associe au timer donne
+ * @note
+ * @param TIM_TypeDef timer : le timer a utiliser
+ * @retval L'angle en degres
*/
int IncrementalEncoder_getAngle(TIM_TypeDef * timer);
/**
- * @brief Récupère la direction du codeur incrémental associé au timer donné
- * @note
- * @param TIM_TypeDef timer : le timer à utiliser
+ * @brief Recupere la direction du codeur incremental associe au timer donne
+ * @note
+ * @param TIM_TypeDef timer : le timer a utiliser
* @retval la direction actuelle du codeur
*/
enum CounterDirection IncrementalEncoder_getDirection(TIM_TypeDef * timer);
diff --git a/Services/ServoMotor.c b/Services/ServoMotor.c
index 17b7c0a..1cd7357 100644
--- a/Services/ServoMotor.c
+++ b/Services/ServoMotor.c
@@ -14,7 +14,8 @@ void ServoMotor_start(TIM_TypeDef * timer)
}
float convertAngleToDutyCycle(int angle) {
- return ((float) angle) / 90.0 / 20 + 0.05;
+ // fonction lineaire [0 - 90] -> [0.05 - 0.1]
+ return (float)angle / 1800.0 + 0.05;
}
void ServoMotor_setAngle(TIM_TypeDef * timer, int channel, int angle)
diff --git a/Src/Orientation.c b/Src/Orientation.c
index 797c75c..9eec407 100644
--- a/Src/Orientation.c
+++ b/Src/Orientation.c
@@ -1,7 +1,7 @@
#include "Orientation.h"
#include "math.h"
-#define THRESHOLD 0
+const float THRESHOLD = 0.05;
const float MIN_DUTY_CYCLE = 0.05;
const float ZERO_DUTY_CYCLE = 0.075;
@@ -15,25 +15,26 @@ const int RECEIVER_CHANNEL = LL_TIM_CHANNEL_CH1;
TIM_TypeDef * DCMOTOR_TIMER = TIM2;
const int DCMOTOR_CHANNEL = LL_TIM_CHANNEL_CH2;
GPIO_TypeDef * DCMOTOR_GPIO = GPIOA;
-const int DCMOTOR_PIN = LL_GPIO_PIN_2;
+const int DCMOTOR_PIN_PWM = LL_GPIO_PIN_2;
+const int DCMOTOR_PIN_DIRECTION = LL_GPIO_PIN_2;
-void Orientation_conf()
+
+void Orientation_conf()
{
- DCMotor_conf(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN);
+ DCMotor_conf(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN_PWM, DCMOTOR_GPIO, DCMOTOR_PIN_DIRECTION);
RFReceiver_conf(RECEIVER_TIMER, LL_TIM_CHANNEL_CH1);
}
void Orientation_background()
{
const float duty_cycle = RFReceiver_getData(RECEIVER_TIMER);
-
+
+ // Calcul de la vitesse du moteur (entre 0 et 1)
const float speed = (duty_cycle - ZERO_DUTY_CYCLE) / (MAX_DUTY_CYCLE - ZERO_DUTY_CYCLE);
-
- //Si la vitesse (en valeur absolue) ne dépasse pas un certain seuil, on ne démarre pas le moteur
- if (THRESHOLD > fabs(speed)) {
- DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, 0);
- }
- else {
- DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, speed);
- }
+
+ // On ne demarre le moteur que si la vitesse depasse un certain seuil
+ if (fabs(speed) > THRESHOLD)
+ DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN_DIRECTION, speed);
+ else
+ DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN_DIRECTION, 0);
}
diff --git a/Src/Roll.c b/Src/Roll.c
index 2543053..4c5630f 100644
--- a/Src/Roll.c
+++ b/Src/Roll.c
@@ -31,12 +31,12 @@ int Roll_getEmergencyState(void)
void Roll_background(void)
{
- const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL);
- //const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL);
-
- const int currentState = abs(xAngle) >= 40;
- if (Roll_isEmergencyState && !currentState)
- Sail_setEmergency(1);
- else if (!Roll_isEmergencyState && currentState)
- Sail_setEmergency(0);
+ const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_X_CHANNEL);
+ const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL);
+
+ const int currentState = abs(yAngle) >= 40;
+ if (Roll_isEmergencyState != currentState) {
+ Sail_setEmergency(currentState);
+ Roll_isEmergencyState = currentState;
+ }
}
diff --git a/Src/main.c b/Src/main.c
index 938ec80..5c9a9f8 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -38,8 +38,8 @@ int secCounter = 0;
/* Private functions ---------------------------------------------------------*/
/**
- * @brief Effectue la tache de fond (programmée toutes les 1ms)
- * @note
+ * @brief Effectue la tache de fond (programmee toutes les ms)
+ * @note
* @param None
* @retval None
*/
@@ -58,8 +58,8 @@ void backgroundTask()
}
/**
- * @brief Configure les périphériques
- * @note
+ * @brief Configure les peripheriques
+ * @note
* @param None
* @retval None
*/
@@ -72,8 +72,8 @@ void configurePeripherals()
}
/**
- * @brief Démarre les périphériques
- * @note
+ * @brief Demarre les peripheriques
+ * @note
* @param None
* @retval None
*/
@@ -90,36 +90,36 @@ void startPeripherals()
* @retval None
*/
int main(void)
-{
+{
/* Configure the system clock to 72 MHz */
SystemClock_Config();
/* Add your application code here */
configurePeripherals();
startPeripherals();
-
+
Scheduler_conf(backgroundTask);
Scheduler_start();
while (1) {
-// Display_background(secCounter);
+ // Display_background(secCounter);
}
-
-
+
+
//// ADC 2 marche niquel, ADC1 ne marche pas :(
// ADC_conf(ADC1);
// ADC_start(ADC1);
-//
+//
// ADC_conf(ADC2);
// ADC_start(ADC2);
-//
+//
// while (1) {
// val1 = ADC_readRaw(ADC1, 10);
// val2 = ADC_readRaw(ADC2, 10);
// }
-
+
}
@@ -150,7 +150,7 @@ void SystemClock_Config(void)
/* Enable HSE oscillator */
// ********* Commenter la ligne ci-dessous pour MCBSTM32 *****************
- // ********* Conserver la ligne si Nucléo*********************************
+ // ********* Conserver la ligne si Nucleo*********************************
// LL_RCC_HSE_EnableBypass();
LL_RCC_HSE_Enable();
while(LL_RCC_HSE_IsReady() != 1)