滚球系统单点定位STM32代码 -

#include "stm32f4xx.h"
#include "usart.h"
#include "delay.h"
#include "sys.h"
#include "led.h"
#include "timer.h"
#include "key.h"
#include "lcd.h"
#include "usmart.h"
#include "usart2.h"
#include "ov2640.h"
#include "dcmi.h"
#include "string.h"
#include "pwm.h"
u16 rgb_buf[240][240];
u16 gray,hang=0,k=500;
u8 X_MAX,Y_MAX=0,key=0;
u8 X_MIN,Y_MIN=240;
u8 X_MAX_LAST,X_MIN_LAST,Y_MAX_LAST,Y_MIN_LAST=0;//小球上一次坐标
u8 X,Y=0;//小球质心坐标
u16 PWM_X,PWM_Y=0;
float ERR_X,ERR_Y=0;
float ERR_X_LAST,ERR_Y_LAST=0;
float AIM_X=107,AIM_Y=115;
float KP,KI,KD=0;
int main(void)
{
u16 i,j,n,m;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init(168);
uart_init(115200);//用于烧录程序
usart2_init(42,115200);//用于摄像头
LED_Init();
LCD_Init();
KEY_Init();
TIM3_Int_Init(200-1,8400-1);
TIM14_PWM_Init(10000-1,168-1);
TIM11_PWM_Init(10000-1,336-1);
usmart_dev.init(84);
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString(30,50,200,16,16,"Explorer STM32F4");
LCD_ShowString(30,70,200,16,16,"OV2640 TEST");
LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
LCD_ShowString(30,110,200,16,16,"2014/5/14");
while(OV2640_Init())//初始化OV2640
{
LCD_ShowString(30,130,240,16,16,"OV2640 ERR");
delay_ms(200);
LCD_Fill(30,130,239,170,WHITE);
delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"OV2640 OK");
OV2640_RGB565_Mode(); //RGB565模式
My_DCMI_Init(); //DCMI配置
DCMI_DMA_Init((u32)rgb_buf,sizeof(rgb_buf)/4,DMA_MemoryDataSize_HalfWord,DMA_MemoryInc_Enable);//DCMI DMA配置
OV2640_OutSize_Set(240,240);
DCMI_Start(); //启动传输
while(1)
{
if(KEY_Scan(0)==WKUP_PRES)
key=!key;
LCD_SetCursor(0,0);
LCD_WriteRAM_Prepare();
for(i=0;i<240;i++)
{
for(j=0;j<240;j++)
{
if(j==239)
{
LCD_SetCursor(0,i+1);
LCD_WriteRAM_Prepare();
}
gray=((rgb_buf[i][j]>>11)*19595+((rgb_buf[i][j]>>5)&0x3f)*38469 +(rgb_buf[i][j]&0x1f)*7472)>>16; //灰度计算。公式请百度
if(gray>=22)
{
LCD->LCD_RAM=WHITE;
if(i==200&&j==45)
LCD->LCD_RAM=RED;
if(i==200&&j==170)
LCD->LCD_RAM=RED;
if(i==30&&j==45)
LCD->LCD_RAM=RED;
if(i==157&&j==138)
LCD->LCD_RAM=RED;
}
else
{
if(i==200&&j==45)
LCD->LCD_RAM=RED;
if(i==200&&j==170)
LCD->LCD_RAM=RED;
if(i==30&&j==45)
LCD->LCD_RAM=RED;
if(i==157&&j==138)//115 107
LCD->LCD_RAM=RED;
else if(i>30&&i<200&&j>45&&j<170)
{
if(i>X_MAX)
X_MAX=i;
if(i<X_MIN)
X_MIN=i;
if(j>Y_MAX)
Y_MAX=j;
if(j<Y_MIN)
Y_MIN=j;
}
LCD->LCD_RAM=BLACK;
}
}
}
X=(X_MAX+X_MIN)/2;
Y=(Y_MIN+Y_MAX)/2;
X_MAX=0;
X_MIN=240;
Y_MAX=0;
Y_MIN=240;
for(n=0;n<2;n++)
for(m=0;m<2;m++)
{
LCD_SetCursor(Y+m,X+n);
LCD_WriteRAM_Prepare();
LCD->LCD_RAM=RED;
}
LCD_ShowxNum(80,80,ERR_X,5,16,0X80);
LCD_ShowxNum(80,120,ERR_Y,5,16,0X80);
LCD_ShowxNum(80,170,hang,5,16,0X80);
}
}
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
{
KP=1.25;//0.75
KD=20;//17.5
KI=0.25;
ERR_X=X-AIM_X;
ERR_Y=Y-AIM_Y;
if(ERR_X<13&&ERR_Y<13)
{
KP=0.75;KD=17.5;
hang++;
if(AIM_X==107)
{
if(hang==500)
{
AIM_X++;
AIM_Y++;
hang=0;
}
}
if(AIM_X<160&&AIM_X>107)
{
if(hang==10)
{
AIM_X++;
AIM_Y++;
hang=0;
}
}
if(AIM_X==160)
{
AIM_X=107;
AIM_Y=115;
}
}
PWM_X=9080+ERR_X*KP+(ERR_X+ERR_X_LAST)*KI+(ERR_X-ERR_X_LAST)*KD;
PWM_Y=9105+ERR_Y*KP+(ERR_X+ERR_X_LAST)*KI+(ERR_Y-ERR_Y_LAST)*KD;
if(PWM_Y>9250)PWM_Y=9250;
if(PWM_Y<9000)PWM_Y=9000;
if(PWM_X>9400)PWM_X=9400;
if(PWM_X<8950)PWM_X=8950;
ERR_X_LAST=ERR_X;
ERR_Y_LAST=ERR_Y;
TIM_SetCompare1(TIM14,PWM_X);
TIM_SetCompare1(TIM11,PWM_Y);
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}

复制代码