File content as of revision 0:27046fed2426:

/*
 * rosserial Motor Shield Example
 *
 * This tutorial demonstrates the usage of the
 * Seeedstudio Motor Shield
 * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
 *
 * Source Code Based on:
 * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
 */

#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include "QEI.h"
#include <std_msgs/String.h>

// マクロ定義(ピン配置)
#define D0  PA_3
#define D1  PA_2
#define D2  PA_10
#define D3  PB_3
#define D4  PB_5
#define D5  PB_4
#define D6  PB_10
#define D7  PA_8
#define D8  PA_9
#define D9  PC_7
#define D10 PB_6
#define D11 PA_7
#define D12 PA_6
#define D13 PA_5
// アナログ
#define A0  PA_0
#define A1  PA_1
#define A2  PA_4
#define A3  PB_0
#define A4  PC_1
#define A5  PC_0

#define B1  PC_13
// その他マクロ定義
#define SAMPLING_TIME           0.01    // 100Hz
#define ENC_PULSE_PER_METER     1965
#define KP_LEFT     0.70
#define KI_LEFT     0
#define KD_LEFT     0
#define KP_RIGHT    0.75
#define KI_RIGHT    0
#define KD_RIGHT    0
#define SPEED_LEFT  (double)(get_enc_left / 19.65)
#define SPEED_RIGHT (double)(get_enc_right / 19.65)

// 入出力モード、各種モードの設定
BusIn sw(B1);
BusIn in(D5, D6, D11, D12);
PwmOut motor_left(D4);
PwmOut motor_right(D10);
DigitalOut left(D3);
DigitalOut right(D9);
QEI enc_left(D5, D6, NC, 48, QEI::X4_ENCODING);
QEI enc_right(D12, D11, NC, 48, QEI::X4_ENCODING);
Ticker control;
ros::NodeHandle nh;

// グローバル変数の宣言
int get_enc_left;
int get_enc_right;
int enc_center;
double SpeedIntegral_left = 0;
double iSpeedBefore_left = 0;
double TargetSpeed_left;
double pwm_left;
double SpeedIntegral_right = 0;
double iSpeedBefore_right = 0;
double TargetSpeed_right;
double pwm_right;
char str_left[13] = "mode left   ";
char str_right[13] = "mode right  ";
char str_forward[13] = "mode forward";
char str_back[13] = "mode back   ";
char str_nutral[13] = "mode nutral ";
// 関数のプロトタイプ宣言
void function(void);
void motor_speed_left( void );
void motor_speed_right( void );
void messageCb(const geometry_msgs::Twist& msg);

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb);

Timer t;

int main()
{
  
  in.mode(PullUp);
    control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
    
  t.start();
  long vel_timer = 0;
  nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(chatter);
  while (1)
  {
    if (t.read_ms() > vel_timer)
    {
      // 停止
        TargetSpeed_left = 0;
        TargetSpeed_right = 0;
        motor_left = 0;
        motor_right = 0;
        vel_timer = t.read_ms() + 500;    // 500ms毎に実行
    }
    nh.spinOnce();
    motor_left = pwm_left;
    motor_right = pwm_right;
    wait_ms(1);
  }
}

// 割り込み関数
void function(void){
    motor_speed_left();
    motor_speed_right();
    }

void motor_speed_left( void ){
    double out;
    get_enc_left = enc_left.getPulses();
    enc_left.reset();
    double div_speed_left = TargetSpeed_left - SPEED_LEFT;
    out = KP_LEFT * div_speed_left;
    if(out < 0){
        out *= -1;
        left = 1;
    } else {
        left = 0;        
    }
    pwm_left = out;
} 
void motor_speed_right( void ){
    double out;
    get_enc_right = enc_right.getPulses();
    enc_right.reset();
    double div_speed_right = TargetSpeed_right - SPEED_RIGHT;
    out = KP_RIGHT * div_speed_right;
    if(out < 0){
        out *= -1;
        right = 1;
    } else {
        right = 0;        
    }
    pwm_right = out;
}

void messageCb(const geometry_msgs::Twist& msg){
  double tmp = 0;
  
  TargetSpeed_left = msg.linear.x;
  TargetSpeed_right = msg.linear.x;
  tmp = 0;
  
  if(msg.angular.z < 0){
      tmp = 1 - (-msg.angular.z / 3);
      TargetSpeed_left = msg.linear.x;
      TargetSpeed_right = msg.linear.x * tmp;
  } else if(msg.angular.z > 0){
      tmp = 1 - (msg.angular.z / 3);
      TargetSpeed_left = msg.linear.x * tmp;
      TargetSpeed_right = msg.linear.x;
  } else {
      TargetSpeed_right = msg.linear.x;
      TargetSpeed_right = msg.linear.x;
  }
  motor_left = pwm_left;
  motor_right = pwm_right;
  
  /*
  if (msg.angular.z == 0 && msg.linear.x == 0){
    // 停止
    TargetSpeed_left = 0;
    TargetSpeed_right = 0;
    motor_left = pwm_left;
    motor_right = pwm_right;
    str_msg.data = str_nutral;
    chatter.publish( &str_msg );
  } else {
    if (msg.angular.z < 0){
      // 右旋回
        TargetSpeed_left = 1.0;
        TargetSpeed_right = 0;
        motor_left = pwm_left;
        motor_right = pwm_right;
        str_msg.data = str_right;
        chatter.publish( &str_msg );
    } else if (msg.angular.z > 0){
      // 左旋回
        TargetSpeed_left = 0;
        TargetSpeed_right = 1.0;
        motor_left = pwm_left;
        motor_right = pwm_right;
        str_msg.data = str_left;
        chatter.publish( &str_msg );
    } else if (msg.linear.x < 0){
      // 後進
        TargetSpeed_left = -1.0;
        TargetSpeed_right = -1.0;
        motor_left = pwm_left;
        motor_right = pwm_right;
        str_msg.data = str_back;
        chatter.publish( &str_msg );
    } else if (msg.linear.x > 0){
      // 前進
        TargetSpeed_left = 1.0;
        TargetSpeed_right = 1.0;
        motor_left = pwm_left;
        motor_right = pwm_right;
        str_msg.data = str_forward;
        chatter.publish( &str_msg );
    }
  }
  */
}