|
@@ -11,25 +11,48 @@ TIM_TypeDef * ENCODER_TIMER = TIM3;
|
11
|
11
|
GPIO_TypeDef * ENCODER_GPIO = GPIOA;
|
12
|
12
|
const int ENCODER_PIN = LL_GPIO_PIN_5;
|
13
|
13
|
|
|
14
|
+const int RESET_ANGLE = 90;
|
|
15
|
+
|
|
16
|
+int Sail_isEmergencyState = 0;
|
|
17
|
+
|
14
|
18
|
void Sail_conf()
|
15
|
19
|
{
|
16
|
20
|
ServoMotor_conf(MOTOR_TIMER, MOTOR_CHANNEL, MOTOR_GPIO, MOTOR_PIN);
|
17
|
21
|
IncrementalEncoder_conf(ENCODER_TIMER, ENCODER_GPIO, ENCODER_PIN);
|
18
|
22
|
}
|
19
|
23
|
|
|
24
|
+int getSailAngle(int windAngle)
|
|
25
|
+{
|
|
26
|
+ if (windAngle > 180)
|
|
27
|
+ return 90 * (windAngle - 45) / 135;
|
|
28
|
+ else
|
|
29
|
+ return 360 - 90 * ((360 - windAngle) - 45) / 135;
|
|
30
|
+}
|
|
31
|
+
|
20
|
32
|
void Sail_background()
|
21
|
33
|
{
|
|
34
|
+ if (Sail_isEmergencyState)
|
|
35
|
+ return;
|
22
|
36
|
|
|
37
|
+ const int windAngle = IncrementalEncoder_getAngle(ENCODER_TIMER);
|
|
38
|
+
|
|
39
|
+ if (windAngle < 45 || windAngle > 315)
|
|
40
|
+ ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, 0);
|
|
41
|
+ else
|
|
42
|
+ ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, getSailAngle(windAngle));
|
23
|
43
|
}
|
24
|
44
|
|
25
|
|
-void Sail_reset()
|
|
45
|
+void Sail_setEmergency(int state)
|
26
|
46
|
{
|
27
|
|
-
|
|
47
|
+ Sail_isEmergencyState = state;
|
|
48
|
+ if (Sail_isEmergencyState)
|
|
49
|
+ ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, RESET_ANGLE);
|
28
|
50
|
}
|
29
|
51
|
|
30
|
52
|
|
31
|
53
|
void Sail_start()
|
32
|
54
|
{
|
33
|
|
- Timer_start(MOTOR_TIMER);
|
34
|
|
- Timer_start(ENCODER_TIMER);
|
|
55
|
+ Sail_isEmergencyState = 0;
|
|
56
|
+ ServoMotor_start(MOTOR_TIMER);
|
|
57
|
+ IncrementalEncoder_start(ENCODER_TIMER);
|
35
|
58
|
}
|