File content as of revision 1:979cc2a56e7e:

/*
 * 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 <std_msgs/String.h>

// マクロ定義(ピン配置)F446


// その他マクロ定義
#define SAMPLING_TIME           0.01    // 100Hz
#define STANDARD_RPM            3307
#define LOW_RPM                 800
// 入出力モード、各種モードの設定

Ticker control;
ros::NodeHandle nh;
CAN can1(PB_8, PB_9, 500000); //Set frequency at declaration
//CAN can1(PB_8, PB_9);   // can_name( RXD, TXD )
// グローバル変数の宣言
int can_rpm_left = 0;
int can_rpm_right = 0;
// ここからメッセージ用
char pub[13] = "publish now ";
char left[13] = "mode left   ";
char right[13] = "mode right  ";
char move[13] = "mode move   ";
char nutral[13] = "mode nutral ";
// 関数のプロトタイプ宣言
void function(void);
void all_motor_stop( void );
void motor( int rpm1,int rpm2 );
void motor1_drive( int rpm );
void motor2_drive( int rpm );
void MD_mode_change(int mode);
void messageCb(const geometry_msgs::Twist& msg);

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

Timer t;

int main(){
    
  t.start();
  long vel_timer = 0;
  nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(chatter);
  
  CANMessage msg;                     // CANメッセージを格納する変数
  //can1.frequency(500000);         // CANのビットレートを500kHzに設定
  
  //control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
  while (1){
    // これ意味ないんじゃないの？
    if (t.read_ms() > vel_timer){
      // 停止
      /*
        can_rpm_left = 0;
        can_rpm_right = 0;
        */
        vel_timer = t.read_ms() + 500;    // 500ms毎に実行
    }
    nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
    // ここでCANの動作？、ここでダメなら、割り込みでCAN
    //motor1_drive( can_rpm_left );
    //motor2_drive( can_rpm_right );    
    wait_ms(1);
  }
}

// 割り込み関数
/*
void function(void){

}
*/
/*
void messageCb(const geometry_msgs::Twist& msg){
  double tmp = 0;
  double rpm_left = 0;
  double rpm_right = 0;
  // たぶんいらない
  //TargetSpeed_left = msg.linear.x;
  //TargetSpeed_right = msg.linear.x;
            // パブリッシュ
    str_msg.data = nutral;
    chatter.publish( &str_msg );
    // パブリッシュここまで  
  if(msg.angular.z < 0){
      // 左旋回
      // z軸変換移動角度量(0~1.0くらい) = z軸移動角度量(ラジアン) / 3 * -1  ・・・z軸移動量がマイナスなので、プラスに変換する
      tmp = -msg.angular.z / 3;
      // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
      rpm_left = STANDARD_RPM * msg.angular.x * tmp;
      // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0) * z軸移動角度量(変換後0~1.0くらい)
      rpm_right = STANDARD_RPM * msg.angular.x;
      // パブリッシュ
    str_msg.data = left;
    chatter.publish( &str_msg );
    // パブリッシュここまで
  } else if(msg.angular.z > 0){
      // 右旋回
      // z軸変換移動角度量(0~1.0くらい) = z軸移動角度量(ラジアン) / 3  ・・・z軸移動量がプラスなので、プラスに変換する必要はない
      tmp = msg.angular.z / 3;
      // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0) * z軸移動角度量(変換後0~1.0くらい)
      rpm_left = STANDARD_RPM * msg.angular.x;
      // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
      rpm_right = STANDARD_RPM * msg.angular.x * tmp;
      // パブリッシュ
    str_msg.data = right;
    chatter.publish( &str_msg );
    // パブリッシュここまで
  } else {
      // z移動角度量が0の場合(つまり、前進か後進)
      // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
      rpm_left = STANDARD_RPM * msg.angular.x;
      // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
      rpm_right = STANDARD_RPM * msg.angular.x;
      // パブリッシュ
    str_msg.data = move;
    chatter.publish( &str_msg );
    // パブリッシュここまで
  }
  // CAN用に少数を整数型にキャストする
  can_rpm_left = rpm_left;
  can_rpm_right = rpm_right;
}
*/

