File content as of revision 2:e59b809f9e0a:

/*******************************************************
 * Kenshiro Program for TEC-MD7U-A and STM NUCLEO-F446RE
 * Version 1.0
 * Created Ksingyuunyuu
 *******************************************************/
// インクルード先の宣言
#include "mbed.h"
#include <string.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>

// その他マクロ定義
#define SAMPLING_TIME           0.01    // 100Hz
#define STANDARD_RPM            3307
#define LOW_RPM                 800
// 入出力モード、各種モードの設定
Ticker control;                 // 割り込みプログラムの宣言
ros::NodeHandle nh;             // ROS使用の宣言
DigitalIn sw1(PC_13);
CAN can1(PB_8, PB_9, 500000);   // CAN使用の宣言 CAN名(RXD,TXD,動作周波数)

// グローバル変数の宣言
int can_rpm_left = 0;                   // V7送信用回転数変数左
int can_rpm_right = 0;                  // V7送信用回転数変数右
int enc_add = ((10&0xf)<<5)|0x600|0x0a; // V7からCANでエンコーダー回転数を受け取るときのアドレス(直書きだと、なぜか動作しない)

// ここからメッセージ用
char *pub_data;
// 関数のプロトタイプ宣言
void function(void);                            // 割り込み関数
void all_motor_stop( void );                    // CAN全モータストップ指令送信用関数
void motor( int rpm1,int rpm2 );                // CANモータ回転数指令送信用関数
void motor1_drive( int rpm );                   // motor1モータ回転数指令送信用関数
void motor2_drive( int rpm );                   // motor2モータ回転数指令送信用関数
void MD_mode_change(int mode);                  // V7モード変更用関数
void messageCb(const geometry_msgs::Twist& msg);// ROS購読者Twist購読関数

// 購読者宣言
//インフォメーション
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
//エンコーダー左
std_msgs::Int32 encleft_msg; //送るメッセージの型を決定する
ros::Publisher pub_encleft("encoder_left", &encleft_msg);  //パブリッシュするトピック名とトピックの中身の変数等を宣言する
//エンコーダー右
std_msgs::Int32 encright_msg; //送るメッセージの型を決定する
ros::Publisher pub_encright("encoder_right", &encright_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);
  nh.advertise(pub_encleft);
  nh.advertise(pub_encright);
    
  CANMessage msg;                     // CANメッセージを格納する変数
  /*
  // インフォメーションパブリッシュ
  pub_data = "Kenshiro Program";
  str_msg.data = pub_data;
  chatter.publish( &str_msg );
  
  pub_data = "for TEC-MD7U-A and STM NUCLEO-F446RE";
  str_msg.data = pub_data;
  chatter.publish( &str_msg );
  
  pub_data = "Program started";
  str_msg.data = pub_data;
  chatter.publish( &str_msg );
  
  pub_data = "";
  str_msg.data = pub_data;
  chatter.publish( &str_msg );
  // インフォメーションここまで
  */
  
  //control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
  while (1){
    // エンコーダー値読み取り
    if (can1.read(msg)) {  //メッセージがあったら即実行 read関数はメッセージがあった場合戻り値が1
      if ( msg.id == enc_add ){
        encleft_msg.data = (int)msg.data[0] << 24 |(int)msg.data[1] << 16 | (int)msg.data[2] << 8 | (int)msg.data[3];
        encright_msg.data = (int)msg.data[4] << 24 |(int)msg.data[5] << 16 | (int)msg.data[6] << 8 | (int)msg.data[7];
        encleft_msg.data *= -1;
      }
    }
    // 割り込みで代用?
    if (t.read_ms() > vel_timer){
        pub_encleft.publish(&encleft_msg);//実際にパブリッシュ
        pub_encright.publish(&encright_msg);//実際にパブリッシュ
        vel_timer = t.read_ms() + 500;    // 500ms毎に実行
    }
    nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
    
    wait_ms(1);     // 連続で呼び出ししないようにウェイトを置く
  }
}

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

}
*/

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)); 
}

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));
}

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));
}

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)); 

}