K AKAGAWA / Mbed 2 deprecated F446_CAN_V7_MotorTest_20190415

V7's MotorTestProgram for F446

Dependencies:   mbed ros_lib_kinetic USBDevice

main.cpp@1:417488dfc3d0, 2019-04-15 (annotated)

Committer:
ksingyuunyuu
Date:
Mon Apr 15 11:37:19 2019 +0000
Revision:
1:417488dfc3d0
Parent:
0:27046fed2426 
V7's MotorTestProgram for F446

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ksingyuunyuu 1:417488dfc3d0 1 // インクルード先の宣言
ksingyuunyuu 1:417488dfc3d0 2 #include "mbed.h"
ksingyuunyuu 1:417488dfc3d0 3 // マクロ定義
ksingyuunyuu 1:417488dfc3d0 4 #define MDcontroll 1
ksingyuunyuu 1:417488dfc3d0 5 #define MDwrite 2
ksingyuunyuu 1:417488dfc3d0 6 #define MDwriteBC 3
ksingyuunyuu 0:27046fed2426 7 /*
ksingyuunyuu 1:417488dfc3d0 8 // 機能の宣言、設定
ksingyuunyuu 1:417488dfc3d0 9 CAN can1(PB_8, PB_9); // can_name( RXD, TXD )
ksingyuunyuu 1:417488dfc3d0 10 Ticker ticker; // 関数tickerを割り込みに使用することの宣言
ksingyuunyuu 1:417488dfc3d0 11 // グローバル変数の宣言
ksingyuunyuu 0:27046fed2426 12
ksingyuunyuu 1:417488dfc3d0 13 char counter = 0;
ksingyuunyuu 1:417488dfc3d0 14
ksingyuunyuu 1:417488dfc3d0 15 void send() {
ksingyuunyuu 1:417488dfc3d0 16 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 17 int CANID;
ksingyuunyuu 1:417488dfc3d0 18
ksingyuunyuu 1:417488dfc3d0 19 txdata[0] = 0x10;
ksingyuunyuu 1:417488dfc3d0 20 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:417488dfc3d0 21 txdata[2] = 0x07; //MT1=2000rpm
ksingyuunyuu 1:417488dfc3d0 22 txdata[3] = 0xD0; //MT1=2000rpm
ksingyuunyuu 0:27046fed2426 23
ksingyuunyuu 1:417488dfc3d0 24 CANID=0x200|((MDwrite)&0xf)<<5|0x0a ;
ksingyuunyuu 1:417488dfc3d0 25 printf("send()\n");
ksingyuunyuu 1:417488dfc3d0 26 if(can1.write(CANMessage(CANID,&txdata[0],4))) {
ksingyuunyuu 1:417488dfc3d0 27 printf("wloop()\n");
ksingyuunyuu 1:417488dfc3d0 28 counter++;
ksingyuunyuu 1:417488dfc3d0 29 printf("Message sent: %d\n", counter);
ksingyuunyuu 1:417488dfc3d0 30 }
ksingyuunyuu 1:417488dfc3d0 31
ksingyuunyuu 1:417488dfc3d0 32 }
ksingyuunyuu 1:417488dfc3d0 33
ksingyuunyuu 1:417488dfc3d0 34 int main() {
ksingyuunyuu 1:417488dfc3d0 35 printf("main()\n");
ksingyuunyuu 1:417488dfc3d0 36 can1.frequency(500000);
ksingyuunyuu 1:417488dfc3d0 37 CANMessage msg;
ksingyuunyuu 1:417488dfc3d0 38 ticker.attach(&send, 1);
ksingyuunyuu 1:417488dfc3d0 39
ksingyuunyuu 0:27046fed2426 40
ksingyuunyuu 1:417488dfc3d0 41 wait(0.2);
ksingyuunyuu 1:417488dfc3d0 42 }
ksingyuunyuu 1:417488dfc3d0 43 */
ksingyuunyuu 1:417488dfc3d0 44 CAN can1(PB_8, PB_9); // can_name( RXD, TXD )
ksingyuunyuu 1:417488dfc3d0 45 Ticker ticker; // 関数tickerを割り込みに使用することの宣言
ksingyuunyuu 1:417488dfc3d0 46 // グローバル変数の宣言
ksingyuunyuu 1:417488dfc3d0 47 int pattern = 0;
ksingyuunyuu 1:417488dfc3d0 48 char input;
ksingyuunyuu 1:417488dfc3d0 49 int state = 1;
ksingyuunyuu 1:417488dfc3d0 50 int dir = 1;
ksingyuunyuu 1:417488dfc3d0 51 // 関数のプロトタイプ宣言
ksingyuunyuu 1:417488dfc3d0 52 void all_motor_stop( void );
ksingyuunyuu 1:417488dfc3d0 53 void motor1_forward( void );
ksingyuunyuu 1:417488dfc3d0 54 void motor1_back( void );
ksingyuunyuu 1:417488dfc3d0 55 void motor2_forward( void );
ksingyuunyuu 1:417488dfc3d0 56 void motor2_back( void );
ksingyuunyuu 1:417488dfc3d0 57 void MD_mode_change(int mode);
ksingyuunyuu 0:27046fed2426 58
ksingyuunyuu 1:417488dfc3d0 59 int main() {
ksingyuunyuu 1:417488dfc3d0 60 // 割り込みの設定
ksingyuunyuu 1:417488dfc3d0 61 //ticker.attach(&LEDwave, 0.2);
ksingyuunyuu 1:417488dfc3d0 62 //CANの設定
ksingyuunyuu 1:417488dfc3d0 63 CANMessage msg; // CANメッセージを格納する変数
ksingyuunyuu 1:417488dfc3d0 64 can1.frequency(500000); // CANのビットレートを500kHzに設定
ksingyuunyuu 1:417488dfc3d0 65
ksingyuunyuu 1:417488dfc3d0 66 // while(pc.readable() == 0);
ksingyuunyuu 1:417488dfc3d0 67 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 68 while( 1 ){
ksingyuunyuu 1:417488dfc3d0 69 switch( pattern ){
ksingyuunyuu 1:417488dfc3d0 70 case 0:
ksingyuunyuu 1:417488dfc3d0 71 all_motor_stop(); // モータストップ状態
ksingyuunyuu 1:417488dfc3d0 72 printf("\r\n");
ksingyuunyuu 1:417488dfc3d0 73 printf("TEC-MD7U-A TestProgram for NUCLEO-F446RE\r\n");
ksingyuunyuu 1:417488dfc3d0 74 printf("\r\n");
ksingyuunyuu 1:417488dfc3d0 75 printf("1: Motor1 Forward\r\n");
ksingyuunyuu 1:417488dfc3d0 76 printf("2: Motor1 Back\r\n");
ksingyuunyuu 1:417488dfc3d0 77 printf("3: Motor2 Forward\r\n");
ksingyuunyuu 1:417488dfc3d0 78 printf("4: Motor2 Back\r\n");
ksingyuunyuu 1:417488dfc3d0 79 printf("\r\n");
ksingyuunyuu 1:417488dfc3d0 80 printf("Press any key of 1-4\r\n");
ksingyuunyuu 1:417488dfc3d0 81 printf("And Press Enter\r\n");
ksingyuunyuu 1:417488dfc3d0 82 pattern = 1; // 次のパターンへ
ksingyuunyuu 1:417488dfc3d0 83 break;
ksingyuunyuu 1:417488dfc3d0 84
ksingyuunyuu 1:417488dfc3d0 85 case 1:
ksingyuunyuu 1:417488dfc3d0 86 scanf("%d",&input); // 入力待ち
ksingyuunyuu 1:417488dfc3d0 87 if( input > 0 && input < 5 ){ // 1-4以外のキーボード入力は受け付けない
ksingyuunyuu 1:417488dfc3d0 88 printf("%d\r\n",input); // 入力データを表示する
ksingyuunyuu 1:417488dfc3d0 89 pattern = (input * 10) + 1; // 遷移先に飛ぶ
ksingyuunyuu 1:417488dfc3d0 90 break;
ksingyuunyuu 1:417488dfc3d0 91 }
ksingyuunyuu 1:417488dfc3d0 92 break;
ksingyuunyuu 1:417488dfc3d0 93
ksingyuunyuu 1:417488dfc3d0 94 case 11:
ksingyuunyuu 1:417488dfc3d0 95 // モータ1前進パターン
ksingyuunyuu 1:417488dfc3d0 96 motor1_forward(); // モータ1前転2000回転
ksingyuunyuu 1:417488dfc3d0 97 printf("MotorForwardActive");
ksingyuunyuu 1:417488dfc3d0 98 printf("Press '0' ,StopMotor");
ksingyuunyuu 1:417488dfc3d0 99 scanf("%d",&input); // 入力待機状態
ksingyuunyuu 1:417488dfc3d0 100 if( input == 0 ){ // 入力に0が渡されたら、待機状態に移行する
ksingyuunyuu 1:417488dfc3d0 101 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 102 break;
ksingyuunyuu 1:417488dfc3d0 103 }
ksingyuunyuu 1:417488dfc3d0 104 break;
ksingyuunyuu 1:417488dfc3d0 105
ksingyuunyuu 1:417488dfc3d0 106 case 21:
ksingyuunyuu 1:417488dfc3d0 107 motor1_back(); // モータ1後転2000回転
ksingyuunyuu 1:417488dfc3d0 108 printf("MotorForwardActive");
ksingyuunyuu 1:417488dfc3d0 109 printf("Press'0',StopMotor");
ksingyuunyuu 1:417488dfc3d0 110 scanf("%d",&input); // 入力待機状態
ksingyuunyuu 1:417488dfc3d0 111 if( input == 0 ){ // 入力に0が渡されたら、待機状態に移行する
ksingyuunyuu 1:417488dfc3d0 112 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 113 break;
ksingyuunyuu 1:417488dfc3d0 114 }
ksingyuunyuu 1:417488dfc3d0 115 break;
ksingyuunyuu 1:417488dfc3d0 116
ksingyuunyuu 1:417488dfc3d0 117 case 31:
ksingyuunyuu 1:417488dfc3d0 118 motor2_forward(); // モータ2前転2000回転
ksingyuunyuu 1:417488dfc3d0 119 printf("MotorForwardActive");
ksingyuunyuu 1:417488dfc3d0 120 printf("Press'0',StopMotor");
ksingyuunyuu 1:417488dfc3d0 121 scanf("%d",&input); // 入力待機状態
ksingyuunyuu 1:417488dfc3d0 122 if( input == 0 ){ // 入力に0が渡されたら、待機状態に移行する
ksingyuunyuu 1:417488dfc3d0 123 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 124 break;
ksingyuunyuu 1:417488dfc3d0 125 }
ksingyuunyuu 1:417488dfc3d0 126 break;
ksingyuunyuu 1:417488dfc3d0 127
ksingyuunyuu 1:417488dfc3d0 128 case 41:
ksingyuunyuu 1:417488dfc3d0 129 motor2_back();
ksingyuunyuu 1:417488dfc3d0 130 printf("MotorForwardActive");
ksingyuunyuu 1:417488dfc3d0 131 printf("Press'0',StopMotor");
ksingyuunyuu 1:417488dfc3d0 132 scanf("%d",&input);
ksingyuunyuu 1:417488dfc3d0 133 if( input == 0 ){
ksingyuunyuu 1:417488dfc3d0 134 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 135 break;
ksingyuunyuu 1:417488dfc3d0 136 }
ksingyuunyuu 1:417488dfc3d0 137 break;
ksingyuunyuu 1:417488dfc3d0 138
ksingyuunyuu 1:417488dfc3d0 139 default:
ksingyuunyuu 1:417488dfc3d0 140 printf("\r\n");
ksingyuunyuu 1:417488dfc3d0 141 printf("Invalid Value\r\n");
ksingyuunyuu 1:417488dfc3d0 142 printf("\r\n");
ksingyuunyuu 1:417488dfc3d0 143 pattern = 0;
ksingyuunyuu 1:417488dfc3d0 144 break;
ksingyuunyuu 1:417488dfc3d0 145 } // switch
ksingyuunyuu 1:417488dfc3d0 146 } // while
ksingyuunyuu 1:417488dfc3d0 147 } // main
ksingyuunyuu 0:27046fed2426 148
ksingyuunyuu 0:27046fed2426 149
ksingyuunyuu 1:417488dfc3d0 150
ksingyuunyuu 0:27046fed2426 151
ksingyuunyuu 1:417488dfc3d0 152 /* CANIDプロトコル
ksingyuunyuu 1:417488dfc3d0 153 / CANID:xxyyyyzzzzzの11bit中
ksingyuunyuu 1:417488dfc3d0 154 / 上位2bit:優先度 :小さいものが高優先　[00]:ブロードキャスト用？？ [01]:M-bedから書き込みに使える [10]:MDからくるレポート [11]:不明 メッセージの衝突回避用
ksingyuunyuu 1:417488dfc3d0 155 / 次の4bit:プロトコルID :[0]高優先度プロトコル　[1]制御プロトコル　[2-3]書き込みプロトコル　[8]高優先度通知プロトコル　[9]イベント通知プロトコル　[10-12]ステータスレポートプロトコル
ksingyuunyuu 1:417488dfc3d0 156 / 下位5bit:デバイスID :デバイスIDとデバイスの各メールボックス
ksingyuunyuu 1:417488dfc3d0 157 /
ksingyuunyuu 1:417488dfc3d0 158 / CANデータフィールド1Byte目:上位４bit：１つ目のデータはレジスタ何個分か　下位4bit：2つ目のデータはレジスタ何個分か RPMCMD1 =2Byte　RPMCMD2 =2Byte
ksingyuunyuu 1:417488dfc3d0 159 / CANデータフィールド2Byte目:書き込む先のアドレス1　レジスタのアドレス　例）RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:417488dfc3d0 160 / CANデータフィールド3Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:417488dfc3d0 161 / CANデータフィールド4Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:417488dfc3d0 162 / CANデータフィールド5Byte目:書き込む先のアドレス2　レジスタのアドレス　RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:417488dfc3d0 163 / CANデータフィールド6Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:417488dfc3d0 164 / CANデータフィールド7Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:417488dfc3d0 165 / CANデータフィールド8Byte目:
ksingyuunyuu 1:417488dfc3d0 166 /
ksingyuunyuu 1:417488dfc3d0 167 */
ksingyuunyuu 1:417488dfc3d0 168
ksingyuunyuu 1:417488dfc3d0 169 #define MDcontroll 1
ksingyuunyuu 1:417488dfc3d0 170 #define MDwrite 2
ksingyuunyuu 1:417488dfc3d0 171 #define MDwriteBC 3
ksingyuunyuu 1:417488dfc3d0 172 void all_motor_stop( void ){ // 全モータストップ
ksingyuunyuu 1:417488dfc3d0 173 //MT1=0rpm MT2=0rpm
ksingyuunyuu 1:417488dfc3d0 174 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 175 int CANID;
ksingyuunyuu 0:27046fed2426 176
ksingyuunyuu 1:417488dfc3d0 177 txdata[0] = 0x11;
ksingyuunyuu 1:417488dfc3d0 178 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:417488dfc3d0 179 txdata[2] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 180 txdata[3] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 181 txdata[4] = 0x41;
ksingyuunyuu 1:417488dfc3d0 182 txdata[5] = 0x00;
ksingyuunyuu 1:417488dfc3d0 183 txdata[6] = 0x00;
ksingyuunyuu 1:417488dfc3d0 184
ksingyuunyuu 1:417488dfc3d0 185 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:417488dfc3d0 186 can1.write(CANMessage(CANID,&txdata[0],7));
ksingyuunyuu 1:417488dfc3d0 187 }
ksingyuunyuu 1:417488dfc3d0 188 void motor1_forward( void ){ // モータ1正転2000rpm
ksingyuunyuu 1:417488dfc3d0 189 //MT1=2000rpm
ksingyuunyuu 1:417488dfc3d0 190 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 191 int CANID;
ksingyuunyuu 1:417488dfc3d0 192
ksingyuunyuu 1:417488dfc3d0 193 txdata[0] = 0x10;
ksingyuunyuu 1:417488dfc3d0 194 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:417488dfc3d0 195 txdata[2] = 0x07; //MT1=2000rpm
ksingyuunyuu 1:417488dfc3d0 196 txdata[3] = 0xD0; //MT1=2000rpm
ksingyuunyuu 1:417488dfc3d0 197
ksingyuunyuu 1:417488dfc3d0 198
ksingyuunyuu 1:417488dfc3d0 199 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:417488dfc3d0 200 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:417488dfc3d0 201 }
ksingyuunyuu 1:417488dfc3d0 202 void motor1_back( void ){ // モータ1反転2000rpm
ksingyuunyuu 1:417488dfc3d0 203 //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 204 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 205 int CANID;
ksingyuunyuu 1:417488dfc3d0 206
ksingyuunyuu 1:417488dfc3d0 207 txdata[0] = 0x10;
ksingyuunyuu 1:417488dfc3d0 208 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:417488dfc3d0 209 txdata[2] = 0xf8; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 210 txdata[3] = 0x30; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 211
ksingyuunyuu 1:417488dfc3d0 212 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:417488dfc3d0 213 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 0:27046fed2426 214 }
ksingyuunyuu 0:27046fed2426 215
ksingyuunyuu 1:417488dfc3d0 216 void motor2_forward( void ){ // モータ2前進2000rpm
ksingyuunyuu 1:417488dfc3d0 217 //MT2=2000rpm
ksingyuunyuu 1:417488dfc3d0 218 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 219 int CANID;
ksingyuunyuu 1:417488dfc3d0 220
ksingyuunyuu 1:417488dfc3d0 221 txdata[0] = 0x10;
ksingyuunyuu 1:417488dfc3d0 222 txdata[1] = 0x41; //rpmcmd2
ksingyuunyuu 1:417488dfc3d0 223 txdata[2] = 0x07; //MT2=2000rpm
ksingyuunyuu 1:417488dfc3d0 224 txdata[3] = 0xD0; //MT2=2000rpm
ksingyuunyuu 0:27046fed2426 225
ksingyuunyuu 1:417488dfc3d0 226 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:417488dfc3d0 227 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 0:27046fed2426 228 }
ksingyuunyuu 0:27046fed2426 229
ksingyuunyuu 1:417488dfc3d0 230 void motor2_back( void ){ // モータ2前進2000rpm
ksingyuunyuu 1:417488dfc3d0 231 //MT2=-2000rpm
ksingyuunyuu 1:417488dfc3d0 232 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 233 int CANID;
ksingyuunyuu 1:417488dfc3d0 234
ksingyuunyuu 1:417488dfc3d0 235 txdata[0] = 0x10;
ksingyuunyuu 1:417488dfc3d0 236 txdata[1] = 0x41; //rpmcmd2
ksingyuunyuu 1:417488dfc3d0 237 txdata[2] = 0xf8; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 238 txdata[3] = 0x30; //MT1=-2000rpm
ksingyuunyuu 1:417488dfc3d0 239 txdata[4] = 0x00;
ksingyuunyuu 1:417488dfc3d0 240 txdata[5] = 0x00;
ksingyuunyuu 1:417488dfc3d0 241 txdata[6] = 0x00;
ksingyuunyuu 1:417488dfc3d0 242 txdata[7] = 0x00;
ksingyuunyuu 1:417488dfc3d0 243
ksingyuunyuu 1:417488dfc3d0 244 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:417488dfc3d0 245 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:417488dfc3d0 246 }
ksingyuunyuu 1:417488dfc3d0 247 void MD_mode_change(int mode){ // モータドライバのモード変更関数
ksingyuunyuu 1:417488dfc3d0 248 char txdata[8];
ksingyuunyuu 1:417488dfc3d0 249 int CANID;
ksingyuunyuu 1:417488dfc3d0 250
ksingyuunyuu 1:417488dfc3d0 251 txdata[0] = 0x05;
ksingyuunyuu 1:417488dfc3d0 252 txdata[1] = mode;
ksingyuunyuu 1:417488dfc3d0 253 txdata[2] = 0x00;
ksingyuunyuu 1:417488dfc3d0 254 txdata[3] = 0x00;
ksingyuunyuu 1:417488dfc3d0 255 txdata[4] = 0x00;
ksingyuunyuu 1:417488dfc3d0 256 txdata[5] = 0x00;
ksingyuunyuu 1:417488dfc3d0 257 txdata[6] = 0x00;
ksingyuunyuu 1:417488dfc3d0 258 txdata[7] = 0x00;
ksingyuunyuu 1:417488dfc3d0 259
ksingyuunyuu 1:417488dfc3d0 260 CANID=0x200 | ((MDcontroll)&0xf)<<5 | 0xa ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：制御モード　下位5bit：デバイスID：１０番
ksingyuunyuu 1:417488dfc3d0 261 can1.write(CANMessage(CANID,&txdata[0],8));
ksingyuunyuu 1:417488dfc3d0 262
ksingyuunyuu 0:27046fed2426 263 }