Diff: main.cpp

Revision:

1:979cc2a56e7e

Parent:

0:27046fed2426

Child:

2:e59b809f9e0a

--- a/main.cpp	Wed Feb 13 09:31:05 2019 +0000
+++ b/main.cpp	Fri May 17 07:40:50 2019 +0000
@@ -12,91 +12,46 @@
 #include "mbed.h"
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
-#include "QEI.h"
 #include <std_msgs/String.h>
 
-// マクロ定義(ピン配置)
-#define D0  PA_3
-#define D1  PA_2
-#define D2  PA_10
-#define D3  PB_3
-#define D4  PB_5
-#define D5  PB_4
-#define D6  PB_10
-#define D7  PA_8
-#define D8  PA_9
-#define D9  PC_7
-#define D10 PB_6
-#define D11 PA_7
-#define D12 PA_6
-#define D13 PA_5
-// アナログ
-#define A0  PA_0
-#define A1  PA_1
-#define A2  PA_4
-#define A3  PB_0
-#define A4  PC_1
-#define A5  PC_0
+// マクロ定義(ピン配置)F446
 
-#define B1  PC_13
+
 // その他マクロ定義
 #define SAMPLING_TIME           0.01    // 100Hz
-#define ENC_PULSE_PER_METER     1965
-#define KP_LEFT     0.70
-#define KI_LEFT     0
-#define KD_LEFT     0
-#define KP_RIGHT    0.75
-#define KI_RIGHT    0
-#define KD_RIGHT    0
-#define SPEED_LEFT  (double)(get_enc_left / 19.65)
-#define SPEED_RIGHT (double)(get_enc_right / 19.65)
+#define STANDARD_RPM            3307
+#define LOW_RPM                 800
+// 入出力モード、各種モードの設定
 
-// 入出力モード、各種モードの設定
-BusIn sw(B1);
-BusIn in(D5, D6, D11, D12);
-PwmOut motor_left(D4);
-PwmOut motor_right(D10);
-DigitalOut left(D3);
-DigitalOut right(D9);
-QEI enc_left(D5, D6, NC, 48, QEI::X4_ENCODING);
-QEI enc_right(D12, D11, NC, 48, QEI::X4_ENCODING);
 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 get_enc_left;
-int get_enc_right;
-int enc_center;
-double SpeedIntegral_left = 0;
-double iSpeedBefore_left = 0;
-double TargetSpeed_left;
-double pwm_left;
-double SpeedIntegral_right = 0;
-double iSpeedBefore_right = 0;
-double TargetSpeed_right;
-double pwm_right;
-char str_left[13] = "mode left   ";
-char str_right[13] = "mode right  ";
-char str_forward[13] = "mode forward";
-char str_back[13] = "mode back   ";
-char str_nutral[13] = "mode nutral ";
+int can_rpm_left = 0;
+int can_rpm_right = 0;
+// ここからメッセージ用
+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 ";
 // 関数のプロトタイプ宣言
 void function(void);
-void motor_speed_left( void );
-void motor_speed_right( 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);
 
 std_msgs::String str_msg;
 ros::Publisher chatter("chatter", &str_msg);
-ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb);
+ros::Subscriber<geometry_msgs::Twist> sub("kenshiro/diff_drive_controller/cmd_vel", &messageCb);
 
 Timer t;
 
