AGV小车PID算法跑直线

  1. #include
  2. #include”sys.h”
  3. extern unsigned char Rec_Dat_U1;
  4. extern unsigned char Rec_String_U1[200];
  5. extern int conut_flag_1;
  6. #define in_1(x)    x?GPIO_ResetBits(GPIOC , GPIO_Pin_0): GPIO_SetBits(GPIOC , GPIO_Pin_0)
  7. #define in_2(x)    x?GPIO_ResetBits(GPIOC , GPIO_Pin_1): GPIO_SetBits(GPIOC , GPIO_Pin_1)
  8. #define in_3(x)    x?GPIO_ResetBits(GPIOB , GPIO_Pin_10): GPIO_SetBits(GPIOB , GPIO_Pin_10)
  9. #define in_4(x)    x?GPIO_ResetBits(GPIOB , GPIO_Pin_11): GPIO_SetBits(GPIOB , GPIO_Pin_11)
  10. #define key_1   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_12)
  11. #define key_2   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_13)
  12. #define key_3   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_14)
  13. #define key_4   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_15)
  14. float  Kp_l =                0.82  ; //比例常数
  15. float  Ti_l =                0.09 ; //微分时间常数
  16. float  Td_l =                0.015 ; //采样周期
  17. float  Kp_r =                0.20  ;
  18. float  Ti_r =                0.09 ;
  19. float  Td_r =                0.015 ;
  20. #define T                  0.02
  21. #define Ki_l                Kp_l*(T/Ti_l)        // Kp Ki Kd
  22. #define Kd_l                Kp_l*(Td_l/T)
  23. #define Ki_r                Kp_l*(T/Ti_l)        // Kp Ki Kd
  24. #define Kd_r                Kp_l*(Td_l/T)
  25. #define left_b in_1(1); in_2(0);
  26. #define left_f in_1(0); in_2(1);
  27. #define right_f in_3(1); in_4(0);
  28. #define right_b in_1(0); in_2(1);
  29. extern float pwm_l;
  30. extern float pwm_r;
  31. void CAR_GPIO_INIT(void);
  32. int main()
  33. {
  34. // int speed=50;
  35. SYS_CONFIG();
  36. CAR_GPIO_INIT();
  37. pwm_init(TIM3_1);
  38. pwm_init(TIM3_2);
  39. TIM4_PWMINPUT_INIT(0xffff,31);  //1M速度采样率
  40. TIM5_PWMINPUT_INIT(0xffff,31);
  41. incPIDinit();
  42. PID_set(Kp_l,Ki_l,Kd_l,Kp_r,Ki_r,Kd_r);
  43. //void PID_set(float pp_1,float ii_1,float dd_1,float pp_2,float ii_2,float dd_2)
  44. right_f
  45. left_f
  46. // set_pwm_percent(TIM3_1,50) ;
  47. // set_pwm_percent(TIM3_2,50) ;
  48. PID_setpoint(&PID_l, 160);
  49. PID_setpoint(&PID_r, 160);
  50. while(1)
  51. {
  52. printf(“%d\r\n”,(int)pwm_l);
  53. delay_ms(200);
  54. }
  55. }
  56. void CAR_GPIO_INIT()
  57. {
  58. GPIO_InitTypeDef GPIO_InitStructure;
  59. RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE);
  60. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
  61. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;
  62. GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
  63. GPIO_Init(GPIOB, &GPIO_InitStructure);
  64. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
  65. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;
  66. GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
  67. GPIO_Init(GPIOB, &GPIO_InitStructure);
  68. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
  69. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;
  70. GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
  71. GPIO_Init(GPIOC, &GPIO_InitStructure);
  72. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
  73. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;
  74. GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
  75. GPIO_Init(GPIOC, &GPIO_InitStructure);
  76. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
  77. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;
  78. GPIO_Init(GPIOD, &GPIO_InitStructure);
  79. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
  80. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;
  81. GPIO_Init(GPIOD, &GPIO_InitStructure);
  82. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
  83. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;
  84. GPIO_Init(GPIOD, &GPIO_InitStructure);
  85. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
  86. GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;
  87. GPIO_Init(GPIOD, &GPIO_InitStructure);
  88. }

———————
作者:求是07
来源:CSDN
原文:https://blog.csdn.net/u011046042/article/details/38359317

© 版权声明
THE END
喜欢就支持一下吧
点赞0 分享
评论 抢沙发

请登录后发表评论

    暂无评论内容