K AKAGAWA / Mbed 2 deprecated F446_rosserial_V7_EncoderVersion_102

kenshiro program

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

main.cpp@2:e59b809f9e0a, 2019-05-25 (annotated)

Committer:
ksingyuunyuu
Date:
Sat May 25 14:09:18 2019 +0000
Revision:
2:e59b809f9e0a
Parent:
1:979cc2a56e7e
Child:
3:e5463b529172
F446_rosserial_V7_EncoderVersion Ver1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ksingyuunyuu 2:e59b809f9e0a 1 /*******************************************************
ksingyuunyuu 2:e59b809f9e0a 2 * Kenshiro Program for TEC-MD7U-A and STM NUCLEO-F446RE
ksingyuunyuu 2:e59b809f9e0a 3 * Version 1.0
ksingyuunyuu 2:e59b809f9e0a 4 * Created Ksingyuunyuu
ksingyuunyuu 2:e59b809f9e0a 5 *******************************************************/
ksingyuunyuu 2:e59b809f9e0a 6 // インクルード先の宣言
ksingyuunyuu 0:27046fed2426 7 #include "mbed.h"
ksingyuunyuu 2:e59b809f9e0a 8 #include <string.h>
ksingyuunyuu 0:27046fed2426 9 #include <ros.h>
ksingyuunyuu 0:27046fed2426 10 #include <geometry_msgs/Twist.h>
ksingyuunyuu 2:e59b809f9e0a 11 #include <std_msgs/Int32.h>
ksingyuunyuu 0:27046fed2426 12 #include <std_msgs/String.h>
ksingyuunyuu 0:27046fed2426 13
ksingyuunyuu 0:27046fed2426 14 // その他マクロ定義
ksingyuunyuu 0:27046fed2426 15 #define SAMPLING_TIME 0.01 // 100Hz
ksingyuunyuu 1:979cc2a56e7e 16 #define STANDARD_RPM 3307
ksingyuunyuu 1:979cc2a56e7e 17 #define LOW_RPM 800
ksingyuunyuu 1:979cc2a56e7e 18 // 入出力モード、各種モードの設定
ksingyuunyuu 2:e59b809f9e0a 19 Ticker control; // 割り込みプログラムの宣言
ksingyuunyuu 2:e59b809f9e0a 20 ros::NodeHandle nh; // ROS使用の宣言
ksingyuunyuu 2:e59b809f9e0a 21 DigitalIn sw1(PC_13);
ksingyuunyuu 2:e59b809f9e0a 22 CAN can1(PB_8, PB_9, 500000); // CAN使用の宣言 CAN名(RXD,TXD,動作周波数)
ksingyuunyuu 0:27046fed2426 23
ksingyuunyuu 0:27046fed2426 24 // グローバル変数の宣言
ksingyuunyuu 2:e59b809f9e0a 25 int can_rpm_left = 0; // V7送信用回転数変数左
ksingyuunyuu 2:e59b809f9e0a 26 int can_rpm_right = 0; // V7送信用回転数変数右
ksingyuunyuu 2:e59b809f9e0a 27 int enc_add = ((10&0xf)<<5)|0x600|0x0a; // V7からCANでエンコーダー回転数を受け取るときのアドレス(直書きだと、なぜか動作しない)
ksingyuunyuu 2:e59b809f9e0a 28
ksingyuunyuu 1:979cc2a56e7e 29 // ここからメッセージ用
ksingyuunyuu 2:e59b809f9e0a 30 char *pub_data;
ksingyuunyuu 0:27046fed2426 31 // 関数のプロトタイプ宣言
ksingyuunyuu 2:e59b809f9e0a 32 void function(void); // 割り込み関数
ksingyuunyuu 2:e59b809f9e0a 33 void all_motor_stop( void ); // CAN全モータストップ指令送信用関数
ksingyuunyuu 2:e59b809f9e0a 34 void motor( int rpm1,int rpm2 ); // CANモータ回転数指令送信用関数
ksingyuunyuu 2:e59b809f9e0a 35 void motor1_drive( int rpm ); // motor1モータ回転数指令送信用関数
ksingyuunyuu 2:e59b809f9e0a 36 void motor2_drive( int rpm ); // motor2モータ回転数指令送信用関数
ksingyuunyuu 2:e59b809f9e0a 37 void MD_mode_change(int mode); // V7モード変更用関数
ksingyuunyuu 2:e59b809f9e0a 38 void messageCb(const geometry_msgs::Twist& msg);// ROS購読者Twist購読関数
ksingyuunyuu 0:27046fed2426 39
ksingyuunyuu 2:e59b809f9e0a 40 // 購読者宣言
ksingyuunyuu 2:e59b809f9e0a 41 //インフォメーション
ksingyuunyuu 0:27046fed2426 42 std_msgs::String str_msg;
ksingyuunyuu 0:27046fed2426 43 ros::Publisher chatter("chatter", &str_msg);
ksingyuunyuu 2:e59b809f9e0a 44 //エンコーダー左
ksingyuunyuu 2:e59b809f9e0a 45 std_msgs::Int32 encleft_msg; //送るメッセージの型を決定する
ksingyuunyuu 2:e59b809f9e0a 46 ros::Publisher pub_encleft("encoder_left", &encleft_msg); //パブリッシュするトピック名とトピックの中身の変数等を宣言する
ksingyuunyuu 2:e59b809f9e0a 47 //エンコーダー右
ksingyuunyuu 2:e59b809f9e0a 48 std_msgs::Int32 encright_msg; //送るメッセージの型を決定する
ksingyuunyuu 2:e59b809f9e0a 49 ros::Publisher pub_encright("encoder_right", &encright_msg); //パブリッシュするトピック名とトピックの中身の変数等を宣言する
ksingyuunyuu 2:e59b809f9e0a 50 // サブスク宣言
ksingyuunyuu 1:979cc2a56e7e 51 ros::Subscriber<geometry_msgs::Twist> sub("kenshiro/diff_drive_controller/cmd_vel", &messageCb);
ksingyuunyuu 0:27046fed2426 52
ksingyuunyuu 0:27046fed2426 53 Timer t;
ksingyuunyuu 0:27046fed2426 54
ksingyuunyuu 1:979cc2a56e7e 55 int main(){
ksingyuunyuu 0:27046fed2426 56
ksingyuunyuu 0:27046fed2426 57 t.start();
ksingyuunyuu 0:27046fed2426 58 long vel_timer = 0;
ksingyuunyuu 0:27046fed2426 59 nh.getHardware()->setBaud(57600);
ksingyuunyuu 0:27046fed2426 60 nh.initNode();
ksingyuunyuu 0:27046fed2426 61 nh.subscribe(sub);
ksingyuunyuu 0:27046fed2426 62 nh.advertise(chatter);
ksingyuunyuu 2:e59b809f9e0a 63 nh.advertise(pub_encleft);
ksingyuunyuu 2:e59b809f9e0a 64 nh.advertise(pub_encright);
ksingyuunyuu 2:e59b809f9e0a 65
ksingyuunyuu 1:979cc2a56e7e 66 CANMessage msg; // CANメッセージを格納する変数
ksingyuunyuu 2:e59b809f9e0a 67 /*
ksingyuunyuu 2:e59b809f9e0a 68 // インフォメーションパブリッシュ
ksingyuunyuu 2:e59b809f9e0a 69 pub_data = "Kenshiro Program";
ksingyuunyuu 2:e59b809f9e0a 70 str_msg.data = pub_data;
ksingyuunyuu 2:e59b809f9e0a 71 chatter.publish( &str_msg );
ksingyuunyuu 2:e59b809f9e0a 72
ksingyuunyuu 2:e59b809f9e0a 73 pub_data = "for TEC-MD7U-A and STM NUCLEO-F446RE";
ksingyuunyuu 2:e59b809f9e0a 74 str_msg.data = pub_data;
ksingyuunyuu 2:e59b809f9e0a 75 chatter.publish( &str_msg );
ksingyuunyuu 2:e59b809f9e0a 76
ksingyuunyuu 2:e59b809f9e0a 77 pub_data = "Program started";
ksingyuunyuu 2:e59b809f9e0a 78 str_msg.data = pub_data;
ksingyuunyuu 2:e59b809f9e0a 79 chatter.publish( &str_msg );
ksingyuunyuu 2:e59b809f9e0a 80
ksingyuunyuu 2:e59b809f9e0a 81 pub_data = "";
ksingyuunyuu 2:e59b809f9e0a 82 str_msg.data = pub_data;
ksingyuunyuu 2:e59b809f9e0a 83 chatter.publish( &str_msg );
ksingyuunyuu 2:e59b809f9e0a 84 // インフォメーションここまで
ksingyuunyuu 2:e59b809f9e0a 85 */
ksingyuunyuu 1:979cc2a56e7e 86
ksingyuunyuu 1:979cc2a56e7e 87 //control.attach(&function, SAMPLING_TIME); //台車の速度制御用のタイマー関数を設定
ksingyuunyuu 1:979cc2a56e7e 88 while (1){
ksingyuunyuu 2:e59b809f9e0a 89 // エンコーダー値読み取り
ksingyuunyuu 2:e59b809f9e0a 90 if (can1.read(msg)) { //メッセージがあったら即実行 read関数はメッセージがあった場合戻り値が1
ksingyuunyuu 2:e59b809f9e0a 91 if ( msg.id == enc_add ){
ksingyuunyuu 2:e59b809f9e0a 92 encleft_msg.data = (int)msg.data[0] << 24 |(int)msg.data[1] << 16 | (int)msg.data[2] << 8 | (int)msg.data[3];
ksingyuunyuu 2:e59b809f9e0a 93 encright_msg.data = (int)msg.data[4] << 24 |(int)msg.data[5] << 16 | (int)msg.data[6] << 8 | (int)msg.data[7];
ksingyuunyuu 2:e59b809f9e0a 94 encleft_msg.data *= -1;
ksingyuunyuu 2:e59b809f9e0a 95 }
ksingyuunyuu 2:e59b809f9e0a 96 }
ksingyuunyuu 2:e59b809f9e0a 97 // 割り込みで代用?
ksingyuunyuu 1:979cc2a56e7e 98 if (t.read_ms() > vel_timer){
ksingyuunyuu 2:e59b809f9e0a 99 pub_encleft.publish(&encleft_msg);//実際にパブリッシュ
ksingyuunyuu 2:e59b809f9e0a 100 pub_encright.publish(&encright_msg);//実際にパブリッシュ
ksingyuunyuu 0:27046fed2426 101 vel_timer = t.read_ms() + 500; // 500ms毎に実行
ksingyuunyuu 0:27046fed2426 102 }
ksingyuunyuu 1:979cc2a56e7e 103 nh.spinOnce(); // 呼び出し待ちのコールバックをここで呼び出す
ksingyuunyuu 2:e59b809f9e0a 104
ksingyuunyuu 2:e59b809f9e0a 105 wait_ms(1); // 連続で呼び出ししないようにウェイトを置く
ksingyuunyuu 0:27046fed2426 106 }
ksingyuunyuu 0:27046fed2426 107 }
ksingyuunyuu 0:27046fed2426 108
ksingyuunyuu 0:27046fed2426 109 // 割り込み関数
ksingyuunyuu 1:979cc2a56e7e 110 /*
ksingyuunyuu 0:27046fed2426 111 void function(void){
ksingyuunyuu 0:27046fed2426 112
ksingyuunyuu 0:27046fed2426 113 }
ksingyuunyuu 1:979cc2a56e7e 114 */
ksingyuunyuu 1:979cc2a56e7e 115
ksingyuunyuu 1:979cc2a56e7e 116 void messageCb(const geometry_msgs::Twist& msg){
ksingyuunyuu 1:979cc2a56e7e 117 double tmp = 0;
ksingyuunyuu 1:979cc2a56e7e 118 double tmp_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 119 double tmp_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 120 if(msg.angular.z < 0){
ksingyuunyuu 1:979cc2a56e7e 121 // 左旋回
ksingyuunyuu 1:979cc2a56e7e 122 tmp = 1 - (-msg.angular.z / 3); //radiun
ksingyuunyuu 1:979cc2a56e7e 123 if( msg.linear.x == 0 ){
ksingyuunyuu 1:979cc2a56e7e 124 // 真左に旋回の場合
ksingyuunyuu 1:979cc2a56e7e 125 tmp_rpm_left = LOW_RPM * tmp;
ksingyuunyuu 1:979cc2a56e7e 126 tmp_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 127 } else {
ksingyuunyuu 1:979cc2a56e7e 128 tmp_rpm_left = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 129 tmp_rpm_right = STANDARD_RPM * msg.linear.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 130 }
ksingyuunyuu 1:979cc2a56e7e 131 } else if(msg.angular.z > 0){
ksingyuunyuu 1:979cc2a56e7e 132 // 右旋回
ksingyuunyuu 1:979cc2a56e7e 133 tmp = 1 - (msg.angular.z / 3); //radiun
ksingyuunyuu 1:979cc2a56e7e 134 if( msg.linear.x == 0 ){
ksingyuunyuu 1:979cc2a56e7e 135 // 真右に旋回の場合
ksingyuunyuu 1:979cc2a56e7e 136 tmp_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 137 tmp_rpm_right = LOW_RPM * tmp;
ksingyuunyuu 1:979cc2a56e7e 138 } else {
ksingyuunyuu 1:979cc2a56e7e 139 tmp_rpm_left = STANDARD_RPM * msg.linear.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 140 tmp_rpm_right = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 141 }
ksingyuunyuu 1:979cc2a56e7e 142 } else {
ksingyuunyuu 1:979cc2a56e7e 143 tmp_rpm_left = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 144 tmp_rpm_right = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 145 }
ksingyuunyuu 1:979cc2a56e7e 146 motor( tmp_rpm_left, tmp_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 147 }
ksingyuunyuu 1:979cc2a56e7e 148 /* CANIDプロトコル
ksingyuunyuu 1:979cc2a56e7e 149 / CANID:xxyyyyzzzzzの11bit中
ksingyuunyuu 1:979cc2a56e7e 150 / 上位2bit:優先度 :小さいものが高優先　[00]:ブロードキャスト用？？ [01]:M-bedから書き込みに使える [10]:MDからくるレポート [11]:不明 メッセージの衝突回避用
ksingyuunyuu 1:979cc2a56e7e 151 / 次の4bit:プロトコルID :[0]高優先度プロトコル　[1]制御プロトコル　[2-3]書き込みプロトコル　[8]高優先度通知プロトコル　[9]イベント通知プロトコル　[10-12]ステータスレポートプロトコル
ksingyuunyuu 1:979cc2a56e7e 152 / 下位5bit:デバイスID :デバイスIDとデバイスの各メールボックス
ksingyuunyuu 1:979cc2a56e7e 153 /
ksingyuunyuu 1:979cc2a56e7e 154 / CANデータフィールド1Byte目:上位４bit：１つ目のデータはレジスタ何個分か　下位4bit：2つ目のデータはレジスタ何個分か RPMCMD1 =2Byte　RPMCMD2 =2Byte
ksingyuunyuu 1:979cc2a56e7e 155 / CANデータフィールド2Byte目:書き込む先のアドレス1　レジスタのアドレス　例）RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:979cc2a56e7e 156 / CANデータフィールド3Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:979cc2a56e7e 157 / CANデータフィールド4Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:979cc2a56e7e 158 / CANデータフィールド5Byte目:書き込む先のアドレス2　レジスタのアドレス　RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:979cc2a56e7e 159 / CANデータフィールド6Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:979cc2a56e7e 160 / CANデータフィールド7Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:979cc2a56e7e 161 / CANデータフィールド8Byte目:
ksingyuunyuu 1:979cc2a56e7e 162 /
ksingyuunyuu 1:979cc2a56e7e 163 */
ksingyuunyuu 1:979cc2a56e7e 164
ksingyuunyuu 1:979cc2a56e7e 165 #define MDcontroll 1
ksingyuunyuu 1:979cc2a56e7e 166 #define MDwrite 2
ksingyuunyuu 1:979cc2a56e7e 167 #define MDwriteBC 3
ksingyuunyuu 1:979cc2a56e7e 168 void all_motor_stop( void ){ // 全モータストップ
ksingyuunyuu 1:979cc2a56e7e 169 //MT1=0rpm MT2=0rpm
ksingyuunyuu 1:979cc2a56e7e 170 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 171 int CANID;
ksingyuunyuu 1:979cc2a56e7e 172
ksingyuunyuu 1:979cc2a56e7e 173 txdata[0] = 0x11;
ksingyuunyuu 1:979cc2a56e7e 174 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 175 txdata[2] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:979cc2a56e7e 176 txdata[3] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:979cc2a56e7e 177 txdata[4] = 0x41;
ksingyuunyuu 1:979cc2a56e7e 178 txdata[5] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 179 txdata[6] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 180
ksingyuunyuu 1:979cc2a56e7e 181 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 182 can1.write(CANMessage(CANID,&txdata[0],7));
ksingyuunyuu 1:979cc2a56e7e 183 }
ksingyuunyuu 1:979cc2a56e7e 184
ksingyuunyuu 1:979cc2a56e7e 185 void motor( int rpm1, int rpm2 ){ // 全モータ前進
ksingyuunyuu 1:979cc2a56e7e 186 //MT1=0rpm MT2=0rpm
ksingyuunyuu 1:979cc2a56e7e 187 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 188 int CANID;
ksingyuunyuu 1:979cc2a56e7e 189
ksingyuunyuu 1:979cc2a56e7e 190 int motor1_rpm = -rpm1;
ksingyuunyuu 1:979cc2a56e7e 191 int motor2_rpm = rpm2;
ksingyuunyuu 1:979cc2a56e7e 192
ksingyuunyuu 1:979cc2a56e7e 193 char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 194 char motor1_data2 = motor1_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 195 char motor2_data1 = (motor2_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 196 char motor2_data2 = motor2_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 197
ksingyuunyuu 1:979cc2a56e7e 198 txdata[0] = 0x11;
ksingyuunyuu 1:979cc2a56e7e 199 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 200 txdata[2] = motor1_data1;
ksingyuunyuu 1:979cc2a56e7e 201 txdata[3] = motor1_data2;
ksingyuunyuu 1:979cc2a56e7e 202 txdata[4] = 0x41;
ksingyuunyuu 1:979cc2a56e7e 203 txdata[5] = motor2_data1;
ksingyuunyuu 1:979cc2a56e7e 204 txdata[6] = motor2_data2;
ksingyuunyuu 1:979cc2a56e7e 205
ksingyuunyuu 1:979cc2a56e7e 206 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 207 can1.write(CANMessage(CANID,&txdata[0],7));
ksingyuunyuu 1:979cc2a56e7e 208 }
ksingyuunyuu 1:979cc2a56e7e 209
ksingyuunyuu 1:979cc2a56e7e 210 void motor1_drive( int rpm ){ // モータ1正転
ksingyuunyuu 1:979cc2a56e7e 211 //MT1=2000rpm
ksingyuunyuu 1:979cc2a56e7e 212 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 213 int CANID;
ksingyuunyuu 1:979cc2a56e7e 214
ksingyuunyuu 1:979cc2a56e7e 215 int motor1_rpm;
ksingyuunyuu 1:979cc2a56e7e 216 if(rpm >= 0){
ksingyuunyuu 1:979cc2a56e7e 217 motor1_rpm = -rpm;
ksingyuunyuu 1:979cc2a56e7e 218 } else {
ksingyuunyuu 1:979cc2a56e7e 219 motor1_rpm = rpm;
ksingyuunyuu 1:979cc2a56e7e 220 }
ksingyuunyuu 1:979cc2a56e7e 221
ksingyuunyuu 1:979cc2a56e7e 222 char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 223 char motor1_data2 = motor1_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 224
ksingyuunyuu 1:979cc2a56e7e 225 txdata[0] = 0x10;
ksingyuunyuu 1:979cc2a56e7e 226 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 227 txdata[2] = motor1_data1;
ksingyuunyuu 1:979cc2a56e7e 228 txdata[3] = motor1_data2;
ksingyuunyuu 1:979cc2a56e7e 229
ksingyuunyuu 1:979cc2a56e7e 230
ksingyuunyuu 1:979cc2a56e7e 231 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 232 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:979cc2a56e7e 233 }
ksingyuunyuu 1:979cc2a56e7e 234
ksingyuunyuu 1:979cc2a56e7e 235 void motor2_drive( int rpm ){ // モータ2前進
ksingyuunyuu 1:979cc2a56e7e 236 //MT2=2000rpm
ksingyuunyuu 1:979cc2a56e7e 237 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 238 int CANID;
ksingyuunyuu 1:979cc2a56e7e 239 char motor2_data1 = (rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 240 char motor2_data2 = rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 241 txdata[0] = 0x10;
ksingyuunyuu 1:979cc2a56e7e 242 txdata[1] = 0x41; //rpmcmd2
ksingyuunyuu 1:979cc2a56e7e 243 txdata[2] = motor2_data1;
ksingyuunyuu 1:979cc2a56e7e 244 txdata[3] = motor2_data2;
ksingyuunyuu 1:979cc2a56e7e 245 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 246 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:979cc2a56e7e 247 }
ksingyuunyuu 1:979cc2a56e7e 248
ksingyuunyuu 1:979cc2a56e7e 249 void MD_mode_change(int mode){ // モータドライバのモード変更関数
ksingyuunyuu 1:979cc2a56e7e 250 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 251 int CANID;
ksingyuunyuu 1:979cc2a56e7e 252
ksingyuunyuu 1:979cc2a56e7e 253 txdata[0] = 0x05;
ksingyuunyuu 1:979cc2a56e7e 254 txdata[1] = mode;
ksingyuunyuu 1:979cc2a56e7e 255 txdata[2] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 256 txdata[3] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 257 txdata[4] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 258 txdata[5] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 259 txdata[6] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 260 txdata[7] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 261
ksingyuunyuu 1:979cc2a56e7e 262 CANID=0x200 | ((MDcontroll)&0xf)<<5 | 0xa ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：制御モード　下位5bit：デバイスID：１０番
ksingyuunyuu 1:979cc2a56e7e 263 can1.write(CANMessage(CANID,&txdata[0],8));
ksingyuunyuu 1:979cc2a56e7e 264
ksingyuunyuu 0:27046fed2426 265 }