603 lines
15 KiB
C
603 lines
15 KiB
C
/*
|
|
* @Author: DaMing zxm5337@163.com
|
|
* @Date: 2024-04-17 18:30:06
|
|
* @LastEditors: 张小明 zxm5337@163.com
|
|
* @LastEditTime: 2024-05-28 16:26:27
|
|
* @FilePath: \Proxi_CheckBoard\User\App\ctrl_logic.c
|
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
|
*/
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include "ctr_logic.h"
|
|
#include "communication.h"
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "dac.h"
|
|
#include "ProximitySwitches.h"
|
|
#include "bldc.h"
|
|
|
|
beep_t beep_1;
|
|
pars_cmd_time_t pars_cmd_time_A;
|
|
pars_cmd_time_t pars_cmd_time_B;
|
|
|
|
static void fun_get_limit_flag(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *limit_flag = &motor->limit_flag;
|
|
BOOL *limit_near_flag = &motor->limit_near_flag;
|
|
BOOL *limit_far_flag = &motor->limit_far_flag;
|
|
BOOL *stall_flag = &motor->stall_flag;
|
|
|
|
modbus_rtu_t *modbus_rtu = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
{
|
|
modbus_rtu = &modbus_rtu_A;
|
|
if (HAL_GPIO_ReadPin(RSVD_LIMIT_A_GPIO_Port, RSVD_LIMIT_A_Pin) == GPIO_PIN_RESET)
|
|
*limit_flag = TRUE;
|
|
else
|
|
*limit_flag = FALSE;
|
|
}
|
|
else if (motor == &motor_B)
|
|
{
|
|
modbus_rtu = &modbus_rtu_B;
|
|
if (HAL_GPIO_ReadPin(RSVD_LIMIT_B_GPIO_Port, RSVD_LIMIT_B_Pin) == GPIO_PIN_RESET)
|
|
*limit_flag = TRUE;
|
|
else
|
|
*limit_flag = FALSE;
|
|
}
|
|
if ((*limit_flag == TRUE) || (*stall_flag == TRUE))
|
|
{
|
|
if (modbus_rtu->distance > DISTANCE_RATED / 2)
|
|
{
|
|
*limit_near_flag = FALSE;
|
|
*limit_far_flag = TRUE;
|
|
}
|
|
else if (modbus_rtu->distance < DISTANCE_RATED / 2)
|
|
{
|
|
*limit_near_flag = TRUE;
|
|
*limit_far_flag = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
*limit_near_flag = FALSE;
|
|
*limit_far_flag = FALSE;
|
|
}
|
|
}
|
|
|
|
static void fun_get_stall_flag(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL en = motor->en;
|
|
BOOL *stall_flag = &motor->stall_flag;
|
|
uint32_t freq = motor->freq;
|
|
encoder_t *encoder = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
encoder = &encoder_A;
|
|
if (motor == &motor_B)
|
|
encoder = &encoder_B;
|
|
|
|
uint32_t *timer = &encoder->timer;
|
|
int32_t enc = encoder->enc;
|
|
int16_t *enc2 = &encoder->enc2;
|
|
int32_t *enc_old = &encoder->enc_old;
|
|
int32_t *enc_sub = &encoder->enc_sub;
|
|
uint32_t alarm_L, alarm_H;
|
|
|
|
if (en == FALSE)
|
|
{
|
|
*enc_old = 0;
|
|
*enc2 = 0;
|
|
return;
|
|
}
|
|
|
|
if ((motor->position_pv >= 0.1f) && (motor->position_zero != 0))
|
|
return;
|
|
|
|
if (*timer + 30 < xTaskGetTickCount())
|
|
{
|
|
*timer = xTaskGetTickCount();
|
|
|
|
*enc_sub = abs(enc - *enc_old);
|
|
|
|
if (*enc_old)
|
|
{
|
|
if (freq == 20000)
|
|
{
|
|
alarm_L = 50;
|
|
alarm_H = 350;
|
|
}
|
|
else if (freq == 80000)
|
|
{
|
|
alarm_L = 200;
|
|
alarm_H = 600;
|
|
}
|
|
if ((*enc_sub > alarm_L) && (*enc_sub < alarm_H))
|
|
*stall_flag = FALSE;
|
|
else
|
|
*stall_flag = TRUE;
|
|
}
|
|
*enc_old = enc;
|
|
}
|
|
}
|
|
|
|
static void fun_get_lvdt_flag(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *lvdt_flag = &motor->lvdt_flag;
|
|
|
|
if (motor == &motor_A)
|
|
*lvdt_flag = modbus_rtu_A.lvdt_flag;
|
|
else if (motor == &motor_B)
|
|
*lvdt_flag = modbus_rtu_B.lvdt_flag;
|
|
}
|
|
|
|
static void fun_get_position(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
modbus_rtu_t *modbus_rtu = NULL;
|
|
if (motor == &motor_A)
|
|
modbus_rtu = &modbus_rtu_A;
|
|
else if (motor == &motor_B)
|
|
modbus_rtu = &modbus_rtu_B;
|
|
|
|
motor->position_pv = modbus_rtu->distance - motor->position_zero;
|
|
}
|
|
|
|
static void fun_get_input(motor_t *motor)
|
|
{
|
|
fun_get_limit_flag(motor);
|
|
fun_get_stall_flag(motor);
|
|
fun_get_lvdt_flag(motor);
|
|
fun_get_position(motor);
|
|
}
|
|
|
|
static void fun_prot_position(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *en = &motor->en;
|
|
uint8_t direction = motor->direction;
|
|
uint32_t *step = &motor->step;
|
|
uint32_t *step_count = &motor->step_count;
|
|
/*
|
|
modbus_rtu_t *modbus_rtu = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
{
|
|
modbus_rtu = &modbus_rtu_A;
|
|
}
|
|
else if (motor == &motor_B)
|
|
{
|
|
modbus_rtu = &modbus_rtu_B;
|
|
}
|
|
|
|
if ((motor->limit_flag == TRUE) && (modbus_rtu->con_flag == FALSE))
|
|
{
|
|
*step = 0;
|
|
*step_count = 0;
|
|
*en = FALSE;
|
|
return;
|
|
}
|
|
*/
|
|
if ((motor->limit_near_flag == TRUE) && (direction == DIR_REVERSED))
|
|
{
|
|
*step = 0;
|
|
*step_count = 0;
|
|
*en = FALSE;
|
|
}
|
|
else if ((motor->limit_far_flag == TRUE) && (direction == DIR_FORWARD))
|
|
{
|
|
*step = 0;
|
|
*step_count = 0;
|
|
*en = FALSE;
|
|
}
|
|
else if (*step > 0)
|
|
{
|
|
*en = TRUE;
|
|
}
|
|
}
|
|
|
|
static void fun_proc_cmd_position(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL limit_near_flag = motor->limit_near_flag;
|
|
BOOL limit_far_flag = motor->limit_far_flag;
|
|
BOOL *force_near_flag = &motor->force_near_flag;
|
|
BOOL *force_far_flag = &motor->force_far_flag;
|
|
|
|
if (*force_near_flag == TRUE)
|
|
{
|
|
if (limit_near_flag != TRUE)
|
|
{
|
|
motor->position_sv = -DISTANCE_RATED;
|
|
motor->pre_position = DISTANCE_RATED;
|
|
}
|
|
else
|
|
{
|
|
motor->end_flag = TRUE;
|
|
*force_near_flag = FALSE;
|
|
}
|
|
}
|
|
else if (*force_far_flag == TRUE)
|
|
{
|
|
if (limit_far_flag != TRUE)
|
|
{
|
|
motor->position_sv = DISTANCE_RATED;
|
|
motor->pre_position = -DISTANCE_RATED;
|
|
}
|
|
else
|
|
{
|
|
motor->end_flag = TRUE;
|
|
*force_far_flag = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// motor->pre_position = motor->position_pv;
|
|
}
|
|
}
|
|
|
|
static void fun_zero_cmd_time()
|
|
{
|
|
for (uint8_t i = 0; i < 4; i++)
|
|
{
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
prox_switch_A_t[i][j] = 0;
|
|
prox_switch_B_t[i][j] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void fun_proc_cmd_time(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
prox_swith_pwr_t *prox_swith_pwr;
|
|
float *prox_switch_vol = NULL;
|
|
uint32_t(*time)[10] = NULL;
|
|
pars_cmd_time_t *pars_cmd_time = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
{
|
|
prox_swith_pwr = &prox_swith_pwr_A;
|
|
prox_switch_vol = prox_switch_A_vol + 1;
|
|
time = prox_switch_A_t;
|
|
pars_cmd_time = &pars_cmd_time_A;
|
|
}
|
|
else if (motor == &motor_B)
|
|
{
|
|
prox_swith_pwr = &prox_swith_pwr_B;
|
|
prox_switch_vol = prox_switch_B_vol + 1;
|
|
time = prox_switch_B_t;
|
|
pars_cmd_time = &pars_cmd_time_B;
|
|
}
|
|
|
|
if (pars_cmd_time->busy_flag == TRUE)
|
|
return;
|
|
|
|
uint8_t i = pars_cmd_time->senor_seq + 1;
|
|
|
|
if (!pars_cmd_time->delay)
|
|
pars_cmd_time->delay = 1000;
|
|
|
|
motor->position_sub = fabs(motor->position_pv - motor->Sn);
|
|
|
|
if (fabs(motor->position_sub) <= 0.2f)
|
|
{
|
|
motor->en = FALSE;
|
|
|
|
pars_cmd_time->current = prox_switch_vol[i] * 1000 / 250.0f;
|
|
|
|
switch (pars_cmd_time->ctr_seq)
|
|
{
|
|
case 0:
|
|
prox_swith_pwr->voltage_sv = PROX_SWITCH_PWR_MIN;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
pars_cmd_time->ctr_seq++;
|
|
break;
|
|
case 1:
|
|
if (pars_cmd_time->timer + pars_cmd_time->delay <= xTaskGetTickCount())
|
|
{
|
|
prox_swith_pwr->voltage_sv = 8200;
|
|
time[0][i - 1] = xTaskGetTickCount();
|
|
pars_cmd_time->ctr_seq++;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
}
|
|
break;
|
|
case 2:
|
|
if ((pars_cmd_time->current >= 1650) && (!time[1][i - 1]))
|
|
{
|
|
time[1][i - 1] = xTaskGetTickCount();
|
|
pars_cmd_time->ctr_seq++;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
break;
|
|
}
|
|
else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
|
|
{
|
|
pars_cmd_time->ctr_seq++;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
}
|
|
break;
|
|
case 3:
|
|
if ((time[1][i - 1]) && (!time[2][i - 1]) && (pars_cmd_time->current < 1650))
|
|
{
|
|
time[2][i - 1] = xTaskGetTickCount();
|
|
pars_cmd_time->ctr_seq++;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
break;
|
|
}
|
|
else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
|
|
{
|
|
pars_cmd_time->ctr_seq++;
|
|
pars_cmd_time->timer = xTaskGetTickCount();
|
|
}
|
|
break;
|
|
case 4:
|
|
if ((!time[3][i - 1]) && (pars_cmd_time->current <= 400))
|
|
{
|
|
time[3][i - 1] = xTaskGetTickCount();
|
|
pars_cmd_time->senor_seq++;
|
|
pars_cmd_time->ctr_seq = 10;
|
|
break;
|
|
}
|
|
else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
|
|
{
|
|
pars_cmd_time->senor_seq++;
|
|
pars_cmd_time->ctr_seq = 10;
|
|
}
|
|
break;
|
|
case 10:
|
|
if (pars_cmd_time->senor_seq > 9)
|
|
{
|
|
pars_cmd_time->senor_seq = 0;
|
|
if (modbus_rtu_hmi.upsend_flag == FALSE)
|
|
modbus_rtu_hmi.upsend_flag = TRUE;
|
|
pars_cmd_time->busy_flag = TRUE;
|
|
// motor->freq = 20000;
|
|
break;
|
|
}
|
|
else
|
|
pars_cmd_time->ctr_seq = 0;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pars_cmd_time->ctr_seq = 0;
|
|
pars_cmd_time->senor_seq = 0;
|
|
pars_cmd_time->busy_flag = FALSE;
|
|
|
|
if (motor->position_pv < motor->Sn)
|
|
motor->direction = DIR_FORWARD;
|
|
else if (motor->position_pv > motor->Sn)
|
|
motor->direction = DIR_REVERSED;
|
|
|
|
motor->en = TRUE;
|
|
// motor->freq = 80000;
|
|
|
|
fun_zero_cmd_time();
|
|
}
|
|
}
|
|
|
|
static void fun_proc_cmd_freq()
|
|
{
|
|
static uint32_t time;
|
|
static uint16_t pre_velocity;
|
|
|
|
if (bldc_A.velocity_pv != pre_velocity)
|
|
{
|
|
if (time + 200 <= xTaskGetTickCount())
|
|
{
|
|
modbus_rtu_hmi.upsend_flag = TRUE;
|
|
time = xTaskGetTickCount();
|
|
}
|
|
}
|
|
pre_velocity = bldc_A.velocity_pv;
|
|
}
|
|
|
|
static void fun_proc_zero_position(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *en = &motor->en;
|
|
BOOL *limit_near_flag = &motor->limit_near_flag;
|
|
uint8_t *direction = &motor->direction;
|
|
uint32_t *freq = &motor->freq;
|
|
float32 *position_zero = &motor->position_zero;
|
|
modbus_rtu_t *modbus_rtu = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
modbus_rtu = &modbus_rtu_A;
|
|
else if (motor == &motor_B)
|
|
modbus_rtu = &modbus_rtu_B;
|
|
|
|
if (*limit_near_flag == TRUE)
|
|
{
|
|
*freq = 20000;
|
|
*en = FALSE;
|
|
*position_zero = modbus_rtu->distance;
|
|
fun_get_position(motor);
|
|
motor->ctr_cmd = CMD_SET_DIST;
|
|
}
|
|
else
|
|
{
|
|
*direction = DIR_REVERSED;
|
|
*freq = 80000;
|
|
*en = TRUE;
|
|
}
|
|
}
|
|
|
|
static void fun_proc_cmd_zero(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
BOOL *en = &motor->en;
|
|
BOOL *limit_near_flag = &motor->limit_near_flag;
|
|
uint8_t *direction = &motor->direction;
|
|
uint8_t *ctr_cmd = &motor->ctr_cmd;
|
|
BOOL *zero_flag = &motor->zero_flag;
|
|
BOOL *zero_ack = &motor->zero_ack;
|
|
BOOL *end_flag = &motor->end_flag;
|
|
uint32_t *freq = &motor->freq;
|
|
uint32_t *step = &motor->step;
|
|
uint32_t *step_count = &motor->step_count;
|
|
float32 *position_zero = &motor->position_zero;
|
|
modbus_rtu_t *modbus_rtu = NULL;
|
|
|
|
if (motor == &motor_A)
|
|
modbus_rtu = &modbus_rtu_A;
|
|
else if (motor == &motor_B)
|
|
modbus_rtu = &modbus_rtu_B;
|
|
|
|
if (*limit_near_flag == TRUE)
|
|
{
|
|
*step = 0;
|
|
*step_count = 0;
|
|
*zero_flag = TRUE;
|
|
*position_zero = modbus_rtu->distance;
|
|
fun_get_position(motor);
|
|
}
|
|
else if (*zero_flag == FALSE)
|
|
{
|
|
*direction = DIR_REVERSED;
|
|
*freq = 80000;
|
|
*en = TRUE;
|
|
}
|
|
|
|
if (*zero_flag == TRUE)
|
|
{
|
|
if (motor->position_pv <= (motor->Sn * 2 - 0.2f))
|
|
{
|
|
*direction = DIR_FORWARD;
|
|
*freq = 80000;
|
|
*en = TRUE;
|
|
}
|
|
else
|
|
{
|
|
*en = FALSE;
|
|
*zero_flag = FALSE;
|
|
if (motor->ctr_cmd == CMD_ZERO_DIST)
|
|
*zero_ack = TRUE;
|
|
*end_flag = TRUE;
|
|
*ctr_cmd = CMD_SET_DIST;
|
|
*freq = 20000;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void fun_get_motor_step(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
uint8_t *direction = &motor->direction;
|
|
uint32_t *step = &motor->step;
|
|
uint32_t *step_count = &motor->step_count;
|
|
float32 *position_sv = &motor->position_sv;
|
|
float32 *position_pv = &motor->position_pv;
|
|
float32 *position_sub = &motor->position_sub;
|
|
float32 *pre_position = &motor->pre_position;
|
|
|
|
if (!motor->position_zero)
|
|
return;
|
|
if (*pre_position == *position_sv)
|
|
return;
|
|
|
|
//*position_sub = *position_sv > *position_pv ? *position_sv - *position_pv : *position_pv - *position_sv;
|
|
*position_sub = fabs(*position_sv - *position_pv);
|
|
*step = *position_sub * SCREW_PITCH * STEP_PER_CIRL;
|
|
*step_count = 0;
|
|
if (*position_sv > *position_pv)
|
|
{
|
|
*direction = DIR_FORWARD;
|
|
}
|
|
else if (*position_sv < *position_pv)
|
|
{
|
|
*direction = DIR_REVERSED;
|
|
}
|
|
*pre_position = *position_sv;
|
|
}
|
|
|
|
static void fun_proc_ctr_cmd(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
cmd_e ctr_cmd = (cmd_e)motor->ctr_cmd;
|
|
|
|
switch (ctr_cmd)
|
|
{
|
|
case CMD_NO_PROC:
|
|
fun_proc_zero_position(motor);
|
|
break;
|
|
case CMD_SET_DIST:
|
|
fun_proc_cmd_position(motor);
|
|
break;
|
|
case CMD_GET_TIME:
|
|
fun_proc_cmd_time(motor);
|
|
break;
|
|
case CMD_SET_FREQ:
|
|
fun_proc_cmd_freq();
|
|
break;
|
|
case CMD_ZERO_DIST:
|
|
fun_proc_cmd_zero(motor);
|
|
break;
|
|
case CMD_RATED_DIST:
|
|
fun_proc_cmd_zero(motor);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void fun_ctr_motor(motor_t *motor)
|
|
{
|
|
if (!motor)
|
|
return;
|
|
|
|
fun_get_input(motor);
|
|
fun_proc_ctr_cmd(motor);
|
|
fun_get_motor_step(motor);
|
|
fun_prot_position(motor);
|
|
fun_set_motor(motor);
|
|
}
|
|
|
|
void fun_ctr_beep(uint16_t delay_ms)
|
|
{
|
|
if (beep_1.en == FALSE)
|
|
return;
|
|
|
|
beep_1.delay = delay_ms;
|
|
|
|
if (modbus_rtu_hmi.upsend_count > 0)
|
|
beep_1.time = xTaskGetTickCount();
|
|
|
|
if (beep_1.time + beep_1.delay > xTaskGetTickCount())
|
|
BEEP_MUTE_OPEN();
|
|
else
|
|
BEEP_MUTE_CLOSE();
|
|
}
|