/*
void messageCb(const geometry_msgs::Twist& msg){
  if (msg.angular.z == 0 && msg.linear.x == 0){
    // 停止
    can_rpm_left = 0;
    can_rpm_right = 0;
    motor1_drive( can_rpm_left );
    motor2_drive( can_rpm_right ); 
    str_msg.data = nutral;
    chatter.publish( &str_msg );
  } else {
    if (msg.angular.z < 0){
      // 右旋回
        can_rpm_left = 1000;
        can_rpm_right = 0;
        motor1_drive( can_rpm_left );
        motor2_drive( can_rpm_right );
        str_msg.data = right;
        chatter.publish( &str_msg );
    } else if (msg.angular.z > 0){
      // 左旋回
        can_rpm_left = 0;
        can_rpm_right = 1000;
        motor1_drive( can_rpm_left );
        motor2_drive( can_rpm_right );
        str_msg.data = left;
        chatter.publish( &str_msg );
    } else if (msg.linear.x < 0){
      // 後進
        can_rpm_left = -1000;
        can_rpm_right = -1000;
        motor1_drive( can_rpm_left );
        motor2_drive( can_rpm_right );
        str_msg.data = move;
        chatter.publish( &str_msg );
    } else if (msg.linear.x > 0){
      // 前進
        can_rpm_left = 1000;
        can_rpm_right = 1000;
        motor1_drive( can_rpm_left );
        motor2_drive( can_rpm_right );
        str_msg.data = move;
        chatter.publish( &str_msg );
    }
  }
}
*/

void messageCb(const geometry_msgs::Twist& msg){
  double tmp = 0;
  double tmp_rpm_left = 0;
  double tmp_rpm_right = 0;
  if(msg.angular.z < 0){
      // 左旋回
      tmp = 1 - (-msg.angular.z / 3);   //radiun
      if( msg.linear.x == 0 ){
          // 真左に旋回の場合
          tmp_rpm_left = LOW_RPM * tmp;
          tmp_rpm_right = 0;
        } else {
          tmp_rpm_left = STANDARD_RPM * msg.linear.x;
          tmp_rpm_right = STANDARD_RPM * msg.linear.x * tmp;
        }
  } else if(msg.angular.z > 0){
      // 右旋回
      tmp = 1 - (msg.angular.z / 3);    //radiun
      if( msg.linear.x == 0 ){
          // 真右に旋回の場合
          tmp_rpm_left = 0;
          tmp_rpm_right = LOW_RPM * tmp;
        } else {
          tmp_rpm_left = STANDARD_RPM * msg.linear.x * tmp;
          tmp_rpm_right = STANDARD_RPM * msg.linear.x;
        }
  } else {
      tmp_rpm_left = STANDARD_RPM * msg.linear.x;
      tmp_rpm_right = STANDARD_RPM * msg.linear.x;
  }
  motor( tmp_rpm_left, tmp_rpm_right );
}
/*  CANIDプロトコル    
/   CANID:xxyyyyzzzzzの11bit中    
/   上位2bit:優先度          :小さいものが高優先　[00]:ブロードキャスト用？？ [01]:M-bedから書き込みに使える [10]:MDからくるレポート [11]:不明    メッセージの衝突回避用
/   次の4bit:プロトコルID    :[0]高優先度プロトコル　[1]制御プロトコル　[2-3]書き込みプロトコル　[8]高優先度通知プロトコル　[9]イベント通知プロトコル　[10-12]ステータスレポートプロトコル
/   下位5bit:デバイスID      :デバイスIDとデバイスの各メールボックス
/   
/   CANデータフィールド1Byte目:上位４bit：１つ目のデータはレジスタ何個分か　下位4bit：2つ目のデータはレジスタ何個分か RPMCMD1 =2Byte　RPMCMD2 =2Byte
/   CANデータフィールド2Byte目:書き込む先のアドレス1　レジスタのアドレス　例）RPMCMD1 =0x40 (64番目
/   CANデータフィールド3Byte目:例）回転数1000の上位2bit
/   CANデータフィールド4Byte目:例）回転数1000の下位2bit
/   CANデータフィールド5Byte目:書き込む先のアドレス2　レジスタのアドレス　RPMCMD1 =0x40 (64番目
/   CANデータフィールド6Byte目:例）回転数1000の上位2bit
/   CANデータフィールド7Byte目:例）回転数1000の下位2bit
/   CANデータフィールド8Byte目:
/
*/

