舵机加了PD算法之后就没反应了，去了PD就能转，求大神

光电1233

1. #include "common.h"
   
2. #include "include.h"
   
3. //舵机相关
   
4. #define S3010_TPM   TPM1         
   
5. #define S3010_CH    TPM_CH0       //  PTB0
   
6. #define S3010_HZ    (50)
   
7. uint8 CCD_BUFF[TSL1401_SIZE];   //存储CCD图像的数组
   
8. uint8 s3010_middle = 73;  //舵机中心值：对应的PWM占空比为（73/1000）  精度为1000u
   
9. uint8 picture_middle = 64;//图像中心值
   
10. uint8 leftline = 1;    //赛道左边界
    
11. uint8 rightline = 127;   //赛道右边界
    
12. struct s3010_pid        //舵机PD控制
    
13. {
    
14.         int err; //定义偏差值
    
15.         int err_last; //定义上一个偏差值
    
16.         float Kp; //定义比例系数
    
17.         float Kd;//定义微分系数
    
18. }pid;
    
19. 
    
20. void PIT_IRQHandler();
    
21. void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize);
    
22. void bin(uint8 * bin, uint8 len);
    
23. 
    
24. void  main(void)
    
25. {
    
26.     uint8 time = 5;            // 修改 PIT0 的定时时间即可修改曝光时间
    
27.     uint32 s3010_pwm = s3010_middle;
    
28.     int h = 0;
    
29. 
    
30.     tsl1401_set_addrs(TSL1401_MAX,CCD_BUFF);        //初始化线性CCD
    
31.     tsl1401_init(time);                         //初始化 线性CCD ，配置 中断时间为 time
    
32. 
    
33.     //配置 中断函数 和 使能中断
    
34.     set_vector_handler(PIT_VECTORn,PIT_IRQHandler);
    
35.     enable_irq(PIT_IRQn);
    
36. 
    
37.     tpm_pwm_init(S3010_TPM, S3010_CH,S3010_HZ,73);      //初始化 舵机 PWM
    
38.     void PID_init();         //舵机PD算法初始化
    
39.     uint32 PID_realize(int x);//舵机PD实现初始化
    
40.     while(1)
    
41.     {  
    
42. 
    
43.         tsl1401_get_img();//采集 线性CCD 图像      
    
44.         bin((uint8 *)&CCD_BUFF[0], TSL1401_SIZE);  //处理图像，找赛道边界
    
45.           
    
46.         //如果把下面改成tpm_pwm_duty(S3010_TPM, S3010_CH,70);这之类的舵机能转，
    
47.         //但是如果是下面的就不能转
    
48.         h = 64 - picture_middle;
    
49.         s3010_pwm = PID_realize( h );
    
50.         tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);
    
51.         DELAY_MS(200);
    
52.   
    
53.         vcan_sendccd((uint8 *)&CCD_BUFF[0],TSL1401_SIZE);//给上位机发送数据
    
54.    
    
55.     }
    
56. }
    
57. void bin(uint8 * bin, uint8 len)
    
58. {
    
59.     uint8 i;
    
60.    
    
61.     for(i=10;i<63;i++)
    
62.     {
    
63.         if(ABS(bin[i - 1]-bin[i]) >= 80)
    
64.         {
    
65.             leftline = i;
    
66.             break;
    
67.         }
    
68.     }
    
69.     for(i=111;i>64;i--)
    
70.     {
    
71.         if(ABS(bin[i + 1]-bin[i]) >= 80)
    
72.         {
    
73.             rightline = i;
    
74.             break;
    
75.         }
    
76.     }
    
77. #if 0
    
78.     for(i = leftline; i < rightline; i++)
    
79.     {
    
80.         bin[i] = 255;
    
81.     }
    
82. #endif
    
83.     picture_middle = (uint8)((leftline + rightline) / 2);
    
84.     bin[picture_middle] = 0;
    
85. }
    
86. //   PIT中断服务函数     由于 TSL1401_INT_TIME 配置 为 PIT0 ，因而使用 PIT0
    
87. void PIT_IRQHandler()
    
88. {
    
89.     if(PIT_TFLG(PIT0) == 1 )        //判断是否 PIT0 进入中断
    
90.     {
    
91.         tsl1401_time_isr();
    
92.         PIT_Flag_Clear(PIT0);
    
93.     }
    
94. }
    
95. //发送图像到上位机显示   不同的上位机，不同的命令
    
96. void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize)
    
97. {
    
98. #define CMD_CCD     2
    
99.     uint8 cmdf[2] = {CMD_CCD, ~CMD_CCD};    //开头命令
    
100.     uint8 cmdr[2] = {~CMD_CCD, CMD_CCD};    //结尾命令
     
101. 
     
102.     uart_putbuff(VCAN_PORT, cmdf, sizeof(cmdf));    //先发送命令
     
103. 
     
104.     uart_putbuff(VCAN_PORT, ccdaddr, ccdsize); //再发送图像
     
105. 
     
106.     uart_putbuff(VCAN_PORT, cmdr, sizeof(cmdr));    //再发送命令
     
107. }
     
108. void PID_init()
     
109. {
     
110.         pid.err=0;
     
111.         pid.err_last=0;
     
112.         pid.Kp=0.7;
     
113.         pid.Kd=0.3;
     
114. }
     
115. uint32 PID_realize(int x) //x为距离中间的偏差
     
116. {
     
117.         uint8 out;   
     
118.         pid.err = x;
     
119.         out = (uint8)(s3010_middle + pid.err * pid.Kp + (pid.err - pid.err_last) * pid.Kd);        
     
120.         pid.err_last = pid.err;       
     
121.         return out;
     
122. }

复制代码