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 test plathform

Browse Source
This commit is contained in:
but0n 2016-10-05 21:33:55 +08:00
parent 6be256a135
commit 71b856f024
3 changed files with 43 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,15 @@
#pragma once
#ifndef PID_H_
#define PID_H_
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 SUM_ERRO_MAX 900
#define SUM_ERRO_MIN -900
void pid(float setPoint, float d);
#endif

26
libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c Normal file
View File

@ -0,0 +1,26 @@
#include "pid.h"
#include "stm32f10x.h"
#include "motor.h"
float g_iErro, g_sumErro = 0;
void pid(float setPoint, float d) {
g_iErro = g_Pitch - setPoint; //计算当前误差
g_sumErro += g_iErro; //对误差进行积分
if(g_sumErro > SUM_ERRO_MAX) g_sumErro = SUM_ERRO_MAX; //积分限幅
else if(g_sumErro < SUM_ERRO_MIN) g_sumErro = SUM_ERRO_MIN;
short resu = (short)(g_iErro * K_P + g_sumErro * K_I + d * K_D); //PID输出
if((MOTOR2 + resu) > MOTOR_MAX)
MOTOR2 = MOTOR_MAX;
else if((MOTOR2 + resu) < MOTOR_LOW)
MOTOR2 = MOTOR_LOW;
else MOTOR2 += resu;
if((MOTOR4 + resu) > MOTOR_MAX)
MOTOR4 = MOTOR_MAX;
else if((MOTOR4 + resu) < MOTOR_LOW)
MOTOR4 = MOTOR_LOW;
else MOTOR4 += resu;
}

31
src/main.c
View File

@ -5,6 +5,7 @@
#include "motor.h"
#include "uart.h"
#include "wifi.h"
#include "pid.h"
#define Kp 100.0f //比例增益支配率(常量)
#define Ki 0.002f //积分增益支配率
@ -12,36 +13,8 @@
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; //eular
float g_Yaw, g_Pitch, g_Roll;
#define K_P 2.57f //0.257 * 0.83 0.255
#define K_I 0
#define K_D 0
#define SUM_ERRO_MAX 900
#define SUM_ERRO_MIN -900
float g_iErro, g_sumErro = 0;
void pid(float setPoint, float d) {
g_iErro = g_Pitch - setPoint; //计算当前误差
g_sumErro += g_iErro; //对误差进行积分
if(g_sumErro > SUM_ERRO_MAX) g_sumErro = SUM_ERRO_MAX; //积分限幅
else if(g_sumErro < SUM_ERRO_MIN) g_sumErro = SUM_ERRO_MIN;
short resu = (short)(g_iErro * K_P + g_sumErro * K_I + d * K_D); //PID输出
if((MOTOR2 + resu) > MOTOR_MAX)
MOTOR2 = MOTOR_MAX;
else if((MOTOR2 + resu) < MOTOR_LOW)
MOTOR2 = MOTOR_LOW;
else MOTOR2 += resu;
if((MOTOR4 + resu) > MOTOR_MAX)
MOTOR4 = MOTOR_MAX;
else if((MOTOR4 + resu) < MOTOR_LOW)
MOTOR4 = MOTOR_LOW;
else MOTOR4 += resu;
}
//ms
void delay(volatile unsigned int count) {