K AKAGAWA / Mbed 2 deprecated F446_rosserial_V7_EncoderVersion_102

kenshiro program

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

main.cpp@3:e5463b529172, 2019-09-03 (annotated)

Committer:
ksingyuunyuu
Date:
Tue Sep 03 13:42:08 2019 +0000
Revision:
3:e5463b529172
Parent:
2:e59b809f9e0a
Child:
4:c79b95a92466
kenshiro_emargency_sw_program;

Who changed what in which revision?

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