diff --git a/Src/Roll.c b/Src/Roll.c index 8e2bc0d..05a39b9 100644 --- a/Src/Roll.c +++ b/Src/Roll.c @@ -1,5 +1,7 @@ #include "Roll.h" +#include "Sail.h" #include "Accelerometer.h" +#include "stdlib.h" ADC_TypeDef * ROLL_ADC = ADC1; const int ROLL_X_CHANNEL = LL_ADC_CHANNEL_11; @@ -8,6 +10,10 @@ GPIO_TypeDef * ROLL_GPIO = GPIOC; const int ROLL_X_PIN = LL_GPIO_PIN_0; const int ROLL_Y_PIN = LL_GPIO_PIN_1; +const int TRIGGER_ANGLE = 40; + +int Roll_isEmergencyState = 0; + void Roll_conf(void) { Accelerometer_conf(ROLL_ADC, ROLL_GPIO, ROLL_X_PIN, ROLL_Y_PIN); @@ -18,8 +24,19 @@ void Roll_start(void) Accelerometer_start(ROLL_ADC); } +int Roll_getEmergencyState(void) +{ + return Roll_isEmergencyState; +} + void Roll_background(void) { const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_X_CHANNEL); - const int yAngle = 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); } diff --git a/Src/Roll.h b/Src/Roll.h index 2b48a8c..97093ee 100644 --- a/Src/Roll.h +++ b/Src/Roll.h @@ -5,6 +5,8 @@ void Roll_conf(void); void Roll_start(void); +int Roll_getEmergencyState(void); + void Roll_background(void); #endif diff --git a/Src/Sail.c b/Src/Sail.c index 5b0ae95..f6b6bc2 100644 --- a/Src/Sail.c +++ b/Src/Sail.c @@ -11,25 +11,48 @@ TIM_TypeDef * ENCODER_TIMER = TIM3; GPIO_TypeDef * ENCODER_GPIO = GPIOA; const int ENCODER_PIN = LL_GPIO_PIN_5; +const int RESET_ANGLE = 90; + +int Sail_isEmergencyState = 0; + void Sail_conf() { ServoMotor_conf(MOTOR_TIMER, MOTOR_CHANNEL, MOTOR_GPIO, MOTOR_PIN); IncrementalEncoder_conf(ENCODER_TIMER, ENCODER_GPIO, ENCODER_PIN); } -void Sail_background() +int getSailAngle(int windAngle) { - + if (windAngle > 180) + return 90 * (windAngle - 45) / 135; + else + return 360 - 90 * ((360 - windAngle) - 45) / 135; } -void Sail_reset() +void Sail_background() { + if (Sail_isEmergencyState) + return; + const int windAngle = IncrementalEncoder_getAngle(ENCODER_TIMER); + + if (windAngle < 45 || windAngle > 315) + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, 0); + else + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, getSailAngle(windAngle)); +} + +void Sail_setEmergency(int state) +{ + Sail_isEmergencyState = state; + if (Sail_isEmergencyState) + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, RESET_ANGLE); } void Sail_start() { - Timer_start(MOTOR_TIMER); - Timer_start(ENCODER_TIMER); + Sail_isEmergencyState = 0; + ServoMotor_start(MOTOR_TIMER); + IncrementalEncoder_start(ENCODER_TIMER); } diff --git a/Src/Sail.h b/Src/Sail.h index cc49ef0..1973501 100644 --- a/Src/Sail.h +++ b/Src/Sail.h @@ -23,7 +23,7 @@ void Sail_background(void); * @param None * @retval None */ -void Sail_reset(void); +void Sail_setEmergency(int state); /** * @brief Réinitialise la voile à sa position initiale