-int main()
-{
-  
-  in.mode(PullUp);
-    control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
+int main(){
     
   t.start();
   long vel_timer = 0;
@@ -104,124 +59,297 @@
   nh.initNode();
   nh.subscribe(sub);
   nh.advertise(chatter);
-  while (1)
-  {
-    if (t.read_ms() > vel_timer)
-    {
+  
+  CANMessage msg;                     // CANメッセージを格納する変数
+  //can1.frequency(500000);         // CANのビットレートを500kHzに設定
+  
+  //control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
+  while (1){
+    // これ意味ないんじゃないの？
+    if (t.read_ms() > vel_timer){
       // 停止
-        TargetSpeed_left = 0;
-        TargetSpeed_right = 0;
-        motor_left = 0;
-        motor_right = 0;
+      /*
+        can_rpm_left = 0;
+        can_rpm_right = 0;
+        */
         vel_timer = t.read_ms() + 500;    // 500ms毎に実行
     }
-    nh.spinOnce();
-    motor_left = pwm_left;
-    motor_right = pwm_right;
+    nh.spinOnce();  // 呼び出し待ちのコールバックをここで呼び出す
+    // ここでCANの動作？、ここでダメなら、割り込みでCAN
+    //motor1_drive( can_rpm_left );
+    //motor2_drive( can_rpm_right );    
     wait_ms(1);
   }
 }
 
 // 割り込み関数
+/*
 void function(void){
-    motor_speed_left();
-    motor_speed_right();
-    }
 
-void motor_speed_left( void ){
-    double out;
-    get_enc_left = enc_left.getPulses();
-    enc_left.reset();
-    double div_speed_left = TargetSpeed_left - SPEED_LEFT;
-    out = KP_LEFT * div_speed_left;
-    if(out < 0){
-        out *= -1;
-        left = 1;
-    } else {
-        left = 0;        
-    }
-    pwm_left = out;
-} 
-void motor_speed_right( void ){
-    double out;
-    get_enc_right = enc_right.getPulses();
-    enc_right.reset();
-    double div_speed_right = TargetSpeed_right - SPEED_RIGHT;
-    out = KP_RIGHT * div_speed_right;
-    if(out < 0){
-        out *= -1;
-        right = 1;
-    } else {
-        right = 0;        
-    }
-    pwm_right = out;
 }
-
+*/
+/*
 void messageCb(const geometry_msgs::Twist& msg){
   double tmp = 0;
-  
-  TargetSpeed_left = msg.linear.x;
-  TargetSpeed_right = msg.linear.x;
-  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){
-      tmp = 1 - (-msg.angular.z / 3);
-      TargetSpeed_left = msg.linear.x;
-      TargetSpeed_right = msg.linear.x * tmp;
+      // 左旋回
+      // 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){
-      tmp = 1 - (msg.angular.z / 3);
-      TargetSpeed_left = msg.linear.x * tmp;
-      TargetSpeed_right = msg.linear.x;
+      // 右旋回
+      // 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 {
-      TargetSpeed_right = msg.linear.x;
-      TargetSpeed_right = msg.linear.x;
+      // 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 );
+    // パブリッシュここまで
   }
-  motor_left = pwm_left;
-  motor_right = pwm_right;
-  
-  /*
+  // 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){
     // 停止
-    TargetSpeed_left = 0;
-    TargetSpeed_right = 0;
-    motor_left = pwm_left;
-    motor_right = pwm_right;
-    str_msg.data = str_nutral;
+    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){
       // 右旋回
-        TargetSpeed_left = 1.0;
-        TargetSpeed_right = 0;
-        motor_left = pwm_left;
-        motor_right = pwm_right;
-        str_msg.data = str_right;
+        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){
       // 左旋回
-        TargetSpeed_left = 0;
-        TargetSpeed_right = 1.0;
-        motor_left = pwm_left;
-        motor_right = pwm_right;
-        str_msg.data = str_left;
+        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){
       // 後進
-        TargetSpeed_left = -1.0;
-        TargetSpeed_right = -1.0;
-        motor_left = pwm_left;
-        motor_right = pwm_right;
-        str_msg.data = str_back;
+        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){
       // 前進
-        TargetSpeed_left = 1.0;
-        TargetSpeed_right = 1.0;
-        motor_left = pwm_left;
-        motor_right = pwm_right;
-        str_msg.data = str_forward;
+        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;
+  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;
+  }
+  motor( tmp_rpm_left, tmp_rpm_right );
+}
+/*  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
+/   CANデータフィールド4Byte目:例）回転数1000の下位2bit
+/   CANデータフィールド5Byte目:書き込む先のアドレス2　レジスタのアドレス　RPMCMD1 =0x40 (64番目
+/   CANデータフィールド6Byte目:例）回転数1000の上位2bit
+/   CANデータフィールド7Byte目:例）回転数1000の下位2bit
+/   CANデータフィールド8Byte目:
+/
+*/
+
+#define MDcontroll  1
+#define MDwrite     2
+#define MDwriteBC   3
+void all_motor_stop( void ){        // 全モータストップ
+    //MT1=0rpm MT2=0rpm
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x11;
+    txdata[1] = 0x40;   //rpmcmd1
+    txdata[2] = 0x00;   //MT1=-2000rpm
+    txdata[3] = 0x00;   //MT1=-2000rpm
+    txdata[4] = 0x41;
+    txdata[5] = 0x00;
+    txdata[6] = 0x00;
+
+    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
+    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;
+    txdata[3] = motor1_data2;
+    txdata[4] = 0x41;
+    txdata[5] = motor2_data1;
+    txdata[6] = motor2_data2;
+
+    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正転
+    //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;
+    txdata[3] = motor1_data2;
+
+
+    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前進
+    //MT2=2000rpm
+    char txdata[8];
+    int CANID;
+    char motor2_data1 = (rpm & 0xff00) >> 8;
+    char motor2_data2 = rpm & 0x00ff;
+    txdata[0] = 0x10;
+    txdata[1] = 0x41;   //rpmcmd2
+    txdata[2] = motor2_data1;
+    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){  // モータドライバのモード変更関数
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x05;
+    txdata[1] = mode;
+    txdata[2] = 0x00;
+    txdata[3] = 0x00;
+    txdata[4] = 0x00;
+    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)); 
+
 }
\ No newline at end of file