K AKAGAWA
V7's MotorTestProgram for F446
Dependencies: mbed ros_lib_kinetic USBDevice
Diff: main.cpp
- Revision:
- 1:417488dfc3d0
- Parent:
- 0:27046fed2426
--- 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目:上位４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;
- 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 ; // 上位２ビット：優先度：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 ; // 上位２ビット：優先度：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 ; // 上位２ビット：優先度：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 ; // 上位２ビット：優先度：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 ; // 上位２ビット：優先度：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 ; // 上位２ビット：優先度：01 次の4bit：プロトコルID：制御モード 下位5bit：デバイスID：１０番
+ can1.write(CANMessage(CANID,&txdata[0],8));
+
}
\ No newline at end of file