控制舵机的算法，我这样写：代码如下，为啥这样写舵机不打角呢，是哪里出问题了

jianr · 发表于 2016-4-15 09:50:16

把一场图像均分分成三份（总共60行，分为远，中，近，各20行），然后分别求三份的偏差；然后把这三个偏差的均值分别乘以不同的系数，然后相加，得到舵机打角的偏差，然后输出，
我的这个思想正确吗？为啥我不管把系数调多大，舵机抖不打角，，代码如下
      uint8 i,jin = 0,zhong = 0,yuan = 0;
      uint8 img_yuan = 0,img_zhong = 20,img_jin = 40;
      short int Angle_error_jin = 0,Angle_error_zhong = 0,Angle_error_yuan = 0;
      short int Angle_error = 0,Steer_out = 0;
      for(i = 59;i >= img_jin;i --)                   //距摄像头进的20行数据的偏差
      {
            Angle_error_jin += (center[i] - 39);
            jin++;
      }
      Angle_error_jin /= jin;
      for(i = img_jin - 1;i >= img_zhong;i --)       //距摄像头远的20行数据的偏差
      {
            Angle_error_zhong += (center[i] - 39);
            zhong++;
      }
      Angle_error_zhong /= zhong;
for(i = img_zhong - 1;i >= img_yuan;i --)       //摄像头图像中间进的20行数据的偏差
      {
            Angle_error_yuan += (center[i] - 39);
            yuan++;
      }
      Angle_error_yuan /= yuan;
      Angle_error = 3*Angle_error_jin + 6*Angle_error_zhong + 3*Angle_error_yuan;
      Steer_out = 200*Angle_error;
      if(Steer_out > 1600)
      Steer_out = 1600;
      if(Steer_out < -1600)
      Steer_out = -1600;
      ftm_pwm_init(FTM1, FTM_CH0,100,5400 +  Steer_out);