add sail and roll logic

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

Src/Roll.h

@@ -5,6 +5,8 @@ void Roll_conf(void);
 
 void Roll_start(void);
 
+int Roll_getEmergencyState(void);
+
 void Roll_background(void);
 
 #endif

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

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