pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

发送启动命令，仿真可以(时间足够),运行不行（某个地方时间没给够）

This commit is contained in:
REASEARCHER\18383 2025-10-18 17:20:02 +08:00
parent 79b49fcc0b
commit 1dcd7bbe76
5 changed files with 74 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
APP/Device/Device_rotate/rotate_servo.c
View File

@ -61,31 +61,52 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed)
{ {
//转换为电机端的r/min //转换为电机端的r/min
speed = speed * PTZ_HORI_RATIO + 0.5; speed = speed * PTZ_HORI_RATIO + 0.5;
/*水平电机*/
if (motorType == horiMotorType) { if ( motorType == horiMotorType )//水平电机
if (speed > PTZ_HORI_MOTOR_MAX_SPEED) { {
speed = PTZ_HORI_MOTOR_MAX_SPEED;
}
else if (speed < PTZ_HORI_MIN_SPEED) {
speed = PTZ_HORI_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000 //设置速度，电机的r/min范围-6000~6000
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed) servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority);
} }
else if (motorType == vertMotorType) { /*垂直电机*/
if (speed > PTZ_VERT_MOTOR_MAX_SPEED) { else
speed = PTZ_VERT_MOTOR_MAX_SPEED; {
}
else if (speed < PTZ_VERT_MOTOR_MIN_SPEED) {
speed = PTZ_VERT_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000 //设置速度，电机的r/min范围-6000~6000
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed) servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); , WRITE_ONE_REG_FRAME_NUM, lowPriority);
} }
// //转换为电机端的r/min
// static float speedTemp;
// speed = speed * PTZ_HORI_RATIO + 0.5;
// speedTemp = speed;
// speedTemp < 0 ? speedTemp *= -1 : speedTemp; //变为正数
//
// if (motorType == horiMotorType) {
// if (speedTemp > PTZ_HORI_MOTOR_MAX_SPEED) {
// speedTemp = PTZ_HORI_MOTOR_MAX_SPEED;
// }
// else if (speedTemp < PTZ_HORI_MIN_SPEED) {
// speedTemp = PTZ_HORI_MOTOR_MIN_SPEED;
// }
// //设置速度，电机的r/min范围-6000~6000
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// }
// else if (motorType == vertMotorType) {
// if (speedTemp > PTZ_VERT_MOTOR_MAX_SPEED) {
// speedTemp = PTZ_VERT_MOTOR_MAX_SPEED;
// }
// else if (speedTemp < PTZ_VERT_MOTOR_MIN_SPEED) {
// speedTemp = PTZ_VERT_MOTOR_MIN_SPEED;
// }
//
// //设置速度，电机的r/min范围-6000~6000
// servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// }
} }
/* /*
@ -99,13 +120,15 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed)
void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{ {
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
static float speedTemp;
switch ( direction ) switch ( direction )
{ {
case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转 case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
case PTZ_HORI_DIR_STOP: speedTemp = speed;
break; break;
case PTZ_HORI_DIR_LEFT: case PTZ_HORI_DIR_LEFT:
speed = -speed; speedTemp = -speed;
break;
default: default:
break; break;
} }
@ -115,10 +138,12 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
//设定转动速度 //设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000 // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); // , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, speed); set_speed_to_servoMotor(horiMotorType, speedTemp);
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机 , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed; OSTimeDlyHMSM(0u, 0u, 0u, 50u);
g_ptz.hori_speed_set = speedTemp;
//设定转动方向 //设定转动方向
g_ptz.hori_direction_last = g_ptz.hori_direction_set; g_ptz.hori_direction_last = g_ptz.hori_direction_set;
g_ptz.hori_direction_set = direction; g_ptz.hori_direction_set = direction;
@ -135,17 +160,16 @@ void ptz_hori_stop(unsigned short int time)
{ {
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
set_speed_to_servoMotor(horiMotorType, 0);
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
// , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
//停止电机 //停止电机
g_ptz.hori_start_stop_set = PTZ_HORI_STOP; g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP; g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
// set_speed_to_servoMotor(horiMotorType, 0);
//设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, 0);
g_ptz.hori_speed_set = 0; g_ptz.hori_speed_set = 0;
g_ptz.hori_speed_actual = 0; g_ptz.hori_speed_actual = 0;
@ -153,10 +177,12 @@ void ptz_hori_stop(unsigned short int time)
{ {
OSTimeDlyHMSM(0u, 0u, 0u, time); OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_hori_stop_count = 0; ptz_hori_stop_count = 0;
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
} }
ptz_hori_stop_count ++; ptz_hori_stop_count ++;
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0) // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 // , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
//电子稳定 //电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D #ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_hori_electric_stable_init(); ptz_hori_electric_stable_init();

5
APP/Device/Device_speed/servoMotor_recv.c
View File

@ -251,13 +251,16 @@ static void ptz_recv_hori_servo_task()
CPU_INT08U err; CPU_INT08U err;
while(1) { while(1) {
OSSemPend(g_horiMotorMutex, 0, &err); OSSemPend(g_horiMotorMutex, 0, &err);
stopTimeOut(H_MOTOR); // stopTimeOut(H_MOTOR);
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false) if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
{ {
// servoLinkListMemPut(horiMotorType);
// H_MOTOR_STOP; // H_MOTOR_STOP;
// stopTimeOut(H_MOTOR);
continue; continue;
} }
// OSTimeDlyHMSM(0u, 0u, 0u, 5u); // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
stopTimeOut(H_MOTOR);
//释放信号量，通知能发送一次 //释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex); OSSemPost(g_horiSpeedMutex);
} }

15
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -546,7 +546,6 @@ void init_speed_module(void)
servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_CONTR_MODE_SELEC, 0), servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式 WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0), servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式 WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
OSTimeDlyHMSM(0u, 0u, 0u, 5u); OSTimeDlyHMSM(0u, 0u, 0u, 5u);
@ -573,10 +572,16 @@ void init_speed_module(void)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min // , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
// OSTimeDlyHMSM(0u, 0u, 0u, 5u); // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1) servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能 , WRITE_ONE_REG_FRAME_NUM, lowPriority);
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1) servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能 , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
OSTimeDlyHMSM(0u, 0u, 0u, 5u); OSTimeDlyHMSM(0u, 0u, 0u, 5u);
} }

1
APP/Service/service_error_count.c
View File

@ -23,6 +23,7 @@ char error_conut_state;
#define COUNT_STATE 1 #define COUNT_STATE 1
#define ERROR_COUNT_SPEED 1.5 #define ERROR_COUNT_SPEED 1.5
//#define ERROR_COUNT_SPEED 3

2
PROJECT/OS2.ewp
View File

@ -826,7 +826,7 @@
</option> </option>
<option> <option>
<name>IlinkIcfFile</name> <name>IlinkIcfFile</name>
<state>D:\psx\Pan-Tilt\1.software\HY\new_ptz\servoMotor\BSP\IAR\GD32F450xE.icf</state> <state>D:\CompanyCode\newPro\servoMotor_xr\BSP\IAR\GD32F450xE.icf</state>
</option> </option>
<option> <option>
<name>IlinkIcfFileSlave</name> <name>IlinkIcfFileSlave</name>