matlab 机器人避障程序

本人使用一个原始避障程序，里面有相应的论文，我进行了改写

/*
 * Robotver3mod.c - Program that runs the Engs 147 robot
 *
 * Dan Magoon Jennifer Jain Jonah Sternthal Joe Leonor
 *
 * Spring 2018
 * June 4th 2018
 */

#memmap xmem    //Force compilation to extended memory when the program gets large
#use BL26XX.lib   //Import Library for BL2600

#define DATA   0x00ff
#define RD     8
#define WR     9
#define CS1    10
#define CS2    11
#define YX     12
#define CD     13
#define BP_RESET        0X01        // reset byte pointer
#define EFLAG_RESET     0X86        // reset E bit of flag register
#define CNT             0x01       // access control register
#define DAT             0x00       // access data register
#define BP_RESETB       0X81        // reset byte pointer （x and y）
#define CLOCK_DATA      2        // FCK frequency divider
#define CLOCK_SETUP     0X98     // transfer PR0 to PSC （x and y）
#define INPUT_SETUP        0XC7     // enable inputs A and B （x and y）
                                   // set indexing function to load OL and
                                   // A and B enable gate TWP 4/2/12
#define QUAD_X1            0XA8     // quadrature multiplier to 1 （x and y）
#define QUAD_X2            0XB0     // quadrature multiplier to 2 （x and y）
#define QUAD_X4            0XB8     // quadrature multiplier to 4 （x and y）
#define CNTR_RESET         0X02     // reset counter
#define CNTR_RESETB        0X82     // reset counter （x and y）
#define TRSFRPR_CNTR       0X08     // transfer preset register to counter
#define TRSFRCNTR_OL       0X90     // transfer CNTR to OL （x and y）
#define XYIDR_SETUP        0XE1     // set index cntrl register to active low
                                   // input. index input is pulled up to +5V
                                   // in hardware to disable index functions
                                   // TWP 4/2/2012
#define HI_Z_STATE  0xFF

/**************** CONSTANT DECLERATIONS ****************/
#define LEFT        1     // left wheel motor encoder ID
#define RIGHT       2     // right wheel motor encoder ID
#define TOP         3     // IR sensor motor encoder ID

// Robot Specifications
#define WHEELbase 15.4     // cm wheelbase of bot
#define STEP 0.0160     //cm per encoder tick of wheel

// Mathematical Constants
#define PIE 3.14159265
#define HALFPIE 1.57079633

// Mapping Constants
#define BOXES 77     // initialize grid size to BOXES x BOXES
#define MAPSIZE 5929     // = BOXES*BOXES
#define BOXLENGTH 5     // cm length per box on grid

#define MAPFILTER 100     // points-per-box required for an obstacle to be live on map
#define REQWEIGHT 4     // points-per-box threashold for an obstacle and neighbors
#define DEWEIGHT 0     // amount below mapfilter to push down values of neighborless boxes
#define NEIGHBORS 2     // amount of neighbors required to be obstacle
#define LOOKAHEAD 10     // cm distance to lead bot in calculations

// Driving Speeds
#define VHIGH 1     // max v

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       7047  2019-02-20 00:43  复杂1.txt

     文件       5772  2019-02-18 15:54  开口.txt

     文件       5772  2019-02-18 16:03  一.txt

     文件      10959  2019-02-21 21:46  原始代码.txt

    .......       333  2018-06-06 02:57  AutonomousRobotNavigation-master\AutonomousRobotNavigation-master\README.md

    .......   7950168  2018-06-06 02:57  AutonomousRobotNavigation-master\AutonomousRobotNavigation-master\Reportpdf.pdf

     文件      30863  2018-06-06 02:57  AutonomousRobotNavigation-master\AutonomousRobotNavigation-master\Robotver5.c

     文件     139776  2019-02-18 20:19  图3.doc

     文件       6163  2019-02-19 13:37  四道杠.txt

     文件       5772  2019-02-18 16:17  二.txt

     文件       7427  2019-02-20 01:04  复杂2.txt

     目录          0  2018-06-06 02:57  AutonomousRobotNavigation-master\AutonomousRobotNavigation-master

     目录          0  2019-02-21 21:51  AutonomousRobotNavigation-master

----------- ---------  ---------- -----  ----

              8170052                    13