火箭发射动画c语言程序设计,»ð¼ýµÄÔ´´úÂë

#include

#include

#include

#include

#include

#define ESC  0x1b            /*ESCµÄASCIIÂ룬ʮÁù½øÖÆÊý*/

void InstallGraph(void);    /*ͼÐγõʼ»¯º¯ÊýµÄÉùÃ÷*/

void DrawGun(void);         /*»­·¢Éä¼Üº¯ÊýµÄÉùÃ÷*/

void DrawPlan(void);        /*»­»ð¼ýº¯ÊýµÄÉùÃ÷*/

void DrawStar(void);        /*»­ÐǺ¯ÊýµÄÉùÃ÷*/

void *buf;                  /*È«¾Ö±äÁ¿£¬ÓÃÓÚ´æ´¢»ð¼ýµÄͼÏñ*/

int x0=300;                 /*ÒÔÏÂÊÇ»ð¼ýµÄλÖóõʼ»¯²ÎÊý*/

int y0=340;

int width1=5;

int width2=20;

int height=25;

int y1=8;

int y2=15;

int y3=23;

int y4=38;

main()

{

int size;

int i=0;

int key;

int station;

InstallGraph();

setbkcolor(BLACK);          /*ÉèÖñ³¾°ÑÕÉ«*/

cleardevice();

setcolor(BLUE);             /*ÉèÖÃ×ÖµÄÑÕÉ«*/

outtextxy(80,100,"Ready!...Any key to SET OFF !!!");

DrawGun();

getch();

cleardevice();

DrawGun();

DrawPlan();

DrawStar();

setcolor(LIGHTRED);

do{putimage(x0-width2,y0-height-3*width1-i,buf,XOR_PUT);/*Ö÷Ñ­»·¿ªÊ¼£¬ÓÃÓÚÒÆ¶¯»ð¼ý£¬·½·¨ÊÇÓÃÒì»ò·½Ê½ÔÚÆÁÄ»ÉÏÁ¬Ðø»­³ö»ð¼ýµÄͼÏñ£¬Ã¿»­Ò»´Î£¬ÐÂͼÏñºÍÔ­À´µÄλÖÃÏà²îÁ½¸öÏñËØµã¡£Õâ¸öÖµÔ½´ó£¬·ÉÐÐËÙ¶ÈÔ½¿ì*/

i+=2;

putimage(x0-width2,y0-height-3*width1-i,buf,XOR_PUT);

key=getch();

if(y0-height-3*width1-i==0)

outtextxy(60,100,"Successfully!!! Press ESC to quit");

if(key==ESC)

{

if(y0-height-3*width1-i>0){

cleardevice();

outtextxy(100,100,"Failue. What did you do?");

outtextxy(130,300,"Press any key to quit.");

getch();

}

}

}

while(key!=ESC);

free(buf);

closegraph();

return 0;       }

void InstallGraph(void)  /*ͼÐγõʼ»¯º¯Êý*/

{

int grdriver=DETECT;

int grmode;

int errorcode;

char *errormsg;

initgraph(&grdriver,&grmode," ");

errorcode=graphresult();

errormsg=grapherrormsg(errorcode);

if(errorcode!=grOk){

printf("Graphics error: %s\n",errormsg);

printf("Press any key to exit.\n");

getch();

exit(1);

}

}

void DrawStar(void)         /*»­ÐǺ¯Êý*/

{

int seed=2000;

int i,dotx,doty,height,width,color,maxcolor;

maxcolor=getmaxcolor();

width=getmaxx();

height=getmaxy();

srand(seed);

for(i=0;i<250;i++){

dotx=i+random(width-1);

doty=i+random(height-1);

color=random(maxcolor);

setcolor(color);

putpixel(dotx,doty,color);

circle(dotx+1,doty+1,1);

}

srand(seed);

}

void DrawGun(void)   /*»­·¢Éä¼Üº¯Êý*/

{

int x0=300;

int y0=430;

int height=45;

int rx=20;

int ry=5;

int rightx,righty,leftx,lefty;

int centery1=30;

rightx=righty=leftx=lefty=12;

setcolor(LIGHTGREEN);

ellipse(x0,y0,180,360,rx,ry);

ellipse(x0,y0-height,0,360,rx,ry);

line(x0+rx,y0,x0+rx,y0-height);

line(x0-rx,y0,x0-rx,y0-height);

moveto(x0+rx,y0);

lineto(x0+rx+rightx,y0+righty);

moveto(x0+rx+rightx,y0+righty);

lineto(x0+rx+rightx+10,y0+righty+10);

circle(x0+rx+rightx+10,y0+righty+10,4);

moveto(x0,y0+ry);

lineto(x0,y0+centery1);

moveto(x0,y0+centery1);

lineto(x0,y0+centery1+10);

circle(x0,y0+centery1+10,4);

moveto(x0-rx,y0);

lineto(x0-rx-leftx,y0+lefty);

moveto(x0-rx-leftx,y0+lefty);

lineto(x0-rx-leftx-10,y0+lefty+10);

circle(x0-rx-leftx-10,y0+lefty+10,4);

line(x0-rx-leftx,y0+lefty,x0,y0+centery1);

line(x0,y0+centery1,x0+rx+rightx,y0+righty);

}

void DrawPlan(void)  /*·¢Éä»ð¼ýº¯Êý*/

{

int size;

setcolor(LIGHTRED);

setfillstyle(1,BLUE);

ellipse(x0,y0-height,0,180,width1,3*width1);

moveto(x0+width1,y0);

lineto(x0+width1,y0-height);

moveto(x0+width1,y0);

lineto(x0+width2,y0+y2);

moveto(x0-width1,y0);

lineto(x0-width1,y0-height);

moveto(x0-width1,y0);

lineto(x0-width2,y0+y2);

moveto(x0+width2,y0+y3);

lineto(x0+width2,y0+y1);

moveto(x0+width2,y0+y3);

lineto(x0+width1,y0+y2);

moveto(x0-width2,y0+y3);

lineto(x0-width2,y0+y1);

moveto(x0-width2,y0+y3);

lineto(x0-width1,y0+y2);

moveto(x0,y0+y4);

lineto(x0+width1,y0+y2);

moveto(x0,y0+y4);

lineto(x0-width1,y0+y2);

setfillstyle(2,LIGHTRED);

floodfill(x0,y0,LIGHTRED);

size=imagesize(x0-width2-1,y0-height-3*width1,x0+width2+1,y0+y4);

buf=malloc(size);

if(!buf){printf("No enough memory!");

exit(0);}

getimage(x0-width2,y0-height-3*width1,x0+width2,y0+y4,buf);

}

ÉÏһƪ£º{ʵÀý}Ó²¼þ²âÊÔ ÈËÆø:6249

ÏÂһƪ£º{ʵÀý}´Ê·¨·ÖÎöÆ÷ ÈËÆø:6811

