Diff: main.cpp

Revision:

2:e59b809f9e0a

Parent:

1:979cc2a56e7e

Child:

3:e5463b529172

--- a/main.cpp	Fri May 17 07:40:50 2019 +0000
+++ b/main.cpp	Sat May 25 14:09:18 2019 +0000
@@ -1,52 +1,53 @@
-/*
- * 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/
- */
-
+/*******************************************************
+ * 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>
 
-// マクロ定義(ピン配置)F446
-
-
 // その他マクロ定義
 #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,動作周波数)
 
-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;
+int can_rpm_left = 0;                   // V7送信用回転数変数左
+int can_rpm_right = 0;                  // V7送信用回転数変数右
+int enc_add = ((10&0xf)<<5)|0x600|0x0a; // V7からCANでエンコーダー回転数を受け取るときのアドレス(直書きだと、なぜか動作しない)
+
 // ここからメッセージ用
-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 ";
+char *pub_data;
 // 関数のプロトタイプ宣言
-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);
+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;
@@ -59,26 +60,49 @@
   nh.initNode();
   nh.subscribe(sub);
   nh.advertise(chatter);
-  
+  nh.advertise(pub_encleft);
+  nh.advertise(pub_encright);
+    
   CANMessage msg;                     // CANメッセージを格納する変数
-  //can1.frequency(500000);         // CANのビットレートを500kHzに設定
+  /*
+  // インフォメーションパブリッシュ
+  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){
-      // 停止
-      /*
-        can_rpm_left = 0;
-        can_rpm_right = 0;
-        */
+        pub_encleft.publish(&encleft_msg);//実際にパブリッシュ
+        pub_encright.publish(&encright_msg);//実際にパブリッシュ
         vel_timer = t.read_ms() + 500;    // 500ms毎に実行
     }
     nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
-    // ここでCANの動作？、ここでダメなら、割り込みでCAN
-    //motor1_drive( can_rpm_left );
-    //motor2_drive( can_rpm_right );    
-    wait_ms(1);
+    
+    wait_ms(1);     // 連続で呼び出ししないようにウェイトを置く
   }
 }
 
@@ -88,106 +112,6 @@
 
 }
 */
-/*
-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;
@@ -281,12 +205,6 @@
 
     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正転
@@ -312,10 +230,6 @@
 
     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前進
@@ -330,10 +244,6 @@
     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){  // モータドライバのモード変更関数