[工学]武科大首安二队技术报告飞思卡尔智能车.doc

第一届“飞思卡尔”杯全国大学生智能汽车邀请赛技 术 报 告附件A 程序源代码附件B 基于路径识别的智能车舵机控制学 校： 武汉科技大学队伍名称： 武科大首安二队参赛队员： 刘 宇 吴斌华 杨 明带队教师： 黄卫华 I关于技术报告和研究论文使用授权的说明 本人完全了解第一届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名：刘 宇 吴斌华 杨 明带队教师签名： 黄卫华 日 期：2006.8.9 目录第一章 引言1第二章 智能车系统设计22.1 总体设计22.2 硬件设计22.2.1 主控制器模块32.2.2 电源模块32.2.3 路径识别模块42.2.4 车速检测模块52.2.5 舵机控制模块72.2.6 直流驱动电机控制模块92.3 软件设计10第三章 控制算法设计123.1 路径搜索算法123.2 PI控制算法133.3 强行加减速控制算法14第四章 实验验证16第五章 结论18参考文献19附录A 程序源代码20附录B 研究论文29 第一章 引言根据第一届“飞思卡尔”杯全国大学生智能汽车邀请赛比赛规则的要求,本队已经完成了智能车系统的设计、制作、安装和调试。该智能车的设计思路是：首先，通过路径识别传感器采集路径信息，经Freescale MC9S12DG128单片机处理输出控制信号控制舵机；同时通过旋转编码器对车速进行采样，送入单片机作为车速控制的参数；再通过PI控制算法完成车速与路径识别传感器的信息的相互协作，实现智能车快速寻迹的目的。文4介绍了一种利用红外反射式传感器实现小车自动寻迹导航的设计与实现。使用红外反射式传感器感知与地面颜色有较大反差的引导线，从而实现自主式寻迹。文6重点讨论了智能小车多传感器数据融合结构和融合算法，其中数据融合结构采用混合式，融合算法采用贝叶斯推理法。文10介绍了一种基于神经网络的PID速度控制器的实现方法。本文所述智能车寻迹系统采用红外反射式传感器识别路径上的黑线，通过加长舵机转臂驱动前轮转向，使用传统的PI控制器实现直流电机的速度控制。该系统以飞思卡尔公司的16位微处理器MC9S12DG128为控制核心，主要由电源模块、核心控制模块、路径识别模块、车速检测模块、舵机控制模块和直流驱动电机控制模块组成。为了使智能车更加快速、平稳、准确地行驶，本系统将路径识别，车速的快速检测与响应，舵机和直流驱动电机的正确控制紧密地结合在一起。技术报告共分为五个部分：第一章为引言；第二章是智能车系统设计，介绍智能车总体设计和软、硬件设计及实现方案；第三章是控制算法设计，详述智能车软件实现；第四章是实验验证，阐述采用PI控制器控制的实验效果；第五章是总结。15 第二章 智能车系统设计第二章 智能车系统设计2.1 总体设计本系统中，智能车总重量1100g，长388mm，宽206mm，高104mm；功耗为810w，电容总量、传感器个数、直流驱动电机个数、舵机个数都满足组委会的要求。模型车机械部分改造、安装完成后整体效果如图2.1所示，其中包括舵机转臂的加长及舵机安装、寻迹传感器及车速传感器的安装、系统电路板的固定。以上改装及安装均在组委会允许的规则范围内进行。图2.1 智能车系统整体效果图智能车寻迹系统利用红外反射式传感器实现其路径识别功能。视觉系统反馈的信息经处理后最终作用于加长转臂的舵机，舵机将帮助智能车转向。PI控制器用于对车速的调节与控制，从而控制系统的加速与减速。通过光电旋转编码器采集系统实时速度，作为PI控制的参数。而各个功能的实现与协作是通过单片机来完成的。总体控制方案分为角度控制和速度控制。该系统速度给定根据路径状况分为三档，分别为直道行驶速度给定值、进弯速度给定值和弯道行驶速度给定值。路径识别系统可以判断智能车处于直道状态、进弯状态还是弯道状态，从而告知智能车系统转向，同时通过速度给定值与当前反馈值的比较决定加速还是减速，并使用PI控制器迅速将车速调节至给定值。2.2 硬件设计本系统硬件部分由电源模块、主控制器模块、路径识别模块、车速检测模块、舵机控制模块和直流驱动电机控制模块组成，系统硬件结构如图2.2所示。图2.2 系统硬件结构2.2.1 主控制器模块本系统中，主控制器模块采用主委会提供的MC9S12EVKX电路板。飞思卡尔公司的16位微处理器MC9S12DG128主要特点就是功能高度的集中，并且易于扩展。该单片机的运算能力强，自由度大，软件编程灵活。其配套的编程环境支持C语言程序设计、汇编语言程序设计以及C语言与汇编语言的混合程序设计，极大地方便了用户的使用，提高了系统开发效率。另外，该芯片还内置在线仿真及编程接口，可以实现在线调试，这大大简化了系统开发和调试的复杂度。2.2.2 电源模块为满足智能车各部分正常工作的需要，本系统采用组委会提供的7.2V 2000mAh Ni-cd蓄电池作为能量源。通过外围电路的整定，电源被分配给各个模块。稳压电路如图2.3所示：图2.3 稳压电路图由于单片机需要5V电源才能工作，所以稳压电路采用7805集成稳压块，该集成电路输出电压稳定，加之直流供电，不需要复杂的滤波系统。所以7805可以满足系统要求。智能车系统中电源分配如下：考虑到单片机系统的核心作用，主控制器模块使用单独的稳压电路进行供电，从而保证控制核心的稳定性；而系统中的路径识别电路（如图2.3所示FOR_IR）、车速检测电路及直流驱动电机的控制电路都采用另一相同的稳压电路供电（如图2.3所示FOR_MCU）；鉴于是舵机是一个时滞性器件，在规则允许下，本队将电源正极串接一个二极管后直接加在舵机上，这样可以进一步提升它的响应速度；电机驱动芯片MC33886的电源由电池直接供给，它的输出将直接供给直流驱动电机。考虑到直流驱动电机运行时，稳压电路要承受12A的电流，稳压芯片都加装了散热片，保障系统的供电。2.2.3 路径识别模块本系统的路径识别模块采用收发一体的红外反射式光电传感器JY043作为路径的基本检测元件。该器件对黑白反应灵敏，几乎不受自然光线影响，反馈的电信号稳定，硬件电路简单且易于实现。图2.4为其硬件原理图。如图所示，发射管1串接一100200的电阻，向反射平面（跑道）发出红外光，如果红外光被黑色路径吸收，则LM393比较器的2号脚将呈现底电平，通过与3号脚设定的参考电平比较，产生高电平输出；相反则产生低电平输出。图2.4 路径检测电路图路径识别模块的安装如图2.5所示，11个JY043按“一”字形排列在20cm长的电路板上。由于大赛跑道的黑色轨迹的宽度为2.5cm，且JY043具有聚光性（非漫反射型），因此选择每个光电管的相互间隔为2cm，依次排开。如此可能出现两个光电管同时检测到黑线，从而增加了光电管检测区间，相应地提高了识别精度。整个路径识别模块电路板安装在车头的两个金属支架上，支架上相隔1cm的孔，可以电路板在车前7.5、8.5、9.5、10.5cm处安装，以应对不同的赛道。电路板安装越靠前，则智能车的预瞄性能越强，检测连续弯道的效果突出；越靠后安装，智能车的直道稳定性越好。因此“一”字形的排布以及这种安装可以应对灵活多变的跑道。图2.5 光电传感器排布图本系统的路径识别电路是开关量输出，因此路径信息可以通过简单电平分析得到。参考本次比赛赛道说明，系统中将十一个光电管的检测范围划分成21个区间，所以本系统对路径的识别共有22种电平状态。由此可以简化软件设计，从而缩减路径判断的时间，进而迅速控制舵机转向。2.2.4 车速检测模块本系统车速检测模块采用韩国Autonics公司的E30S-360-3-2型旋转编码器作为车速检测器件。该旋转编码器不仅硬件电路简单，而且信号采集速度快，360线的精度足以满足PI控制算法调节的需要。图2.6为车速检测模块硬件电路图。旋转编码器通过旋转可以从输出口获得脉冲信号。图2.6 车速检测模块电路图E30S-360-3-2型旋转编码器为NPN型集电极开路输出，所以它的输出需要上拉一电阻到电源。本系统将旋转编码器的A相输出接一10k的上拉电阻到5V电源，保证其有效输出。旋转编码器输出的脉冲信号通过PT0口传输给MC9S12DG128单片机，单片机的8位输入脉冲累加模式将对旋转编码器的输出脉冲信号进行累加计数，计数的数值可以反映车速。车速检测模块安装如图2.7所示：图2.7 旋转编码器的安装图在旋转编码器的中轴上安装有一直径为4cm的齿轮1（齿数为76），其咬合在与后轮同轴的传动齿轮2（齿数为76）上。当直流驱动电机驱动后轮旋转时，齿轮2和齿轮1将驱动旋转编码器转动产生脉冲。因为齿轮1、齿轮2的齿数比为1：1，所以智能车车轮转动一圈即可以用360个脉冲表示。此时只需要测量一定时间周期内累加器的数值就能准确地计算出车速。例如：在时间周期内累计个脉冲，轮子周长为，则车速为。由于程序设计中定时器周期是人为设定地，因此为了减少程序运算量，本系统直接用累加值来表示车速。2.2.5 舵机控制模块本系统中，舵机控制模块的控制对象是组委会提供的SANWA SRM102型舵机，该舵机可以输出力矩驱动智能车转向。图2.8为舵机硬件电路，其中控制信号线与MC9S12DG128的PWM1相连。图2.8舵机接线图智能车在行驶过程中，舵机的响应时间对系统的稳定性尤为重要，如果舵机足够灵敏，那么智能车的行驶轨迹将非常平滑。但主办方提供的舵机响应速度有限，尤其是在寻迹连续S型缓弯时舵机响应有滞后。为了提高舵机的响应速度，本对采取了三种方案：(1)将舵机电源线串接一个二极管后直接接在电池正极，这样可以提高舵机其中电机的转速，从而提高响应速度。图2.9 舵机转臂加长结构分析图图2.10舵机安装图舵机转臂加长结构分析图(2)将舵机在原位置上架高2.5cm，并使转臂加长至3.5cm。舵机自身硬件特性决定：在给定电压一定时，空载和带载时的角速度分别保持恒值，而线速度，正比于转臂的长度。当舵机所需转动幅度一定时，长转臂要比短转臂转动的机械角要小，即响应更快。如图2.9所示，对于转臂1和2，当且转动相同的位移时，转角。因此对于相同的角速度，可得转臂响应时间。显然利用舵机的转距余量可以提高其响应速度。图2.10所示为舵机控制模块的安装图，对于加长转臂后的舵机的控制方法可以参看附录B。(3)合并两个8位PWM寄存器为一个16位PWM寄存器，将舵机的PWM控制周期放大至2000，从而细化PWM控制量，使转臂变化更加的均匀、细腻。2.2.6 直流驱动电机控制模块本系统中，直流驱动电机控制模块由组委会提供的RS-380SH型直流电机、功率驱动芯片ULN2003及电机驱动芯片MC33886及MC9S12DG128微处理器组成。图2.11为电路ULN2003驱动单元。功率驱动芯片ULN2003为单片高电流增益双极型大功率高速集成电路，由7组达林顿晶体管阵列和相应的电阻网络以及钳位二极管网络构成，可同时驱动7组大电流负载。本系统使用了其中的2组用于增强单片机输出的PWM信号驱动能力。图2.11 ULN2003驱动单元图2.12为直流驱动电机硬件控制电路图。其中电机驱动芯片MC33886是单片集成的H桥元件，其适用于驱动小马力直流电机，并且有单桥和双桥两种控制方式。D1、D2为使能端，IN1、IN2为PWM信号控制输入端，OUT1、OUT2为MC33886输出。由于智能车进弯时的速度给定要小于直道速度给定，所以当智能车从直道高速进弯时需要紧急降速来保证系统的稳定。当电机正转时给其以反向制动力矩的降速效果肯定要优于给占空比为零的PWM信号的效果。因此本系统选择了MC33886的全桥工作方式。图2.12 直流驱动电机硬件控制电路图MC33886的输出OUT1、OUT2分别与RS-380SH的正负极相连，IN1和IN2分别与MC9S12DG128的PWM5和PWM3相连。当PI控制器计算值为负时，令PWM5输出的PWM信号占空比为零，PWM3输出的PWM信号占空比与计算值的绝对值相同。并且计算值越负，OUT2的电平就越高于OUT1，电机为反转趋势，智能车减速；反之，当PI控制器计算值为正时，PWM3输出的PWM信号占空比为零，PWM5输出的PWM信号占空比与计算值的绝对值相同。同理，计算值越大，OUT1的电平越高于OUT2，电机呈正转趋势，智能车加速行驶。另外，由于直流驱动电机在PI控制器的作用下需要不停的加速与制动，这样会使电机周围的磁场频繁变化，进而干扰单片机的正常运行，因此本系统中采用铝皮将电机包裹，对其磁场进行屏蔽。2.3 软件设计本系统的控制方案是根据路径识别模块和车速检测模块获得的路径和车速的当前信息，控制舵机和直流驱动电机相应的调整智能车的行驶方向和速度。最终目的是当智能车在直道行驶时，方向迅速保持稳定、不颤动，同时速度迅速升到设定的最大值并稳定；当智能车由直道高速进入弯道时，方向和速度根据弯道的曲率迅速做出相应的改变，原则是弯道曲率越大则方向变化角度越大，速度下降越低；而当智能车遇到十字交叉路段或是脱离轨迹等特殊情况时，智能车保持与上次正常情况一致的方向，速度则相应的降低。首先通过路径识别模块的11个“一”字形排列的光电传感器检测轨迹黑线的当前位置。然后根据检测结果判断智能车与轨迹偏离的情况。若被正中间的光电传感器检测到黑线，就表示智能车未偏离轨迹，则控制舵机使转向轮不偏转，同时控制驱动电机使速度上升到设定的最大速度。若被左（右）边的光电传感器检测到黑线，就表示智能车向右（左）偏离轨迹。越被靠左（右）的传感器检测到，表示智能车向右（左）偏离轨迹的程度越大。控制舵机使转向轮向左（右）偏转，偏离程度越大，则偏转角度越大。同时根据车速检测模块检测的当前速度，控制直流驱动电机使速度下降。当角度偏离程度越大且当前速度越大时，转速下降越大。若没有光电传感器检测到黑线或是其它的检测结果，就表示智能车脱离轨迹或是遇到十字交叉路段，则控制舵机使转向轮保持偏转角度不变，同时控制直流驱动电机使速度下降到速度最小设定值。第三章 控制算法设计本智能车系统的软件设计基于Metrowerks CodeWarrior CW12 V3.1编程环境，使用C语言实现。整个系统软件开发、制作、安装、调试都是在此环境下实现的。单片机系统需要接收路径识别电路的信号、车速传感器的信号，采用某种路径搜索算法进行寻线判断，进而控制舵机和直流驱动电机的工作。系统程序流程图如图3.1所示。图3.1 系统程序流程图3.1 路径搜索算法智能模型车的路径搜索算法（Line Searching Algorithm）是智能车设计的关键部分，智能车设计的大部分工作都是围绕它来展开的，路径搜索算法的好坏直接决定着整个系统控制效果的好坏.本系统路径搜索算法采用简单的switch语句，根据光电传感器的检测信号判断舵机的偏转角度，其程序流程图如图3.2所示：图3.2 路径检测模块流程图3.2 PI控制算法本系统转速控制是基于传统的PI控制算法的，其中PI控制算法的计算公式如下： 公式（3.1） 公式（3.2）式（3.1）和式（3.2）中, MVn,、MVn-1分别为当前和上次操作量；MVn为当前操作量微分；en、en-1分别为当前、上次偏差；P、I分别为PI控制器的比例系数、积分常数。速度的反馈值由旋转编码器测得，速度的设定值与反馈值的差即为转速的偏差，当前转速的偏差与上次转速的偏差就是转速变化率，将此转速偏差与变化率代入公式3.1和3.2计算后,可得当前电机的PWM控制信号输出值。3.3 强行加减速控制算法为了达到良好的控制效果，本系统在传统PI控制算法的基础上加入了参数自整定控制环节，即在系统运行过程中，PI控制器参数值不是恒定的，而是根据当前系统的运行状态给出不同的参数值，以适应不同的路径情况。当智能车在直道行驶时，P，I值比较适中，使系统能恒速运行；而当智能车以高速进入弧度较大的弯道时，电机的转速较大，而转速的设定值较小，因此转速的偏差较大，由PI控制器计算得出的电机PWM信号较小，要求电机迅速降速，此时只采用P控制，即令I=0，使电机能迅速的降速，而仅由PI控制器控制的降速效果不理想，为此本系统使用电机反转来强行降速，当PI控制器的计算值小于某一个值时，使电机反转，起到刹车的作用，迅速降速。 当智能车以较低转速由弧度较大的弯道进入直道时，转速的偏差也较大，要求电机能迅速的加速，同样的，仅由PI控制器控制的加速效果不理想，本系统采用强行加速的方法使电机的转速能迅速的提升起来，即当PI控制器的输出值达到某一个较大的值时，程序就认为当前要迅速加速，则以最大的PWM信号给电机加速，直到转速超调，然后再由PI控制器作进一步的调整，直到转速达到设定值。 本系统PI控制器参数是通过理论推导和实验数据综合后获得的。转速用0.01s内光电码盘检测到的脉冲数来表示，直道上转速的偏差值在-5至+5的范围内，偏差比较小，故将P值设定为100，为消除残差，使直道转速稳定，将I值设定为0.7；当偏差的绝对值大于5，则表明小车已进入弯道，为保证小车能迅速的降速，将P值设定为160，I值设定为0.经PI控制器运算并对计算值限幅后，如果计算值u_s<200,，则表明需反转降速，如果u_s>990,则表明需以最大速度加速，否则，如果u_s在200到990的范围内时，则直接将此计算值赋给PWM占空比寄存器PWMDTY01。详细代码见附录A。程序流程图如图3.3所示。图3.3 强行加、减速PI控制器流程图 第一届全国大学生智能汽车邀请赛技术报告第四章 实验验证在智能车的行进过程中，当智能车从长直道进入连续弯道时，极容易冲出跑道，跑道图如图4.1中的粗曲线所示。因为在长直道上智能车的转速很高，从直道进入弯道时，弯道的曲率不是很大，此时转速的设定值也较大，转速不会有太大的降落，加之舵机的响应时间的限制，智能车很容易冲出跑道。所以从长直道进入连续S型缓弯时舵机的转向控制是很难的。本系统所采用的角度控制算法能减小舵机转角的滞后，使智能车在连续S型缓弯中也能较好的寻径。图4.1是智能车从长直道进入连续弯道时的效果图，其中粗黑线为跑道轨迹，细黑线为智能车实际运行轨迹。整个测试跑道长度为350cm，由图可知，在110cm-140cm，160cm-200cm,210cm-250cm,260cm-300cm区域内，舵机转角有所滞后，不过滞后程度在智能车行进的误差允许范围内。图4.1 小车过弯效果图对智能车采用了强行加减速后，其控制效果比单纯的PI控制效果更加明显，下面以加速减速过程为例，加速过程转速初始值为0，设定值为50，减速过程转速初始值为50，设定值为30，每隔0.01s对转速进行采样，对比强行加减速与PI控制的控制效果，可以看出，采用强行加（减）速后，智能车速度的提升（降落）要比采用PI控制快得多，调节时间要比PI控制的调节时间短，实验过程中采集到的数据如表4.1和表4.2所示：表4.1 强行加速与PI控制效果对比0.010.020.030.040.050.060.070.080.090.1强行加速7193041505348505150PI控制1102132435053484545表4.2 强行减速与PI控制效果对比0.010.020.030.040.050.060.070.080.090.1强行减速49392720293229303130PI控制49444036332927293131由表中数据可知，采用强行加减速控制的加减速效果明显要比采用PI控制的控制效果要好，其唯一的缺点是超调量大，不过超调对于需要急剧加减速的小车系统来说，是可以接受的。37第五章 结论本文基于Freescale MCS12DG128B主控芯片设计了智能车控制系统，实现了智能车自动寻径的功能；并且根据路径信息，智能车可以实现自动加、减速的功能。本文具体的工作如下：(1) 在硬件上，采用MC9S12DG128B为控制核心，实现智能车单片机系统。该系统由电源模块、路径识别模块、车速检测模块、舵机控制模块、直流驱动电机控制模块共同协调工作；(2) 在控制算法上，采用路径搜索算法和PI控制算法实现对智能车的舵机转角和电机转速控制。(3) 在PI控制器的基础上加入强行加、减速功能，使车速迅速达到设定值后，智能车由PI控制器进行微调，使转速的调节即稳定又精确。本文将PI控制算法应用在智能车控制系统中，并对传统的PI控制器进行了相应调整，使智能车在行驶过程中能准确的寻径，及时的加减速以适应不同的路况。该系统还完成对加长转臂舵机的控制，并实现了转向伺服电机与车速的配合控制。使得智能车能根据路径的不同迅速地实现加减速。实验结果表明，该智能车系统响应快，动态性能良好，整体控制性能良好。参考文献1 Chen Shuwang，Wang mingshi. Leakage Detection of Pipelines UnderGround by Using Infrared DetectorC. In：Progress in Safety Science and Technology. Vol.4，Proceedings of the 2004 International Symposium on Safety Science and Technology Shanghai，China，2004，4：1433-1435.2 谢自美.电子线路设计·实验·测试M.武汉：华中科技大学出版社，2000.3 邵贝贝.单片机嵌入式应用的在线开发方法M.北京：清华大学出版社，2004.4 吴建平，殷战国，等.红外反射式传感器再自主式寻迹小车导航中的应用J.中国测试技术，2004，30（6）：21-23.5 徐友春，王荣本，李兵，等.一种机器视觉导航的智能车辆转向控制模型设计J.中国公路学报，2001，14（3）：96-100.6 陈懂，等.智能小车的多传感器数据融合J.现代电子技术，2005，6：3-5.7 吴卫国，蒋平，陈辉堂，等.移动小车轨迹跟踪的力矩控制J.控制与决策，1999，14（2）：177-180.8 秦娟英，关军明，王荣铸，等.参数自整定模糊PID直流电机调速系统研究J.机电设备，2004，2：38-41.9 程启明，黄林.船舶操纵的自整定PID参数模糊控制器研究J.船舶工程，1997，4：45-47.10 缪欣，袁艳.神经网络PID速度控制器的研究.计算机技术与自动化，1999，（12）：33-35.附录A 程序源代码#include <hidef.h> /* common defines and macros */#include <mc9s12dg128.h> /* derivative information */#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"#define Speed1 40#define Speed2 30 #define Speed3 50int angle_feedback,LastAngle,interval = 625;int speed_track500,ee500,uu500,ym500,i,nn=0;int speed_enactment,speed_feedback,e_s,u_s,NowAngle;int ek,ek1,dek,PB_Watch,tmmp,tmp,tmmmp;float p_s,i_s,uk,temp,intvl;void speed_sample(void);void angle_sample(void); float PI(int e,float P,float I);void main(void) COPCTL = 0b00000101; DDRH = 0x00; DDRA = 0x00; DDRB = 0xFF; EnableInterrupts; PWME = 0x00; PWMPRCLK = 0; PWMSCLA = 40; PWMCLK_PCLK1 = 1; PWMCLK_PCLK5 = 0; PWMPOL = 0xff; PWMCAE = 0x00; PWMCTL_CON01 = 1; PWMCTL_CON23 = 1; PWMCTL_CON45 = 1; PWMPER01 = 2000; PWMPER23 = 1000; PWMPER45 = 1000; PWMDTY01 = 156; /angle ini PWMDTY23 = 0; PWMDTY45 = 990; /speed ini PWME = 0xff; TSCR2_PR = 7; /bus/128 TIOS = 0b00000100; /output compare TCTL2 = 0b00010000; /input capture TCTL4 = 0b00001010; PBCTL_PBEN =1; TIE_C0I = 0; /interrupt disable TIE_C1I = 0; /interrupt disable TIE_C2I = 1; /interrupt able TC2 = 625; TFLG1_C2F = 1; ICPAR_PA0EN = 1; PACTL_PAI = 1; PACTL_PAOVI = 1; PACTL_PEDGE = 1; PACTL_PAEN = 1; TSCR1_TEN = 1; /start Timer speed_enactment = Speed3;/40 p_s = 160; /150 i_s = 0; for(;) angle_sample(); PORTB = PB_Watch; ARMCOP = 0x55;ARMCOP = 0xaa; /* wait forever */void angle_sample(void) tmmp = (PTH&0x01); tmmmp = PTH; tmp = (tmmmp>>1); tmmp = (tmmp<<10); angle_feedback = PORTA&0x07; angle_feedback = (angle_feedback<<7) ; angle_feedback = (angle_feedback + tmp); angle_feedback = (angle_feedback + tmmp); angle_feedback = (angle_feedback&0x07FF); PB_Watch = (angle_feedback>>3); switch(angle_feedback) case 0b10000000000: NowAngle = 10; PWMDTY01 = 136; speed_enactment = Speed1; break; case 0b11000000000: NowAngle = 9; PWMDTY01 = 138; speed_enactment = Speed1; break; case 0b01000000000: /left NowAngle = 8; PWMDTY01 = 142; speed_enactment = Speed2; break; / case 0b01100000000: NowAngle = 7; if(LastAngle>NowAngle) PWMDTY01 = 145; else PWMDTY01 = 144; speed_enactment = Speed2; break; case 0b00100000000: /3 NowAngle = 6; if(LastAngle>NowAngle) PWMDTY01 = 148; else PWMDTY01 = 146; speed_enactment = Speed2; break; case 0b00110000000: NowAngle = 5; if(LastAngle>NowAngle) PWMDTY01 = 151; else PWMDTY01 = 149; speed_enactment = Speed3; break; / case 0b00010000000: NowAngle = 4; /2 if(LastAngle>NowAngle) PWMDTY01 = 153; else PWMDTY01 = 151; speed_enactment = Speed3; break; case 0b00011000000: NowAngle = 3; if(LastAngle>NowAngle) PWMDTY01 = 153; else PWMDTY01 = 151; speed_enactment = Speed3; break; case 0b00001000000: /1 NowAngle = 2; if(LastAngle>NowAngle) PWMDTY01 = 155; else PWMDTY01 = 153; speed_enactment = Speed3; break; case 0b00001100000: NowAngle = 1; if(LastAngle>NowAngle) PWMDTY01 = 155; else PWMDTY01 = 153; speed_enactment = Speed3; break; case 0b00000100000: NowAngle = 0; /mid if(LastAngle>NowAngle) PWMDTY01 = 157; else PWMDTY01 = 155; speed_enactment = Speed3; break; case 0b00000110000: NowAngle = -1; if(LastAngle>NowAngle) PWMDTY01 = 159; else PWMDTY01 = 157; speed_enactment = Speed3; break; case 0b00000010000: NowAngle = -2; /1 if(LastAngle>NowAngle) P