°Ù¶È¶¯Ì¬Ò»Öܾ«²Ê²¥±¨µÚ70ÆÚ¡ª¡ª10ÔÂ27ÈÕÖÁ11ÔÂ2ÈÕ

本周度网发布了第七期动态,公司正式加入全球合作组织“全球通讯”(GlobalCom)。本期报道了度网近期对外正式公布的信息及公司的最新进展。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

°Ù¶È·ÛË¿Íø°Ù¶È·ÛË¿ÍøÔ­´´±¨µÀ£ºµÚÆßÊ®ÆÚ°Ù¶È¶¯Ì¬Ò»Öܲ¥±¨Óë´ó¼Ò¼ûÃæÁË¡£»¶Ó­´ó¼ÒÔĶÁ£¡ ±¾ÖܵİٶÈÏà¹ØÐÂÎŲ»¶à£¬Öܶþ£¬°Ù¶È½ñÈÕ¶ÔÍâÕýʽÐû²¼³Æ£¬¹«Ë¾ÒÑÕýʽ³Ðŵ¼ÓÈëÁªºÏ¹ú¡°È«ÇòÆõÔ¼¡±×éÖ¯(Global Compa ...
////////////////////////////////////////////////////////////////////////////////// //±¾³ÌÐòÖ»¹©Ñ§Ï°Ê¹Óã¬Î´¾­×÷ÕßÐí¿É£¬&sup2;»µÃÓÃÓÚÆäËüÈκÎÓÃ; //ÐÀÞ±µç×Ó // // ÎÄ ¼þ Ãû :yanwu.c // °æ ±¾ ºÅ : v1.0 // ×÷ Õß : ÐÀÞ±µç×Ó // Éú³ÉÈÕÆÚ : 20200101 // ×î&frac12;üÐÞ¸Ä : // ¹¦ÄÜÃèÊö :Ö÷º¯Êý // ÐÞ¸ÄÀúÊ· : // ÈÕ ÆÚ : // ×÷ Õß : ÐÀÞ±µç×Ó // ÐÞ¸ÄÄÚÈÝ : ´´&frac12;¨Îļþ //°æÈ¨ËùÓУ¬µÁ°æ±Ø¾¿¡£ //Copyright(C) ÐÀÞ±µç×Ó2020/3/16 //All rights reserved //******************************************************************************/. //³ÌÐòÍ·º¯Êý #include <reg52.h> #include <intrins.h> //°üº¬Í·Îļþ //ÏÔʾº¯Êý #include <display.h> //ºê¶¨Òå #define uint unsigned int #define uchar unsigned char //¹Ü&frac12;ÅÉùÃ÷ sbit LED_R= P2^2;//ºìµÆ sbit LED_G= P2^0;//ÂÌµÆ sbit FENG = P2^5;//·äÃùÆ÷ //sbit san=P3^4; //·çÉÈ¿ØÖÆ£¨Ñ¡Å䣩 sbit CS = P1^4; sbit Clk = P1^2; sbit DATI = P1^3; sbit DATO = P1^3; //ADC0832Òý&frac12;Å //°´¼ü sbit Key1=P2^6; //ÉèÖüü sbit Key2=P2^7; //¼Ó°´¼ü sbit Key3=P3^7; //¼õ°´¼ü bit bdata flag; //±¨¾¯±ê־λ uchar set; //ÉèÖÃ״̬ /*******************************¶¨ÒåÈ«¾Ö±äÁ¿********************************/ unsigned char dat = 0; //ADÖµ unsigned char CH=0; //ͨµÀ±äÁ¿ unsigned int sum=0; //Æ&frac12;¾ùÖµ¼ÆËãʱµÄ×ÜÊý unsigned char m=0; //º¯ÊýÉùÃ÷ extern uchar ADC0809(); extern void Key(); //¾Æ¾«º¬Á¿±äÁ¿ uchar temp=0; uchar WARNING=25; //±¨¾¯Öµ /**************************************************************************** º¯Êý¹¦ÄÜ:ADת»»×Ó³ÌÐò Èë¿Ú&sup2;ÎÊý:CH ³ö¿Ú&sup2;ÎÊý:dat ****************************************************************************/ unsigned char adc0832(unsigned char CH) { unsigned char i,test,adval; adval = 0x00; test = 0x00; Clk = 0; //³õʼ»¯ DATI = 1; _nop_(); CS = 0; _nop_(); Clk = 1; _nop_(); if ( CH == 0x00 ) //ͨµÀÑ¡Ôñ { Clk = 0; DATI = 1; //ͨµÀ0µÄµÚһλ _nop_(); Clk = 1; _nop_(); Clk = 0; DATI = 0; //ͨµÀ0µÄµÚ¶þλ _nop_(); Clk = 1; _nop_(); } else { Clk = 0; DATI = 1; //ͨµÀ1µÄµÚһλ _nop_(); Clk = 1; _nop_(); Clk = 0; DATI = 1; //ͨµÀ1µÄµÚ¶þλ _nop_(); Clk = 1; _nop_(); } Clk = 0; DATI = 1; for( i = 0;i < 8;i++ ) //¶Áȡǰ8λµÄÖµ { _nop_(); adval <<= 1; Clk = 1; _nop_(); Clk = 0; if (DATO) adval |= 0x01; else adval |= 0x00; } for (i = 0; i < 8; i++) //¶ÁÈ¡ºó8λµÄÖµ { test >>= 1; if (DATO) test |= 0x80; else test |= 0x00; _nop_(); Clk = 1; _nop_(); Clk = 0; } if (adval == test) //±È&frac12;Ïǰ8λÓëºó8λµÄÖµ£¬Èç¹û&sup2;»ÏàͬÉáÈ¥¡£ÈôÒ»Ö±³öÏÖÏÔʾΪÁ㣬Çë&frac12;«¸ÃÐÐÈ¥µô dat = test; nop_(); CS = 1; //ÊÍ·ÅADC0832 DATO = 1; Clk = 1; return dat; } void init() //³õʼ»¯º¯Êý { TMOD=0x01; //¹¤×÷·&frac12;Ê&frac12; TL0=0xb0; TH0=0x3c; //¸³³õÖµ£¨12MHz¾§ÕñµÄ50ms£© EA=1; //´ò¿ªÖжÏ×Ü¿ª¹Ø ET0=1; //´ò¿ªÖжÏÔÊÐí¿ª¹Ø TR0=1; //´ò¿ª¶¨Ê±Æ÷¿ª¹Ø } void main() //Ö÷º¯Êý { Init1602();//³õʼ»¯ÏÔʾ init(); //³õʼ»¯¶¨Ê±Æ÷ while(1) //&frac12;øÈëÑ­»· { for(m=0;m<50;m++) //¶Á50´ÎADÖµ sum = adc0832(0)+sum; //¶Áµ&frac12;µÄADÖµ£¬&frac12;«¶Áµ&frac12;µÄÊý¾ÝÀÛ¼Óµ&frac12;sum temp=sum/50; //Ìø³öÉÏÃæµÄforÑ­»·ºó£¬&frac12;«ÀÛ¼ÓµÄ×ÜÊý³ýÒÔ50µÃµ&frac12;Æ&frac12;¾ùÖµtemp sum=0; //Æ&frac12;¾ùÖµ¼ÆËãÍê³Éºó£¬&frac12;«×ÜÊýÇåÁã if(set==0) //Ö»ÓÐÔÚ·ÇÉèÖÃ״̬ʱ£¬ Display_1602(temp,WARNING); //&sup2;ÅË¢ÐÂÏÔʾʵʱŨ¶ÈÖµ if(temp>WARNING&&set==0) //·ÇÉèÖÃʱµ±Å¨¶ÈֵСÓÚ±¨¾¯ÖµÊ± { flag=0; //±¨¾¯±ê־λÖÃ0£¬&sup2;»±¨¾¯ } else if(temp<WARNING&&set==0) //·ÇÉèÖÃʱµ±Å¨¶ÈÖµ´óÓÚ±¨¾¯ÖµÊ± { flag=1; //±¨¾¯±ê־λÖÃ1 } Key(); //ɨÃè°´¼ü } } void Key() //°´¼üº¯Êý { if(Key1==0) //ÉèÖüü°´ÏÂʱ { while(Key1==0); //¼ì&sup2;â°´¼üÊÇ·ñÊÍ·Å FENG=0; //·äÃùÆ÷Ïì set++; //ÉèÖÃ״̬±êÖ¾¼Ó flag=0; //Í£Ö¹±¨¾¯ // san=1; //·çÉÈֹͣת¶¯£¨Ñ¡Å䣩 TR0=0; //¶¨Ê±Æ÷Í£Ö¹ } if(set==1) //ÉèÖÃʱ { write_com(0x38);//ÆÁÄ»³õʼ»¯ write_com(0x80+0x40+13);//Ñ¡Öб¨¾¯ÖµµÄλÖà write_com(0x0f);//´ò¿ªÏÔʾ ÎÞ¹â±ê ¹â±êÉÁ˸ write_com(0x06);//µ±¶Á»òдһ¸ö×Ö·ûÊÇÖ¸Õëºóһһλ FENG=1; //·äÃùÆ÷Í£Ö¹ÃùÏì } else if(set>=2) //ÔÙ°´Ò»ÏÂÉèÖüüʱ£¬Í˳öÉèÖà { set=0; //ÉèÖÃ״̬ÇåÁã write_com(0x38);//ÆÁÄ»³õʼ»¯ write_com(0x0c);//´ò¿ªÏÔʾ ÎÞ¹â±ê ÎÞ¹â±êÉÁ˸ FENG=1; //·äÃùÆ÷Í£Ö¹Ïì flag=1; //±¨¾¯±ê־λÖÃ1 TR0=1; //¶¨Ê±Æ÷¿ªÊ¼¼ÆÊ± } if(Key2==0&&set!=0) //µ±ÔÚÉèÖÃ״̬ʱ£¬°´Ï¼Ӽüʱ { while(Key2==0); //°´¼üÊÍ·Å FENG=0; //·äÃùÆ÷ÃùÏì WARNING++; //±¨¾¯ãÐÖµ¼Ó if(WARNING>=255)//ãÐÖµ×î´ó¼Óµ&frac12;255 WARNING=0; //ÇåÁã write_com(0x80+0x40+11); //ÔÚãÐÖµµÄλÖÃдÈëÉèÖõÄÊý¾Ý write_data('0'+WARNING/100); //ÏÔʾ°Ù루&frac12;«123³ýÒÔ100µÃµ&frac12;µÄÉÌÊÇ1£¬¾ÍÊǰÙλÊý¾Ý£©123ΪÀý×Ó write_data('0'+WARNING/10%10);//ÏÔʾʮ루&frac12;«123³ýÒÔ10µÃµ&frac12;ÉÌÊÇ12£¬&frac12;«12³ýÒÔ10µÄÓàÊý¾ÍÊÇʮ룩 write_data('0'+WARNING%10); //ÏÔʾ¸ö루&frac12;«123³ýÒÔ10µÄÓàÊý3¾ÍÊǸöλÊý¾Ý£©123ΪÀý×Ó write_com(0x80+0x40+13);//λÖà FENG=1;//·äÃùÆ÷Í£Ö¹ } if(Key3==0&&set!=0) //¼õ°´¼ü×¢ÊÍ&sup2;ο¼¼Ó°´¼ü&sup2;¿·Ö { while(Key3==0); FENG=0; WARNING--; if(WARNING<=0) WARNING=255; write_com(0x80+0x40+11); write_data('0'+WARNING/100); write_data('0'+WARNING/10%10); write_data('0'+WARNING%10); write_com(0x80+0x40+13);//λÖà FENG=1; } } void time1_int(void) interrupt 1 //¶¨Ê±Æ÷º¯Êý { uchar count; TL0=0xb0; TH0=0x3c; //ÖØÐ¸³³õÖµ count++; //¼ÆÊ±±äÁ¿¼Ó if(count==10)//¶¨Ê±Æ÷¶¨Ê±ÊÇ50ms£¬´Ë´¦¼ÆÊý10´Î£¬ÕýºÃÊÇ500ms£¬ÓÃÓÚ±¨¾¯Ê± µÆÁÁºÍ·äÃùÆ÷Ïì { if(flag==0) //±¨¾¯±ê־Ϊ0ʱ { LED_G=0; //Â̵ÆÁÁ LED_R=1; //ºìµÆÃð FENG=1; //·äÃùÆ÷&sup2;»Ïì // san=1; //·çÉÈ&sup2;»×ª£¨Ñ¡Å䣩 } if(flag==1) //±¨¾¯±ê־λΪ1ʱ { LED_G=1; //Â̵ÆÃð LED_R=0; //ºìµÆÁÁ FENG=0; //·äÃùÆ÷Ïì // san=0; //·çÉÈת¶¯£¨Ñ¡Å䣩 } } if(count==20) //¼ÆÊýµ&frac12;20ʱ£¬ÕýºÃÊÇ1000ms£¬¾ÍÊÇ1s£¬ÕâÀï¾ÍÊÇÈõÆÃ𣬷äÃùÆ÷&sup2;»Ï죬´Ó¶ø×ö³öÉÁ˸µÄЧ¹û { count=0; //¼Æµ&frac12;1sʱ£¬&frac12;«countÇåÁ㣬׼±¸ÖØÐ¼ÆÊý if(flag==0) { LED_G=1; LED_R=1; FENG=1; // san=1; //È«&sup2;¿¹Ø±Õ } if(flag==1) { LED_G=1; LED_R=1; FENG=1; // san=0; //±¨¾¯Öµ£¬·çÉÈÊÇһֱת¶¯µÄ£¨Ñ¡Å䣩 } } } 修改蜂鸣器报警时间为10s
最新发布
07-05
#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; // Ä£Ê&frac12;Ñ¡Ôñ£¬Ä¬ÈÏÊÇÆÕͨµÄ¿ØÖÆÄ£Ê&frac12; Motor_parameter MotorA, MotorB; // ×óÓÒµç»úÏà¹Ø±äÁ¿ int Servo_PWM = SERVO_INIT; // °¢¿ËÂü¶æ»úÏà¹Ø±äÁ¿ u8 Lidar_Detect = Lidar_Detect_ON; // µç´ÅÑ&sup2;ÏßÄ£Ê&frac12;À×´ï¼ì&sup2;âÕϰ­ÎĬÈÏ¿ªÆô float CCD_Move_X = 0.3; // CCDÑ&sup2;ÏßËÙ¶È float ELE_Move_X = 0.3; // µç´ÅÑ&sup2;ÏßËÙ¶È 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¶¨Ê±ÖжϿØÖƺ¯Êý Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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(); //Ä£Ê&frac12;Ò» 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(); } } //Ä£Ê&frac12;¶þ 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(); } } //Ä£Ê&frac12;Èý 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 º¯Êý¹¦ÄÜ£ºÊÖ»úÀ¶ÑÀ¿ØÖÆ Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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; // ǰ&frac12;ø 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 // °¢¿ËÂü&frac12;ṹС³µ×ª»»ÎªÇ°ÂÖתÏò&frac12;Ç¶È 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ÊÖ±ú¿ØÖÆ Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void PS2_Control(void) { int LY, RX; // ÊÖ±úADCµÄÖµ int Threshold = 20; // ãÐÖµ£¬ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷ static u8 Key1_Count = 0, Key2_Count = 0; // ÓÃÓÚ¿ØÖƶÁȡҡ¸ËµÄËÙ¶È // ת»¯Îª128µ&frac12;-128µÄÊýÖµ LY = -(PS2_LY - 128); // ×ó±ßYÖá¿ØÖÆÇ°&frac12;øºóÍË 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) // °¢¿ËÂü³µ×ªÏò¿ØÖÆ£¬Á¦¶È±íʾתÏò&frac12;Ç¶È 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) // µ÷&frac12;Ú°´¼ü·´Ó¦ËÙ¶È { PS2_KEY = 0; Key1_Count = 0; if ((RC_Velocity += X_Step) > MAX_RC_Velocity) // ǰ&frac12;ø×î´óËÙ¶È800mm/s RC_Velocity = MAX_RC_Velocity; if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷&frac12;ÚתÏòËÙ¶È { 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) // ·Ç°¢¿ËÂü³µ¿Éµ÷&frac12;ÚתÏòËÙ¶È { 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; // ¶ÁÈ¡µ&frac12;ÆäËû°´¼üÖØÐ¼ÆÊý Move_X = Move_X / 1000; Move_Z = -Move_Z; // ËÙ¶ÈMove_XתΪm/s } /************************************************************************** Function: Get_Velocity_From_Encoder Input : none Output : none º¯Êý¹¦ÄÜ£º¶ÁÈ¡±àÂëÆ÷ºÍת»»³ÉËÙ¶È Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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 // ¸ù¾Ý&sup2;»Í¬Ð¡³µÐͺžö¶¨±àÂëÆ÷ÊýÖµ¼«ÐÔ 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£¨Á&frac12;·Âö³å£©*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 º¯Êý¹¦ÄÜ£ºÔ˶¯Ñ§Äæ&frac12;â Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ // Ô˶¯Ñ§Äæ&frac12;⣬ÓÉxºÍyµÄËٶȵõ&frac12;±àÂëÆ÷µÄËÙ¶È,VxÊÇm/s,Vzµ¥Î»ÊǶÈ/s(&frac12;ǶÈÖÆ) // °¢¿ËÂü³µVzÊǶæ»úתÏòµÄ&frac12;ǶÈ(»¡¶ÈÖÆ) 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´ú±íÓÒǰÂÖתÏò&frac12;Ç¶È 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 // ǰÂÖתÏò&frac12;ǶÈÏÞ·ù(¶æ»ú¿ØÖÆÇ°ÂÖתÏò&frac12;ǶÈ)£¬µ¥Î»£ºrad AngleR = target_limit_float(AngleR, -0.49f, 0.32f); // Inverse kinematics //Ô˶¯Ñ§Äæ&frac12;â 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Öµ£¬¶æ»ú¿ØÖÆÇ°ÂÖתÏò&frac12;Ç¶È 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) // &sup2;îËÙС³µ { if (Vx < 0) Vz = -Vz; else Vz = Vz; // Inverse kinematics //Ô˶¯Ñ§Äæ&frac12;â 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 Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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) { // ÂË&sup2;¨£¬Ê¹Æð&sup2;&frac12;ºÍÍ£Ö¹ÉÔ΢Æ&frac12;»¬Ò»Ð© 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¸³Öµ Èë¿Ú&sup2;ÎÊý: IN£ºÊäÈë&sup2;ÎÊý 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 º¯Êý¹¦ÄÜ£ºÏÞ·ùº¯Êý Èë¿Ú&sup2;ÎÊý£º·ùÖµ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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 º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú Èë¿Ú&sup2;ÎÊý: ÎÞ ·µ»Ø Öµ£º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 º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯ÂË&sup2;¨ Èë¿Ú&sup2;ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂË&sup2;¨ºóµÄÊý¾Ý **************************************************************************/ 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 º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯ÂË&sup2;¨ Èë¿Ú&sup2;ÎÊý£ºÊý¾Ý ·µ»Ø Öµ£ºÂË&sup2;¨ºóµÄÊý¾Ý **************************************************************************/ 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 º¯Êý¹¦ÄÜ£ºÀ×´ï±ÜÕÏÄ£Ê&frac12; Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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¶Èµ&frac12;50¶Èת»¯Îª-50¶Èµ&frac12;50¶È if (Dataprocess[i].distance < 200) // ¼Ç¼СÓÚ200mmµÄµãµÄ¼ÆÊý distance_count++; } } } if (calculation_angle_cnt < 8) // СÓÚ8µã&sup2;»ÐèÒª±ÜÕÏ£¬È¥³ýһЩÔëµã { if ((Move_X += 0.1) >= Aovid_Speed) // ±ÜÕϵÄËÙ¶ÈÉ趨Ϊ260£¬Öð&frac12;¥Ôö¼Óµ&frac12;260¿ÉÉÔ΢Æ&frac12;»¬Ò»Ð© Move_X = Aovid_Speed; Move_Z = 0; // &sup2;»±ÜÕÏʱ&sup2;»ÐèҪתÍä } else // ÐèÒª±ÜÕÏ£¬¼òµ¥µØÅжÏÕϰ­Îï·&frac12;λ { if (Car_Num == Akm_Car) // °¢¿ËÂü³µÐÍÓжæ»ú£¬ÐèÒªÌØÊâ´¦Àí { if (distance_count > 8) // ¾àÀëСÓÚ±ÜÕ&frac12;¾àÀë Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËÙ¶È&frac12;µµ&frac12;µÍËÙ0.25 Move_X = Aovid_Speed * 0.5; if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ Move_Z = -Pi / 5; // ÿ´ÎתÍä&frac12;ǶÈΪPI/5£¬Ö±µ&frac12;100¶È·¶Î§ÄÚÎÞÕϰ­Îï¾ÍÍ£Ö¹ else // Æ«×ó Move_Z = Pi / 5; } } else { if (distance_count > 8) // СÓÚ±ÜÕ&frac12;¾àÀëµÄʱºò Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË else { if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËÙ¶È&frac12;µµ&frac12;µÍËÙ¶È0.15 Move_X = (Aovid_Speed * 0.5); if (angle_sum > 0) // Õϰ­ÎïÆ«ÓÒ { if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ&frac12;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¶È£¬Ö±µ&frac12;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 º¯Êý¹¦ÄÜ£ºÀ×´ï¸úËæÄ£Ê&frac12; Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ float angle1 = 0; // ¸úËæµÄ&frac12;Ç¶È u16 mini_distance1; void Lidar_Follow(void) { static u16 cnt = 0; int i; int calculation_angle_cnt = 0; static float angle = 0; // ¸úËæµÄ&frac12;Ç¶È static float last_angle = 0; // u16 mini_distance = 65535; static u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿ // ÐèÒªÕÒ³ö¸úËæµÄÄǸöµãµÄ&frac12;Ç¶È 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) // ×öÒ»¶¨Ïû¶¶£¬&sup2;¨¶¯´óÓÚ10¶ÈµÄÐèÒª×öÅÐ¶Ï { if (++data_count == 60) // Á¬Ðø60´Î&sup2;ɼ¯µ&frac12;µÄÖµ(300msºó)ºÍÉϴεıȴóÓÚ10¶È£¬´Ëʱ&sup2;ÅÊÇÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } } else // &sup2;¨¶¯Ð¡ÓÚ10¶ÈµÄ¿ÉÒÔÖ±&frac12;ÓÈÏΪÊÇÓÐЧֵ { data_count = 0; last_angle = angle; } if (calculation_angle_cnt < 6) // ÔÚ¸úËæ·¶Î§ÄڵĵãÉÙÓÚ6¸ö { if (cnt < 40) // Á¬Ðø¼ÆÊý³¬40´ÎûÓÐÒª¸úËæµÄµã£¬´Ëʱ&sup2;ÅÊÇ&sup2;»ÓøúËæ 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)) // °¢¿¨Âü³µÐÍ´¦Àí³µÍ·&sup2;»¶ÔןúËæÎÏ൱ÓÚºó³µÒ»Ñù£¬Ò»´Î&sup2;»¶Ô×¼£¬ÄǺóÍËÔÙÀ´¶Ô×¼ { 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; // &frac12;ǶÈ&sup2;î¾à¹ý´óÖ±&frac12;Ó¿ìËÙתÏò Move_X = 0; // &sup2;îËÙС³µºÍÂÄ´øÐ¡³µ¿ÉÒÔʵÏÖÔ­µØ×ª¶¯ } 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); } /************************************************************************** º¯Êý¹¦ÄÜ£ºÐ¡³µ×ßÖ±ÏßÄ£Ê&frac12; Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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 º¯Êý¹¦ÄÜ£º¼ÆËãС³µ¸÷ÂÖ×ÓµÄÖܳ¤ Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ 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 º¯Êý¹¦ÄÜ£º³¬Éù&sup2;¨¸úËæÄ£Ê&frac12; Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Ultrasonic_Follow(void) // ³¬Éù&sup2;¨¸úËæ£¬Ö»Äܵ¥·&frac12;Ïò¸úËæ { Move_Z = 0; Read_Distane(); // ¶ÁÈ¡³¬Éù&sup2;¨µÄ¾àÀë if (Distance1 < 200) // ¾àÀëСÓÚ200mm£¬Í˺ó { if ((Move_X -= 3) < -210) Move_X = -210; // ¸øÒ»210ºóÍËËÙ¶È } else if (Distance1 > 270 && Distance1 < 750) // ¾àÀëÔÚ270µ&frac12;750Ö®¼äÊÇÐèÒª¸úËæÇ°&frac12;ø { if ((Move_X += 3) > 210) // ËÙ¶ÈÖð&frac12;¥Ôö¼Ó£¬¸øÇ°&frac12;øËÙ¶È Move_X = 210; } else { if (Move_X > 0) { if ((Move_X -= 20) < 0) // ËÙ¶ÈÖð&frac12;¥¼õµ&frac12;0 Move_X = 0; } else { if ((Move_X += 20) > 0) // ËÙ¶ÈÖð&frac12;¥¼õµ&frac12;0 Move_X = 0; } } } /************************************************************************** Function: Get angle Input : way£ºThe algorithm of getting angle 1£ºDMP 2£ºkalman 3£ºComplementary filtering Output : none º¯Êý¹¦ÄÜ£º»ñÈ¡&frac12;Ç¶È Èë¿Ú&sup2;ÎÊý£ºway£º»ñÈ¡&frac12;ǶȵÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥&sup2;¹ÂË&sup2;¨ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Get_Angle(u8 way) { if (way == 1) // DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý&sup2;ɼ¯Öж϶ÁÈ¡£¬Ñϸñ×ñѭʱÐòÒªÇó { Read_DMP(); // ¶ÁÈ¡¼ÓËÙ¶È¡¢&frac12;ÇËÙ¶È¡¢Çã&frac12;Ç } 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; //Êý¾ÝÀàÐÍת»» Ò&sup2;¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»» // if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò&sup2;¿Éͨ¹ý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; // ¼ÆËãÇã&frac12;Ç£¬×ª»»µ¥Î»Îª¶È Accel_Angle_y = atan2(Accel_X, Accel_Z) * 180 / Pi; // ¼ÆËãÇã&frac12;Ç£¬×ª»»µ¥Î»Îª¶È Gyro_X = Gyro_X / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»£¬Á¿³Ì¡À500¡ã/s¶ÔÓ¦ÁéÃô¶È65.5£¬¿É&sup2;éÊÖ&sup2;á Gyro_Y = Gyro_Y / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»» if (way == 2) { Roll = -Kalman_Filter_x(Accel_Angle_x, Gyro_X); // ¿¨¶ûÂüÂË&sup2;¨ Pitch = -Kalman_Filter_y(Accel_Angle_y, Gyro_Y); } else if (way == 3) { Roll = -Complementary_Filter_x(Accel_Angle_x, Gyro_X); // »¥&sup2;¹ÂË&sup2;¨ Pitch = -Complementary_Filter_y(Accel_Angle_y, Gyro_Y); } } } /************************************************************************** Function: The remote control command of model aircraft is processed Input : none Output : none º¯Êý¹¦ÄÜ£º¶Ôº&frac12;Ä£Ò£¿Ø¿ØÖÆÃüÁî&frac12;øÐд¦Àí Èë¿Ú&sup2;ÎÊý£ºÎÞ ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Remote_Control(void) { // Data within 1 second after entering the model control mode will not be processed // ¶Ô&frac12;øÈëº&frac12;Ä£¿ØÖÆÄ£Ê&frac12;ºó1ÃëÄÚµÄÊý¾Ý&sup2;»´¦Àí 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. // ×óÒ¡¸Ëǰºó·&frac12;Ïò¡£¿ØÖÆÇ°&frac12;øºóÍË¡£ LX = Remoter_Ch2 - 1500; // //Left joystick left and right. Control left and right movement. // //×óÒ¡¸Ë×óÓÒ·&frac12;Ïò¡£¿ØÖÆ×óÓÒÒÆ¶¯¡£¡£ // LY=Remoter_Ch2-1500; // Right stick left and right. To control the rotation. // ÓÒÒ¡¸Ë×óÓÒ·&frac12;Ïò¡£¿ØÖÆ×Ôת¡£ 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 // ¶Ôº&frac12;Ä£Ò£¿Ø¿ØÖÆÃüÁî&frac12;øÐд¦Àí 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 // ¶Ô&frac12;øÈëº&frac12;Ä£¿ØÖÆÄ£Ê&frac12;ºó1ÃëÄÚµÄÊý¾Ý&sup2;»´¦Àí if (thrice > 0) Move_X = 0, Move_Z = 0, thrice--; // Control target value is obtained and kinematics analysis is performed // µÃµ&frac12;¿ØÖÆÄ¿±êÖµ£¬&frac12;øÐÐÔ˶¯Ñ§·ÖÎö // 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
#include "control.h" /************************************************************************** ×÷ÕߣºÆ&frac12;ºâС³µÖ®¼Ò ÎÒµÄÌÔ±¦Ð¡µê£ºhttp://shop114407458.taobao.com/ **************************************************************************/ #define T 0.156f #define L 0.1445f #define K 311.4f u8 Flag_Target; int Voltage_Temp,Voltage_Count,Voltage_All,sum; /************************************************************************** º¯Êý¹¦ÄÜ£ºÐ¡³µÔ˶¯ÊýѧģÐÍ Èë¿Ú&sup2;ÎÊý£ºËٶȺÍת&frac12;Ç ·µ»Ø Öµ£ºÎÞ **************************************************************************/ void Kinematic_Analysis(float velocity,float turn) { Target_A=(velocity+turn); Target_B=(velocity-turn); //ºóÂÖ&sup2;îËÙ } void SelfLocalization(void) //С³µ×Ô¶¨Î» { const double T_Sampling=0.005*2; //5ms*2 &sup2;ÉÑùÖÜÆÚ const double Encoder_Round=390*4; //±àÂëÆ÷ÏßÊý*4±¶ÆµÏ¸·Ö const double R=0.032, B=0.155; //³µÂÖ°ë¾¶£¬Â־࣬m //Ö±&frac12;Ó¿ÉÓõÄÈ«¾Ö±äÁ¿ //extern int Encoder_Left,Encoder_Right; //×óÓÒ±àÂëÆ÷µÄÂö³å¼ÆÊý //extern float X_me,Y_me,theta_me; //С³µÎ»ÖúÍ×Ë̬ //extern float V_me,W_me; //С³µÏßËٶȺÍ&frac12;ÇËÙ¶È-±àÂëÆ÷¹À¼ÆÖµ //&sup2;¹È«ÈçÏ´úÂ룬¼ÆËãС³µÏßËٶȺÍ&frac12;ÇËÙ¶È£¬ÒÔ¼°Ð¡³µÎ»ÖúÍ×Ë̬ V_me=; W_me=?; X_me=?; Y_me=?; theta_me=; //¾­¹ý¼ÆËãµÄ&frac12;ǶÈÒ»¶¨Òª¹æÕûµ&frac12;(-PI, PI] theta_me=£¿; } void PathPlanning(void) //·¾¶¹æ»® { static int Flag_Start=0; double X_des=3, Y_des=2, Angle_des=0; //Ä¿±êλÖúÍ×Ë̬ const double Kd=20,Ka=-7,Kb=0; double ds,da; if(Flag_Stop==1) { Flag_Start=0; Velocity=0,Turn=0; } else Flag_Start++; if(Flag_Start<3*1000/10) return; //µÈ´ý3sºóʵÑé×Ô¶¯¿ªÊ¼ //¸ù¾ÝÄ¿±êλÖ㨺Í×Ë̬£©£¬ÒÔ¼°µ±Ç°»úÆ÷È˵ÄλÖúÍ×Ë̬£¬¹æ»®³ö»úÆ÷ÈËÏÂһʱ¿ÌµÄÏßËٶȺÍ&frac12;ÇËÙ¶È //Ö±&frac12;Ó¿ÉÓõÄÈ«¾Ö±äÁ¿ //extern float X_me,Y_me,theta_me; //С³µÎ»ÖúÍ×Ë̬ //extern float V_me,W_me; //С³µÏßËٶȺÍ&frac12;ÇËÙ¶È-±àÂëÆ÷¹À¼ÆÖµ //extern float Velocity,Turn; //VelocityΪҪ¹æ»®³öµÄÏßËÙ¶È,TurnΪҪ¹æ»®³öµÄ&frac12;ÇËÙ¶È£¨ÊÜÓ&sup2;¼þʵÏÖµÄÒªÇó£¬Turn˳ʱÕëΪÕý£© //&sup2;¹È«ÈçÏ´úÂë // Velocity=£¿;
03-25
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值