Diff: main.cpp

Revision:

3:e5463b529172

Parent:

2:e59b809f9e0a

--- a/main.cpp	Sat May 25 14:09:18 2019 +0000
+++ b/main.cpp	Tue Sep 03 13:42:08 2019 +0000
@@ -1,6 +1,6 @@
 /*******************************************************
- * Kenshiro Program for TEC-MD7U-A and STM NUCLEO-F446RE
- * Version 1.0
+ * Kenshiro for TEC-MD7U-A and STM NUCLEO-F446RE
+ * Version 1.0.1
  * Created Ksingyuunyuu
  *******************************************************/
 // インクルード先の宣言
@@ -10,15 +10,21 @@
 #include <geometry_msgs/Twist.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
-
+#include <std_msgs/Bool.h>
 // その他マクロ定義
 #define SAMPLING_TIME           0.01    // 100Hz
 #define STANDARD_RPM            3307
 #define LOW_RPM                 800
+#define WEEL                    0.075   // タイヤ半径
+#define TREAD                   0.43    // トレッド
+#define DIFF_GEAR               26
+#define PI                      3.1415
 // 入出力モード、各種モードの設定
 Ticker control;                 // 割り込みプログラムの宣言
 ros::NodeHandle nh;             // ROS使用の宣言
 DigitalIn sw1(PC_13);
+DigitalIn emangency_sw(D5);
+DigitalOut led(D6);
 CAN can1(PB_8, PB_9, 500000);   // CAN使用の宣言 CAN名(RXD,TXD,動作周波数)
 
 // グローバル変数の宣言
@@ -37,7 +43,7 @@
 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);
@@ -47,7 +53,10 @@
 //エンコーダー右
 std_msgs::Int32 encright_msg; //送るメッセージの型を決定する
 ros::Publisher pub_encright("encoder_right", &encright_msg);  //パブリッシュするトピック名とトピックの中身の変数等を宣言する
-// サブスク宣言
+//電源センサ(フォトカプラ)
+std_msgs::Bool emangencysw_msg;
+ros::Publisher pub_emangencysw("emergency", &emangencysw_msg);
+//サブスク宣言
 ros::Subscriber<geometry_msgs::Twist> sub("kenshiro/diff_drive_controller/cmd_vel", &messageCb);
 
 Timer t;
@@ -56,35 +65,16 @@
     
   t.start();
   long vel_timer = 0;
-  nh.getHardware()->setBaud(57600);
+  nh.getHardware()->setBaud(115200);
   nh.initNode();
   nh.subscribe(sub);
   nh.advertise(chatter);
   nh.advertise(pub_encleft);
   nh.advertise(pub_encright);
+  nh.advertise(pub_emangencysw);
     
   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
@@ -94,10 +84,14 @@
         encleft_msg.data *= -1;
       }
     }
+    
     // 割り込みで代用?
     if (t.read_ms() > vel_timer){
         pub_encleft.publish(&encleft_msg);//実際にパブリッシュ
         pub_encright.publish(&encright_msg);//実際にパブリッシュ
+        emangencysw_msg.data = emangency_sw;
+        pub_emangencysw.publish(&emangencysw_msg);
+        led = emangency_sw;
         vel_timer = t.read_ms() + 500;    // 500ms毎に実行
     }
     nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
@@ -114,35 +108,11 @@
 */
 
 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;
-  }
+  
+  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プロトコル