include “sys.h”

include “delay.h”

include “usart.h”

include “led.h”

include “lcd.h”

include “key.h”

include “rs485.h”

include “usart3.h”

u8 Data1[5]={0x77,0x04,0x00,0x04,0x08};
int main(void)
{
u8 key=0;
u8 i=0,t=0;
u8 cnt=0;
u8 *p;
u8 offset=0;
double p2;

float curr;
float set;
float dk1;

// float dk2;
float t1;
float t2;
// float t3;
float En;
float En_1;
float En_2;
float Kp;
float Tsam;
float Ti;
float Td;
float Dout;
u16 calc_cycle;
float currpwm;
float weifu;
float pwm_cycle;
u8 rs485buf[14]={0};
// char xbuff[14]={0};
set =0;
currpwm=0;
pwm_cycle=10000;
calc_cycle=1;
// Td=20;
Ti=8;
Kp=500;
Tsam=10;

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init(168);
uart_init(9600);
usart3_init(9600);  LED_Init();
LCD_Init();
KEY_Init();
POINT_COLOR=BLUE;
LCD_ShowString(30,210,200,16,16,"Receive Data:");           while(1)
{u3_Send_Data(Data1,5);delay_ms(30);
u3_Receive_Data(rs485buf,&key);if(key){printf("%c",key);for(i=0;i<key;i++){LCD_ShowxNum(30+i*32,230,rs485buf[i],3,16,0X80);    offset+=sprintf((char*)p+offset,"%x,",rs485buf[i]);if (i==7){if(rs485buf[i]>=10){p2=rs485buf[i]-10;}else{p2=rs485buf[i];}}if(i==8){p2=p2+rs485buf[i];}if(i==9){p2=p2+(rs485buf[i])/100.0;}}printf((char*)p);offset=0;LCD_ShowString(30,430,400,16,16,p);}t++; key=0;if(t==20){LED0=!LED0;//ÌáÊ¾ÏµÍ³ÕýÔÚÔËÐÐ   t=0;cnt++;                                     LCD_ShowxNum(30+48,150,cnt,3,16,0X80);  }

// p2=atof(rs485buf[i]);
p2=rs485buf[i];
curr=p2;
// curr=atof(p2);

En=set-curr;
dk1=En-En_1;

t1=Kp*dk1;

t2=(Kp*Tsam)/Ti;
t2=t2*En;

Dout=t1+t2;
currpwm=currpwm+Dout;
if(currpwm>pwm_cycle)
{
currpwm=pwm_cycle;
}
else if(currpwm<-pwm_cycle)
{
currpwm=-pwm_cycle;
}
currpwm=currpwm*1;
En_2=En_1;
En_1=En;

RCC->AHB1ENR |= (1<<2);

GPIOC->MODER |= 0X000AA000;

GPIOC->OSPEEDR |= 0x000ff000;
GPIOC->PUPDR |= 0x00055000;

GPIOC->AFR[0] |= 0x22000000;
GPIOC->AFR[1] |= 0x00000022;

RCC->APB1ENR |= (1<<1);
TIM3->PSC = 83;
TIM3->ARR = 10000;
TIM3->EGR |= 1;

TIM3->CCMR1 |= 0x6060;
TIM3->CCMR2 |= 0x6060;

TIM3->CCR4 = 2000;if(currpwm<0){

TIM3->CCR1 = 0000;
TIM3->CCR2 = 10000;
TIM3->CCR3 = -currpwm;
//TIM3->CCR4 = 500;

TIM3->CCER |= 0x1111;

TIM3->CCMR1 |= 0x0808;
TIM3->CCMR2 |= 0x0808;

TIM3->CR1 |= 1;
}
if(currpwm>0)
{
TIM3->CCR1 = 10000;
TIM3->CCR2 = 0000;
TIM3->CCR3 = currpwm;
//TIM3->CCR4 = 500;

TIM3->CCER |= 0x1111;

TIM3->CCMR1 |= 0x0808;
TIM3->CCMR2 |= 0x0808;

TIM3->CR1 |= 1;
}
if(curr>-0.2&&curr<0.2)
{
TIM3->CCR1 = 0000;
TIM3->CCR2 = 0000;
TIM3->CCR3 = 0000;
//TIM3->CCR4 = 0000;

TIM3->CCER |= 0x1111;

TIM3->CCMR1 |= 0x0808;
TIM3->CCMR2 |= 0x0808;

TIM3->CR1 |= 1;
}

if (currpwm<0)
{
LCD_ShowChar(30+40,350,'-',16,0);
weifu=-currpwm;
LCD_ShowNum(30+48,350,weifu,5,16);
}
if (currpwm>=0)
{
LCD_ShowNum(30+48,350,currpwm,5,16);
}
}

}
无法实现姿态控制
// p2=atof(rs485buf[i]);
p2=rs485buf[i];
curr=p2;
// curr=atof(p2);
是不是这几句出现问题？？
求大神解惑

PID实现水平姿态角控制相关推荐

Ardupilot飞控姿态角与姿态角速度控制过程
目录目录摘要 1.自稳模式初始化 2.自稳模式更新函数这个代码主要把横滚输入,俯仰输入量转换成目标角度需要的范围,也就是+4500,-4500 3.姿态角速度代码控制过程分析 4.电机PWM控制 ...
固定翼飞机姿态角Backstepping反步法控制
固定翼飞机姿态角Backstepping反步法控制 1. 数学建模 2. 系统解耦 3. 反步法控制律设计 4. 仿真结果 5. 仿真结果分析 6. 参考文献在文章固定翼飞机数学建模入门(姿态角篇) ...
Ardupilot飞控姿态角与姿态角速度控制过程分析(超长篇)
目录文章目录目录摘要 1.自稳模式初始化 2.自稳模式更新函数 **这个代码主要把横滚输入,俯仰输入量转换成目标角度需要的范围,也就是+4500,-4500** 3.姿态角速度代码控制过程分析 ...
姿态角（Euler角）：yaw pitch roll
姿态角和自由度相关联: 姿态角在控制,机械领域广泛使用: 这里,简单了解一下: 什么是姿态角(Euler角)? yaw,pitch,roll的识别? 步骤阅读百度经验:jingyan.baidu.c ...
STM32F4单片机陀螺仪获取姿态角（有代码）
mpu6050简介: MPU-6000(6050)为全球首例整合性6轴运动处理组件,相较于多组件方案,免除了组合陀螺仪与加速器时间轴之差的问题,减少了大量的封装空间.当连接到三轴磁强计时,MPU-60 ...
四元数解算姿态角解析
本文来自:链接一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解算法,下面我们先来看看几种求解姿态角的算法: 1. 欧拉角法: 欧拉 ...
关于无人机四元数解算姿态角解析你知道吗？
原文链接http://www.elecfans.com/d/705815.html 一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解 ...
单片机开发教程3——串口发送MPU6050姿态角
文章目录 1. 简介 1.1 模块原理图 1.2 引脚说明 1.3 接线方式 2. IIC通信 2.1 IIC介绍 2.2 例程讲解 3. 姿态解算 3.1 欧拉角 3.2 解算方法 3.3 一阶互补 ...
matlab三维重建中将灭点坐标反归一化,基于灭点查找的姿态角测量方法在路面三维重建中的应用...
基于灭点查找的姿态角测量方法在路面三维重建中的应用第24卷第7期传感技术学报 V01．24No．7 2011年7月 CHINESEJOURNAL0FSENSORSANDAclUATORS July2 ...
加速度计、磁力计求初始姿态角
1.几个基本概念. 地理坐标系:北东地.北为X轴.东为Y轴.地为Z轴. 机体坐标系:见下图. 姿态角:我的理解是,roll角是Zb在ZOY平面与Z轴的夹角.pitch角是Zb在ZOX平面与Z轴的夹角. ...

PID实现水平姿态角控制

include “sys.h”

include “delay.h”

include “usart.h”

include “led.h”

include “lcd.h”

include “key.h”

include “rs485.h”

include “usart3.h”

PID实现水平姿态角控制相关推荐

最新文章

热门文章