V7's MotorTestProgram for F446

Dependencies:   mbed ros_lib_kinetic USBDevice

Files at this revision

API Documentation at this revision

Comitter:
ksingyuunyuu
Date:
Mon Apr 15 11:37:19 2019 +0000
Parent:
0:27046fed2426
Commit message:
V7's MotorTestProgram for F446

Changed in this revision

CAN.h Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show diff for this revision Revisions of this file
can_api.h Show annotated file Show diff for this revision Revisions of this file
can_helper.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN.h	Mon Apr 15 11:37:19 2019 +0000
@@ -0,0 +1,244 @@
+/* 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
+
--- a/QEI.lib	Wed Feb 13 09:31:05 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can_api.h	Mon Apr 15 11:37:19 2019 +0000
@@ -0,0 +1,81 @@
+/* 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
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can_helper.h	Mon Apr 15 11:37:19 2019 +0000
@@ -0,0 +1,54 @@
+/* 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
+
--- a/main.cpp	Wed Feb 13 09:31:05 2019 +0000
+++ b/main.cpp	Mon Apr 15 11:37:19 2019 +0000
@@ -1,227 +1,263 @@
+// インクルード先の宣言
+#include "mbed.h"
+// マクロ定義
+#define MDcontroll  1
+#define MDwrite     2
+#define MDwriteBC   3
 /*
- * 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/
- */
+// 機能の宣言、設定
+CAN can1(PB_8, PB_9);   // can_name( RXD, TXD )
+Ticker ticker;          // 関数tickerを割り込みに使用することの宣言
+// グローバル変数の宣言
 
-#include "mbed.h"
-#include <ros.h>
-#include <geometry_msgs/Twist.h>
-#include "QEI.h"
-#include <std_msgs/String.h>
+char counter = 0;
+ 
+void send() {
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x10;
+    txdata[1] = 0x40;   //rpmcmd1
+    txdata[2] = 0x07;   //MT1=2000rpm
+    txdata[3] = 0xD0;   //MT1=2000rpm
 
-// マクロ定義(ピン配置)
-#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
+    CANID=0x200|((MDwrite)&0xf)<<5|0x0a ;
+    printf("send()\n");
+    if(can1.write(CANMessage(CANID,&txdata[0],4))) {
+        printf("wloop()\n");
+        counter++;
+        printf("Message sent: %d\n", counter);
+    } 
+
+}
+ 
+int main() {
+    printf("main()\n");
+    can1.frequency(500000);
+    CANMessage msg;
+    ticker.attach(&send, 1);
+    
 
-#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)
+        wait(0.2);
+}
+*/
+CAN can1(PB_8, PB_9);   // can_name( RXD, TXD )
+Ticker ticker;          // 関数tickerを割り込みに使用することの宣言
+// グローバル変数の宣言
+int pattern = 0;
+char input;
+int state = 1;
+int dir = 1;
+// 関数のプロトタイプ宣言
+void all_motor_stop( void );
+void motor1_forward( void );
+void motor1_back( void );
+void motor2_forward( void );
+void motor2_back( void );
+void MD_mode_change(int mode);
 
-// 入出力モード、各種モードの設定
-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;
-
-// グローバル変数の宣言
-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 ";
-// 関数のプロトタイプ宣言
-void function(void);
-void motor_speed_left( void );
-void motor_speed_right( void );
-void messageCb(const geometry_msgs::Twist& msg);
+int main() {
+        // 割り込みの設定
+    //ticker.attach(&LEDwave, 0.2);   
+        //CANの設定
+    CANMessage msg;                     // CANメッセージを格納する変数
+    can1.frequency(500000);         // CANのビットレートを500kHzに設定
+    
+           // while(pc.readable() == 0);
+            pattern = 0;
+            while( 1 ){ 
+                switch( pattern ){
+                    case 0:
+                        all_motor_stop();       // モータストップ状態
+                        printf("\r\n");
+                        printf("TEC-MD7U-A TestProgram for NUCLEO-F446RE\r\n");
+                        printf("\r\n");
+                        printf("1: Motor1 Forward\r\n");
+                        printf("2: Motor1 Back\r\n");
+                        printf("3: Motor2 Forward\r\n");
+                        printf("4: Motor2 Back\r\n");
+                        printf("\r\n");
+                        printf("Press any key of 1-4\r\n");
+                        printf("And Press Enter\r\n");
+                        pattern = 1;            // 次のパターンへ
+                        break;
+                    
+                    case 1:
+                        scanf("%d",&input);             // 入力待ち
+                        if( input > 0 && input < 5 ){   // 1-4以外のキーボード入力は受け付けない
+                            printf("%d\r\n",input);     // 入力データを表示する
+                            pattern = (input * 10) + 1; // 遷移先に飛ぶ
+                            break;
+                        }
+                        break;
+                    
+                    case 11:
+                    // モータ1前進パターン
+                        motor1_forward();                   // モータ1前転2000回転
+                        printf("MotorForwardActive");
+                        printf("Press '0' ,StopMotor");
+                        scanf("%d",&input);                 // 入力待機状態
+                        if( input == 0 ){                   // 入力に0が渡されたら、待機状態に移行する
+                            pattern = 0;
+                            break;
+                        }
+                        break;
+                        
+                        case 21:
+                        motor1_back();                      // モータ1後転2000回転
+                        printf("MotorForwardActive");
+                        printf("Press'0',StopMotor");
+                        scanf("%d",&input);                 // 入力待機状態
+                        if( input == 0 ){                   // 入力に0が渡されたら、待機状態に移行する
+                            pattern = 0;
+                            break;
+                        }
+                        break;
+                        
+                        case 31:
+                        motor2_forward();                   // モータ2前転2000回転
+                        printf("MotorForwardActive");
+                        printf("Press'0',StopMotor");
+                        scanf("%d",&input);                 // 入力待機状態
+                        if( input == 0 ){                   // 入力に0が渡されたら、待機状態に移行する
+                            pattern = 0;
+                            break;
+                        }
+                        break;
+                        
+                        case 41:
+                        motor2_back();
+                        printf("MotorForwardActive");
+                        printf("Press'0',StopMotor");
+                        scanf("%d",&input);
+                        if( input == 0 ){
+                            pattern = 0;
+                            break;
+                        }
+                        break;
+                        
+                    default:
+                        printf("\r\n");
+                        printf("Invalid Value\r\n");
+                        printf("\r\n");
+                        pattern = 0;
+                        break;
+                }   // switch
+            }   // while
+}                   // main
 