#define MDcontroll  1
#define MDwrite     2
#define MDwriteBC   3
void all_motor_stop( void ){        // 全モータストップ
    //MT1=0rpm MT2=0rpm
    char txdata[8];
    int CANID;
    
    txdata[0] = 0x11;
    txdata[1] = 0x40;   //rpmcmd1
    txdata[2] = 0x00;   //MT1=-2000rpm
    txdata[3] = 0x00;   //MT1=-2000rpm
    txdata[4] = 0x41;
    txdata[5] = 0x00;
    txdata[6] = 0x00;

    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
    can1.write(CANMessage(CANID,&txdata[0],7)); 
}

void motor( int rpm1, int rpm2 ){      // 全モータ前進
    //MT1=0rpm MT2=0rpm
    char txdata[8];
    int CANID;
    
    int motor1_rpm = -rpm1;
    int motor2_rpm = rpm2;
    
    char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
    char motor1_data2 = motor1_rpm & 0x00ff;
    char motor2_data1 = (motor2_rpm & 0xff00) >> 8;
    char motor2_data2 = motor2_rpm & 0x00ff;
    
    txdata[0] = 0x11;
    txdata[1] = 0x40;           //rpmcmd1
    txdata[2] = motor1_data1;
    txdata[3] = motor1_data2;
    txdata[4] = 0x41;
    txdata[5] = motor2_data1;
    txdata[6] = motor2_data2;

    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
    can1.write(CANMessage(CANID,&txdata[0],7)); 
    /*
    printf("motor1_data1 = %02x\n",motor1_data1);
    printf("motor1_data2 = %02x\n",motor1_data2);
    printf("motor2_data1 = %02x\n",motor2_data1);
    printf("motor2_data2 = %02x\n",motor2_data2);
    */
}

void motor1_drive( int rpm ){     // モータ1正転
    //MT1=2000rpm
    char txdata[8];
    int CANID;
    
    int motor1_rpm;
    if(rpm >= 0){
     motor1_rpm = -rpm;
    } else {
     motor1_rpm = rpm;
    }
    
    char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
    char motor1_data2 = motor1_rpm & 0x00ff;
    
    txdata[0] = 0x10;
    txdata[1] = 0x40;   //rpmcmd1
    txdata[2] = motor1_data1;
    txdata[3] = motor1_data2;


    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
    can1.write(CANMessage(CANID,&txdata[0],4));
    /*
    printf("motor1_data1 = %02x\n",motor1_data1);
    printf("motor1_data2 = %02x\n",motor1_data2); 
    */
}

void motor2_drive( int rpm ){     // モータ2前進
    //MT2=2000rpm
    char txdata[8];
    int CANID;
    char motor2_data1 = (rpm & 0xff00) >> 8;
    char motor2_data2 = rpm & 0x00ff;
    txdata[0] = 0x10;
    txdata[1] = 0x41;   //rpmcmd2
    txdata[2] = motor2_data1;
    txdata[3] = motor2_data2;
    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
    can1.write(CANMessage(CANID,&txdata[0],4));
    /*
    printf("motor2_data1 = %02x\n",motor2_data1);
    printf("motor2_data2 = %02x\n",motor2_data2);
    */
}

void MD_mode_change(int mode){  // モータドライバのモード変更関数
    char txdata[8];
    int CANID;
    
    txdata[0] = 0x05;
    txdata[1] = mode;
    txdata[2] = 0x00;
    txdata[3] = 0x00;
    txdata[4] = 0x00;
    txdata[5] = 0x00;
    txdata[6] = 0x00;
    txdata[7] = 0x00;
    
    CANID=0x200 |  ((MDcontroll)&0xf)<<5 | 0xa ;    // 上位２ビット：優先度：01　次の4bit：プロトコルID：制御モード　下位5bit：デバイスID：１０番
    can1.write(CANMessage(CANID,&txdata[0],8)); 

}