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

将ICM42688改为硬件SPI通讯。

Browse Source
This commit is contained in:
lijie
2024-02-22 10:16:24 +08:00
parent 08cbf4ba74
commit 7749d1d7b7
19 changed files with 1391 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
APP/LED_RGB.c Normal file
View File

@ -0,0 +1,38 @@
//
// Created by hu123456 on 2024/2/22.
//
#include "LED_RGB.h"
void LED_RGB_Init()
{
HAL_TIM_Base_Start(&htim4); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
}
void Set_RGB_color(enum RGB_Color color)
{
switch (color) {
case black:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case red:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,200);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case green:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,200);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case blue:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,200);
break;
}
}

20
APP/LED_RGB.h Normal file
View File

@ -0,0 +1,20 @@
//
// Created by hu123456 on 2024/2/22.
//
#ifndef FOC_N_LED_RGB_H
#define FOC_N_LED_RGB_H
#include "mymain.h"
enum RGB_Color
{
black,
red,
green,
blue
};
void LED_RGB_Init();
void Set_RGB_color(enum RGB_Color color);
#endif //FOC_N_LED_RGB_H

113
APP/Usart1.c Normal file
View File

@ -0,0 +1,113 @@
//
// Created by hu123456 on 2024/2/20.
//
#include "Usart1.h"
#include "stdio.h"
#include <stdlib.h>
#if USART_EN_RX
uint16_t g_usart_rx_sta = 0;
uint8_t g_usart_rx_buf[USART_REC_LEN];
uint8_t g_rx_buffer[RXBUFFERSIZE];
void set_pid();
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart->Instance == USART1)
{
if((g_usart_rx_sta & 0x8000) == 0)
{
if(g_usart_rx_sta & 0x4000)
{
if(g_rx_buffer[0] != 0x0a)
{
g_usart_rx_sta = 0;
}
else
{
g_usart_rx_sta |= 0x8000;
}
}
else
{
if(g_rx_buffer[0] == 0x0d)
{
g_usart_rx_sta |= 0x4000;
}
else
{
g_usart_rx_buf[g_usart_rx_sta & 0X3FFF] = g_rx_buffer[0] ;
g_usart_rx_sta++;
if(g_usart_rx_sta > (USART_REC_LEN - 1))
{
g_usart_rx_sta = 0;
}
}
}
}
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE);
}
}
#endif
//xzp1.16
void commander_run(void)
{
if((g_usart_rx_sta&0x8000)!=0)
{
switch(g_usart_rx_buf[0])
{
case 'H':
printf("Hello World!\r\n");
break;
case 'x': //T6.28
set_pid();
break;
case 'y':
set_pid();
break;
}
g_usart_rx_sta=0;
}
}
void set_pid()
{
float a =0;
if(g_usart_rx_buf[1] == 'z')
{
switch (g_usart_rx_buf[2])
{
case 'p':
a = atof((const char *) (g_usart_rx_buf + 3));
printf("xzp:%f\r\n",a);
break;
case 'i':
FOCStruct_X.ZT_KI = atof((const char *) (g_usart_rx_buf + 3));
printf("xzi:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'd':
FOCStruct_X.ZT_KD = atof((const char *) (g_usart_rx_buf + 3));
printf("xzd:%f\r\n",FOCStruct_X.ZT_KP);
break;
}
}
else
{
switch (g_usart_rx_buf[2])
{
case 'p':
FOCStruct_X.V_KP = atof((const char *) (g_usart_rx_buf + 3));
printf("xvp:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'i':
FOCStruct_X.V_KI = atof((const char *) (g_usart_rx_buf + 3));
printf("xvi:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'd':
FOCStruct_X.V_KD = atof((const char *) (g_usart_rx_buf + 3));
printf("xvd:%f\r\n",FOCStruct_X.ZT_KP);
break;
}
}
}

20
APP/Usart1.h Normal file
View File

@ -0,0 +1,20 @@
//
// Created by hu123456 on 2024/2/20.
//
#ifndef FOC_N_USART1_H
#define FOC_N_USART1_H
#define USART_REC_LEN 200
#define USART_EN_RX 1
#define RXBUFFERSIZE 1
#include "mymain.h"
extern uint8_t g_usart_rx_buf[USART_REC_LEN];
extern uint16_t g_usart_rx_sta;
extern uint8_t g_rx_buffer[RXBUFFERSIZE];
void commander_run(void);
#endif //FOC_N_USART1_H

40
APP/foc.c
View File

@ -17,12 +17,12 @@ 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_KP = 4;
FOCStruct_X.ZT_KI = 0;
FOCStruct_X.ZT_KD = 0.2;
FOCStruct_X.ZT_KD = 0.5;
FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1.5;
FOCStruct_X.V_KI = 2;
FOCStruct_X.V_KD = 0;
//2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
FOCStruct_Y.ZT_KP = 2;
@ -33,6 +33,22 @@ void PID_Iint(void)
FOCStruct_Y.V_KI = 1;
FOCStruct_Y.V_KD = 0;
// FOCStruct_X.ZT_KP = 4;
// FOCStruct_X.ZT_KI = 0;
// FOCStruct_X.ZT_KD = 0.5;
//
// FOCStruct_X.V_KP = 0.1;
// FOCStruct_X.V_KI = 2;
// 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;
@ -50,7 +66,6 @@ void PID_Iint(void)
// 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)
{
@ -67,7 +82,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
}
else
{// only Uq available - no need for atan2 and sqrt
{
// 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
@ -121,16 +137,16 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
}
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);
__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);
__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);
}
}
@ -160,7 +176,7 @@ void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target)
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;
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;

23
APP/mymain.c
View File

@ -6,6 +6,7 @@
void mymain()
{
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
HAL_Delay(2000);
led_init();
tle5012b_init();
@ -17,13 +18,13 @@ void mymain()
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,2.3);
i++;
}
i=0;
// while(i<3300*3)
// {
// Angel_closed_loop(motorx,&FOCStruct_X,-0.2);
// Angel_closed_loop(motory,&FOCStruct_Y,2.3);
// i++;
// }
// i=0;
LED_ON;
while(1)
{
@ -33,6 +34,7 @@ void mymain()
y = Angle.roll ;
Zitai_closed_loop(motorx,&FOCStruct_X,x);
Zitai_closed_loop(motory,&FOCStruct_Y,-y);
commander_run();
// x = getAngle(motorx);
// y = getAngle(motory);
// printf("%fY%f\n",x,y);
@ -45,7 +47,10 @@ void mymain()
// }
// i++;
//
Data_send(x,y);
// Data_send(x,y);
// HAL_Delay(3);
}
}
}

2
APP/mymain.h
View File

@ -28,6 +28,8 @@
#include "alldata.h"
#include "usart_ano.h"
#include "LED.h"
#include "Usart1.h"
#include "LED_RGB.h"
void mymain();
#endif //FOC_N_MYMAIN_H