348 lines
11 KiB
C
348 lines
11 KiB
C
#include "system/includes.h"
|
|
#include "app_mode.h"
|
|
#include "bsp_hw.h"
|
|
|
|
#define LOG_TAG_CONST APP
|
|
#define LOG_TAG "[APP_MODE]"
|
|
#define LOG_INFO_ENABLE
|
|
#include "debug.h"
|
|
|
|
#define MAGIC_BOX_HIGH_PERCENT 100
|
|
#define MAGIC_BOX_MEDIUM_LOW_PERCENT 45
|
|
#define MAGIC_BOX_MODE1_FORWARD_MS 585
|
|
#define MAGIC_BOX_MODE1_BACKWARD_MS 600
|
|
#define MAGIC_BOX_MODE2_FORWARD_MS 260
|
|
#define MAGIC_BOX_MODE2_BACKWARD_MS 200
|
|
#define MAGIC_BOX_MODE3_FORWARD_MS 585
|
|
#define MAGIC_BOX_MODE3_BACKWARD_MS 600
|
|
#define MAGIC_BOX_MODE3_STOP_MS 2000
|
|
#define MAGIC_BOX_MODE3_LOWER_SEGMENTS 10
|
|
#define MAGIC_BOX_MODE4_FORWARD_MS 390
|
|
#define MAGIC_BOX_MODE4_BACKWARD_MS 300
|
|
#define MAGIC_BOX_MODE4_STOP_MS 3000
|
|
#define MAGIC_BOX_MODE5_BACKWARD_MS 600
|
|
#define MAGIC_BOX_MODE5_UPPER_STOP_MS 2000
|
|
#define MAGIC_BOX_MODE5_CYCLE_MS 20000
|
|
|
|
typedef struct {
|
|
uint8_t initialized;
|
|
uint8_t active;
|
|
uint8_t output_valid;
|
|
uint8_t upper_phase;
|
|
uint8_t lower_pattern;
|
|
uint8_t lower_segment;
|
|
int8_t upper_direction;
|
|
int8_t lower_direction;
|
|
uint8_t upper_speed;
|
|
uint16_t upper_duration_ms;
|
|
uint16_t lower_duration_ms;
|
|
uint32_t state_started_ms;
|
|
uint32_t upper_started_ms;
|
|
uint32_t lower_started_ms;
|
|
int8_t output_upper_direction;
|
|
int8_t output_lower_direction;
|
|
uint8_t output_upper_speed;
|
|
uint8_t output_lower_speed;
|
|
} AppModeCtx_t;
|
|
|
|
static AppModeCtx_t s_mode;
|
|
|
|
static void app_mode_outputs_set(int8_t upper_direction, uint8_t upper_speed,
|
|
int8_t lower_direction, uint8_t lower_speed)
|
|
{
|
|
if (s_mode.output_valid &&
|
|
upper_direction == s_mode.output_upper_direction &&
|
|
upper_speed == s_mode.output_upper_speed &&
|
|
lower_direction == s_mode.output_lower_direction &&
|
|
lower_speed == s_mode.output_lower_speed) {
|
|
return;
|
|
}
|
|
|
|
bsp_drive_set((BspMotorDir_t)upper_direction, (uint16_t)upper_speed * 100,
|
|
(BspMotorDir_t)lower_direction, (uint16_t)lower_speed * 100);
|
|
s_mode.output_upper_direction = upper_direction;
|
|
s_mode.output_upper_speed = upper_speed;
|
|
s_mode.output_lower_direction = lower_direction;
|
|
s_mode.output_lower_speed = lower_speed;
|
|
s_mode.output_valid = 1;
|
|
}
|
|
|
|
static uint16_t app_mode4_random_step_ms(void)
|
|
{
|
|
static const uint16_t step_ms[] = {80, 100, 150, 200};
|
|
|
|
return step_ms[rand32() % ARRAY_SIZE(step_ms)];
|
|
}
|
|
|
|
static uint16_t app_mode5_random_forward_ms(void)
|
|
{
|
|
static const uint16_t forward_ms[] = {555, 780, 1140};
|
|
|
|
return forward_ms[rand32() % ARRAY_SIZE(forward_ms)];
|
|
}
|
|
|
|
static void app_mode5_select_lower_pattern(uint32_t now_ms)
|
|
{
|
|
s_mode.lower_pattern = rand32() % 4;
|
|
s_mode.lower_segment = 0;
|
|
s_mode.lower_started_ms = now_ms;
|
|
|
|
switch (s_mode.lower_pattern) {
|
|
case 0:
|
|
s_mode.lower_direction = BSP_MOTOR_FORWARD;
|
|
s_mode.lower_duration_ms = 2000;
|
|
break;
|
|
case 1:
|
|
s_mode.lower_direction = BSP_MOTOR_BACKWARD;
|
|
s_mode.lower_duration_ms = 2000;
|
|
break;
|
|
case 2:
|
|
s_mode.lower_direction = (rand32() & 1) ?
|
|
BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
|
|
s_mode.lower_duration_ms = 100;
|
|
break;
|
|
default:
|
|
s_mode.lower_direction = (rand32() & 1) ?
|
|
BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
|
|
s_mode.lower_duration_ms = 200;
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void app_mode5_start_cycle(uint32_t now_ms)
|
|
{
|
|
s_mode.state_started_ms = now_ms;
|
|
s_mode.upper_phase = 0;
|
|
s_mode.upper_started_ms = now_ms;
|
|
s_mode.upper_duration_ms = app_mode5_random_forward_ms();
|
|
s_mode.upper_direction = BSP_MOTOR_FORWARD;
|
|
s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
app_mode5_select_lower_pattern(now_ms);
|
|
}
|
|
|
|
static void app_mode_start_current(uint32_t now_ms)
|
|
{
|
|
bsp_drive_stop();
|
|
s_mode.output_valid = 0;
|
|
s_mode.state_started_ms = now_ms;
|
|
s_mode.upper_phase = 0;
|
|
s_mode.upper_started_ms = now_ms;
|
|
s_mode.lower_started_ms = now_ms;
|
|
|
|
if (s_mode.active == 4) {
|
|
s_mode.lower_direction = (rand32() & 1) ?
|
|
BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
|
|
s_mode.lower_duration_ms = app_mode4_random_step_ms();
|
|
} else if (s_mode.active == 5) {
|
|
app_mode5_start_cycle(now_ms);
|
|
}
|
|
}
|
|
|
|
static void app_mode1_process(uint32_t now_ms)
|
|
{
|
|
const uint16_t pair_ms = MAGIC_BOX_MODE1_FORWARD_MS +
|
|
MAGIC_BOX_MODE1_BACKWARD_MS;
|
|
uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % pair_ms;
|
|
|
|
if (elapsed_ms < MAGIC_BOX_MODE1_FORWARD_MS) {
|
|
app_mode_outputs_set(BSP_MOTOR_FORWARD, MAGIC_BOX_HIGH_PERCENT,
|
|
BSP_MOTOR_STOP, 0);
|
|
} else {
|
|
app_mode_outputs_set(BSP_MOTOR_BACKWARD, MAGIC_BOX_HIGH_PERCENT,
|
|
BSP_MOTOR_STOP, 0);
|
|
}
|
|
}
|
|
|
|
static void app_mode2_process(uint32_t now_ms)
|
|
{
|
|
const uint16_t pair_ms = MAGIC_BOX_MODE2_FORWARD_MS +
|
|
MAGIC_BOX_MODE2_BACKWARD_MS;
|
|
uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % pair_ms;
|
|
|
|
if (elapsed_ms < MAGIC_BOX_MODE2_FORWARD_MS) {
|
|
app_mode_outputs_set(BSP_MOTOR_FORWARD, MAGIC_BOX_HIGH_PERCENT,
|
|
BSP_MOTOR_STOP, 0);
|
|
} else {
|
|
app_mode_outputs_set(BSP_MOTOR_BACKWARD, MAGIC_BOX_HIGH_PERCENT,
|
|
BSP_MOTOR_STOP, 0);
|
|
}
|
|
}
|
|
|
|
static void app_mode3_process(uint32_t now_ms)
|
|
{
|
|
const uint16_t upper_move_ms = MAGIC_BOX_MODE3_FORWARD_MS +
|
|
MAGIC_BOX_MODE3_BACKWARD_MS;
|
|
const uint16_t cycle_ms = upper_move_ms + MAGIC_BOX_MODE3_STOP_MS;
|
|
uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % cycle_ms;
|
|
uint8_t lower_segment = ((uint32_t)elapsed_ms *
|
|
MAGIC_BOX_MODE3_LOWER_SEGMENTS) / cycle_ms;
|
|
int8_t upper_direction;
|
|
uint8_t upper_speed;
|
|
int8_t lower_direction;
|
|
|
|
if (elapsed_ms < MAGIC_BOX_MODE3_FORWARD_MS) {
|
|
upper_direction = BSP_MOTOR_FORWARD;
|
|
upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
} else if (elapsed_ms < upper_move_ms) {
|
|
upper_direction = BSP_MOTOR_BACKWARD;
|
|
upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
} else {
|
|
upper_direction = BSP_MOTOR_STOP;
|
|
upper_speed = 0;
|
|
}
|
|
|
|
lower_direction = (lower_segment & 1) ?
|
|
BSP_MOTOR_BACKWARD : BSP_MOTOR_FORWARD;
|
|
app_mode_outputs_set(upper_direction, upper_speed,
|
|
lower_direction, MAGIC_BOX_HIGH_PERCENT);
|
|
}
|
|
|
|
static void app_mode4_process(uint32_t now_ms)
|
|
{
|
|
const uint16_t upper_move_ms = MAGIC_BOX_MODE4_FORWARD_MS +
|
|
MAGIC_BOX_MODE4_BACKWARD_MS;
|
|
const uint16_t cycle_ms = upper_move_ms + MAGIC_BOX_MODE4_STOP_MS;
|
|
uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms);
|
|
int8_t upper_direction;
|
|
uint8_t upper_speed;
|
|
|
|
if (elapsed_ms >= cycle_ms) {
|
|
s_mode.state_started_ms = now_ms;
|
|
elapsed_ms = 0;
|
|
s_mode.lower_started_ms = now_ms;
|
|
s_mode.lower_direction = -s_mode.lower_direction;
|
|
s_mode.lower_duration_ms = app_mode4_random_step_ms();
|
|
}
|
|
if ((uint32_t)(now_ms - s_mode.lower_started_ms) >=
|
|
s_mode.lower_duration_ms) {
|
|
s_mode.lower_started_ms = now_ms;
|
|
s_mode.lower_direction = -s_mode.lower_direction;
|
|
s_mode.lower_duration_ms = app_mode4_random_step_ms();
|
|
}
|
|
|
|
if (elapsed_ms < MAGIC_BOX_MODE4_FORWARD_MS) {
|
|
upper_direction = BSP_MOTOR_FORWARD;
|
|
upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
} else if (elapsed_ms < upper_move_ms) {
|
|
upper_direction = BSP_MOTOR_BACKWARD;
|
|
upper_speed = MAGIC_BOX_MEDIUM_LOW_PERCENT;
|
|
} else {
|
|
upper_direction = BSP_MOTOR_STOP;
|
|
upper_speed = 0;
|
|
}
|
|
app_mode_outputs_set(upper_direction, upper_speed,
|
|
s_mode.lower_direction, MAGIC_BOX_HIGH_PERCENT);
|
|
}
|
|
|
|
static void app_mode5_process(uint32_t now_ms)
|
|
{
|
|
if ((uint32_t)(now_ms - s_mode.state_started_ms) >=
|
|
MAGIC_BOX_MODE5_CYCLE_MS) {
|
|
app_mode5_start_cycle(now_ms);
|
|
}
|
|
|
|
if ((uint32_t)(now_ms - s_mode.upper_started_ms) >=
|
|
s_mode.upper_duration_ms) {
|
|
s_mode.upper_started_ms = now_ms;
|
|
s_mode.upper_phase++;
|
|
if (s_mode.upper_phase == 1) {
|
|
s_mode.upper_direction = BSP_MOTOR_BACKWARD;
|
|
s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
s_mode.upper_duration_ms = MAGIC_BOX_MODE5_BACKWARD_MS;
|
|
} else if (s_mode.upper_phase == 2) {
|
|
s_mode.upper_direction = BSP_MOTOR_STOP;
|
|
s_mode.upper_speed = 0;
|
|
s_mode.upper_duration_ms = MAGIC_BOX_MODE5_UPPER_STOP_MS;
|
|
} else {
|
|
s_mode.upper_phase = 0;
|
|
s_mode.upper_direction = BSP_MOTOR_FORWARD;
|
|
s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
|
|
s_mode.upper_duration_ms = app_mode5_random_forward_ms();
|
|
}
|
|
}
|
|
|
|
if ((uint32_t)(now_ms - s_mode.lower_started_ms) >=
|
|
s_mode.lower_duration_ms) {
|
|
if (s_mode.lower_pattern >= 2 && ++s_mode.lower_segment < 8) {
|
|
s_mode.lower_started_ms = now_ms;
|
|
s_mode.lower_direction = -s_mode.lower_direction;
|
|
} else {
|
|
app_mode5_select_lower_pattern(now_ms);
|
|
}
|
|
}
|
|
app_mode_outputs_set(s_mode.upper_direction, s_mode.upper_speed,
|
|
s_mode.lower_direction, MAGIC_BOX_HIGH_PERCENT);
|
|
}
|
|
|
|
AppMode_Ret_t app_mode_init(void)
|
|
{
|
|
memset(&s_mode, 0, sizeof(s_mode));
|
|
s_mode.initialized = 1;
|
|
bsp_drive_stop();
|
|
return APP_MODE_OK;
|
|
}
|
|
|
|
AppMode_Ret_t app_mode_get_status(void)
|
|
{
|
|
return s_mode.initialized ? APP_MODE_OK : APP_MODE_ERR_NOT_INIT;
|
|
}
|
|
|
|
uint8_t app_mode_get_active(void)
|
|
{
|
|
return s_mode.active;
|
|
}
|
|
|
|
AppMode_Ret_t app_mode_start(uint8_t mode)
|
|
{
|
|
if (!s_mode.initialized) {
|
|
return APP_MODE_ERR_NOT_INIT;
|
|
}
|
|
if (mode < 1 || mode > 5) {
|
|
app_mode_stop();
|
|
return APP_MODE_ERR_PARAM;
|
|
}
|
|
|
|
s_mode.active = mode;
|
|
app_mode_start_current(timer_get_ms());
|
|
log_info("magic box mode %d start\n", mode);
|
|
return APP_MODE_OK;
|
|
}
|
|
|
|
void app_mode_stop(void)
|
|
{
|
|
s_mode.active = 0;
|
|
s_mode.output_valid = 0;
|
|
bsp_drive_stop();
|
|
}
|
|
|
|
void app_mode_run(void)
|
|
{
|
|
uint32_t now_ms;
|
|
|
|
bsp_motor_switch_tick();
|
|
if (!s_mode.initialized || !s_mode.active) {
|
|
return;
|
|
}
|
|
now_ms = timer_get_ms();
|
|
switch (s_mode.active) {
|
|
case 1:
|
|
app_mode1_process(now_ms);
|
|
break;
|
|
case 2:
|
|
app_mode2_process(now_ms);
|
|
break;
|
|
case 3:
|
|
app_mode3_process(now_ms);
|
|
break;
|
|
case 4:
|
|
app_mode4_process(now_ms);
|
|
break;
|
|
case 5:
|
|
app_mode5_process(now_ms);
|
|
break;
|
|
default:
|
|
app_mode_stop();
|
|
break;
|
|
}
|
|
}
|