12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667 |
- #include "Sail.h"
- #include "ServoMotor.h"
- #include "IncrementalEncoder.h"
-
- TIM_TypeDef * MOTOR_TIMER = TIM4;
- const int MOTOR_CHANNEL = LL_TIM_CHANNEL_CH3;
- GPIO_TypeDef * MOTOR_GPIO = GPIOB;
- const int MOTOR_PIN = LL_GPIO_PIN_8;
-
- TIM_TypeDef * ENCODER_TIMER = TIM3;
- GPIO_TypeDef * ENCODER_GPIO = GPIOA;
- const int ENCODER_PIN = LL_GPIO_PIN_5;
-
- const int RESET_ANGLE = 0;
-
- 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_start()
- {
- Sail_isEmergencyState = 0;
- ServoMotor_start(MOTOR_TIMER);
- IncrementalEncoder_start(ENCODER_TIMER);
- }
-
- int windToSailAngle(int windAngle)
- {
- if (windAngle < 180)
- return 90 * (windAngle - 45) / 135;
- else
- return 90 * ((360 - windAngle) - 45) / 135;
- }
-
- int Sail_getSailAngle(void)
- {
- return ServoMotor_getAngle(MOTOR_TIMER, MOTOR_CHANNEL);
- }
-
- void Sail_setEmergency(int state)
- {
- Sail_isEmergencyState = state;
- if (Sail_isEmergencyState)
- ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, RESET_ANGLE);
- }
-
- int Sail_getEmergencyState()
- {
- return Sail_isEmergencyState;
- }
-
- 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, windToSailAngle(windAngle));
- }
|