lijie/foc_Gimbal
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
lijie
2024-01-24 15:56:10 +08:00
commit 2c58355e63
40 changed files with 5524 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
APP/LED.c Normal file
View File

@ -0,0 +1,20 @@
//
// Created by hu123456 on 2024/1/24.
//
#include "LED.h"
void led_init()
{
// <20><>ʼ<EFBFBD><CABC>TLE5012B<32><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOE_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
}

17
APP/LED.h Normal file
View File

@ -0,0 +1,17 @@
//
// Created by hu123456 on 2024/1/24.
//
#ifndef FOC_N_LED_H
#define FOC_N_LED_H
#include "mymain.h"
#define LED_ON HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, 1)
#define LED_OFF HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, 0)
void led_init();
#endif //FOC_N_LED_H

399
APP/foc.c Normal file
View File

@ -0,0 +1,399 @@
//
// Created by hu123456 on 2024/1/16.
//
#include "foc.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
float Voltage_PowerSupply = 12;
float Pole_Pairs = 14;
float Voltage_Limit = 6.9;
float Velocity_Limit = 100;
float V_Acc_Limit = 100;
float dir = 1;
FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
void PID_Iint(void)
{
FOCStruct_X.ZT_KP = 2;
FOCStruct_X.ZT_KI = 0;
FOCStruct_X.ZT_KD = 0.2;
FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1.5;
FOCStruct_X.V_KD = 0;
//2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
FOCStruct_Y.ZT_KP = 2;
FOCStruct_Y.ZT_KI = 0;
FOCStruct_Y.ZT_KD = 0.5;
FOCStruct_Y.V_KP = 0.1;
FOCStruct_Y.V_KI = 1;
FOCStruct_Y.V_KD = 0;
////2024/1/23 xY<78><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_X.ZT_KP = 2;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.2;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 1;
// FOCStruct_X.V_KD = 0;
////2024/1/22 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_Y.ZT_KP = 2;
// FOCStruct_Y.ZT_KI = 0;
// FOCStruct_Y.ZT_KD = 0.5;
//
// FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0;
}
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
{
float Uout;
unsigned int sector;
float T0,T1,T2;
float Ta,Tb,Tc;
if(Ud) // only if Ud and Uq set
{
Uout = sqrt(Ud*Ud + Uq*Uq) / Voltage_PowerSupply;
// angle normalisation in between 0 and 2pi
// only necessary if using _sin and _cos - approximation functions
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
}
else
{// only Uq available - no need for atan2 and sqrt
Uout = Uq / Voltage_PowerSupply;
// angle normalisation in between 0 and 2pi
// only necessary if using _sin and _cos - approximation functions
angle_el = _normalizeAngle(angle_el + _PI_2);
}
if(Uout> 0.577)Uout= 0.577;
if(Uout<-0.577)Uout=-0.577;
sector = (angle_el / _PI_3) + 1;
T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout;
T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
T0 = 1 - T1 - T2;
// calculate the duty cycles(times)
switch(sector)
{
case 1:
Ta = T1 + T2 + T0/2;
Tb = T2 + T0/2;
Tc = T0/2;
break;
case 2:
Ta = T1 + T0/2;
Tb = T1 + T2 + T0/2;
Tc = T0/2;
break;
case 3:
Ta = T0/2;
Tb = T1 + T2 + T0/2;
Tc = T2 + T0/2;
break;
case 4:
Ta = T0/2;
Tb = T1+ T0/2;
Tc = T1 + T2 + T0/2;
break;
case 5:
Ta = T2 + T0/2;
Tb = T0/2;
Tc = T1 + T2 + T0/2;
break;
case 6:
Ta = T1 + T2 + T0/2;
Tb = T0/2;
Tc = T1 + T0/2;
break;
default: // possible error state
Ta = 0;
Tb = 0;
Tc = 0;
}
if (motor == motory)
{
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);
}
else if (motor == motorx)
{
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,Tc*4800);
}
}
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target)
{
float Uq,Ud;
double electrical_angle;
unsigned long Time_Now;
double Ts;
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
else
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
FOCStruct->Time_Prev = Time_Now;
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>V_Now
double Angle_Now=getAngle(motor);
double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
FOCStruct->Angle_Prev = Angle_Now;
V_Now = dir*LPF_velocity(V_Now,FOCStruct);
double V_Error = V_Target - V_Now;
float V_Pid_P = FOCStruct->V_KP * V_Error;
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
float V_Out = V_Pid_P+V_Pid_I;
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;
FOCStruct->V_Pid_IPrev = V_Pid_I;
FOCStruct->V_Out_Prev = V_Out;
FOCStruct->V_Error_Prev = V_Error;
Uq = V_Out;
Ud = 0;
// double shaft_angle = dir*Angle_Now;
double shaft_angle = dir*Angle_Now;
// electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error)
{
float Uq,Ud;
double electrical_angle;
unsigned long Time_Now;
double Ts;
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
else
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
FOCStruct->Time_Prev = Time_Now;
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
//<2F><>̬<EFBFBD><CCAC>
float V_Target = FOCStruct->ZT_KP * ZT_Error + FOCStruct->ZT_KD * (ZT_Error - FOCStruct->ZT_ErrorPrev) / Ts;
V_Target = _constrain(V_Target, -Velocity_Limit, Velocity_Limit);
FOCStruct->ZT_ErrorPrev = ZT_Error;
//<2F>ٶȻ<D9B6>
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>V_Now
double Angle_Now=getAngle(motor);
double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
FOCStruct->Angle_Prev = Angle_Now;
V_Now = dir*LPF_velocity(V_Now,FOCStruct);
double V_Error = V_Target - V_Now;
float V_Pid_P = FOCStruct->V_KP * V_Error;
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
float V_Out = V_Pid_P+V_Pid_I;
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;
FOCStruct->V_Pid_IPrev = V_Pid_I;
FOCStruct->V_Out_Prev = V_Out;
FOCStruct->V_Error_Prev = V_Error;
Uq = V_Out;
Ud = 0;
// double shaft_angle = dir*Angle_Now;
double shaft_angle = dir*Angle_Now;
// electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target)
{
float Uq,Ud;
double electrical_angle;
unsigned long Time_Now;
double Ts;
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
else
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
FOCStruct->Time_Prev = Time_Now;
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
//<2F>ǶȻ<C7B6>
double Angle_Now=getAngle(motor);
float error = Angle_target - Angle_Now;
Uq = 20 * error;
Uq = _constrain(Uq, -Voltage_Limit, Voltage_Limit);
double shaft_angle = dir*Angle_Now;
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}
double LPF_velocity(double x,FOCTypeDef *FOCStruct)
{
float y = 0.9*(FOCStruct->V_Prev)+ 0.1*x;
FOCStruct->V_Prev=y;
return y;
}
// normalizing radian angle to [0,2PI]
double _normalizeAngle(float angle)
{
float a = fmod(angle, _2PI);
return a >= 0 ? a : (a + _2PI);
}
void PWM_Start()
{
HAL_TIM_Base_Start(&htim2); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
HAL_TIM_Base_Start(&htim3); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
}
void moto_Init()
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1);
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1);
PWM_Start();
HAL_Delay(1000);
alignSensor(motory,&FOCStruct_Y);
alignSensor(motorx,&FOCStruct_X);
HAL_Delay(50);
}
int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct)
{
long i;
float angle;
float mid_angle = 0,end_angle = 0;
float moved;
float q = 3.5;
printf("MOT: Align sensor.\r\n");
// find natural direction
// move one electrical revolution forward
HAL_Delay(100);
for(i=0; i<=500; i++)
{
angle = _3PI_2 + _2PI * i / 500.0;
setPhaseVoltage(moto,q, 0, angle);
HAL_Delay(2);
}
mid_angle=getAngle(moto);
for(i=500; i>=0; i--)
{
angle = _3PI_2 + _2PI * i / 500.0 ;
setPhaseVoltage(moto,q, 0, angle);
HAL_Delay(2);
}
end_angle=getAngle(moto);
setPhaseVoltage(moto,0, 0, 0);
HAL_Delay(200);
printf("mid_angle=%.4f\r\n",mid_angle);
printf("end_angle=%.4f\r\n",end_angle);
moved = fabs(mid_angle - end_angle);
if((mid_angle == end_angle)||(moved < 0.02)) //<2F><><EFBFBD>Ȼ<EFBFBD><C8BB>߼<EFBFBD><DFBC><EFBFBD>û<EFBFBD>ж<EFBFBD>
{
printf("MOT: Failed to notice movement loop222.\r\n");
// M1_Disable; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲻ<EFBFBD><E2B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD>
return 0;
}
else if(mid_angle < end_angle)
{
printf("MOT: sensor_direction==CCW\r\n");
// dir = -1;
}
else
{
printf("MOT: sensor_direction==CW\r\n");
// dir = 1;
}
printf("MOT: PP check: \n"); //<2F><><EFBFBD><EFBFBD>Pole_Pairs
int pp;
if( fabs(moved*Pole_Pairs - _2PI) > 0.5 ) // 0.5 is arbitrary number it can be lower or higher!
{
printf("fail - estimated pp\n");
pp = _2PI/moved+0.5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Σ<EFBFBD><CEA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
printf("pp=%d\r\n",pp);
}
else
printf("OK!\r\n");
setPhaseVoltage(moto,q,0,_3PI_2); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƽǶ<C6BD>
HAL_Delay(700);
FOCStruct->zero_electric_angle = _normalizeAngle(dir*getAngle(moto) * Pole_Pairs);
// delay_ms(20);
// printf("MOT: Zero elec. angle:");
// printf("%.4f\r\n",zero_electric_angle);
setPhaseVoltage(moto,0, 0, 0);
HAL_Delay(50);
return 1;
}

