Diff: main.cpp

Revision:

4:c79b95a92466

Parent:

3:e5463b529172

--- a/main.cpp	Tue Sep 03 13:42:08 2019 +0000
+++ b/main.cpp	Sun Feb 09 09:38:02 2020 +0000
@@ -62,7 +62,7 @@
 Timer t;
 
 int main(){
-    
+
   t.start();
   long vel_timer = 0;
   nh.getHardware()->setBaud(115200);
@@ -72,9 +72,9 @@
   nh.advertise(pub_encleft);
   nh.advertise(pub_encright);
   nh.advertise(pub_emangencysw);
-    
+
   CANMessage msg;                     // CANメッセージを格納する変数
-  
+
   while (1){
     // エンコーダー値読み取り
     if (can1.read(msg)) {  //メッセージがあったら即実行 read関数はメッセージがあった場合戻り値が1
@@ -84,7 +84,7 @@
         encleft_msg.data *= -1;
       }
     }
-    
+
     // 割り込みで代用?
     if (t.read_ms() > vel_timer){
         pub_encleft.publish(&encleft_msg);//実際にパブリッシュ
@@ -95,7 +95,7 @@
         vel_timer = t.read_ms() + 500;    // 500ms毎に実行
     }
     nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
-    
+
     wait_ms(1);     // 連続で呼び出ししないようにウェイトを置く
   }
 }
@@ -110,17 +110,17 @@
 void messageCb(const geometry_msgs::Twist& msg){
   double tmp_rpm_left = 0;
   double tmp_rpm_right = 0;
-  
+
   tmp_rpm_left = 1 / WEEL * (msg.linear.x - TREAD / 2 * msg.angular.z) * 30 / PI * DIFF_GEAR;
   tmp_rpm_right = 1 / WEEL * (msg.linear.x + TREAD / 2 * msg.angular.z) * 30 / PI * DIFF_GEAR;
   motor( tmp_rpm_left, tmp_rpm_right );
 }
-/*  CANIDプロトコル    
-/   CANID:xxyyyyzzzzzの11bit中    
+/*  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
@@ -139,7 +139,7 @@
     //MT1=0rpm MT2=0rpm
     char txdata[8];
     int CANID;
-    
+
     txdata[0] = 0x11;
     txdata[1] = 0x40;   //rpmcmd1
     txdata[2] = 0x00;   //MT1=-2000rpm
@@ -149,22 +149,22 @@
     txdata[6] = 0x00;
 
     CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
-    can1.write(CANMessage(CANID,&txdata[0],7)); 
+    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;
@@ -174,24 +174,24 @@
     txdata[6] = motor2_data2;
 
     CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
-    can1.write(CANMessage(CANID,&txdata[0],7)); 
+    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;
@@ -219,7 +219,7 @@
 void MD_mode_change(int mode){  // モータドライバのモード変更関数
     char txdata[8];
     int CANID;
-    
+
     txdata[0] = 0x05;
     txdata[1] = mode;
     txdata[2] = 0x00;
@@ -228,8 +228,7 @@
     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)); 
-
+    can1.write(CANMessage(CANID,&txdata[0],8));
 }
\ No newline at end of file