ELCT/hero
Archived
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加文件1和文件2

Browse Source
This commit is contained in:
echo
2025-11-27 20:08:38 +08:00
parent 5d4099938c
commit edc427c423
1675 changed files with 1068907 additions and 2830 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
Mecanum Wheel moving project 1/Core/Src/M3508.c Normal file
View File

@@ -0,0 +1,170 @@
#include "main.h"
#include "M3508.h"
#include "can.h"
#include "gpio.h"
#include <string.h>
#include <stdlib.h>
#include <math.h> // <20><><EFBFBD><EFBFBD>isnan<61><6E>isinf<6E><66><EFBFBD><EFBFBD>
#include "PID.h"
#include "Remote.h"
#include "CToC.h"
// <20>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>
Motor_Controller_t motor_controller[4] = {0};
// <20>洢<EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int16_t motor_commands[4] = {0};
motor_measure_t motor_chassis[4] = {0};
static CAN_TxHeaderTypeDef chassis_tx_message;
static uint8_t chassis_can_send_data[8];
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
uint32_t send_mail_box;
chassis_tx_message.StdId = 0x200;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
chassis_can_send_data[0] = motor1 >> 8;
chassis_can_send_data[1] = motor1;
chassis_can_send_data[2] = motor2 >> 8;
chassis_can_send_data[3] = motor2;
chassis_can_send_data[4] = motor3 >> 8;
chassis_can_send_data[5] = motor3;
chassis_can_send_data[6] = motor4 >> 8;
chassis_can_send_data[7] = motor4;
HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message,
chassis_can_send_data, &send_mail_box);
}
/**
* @brief <20><>RPMת<4D><D7AA>Ϊrad/s
*/
float RPM_To_RadPerSec(int16_t rpm)
{
return (float)rpm * 2.0f * 3.1415926535f / 60.0f;
}
/**
* @brief <20><>rad/sת<73><D7AA>ΪRPM
*/
int16_t RadPerSec_To_RPM(float rad_per_sec)
{
return (int16_t)(rad_per_sec * 60.0f / (2.0f * 3.1415926535f));
}
/**
* @brief <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
* @param motor_id: <20><><EFBFBD><EFBFBD>ID (0-3)
* @param target_speed_rad_s: Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD> (rad/s)
*/
void Set_Motor_Target_Speed(uint8_t motor_id, float target_speed_rad_s)
{
if (motor_id < 4) {
motor_controller[motor_id].target_speed = target_speed_rad_s;
}
}
/**
* @brief <20><><EFBFBD><EFBFBD><EFBFBD>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>񣨴<EFBFBD><F1A3A8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><D6A4>
*/
void Motor_Speed_Control_Task(float dt)
{
for (int i = 0; i < 4; i++) {
// <20><>֤<EFBFBD><D6A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (isnan(motor_controller[i].measure.speed_rpm) ||
motor_controller[i].measure.speed_rpm > 10000 ||
motor_controller[i].measure.speed_rpm < -10000) {
// <20><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3>ʹ<EFBFBD><CAB9>Ĭ<EFBFBD><C4AC>ֵ
motor_controller[i].measure.speed_rpm = 0;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>RPMת<4D><D7AA>Ϊrad/s
float actual_speed_rads = RPM_To_RadPerSec(motor_controller[i].measure.speed_rpm);
// <20><>֤ת<D6A4><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (isnan(actual_speed_rads) || isinf(actual_speed_rads)) {
actual_speed_rads = 0.0f;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
float speed_error = motor_controller[i].target_speed - actual_speed_rads;
// <20><>֤<EFBFBD><D6A4><EFBFBD><EFBFBD>ֵ
if (isnan(speed_error) || isinf(speed_error)) {
speed_error = 0.0f;
}
// PID<49><44><EFBFBD><EFBFBD>
float pid_output = PID_Calculate(&motor_controller[i].speed_pid, speed_error, dt);
// <20><>֤PID<49><44><EFBFBD><EFBFBD>
if (isnan(pid_output) || isinf(pid_output)) {
pid_output = 0.0f;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID״̬
PID_PositionClean(&motor_controller[i].speed_pid);
}
// <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
motor_commands[i] = (int16_t)pid_output;
}
}
/**
* @brief <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>ٶȽ<D9B6><C8BD>㺯<EFBFBD><E3BAAF>
* @param vx: X<><58><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> (m/s) - <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param vy: Y<><59><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> (m/s) - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param vz: <20><>ת<EFBFBD><D7AA><EFBFBD>ٶ<EFBFBD> (rad/s) - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD>˳ʱ<CBB3><CAB1>
*/
void Drive_Motor(float vx, float vy, float vz)
{
float A = WHEELBASE_WIDTH + WHEELBASE_LENGTH;
// <20><>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD>㹫ʽ
// <20><><EFBFBD>ӱ<EFBFBD><D3B1>ţ<EFBFBD>0-<2D><><EFBFBD><EFBFBD>, 1-<2D><><EFBFBD><EFBFBD>, 2-<2D><><EFBFBD><EFBFBD>, 3-<2D><><EFBFBD><EFBFBD>
// <20><>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD>㹫ʽ
float wheel1_speed = ( vx +vy + vz * A ) / WHEEL_RADIUS; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float wheel2_speed = ( vx - vy + vz * A ) / WHEEL_RADIUS; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float wheel3_speed = (-vx -vy + vz * A ) / WHEEL_RADIUS; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float wheel4_speed = (-vx +vy +vz * A ) / WHEEL_RADIUS; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_Motor_Target_Speed(0, wheel1_speed); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_Motor_Target_Speed(1, wheel2_speed); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_Motor_Target_Speed(2, wheel3_speed); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_Motor_Target_Speed(3, wheel4_speed); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
float Map_Remote_to_Speed(int16_t input, float max_speed)
{
if (input > 660) input = 660;
if (input < -660) input = -660;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӷ<EFBFBD><D3B7><EFBFBD>0
if (abs(input) < 50) {
return 0.0f;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
return (input / 660.0f) * max_speed;
}
/**
* @brief <20><><EFBFBD><EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>ٶ<EFBFBD>
*/
void Process_Remote_Control(void)
{
// ӳ<><D3B3>ң<EFBFBD><D2A3><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ٶ<EFBFBD> (m/s)
float vx = Map_Remote_to_Speed(Remote_RxData.Remote_L_UD,2.0f); // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2m/s
float vy = Map_Remote_to_Speed(Remote_RxData.Remote_L_RL, 2.0f); // <20><><EFBFBD>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2m/s
float vz = Map_Remote_to_Speed(Remote_RxData.Remote_R_RL, 3.0f); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3rad/s
// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD>
Drive_Motor(vx, vy, vz);
}