舵机加上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.     tsl1401_set_addrs(TSL1401_MAX,CCD_BUFF);        //初始化线性CCD
    
30.     tsl1401_init(time);                         //初始化 线性CCD ，配置 中断时间为 time
    
31. 
    
32.     //配置 中断函数 和 使能中断
    
33.     set_vector_handler(PIT_VECTORn,PIT_IRQHandler);
    
34.     enable_irq(PIT_IRQn);
    
35. 
    
36.     tpm_pwm_init(S3010_TPM, S3010_CH,S3010_HZ,73);      //初始化 舵机 PWM
    
37.     void PID_init();         //舵机PD算法初始化
    
38.     uint32 PID_realize(int x);//舵机PD实现初始化
    
39.     while(1)
    
40.     {  
    
41. 
    
42.         tsl1401_get_img();//采集 线性CCD 图像      
    
43.         bin((uint8 *)&CCD_BUFF[0], TSL1401_SIZE);  //处理图像，找赛道边界
    
44.    
    
45.         //问题就在这，如果把下面的代码换成tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);这之类的，舵机就能转
    
46.         h = 64 - picture_middle;
    
47.         s3010_pwm = PID_realize( h );
    
48.         tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);
    
49.         DELAY_MS(200);
    
50.   
    
51.         vcan_sendccd((uint8 *)&CCD_BUFF[0],TSL1401_SIZE);//给上位机发送数据
    
52.    
    
53.     }
    
54. }
    
55. void bin(uint8 * bin, uint8 len)
    
56. {
    
57.     uint8 i;
    
58.    
    
59.     for(i=10;i<63;i++)
    
60.     {
    
61.         if(ABS(bin[i - 1]-bin[i]) >= 80)
    
62.         {
    
63.             leftline = i;
    
64.             break;
    
65.         }
    
66.     }
    
67.     for(i=111;i>64;i--)
    
68.     {
    
69.         if(ABS(bin[i + 1]-bin[i]) >= 80)
    
70.         {
    
71.             rightline = i;
    
72.             break;
    
73.         }
    
74.     }
    
75. #if 0
    
76.     for(i = leftline; i < rightline; i++)
    
77.     {
    
78.         bin[i] = 255;
    
79.     }
    
80. #endif
    
81.     picture_middle = (uint8)((leftline + rightline) / 2);
    
82.     bin[picture_middle] = 0;
    
83. }
    
84. //   PIT中断服务函数     由于 TSL1401_INT_TIME 配置 为 PIT0 ，因而使用 PIT0
    
85. void PIT_IRQHandler()
    
86. {
    
87.     if(PIT_TFLG(PIT0) == 1 )        //判断是否 PIT0 进入中断
    
88.     {
    
89.         tsl1401_time_isr();
    
90.         PIT_Flag_Clear(PIT0);
    
91.     }
    
92. }
    
93. //发送图像到上位机显示   不同的上位机，不同的命令
    
94. void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize)
    
95. {
    
96. #define CMD_CCD     2
    
97.     uint8 cmdf[2] = {CMD_CCD, ~CMD_CCD};    //开头命令
    
98.     uint8 cmdr[2] = {~CMD_CCD, CMD_CCD};    //结尾命令
    
99. 
    
100.     uart_putbuff(VCAN_PORT, cmdf, sizeof(cmdf));    //先发送命令
     
101. 
     
102.     uart_putbuff(VCAN_PORT, ccdaddr, ccdsize); //再发送图像
     
103. 
     
104.     uart_putbuff(VCAN_PORT, cmdr, sizeof(cmdr));    //再发送命令
     
105. }
     
106. void PID_init()
     
107. {
     
108.         pid.err=0;
     
109.         pid.err_last=0;
     
110.         pid.Kp=0.7;
     
111.         pid.Kd=0.3;
     
112. }
     
113. uint32 PID_realize(int x) //x为距离中间的偏差
     
114. {
     
115.         uint8 out;   
     
116.         pid.err = x;
     
117.         out = (uint8)(s3010_middle + pid.err * pid.Kp + (pid.err - pid.err_last) * pid.Kd);        
     
118.         pid.err_last = pid.err;       
     
119.         return out;
     
120. }

复制代码

光电1233

顶起:)

光电1233

再顶