40
APP/foc.h Normal file
View File

@ -0,0 +1,40 @@
//
// Created by hu123456 on 2024/1/16.
//
#ifndef FOC_N_FOC_H
#define FOC_N_FOC_H
#include "mymain.h"
typedef struct
{
float ZT_KP;
float ZT_KI;
float ZT_KD;
float V_KP;
float V_KI;
float V_KD;
float ZT_ErrorPrev;
float V_Prev;
float V_Pid_IPrev;
float V_Error_Prev;
float V_Out_Prev;
float zero_electric_angle;
double Angle_Prev;
unsigned long Time_Prev;
}FOCTypeDef;
extern FOCTypeDef FOCStruct_X;
extern FOCTypeDef FOCStruct_Y;
int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct);
void PID_Iint(void);
double _normalizeAngle(float angle);
double LPF_velocity(double x,FOCTypeDef *FOCStruct);
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target);
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error);
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target);
void PWM_Start();
void moto_Init();
#endif //FOC_N_FOC_H

55
APP/mymain.c Normal file
View File

@ -0,0 +1,55 @@
//
// Created by hu123456 on 2024/1/16.
//
#include "mymain.h"
void mymain()
{
HAL_Delay(2000);
led_init();
tle5012b_init();
Init_ICM42688();
imu_rest();
moto_Init();
PID_Iint();
setvbuf(stdout, NULL, _IONBF, 0); //<2F><><EFBFBD><EFBFBD>printfû<66><C3BB>\n<><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float x = 0,y=0;
unsigned int i =0;
HAL_TIM_Base_Start(&htim5); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
while(i<3300*3)
{
Angel_closed_loop(motorx,&FOCStruct_X,-0.2);
Angel_closed_loop(motory,&FOCStruct_Y,0.7);
i++;
}
LED_ON;
while(1)
{
MpuGetData();//
GetAngle(&ICM42688,&Angle);
x = Angle.pitch;
y = Angle.roll ;
// Zitai_closed_loop(motorx,&FOCStruct_X,x);
// Zitai_closed_loop(motory,&FOCStruct_Y,-y);
// x = getAngle(motorx);
// y = getAngle(motory);
// printf("%fY%f\n",x,y);
// speed_closed_loop(motorx,&FOCStruct_X,_2PI*5);
// speed_closed_loop(motory,&FOCStruct_Y,_2PI*2);
// printf("b%d\n",b);
// if(i%3000 ==0)
// {
// printf("%d\n",i);
// }
// i++;
//
Data_send(x,y,0,0);
// HAL_Delay(3);
}
}

