Revision 1:979cc2a56e7e, committed 2019-05-17

Comitter:

ksingyuunyuu

Date:

Fri May 17 07:40:50 2019 +0000

Parent:

0:27046fed2426

Commit message:

20190517;

Changed in this revision

diff -r 27046fed2426 -r 979cc2a56e7e CAN.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN.h	Fri May 17 07:40:50 2019 +0000
@@ -0,0 +1,243 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2013 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_CAN_H
+#define MBED_CAN_H
+
+#include "platform.h"
+
+#if DEVICE_CAN
+
+#include "can_api.h"
+#include "can_helper.h"
+#include "FunctionPointer.h"
+
+namespace mbed {
+
+/** CANMessage class
+ */
+class CANMessage : public CAN_Message {
+
+public:
+    /** Creates empty CAN message.
+     */
+    CANMessage() : CAN_Message() {
+        len    = 8;
+        type   = CANData;
+        format = CANStandard;
+        id     = 0;
+        memset(data, 0, 8);
+    }
+
+    /** Creates CAN message with specific content.
+     */
+    CANMessage(int _id, const char *_data, char _len = 8, CANType _type = CANData, CANFormat _format = CANStandard) {
+      len    = _len & 0xF;
+      type   = _type;
+      format = _format;
+      id     = _id;
+      memcpy(data, _data, _len);
+    }
+
+    /** Creates CAN remote message.
+     */
+    CANMessage(int _id, CANFormat _format = CANStandard) {
+      len    = 0;
+      type   = CANRemote;
+      format = _format;
+      id     = _id;
+      memset(data, 0, 8);
+    }
+};
+
+/** A can bus client, used for communicating with can devices
+ */
+class CAN {
+
+public:
+    /** Creates an CAN interface connected to specific pins.
+     *
+     *  @param rd read from transmitter
+     *  @param td transmit to transmitter
+     *
+     * Example:
+     * @code
+     * #include "mbed.h"
+     *
+     * Ticker ticker;
+     * DigitalOut led1(LED1);
+     * DigitalOut led2(LED2);
+     * CAN can1(p9, p10);
+     * CAN can2(p30, p29);
+     *
+     * char counter = 0;
+     *
+     * void send() {
+     *     if(can1.write(CANMessage(1337, &counter, 1))) {
+     *         printf("Message sent: %d\n", counter);
+     *         counter++;
+     *     }
+     *     led1 = !led1;
+     * }
+     *
+     * int main() {
+     *     ticker.attach(&send, 1);
+     *    CANMessage msg;
+     *     while(1) {
+     *         if(can2.read(msg)) {
+     *             printf("Message received: %d\n\n", msg.data[0]);
+     *             led2 = !led2;
+     *         }
+     *         wait(0.2);
+     *     }
+     * }
+     * @endcode
+     */
+    CAN(PinName rd, PinName td);
+    virtual ~CAN();
+
+    /** Set the frequency of the CAN interface
+     *
+     *  @param hz The bus frequency in hertz
+     *
+     *  @returns
+     *    1 if successful,
+     *    0 otherwise
+     */
+    int frequency(int hz);
+
+    /** Write a CANMessage to the bus.
+     *
+     *  @param msg The CANMessage to write.
+     *
+     *  @returns
+     *    0 if write failed,
+     *    1 if write was successful
+     */
+    int write(CANMessage msg);
+
+    /** Read a CANMessage from the bus.
+     *
+     *  @param msg A CANMessage to read to.
+     *  @param handle message filter handle (0 for any message)
+     *
+     *  @returns
+     *    0 if no message arrived,
+     *    1 if message arrived
+     */
+    int read(CANMessage &msg, int handle = 0);
+
+    /** Reset CAN interface.
+     *
+     * To use after error overflow.
+     */
+    void reset();
+
+    /** Puts or removes the CAN interface into silent monitoring mode
+     *
+     *  @param silent boolean indicating whether to go into silent mode or not
+     */
+    void monitor(bool silent);
+
+    enum Mode {
+        Reset = 0,
+        Normal,
+        Silent,
+        LocalTest,
+        GlobalTest,
+        SilentTest
+    };
+
+    /** Change CAN operation to the specified mode
+     *
+     *  @param mode The new operation mode (CAN::Normal, CAN::Silent, CAN::LocalTest, CAN::GlobalTest, CAN::SilentTest)
+     *
+     *  @returns
+     *    0 if mode change failed or unsupported,
+     *    1 if mode change was successful
+     */
+    int mode(Mode mode);
+
+    /** Filter out incomming messages
+     *
+     *  @param id the id to filter on
+     *  @param mask the mask applied to the id
+     *  @param format format to filter on (Default CANAny)
+     *  @param handle message filter handle (Optional)
+     *
+     *  @returns
+     *    0 if filter change failed or unsupported,
+     *    new filter handle if successful
+     */
+    int filter(unsigned int id, unsigned int mask, CANFormat format = CANAny, int handle = 0);
+
+    /** Returns number of read errors to detect read overflow errors.
+     */
+    unsigned char rderror();
+
+    /** Returns number of write errors to detect write overflow errors.
+     */
+    unsigned char tderror();
+
+    enum IrqType {
+        RxIrq = 0,
+        TxIrq,
+        EwIrq,
+        DoIrq,
+        WuIrq,
+        EpIrq,
+        AlIrq,
+        BeIrq,
+        IdIrq
+    };
+
+    /** Attach a function to call whenever a CAN frame received interrupt is
+     *  generated.
+     *
+     *  @param fptr A pointer to a void function, or 0 to set as none
+     *  @param event Which CAN interrupt to attach the member function to (CAN::RxIrq for message received, CAN::TxIrq for transmitted or aborted, CAN::EwIrq for error warning, CAN::DoIrq for data overrun, CAN::WuIrq for wake-up, CAN::EpIrq for error passive, CAN::AlIrq for arbitration lost, CAN::BeIrq for bus error)
+     */
+    void attach(void (*fptr)(void), IrqType type=RxIrq);
+
+   /** Attach a member function to call whenever a CAN frame received interrupt
+    *  is generated.
+    *
+    *  @param tptr pointer to the object to call the member function on
+    *  @param mptr pointer to the member function to be called
+    *  @param event Which CAN interrupt to attach the member function to (CAN::RxIrq for message received, TxIrq for transmitted or aborted, EwIrq for error warning, DoIrq for data overrun, WuIrq for wake-up, EpIrq for error passive, AlIrq for arbitration lost, BeIrq for bus error)
+    */
+   template<typename T>
+   void attach(T* tptr, void (T::*mptr)(void), IrqType type=RxIrq) {
+        if((mptr != NULL) && (tptr != NULL)) {
+            _irq[type].attach(tptr, mptr);
+            can_irq_set(&_can, (CanIrqType)type, 1);
+        }
+        else {
+            can_irq_set(&_can, (CanIrqType)type, 0);
+        }
+    }
+
+    static void _irq_handler(uint32_t id, CanIrqType type);
+
+protected:
+    can_t           _can;
+    FunctionPointer _irq[9];
+};
+
+} // namespace mbed
+
+#endif
+
+#endif    // MBED_CAN_H

