67 lines
1.4 KiB
C
67 lines
1.4 KiB
C
#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 = 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_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 360 - 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));
|
|
}
|