-std_msgs::String str_msg;
-ros::Publisher chatter("chatter", &str_msg);
-ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb);
 
-Timer t;
+
 
-int main()
-{
-  
-  in.mode(PullUp);
-    control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
+/*  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目:上位4bit:1つ目のデータはレジスタ何個分か 下位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;
     
-  t.start();
-  long vel_timer = 0;
-  nh.getHardware()->setBaud(57600);
-  nh.initNode();
-  nh.subscribe(sub);
-  nh.advertise(chatter);
-  while (1)
-  {
-    if (t.read_ms() > vel_timer)
-    {
-      // 停止
-        TargetSpeed_left = 0;
-        TargetSpeed_right = 0;
-        motor_left = 0;
-        motor_right = 0;
-        vel_timer = t.read_ms() + 500;    // 500ms毎に実行
-    }
-    nh.spinOnce();
-    motor_left = pwm_left;
-    motor_right = pwm_right;
-    wait_ms(1);
-  }
+    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 ;  // 上位2ビット:優先度:01 次の4bit:プロトコルID:受信(MD側) 下位5bit:デバイスID:01番
+    can1.write(CANMessage(CANID,&txdata[0],7)); 
+}
+void motor1_forward( void ){                                // モータ1正転2000rpm
+    //MT1=2000rpm
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x10;
+    txdata[1] = 0x40;   //rpmcmd1
+    txdata[2] = 0x07;   //MT1=2000rpm
+    txdata[3] = 0xD0;   //MT1=2000rpm
+
+
+    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位2ビット:優先度:01 次の4bit:プロトコルID:受信(MD側) 下位5bit:デバイスID:01番
+    can1.write(CANMessage(CANID,&txdata[0],4)); 
+}
+void motor1_back( void ){                                       // モータ1反転2000rpm
+    //MT1=-2000rpm
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x10;
+    txdata[1] = 0x40;   //rpmcmd1
+    txdata[2] = 0xf8;   //MT1=-2000rpm
+    txdata[3] = 0x30;   //MT1=-2000rpm
+
+    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位2ビット:優先度:01 次の4bit:プロトコルID:受信(MD側) 下位5bit:デバイスID:01番
+    can1.write(CANMessage(CANID,&txdata[0],4)); 
 }
 
-// 割り込み関数
-void function(void){
-    motor_speed_left();
-    motor_speed_right();
-    }
+void motor2_forward( void ){                                        // モータ2前進2000rpm
+    //MT2=2000rpm
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x10;
+    txdata[1] = 0x41;   //rpmcmd2
+    txdata[2] = 0x07;   //MT2=2000rpm
+    txdata[3] = 0xD0;   //MT2=2000rpm
 
-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;
+    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位2ビット:優先度:01 次の4bit:プロトコルID:受信(MD側) 下位5bit:デバイスID:01番
+    can1.write(CANMessage(CANID,&txdata[0],4));
 }
 
-void messageCb(const geometry_msgs::Twist& msg){
-  double tmp = 0;
-  
-  TargetSpeed_left = msg.linear.x;
-  TargetSpeed_right = msg.linear.x;
-  tmp = 0;
-  
-  if(msg.angular.z < 0){
-      tmp = 1 - (-msg.angular.z / 3);
-      TargetSpeed_left = msg.linear.x;
-      TargetSpeed_right = msg.linear.x * tmp;
-  } else if(msg.angular.z > 0){
-      tmp = 1 - (msg.angular.z / 3);
-      TargetSpeed_left = msg.linear.x * tmp;
-      TargetSpeed_right = msg.linear.x;
-  } else {
-      TargetSpeed_right = msg.linear.x;
-      TargetSpeed_right = msg.linear.x;
-  }
-  motor_left = pwm_left;
-  motor_right = pwm_right;
-  
-  /*
-  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;
-    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;
-        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;
-        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;
-        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;
-        chatter.publish( &str_msg );
-    }
-  }
-  */
+void motor2_back( void ){                                       // モータ2前進2000rpm
+    //MT2=-2000rpm
+    char txdata[8];
+    int CANID;
+    
+    txdata[0] = 0x10;
+    txdata[1] = 0x41;   //rpmcmd2
+    txdata[2] = 0xf8;   //MT1=-2000rpm
+    txdata[3] = 0x30;   //MT1=-2000rpm
+    txdata[4] = 0x00;
+    txdata[5] = 0x00;
+    txdata[6] = 0x00;
+    txdata[7] = 0x00;
+
+    CANID=0x200 |  ((MDwrite)&0xf)<<5 | 0x0a ;  // 上位2ビット:優先度:01 次の4bit:プロトコルID:受信(MD側) 下位5bit:デバイスID:01番
+    can1.write(CANMessage(CANID,&txdata[0],4));
+}
+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 ;    // 上位2ビット:優先度:01 次の4bit:プロトコルID:制御モード 下位5bit:デバイスID:10番
+    can1.write(CANMessage(CANID,&txdata[0],8)); 
+
 }
\ No newline at end of file