diff -r 27046fed2426 -r 979cc2a56e7e can_api.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can_api.h	Fri May 17 07:40:50 2019 +0000
@@ -0,0 +1,80 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2013 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_CAN_API_H
+#define MBED_CAN_API_H
+
+#include "device.h"
+
+#if DEVICE_CAN
+
+#include "PinNames.h"
+#include "PeripheralNames.h"
+#include "can_helper.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    IRQ_RX,
+    IRQ_TX,
+    IRQ_ERROR,
+    IRQ_OVERRUN,
+    IRQ_WAKEUP,
+    IRQ_PASSIVE,
+    IRQ_ARB,
+    IRQ_BUS,
+    IRQ_READY
+} CanIrqType;
+
+
+typedef enum {
+    MODE_RESET,
+    MODE_NORMAL,
+    MODE_SILENT,
+    MODE_TEST_LOCAL,
+    MODE_TEST_GLOBAL,
+    MODE_TEST_SILENT
+} CanMode;
+
+typedef void (*can_irq_handler)(uint32_t id, CanIrqType type);
+
+typedef struct can_s can_t;
+
+void          can_init     (can_t *obj, PinName rd, PinName td);
+void          can_free     (can_t *obj);
+int           can_frequency(can_t *obj, int hz);
+
+void          can_irq_init (can_t *obj, can_irq_handler handler, uint32_t id);
+void          can_irq_free (can_t *obj);
+void          can_irq_set  (can_t *obj, CanIrqType irq, uint32_t enable);
+
+int           can_write    (can_t *obj, CAN_Message, int cc);
+int           can_read     (can_t *obj, CAN_Message *msg, int handle);
+int           can_mode     (can_t *obj, CanMode mode);
+int           can_filter(can_t *obj, uint32_t id, uint32_t mask, CANFormat format, int32_t handle);
+void          can_reset    (can_t *obj);
+unsigned char can_rderror  (can_t *obj);
+unsigned char can_tderror  (can_t *obj);
+void          can_monitor  (can_t *obj, int silent);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif    // MBED_CAN_API_H
+
+#endif

diff -r 27046fed2426 -r 979cc2a56e7e can_helper.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can_helper.h	Fri May 17 07:40:50 2019 +0000
@@ -0,0 +1,53 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2013 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_CAN_HELPER_H
+#define MBED_CAN_HELPER_H
+
+#if DEVICE_CAN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum CANFormat {
+    CANStandard = 0,
+    CANExtended = 1,
+    CANAny = 2
+};
+typedef enum CANFormat CANFormat;
+
+enum CANType {
+    CANData   = 0,
+    CANRemote = 1
+};
+typedef enum CANType CANType;
+
+struct CAN_Message {
+    unsigned int   id;                 // 29 bit identifier
+    unsigned char  data[8];            // Data field
+    unsigned char  len;                // Length of data field in bytes
+    CANFormat      format;             // 0 - STANDARD, 1- EXTENDED IDENTIFIER
+    CANType        type;               // 0 - DATA FRAME, 1 - REMOTE FRAME
+};
+typedef struct CAN_Message CAN_Message;
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif
+
+#endif // MBED_CAN_HELPER_H

diff -r 27046fed2426 -r 979cc2a56e7e main.cpp
--- 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