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目:上位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