#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?<}

void?right_encoder_callback（int?way）
{
??right_encoder_count?+=?way;
??rspeed_encoder_count+=?way;
??//std::cout?<??//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