GitHub/Avem
1
0
Fork 0
mirror of https://github.com/avem-labs/Avem.git synced 2023-09-01 15:18:49 +08:00
Code Issues Projects Releases Wiki Activity

[MOD]: PID Control差不多完成了

Browse Source
This commit is contained in:
but0n 2016-10-10 18:31:36 +08:00
parent e62614ca08
commit 1ffe818e33
4 changed files with 98 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h
View File

@ -4,23 +4,26 @@
extern float g_Yaw, g_Pitch, g_Roll; //eular
#define K_P 2.57f //0.257 * 0.83 0.255
#define K_I 0
#define K_D 0
#define OUTTER_LOOP_KP 2.57f //0.257 * 0.83 0.255
#define OUTTER_LOOP_KI 0.1f
#define OUTTER_LOOP_KD 0.5f
#define INNER_LOOP_KP 0.1f
#define INNER_LOOP_KI 0.1f
#define INNER_LOOP_KD 0.1f
#define SUM_ERRO_MAX 900
#define SUM_ERRO_MIN -900
#define PID_IMAX 300
#define PID_IMIN -300
#define KP 1
#define KI 1
#define KD 1
#define PID_IMAX 30
#define PID_IMIN -30
typedef struct {
float Last;
float InnerLast;
float OutterLast;
float *Feedback;
float Erro;
float *Gyro;
float Error;
float p;
float i;
float d;

27
libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/tty.h Normal file
View File

@ -0,0 +1,27 @@
#ifndef TTY_H
#define TTY_H
#define TTY_NONE() uart_sendStr("\033[m")
#define TTY_RED() uart_sendStr("\033[0;32;31m")
#define TTY_LIGHT_RED() uart_sendStr("\033[1;31m")
#define TTY_GREEN() uart_sendStr("\033[0;32;32m")
#define TTY_LIGHT_GREEN() uart_sendStr("\033[1;32m")
#define TTY_BLUE() uart_sendStr("\033[46;30m")
#define TTY_LIGHT_BLUE() uart_sendStr("\033[1;34m")
#define TTY_DARY_GRAY() uart_sendStr("\033[1;30m")
#define TTY_CYAN() uart_sendStr("\033[0;36m")
#define TTY_LIGHT_CYAN() uart_sendStr("\033[1;36m")
#define TTY_PURPLE() uart_sendStr("\033[0;35m")
#define TTY_LIGHT_PURPLE() uart_sendStr("\033[1;35m")
#define TTY_BROWN() uart_sendStr("\033[0;33m")
#define TTY_YELLOW() uart_sendStr("\033[1;33m")
#define TTY_LIGHT_GRAY() uart_sendStr("\033[0;37m")
#define TTY_WHITE() uart_sendStr("\033[1;37m")
#define TTY_COLORRTN() uart_sendStr("\33[7m")
#define TTY_CLEAR() uart_sendStr("\33[2J")
#define TTY_UNDERLINE() uart_sendStr("\33[4m")
#define TTY_FLASH() uart_sendStr("\33[5m")
#define TTY_OPTOFF() uart_sendStr("\33[0m")
#endif

23
libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c
View File

@ -3,17 +3,26 @@
#include "motor.h"
void pid_SingleAxis(pid_pst temp, float setPoint) {
temp->Erro = setPoint - *temp->Feedback;
temp->i += temp->Erro;
temp->Error = setPoint - *temp->Feedback;
//Outter Loop PID
temp->i += temp->Error;
if (temp->i > PID_IMAX) temp->i = PID_IMAX;
else if (temp->i < PID_IMIN) temp->i = PID_IMIN;
temp->d = *temp->Feedback - temp->Last;
temp->d = *temp->Feedback - temp->OutterLast;
temp->output = (short)(KP * (temp->Erro) + KI * temp->i + KD * temp->d);
temp->Last = *temp->Feedback;
*temp->Channel1 = temp->output;
temp->output = (short)(OUTTER_LOOP_KP * (temp->Error) + OUTTER_LOOP_KI * temp->i + OUTTER_LOOP_KD * temp->d);
temp->OutterLast = *temp->Feedback; //Save Old Data
//Inner Loop PD
temp->p = temp->output + *temp->Gyro * 3.5f;
temp->d = *temp->Gyro - temp->InnerLast;
temp->output = INNER_LOOP_KP * temp->p + INNER_LOOP_KD * temp->d;
if (*temp->Channel1+temp->output > MOTOR_MAX) *temp->Channel1 = MOTOR_MAX;
else if (*temp->Channel1 + temp->output < MOTOR_LOW) *temp->Channel1 = MOTOR_LOW;
temp->InnerLast = *temp->Gyro;
*temp->Channel1 += (short)temp->output;
}
// float g_iErro, g_sumErro = 0;

58
src/main.c
View File

@ -6,6 +6,7 @@
#include "uart.h"
#include "wifi.h"
#include "pid.h"
#include "tty.h"
#define Kp 100.0f //比例增益支配率(常量)
#define Ki 0.002f //积分增益支配率
@ -15,19 +16,7 @@ float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0; //Quaternion
float g_exInt = 0, g_eyInt = 0, g_ezInt = 0;
float g_Yaw, g_Pitch, g_Roll;
// float Last;
// float *Feedback;
// float Erro;
// float p;
// float i;
// float d;
// short output;
pid_st g_pid_roll = {
.Last = 0,
.Feedback = &g_Roll,
.i = 0,
.Channel1 = &MOTOR1,
};
//ms
void delay(volatile unsigned int count) {
@ -93,6 +82,21 @@ int main() {
#if defined (DEBUG_PID) || defined (DEBUG_MPU6050_EULER) || defined (DEBUG_MPU6050_SOURCEDATA) || defined (DEBUG_BLDC)
SixAxis sourceData;
#endif
// float Last;
// float *Feedback;
// float Erro;
// float p;
// float i;
// float d;
// short output;
pid_st g_pid_roll = {
.InnerLast = 0,
.OutterLast = 0,
.Feedback = &g_Roll,
.i = 0,
.Channel1 = &MOTOR1,
.Gyro = &sourceData.gX,
};
//Brushless motor auto init
#ifdef DEBUG_BLDC
@ -128,16 +132,36 @@ int main() {
Comput(sourceData);
pid_SingleAxis(&g_pid_roll, 0);
uart_sendStr("\n\nMotor占空比:\t\t");
TTY_CLEAR();
TTY_RED();
uart_sendStr(" Motor占空比: ");
TTY_NONE();
TTY_BLUE();
uart_showData(MOTOR1);
TTY_NONE();
uart_sendStr("\n\nRoll:\t\t");
uart_sendStr("\n\rRoll:\t");
uart_Float2Char(*g_pid_roll.Feedback);
uart_sendStr("\n\nP:\t\t");
uart_sendStr("\tGyro:\t");
uart_Float2Char(*g_pid_roll.Gyro);
uart_sendStr("\n\rP:\t");
uart_Float2Char(g_pid_roll.p);
uart_sendStr("\n\rI:\t");
uart_Float2Char(g_pid_roll.i);
uart_sendStr("\n\rD:\t");
uart_Float2Char(g_pid_roll.d);
uart_sendStr("\n\r=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=\n\rInner Cache:\t");
uart_Float2Char(g_pid_roll.InnerLast);
uart_sendStr("\n\rOutter Cache:\t");
uart_Float2Char(g_pid_roll.OutterLast);
UART_CR();
#endif
#ifdef DEBUG_MPU6050_EULER