33
APP/mymain.h Normal file
View File

@ -0,0 +1,33 @@
//
// Created by hu123456 on 2024/1/16.
//
#ifndef FOC_N_MYMAIN_H
#define FOC_N_MYMAIN_H
#define motory 0x01
#define motorx 0x02
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "tle5012b.h"
#include "stdio.h"
#include "math.h"
#include "foc.h"
#include "spi.h"
#include "imu.h"
#include "icm42688.h"
#include "alldata.h"
#include "usart_ano.h"
#include "LED.h"
void mymain();
#endif //FOC_N_MYMAIN_H

214
APP/tle5012b.c Normal file
View File

@ -0,0 +1,214 @@
//
// Created by hu123456 on 2024/1/16.
//
#include "tle5012b.h"
long cpr =32767;
float motory_full_rotation_offset=0,motorx_full_rotation_offset=0;
long motory_angle_data_prev =0,motorx_angle_data_prev = 0;
//void tle5012b_init(void)
//{
// // <20><>ʼ<EFBFBD><CABC>TLE5012B<32><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// GPIO_InitTypeDef GPIO_InitStruct = {0};
// __HAL_RCC_GPIOA_CLK_ENABLE();
//
// GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5;
// GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5, GPIO_PIN_RESET);
//
// HAL_Delay(10);
// getAngle(motory);
// getAngle(motorx);
//}
//
//void mosi()
//{
// GPIO_InitTypeDef GPIO_InitStruct = {0};
// __HAL_RCC_GPIOA_CLK_ENABLE();
//
// GPIO_InitStruct.Pin = GPIO_PIN_7;
// GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//
// /*Configure GPIO pins : PBPin PB14 */
// GPIO_InitStruct.Pin = GPIO_PIN_6;
// GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//}
//
//void miso()
//{
// GPIO_InitTypeDef GPIO_InitStruct = {0};
// __HAL_RCC_GPIOA_CLK_ENABLE();
//
// GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
// GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//}
//
//static void Write_16bit(unsigned short dat)
//{
// mosi();
// for(uint8_t i = 0; i < 16; i++)
// {
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
//
// if(dat&0x8000) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,1);
// else HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,0);
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
// dat <<= 1;
// }
//}
//
//static void Read_16bit(unsigned short *dat)
//{
// miso();
// *dat = 0;
// for(uint8_t i = 0; i < 16; i++) {
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
// *dat <<= 1;
// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
// *dat |= HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_6) ? 1 : 0;
// }
//}
//
//uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
//{
// uint16_t u16Data;
// Write_16bit(u16RegValue);
// Read_16bit(&u16Data);
// return u16Data;
//}
//
//double getAngle(unsigned char motor)
//{
// float angle_data,d_angle;
// if(motor == motory)
// {
// TLE_CS1_ENABLE;
// angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
// d_angle = angle_data - motory_angle_data_prev;
// if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
// motory_angle_data_prev = angle_data;
// TLE_CS1_DISABLE;
// return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
// }
// else
// {
// TLE_CS2_ENABLE;
// angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
// d_angle = angle_data - motorx_angle_data_prev;
// if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
// motorx_angle_data_prev = angle_data;
// TLE_CS2_DISABLE;
// return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
// }
//}
/******************************************************************************/
///////Ӳ<><D3B2>
void tle5012b_init(void)
{
// <20><>ʼ<EFBFBD><CABC>TLE5012B<32><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4, GPIO_PIN_RESET);
HAL_Delay(10);
getAngle(motory);
getAngle(motorx);
FOCStruct_X.Angle_Prev = getAngle(motorx);
FOCStruct_Y.Angle_Prev = getAngle(motory);
}
void SPI2_TX_OFF()
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOA_CLK_ENABLE();
/**SPI1 GPIO Configuration
PA5 ------> SPI1_SCK
PA6 ------> SPI1_MISO
PA7 ------> SPI1_MOSI
*/
GPIO_InitStruct.Pin = GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
void SPI2_TX_ON()
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
double getAngle(unsigned char motor)
{
long angle_data,d_angle;
if(motor == motory)
{
TLE_CS1_ENABLE;
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
d_angle = angle_data - motory_angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motory_angle_data_prev = angle_data;
TLE_CS1_DISABLE;
return (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
}
else
{
TLE_CS2_ENABLE;
angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
d_angle = angle_data - motorx_angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
motorx_angle_data_prev = angle_data;
TLE_CS2_DISABLE;
return (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
}
}
uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
{
uint16_t u16Data;
HAL_SPI_Transmit( &hspi1, (uint8_t *)(&u16RegValue), sizeof(u16RegValue)/sizeof(uint16_t), 0xff );
SPI2_TX_OFF();
HAL_SPI_Receive( &hspi1,(uint8_t *)(&u16Data), sizeof(u16Data)/sizeof(uint16_t), 0xff );
SPI2_TX_ON();
return (u16Data & 0x7FFF);
}
//uint16_t ReadSpeed(void)
//{
// return ReadTLE5012B_1(READ_SPEED_VALUE);
//}

21
APP/tle5012b.h Normal file
View File

@ -0,0 +1,21 @@
//
// Created by hu123456 on 2024/1/16.
//
#ifndef FOC_N_TLE5012B_H
#define FOC_N_TLE5012B_H
#include "mymain.h"
#define READ_ANGLE_VALUE 0x8020 //8021 // 20: NO CRC
#define READ_SPEED_VALUE 0x8030 //8031 // 30: NO CRC
#define TLE_CS1_ENABLE HAL_GPIO_WritePin( GPIOA, GPIO_PIN_3, 0 )
#define TLE_CS1_DISABLE HAL_GPIO_WritePin( GPIOA, GPIO_PIN_3, 1 )
#define TLE_CS2_ENABLE HAL_GPIO_WritePin( GPIOA, GPIO_PIN_4, 0 )
#define TLE_CS2_DISABLE HAL_GPIO_WritePin( GPIOA, GPIO_PIN_4, 1 )
void tle5012b_init(void);
double getAngle(unsigned char motor);
uint16_t ReadTLE5012B_1(uint16_t u16RegValue);
#endif //FOC_N_TLE5012B_H

51
APP/usart_ano.c Normal file
View File

@ -0,0 +1,51 @@
//
// Created by hu123456 on 2024/1/22.
//
#include "usart_ano.h"
unsigned char Data_Buff[32] ={0XAA,0XFF,0XF1};
//AA FF F1 10 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 AA 37
void Data_send(int32_t _a,int32_t _b,int32_t _c,int32_t _d)
{
unsigned char i,cnt=4;
unsigned char sc=0,ac=0;
Data_Buff[cnt++] = BYTE0(_a);
Data_Buff[cnt++] = BYTE1(_a);
Data_Buff[cnt++] = BYTE2(_a);
Data_Buff[cnt++] = BYTE3(_a);
Data_Buff[cnt++] = BYTE0(_b);
Data_Buff[cnt++] = BYTE1(_b);
Data_Buff[cnt++] = BYTE2(_b);
Data_Buff[cnt++] = BYTE3(_b);
Data_Buff[cnt++] = BYTE0(_c);
Data_Buff[cnt++] = BYTE1(_c);
Data_Buff[cnt++] = BYTE2(_c);
Data_Buff[cnt++] = BYTE3(_c);
Data_Buff[cnt++] = BYTE0(_d);
Data_Buff[cnt++] = BYTE1(_d);
Data_Buff[cnt++] = BYTE2(_d);
Data_Buff[cnt++] = BYTE3(_d);
Data_Buff[3]=cnt-4;
for(i=0;i<cnt;i++)
{
sc+=Data_Buff[i];
ac+=sc;
}
Data_Buff[cnt++] = sc;
Data_Buff[cnt++] = ac;
for( i = 0 ; i < cnt; i++)
{
USART1->TDR = Data_Buff[i];
while((USART1->ISR & USART_ISR_TC) == 0)
{}
}
}

15
APP/usart_ano.h Normal file
View File

@ -0,0 +1,15 @@
//
// Created by hu123456 on 2024/1/22.
//
#ifndef FOC_N_USART_ANO_H
#define FOC_N_USART_ANO_H
#include "mymain.h"
#define BYTE0(dwTemp) ( *( (char *)(&dwTemp) + 0) )
#define BYTE1(dwTemp) ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp) ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp) ( *( (char *)(&dwTemp) + 3) )
void Data_send(int32_t _a,int32_t _b,int32_t _c,int32_t _d);
#endif //FOC_N_USART_ANO_H