4x-tech/foc_ccs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chenged to 2 motor velocity-closed-loop; before testing

Browse Source
This commit is contained in:
4x-tech
2025-11-20 09:50:13 +08:00
parent dd8f7c006f
commit ed09f10c03
13 changed files with 276 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
3rd/mt6701.c
View File

@@ -160,7 +160,8 @@ volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
// 单圈值
float GetAngle_NoTrack(void) {
float GetAngle_NoTrack(struct AS5600_Sensor *AS5600) {
Set_Ang_Sensor(AS5600->Mot_num);
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
@@ -169,52 +170,43 @@ float GetAngle_NoTrack(void) {
}
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle_rad = 0.0;
// 多圈值
float GetAngle(void) {
volatile float D_Angle_rad = 0.0;
volatile float Angle_rad = GetAngle_NoTrack();
D_Angle_rad = Angle_rad - Last_Angle_rad;
float GetAngle(struct AS5600_Sensor *AS5600) {
// float D_Angle = 0.0;
AS5600->Angle = GetAngle_NoTrack(AS5600);
float D_Angle = AS5600->Angle - AS5600->Last_Angle;
if (fabs(D_Angle_rad) > (0.8f * 2 * PI)) {
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
if (fabs(D_Angle) > (0.8f * 2 * PI)) {
AS5600->full_rotations = AS5600->full_rotations + ((D_Angle > 0) ? -1 : 1);
}
Last_Angle_rad = Angle_rad;
AS5600->Last_Angle = AS5600->Angle;
return (full_rotations * 2 * PI + Last_Angle_rad);
AS5600->Angle = (AS5600->full_rotations * _2PI + AS5600->Last_Angle);
return AS5600->Angle;
}
volatile float Last_Vel_ts = 0.0;
volatile float Vel_Last_Angle = 0.0;
float GetVelocity(void) {
volatile float dt = 0.0;
volatile float Vel_ts = SysTick->VAL;
if (Vel_ts < Last_Vel_ts) {
dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f;
} else {
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
float GetVelocity(struct AS5600_Sensor *AS5600_Vel) {
float dt = 0.0;
float Vel_ts = DL_SYSTICK_getValue();
if (Vel_ts < AS5600_Vel->Last_Vel_ts)
dt = (AS5600_Vel->Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
else
dt = (0xFFFFFF - Vel_ts + AS5600_Vel->Last_Vel_ts) / 9 * 1e-6f;
if (dt < 0.0001) {
if (dt < 0.0001)
dt = 10000;
}
float Vel_Angle = GetAngle();
float Vel_Angle = GetAngle(AS5600_Vel);
float dv = Vel_Angle - Vel_Last_Angle;
float dv = Vel_Angle - AS5600_Vel->Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
AS5600_Vel->velocity = (Vel_Angle - AS5600_Vel->Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
AS5600_Vel->Last_Vel_ts = Vel_ts;
AS5600_Vel->Vel_Last_Angle = Vel_Angle;
return velocity;
}
void MT6701_get_angle_degree(void) {
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f = (float)angle * 360 / 16384;
return AS5600_Vel->velocity;
}