树莓派无刷电机控制器调速和PID控制

基于树莓派无刷电机控制，编码器数据采集和pid速度环控制

#include 
#include 
#include 
#include 
#include 

#include 
//#include 

#include 
#include 
#include 
#include 

//编码器
#define LEFT_ENCODER_PINA      23
#define LEFT_ENCODER_PINB      24

#define RIGHT_ENCODER_PINA     20
#define RIGHT_ENCODER_PINB     21

//电机PWM和转向
#define LEFT_MOTOR_PWM         17
#define LEFT_MOTOR_DIRECTION   2

#define RIGHT_MOTOR_PWM        27
#define RIGHT_MOTOR_DIRECTION  3

//两轮子的间距-米
#define WHEEL_SEPARATION       0.320
//轮子半径-米
#define WHEEL_RADIUS           0.0625

#define KP 1.3
#define KI 0.0
#define KD 0.15

//轮子前进每米编码器产生的脉冲数
#define TICKSPERMETER  76394.4
//每弧度（轮子的，注意乘以减速比）编码器产生的脉冲数
#define TICKSPERRADIAN 4774.6

//------------------------------------encoder callback----------------------------------------
//编码器读数
static long int left_encoder_count = 0;
static long int right_encoder_count = 0;
static long int lspeed_encoder_count = 0;
static long int rspeed_encoder_count = 0;
int rcallbackcount = 0;
float left_speed_cmd = 0.0;
float right_speed_cmd = 0.0;

void left_encoder_callback（int way）
{
  left_encoder_count += way;
  lspeed_encoder_count+=way;
  //std::cout << “posl=“ << left_encoder_count << std::endl;
}

void right_encoder_callback（int way）
{
  right_encoder_count += way;
  rspeed_encoder_count+= way;
  //std::cout << “posr=“ << right_encoder_count << std::endl;
  //if（rcallbackcount>51）
  //rcallbackcount=51;
  if（left_speed_cmd==right_speed_cmd&&left_speed_cmd>0）{
     rcallbackcount++;
     if（rcallbackcount==52）
     {
        right_encoder_count=right_encoder_count-1;
        rspeed_encoder_count=rspeed_encoder_count-1;
        rcallbackcount=0;
        
     }
  }

  if（left_speed_cmd==right_speed_cmd&&left_speed_cmd<0）{
     rcallbackcount++;
     if（rcallbackcount==50）
     {
        right_encoder_count=right_encoder_count+1;
        rspeed_encoder_count=rspeed_encoder_count+1;
        rcallbackcount=0;
     }
  }
}


//------------------------------------cmd_vel callback---------------------------------------
//上位机速度命令，弧度/秒


void cmd_callback（const geometry_msgs::Twist::ConstPtr& cmd_vel）
{
  float cmd_x = cmd_vel->linear.x;
  float cmd_theta = cmd_vel->angular.z;	
  //根据线速度和角速度计算左右轮子转速，弧度/秒
  left_speed_cmd  = 100.0*（cmd_x - cmd_theta * WHEEL_SEPARATION / 2.0）/WHEEL_RADIUS;
  right_speed_cmd = 100.0*（cmd_x + cmd_theta * WHEEL_SEPARATION / 2.0）/WHEEL_RADIUS;			
}


//------------------------------------PID controller---------------------------------------
//last encoder ticks
long int last_left_encoder_count = 0;
long int last_right_encoder_count = 0;

//编码器测得轮子转速，rad/s
float left_speed = 0.0;
float last_left_speed = 0.0;
float right_speed = 0.0;
float last_right_speed = 0.0;

//速度偏差
float left_speed_integral = 0.0;
float right_speed_integral = 0.0;

float last_left_speed_error = 0.0;
float las

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        541  2016-09-08 05:07  raspi_driver\CMakeLists.txt

     文件        723  2016-09-02 23:57  raspi_driver\include\bucketbot_driver\rotary_encoder.hpp

     文件        586  2016-09-08 04:13  raspi_driver\package.xml

     文件        244  2017-01-18 10:53  raspi_driver\readme.txt

     文件      10745  2016-09-23 05:59  raspi_driver\src\bucketbot_node.cpp

     文件       1855  2016-09-05 03:47  raspi_driver\src\rotary_encoder.cpp

     目录          0  2017-04-18 11:03  raspi_driver\include\bucketbot_driver

     目录          0  2017-04-18 11:03  raspi_driver\include

     目录          0  2017-04-18 11:03  raspi_driver\src

     目录          0  2017-04-18 11:03  raspi_driver

----------- ---------  ---------- -----  ----

                14694                    10