Есть два мотора. Я отслеживаю их позиции. Если при движении вперед позиция первого мотора больше – он двигается быстрей – я его притормаживаю а второй ускоряю. Ну и так далее.
Исходник
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
uint32_t GetPosDiff(uint32_t group)
{
uint32_t pos_dif;
uint32_t mot_idx1 = pos_dif_control[group].mot_idx1;
uint32_t mot_idx2 = pos_dif_control[group].mot_idx2;
if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position)
pos_dif = motor_rt_params[mot_idx1].position - motor_rt_params[mot_idx2].position;
else
pos_dif = motor_rt_params[mot_idx2].position - motor_rt_params[mot_idx1].position;
return pos_dif;
}
uint32_t PosDiffAdjust (uint32_t group)
{
uint32_t pos_dif = GetPosDiff(group);
uint32_t mot_idx1 = pos_dif_control[group].mot_idx1;
uint32_t mot_idx2 = pos_dif_control[group].mot_idx2;
uint32_t skip = 0;
if (pos_dif >= pos_dif_control[group].pos_dif)
{
sys_status |= MOT_POS_DIFF;
MASTER_StopAll();
return pos_dif;
}
if (pos_dif < pos_dif_control[group].pos_delta)
{
skip = 1;
motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max;
}
if (skip == 0)
{
if (motor_rt_params[mot_idx1].direction == MOT_DIR_FWD)
{
if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position) //motor1 runs faster
{
motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;
if (motor_rt_params[mot_idx1].speed < pos_dif_control[group].pwm_min)
motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_min;
if (motor_rt_params[mot_idx2].speed > pos_dif_control[group].pwm_max)
motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max;
}
else if (motor_rt_params[mot_idx2].position > motor_rt_params[mot_idx1].position) //motor2 runs faster
{
motor_rt_params[mot_idx1].speed += pos_dif_control[group].pwm_delta;
motor_rt_params[mot_idx2].speed -= pos_dif_control[group].pwm_delta;
if (motor_rt_params[mot_idx1].speed > pos_dif_control[group].pwm_max)
motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
if (motor_rt_params[mot_idx2].speed < pos_dif_control[group].pwm_min)
motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_min;
}
}
if (motor_rt_params[mot_idx1].direction == MOT_DIR_REV)
{
if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position) //motor1 runs slower
{
motor_rt_params[mot_idx1].speed += pos_dif_control[group].pwm_delta;
motor_rt_params[mot_idx2].speed -= pos_dif_control[group].pwm_delta;
if (motor_rt_params[mot_idx1].speed > pos_dif_control[group].pwm_max)
motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
if (motor_rt_params[mot_idx2].speed < pos_dif_control[group].pwm_min)
motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_min;
}
else if (motor_rt_params[mot_idx2].position > motor_rt_params[mot_idx1].position) //motor2 runs slower
{
motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;
if (motor_rt_params[mot_idx1].speed < pos_dif_control[group].pwm_min)
motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_min;
if (motor_rt_params[mot_idx2].speed > pos_dif_control[group].pwm_max)
motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max;
}
}
}
return pos_dif;
}
Возник вопрос – что делать если разница в позициях достигла предельно допустимой?
То есть я конечно остановлю оба мотора. Но потом, если пользователь продолжает жать кнопку вперед – я подумал тот мотор который отстает все таки запустить – он сравняется с позицией со вторым мотором – я запущу второй мотор и продолжу отслеживать позиции.
Вобщем я начал кодить, добавлять эту часть – такая каша получается, мама не горюй. Есть какая то алгоритмика в этих вопросах?
Модератор: Просьба большие простыни убирать под спойлер. Поправил.
