107 lines
2.6 KiB
C
107 lines
2.6 KiB
C
#include "mt6701.h"
|
|
#include "config.h"
|
|
#include "soft_i2c.h"
|
|
#include "stdio.h"
|
|
#include "ti_msp_dl_config.h"
|
|
#include "uart_redircet.h"
|
|
|
|
volatile int16_t angle;
|
|
volatile float angle_f;
|
|
volatile float angle_f_rad;
|
|
|
|
volatile bool gIsI2cError = false;
|
|
|
|
#define DEBUG_I2C false
|
|
|
|
/* Data sent to the Target */
|
|
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] = {0x03};
|
|
|
|
/* Data received from Target */
|
|
volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0};
|
|
|
|
/* I2C clock configuration */
|
|
DL_I2C_ClockConfig gI2CclockConfig;
|
|
/* Frequency of selected I2C clock*/
|
|
volatile uint32_t gClockSelFreq;
|
|
|
|
/* Cycles to delay after controller transfer initiated */
|
|
volatile uint32_t gDelayCycles;
|
|
|
|
/* I2C Target address */
|
|
#define I2C_TARGET_ADDRESS (0x06 << 1)
|
|
|
|
void MT6701_iic_read_angel(void) {
|
|
|
|
I2C_Start();
|
|
I2C_SendByte(I2C_TARGET_ADDRESS);
|
|
I2C_RecviveAck();
|
|
I2C_SendByte(0X03);
|
|
I2C_RecviveAck();
|
|
|
|
I2C_Start();
|
|
I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
|
|
I2C_RecviveAck();
|
|
gRxPacket[0] = I2C_RecviveData();
|
|
I2C_SendAck(1);
|
|
|
|
I2C_Start();
|
|
I2C_SendByte(I2C_TARGET_ADDRESS | 0x01);
|
|
I2C_RecviveAck();
|
|
gRxPacket[1] = I2C_RecviveData();
|
|
I2C_SendAck(1);
|
|
I2C_Stop();
|
|
}
|
|
|
|
// 单圈值
|
|
float GetAngle_NoTrack(struct MT6701 *mt6701) {
|
|
Set_Ang_Sensor(mt6701->Mot_num);
|
|
MT6701_iic_read_angel();
|
|
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
|
|
angle_f_rad = (float)angle * _2PI / 16384;
|
|
if (DEBUG_ENABLED & DEBUG_MT_ENABLED) {
|
|
printf("angle_rad read back is %f \n", angle_f_rad);
|
|
}
|
|
return angle_f_rad;
|
|
}
|
|
|
|
// 多圈值
|
|
float GetAngle(struct MT6701 *mt6701) {
|
|
// float D_Angle = 0.0;
|
|
mt6701->Angle = GetAngle_NoTrack(mt6701);
|
|
float D_Angle = mt6701->Angle - mt6701->Last_Angle;
|
|
|
|
if (fabs(D_Angle) > (0.8f * 2 * PI)) {
|
|
mt6701->full_rotations = mt6701->full_rotations + ((D_Angle > 0) ? -1 : 1);
|
|
}
|
|
|
|
mt6701->Last_Angle = mt6701->Angle;
|
|
|
|
mt6701->Angle = (mt6701->full_rotations * _2PI + mt6701->Last_Angle);
|
|
|
|
return mt6701->Angle;
|
|
}
|
|
|
|
float GetVelocity(struct MT6701 *mt6701_Vel) {
|
|
float dt = 0.0;
|
|
float Vel_ts = DL_SYSTICK_getValue();
|
|
if (Vel_ts < mt6701_Vel->Last_Vel_ts)
|
|
dt = (mt6701_Vel->Last_Vel_ts - Vel_ts) / MCLK_IN_MHZ * 1e-6f;
|
|
else
|
|
dt = (RELOAD_CYCLES - Vel_ts + mt6701_Vel->Last_Vel_ts) / MCLK_IN_MHZ *
|
|
1e-6f;
|
|
|
|
if (dt < 0.0001)
|
|
dt = 10000;
|
|
|
|
float Vel_Angle = GetAngle(mt6701_Vel);
|
|
|
|
float dv = Vel_Angle - mt6701_Vel->Vel_Last_Angle;
|
|
|
|
mt6701_Vel->velocity = (Vel_Angle - mt6701_Vel->Vel_Last_Angle) / dt;
|
|
|
|
mt6701_Vel->Last_Vel_ts = Vel_ts;
|
|
mt6701_Vel->Vel_Last_Angle = Vel_Angle;
|
|
|
|
return mt6701_Vel->velocity;
|
|
}
|