解释一下这段代码过程void CMP_ISR(void) interrupt 21 { u8 i; CMPCR1 &= ~0x40; // ÐèÈí¼þÇå³ýÖжϱê־λ if(XiaoCiCnt == 0) //Ïû´Åºó²Å¼ì²â¹ý0ʼþ, XiaoCiCnt=1:ÐèÒªÏû´Å, =2:ÕýÔÚÏû´Å, =0ÒѾ­Ïû´Å { T4T3M &= ~(1<<3); // Timer3Í£Ö¹ÔËÐÐ P27 = 0; if(B_Timer3_OverFlow) //Çл»Ê±¼ä¼ä¸ô(Timer3)ÓÐÒç³ö { B_Timer3_OverFlow = 0; PhaseTime = 8000; //»»Ïàʱ¼ä×î´ó8ms, 2212µç»ú12V¿Õת×î¸ßËÙ130usÇл»Ò»Ïà(200RPS 12000RPM), 480mA } else { i=T3H; PhaseTime = (((u16)i << 8) + T3L) >> 1; //µ¥Î»Îª1us if(PhaseTime >= 8000) PhaseTime = 8000; //»»Ïàʱ¼ä×î´ó8ms, 2212µç»ú12V¿Õת×î¸ßËÙ130usÇл»Ò»Ïà(200RPS 12000RPM), 480mA } T3H = 0; T3L = 0; T4T3M |= (1<<3); //Timer3¿ªÊ¼ÔËÐÐ PhaseTimeTmp[TimeIndex] = PhaseTime; //±£´æÒ»´Î»»Ïàʱ¼ä if(++TimeIndex >= 16) TimeIndex = 0; //ÀÛ¼Ó8´Î for(PhaseTime=0, i=0; i<16; i++) PhaseTime += PhaseTimeTmp[i]; //Çó16´Î»»Ïàʱ¼äÀÛ¼ÓºÍ PhaseTime = PhaseTime >> 5; //Çó16´Î»»Ïàʱ¼äµÄƽ¾ùÖµµÄÒ»°ë, ¼´30¶Èµç½Ç¶È if((PhaseTime >= 40) && (PhaseTime <= 1000)) TimeOut = 150; //¶Âת600ms³¬Ê± if( PhaseTime >= 60) PhaseTime -= 40; //ÐÞÕýÓÉÓÚÂ˲¨µçÈÝÒýÆðµÄÖͺóʱ¼ä else PhaseTime = 20; // PhaseTime = 20; //Ö»¸ø20us, ÔòÎÞÖͺóÐÞÕý, ÓÃÓÚ¼ì²âÂ˲¨µçÈÝÒýÆðµÄÖͺóʱ¼ä T4T3M &= ~(1<<7); //Timer4Í£Ö¹ÔËÐÐ PhaseTime = PhaseTime << 1; //2¸ö¼ÆÊý1us PhaseTime = 0 - PhaseTime; T4H = (u8)(PhaseTime >> 8); //×°ÔØ30¶È½ÇÑÓʱ T4L = (u8)PhaseTime; T4T3M |= (1<<7); //Timer4¿ªÊ¼ÔËÐÐ XiaoCiCnt = 1; //1:ÐèÒªÏû´Å, 2:ÕýÔÚÏû´Å, 0ÒѾ­Ïû´Å P27 = 1; } }
07-08
#include "dcmi_ov2640.h" #include "dcmi_ov2640_cfg.h" DCMI_HandleTypeDef hdcmi; // DCMI¾ä±ú DMA_HandleTypeDef DMA_Handle_dcmi; // DMA¾ä±ú volatile uint8_t DCMI_FrameState = 0; // DCMI״̬±êÖ¾£¬µ±Êý¾ÝÖ¡´«ÊäÍê³Éʱ£¬»á±» HAL_DCMI_FrameEventCallback() Öжϻص÷º¯ÊýÖà 1 volatile uint8_t OV2640_FPS ; // Ö¡ÂÊ /************************************************************************************************* * º¯ Êý Ãû: HAL_DCMI_MspInit * Èë¿Ú²ÎÊý: hdcmi - DCMI_HandleTypeDef¶¨ÒåµÄ±äÁ¿£¬¼´±íʾ¶¨ÒåµÄ DCMI ¾ä±ú * ·µ »Ø Öµ: ÎÞ * º¯Êý¹¦ÄÜ: ³õʼ»¯ DCMI Òý½Å * ˵ Ã÷: ÎÞ *************************************************************************************************/ void HAL_DCMI_MspInit(DCMI_HandleTypeDef* hdcmi) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(hdcmi->Instance==DCMI) { __HAL_RCC_DCMI_CLK_ENABLE(); // ʹÄÜ DCMI ÍâÉèʱÖÓ __HAL_RCC_GPIOE_CLK_ENABLE(); // ʹÄÜÏàÓ¦µÄGPIOʱÖÓ __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOG_CLK_ENABLE(); GPIO_OV2640_PWDN_CLK_ENABLE; // ʹÄÜPWDN Òý½ÅµÄ GPIO ʱÖÓ /**************************************************************************** Êý¾ÝÒý½Å ʱÖÓºÍͬ²½Òý½Å PH9 ------> DCMI_D0 PG9 ------> DCMI_VSYNC PH10 ------> DCMI_D1 PH8 ------> DCMI_HSYNC PH11 ------> DCMI_D2 PA6 ------> DCMI_PIXCLK PH12 ------> DCMI_D3 PH14 ------> DCMI_D4 SCCB ¿ØÖÆÒý½Å£¬³õʼ»¯ÔÚ camera_sccb.c Îļþ PD3 ------> DCMI_D5 PH7 ------> SCCB_SCL PE5 ------> DCMI_D6 PH13 ------> SCCB_SDA PE6 ------> DCMI_D7 µôµç¿ØÖÆÒý½Å PH15 ------> PWDN ******************************************************************************/ GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF13_DCMI; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_6; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF13_DCMI; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF13_DCMI; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_3; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF13_DCMI; HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Alternate = GPIO_AF13_DCMI; HAL_GPIO_Init(GPIOG, &GPIO_InitStruct); // ³õʼ»¯ PWDN Òý½Å OV2640_PWDN_ON; // ¸ßµçƽ£¬½øÈëµôµçģʽ£¬ÉãÏñÍ·Í£Ö¹¹¤×÷£¬´Ëʱ¹¦ºÄ½µµ½×îµÍ GPIO_InitStruct.Pin = OV2640_PWDN_PIN; // PWDN Òý½Å GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; // ÍÆÍìÊä³öģʽ GPIO_InitStruct.Pull = GPIO_PULLUP; // ÉÏÀ­ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; // Ëٶȵȼ¶µÍ HAL_GPIO_Init(OV2640_PWDN_PORT, &GPIO_InitStruct); // ³õʼ»¯ } } /*************************************************************************************************************************************** * º¯ Êý Ãû: MX_DCMI_Init * * º¯Êý¹¦ÄÜ: ÅäÖÃDCMIÏà¹Ø²ÎÊý * * ˵ Ã÷: 8λÊý¾Ýģʽ£¬È«Êý¾Ý¡¢È«Ö¡²¶×½£¬¿ªÆôÖÐ¶Ï * *****************************************************************************************************************************************/ void MX_DCMI_Init(void) { hdcmi.Instance = DCMI; hdcmi.Init.SynchroMode = DCMI_SYNCHRO_HARDWARE; // Ó²¼þͬ²½Ä£Ê½£¬¼´Ê¹ÓÃÍⲿµÄVS¡¢HSÐźŽøÐÐͬ²½ hdcmi.Init.PCKPolarity = DCMI_PCKPOLARITY_RISING; // ÏñËØÊ±ÖÓÉÏÉýÑØÓÐЧ hdcmi.Init.VSPolarity = DCMI_VSPOLARITY_LOW; // VSµÍµçƽÓÐЧ hdcmi.Init.HSPolarity = DCMI_HSPOLARITY_LOW; // HSµÍµçƽÓÐЧ hdcmi.Init.CaptureRate = DCMI_CR_ALL_FRAME; // ²¶»ñµÈ¼¶£¬ÉèÖÃÿһ֡¶¼½øÐв¶»ñ hdcmi.Init.ExtendedDataMode = DCMI_EXTEND_DATA_8B; // 8λÊý¾Ýģʽ hdcmi.Init.JPEGMode = DCMI_JPEG_DISABLE; // ²»Ê¹ÓÃDCMIµÄJPEGģʽ hdcmi.Init.ByteSelectMode = DCMI_BSM_ALL; // DCMI½Ó¿Ú²¶×½ËùÓÐÊý¾Ý hdcmi.Init.ByteSelectStart = DCMI_OEBS_ODD; // Ñ¡Ôñ¿ªÊ¼×Ö½Ú£¬´Ó Ö¡/ÐÐ µÄµÚÒ»¸öÊý¾Ý¿ªÊ¼²¶»ñ hdcmi.Init.LineSelectMode = DCMI_LSM_ALL; // ²¶»ñËùÓÐÐÐ hdcmi.Init.LineSelectStart = DCMI_OELS_ODD; // Ñ¡Ôñ¿ªÊ¼ÐÐ,ÔÚÖ¡¿ªÊ¼ºó²¶»ñµÚÒ»ÐÐ HAL_DCMI_Init(&hdcmi) ; HAL_NVIC_SetPriority(DCMI_IRQn, 0 ,5); // ÉèÖÃÖжÏÓÅÏȼ¶ HAL_NVIC_EnableIRQ(DCMI_IRQn); // ¿ªÆôDCMIÖÐ¶Ï //// ÔÚJPGģʽÏ£¬Ò»¶¨Òªµ¥¶ÀʹÄܸÃÖÐ¶Ï // __HAL_DCMI_ENABLE_IT(&hdcmi, DCMI_IT_FRAME); // ʹÄÜ FRAME ÖÐ¶Ï } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DMA_Init * * º¯Êý¹¦ÄÜ: ÅäÖà DMA Ïà¹Ø²ÎÊý * * ˵ Ã÷: ʹÓõÄÊÇDMA2£¬ÍâÉèµ½´æ´¢Æ÷ģʽ¡¢Êý¾Ýλ¿í32bit¡¢²¢¿ªÆôÖÐ¶Ï * *****************************************************************************************************************************************/ void OV2640_DMA_Init(void) { __HAL_RCC_DMA2_CLK_ENABLE(); // ʹÄÜDMA2ʱÖÓ DMA_Handle_dcmi.Instance = DMA2_Stream7; // DMA2Êý¾ÝÁ÷7 DMA_Handle_dcmi.Init.Request = DMA_REQUEST_DCMI; // DMAÇëÇóÀ´×ÔDCMI DMA_Handle_dcmi.Init.Direction = DMA_PERIPH_TO_MEMORY; // ÍâÉèµ½´æ´¢Æ÷ģʽ DMA_Handle_dcmi.Init.PeriphInc = DMA_PINC_DISABLE; // ÍâÉèµØÖ·½ûÖ¹×ÔÔö DMA_Handle_dcmi.Init.MemInc = DMA_MINC_ENABLE; // ´æ´¢Æ÷µØÖ·×ÔÔö DMA_Handle_dcmi.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; // DCMIÊý¾Ýλ¿í£¬32λ DMA_Handle_dcmi.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; // ´æ´¢Æ÷Êý¾Ýλ¿í£¬32λ DMA_Handle_dcmi.Init.Mode = DMA_CIRCULAR; // Ñ­»·Ä£Ê½ DMA_Handle_dcmi.Init.Priority = DMA_PRIORITY_LOW; // ÓÅÏȼ¶µÍ DMA_Handle_dcmi.Init.FIFOMode = DMA_FIFOMODE_ENABLE; // ʹÄÜfifo DMA_Handle_dcmi.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; // È«fifoģʽ£¬4*32bit´óС DMA_Handle_dcmi.Init.MemBurst = DMA_MBURST_SINGLE; // µ¥´Î´«Êä DMA_Handle_dcmi.Init.PeriphBurst = DMA_PBURST_SINGLE; // µ¥´Î´«Êä HAL_DMA_Init(&DMA_Handle_dcmi); // ÅäÖÃDMA __HAL_LINKDMA(&hdcmi, DMA_Handle, DMA_Handle_dcmi); // ¹ØÁªDCMI¾ä±ú HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 0, 0); // ÉèÖÃÖжÏÓÅÏȼ¶ HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn); // ʹÄÜÖÐ¶Ï } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Delay * Èë¿Ú²ÎÊý: Delay - ÑÓʱʱ¼ä£¬µ¥Î» ms * º¯Êý¹¦ÄÜ: ¼òµ¥ÑÓʱº¯Êý£¬²»ÊǺܾ«È· * ˵ Ã÷: ΪÁËÒÆÖ²µÄ¼ò±ãÐÔ,´Ë´¦²ÉÓÃÈí¼þÑÓʱ£¬Êµ¼ÊÏîÄ¿ÖпÉÒÔÌæ»»³ÉRTOSµÄÑÓʱ»òÕßHAL¿âµÄÑÓʱ *****************************************************************************************************************************************/ void OV2640_Delay(volatile uint32_t Delay) { volatile uint16_t i; while (Delay --) { for (i = 0; i < 20000; i++); } // HAL_Delay(Delay); // ¿ÉʹÓÃHAL¿âµÄÑÓʱ } /*************************************************************************************************************************************** * º¯ Êý Ãû: DCMI_OV2640_Init * * º¯Êý¹¦ÄÜ: ³õʼSCCB¡¢DCMI¡¢DMAÒÔ¼°ÅäÖÃOV2640 * *****************************************************************************************************************************************/ int8_t DCMI_OV2640_Init(void) { uint16_t Device_ID; // ¶¨Òå±äÁ¿´æ´¢Æ÷¼þID SCCB_GPIO_Config(); // SCCBÒý½Å³õʼ»¯ MX_DCMI_Init(); // ³õʼ»¯DCMIÅäÖÃÒý½Å OV2640_DMA_Init(); // ³õʼ»¯DMAÅäÖà OV2640_Reset(); // Ö´ÐÐÈí¼þ¸´Î» Device_ID = OV2640_ReadID(); // ¶ÁÈ¡Æ÷¼þID if( (Device_ID == 0x2640) || (Device_ID == 0x2642) ) // ½øÐÐÆ¥Å䣬ʵ¼ÊµÄÆ÷¼þID¿ÉÄÜÊÇ 0x2640 »òÕß 0x2642 { printf ("OV2640 OK,ID:0x%X\r\n",Device_ID); // Æ¥Åäͨ¹ý OV2640_Config( OV2640_SVGA_Config ); // ÅäÖà SVGAģʽ ------> 800*600£¬ ×î´óÖ¡ÂÊ30Ö¡ // OV2640_Config( OV2640_UXGA_Config ); // ÅäÖà UXGAģʽ ------> 1600*1200£¬×î´óÖ¡ÂÊ15Ö¡ OV2640_Set_Framesize(OV2640_Width,OV2640_Height); // ÉèÖÃOV2640Êä³öµÄͼÏñ´óС OV2640_DCMI_Crop(Display_Width, Display_Height, OV2640_Width, OV2640_Height ); // ½«OV2640Êä³öͼÏñ²Ã¼ô³ÉÊÊÓ¦ÆÁÄ»µÄ´óС return OV2640_Success; // ·µ»Ø³É¹¦±êÖ¾ } else { printf ("OV2640 ERROR!!!!! ID:%X\r\n",Device_ID); // ¶ÁÈ¡ID´íÎó return OV2640_Error; // ·µ»Ø´íÎó±êÖ¾ } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DMA_Transmit_Continuous * * Èë¿Ú²ÎÊý: DMA_Buffer - DMA½«Òª´«ÊäµÄµØÖ·£¬¼´ÓÃÓÚ´æ´¢ÉãÏñÍ·Êý¾ÝµÄ´æ´¢ÇøµØÖ· * DMA_BufferSize - ´«ÊäµÄÊý¾Ý´óС£¬32λ¿í * * º¯Êý¹¦ÄÜ: Æô¶¯DMA´«Ê䣬Á¬ÐøÄ£Ê½ * * ˵ Ã÷: 1. ¿ªÆôÁ¬ÐøÄ£Ê½Ö®ºó£¬»áÒ»Ö±½øÐд«Ê䣬³ý·Ç¹ÒÆð»òÕßÍ£Ö¹DCMI * 2. OV2640ʹÓÃRGB565ģʽʱ£¬1¸öÏñËØµãÐèÒª2¸ö×Ö½ÚÀ´´æ´¢ * 3. ÒòΪDMAÅäÖô«ÊäÊý¾ÝΪ32λ¿í£¬¼ÆËã DMA_BufferSize ʱ£¬ÐèÒª³ýÒÔ4£¬ÀýÈ磺 * Òª»ñÈ¡ 240*240·Ö±æÂÊ µÄͼÏñ£¬ÐèÒª´«Êä 240*240*2 = 115200 ×Ö½ÚµÄÊý¾Ý£¬ * Ôò DMA_BufferSize = 115200 / 4 = 28800 ¡£ *LXB *****************************************************************************************************************************************/ void OV2640_DMA_Transmit_Continuous(uint32_t DMA_Buffer,uint32_t DMA_BufferSize) { DMA_Handle_dcmi.Init.Mode = DMA_CIRCULAR; // Ñ­»·Ä£Ê½ HAL_DMA_Init(&DMA_Handle_dcmi); // ÅäÖÃDMA // ʹÄÜDCMI²É¼¯Êý¾Ý,Á¬Ðø²É¼¯Ä£Ê½ HAL_DCMI_Start_DMA(&hdcmi, DCMI_MODE_CONTINUOUS, (uint32_t)DMA_Buffer,DMA_BufferSize); } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DMA_Transmit_Snapshot * * Èë¿Ú²ÎÊý: DMA_Buffer - DMA½«Òª´«ÊäµÄµØÖ·£¬¼´ÓÃÓÚ´æ´¢ÉãÏñÍ·Êý¾ÝµÄ´æ´¢ÇøµØÖ· * DMA_BufferSize - ´«ÊäµÄÊý¾Ý´óС£¬32λ¿í * * º¯Êý¹¦ÄÜ: Æô¶¯DMA´«Ê䣬¿ìÕÕģʽ£¬´«Êäһ֡ͼÏñºóÍ£Ö¹ * * ˵ Ã÷: 1. ¿ìÕÕģʽ£¬Ö»´«ÊäÒ»Ö¡µÄÊý¾Ý * 2. OV2640ʹÓÃRGB565ģʽʱ£¬1¸öÏñËØµãÐèÒª2¸ö×Ö½ÚÀ´´æ´¢ * 3. ÒòΪDMAÅäÖô«ÊäÊý¾ÝΪ32λ¿í£¬¼ÆËã DMA_BufferSize ʱ£¬ÐèÒª³ýÒÔ4£¬ÀýÈ磺 * Òª»ñÈ¡ 240*240·Ö±æÂÊ µÄͼÏñ£¬ÐèÒª´«Êä 240*240*2 = 115200 ×Ö½ÚµÄÊý¾Ý£¬ * Ôò DMA_BufferSize = 115200 / 4 = 28800 ¡£ * 4. ʹÓøÃģʽ´«ÊäÍê³ÉÖ®ºó£¬DCMI»á±»¹ÒÆð£¬ÔÙ´ÎÆôÓô«Êä֮ǰ£¬ÐèÒªµ÷Óà OV2640_DCMI_Resume() »Ö¸´DCMI * *****************************************************************************************************************************************/ void OV2640_DMA_Transmit_Snapshot(uint32_t DMA_Buffer,uint32_t DMA_BufferSize) { DMA_Handle_dcmi.Init.Mode = DMA_NORMAL; // Õý³£Ä£Ê½ HAL_DMA_Init(&DMA_Handle_dcmi); // ÅäÖÃDMA HAL_DCMI_Start_DMA(&hdcmi, DCMI_MODE_SNAPSHOT, (uint32_t)DMA_Buffer,DMA_BufferSize); } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DCMI_Suspend * * º¯Êý¹¦ÄÜ: ¹ÒÆðDCMI£¬Í£Ö¹²¶»ñÊý¾Ý * * ˵ Ã÷: 1. ¿ªÆôÁ¬ÐøÄ£Ê½Ö®ºó£¬ÔÙµ÷Óøú¯Êý£¬»áÍ£Ö¹²¶»ñDCMIµÄÊý¾Ý * 2. ¿ÉÒÔµ÷Óà OV2640_DCMI_Resume() »Ö¸´DCMI * 3. ÐèҪעÒâµÄ£¬¹ÒÆðDCMIÆÚ¼ä£¬DMAÊÇûÓÐÍ£Ö¹¹¤×÷µÄ *LXB *****************************************************************************************************************************************/ void OV2640_DCMI_Suspend(void) { HAL_DCMI_Suspend(&hdcmi); // ¹ÒÆðDCMI } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DCMI_Resume * * º¯Êý¹¦ÄÜ: »Ö¸´DCMI£¬¿ªÊ¼²¶»ñÊý¾Ý * * ˵ Ã÷: 1. µ±DCMI±»¹ÒÆðʱ£¬¿ÉÒÔµ÷Óøú¯Êý»Ö¸´ * 2. ʹÓà OV2640_DMA_Transmit_Snapshot() ¿ìÕÕģʽ£¬´«ÊäÍê³ÉÖ®ºó£¬DCMIÒ²»á±»¹ÒÆð£¬ÔÙ´ÎÆôÓô«Êä֮ǰ£¬ * ÐèÒªµ÷Óñ¾º¯Êý»Ö¸´DCMI²¶»ñ * *****************************************************************************************************************************************/ void OV2640_DCMI_Resume(void) { (&hdcmi)->State = HAL_DCMI_STATE_BUSY; // ±ä¸üDCMI±êÖ¾ (&hdcmi)->Instance->CR |= DCMI_CR_CAPTURE; // ¿ªÆôDCMI²¶»ñ } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DCMI_Stop * * º¯Êý¹¦ÄÜ: ½ûÖ¹DCMIµÄDMAÇëÇó£¬Í£Ö¹DCMI²¶»ñ£¬½ûÖ¹DCMIÍâÉè * *****************************************************************************************************************************************/ void OV2640_DCMI_Stop(void) { HAL_DCMI_Stop(&hdcmi); } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_DCMI_Crop * * Èë¿Ú²ÎÊý: Displey_XSize ¡¢Displey_YSize - ÏÔʾÆ÷µÄ³¤¿í * Sensor_XSize¡¢Sensor_YSize - ÉãÏñÍ·´«¸ÐÆ÷Êä³öͼÏñµÄ³¤¿í * * º¯Êý¹¦ÄÜ: ʹÓÃDCMIµÄ²Ã¼ô¹¦ÄÜ£¬½«´«¸ÐÆ÷Êä³öµÄͼÏñ²Ã¼ô³ÉÊÊÓ¦ÆÁÄ»µÄ´óС * * ˵ Ã÷: 1. ÒòΪÉãÏñÍ·Êä³öµÄ»­Ãæ±ÈÀý¹Ì¶¨Îª4:3£¬²»Ò»¶¨Æ¥ÅäÏÔʾÆ÷ * 2. ÐèҪעÒâµÄÊÇ£¬ÉãÏñÍ·Êä³öµÄͼÏñ³¤¡¢¿í±ØÐëÒªÄܱ»4Õû³ý£¡£¨ ʹÓÃOV2640_Set_Framesizeº¯Êý½øÐÐÉèÖà £© * 3. DCMIµÄˮƽÓÐЧÏñËØÒ²±ØÐëÒªÄܱ»4Õû³ý£¡ * 4. º¯Êý»á¼ÆËãˮƽºÍ´¹Ö±Æ«ÒÆ£¬¾¡Á¿Èû­Ãæ¾ÓÖвüô *****************************************************************************************************************************************/ int8_t OV2640_DCMI_Crop(uint16_t Displey_XSize,uint16_t Displey_YSize,uint16_t Sensor_XSize,uint16_t Sensor_YSize ) { uint16_t DCMI_X_Offset,DCMI_Y_Offset; // ˮƽºÍ´¹Ö±Æ«ÒÆ£¬´¹Ö±´ú±íµÄÊÇÐÐÊý£¬Ë®Æ½´ú±íµÄÊÇÏñËØÊ±ÖÓÊý£¨PCLKÖÜÆÚÊý£© uint16_t DCMI_CAPCNT; // ˮƽÓÐЧÏñËØ£¬´ú±íµÄÊÇÏñËØÊ±ÖÓÊý£¨PCLKÖÜÆÚÊý£© uint16_t DCMI_VLINE; // ´¹Ö±ÓÐЧÐÐÊý if( (Displey_XSize>=Sensor_XSize)|| (Displey_YSize>=Sensor_YSize) ) { // printf("ʵ¼ÊÏÔʾµÄ³ß´ç´óÓÚ»òµÈÓÚÉãÏñÍ·Êä³öµÄ³ß´ç£¬Í˳öDCMI²Ã¼ô\r\n"); return OV2640_Error; //Èç¹ûʵ¼ÊÏÔʾµÄ³ß´ç´óÓÚ»òµÈÓÚÉãÏñÍ·Êä³öµÄ³ß´ç£¬ÔòÍ˳öµ±Ç°º¯Êý£¬²»½øÐвüô } // ÔÚÉèÖÃΪRGB565¸ñʽʱ£¬Ë®Æ½Æ«ÒÆ£¬±ØÐëÊÇÆæÊý£¬·ñÔò»­ÃæÉ«²Ê²»ÕýÈ·£¬ // ÒòΪһ¸öÓÐЧÏñËØÊÇ2¸ö×Ö½Ú£¬ÐèÒª2¸öPCLKÖÜÆÚ£¬ËùÒÔ±ØÐë´ÓÆæÊýλ¿ªÊ¼£¬²»È»Êý¾Ý»á´íÂÒ£¬ // ÐèҪעÒâµÄÊÇ£¬¼Ä´æÆ÷ÖµÊÇ´Ó0¿ªÊ¼ËãÆðµÄ £¡ DCMI_X_Offset = Sensor_XSize - Displey_XSize; // ʵ¼Ê¼ÆËã¹ý³ÌΪ£¨Sensor_XSize - LCD_XSize£©/2*2 // ¼ÆËã´¹Ö±Æ«ÒÆ£¬¾¡Á¿Èû­Ãæ¾ÓÖвüô£¬¸ÃÖµ´ú±íµÄÊÇÐÐÊý£¬ DCMI_Y_Offset = (Sensor_YSize - Displey_YSize)/2-1; // ¼Ä´æÆ÷ÖµÊÇ´Ó0¿ªÊ¼ËãÆðµÄ£¬ËùÒÔÒª-1 // ÒòΪһ¸öÓÐЧÏñËØÊÇ2¸ö×Ö½Ú£¬ÐèÒª2¸öPCLKÖÜÆÚ£¬ËùÒÔÒª³Ë2 // ×îÖյõ½µÄ¼Ä´æÆ÷Öµ£¬±ØÐëÒªÄܱ»4Õû³ý£¡ DCMI_CAPCNT = Displey_XSize*2-1; // ¼Ä´æÆ÷ÖµÊÇ´Ó0¿ªÊ¼ËãÆðµÄ£¬ËùÒÔÒª-1 DCMI_VLINE = Displey_YSize-1; // ´¹Ö±ÓÐЧÐÐÊý // printf("%d %d %d %d\r\n",DCMI_X_Offset,DCMI_Y_Offset,DCMI_CAPCNT,DCMI_VLINE); HAL_DCMI_ConfigCrop (&hdcmi,DCMI_X_Offset,DCMI_Y_Offset,DCMI_CAPCNT,DCMI_VLINE);// ÉèÖòüô´°¿Ú HAL_DCMI_EnableCrop(&hdcmi); // ʹÄܲüô return OV2640_Success; } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Reset * * º¯Êý¹¦ÄÜ: Ö´ÐÐÈí¼þ¸´Î» * * ˵ Ã÷: ÔÚÅäÖÃOV2640֮ǰ£¬ÐèÒªÖ´ÐÐÒ»´ÎÈí¼þ¸´Î» * *****************************************************************************************************************************************/ void OV2640_Reset(void) { OV2640_Delay(5); // µÈ´ýÄ£¿éÉϵçÎȶ¨£¬×îÉÙ5ms£¬È»ºóÀ­µÍPWDN OV2640_PWDN_OFF; // PWDN Òý½ÅÊä³öµÍµçƽ£¬²»¿ªÆôµôµçģʽ£¬ÉãÏñÍ·Õý³£¹¤×÷£¬´ËʱÉãÏñÍ·Ä£¿éµÄ°×É«LED»áµãÁÁ // ¸ù¾ÝOV2640µÄÉϵçʱÐò£¬Ó²¼þ¸´Î»µÄ³ÖÐøÊ±¼äÒª>=3ms£¬Â¹Ð¡°àµÄOV2640²ÉÓÃÓ²¼þRC¸´Î»£¬³ÖÐøÊ±¼ä´ó¸ÅÔÚ6ms×óÓÒ // Òò´Ë¼ÓÈëÑÓʱ£¬µÈ´ýÓ²¼þ¸´Î»Íê³É²¢Îȶ¨ÏÂÀ´ OV2640_Delay(5); SCCB_WriteReg( OV2640_SEL_Registers, OV2640_SEL_SENSOR); // Ñ¡Ôñ SENSOR ¼Ä´æÆ÷×é SCCB_WriteReg( OV2640_SENSOR_COM7, 0x80); // Æô¶¯Èí¼þ¸´Î» // ¸ù¾ÝOV2640µÄÈí¼þ¸´Î»Ê±Ðò£¬Èí¼þ¸´Î»Ö´Ðкó£¬Òª>=2ms·½¿ÉÖ´ÐÐSCCBÅäÖ㬴˴¦²ÉÓñ£ÊØÒ»µãµÄ²ÎÊý£¬ÑÓʱ10ms OV2640_Delay(10); } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_ReadID * * º¯Êý¹¦ÄÜ: ¶ÁÈ¡ OV2640 µÄÆ÷¼þID * * ˵ Ã÷: ʵ¼ÊµÄÆ÷¼þID¿ÉÄÜÊÇ 0x2640 »òÕß 0x2642£¬Åú´Î²»Í¬ID¿ÉÄܻ᲻һÑù * *****************************************************************************************************************************************/ uint16_t OV2640_ReadID(void) { uint8_t PID_H,PID_L; // ID±äÁ¿ SCCB_WriteReg( OV2640_SEL_Registers, OV2640_SEL_SENSOR); // Ñ¡Ôñ SENSOR ¼Ä´æÆ÷×é PID_H = SCCB_ReadReg(OV2640_SENSOR_PIDH); // ¶ÁÈ¡ID¸ß×Ö½Ú PID_L = SCCB_ReadReg(OV2640_SENSOR_PIDL); // ¶ÁÈ¡IDµÍ×Ö½Ú return(PID_H<<8)|PID_L; // ·µ»ØÍêÕûµÄÆ÷¼þID } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Config * * Èë¿Ú²ÎÊý: (*ConfigData)[2] - ÒªÅäÖõIJÎÊý£¬¿ÉÅäÖÃΪ OV2640_SVGA_Config »ò OV2640_UXGA_Config * * º¯Êý¹¦ÄÜ: ÅäÖà OV2640 ´«¸ÐÆ÷ºÍDSP²ÎÊý * * ˵ Ã÷: 1. ¿ÉÅäÖÃΪ SVGA »òÕß UXGAģʽ * 2. SVGA ·Ö±æÂÊΪ800*600£¬×î¸ßÖ§³Ö30Ö¡ * 3. UXGA ·Ö±æÂÊΪ1600*1200£¬×î¸ßÖ§³Ö15Ö¡ * 4. ²ÎÊý¶¨ÒåÔÚ dcmi_ov2640_cfg.h * *****************************************************************************************************************************************/ void OV2640_Config( const uint8_t (*ConfigData)[2] ) { uint32_t i; // ¼ÆÊý±äÁ¿ for( i=0; ConfigData[i][0]; i++) { SCCB_WriteReg( ConfigData[i][0], ConfigData[i][1]); // ½øÐвÎÊýÅäÖà } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Framesize * * Èë¿Ú²ÎÊý: pixformat - ÏñËØ¸ñʽ£¬¿ÉÑ¡Ôñ Pixformat_RGB565¡¢Pixformat_JPEG * * º¯Êý¹¦ÄÜ: ÉèÖÃÊä³öµÄÏñËØ¸ñʽ * *****************************************************************************************************************************************/ void OV2640_Set_Pixformat(uint8_t pixformat) { const uint8_t (*ConfigData)[2]; uint32_t i; // ¼ÆÊý±äÁ¿ switch (pixformat) { case Pixformat_RGB565: ConfigData = OV2640_RGB565_Config; break; case Pixformat_JPEG: ConfigData = OV2640_JPEG_Config; break; default: break; } for( i=0; ConfigData[i][0]; i++) { SCCB_WriteReg( ConfigData[i][0], ConfigData[i][1]); // ½øÐвÎÊýÅäÖà } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Framesize * * Èë¿Ú²ÎÊý: width - ʵ¼ÊÊä³öͼÏñµÄ³¤¶È£¬height - ʵ¼ÊÊä³öͼÏñµÄ¿í¶È * * º¯Êý¹¦ÄÜ: ÉèÖÃʵ¼ÊÊä³öµÄͼÏñ´óС * * ˵ Ã÷: 1. OV2640ÉèÖÃΪ SVGA£¨800*600£© »òÕß UXGA£¨1600*1200£©Ä£Ê½£¬Í¼Ïñ´óСͨ³£Óëʵ¼ÊÓÃµÄÆÁÄ»·Ö±æÂʲ»Ò»Ñù£¬ * Òò´Ë¿ÉÒÔµ÷Óôκ¯Êý£¬ÉèÖÃʵ¼ÊÊä³öµÄͼÏñ´óС * 2. ÐèҪעÒâµÄÊÇ£¬ÒªÉèÖõÄͼÏñ³¤¡¢¿í±ØÐëÄܱ»4Õû³ý£¡ * 3. ²¢²»ÊÇÉèÖÃÊä³öµÄͼÏñ·Ö±æÂÊԽС֡ÂʾÍÔ½¸ß£¬Ö¡ÂÊÖ»ºÍÅäÖõÄģʽÓйأ¬ÀýÈçÅäÖÃΪSVGA×î¸ßÖ»ÄÜÖ§³Ö30Ö¡ * *****************************************************************************************************************************************/ int8_t OV2640_Set_Framesize(uint16_t width,uint16_t height) { if( (width%4)||(height%4) ) // Êä³öͼÏñµÄ´óСһ¶¨ÒªÄܱ»4Õû³ý { return OV2640_Error; // ·µ»Ø´íÎó±êÖ¾ } SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_DSP); // Ñ¡Ôñ DSP¼Ä´æÆ÷×é SCCB_WriteReg(0X5A, width/4 &0XFF); // ʵ¼ÊͼÏñÊä³öµÄ¿í¶È£¨OUTW£©£¬7~0 bit£¬¼Ä´æÆ÷µÄÖµµÈÓÚʵ¼ÊÖµ/4 SCCB_WriteReg(0X5B, height/4 &0XFF); // ʵ¼ÊͼÏñÊä³öµÄ¸ß¶È£¨OUTH£©£¬7~0 bit£¬¼Ä´æÆ÷µÄÖµµÈÓÚʵ¼ÊÖµ/4 SCCB_WriteReg(0X5C, (width/4>>8&0X03)|(height/4>>6&0x04) ); // ÉèÖÃZMHHµÄBit[2:0]£¬Ò²¾ÍÊÇOUTH µÄµÚ 8 bit£¬OUTW µÄµÚ 9~8 bit£¬ SCCB_WriteReg(OV2640_DSP_RESET,0X00); // ¸´Î» return OV2640_Success; // ³É¹¦ } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Horizontal_Mirror * * Èë¿Ú²ÎÊý: ConfigState - ÖÃ1ʱ£¬Í¼Ïñ»áˮƽ¾µÏñ£¬ÖÃ0ʱ»Ö¸´Õý³£ * * º¯Êý¹¦ÄÜ: ÓÃÓÚÉèÖÃÊä³öµÄͼÏñÊÇ·ñ½øÐÐˮƽ¾µÏñ * *****************************************************************************************************************************************/ int8_t OV2640_Set_Horizontal_Mirror( int8_t ConfigState ) { uint8_t OV2640_Reg; // ¼Ä´æÆ÷µÄÖµ SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_SENSOR); // Ñ¡Ôñ SENSOR ¼Ä´æÆ÷×é OV2640_Reg = SCCB_ReadReg(OV2640_SENSOR_REG04); // ¶ÁÈ¡ 0x04 µÄ¼Ä´æÆ÷Öµ // REG04,¼Ä´æÆ÷×é4£¬¼Ä´æÆ÷µØÖ·Îª 0x04£¬¸Ã¼Ä´æÆ÷µÄBit[7]£¬ÓÃÓÚÉèÖÃˮƽÊÇ·ñ¾µÏñ if ( ConfigState == OV2640_Enable ) // Èç¹ûʹÄܾµÏñ { OV2640_Reg |= 0X80; // Bit[7]ÖÃ1Ôò¾µÏñ } else // È¡Ïû¾µÏñ { OV2640_Reg &= ~0X80; // Bit[7]ÖÃ0ÔòÊÇÕý³£Ä£Ê½ } return SCCB_WriteReg(OV2640_SENSOR_REG04,OV2640_Reg); // дÈë¼Ä´æÆ÷ } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Vertical_Flip * * Èë¿Ú²ÎÊý: ConfigState - ÖÃ1ʱ£¬Í¼Ïñ»á´¹Ö±·­×ª£¬ÖÃ0ʱ»Ö¸´Õý³£ * * º¯Êý¹¦ÄÜ: ÓÃÓÚÉèÖÃÊä³öµÄͼÏñÊÇ·ñ½øÐд¹Ö±·­×ª * *****************************************************************************************************************************************/ int8_t OV2640_Set_Vertical_Flip( int8_t ConfigState ) { uint8_t OV2640_Reg; // ¼Ä´æÆ÷µÄÖµ SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_SENSOR); // Ñ¡Ôñ SENSOR ¼Ä´æÆ÷×é OV2640_Reg = SCCB_ReadReg(OV2640_SENSOR_REG04); // ¶ÁÈ¡ 0x04 µÄ¼Ä´æÆ÷Öµ // REG04,¼Ä´æÆ÷×é4£¬¼Ä´æÆ÷µØÖ·Îª 0x04£¬¸Ã¼Ä´æÆ÷µÄµÚBit[6]£¬ÓÃÓÚÉèÖÃˮƽÊÇ´¹Ö±·­×ª if ( ConfigState == OV2640_Enable ) { // ´Ë´¦ÉèÖòο¼OpenMVµÄÇý¶¯ // Bit[4]¾ßÌåµÄ×÷ÓÃÊÇʲôÊÖ²áûÓÐ˵£¬Èç¹û´¹Ö±·­×ªÖ®ºó£¬¸Ãλ²»ÖÃ1µÄ»°£¬ÑÕÉ«»á²»¶Ô OV2640_Reg |= 0X40|0x10 ; // Bit[6]ÖÃ1ʱ£¬Í¼Ïñ»á´¹Ö±·­×ª } else // È¡Ïû·­×ª { OV2640_Reg &= ~(0X40|0x10 ); // ½«Bit[6]ºÍBit[4]¶¼Ð´0 } return SCCB_WriteReg(OV2640_SENSOR_REG04,OV2640_Reg); // дÈë¼Ä´æÆ÷ } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Saturation * * Èë¿Ú²ÎÊý: Saturation - ±¥ºÍ¶È£¬¿ÉÉèÖÃΪ5¸öµÈ¼¶£º2£¬1£¬0£¬-1£¬-2 * * ˵ Ã÷: 1. ÊÖ²áÀïûÓÐ˵Ã÷ÅäÖÃÖеÄÄÇ2¸ö¼Ä´æÆ÷ÈçºÎʹÓã¬Òò´ËÕâÀïÖ±½ÓʹÓÃOV2640±à³ÌÊÖ²á¸ø³öµÄ´úÂë * 2.±¥ºÍ¶ÈÔ½¸ß£¬É«²Ê¾ÍÔ½ÏÊÑÞ£¬µ«µ±ÏàÓ¦µÄÇåÎú¶È»áϽµ£¬Ôëµã±ä¶à * *****************************************************************************************************************************************/ void OV2640_Set_Saturation(int8_t Saturation) { SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_DSP); // Ñ¡Ôñ DSP¼Ä´æÆ÷×é switch (Saturation) { case 2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x02); SCCB_WriteReg(OV2640_DSP_BPADDR,0x03); SCCB_WriteReg(OV2640_DSP_BPDATA,0x68); SCCB_WriteReg(OV2640_DSP_BPDATA,0x68); break; case 1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x02); SCCB_WriteReg(OV2640_DSP_BPADDR,0x03); SCCB_WriteReg(OV2640_DSP_BPDATA,0x58); SCCB_WriteReg(OV2640_DSP_BPDATA,0x58); break; case 0: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x02); SCCB_WriteReg(OV2640_DSP_BPADDR,0x03); SCCB_WriteReg(OV2640_DSP_BPDATA,0x48); SCCB_WriteReg(OV2640_DSP_BPDATA,0x48); break; case -1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x02); SCCB_WriteReg(OV2640_DSP_BPADDR,0x03); SCCB_WriteReg(OV2640_DSP_BPDATA,0x38); SCCB_WriteReg(OV2640_DSP_BPDATA,0x38); break; case -2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x02); SCCB_WriteReg(OV2640_DSP_BPADDR,0x03); SCCB_WriteReg(OV2640_DSP_BPDATA,0x28); SCCB_WriteReg(OV2640_DSP_BPDATA,0x28); break; default: break; } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Brightness * * Èë¿Ú²ÎÊý: Brightness - ÁÁ¶È£¬¿ÉÉèÖÃΪ5¸öµÈ¼¶£º2£¬1£¬0£¬-1£¬-2 * * ˵ Ã÷: 1. ÊÖ²áÀïûÓÐ˵Ã÷ÅäÖÃÖеÄÄÇ2¸ö¼Ä´æÆ÷ÈçºÎʹÓã¬Òò´ËÕâÀïÖ±½ÓʹÓÃOV2640±à³ÌÊÖ²á¸ø³öµÄ´úÂë * 2. ÁÁ¶ÈÔ½¸ß£¬»­Ãæ¾ÍÔ½Ã÷ÁÁ£¬µ«ÊÇ»á±äÄ£ºýһЩ * *****************************************************************************************************************************************/ void OV2640_Set_Brightness(int8_t Brightness) { SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_DSP); // Ñ¡Ôñ DSP¼Ä´æÆ÷×é switch (Brightness) { case 2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x09); SCCB_WriteReg(OV2640_DSP_BPDATA,0x40); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); break; case 1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x09); SCCB_WriteReg(OV2640_DSP_BPDATA,0x30); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); break; case 0: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x09); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); break; case -1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x09); SCCB_WriteReg(OV2640_DSP_BPDATA,0x10); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); break; case -2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x09); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); break; default: break; } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Contrast * * Èë¿Ú²ÎÊý: Contrast - ¶Ô±È¶È£¬¿ÉÉèÖÃΪ5¸öµÈ¼¶£º2£¬1£¬0£¬-1£¬-2 * * ˵ Ã÷: 1. ÊÖ²áÀïûÓÐ˵Ã÷ÅäÖÃÖеÄÄÇ2¸ö¼Ä´æÆ÷ÈçºÎʹÓã¬Òò´ËÕâÀïÖ±½ÓʹÓÃOV2640±à³ÌÊÖ²á¸ø³öµÄ´úÂë * 2. ¶Ô±È¶ÈÔ½¸ß£¬»­ÃæÔ½ÇåÎú£¬ºÚ°×Ô½¼Ó·ÖÃ÷ * *****************************************************************************************************************************************/ void OV2640_Set_Contrast(int8_t Contrast) { SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_DSP); // Ñ¡Ôñ DSP¼Ä´æÆ÷×é switch (Contrast) { case 2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x07); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPADDR,0x28); SCCB_WriteReg(OV2640_DSP_BPDATA,0x0c); SCCB_WriteReg(OV2640_DSP_BPDATA,0x06); break; case 1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x07); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPADDR,0x24); SCCB_WriteReg(OV2640_DSP_BPDATA,0x16); SCCB_WriteReg(OV2640_DSP_BPDATA,0x06); break; case 0: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x07); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPADDR,0x20); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPDATA,0x06); break; case -1: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x07); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPADDR,0x1c); SCCB_WriteReg(OV2640_DSP_BPDATA,0x2a); SCCB_WriteReg(OV2640_DSP_BPDATA,0x06); break; case -2: SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x04); SCCB_WriteReg(OV2640_DSP_BPADDR,0x07); SCCB_WriteReg(OV2640_DSP_BPDATA,0x20); SCCB_WriteReg(OV2640_DSP_BPADDR,0x18); SCCB_WriteReg(OV2640_DSP_BPDATA,0x34); SCCB_WriteReg(OV2640_DSP_BPDATA,0x06); break; default: break; } } /*************************************************************************************************************************************** * º¯ Êý Ãû: OV2640_Set_Effect * * Èë¿Ú²ÎÊý: effect_Mode - ÌØÐ§Ä£Ê½£¬¿ÉÑ¡Ôñ²ÎÊý OV2640_Effect_Normal¡¢OV2640_Effect_Negative¡¢ * OV2640_Effect_BW¡¢OV2640_Effect_BW_Negative * * º¯Êý¹¦ÄÜ: ÓÃÓÚÉèÖÃOV2640µÄÌØÐ§£¬Õý³£¡¢¸ºÆ¬¡¢ºÚ°×¡¢ºÚ°×+¸ºÆ¬µÈģʽ * * ˵ Ã÷: ÊÖ²áÀïûÓÐ˵Ã÷ÅäÖÃÖеÄÄÇ2¸ö¼Ä´æÆ÷ÈçºÎʹÓã¬Òò´ËÕâÀïÖ±½ÓʹÓÃOV2640±à³ÌÊÖ²á¸ø³öµÄ´úÂë * *****************************************************************************************************************************************/ void OV2640_Set_Effect(uint8_t effect_Mode) { SCCB_WriteReg(OV2640_SEL_Registers,OV2640_SEL_DSP); // Ñ¡Ôñ DSP¼Ä´æÆ÷×é switch (effect_Mode) { case OV2640_Effect_Normal: // Õý³£Ä£Ê½ SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x00); SCCB_WriteReg(OV2640_DSP_BPADDR,0x05); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); break; case OV2640_Effect_Negative: // ¸ºÆ¬Ä£Ê½£¬Ò²¾ÍÊÇÑÕɫȫ²¿È¡·´ SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x40); SCCB_WriteReg(OV2640_DSP_BPADDR,0x05); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); break; case OV2640_Effect_BW: // ºÚ°×ģʽ SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x18); SCCB_WriteReg(OV2640_DSP_BPADDR,0x05); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); break; case OV2640_Effect_BW_Negative: // ºÚ°×+¸ºÆ¬Ä£Ê½ SCCB_WriteReg(OV2640_DSP_BPADDR,0x00); SCCB_WriteReg(OV2640_DSP_BPDATA,0x58); SCCB_WriteReg(OV2640_DSP_BPADDR,0x05); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); SCCB_WriteReg(OV2640_DSP_BPDATA,0x80); break; default: break; } } /*************************************************************************************************************************************** * º¯ Êý Ãû: HAL_DCMI_FrameEventCallback * * º¯Êý¹¦ÄÜ: Ö¡»Øµ÷º¯Êý£¬Ã¿´«ÊäÒ»Ö¡Êý¾Ý£¬»á½øÈë¸ÃÖжϷþÎñº¯Êý * * ˵ Ã÷: ÿ´Î´«ÊäÍêÒ»Ö¡£¬¶ÔÏàÓ¦µÄ±ê־λ½øÐвÙ×÷£¬²¢¼ÆËãÖ¡ÂÊ *****************************************************************************************************************************************/ void HAL_DCMI_FrameEventCallback(DCMI_HandleTypeDef *hdcmi) { static uint32_t DCMI_Tick = 0; // ÓÃÓÚ±£´æµ±Ç°µÄʱ¼ä¼ÆÊýÖµ static uint8_t DCMI_Frame_Count = 0; // Ö¡Êý¼ÆÊý if(HAL_GetTick() - DCMI_Tick >= 1000) // ÿ¸ô 1s ¼ÆËãÒ»´ÎÖ¡ÂÊ { DCMI_Tick = HAL_GetTick(); // ÖØÐ»ñÈ¡µ±Ç°Ê±¼ä¼ÆÊýÖµ OV2640_FPS = DCMI_Frame_Count; // »ñµÃfps DCMI_Frame_Count = 0; // ¼ÆÊýÇå0 } DCMI_Frame_Count ++; // û½øÈëÒ»´ÎÖжϣ¨Ã¿´Î´«ÊäÍêÒ»Ö¡Êý¾Ý£©£¬¼ÆÊýÖµ+1 DCMI_FrameState = 1; // ´«ÊäÍê³É±ê־λÖÃ1 } /*************************************************************************************************************************************** * º¯ Êý Ãû: HAL_DCMI_ErrorCallback * * º¯Êý¹¦ÄÜ: ´íÎ󻨵÷º¯Êý * * ˵ Ã÷: µ±·¢ÉúDMA´«Êä´íÎó»òÕßFIFOÒç³ö´íÎó¾Í»á½øÈë *****************************************************************************************************************************************/ void HAL_DCMI_ErrorCallback(DCMI_HandleTypeDef *hdcmi) { // if( HAL_DCMI_GetError(hdcmi) == HAL_DCMI_ERROR_OVR) // { // printf("FIFOÒç³ö´íÎ󣡣¡£¡\r\n"); // } printf("error:0x%x£¡£¡£¡\r\n",HAL_DCMI_GetError(hdcmi)); } /*********************************************************************************************************************************************************************************************************************************************LXB*************/ // ¹С°à 对代码进行分析
05-12
#include "control.h" #include "Lidar.h" float Move_X = 0, Move_Z = 0; // Ä¿±êËٶȺÍÄ¿±êתÏòËÙ¶È float PWM_Left, PWM_Right; // ×óÓÒµç»úPWMÖµ float RC_Velocity, RC_Turn_Velocity; // Ò£¿Ø¿ØÖƵÄËÙ¶È u8 Mode = Normal_Mode; // ģʽѡÔñ£¬Ä¬ÈÏÊÇÆÕͨµÄ¿ØÖÆÄ£Ê½ Motor_parameter MotorA, MotorB; // ×óÓÒµç»úÏà¹Ø±äÁ¿ int Servo_PWM = SERVO_INIT; // °¢¿ËÂü¶æ»úÏà¹Ø±äÁ¿ u8 Lidar_Detect = Lidar_Detect_ON; // µç´ÅѲÏßģʽÀ×´ï¼ì²âÕϰ­ÎĬÈÏ¿ªÆô float CCD_Move_X = 0.3; // CCDѲÏßËÙ¶È float ELE_Move_X = 0.3; // µç´ÅѲÏßËÙ¶È u8 Ros_count = 0, Lidar_flag_count = 0; u8 cnt = 0; Encoder OriginalEncoder; // Encoder raw data //±àÂëÆ÷ԭʼÊý¾Ý short Accel_Y, Accel_Z, Accel_X, Accel_Angle_x, Accel_Angle_y, Gyro_X, Gyro_Z, Gyro_Y; int forward_cnt = 800; int left_cnt = 120; int right_cnt = 125; int stop_cnt = 200; int stop_flag = 0; int mode_cnt = 0; int state_cnt = 0; int stop_protect = 0; /************************************************************************** Function: Control Function Input : none Output : none º¯Êý¹¦ÄÜ£º5ms¶¨Ê±ÖжϿØÖƺ¯Êý Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void forward(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 2040; } void left(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 2050; } void right(){ MotorA.Motor_Pwm = 1999; MotorB.Motor_Pwm = 0; } void stop(){ MotorA.Motor_Pwm = 0; MotorB.Motor_Pwm = 0; } void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim == &htim5) { Get_KeyVal(); if(mode_cnt == 0) stop(); //ģʽһ if(mode_cnt == 1){ if(forward_cnt > 0){ forward(); forward_cnt--; // if(left_cnt > 0){ // left(); // left_cnt--; // if(right_cnt > 0){ // right(); // right_cnt--; } else{ stop(); } } //ģʽ¶þ else if(mode_cnt == 2){ if(forward_cnt > 0){ forward(); forward_cnt--; } else if(stop_cnt > 0 && forward_cnt == 0){ stop(); stop_cnt--; } else if(stop_cnt == 0 && stop_flag == 0){ forward_cnt = 800; stop_flag = 1; } else{ stop(); } } //ģʽÈý else if(mode_cnt == 3){ if(forward_cnt > 0 && state_cnt == 0){//µÚ1״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(right_cnt > 0 && state_cnt == 1){//µÚ2״̬£ºÓÒת right(); right_cnt--; } else if(forward_cnt > 0 && state_cnt == 2){//µÚ3״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 3){//µÚ4״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 4){//µÚ5״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else if(left_cnt > 0 && state_cnt == 5){//µÚ6״̬£º×óת left(); left_cnt--; } else if(forward_cnt > 0 && state_cnt == 6){//µÚ7״̬£ºÖ±ÐÐ forward(); forward_cnt--; } else{ if(stop_protect == 0 && stop_cnt == 0){ stop_cnt = 100; stop_protect = 1; } else if(stop_cnt > 0){ stop(); stop_cnt--; } else if(stop_protect == 1 && stop_cnt == 0){ stop_protect = 0; if(state_cnt == 0){//״̬1Çл»×´Ì¬2 state_cnt++; right_cnt = 125; } else if(state_cnt == 1){//״̬2Çл»×´Ì¬3 state_cnt++; forward_cnt = 585; } else if(state_cnt == 2){//״̬3Çл»×´Ì¬4 state_cnt++; left_cnt = 120; } else if(state_cnt == 3){//״̬4Çл»×´Ì¬5 state_cnt++; forward_cnt = 585; } else if(state_cnt == 4){//״̬5Çл»×´Ì¬6 state_cnt++; left_cnt = 120; } else if(state_cnt == 5){//״̬6Çл»×´Ì¬7 state_cnt++; forward_cnt = 585; } } } } Set_Pwm(-MotorA.Motor_Pwm, MotorB.Motor_Pwm); // Çý¶¯µç»ú } } /************************************************************************** Function: Bluetooth_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºÊÖ»úÀ¶ÑÀ¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Bluetooth_Control(void) { if (Flag_Direction == 0) Move_X = 0, Move_Z = 0; // Í£Ö¹ else if (Flag_Direction == 1) Move_X = RC_Velocity, Move_Z = 0; // ǰ½ø else if (Flag_Direction == 2) Move_X = RC_Velocity, Move_Z = Pi / 2; // ÓÒǰ else if (Flag_Direction == 3) Move_X = 0, Move_Z = Pi / 2; // ÏòÓÒ else if (Flag_Direction == 4) Move_X = -RC_Velocity, Move_Z = Pi / 2; // ÓÒºó else if (Flag_Direction == 5) Move_X = -RC_Velocity, Move_Z = 0; // ºóÍË else if (Flag_Direction == 6) Move_X = -RC_Velocity, Move_Z = -Pi / 2; // ×óºó else if (Flag_Direction == 7) Move_X = 0, Move_Z = -Pi / 2; // Ïò×ó else if (Flag_Direction == 8) Move_X = RC_Velocity, Move_Z = -Pi / 2; // ×óǰ else Move_X = 0, Move_Z = 0; if (Car_Num == Akm_Car) { // Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed // °¢¿ËÂü½á¹¹Ð¡³µ×ª»»ÎªÇ°ÂÖתÏò½Ç¶È Move_Z = Move_Z * 2 / 10; } Move_X = Move_X / 1000; Move_Z = -Move_Z; // ת»»ÎªËÙ¶ÈתΪm/s } /************************************************************************** Function: PS2_Control Input : none Output : none º¯Êý¹¦ÄÜ£ºPS2ÊÖ±ú¿ØÖÆ Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void PS2_Control(void) { int LY, RX; // ÊÖ±úADCµÄÖµ int Threshold = 20; // ãÐÖµ£¬ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ static u8 Key1_Count = 0, Key2_Count = 0; // ÓÃÓÚ¿ØÖƶÁȡҡ¸ËµÄËÙ¶È // ת»¯Îª128µ½-128µÄÊýÖµ LY = -(PS2_LY - 128); // ×ó±ßYÖá¿ØÖÆÇ°½øºóÍË RX = -(PS2_RX - 128); // ÓÒ±ßXÖá¿ØÖÆ×ªÏò if (LY > -Threshold && LY < Threshold) LY = 0; if (RX > -Threshold && RX < Threshold) RX = 0; // ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ Move_X = (RC_Velocity / 128) * LY; // ËÙ¶È¿ØÖÆ£¬Á¦¶È±íʾËÙ¶È´óС if (Car_Num == Akm_Car) // °¢¿ËÂü³µ×ªÏò¿ØÖÆ£¬Á¦¶È±íʾתÏò½Ç¶È Move_Z = -(RC_Turn_Velocity / 128) * RX; else // ÆäËû³µÐÍתÏò¿ØÖÆ { if (Move_X >= 0) Move_Z = -(RC_Turn_Velocity / 128) * RX; // תÏò¿ØÖÆ£¬Á¦¶È±íʾתÏòËÙ¶È else Move_Z = (RC_Turn_Velocity / 128) * RX; } if (PS2_KEY == PSB_L1) // °´ÏÂ×ó1¼ü¼ÓËÙ£¨°´¼üÔÚ¶¥ÉÏ£© { if ((++Key1_Count) == 20) // µ÷½Ú°´¼ü·´Ó¦ËÙ¶È { PS2_KEY = 0; Key1_Count = 0; if ((RC_Velocity += X_Step) > MAX_RC_Velocity) // ǰ½ø×î´óËÙ¶È800mm/s RC_Velocity = MAX_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity += Z_Step) > MAX_RC_Turn_Bias) // תÏò×î´óËÙ¶È325 RC_Turn_Velocity = MAX_RC_Turn_Bias; } } } else if (PS2_KEY == PSB_R1) // °´ÏÂÓÒ1¼ü¼õËÙ { if ((++Key2_Count) == 20) { PS2_KEY = 0; Key2_Count = 0; if ((RC_Velocity -= X_Step) < MINI_RC_Velocity) // ǰºó×îСËÙ¶È210mm/s RC_Velocity = MINI_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È { if ((RC_Turn_Velocity -= Z_Step) < MINI_RC_Turn_Velocity) // תÏò×îСËÙ¶È45 RC_Turn_Velocity = MINI_RC_Turn_Velocity; } } } else Key2_Count = 0, Key2_Count = 0; // ¶ÁÈ¡µ½ÆäËû°´¼üÖØÐ¼ÆÊý Move_X = Move_X / 1000; Move_Z = -Move_Z; // ËÙ¶ÈMove_XתΪm/s } /************************************************************************** Function: Get_Velocity_From_Encoder Input : none Output : none º¯Êý¹¦ÄÜ£º¶ÁÈ¡±àÂëÆ÷ºÍת»»³ÉËÙ¶È Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Velocity_From_Encoder(void) { // Retrieves the original data of the encoder // »ñÈ¡±àÂëÆ÷µÄԭʼÊý¾Ý float Encoder_A_pr, Encoder_B_pr; OriginalEncoder.A = Read_Encoder(Encoder1); OriginalEncoder.B = Read_Encoder(Encoder2); // Decide the encoder numerical polarity according to different car models // ¸ù¾Ý²»Í¬Ð¡³µÐͺžö¶¨±àÂëÆ÷ÊýÖµ¼«ÐÔ switch (Car_Num) { case Akm_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Diff_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Small_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; case Big_Tank_Car: Encoder_A_pr = OriginalEncoder.A; Encoder_B_pr = -OriginalEncoder.B; break; } // The encoder converts the raw data to wheel speed in m/s // ±àÂëÆ÷ԭʼÊý¾Ýת»»Îª³µÂÖËÙ¶È£¬µ¥Î»m/s MotorA.Current_Encoder = Encoder_A_pr * Frequency * Perimeter / 60000.0f; // 60000 = 4*500*30 MotorB.Current_Encoder = Encoder_B_pr * Frequency * Perimeter / 60000.0f; // 1560=4*13*30=2£¨Á½Â·Âö³å£©*2£¨ÉÏÏÂÑØ¼ÆÊý£©*»ô¶û±àÂëÆ÷13Ïß*µç»úµÄ¼õËÙ±È // MotorA.Current_Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Akm_wheelspacing//(4*13*30); // MotorB.Current_Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Akm_wheelspacing/Encoder_precision; } /************************************************************************** Function: Drive_Motor Input : none Output : none º¯Êý¹¦ÄÜ£ºÔ˶¯Ñ§Äæ½â Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ // Ô˶¯Ñ§Äæ½â£¬ÓÉxºÍyµÄËٶȵõ½±àÂëÆ÷µÄËÙ¶È,VxÊÇm/s,Vzµ¥Î»ÊǶÈ/s(½Ç¶ÈÖÆ) // °¢¿ËÂü³µVzÊǶæ»úתÏòµÄ½Ç¶È(»¡¶ÈÖÆ) void Get_Target_Encoder(float Vx, float Vz) { float MotorA_Velocity, MotorB_Velocity; float amplitude = 3.5f; // Wheel target speed limit //³µÂÖÄ¿±êËÙ¶ÈÏÞ·ù Move_X = target_limit_float(Move_X, -1.2, 1.2); Move_Z = target_limit_float(Move_Z, -Pi / 3, Pi / 3); if (Car_Num == Akm_Car) // °¢¿ËÂü³µ { // Ackerman car specific related variables //°¢¿ËÂüС³µ×¨ÓÃÏà¹Ø±äÁ¿ float R, ratio = 636.56, AngleR, Angle_Servo; // For Ackerman small car, Vz represents the front wheel steering Angle // ¶ÔÓÚ°¢¿ËÂüС³µVz´ú±íÓÒǰÂÖתÏò½Ç¶È AngleR = Vz; R = Akm_axlespacing / tan(AngleR) - 0.5f * Wheelspacing; // Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad // ǰÂÖתÏò½Ç¶ÈÏÞ·ù(¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È)£¬µ¥Î»£ºrad AngleR = target_limit_float(AngleR, -0.49f, 0.32f); // Inverse kinematics //Ô˶¯Ñ§Äæ½â if (AngleR != 0) { MotorA.Target_Encoder = Vx * (R - 0.081f) / R; MotorB.Target_Encoder = Vx * (R + 0.081f) / R; } else { MotorA.Target_Encoder = Vx; MotorB.Target_Encoder = Vx; } // The PWM value of the servo controls the steering Angle of the front wheel // ¶æ»úPWMÖµ£¬¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È Angle_Servo = -0.628f * pow(AngleR, 3) + 1.269f * pow(AngleR, 2) - 1.772f * AngleR + 1.573f; Servo_PWM = SERVO_INIT + (Angle_Servo - 1.572f) * ratio; // printf("%d\r\n",Servo_PWM); // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); Servo_PWM = target_limit_int(Servo_PWM, 800, 2200); // Servo PWM value limit //¶æ»úPWMÖµÏÞ·ù } else if (Car_Num == Diff_Car) // ²îËÙС³µ { if (Vx < 0) Vz = -Vz; else Vz = Vz; // Inverse kinematics //Ô˶¯Ñ§Äæ½â MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Small_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È // Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } else if (Car_Num == Big_Tank_Car) { if (Vx < 0) Vz = -Vz; else Vz = Vz; MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude); MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude); } } /************************************************************************** Function: Get_Motor_PWM Input : none Output : none º¯Êý¹¦ÄÜ£º×ª»»³ÉÇý¶¯µç»úµÄPWM Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Motor_PWM(void) { // ¼ÆËã×óÓÒµç»ú¶ÔÓ¦µÄPWM MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder); MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder); if (Mode == Normal_Mode || Mode == Measure_Distance_Mode) { // Â˲¨£¬Ê¹Æð²½ºÍÍ£Ö¹ÉÔ΢ƽ»¬Ò»Ð© MotorA.Motor_Pwm = Mean_Filter_Left(MotorA.Motor_Pwm); MotorB.Motor_Pwm = Mean_Filter_Right(MotorB.Motor_Pwm); } // ÏÞ·ù MotorA.Motor_Pwm = PWM_Limit(MotorA.Motor_Pwm, PWM_MAX, PWM_MIN); MotorB.Motor_Pwm = PWM_Limit(MotorB.Motor_Pwm, PWM_MAX, PWM_MIN); } /************************************************************************** Function: PWM_Limit Input : IN;max;min Output : OUT º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ Èë¿Ú²ÎÊý: IN£ºÊäÈë²ÎÊý max£ºÏÞ·ù×î´óÖµ min£ºÏÞ·ù×îСֵ ·µ»Ø Öµ£ºÏÞ·ùºóµÄÖµ **************************************************************************/ float PWM_Limit(float IN, int max, int min) { float OUT = IN; if (OUT > max) OUT = max; if (OUT < min) OUT = min; return OUT; } /************************************************************************** Function: Limiting function Input : Value Output : none º¯Êý¹¦ÄÜ£ºÏÞ·ùº¯Êý Èë¿Ú²ÎÊý£º·ùÖµ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float target_limit_float(float insert, float low, float high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } int target_limit_int(int insert, int low, int high) { if (insert < low) return low; else if (insert > high) return high; else return insert; } /************************************************************************** Function: Check whether it is abnormal Input : none Output : 1:Abnormal;0:Normal º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú²ÎÊý: ÎÞ ·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£ **************************************************************************/ u8 Turn_Off(void) { u8 temp = Normal; Flag_Stop = KEY2_STATE; // ¶ÁÈ¡°´¼ü2״̬£¬°´¼ü2¿ØÖƵç»úµÄ¿ª¹Ø if (Voltage < 1000) // µç³ØµçѹµÍÓÚ10V¹Ø±Õµç»ú,LEDµÆ¿ìËÙÉÁ˸ LED_Flash(50), temp = Abnormal; else LED_Flash(200); // ÿһÃëÉÁÒ»´Î£¬Õý³£ÔËÐÐ if (Flag_Stop) temp = Abnormal; return temp; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Left(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Data sliding filtering Input : data Output : Filtered data º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨ Èë¿Ú²ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý **************************************************************************/ float Mean_Filter_Right(float data) { u8 i; float Sum_Data = 0; float Filter_Data; static float Speed_Buf[FILTERING_TIMES] = {0}; for (i = 1; i < FILTERING_TIMES; i++) { Speed_Buf[i - 1] = Speed_Buf[i]; } Speed_Buf[FILTERING_TIMES - 1] = data; for (i = 0; i < FILTERING_TIMES; i++) { Sum_Data += Speed_Buf[i]; } Filter_Data = (s32)(Sum_Data / FILTERING_TIMES); return Filter_Data; } /************************************************************************** Function: Lidar_Avoid Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï±ÜÕÏģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_Avoid(void) { int i = 0; u8 calculation_angle_cnt = 0; // ÓÃÓÚÅжÏ100¸öµãÖÐÐèÒª×ö±ÜÕϵĵã float angle_sum = 0; // ´ÖÂÔ¼ÆËãÕϰ­ÎïλÓÚ×ó»òÕßÓÒ u8 distance_count = 0; // ¾àÀëСÓÚijֵµÄ¼ÆÊý int distance = 350; // É趨±ÜÕϾàÀë,ĬÈÏÊÇ300 if (Car_Num == Akm_Car) distance = 400; // °¢¿ËÂü³µÉ趨ÊÇ400mm else if (Car_Num == Big_Tank_Car) distance = 500; // ´óÂÄ´ø³µÉ趨ÊÇ500mm for (i = 0; i < lap_count; i++) { if ((Dataprocess[i].angle > 310) || (Dataprocess[i].angle < 50)) { if ((0 < Dataprocess[i].distance) && (Dataprocess[i].distance < distance)) // ¾àÀëСÓÚ350mmÐèÒª±ÜÕÏ,Ö»ÐèÒª100¶È·¶Î§ÄÚµã { calculation_angle_cnt++; // ¼ÆËã¾àÀëСÓÚ±ÜÕϾàÀëµÄµã¸öÊý if (Dataprocess[i].angle < 50) angle_sum += Dataprocess[i].angle; else if (Dataprocess[i].angle > 310) angle_sum += (Dataprocess[i].angle - 360); // 310¶Èµ½50¶Èת»¯Îª-50¶Èµ½50¶È if (Dataprocess[i].distance < 200) // ¼Ç¼СÓÚ200mmµÄµãµÄ¼ÆÊý distance_count++; } } } if (calculation_angle_cnt < 8) // СÓÚ8µã²»ÐèÒª±ÜÕÏ£¬È¥³ýһЩÔëµã { if ((Move_X += 0.1) >= Aovid_Speed) // ±ÜÕϵÄËÙ¶ÈÉ趨Ϊ260£¬Öð½¥Ôö¼Óµ½260¿ÉÉÔ΢ƽ»¬Ò»Ð© Move_X = Aovid_Speed; Move_Z = 0; // ²»±ÜÕÏʱ²»ÐèҪתÍä } else // ÐèÒª±ÜÕÏ£¬¼òµ¥µØÅжÏÕϰ­Î﷽λ { if (Car_Num == Akm_Car) // °¢¿ËÂü³µÐÍÓжæ»ú£¬ÐèÒªÌØÊâ´¦Àí { if (distance_count > 8) // ¾àÀëСÓÚ±ÜÕ½¾àÀë Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ0.25 Move_X = Aovid_Speed * 0.5; if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ Move_Z = -Pi / 5; // ÿ´ÎתÍä½Ç¶ÈΪPI/5£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ else // Æ«×ó Move_Z = Pi / 5; } } else { if (distance_count > 8) // СÓÚ±ÜÕ½¾àÀëµÄʱºò Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ¶È0.15 Move_X = (Aovid_Speed * 0.5); if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = -1; else if (Car_Num == Small_Tank_Car) Move_Z = -1; else Move_Z = -1; } else // Æ«×ó { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ Move_Z = 1; else if (Car_Num == Small_Tank_Car) Move_Z = 1; else Move_Z = 1; } } } } Move_Z = -Move_Z; } /************************************************************************** Function: Lidar_Follow Input : none Output : none º¯Êý¹¦ÄÜ£ºÀ×´ï¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float angle1 = 0; // ¸úËæµÄ½Ç¶È u16 mini_distance1; void Lidar_Follow(void) { static u16 cnt = 0; int i; int calculation_angle_cnt = 0; static float angle = 0; // ¸úËæµÄ½Ç¶È static float last_angle = 0; // u16 mini_distance = 65535; static u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ // ÐèÒªÕÒ³ö¸úËæµÄÄǸöµãµÄ½Ç¶È for (i = 0; i < lap_count; i++) { if (100 < Dataprocess[i].distance && Dataprocess[i].distance < Follow_Distance) // 1200·¶Î§ÄÚ¾ÍÐèÒª¸úËæ { calculation_angle_cnt++; if (Dataprocess[i].distance < mini_distance) // ÕÒ³ö¾àÀë×îСµÄµã { mini_distance = Dataprocess[i].distance; angle = Dataprocess[i].angle; } } } if (angle > 180) angle -= 360; // 0--360¶Èת»»³É0--180£»-180--0£¨Ë³Ê±Õ룩 if (angle - last_angle > 10 || angle - last_angle < -10) // ×öÒ»¶¨Ïû¶¶£¬²¨¶¯´óÓÚ10¶ÈµÄÐèÒª×öÅÐ¶Ï { if (++data_count == 60) // Á¬Ðø60´Î²É¼¯µ½µÄÖµ(300msºó)ºÍÉϴεıȴóÓÚ10¶È£¬´Ëʱ²ÅÊÇÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } } else // ²¨¶¯Ð¡ÓÚ10¶ÈµÄ¿ÉÒÔÖ±½ÓÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } if (calculation_angle_cnt < 6) // ÔÚ¸úËæ·¶Î§ÄڵĵãÉÙÓÚ6¸ö { if (cnt < 40) // Á¬Ðø¼ÆÊý³¬40´ÎûÓÐÒª¸úËæµÄµã£¬´Ëʱ²ÅÊDz»ÓøúËæ cnt++; if (cnt >= 40) { Move_X = 0; // ËÙ¶ÈΪ0 Move_Z = 0; } } else { cnt = 0; if (Car_Num == Akm_Car) { if ((((angle > 15) && (angle < 180)) || ((angle > -180) && angle < -15)) && (mini_distance < 500)) // °¢¿¨Âü³µÐÍ´¦Àí³µÍ·²»¶ÔןúËæÎÏ൱ÓÚºó³µÒ»Ñù£¬Ò»´Î²»¶Ô×¼£¬ÄǺóÍËÔÙÀ´¶Ô×¼ { Move_X = -0.20; Move_Z = -Follow_Turn_PID(last_angle, 0); } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); } } else // ÆäÓà³µÐÍ { if ((angle > 50 || angle < -50) && (mini_distance > 400)) { Move_Z = -0.0298f * last_angle; // ½Ç¶È²î¾à¹ý´óÖ±½Ó¿ìËÙתÏò Move_X = 0; // ²îËÙС³µºÍÂÄ´øÐ¡³µ¿ÉÒÔʵÏÖÔ­µØ×ª¶¯ } else { Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm Move_Z = Follow_Turn_PID(last_angle, 0); // תÏòPID£¬³µÍ·ÓÀÔ¶¶ÔןúËæÎïÆ· } } } Move_Z = target_limit_float(Move_Z, -Pi / 6, Pi / 6); // ÏÞ·ù Move_X = target_limit_float(Move_X, -0.6, 0.6); } /************************************************************************** º¯Êý¹¦ÄÜ£ºÐ¡³µ×ßÖ±Ïßģʽ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Lidar_along_wall(void) { static u32 target_distance = 0; static int n = 0; u32 distance; u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ for (int i = 0; i < lap_count; i++) { if (Dataprocess[i].angle > 75 && Dataprocess[i].angle < 77) { if (n == 0) { target_distance = Dataprocess[i].distance; // »ñÈ¡µÄµÚÒ»¸öµã×÷ΪĿ±ê¾àÀë n++; } if (Dataprocess[i].distance < target_distance + 100) //+100ÏÞÖÆ»ñÈ¡¾àÀëµÄ·¶Î§Öµ { distance = Dataprocess[i].distance; // »ñȡʵʱ¾àÀë data_count++; } } } // if(data_count <= 0) // Move_X = 0; // Move_X = forward_velocity; // ³õʼËÙ¶È Move_Z = -Along_Adjust_PID(distance, target_distance); if (Car_Num == Akm_Car) { Move_Z = target_limit_float(Move_Z, -Pi / 4, Pi / 4); // ÏÞ·ù } else if (Car_Num == Diff_Car) Move_Z = target_limit_float(Move_Z, -Pi / 5, Pi / 5); // ÏÞ·ù } /************************************************************************** Function: Car_Perimeter_Init Input : none Output : none º¯Êý¹¦ÄÜ£º¼ÆËãС³µ¸÷ÂÖ×ÓµÄÖܳ¤ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Car_Perimeter_Init(void) { if (Car_Num == Diff_Car || Car_Num == Akm_Car) { Perimeter = Diff_Car_Wheel_diameter * Pi; Wheelspacing = Diff_wheelspacing; } else if (Car_Num == Small_Tank_Car) { Perimeter = Small_Tank_WheelDiameter * Pi; Wheelspacing = Small_Tank_wheelspacing; } else { Perimeter = Big_Tank_WheelDiameter * Pi; Wheelspacing = Big_Tank_wheelspacing; } } /************************************************************************** Function: Ultrasonic_Follow Input : none Output : none º¯Êý¹¦ÄÜ£º³¬Éù²¨¸úËæÄ£Ê½ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Ultrasonic_Follow(void) // ³¬Éù²¨¸úËæ£¬Ö»Äܵ¥·½Ïò¸úËæ { Move_Z = 0; Read_Distane(); // ¶ÁÈ¡³¬Éù²¨µÄ¾àÀë if (Distance1 < 200) // ¾àÀëСÓÚ200mm£¬Í˺ó { if ((Move_X -= 3) < -210) Move_X = -210; // ¸øÒ»210ºóÍËËÙ¶È } else if (Distance1 > 270 && Distance1 < 750) // ¾àÀëÔÚ270µ½750Ö®¼äÊÇÐèÒª¸úËæÇ°½ø { if ((Move_X += 3) > 210) // ËÙ¶ÈÖð½¥Ôö¼Ó£¬¸øÇ°½øËÙ¶È Move_X = 210; } else { if (Move_X > 0) { if ((Move_X -= 20) < 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } else { if ((Move_X += 20) > 0) // ËÙ¶ÈÖð½¥¼õµ½0 Move_X = 0; } } } /************************************************************************** Function: Get angle Input : way£ºThe algorithm of getting angle 1£ºDMP 2£ºkalman 3£ºComplementary filtering Output : none º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È Èë¿Ú²ÎÊý£ºway£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Angle(u8 way) { if (way == 1) // DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯Öж϶ÁÈ¡£¬Ñϸñ×ñѭʱÐòÒªÇó { Read_DMP(); // ¶ÁÈ¡¼ÓËÙ¶È¡¢½ÇËÙ¶È¡¢Çã½Ç } else { Gyro_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_L); // ¶ÁÈ¡XÖáÍÓÂÝÒÇ Gyro_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_L); // ¶ÁÈ¡YÖáÍÓÂÝÒÇ Gyro_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_L); // ¶ÁÈ¡ZÖáÍÓÂÝÒÇ Accel_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_L); // ¶ÁÈ¡XÖá¼ÓËÙ¶È¼Æ Accel_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_L); // ¶ÁÈ¡ZÖá¼ÓËÙ¶È¼Æ // if(Gyro_X>32768) Gyro_X-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Z>32768) Gyro_Z-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_X>32768) Accel_X-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Y>32768) Accel_Y-=65536; //Êý¾ÝÀàÐÍת»» // if(Accel_Z>32768) Accel_Z-=65536; //Êý¾ÝÀàÐÍת»» Accel_Angle_x = atan2(Accel_Y, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Accel_Angle_y = atan2(Accel_X, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È Gyro_X = Gyro_X / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»£¬Á¿³Ì¡À500¡ã/s¶ÔÓ¦ÁéÃô¶È65.5£¬¿É²éÊÖ²á Gyro_Y = Gyro_Y / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»» if (way == 2) { Roll = -Kalman_Filter_x(Accel_Angle_x, Gyro_X); // ¿¨¶ûÂüÂ˲¨ Pitch = -Kalman_Filter_y(Accel_Angle_y, Gyro_Y); } else if (way == 3) { Roll = -Complementary_Filter_x(Accel_Angle_x, Gyro_X); // »¥²¹Â˲¨ Pitch = -Complementary_Filter_y(Accel_Angle_y, Gyro_Y); } } } /************************************************************************** Function: The remote control command of model aircraft is processed Input : none Output : none º¯Êý¹¦ÄÜ£º¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Remote_Control(void) { // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí static u8 thrice = 200; int Threshold = 100; // limiter //ÏÞ·ù int LX, RY; // static float Target_LX,Target_LY,Target_RY,Target_RX; Remoter_Ch1 = target_limit_int(Remoter_Ch1, 1000, 2000); Remoter_Ch2 = target_limit_int(Remoter_Ch2, 1000, 2000); // Front and back direction of left rocker. Control forward and backward. // ×óÒ¡¸Ëǰºó·½Ïò¡£¿ØÖÆÇ°½øºóÍË¡£ LX = Remoter_Ch2 - 1500; // //Left joystick left and right. Control left and right movement. // //×óÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×óÓÒÒÆ¶¯¡£¡£ // LY=Remoter_Ch2-1500; // Right stick left and right. To control the rotation. // ÓÒÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×Ôת¡£ RY = Remoter_Ch1 - 1500; // if (LX > -Threshold && LX < Threshold) LX = 0; if (RY > -Threshold && RY < Threshold) RY = 0; // if(LX==0) Target_LX=Target_LX/1.2f; // if(LY==0) Target_LY=Target_LY/1.2f; // if(RY==0) Target_RY=Target_RY/1.2f; // //Throttle related //ÓÍÃÅÏà¹Ø // Remote_RCvelocity=RC_Velocity+RX; // if(Remote_RCvelocity<0)Remote_RCvelocity=0; // The remote control command of model aircraft is processed // ¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí Move_X = LX; Move_Z = -RY; Move_X = Move_X * 1.3; //*1.3ÊÇΪÁËÀ«´óËÙ¶È if (Car_Num == Akm_Car) Move_Z = Move_Z * (Pi / 8) / 350.0; else Move_Z = Move_Z * 2 * (Pi / 4) / 350.0; // Unit conversion, mm/s -> m/s // µ¥Î»×ª»»£¬mm/s -> m/s Move_X = Move_X / 1000; // ZÖáÊý¾Ýת»¯ #if _4WD if (Move_X < 0) Move_Z = -Move_Z; #endif // Data within 1 second after entering the model control mode will not be processed // ¶Ô½øÈ뺽ģ¿ØÖÆÄ£Ê½ºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí if (thrice > 0) Move_X = 0, Move_Z = 0, thrice--; // Control target value is obtained and kinematics analysis is performed // µÃµ½¿ØÖÆÄ¿±êÖµ£¬½øÐÐÔ˶¯Ñ§·ÖÎö // Get_Target_Encoder(Move_X,Move_Z); } 怎么把OpenMV与STM32串口通信实现二维码/APRILTag码识别及控制的代码加到上述代码中,现在openmv的二维码识别已做好,二维码采用的是Apriltag的36h11类型,id:0表示停止,id:1表示直行,id:2表示左转,id:3表示右转,现在需要再在STM32中修改或添加代码,使其能在接收到openmv识别到的指令后作出相应的动作
05-16
static float Bias,Pwm,Integral_bias,Last_Bias; int Position_PID (int Encoder,int Target) { Bias=Encoder-Target; //¼ÆËãÆ«²î Integral_bias+=Bias; //Çó³öÆ«²îµÄ»ý·Ö // Pwm=70*Bias*Ratio+0.00*Integral_bias*Ratio+200*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Pwm=100*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Last_Bias=Bias; //±£´æÉÏÒ»´ÎÆ«²î return Pwm; //ÔöÁ¿Êä³ö } int Position_PID_1 (int Encoder,float Target) { static float Bias,Pwm,Integral_bias,Last_Bias,encoder; encoder=(float)Encoder; Bias=encoder-Target; //¼ÆËãÆ«²î Integral_bias+=Bias; //Çó³öÆ«²îµÄ»ý·Ö // Pwm=90*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Pwm=90*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Last_Bias=Bias; //±£´æÉÏÒ»´ÎÆ«²î return Pwm; //ÔöÁ¿Êä³ö } int Position_PID_3 (int Encoder,int Target) { static float Bias,Pwm,Integral_bias,Last_Bias; Bias=Encoder-Target; //¼ÆËãÆ«²î Integral_bias+=Bias; //Çó³öÆ«²îµÄ»ý·Ö // Pwm=70*Bias*Ratio+0.00*Integral_bias*Ratio+200*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ // Pwm=280*Bias*Ratio+0.00*Integral_bias*Ratio+300*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Pwm=90*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio; //λÖÃʽPID¿ØÖÆÆ÷ Last_Bias=Bias; //±£´æÉÏÒ»´ÎÆ«²î return Pwm; //ÔöÁ¿Êä³ö } u16 Count_Next=0; static int Angle_Max,Position_Max; static u8 Flag_Back; u16 number=0; //int Count_FZ,Target_Position=10580; /************************************************************************** º¯Êý¹¦ÄÜ£º×Ô¶¯Æð°Ú³ÌÐò Èë¿Ú²ÎÊý£ºint ·µ»Ø Öµ£º **************************************************************************/ void Run(u8 Way) { static float Count_FZ,Target_Position=10450; static float Count_Big_Angle=0.046542; if(Way==2) //ÊÖ¶¯Æð°Ú³ÌÐò { if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200)) //µ½µ×½Ó½üƽºâλÖà ¼´¿É¿ªÆôƽºâϵͳ { State=1; //µ¹Á¢×´Ì¬ÖÃ1 Way_Turn=0;//Æð°Ú±ê־λÇåÁã } } } /************************************************************************** º¯Êý¹¦ÄÜ£ºÇã½ÇPD¿ØÖÆ Èë¿Ú²ÎÊý£º½Ç¶È ·µ»Ø Öµ£ºÇã½Ç¿ØÖÆPWM **************************************************************************/ int balance_0(float Angle) { float Bias; //Çã½ÇÆ«²î static float Last_Bias,D_Bias,I_Bias; //PIDÏà¹Ø±äÁ¿ int balance; Bias=Angle-ZHONGZHI; I_Bias+=Bias; D_Bias=Bias-Last_Bias; balance=-Balance_KP*Bias- D_Bias*Balance_KD+ I_Bias*Balance_Ratio; Last_Bias=Bias; return balance; //PWM·µ»ØÖµ //Çó³öƽºâµÄ½Ç¶ÈÖÐÖµ ºÍ»úеÏà¹Ø //Îó²î×öÀÛ¼Ó£¬×ö»ý·Ö¶ÔÏó //Çó³öÆ«²îµÄ΢·Ö ½øÐÐ΢·Ö¿ØÖÆ //ʹÓÃλÖÃPIDËã·¨¹«Ê½¸øPWM¸³Öµ£¬Æ½ºâ¿ØÖÆÐèÒª¿ìËÙ·´Ó¦£¬Ê¹ÓÃPD¿ØÖÆËã·¨£¬¼´»ý·Ö²ÎÊýΪÁã //ÉÏ´ÎÎó²îµÈÓÚµ±Ç°Îó²î //º¯Êý·µ»ØÖµÎªÇã½Ç¿ØÖÆPWMÖµ } /************************************************************************** º¯Êý¹¦ÄÜ£ºÎ»ÖÃPD¿ØÖÆ Èë¿Ú²ÎÊý£º±àÂëÆ÷ ·µ»Ø Öµ£ºÎ»ÖÿØÖÆPWM **************************************************************************/ int Position_0(int Encoder) { static float Position_PWM, Last_Position, Position_Bias, Position_Differential; static float Position_Least; Position_Least=Encoder-Position_Zero; //=-???? Position_Bias *=0.8; Position_Bias += Position_Least*0.2; Position_Differential=Position_Bias-Last_Position; Last_Position=Position_Bias; Position_PWM =Position_Bias*Position_KP+Position_Differential*Position_KD; return Position_PWM; //¶¨ÒåλÖÿØÖƱäÁ¿£¬·Ö±ðÐèÒª¶¨Òå¿ØÖÆPWMÖµ¡¢ÉÏÒ»´ÎµÄλÖÃÖµ¡¢µ±Ç°Î»ÖÃÖµ¡¢Î¢·Ö¶ÔÏóÖµ //¶¨ÒåλÖÃÎó²î±äÁ¿ //===»ñȡλÖÃÆ«²îÁ¿£¬¼´Îó²îÁ¿ //Ò»½×µÍͨÂ˲¨Æ÷Ëã·¨ //===Ò»½×µÍͨÂ˲¨Æ÷ //===»ñȡƫ²î±ä»¯ÂÊ£¬µ±Ç°Îó²îÓëÉÏ´ÎÎó²îÇó²î£¬¼´Î¢·Ö¶ÔÏóÖµ //ÉÏ´ÎÎó²îµÈÓÚµ±Ç°Îó²î£¬±£´æÉÏÒ»´ÎµÄÆ«²î //===ʹÓÃλÖÃPIDËã·¨¹«Ê½¸øPWM¸³Öµ£¬ÐèҪλÖÿìËÙ·´Ó¦£¬²ÉÓÃPDËã·¨£¬¼´»ý·Ö²ÎÊýΪÁã //º¯Êý·µ»ØÖµÎªµ¹Á¢°ÚλÖÿØÖÆPWMÖµ } /************************************************************************** º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ Èë¿Ú²ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Xianfu_Pwm(void) { int Amplitude=6900; //===PWMÂú·ùÊÇ7200 ÏÞÖÆÔÚ6900 if(Moto<-Amplitude) Moto=-Amplitude; if(Moto>Amplitude) Moto=Amplitude; } /************************************************************************** º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú²ÎÊý£ºµçѹ ·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£ **************************************************************************/ u8 Turn_Off(void) { u8 temp; if(Flag_Stop==1||(State==1 && (Angle_Balance<(ZHONGZHI-460)||Angle_Balance>(ZHONGZHI+460)))) //¹Ø±Õµç»ú { Flag_Stop=0; Way_Turn=0; Flag_qb2=0; Angle_Max=0; Position_Max=0; State=0; temp=1; TIM_SetCounter(TIM2,10000);//³õʼ»¯¼ÆÊýÆ÷³õÖµ } else { temp=0; } return temp; } void Encoder_App(void) { if(State == 0) //Æð°Úʱ²ÉÑùÈ¡3´ÎÇ󯽾ù£¬5´ÎÖÜÆÚÌ«³¤ { Angle_Balance=Get_ADC(5); } if(State==1) { Angle_Balance=Get_ADC(20);//´Ë¿ØÖÆÖÜÆÚ²»ÐèÒªÄÇô¶Ì£¬Æð°Ú³É¹¦ºó²ÉÑùÈ¡5´ÎÇ󯽾ùÖµ£¬ } D_Angle_Balance= Angle_Balance-Last_Angle_Balance ; //===»ñȡ΢·ÖÖµ if (State == 0 && Way_Turn ==1) { if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200)) //µ½µ×½Ó½üƽºâλÖà ¼´¿É¿ªÆôƽºâϵͳ { State=1; //µ¹Á¢×´Ì¬ÖÃ1 Way_Turn=0;//×Ô¶¯Æð°Ú±ê־λÇåÁã Flag_qb=0; //×Ô¶¯Æð°Ú²½ÖèÇåÁã Angle_Max=0; Flag_Back=0; } } if(State==0) { Run(Way_Turn);//Æð°Ú ÓÉÈë¿Ú²ÎÊý¿ØÖÆ 1£º×Ô¶¯Æð°Ú 2£ºÊÖ¶¯Æð°Ú if(Turn_Off()==1) //°´ÏÂÍ£Ö¹¼üʱֹͣµç»ú { Moto_qb=0; } } if(State==1) //Æð°Ú³É¹¦Ö®ºó£¬½øÐе¹Á¢¿ØÖÆ { Balance_Pwm =balance_0(Angle_Balance); //¿ªÆôƽºâ¿ØÖÆ if(Flag_qb2==0) //λÖÿØÖÆÑÓʱÆô¶¯ { if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200))Count_Position++; // if(Count_Position>20)Flag_qb2=1, Count_Position=0,TIM_SetCounter(TIM2,10000); //ÔÚÆ½ºâλÖõ¹Á¢³¬¹ý300ms ¿ªÆôλÖÿØÖÆ } if(Flag_qb2==1) //¿ªÆôλÖÿØÖÆ { Encoder=TIM_GetCounter(TIM2); //===¸üбàÂëÆ÷λÖÃÐÅÏ¢ if(++Count_P2>=4) Position_Pwm=Position_0(Encoder),Count_P2=0; //===λÖÃPD¿ØÖÆ 25ms½øÐÐÒ»´ÎλÖÿØÖÆ } Moto=Balance_Pwm-Position_Pwm; //===¼ÆËãµç»ú×îÖÕPWM Xianfu_Pwm(); //===PWMÏÞ·ù if(Turn_Off()==1) //°´ÏÂÍ£Ö¹¼ü»òÕßÇã½Ç¹ý´ó±£»¤£¬µç»úÍ£Ö¹ { Moto=0; } if(Moto>7200) Moto=7200; if(Moto<-7200)Moto=-7200; Set_Pwm(Moto); //===¸³Öµ¸øPWM¼Ä´æÆ÷ } Last_Angle_Balance=Angle_Balance; //===±£´æÉÏÒ»´ÎµÄÇã½ÇÖµ } 解释上述代码
最新发布
05-22
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值