舵机不转

光电1233

1. /*!             引脚连接说明
   
2.                   UART0   
   
3.                       TX   A14
   
4.                       RX   A15
   
5.                   CCD
   
6.                       A0   E16   ADC0               
   
7.                       SI   E22
   
8.                       CLK  E23
   
9.                   舵机PWM
   
10.                       TPM1_CH0  A12
    
11. 
    
12.                   电机PWM
    
13.                       TPM0_CH0  C1
    
14.                       TPM0_CH1  C2
    
15.                       TPM0_CH2  C3
    
16.                       TPM0_CH3  C4
    
17. */
    
18. 
    
19. #include "common.h"
    
20. #include "include.h"
    
21. //电机相关
    
22. #define MOTOR_TPM   TPM0
    
23. #define MOTOR1_PWM  TPM_CH0            
    
24. #define MOTOR2_PWM  TPM_CH1            
    
25. #define MOTOR3_PWM  TPM_CH2
    
26. #define MOTOR4_PWM  TPM_CH3
    
27. #define MOTOR1_PWM_IO  TPM0_CH0
    
28. #define MOTOR2_PWM_IO  TPM0_CH1
    
29. #define MOTOR3_PWM_IO  TPM0_CH2
    
30. #define MOTOR4_PWM_IO  TPM0_CH3
    
31. #define MOTOR_HZ    (20*1000)
    
32. //舵机相关
    
33. #define S3010_TPM   TPM1          //
    
34. #define S3010_CH    TPM_CH0       //  PTB0
    
35. #define S3010_HZ    (100)
    
36. uint8 CCD_BUFF[TSL1401_SIZE];   //存储CCD图像的数组
    
37. uint8 s3010_middle = 73;  //舵机中心值：对应的PWM占空比为（73/1000）  精度为1000u
    
38. uint8 picture_middle = 64;//图像中心值
    
39. uint8 leftline = 1;    //赛道左边界
    
40. uint8 rightline = 127;   //赛道右边界
    
41. struct s3010_pid        //舵机PD控制
    
42. {
    
43.         int err; //定义偏差值
    
44.         int err_last; //定义上一个偏差值
    
45.         float Kp; //定义比例系数
    
46.         float Kd;//定义微分系数
    
47. }pid;
    
48. 
    
49. void PID_init();         //舵机PD算法初始化
    
50. uint32 PID_realize(int x);//舵机PD实现初始化
    
51. void PIT_IRQHandler();
    
52. void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize);
    
53. void bin(uint8 * bin, uint8 len);
    
54. 
    
55. void  main(void)
    
56. {
    
57.     uint8 time = 8;            // 修改 PIT0 的定时时间即可修改曝光时间
    
58.     uint32 s3010_pwm = s3010_middle;
    
59.    
    
60.     tpm_pwm_init(MOTOR_TPM, MOTOR1_PWM,MOTOR_HZ,100);      //初始化 电机 PWM
    
61.     tpm_pwm_init(MOTOR_TPM, MOTOR2_PWM,MOTOR_HZ,100);      //初始化 电机 PWM
    
62.     tpm_pwm_init(MOTOR_TPM, MOTOR3_PWM,MOTOR_HZ,100);      //初始化 电机 PWM
    
63.     tpm_pwm_init(MOTOR_TPM, MOTOR4_PWM,MOTOR_HZ,100);      //初始化 电机 PWM
    
64.    
    
65.     tsl1401_set_addrs(TSL1401_MAX,CCD_BUFF);        //初始化线性CCD
    
66.     tsl1401_init(time);                         //初始化 线性CCD ，配置 中断时间为 time
    
67. 
    
68.     //配置 中断函数 和 使能中断
    
69.     set_vector_handler(PIT_VECTORn,PIT_IRQHandler);
    
70.     enable_irq(PIT_IRQn);
    
71. 
    
72.     tpm_pwm_init(S3010_TPM, S3010_CH,S3010_HZ,100);      //初始化 舵机 PWM
    
73.    
    
74.     while(1)
    
75.     {  
    
76. 
    
77.         tsl1401_get_img();//采集 线性CCD 图像      
    
78.         bin((uint8 *)&CCD_BUFF[0], TSL1401_SIZE);  //处理图像，找赛道边界
    
79.    
    
80.         s3010_pwm = PID_realize(64 - picture_middle);
    
81.         tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);
    
82.         DELAY_MS(200);
    
83. 
    

复制代码