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

as5600 --> mt6701

Browse Source
This commit is contained in:
4x-tech
2025-11-10 18:22:44 +08:00
parent ae9f390667
commit cd01dcf51e
3 changed files with 60 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
3rd/mt6701.c
View File

@@ -3,6 +3,7 @@
volatile int16_t angle;
volatile float angle_f;
volatile float angle_f_rad;
/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
@@ -103,8 +104,56 @@ void MT6701_iic_read_angel(void)
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
}
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if( fabs(D_Angle) > (0.8f*2*PI) )
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1);
}
Last_Angle = Angle;
return (full_rotations * 2 * PI + Last_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)/9*1e-6f;
else dt = (0xFFFFFF - Vel_ts + Last_Vel_ts)/9*1e-6f;
if(dt < 0.0001) dt = 10000;
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle)/dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
}
void MT6701_get_angle_degree(void)
{

7
3rd/mt6701.h
View File

@@ -7,6 +7,10 @@
extern "C"
{
#endif
#define PI 3.14159265359f
#define _2PI 6.28318530718f
#define I2C_TX_PACKET_SIZE (1)
/*
@@ -17,6 +21,9 @@ extern "C"
#define I2C_RX_PACKET_SIZE (2)
void MT6701_iic_read_angel(void);
void MT6701_get_angle_degree(void);
float GetAngle(void);
float GetAngle_NoTrack(void);
float GetVelocity(void);
#ifdef __cplusplus
}