K AKAGAWA / Mbed 2 deprecated F446_rosserial_V7_EncoderVersion_101

Kenshiro_program_sw

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

main.cpp@1:979cc2a56e7e, 2019-05-17 (annotated)

Committer:
ksingyuunyuu
Date:
Fri May 17 07:40:50 2019 +0000
Revision:
1:979cc2a56e7e
Parent:
0:27046fed2426
Child:
2:e59b809f9e0a
20190517;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ksingyuunyuu 0:27046fed2426 1 /*
ksingyuunyuu 0:27046fed2426 2 * rosserial Motor Shield Example
ksingyuunyuu 0:27046fed2426 3 *
ksingyuunyuu 0:27046fed2426 4 * This tutorial demonstrates the usage of the
ksingyuunyuu 0:27046fed2426 5 * Seeedstudio Motor Shield
ksingyuunyuu 0:27046fed2426 6 * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
ksingyuunyuu 0:27046fed2426 7 *
ksingyuunyuu 0:27046fed2426 8 * Source Code Based on:
ksingyuunyuu 0:27046fed2426 9 * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
ksingyuunyuu 0:27046fed2426 10 */
ksingyuunyuu 0:27046fed2426 11
ksingyuunyuu 0:27046fed2426 12 #include "mbed.h"
ksingyuunyuu 0:27046fed2426 13 #include <ros.h>
ksingyuunyuu 0:27046fed2426 14 #include <geometry_msgs/Twist.h>
ksingyuunyuu 0:27046fed2426 15 #include <std_msgs/String.h>
ksingyuunyuu 0:27046fed2426 16
ksingyuunyuu 1:979cc2a56e7e 17 // マクロ定義(ピン配置)F446
ksingyuunyuu 0:27046fed2426 18
ksingyuunyuu 1:979cc2a56e7e 19
ksingyuunyuu 0:27046fed2426 20 // その他マクロ定義
ksingyuunyuu 0:27046fed2426 21 #define SAMPLING_TIME 0.01 // 100Hz
ksingyuunyuu 1:979cc2a56e7e 22 #define STANDARD_RPM 3307
ksingyuunyuu 1:979cc2a56e7e 23 #define LOW_RPM 800
ksingyuunyuu 1:979cc2a56e7e 24 // 入出力モード、各種モードの設定
ksingyuunyuu 0:27046fed2426 25
ksingyuunyuu 0:27046fed2426 26 Ticker control;
ksingyuunyuu 0:27046fed2426 27 ros::NodeHandle nh;
ksingyuunyuu 1:979cc2a56e7e 28 CAN can1(PB_8, PB_9, 500000); //Set frequency at declaration
ksingyuunyuu 1:979cc2a56e7e 29 //CAN can1(PB_8, PB_9); // can_name( RXD, TXD )
ksingyuunyuu 0:27046fed2426 30 // グローバル変数の宣言
ksingyuunyuu 1:979cc2a56e7e 31 int can_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 32 int can_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 33 // ここからメッセージ用
ksingyuunyuu 1:979cc2a56e7e 34 char pub[13] = "publish now ";
ksingyuunyuu 1:979cc2a56e7e 35 char left[13] = "mode left ";
ksingyuunyuu 1:979cc2a56e7e 36 char right[13] = "mode right ";
ksingyuunyuu 1:979cc2a56e7e 37 char move[13] = "mode move ";
ksingyuunyuu 1:979cc2a56e7e 38 char nutral[13] = "mode nutral ";
ksingyuunyuu 0:27046fed2426 39 // 関数のプロトタイプ宣言
ksingyuunyuu 0:27046fed2426 40 void function(void);
ksingyuunyuu 1:979cc2a56e7e 41 void all_motor_stop( void );
ksingyuunyuu 1:979cc2a56e7e 42 void motor( int rpm1,int rpm2 );
ksingyuunyuu 1:979cc2a56e7e 43 void motor1_drive( int rpm );
ksingyuunyuu 1:979cc2a56e7e 44 void motor2_drive( int rpm );
ksingyuunyuu 1:979cc2a56e7e 45 void MD_mode_change(int mode);
ksingyuunyuu 0:27046fed2426 46 void messageCb(const geometry_msgs::Twist& msg);
ksingyuunyuu 0:27046fed2426 47
ksingyuunyuu 0:27046fed2426 48 std_msgs::String str_msg;
ksingyuunyuu 0:27046fed2426 49 ros::Publisher chatter("chatter", &str_msg);
ksingyuunyuu 1:979cc2a56e7e 50 ros::Subscriber<geometry_msgs::Twist> sub("kenshiro/diff_drive_controller/cmd_vel", &messageCb);
ksingyuunyuu 0:27046fed2426 51
ksingyuunyuu 0:27046fed2426 52 Timer t;
ksingyuunyuu 0:27046fed2426 53
ksingyuunyuu 1:979cc2a56e7e 54 int main(){
ksingyuunyuu 0:27046fed2426 55
ksingyuunyuu 0:27046fed2426 56 t.start();
ksingyuunyuu 0:27046fed2426 57 long vel_timer = 0;
ksingyuunyuu 0:27046fed2426 58 nh.getHardware()->setBaud(57600);
ksingyuunyuu 0:27046fed2426 59 nh.initNode();
ksingyuunyuu 0:27046fed2426 60 nh.subscribe(sub);
ksingyuunyuu 0:27046fed2426 61 nh.advertise(chatter);
ksingyuunyuu 1:979cc2a56e7e 62
ksingyuunyuu 1:979cc2a56e7e 63 CANMessage msg; // CANメッセージを格納する変数
ksingyuunyuu 1:979cc2a56e7e 64 //can1.frequency(500000); // CANのビットレートを500kHzに設定
ksingyuunyuu 1:979cc2a56e7e 65
ksingyuunyuu 1:979cc2a56e7e 66 //control.attach(&function, SAMPLING_TIME); //台車の速度制御用のタイマー関数を設定
ksingyuunyuu 1:979cc2a56e7e 67 while (1){
ksingyuunyuu 1:979cc2a56e7e 68 // これ意味ないんじゃないの？
ksingyuunyuu 1:979cc2a56e7e 69 if (t.read_ms() > vel_timer){
ksingyuunyuu 0:27046fed2426 70 // 停止
ksingyuunyuu 1:979cc2a56e7e 71 /*
ksingyuunyuu 1:979cc2a56e7e 72 can_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 73 can_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 74 */
ksingyuunyuu 0:27046fed2426 75 vel_timer = t.read_ms() + 500; // 500ms毎に実行
ksingyuunyuu 0:27046fed2426 76 }
ksingyuunyuu 1:979cc2a56e7e 77 nh.spinOnce(); // 呼び出し待ちのコールバックをここで呼び出す
ksingyuunyuu 1:979cc2a56e7e 78 // ここでCANの動作？、ここでダメなら、割り込みでCAN
ksingyuunyuu 1:979cc2a56e7e 79 //motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 80 //motor2_drive( can_rpm_right );
ksingyuunyuu 0:27046fed2426 81 wait_ms(1);
ksingyuunyuu 0:27046fed2426 82 }
ksingyuunyuu 0:27046fed2426 83 }
ksingyuunyuu 0:27046fed2426 84
ksingyuunyuu 0:27046fed2426 85 // 割り込み関数
ksingyuunyuu 1:979cc2a56e7e 86 /*
ksingyuunyuu 0:27046fed2426 87 void function(void){
ksingyuunyuu 0:27046fed2426 88
ksingyuunyuu 0:27046fed2426 89 }
ksingyuunyuu 1:979cc2a56e7e 90 */
ksingyuunyuu 1:979cc2a56e7e 91 /*
ksingyuunyuu 0:27046fed2426 92 void messageCb(const geometry_msgs::Twist& msg){
ksingyuunyuu 0:27046fed2426 93 double tmp = 0;
ksingyuunyuu 1:979cc2a56e7e 94 double rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 95 double rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 96 // たぶんいらない
ksingyuunyuu 1:979cc2a56e7e 97 //TargetSpeed_left = msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 98 //TargetSpeed_right = msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 99 // パブリッシュ
ksingyuunyuu 1:979cc2a56e7e 100 str_msg.data = nutral;
ksingyuunyuu 1:979cc2a56e7e 101 chatter.publish( &str_msg );
ksingyuunyuu 1:979cc2a56e7e 102 // パブリッシュここまで
ksingyuunyuu 0:27046fed2426 103 if(msg.angular.z < 0){
ksingyuunyuu 1:979cc2a56e7e 104 // 左旋回
ksingyuunyuu 1:979cc2a56e7e 105 // z軸変換移動角度量(0~1.0くらい) = z軸移動角度量(ラジアン) / 3 * -1 ・・・z軸移動量がマイナスなので、プラスに変換する
ksingyuunyuu 1:979cc2a56e7e 106 tmp = -msg.angular.z / 3;
ksingyuunyuu 1:979cc2a56e7e 107 // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
ksingyuunyuu 1:979cc2a56e7e 108 rpm_left = STANDARD_RPM * msg.angular.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 109 // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0) * z軸移動角度量(変換後0~1.0くらい)
ksingyuunyuu 1:979cc2a56e7e 110 rpm_right = STANDARD_RPM * msg.angular.x;
ksingyuunyuu 1:979cc2a56e7e 111 // パブリッシュ
ksingyuunyuu 1:979cc2a56e7e 112 str_msg.data = left;
ksingyuunyuu 1:979cc2a56e7e 113 chatter.publish( &str_msg );
ksingyuunyuu 1:979cc2a56e7e 114 // パブリッシュここまで
ksingyuunyuu 0:27046fed2426 115 } else if(msg.angular.z > 0){
ksingyuunyuu 1:979cc2a56e7e 116 // 右旋回
ksingyuunyuu 1:979cc2a56e7e 117 // z軸変換移動角度量(0~1.0くらい) = z軸移動角度量(ラジアン) / 3 ・・・z軸移動量がプラスなので、プラスに変換する必要はない
ksingyuunyuu 1:979cc2a56e7e 118 tmp = msg.angular.z / 3;
ksingyuunyuu 1:979cc2a56e7e 119 // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0) * z軸移動角度量(変換後0~1.0くらい)
ksingyuunyuu 1:979cc2a56e7e 120 rpm_left = STANDARD_RPM * msg.angular.x;
ksingyuunyuu 1:979cc2a56e7e 121 // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
ksingyuunyuu 1:979cc2a56e7e 122 rpm_right = STANDARD_RPM * msg.angular.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 123 // パブリッシュ
ksingyuunyuu 1:979cc2a56e7e 124 str_msg.data = right;
ksingyuunyuu 1:979cc2a56e7e 125 chatter.publish( &str_msg );
ksingyuunyuu 1:979cc2a56e7e 126 // パブリッシュここまで
ksingyuunyuu 0:27046fed2426 127 } else {
ksingyuunyuu 1:979cc2a56e7e 128 // z移動角度量が0の場合(つまり、前進か後進)
ksingyuunyuu 1:979cc2a56e7e 129 // 左回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
ksingyuunyuu 1:979cc2a56e7e 130 rpm_left = STANDARD_RPM * msg.angular.x;
ksingyuunyuu 1:979cc2a56e7e 131 // 右回転数 = 基準回転数(2000) * コントローラ入力量(0~1.0)
ksingyuunyuu 1:979cc2a56e7e 132 rpm_right = STANDARD_RPM * msg.angular.x;
ksingyuunyuu 1:979cc2a56e7e 133 // パブリッシュ
ksingyuunyuu 1:979cc2a56e7e 134 str_msg.data = move;
ksingyuunyuu 1:979cc2a56e7e 135 chatter.publish( &str_msg );
ksingyuunyuu 1:979cc2a56e7e 136 // パブリッシュここまで
ksingyuunyuu 0:27046fed2426 137 }
ksingyuunyuu 1:979cc2a56e7e 138 // CAN用に少数を整数型にキャストする
ksingyuunyuu 1:979cc2a56e7e 139 can_rpm_left = rpm_left;
ksingyuunyuu 1:979cc2a56e7e 140 can_rpm_right = rpm_right;
ksingyuunyuu 1:979cc2a56e7e 141 }
ksingyuunyuu 1:979cc2a56e7e 142 */
ksingyuunyuu 1:979cc2a56e7e 143
ksingyuunyuu 1:979cc2a56e7e 144 /*
ksingyuunyuu 1:979cc2a56e7e 145 void messageCb(const geometry_msgs::Twist& msg){
ksingyuunyuu 0:27046fed2426 146 if (msg.angular.z == 0 && msg.linear.x == 0){
ksingyuunyuu 0:27046fed2426 147 // 停止
ksingyuunyuu 1:979cc2a56e7e 148 can_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 149 can_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 150 motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 151 motor2_drive( can_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 152 str_msg.data = nutral;
ksingyuunyuu 0:27046fed2426 153 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 154 } else {
ksingyuunyuu 0:27046fed2426 155 if (msg.angular.z < 0){
ksingyuunyuu 0:27046fed2426 156 // 右旋回
ksingyuunyuu 1:979cc2a56e7e 157 can_rpm_left = 1000;
ksingyuunyuu 1:979cc2a56e7e 158 can_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 159 motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 160 motor2_drive( can_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 161 str_msg.data = right;
ksingyuunyuu 0:27046fed2426 162 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 163 } else if (msg.angular.z > 0){
ksingyuunyuu 0:27046fed2426 164 // 左旋回
ksingyuunyuu 1:979cc2a56e7e 165 can_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 166 can_rpm_right = 1000;
ksingyuunyuu 1:979cc2a56e7e 167 motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 168 motor2_drive( can_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 169 str_msg.data = left;
ksingyuunyuu 0:27046fed2426 170 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 171 } else if (msg.linear.x < 0){
ksingyuunyuu 0:27046fed2426 172 // 後進
ksingyuunyuu 1:979cc2a56e7e 173 can_rpm_left = -1000;
ksingyuunyuu 1:979cc2a56e7e 174 can_rpm_right = -1000;
ksingyuunyuu 1:979cc2a56e7e 175 motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 176 motor2_drive( can_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 177 str_msg.data = move;
ksingyuunyuu 0:27046fed2426 178 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 179 } else if (msg.linear.x > 0){
ksingyuunyuu 0:27046fed2426 180 // 前進
ksingyuunyuu 1:979cc2a56e7e 181 can_rpm_left = 1000;
ksingyuunyuu 1:979cc2a56e7e 182 can_rpm_right = 1000;
ksingyuunyuu 1:979cc2a56e7e 183 motor1_drive( can_rpm_left );
ksingyuunyuu 1:979cc2a56e7e 184 motor2_drive( can_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 185 str_msg.data = move;
ksingyuunyuu 0:27046fed2426 186 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 187 }
ksingyuunyuu 0:27046fed2426 188 }
ksingyuunyuu 1:979cc2a56e7e 189 }
ksingyuunyuu 1:979cc2a56e7e 190 */
ksingyuunyuu 1:979cc2a56e7e 191
ksingyuunyuu 1:979cc2a56e7e 192 void messageCb(const geometry_msgs::Twist& msg){
ksingyuunyuu 1:979cc2a56e7e 193 double tmp = 0;
ksingyuunyuu 1:979cc2a56e7e 194 double tmp_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 195 double tmp_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 196 if(msg.angular.z < 0){
ksingyuunyuu 1:979cc2a56e7e 197 // 左旋回
ksingyuunyuu 1:979cc2a56e7e 198 tmp = 1 - (-msg.angular.z / 3); //radiun
ksingyuunyuu 1:979cc2a56e7e 199 if( msg.linear.x == 0 ){
ksingyuunyuu 1:979cc2a56e7e 200 // 真左に旋回の場合
ksingyuunyuu 1:979cc2a56e7e 201 tmp_rpm_left = LOW_RPM * tmp;
ksingyuunyuu 1:979cc2a56e7e 202 tmp_rpm_right = 0;
ksingyuunyuu 1:979cc2a56e7e 203 } else {
ksingyuunyuu 1:979cc2a56e7e 204 tmp_rpm_left = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 205 tmp_rpm_right = STANDARD_RPM * msg.linear.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 206 }
ksingyuunyuu 1:979cc2a56e7e 207 } else if(msg.angular.z > 0){
ksingyuunyuu 1:979cc2a56e7e 208 // 右旋回
ksingyuunyuu 1:979cc2a56e7e 209 tmp = 1 - (msg.angular.z / 3); //radiun
ksingyuunyuu 1:979cc2a56e7e 210 if( msg.linear.x == 0 ){
ksingyuunyuu 1:979cc2a56e7e 211 // 真右に旋回の場合
ksingyuunyuu 1:979cc2a56e7e 212 tmp_rpm_left = 0;
ksingyuunyuu 1:979cc2a56e7e 213 tmp_rpm_right = LOW_RPM * tmp;
ksingyuunyuu 1:979cc2a56e7e 214 } else {
ksingyuunyuu 1:979cc2a56e7e 215 tmp_rpm_left = STANDARD_RPM * msg.linear.x * tmp;
ksingyuunyuu 1:979cc2a56e7e 216 tmp_rpm_right = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 217 }
ksingyuunyuu 1:979cc2a56e7e 218 } else {
ksingyuunyuu 1:979cc2a56e7e 219 tmp_rpm_left = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 220 tmp_rpm_right = STANDARD_RPM * msg.linear.x;
ksingyuunyuu 1:979cc2a56e7e 221 }
ksingyuunyuu 1:979cc2a56e7e 222 motor( tmp_rpm_left, tmp_rpm_right );
ksingyuunyuu 1:979cc2a56e7e 223 }
ksingyuunyuu 1:979cc2a56e7e 224 /* CANIDプロトコル
ksingyuunyuu 1:979cc2a56e7e 225 / CANID:xxyyyyzzzzzの11bit中
ksingyuunyuu 1:979cc2a56e7e 226 / 上位2bit:優先度 :小さいものが高優先　[00]:ブロードキャスト用？？ [01]:M-bedから書き込みに使える [10]:MDからくるレポート [11]:不明 メッセージの衝突回避用
ksingyuunyuu 1:979cc2a56e7e 227 / 次の4bit:プロトコルID :[0]高優先度プロトコル　[1]制御プロトコル　[2-3]書き込みプロトコル　[8]高優先度通知プロトコル　[9]イベント通知プロトコル　[10-12]ステータスレポートプロトコル
ksingyuunyuu 1:979cc2a56e7e 228 / 下位5bit:デバイスID :デバイスIDとデバイスの各メールボックス
ksingyuunyuu 1:979cc2a56e7e 229 /
ksingyuunyuu 1:979cc2a56e7e 230 / CANデータフィールド1Byte目:上位４bit：１つ目のデータはレジスタ何個分か　下位4bit：2つ目のデータはレジスタ何個分か RPMCMD1 =2Byte　RPMCMD2 =2Byte
ksingyuunyuu 1:979cc2a56e7e 231 / CANデータフィールド2Byte目:書き込む先のアドレス1　レジスタのアドレス　例）RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:979cc2a56e7e 232 / CANデータフィールド3Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:979cc2a56e7e 233 / CANデータフィールド4Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:979cc2a56e7e 234 / CANデータフィールド5Byte目:書き込む先のアドレス2　レジスタのアドレス　RPMCMD1 =0x40 (64番目
ksingyuunyuu 1:979cc2a56e7e 235 / CANデータフィールド6Byte目:例）回転数1000の上位2bit
ksingyuunyuu 1:979cc2a56e7e 236 / CANデータフィールド7Byte目:例）回転数1000の下位2bit
ksingyuunyuu 1:979cc2a56e7e 237 / CANデータフィールド8Byte目:
ksingyuunyuu 1:979cc2a56e7e 238 /
ksingyuunyuu 1:979cc2a56e7e 239 */
ksingyuunyuu 1:979cc2a56e7e 240
ksingyuunyuu 1:979cc2a56e7e 241 #define MDcontroll 1
ksingyuunyuu 1:979cc2a56e7e 242 #define MDwrite 2
ksingyuunyuu 1:979cc2a56e7e 243 #define MDwriteBC 3
ksingyuunyuu 1:979cc2a56e7e 244 void all_motor_stop( void ){ // 全モータストップ
ksingyuunyuu 1:979cc2a56e7e 245 //MT1=0rpm MT2=0rpm
ksingyuunyuu 1:979cc2a56e7e 246 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 247 int CANID;
ksingyuunyuu 1:979cc2a56e7e 248
ksingyuunyuu 1:979cc2a56e7e 249 txdata[0] = 0x11;
ksingyuunyuu 1:979cc2a56e7e 250 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 251 txdata[2] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:979cc2a56e7e 252 txdata[3] = 0x00; //MT1=-2000rpm
ksingyuunyuu 1:979cc2a56e7e 253 txdata[4] = 0x41;
ksingyuunyuu 1:979cc2a56e7e 254 txdata[5] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 255 txdata[6] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 256
ksingyuunyuu 1:979cc2a56e7e 257 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 258 can1.write(CANMessage(CANID,&txdata[0],7));
ksingyuunyuu 1:979cc2a56e7e 259 }
ksingyuunyuu 1:979cc2a56e7e 260
ksingyuunyuu 1:979cc2a56e7e 261 void motor( int rpm1, int rpm2 ){ // 全モータ前進
ksingyuunyuu 1:979cc2a56e7e 262 //MT1=0rpm MT2=0rpm
ksingyuunyuu 1:979cc2a56e7e 263 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 264 int CANID;
ksingyuunyuu 1:979cc2a56e7e 265
ksingyuunyuu 1:979cc2a56e7e 266 int motor1_rpm = -rpm1;
ksingyuunyuu 1:979cc2a56e7e 267 int motor2_rpm = rpm2;
ksingyuunyuu 1:979cc2a56e7e 268
ksingyuunyuu 1:979cc2a56e7e 269 char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 270 char motor1_data2 = motor1_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 271 char motor2_data1 = (motor2_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 272 char motor2_data2 = motor2_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 273
ksingyuunyuu 1:979cc2a56e7e 274 txdata[0] = 0x11;
ksingyuunyuu 1:979cc2a56e7e 275 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 276 txdata[2] = motor1_data1;
ksingyuunyuu 1:979cc2a56e7e 277 txdata[3] = motor1_data2;
ksingyuunyuu 1:979cc2a56e7e 278 txdata[4] = 0x41;
ksingyuunyuu 1:979cc2a56e7e 279 txdata[5] = motor2_data1;
ksingyuunyuu 1:979cc2a56e7e 280 txdata[6] = motor2_data2;
ksingyuunyuu 1:979cc2a56e7e 281
ksingyuunyuu 1:979cc2a56e7e 282 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 283 can1.write(CANMessage(CANID,&txdata[0],7));
ksingyuunyuu 1:979cc2a56e7e 284 /*
ksingyuunyuu 1:979cc2a56e7e 285 printf("motor1_data1 = %02x\n",motor1_data1);
ksingyuunyuu 1:979cc2a56e7e 286 printf("motor1_data2 = %02x\n",motor1_data2);
ksingyuunyuu 1:979cc2a56e7e 287 printf("motor2_data1 = %02x\n",motor2_data1);
ksingyuunyuu 1:979cc2a56e7e 288 printf("motor2_data2 = %02x\n",motor2_data2);
ksingyuunyuu 1:979cc2a56e7e 289 */
ksingyuunyuu 1:979cc2a56e7e 290 }
ksingyuunyuu 1:979cc2a56e7e 291
ksingyuunyuu 1:979cc2a56e7e 292 void motor1_drive( int rpm ){ // モータ1正転
ksingyuunyuu 1:979cc2a56e7e 293 //MT1=2000rpm
ksingyuunyuu 1:979cc2a56e7e 294 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 295 int CANID;
ksingyuunyuu 1:979cc2a56e7e 296
ksingyuunyuu 1:979cc2a56e7e 297 int motor1_rpm;
ksingyuunyuu 1:979cc2a56e7e 298 if(rpm >= 0){
ksingyuunyuu 1:979cc2a56e7e 299 motor1_rpm = -rpm;
ksingyuunyuu 1:979cc2a56e7e 300 } else {
ksingyuunyuu 1:979cc2a56e7e 301 motor1_rpm = rpm;
ksingyuunyuu 1:979cc2a56e7e 302 }
ksingyuunyuu 1:979cc2a56e7e 303
ksingyuunyuu 1:979cc2a56e7e 304 char motor1_data1 = (motor1_rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 305 char motor1_data2 = motor1_rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 306
ksingyuunyuu 1:979cc2a56e7e 307 txdata[0] = 0x10;
ksingyuunyuu 1:979cc2a56e7e 308 txdata[1] = 0x40; //rpmcmd1
ksingyuunyuu 1:979cc2a56e7e 309 txdata[2] = motor1_data1;
ksingyuunyuu 1:979cc2a56e7e 310 txdata[3] = motor1_data2;
ksingyuunyuu 1:979cc2a56e7e 311
ksingyuunyuu 1:979cc2a56e7e 312
ksingyuunyuu 1:979cc2a56e7e 313 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 314 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:979cc2a56e7e 315 /*
ksingyuunyuu 1:979cc2a56e7e 316 printf("motor1_data1 = %02x\n",motor1_data1);
ksingyuunyuu 1:979cc2a56e7e 317 printf("motor1_data2 = %02x\n",motor1_data2);
ksingyuunyuu 1:979cc2a56e7e 318 */
ksingyuunyuu 1:979cc2a56e7e 319 }
ksingyuunyuu 1:979cc2a56e7e 320
ksingyuunyuu 1:979cc2a56e7e 321 void motor2_drive( int rpm ){ // モータ2前進
ksingyuunyuu 1:979cc2a56e7e 322 //MT2=2000rpm
ksingyuunyuu 1:979cc2a56e7e 323 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 324 int CANID;
ksingyuunyuu 1:979cc2a56e7e 325 char motor2_data1 = (rpm & 0xff00) >> 8;
ksingyuunyuu 1:979cc2a56e7e 326 char motor2_data2 = rpm & 0x00ff;
ksingyuunyuu 1:979cc2a56e7e 327 txdata[0] = 0x10;
ksingyuunyuu 1:979cc2a56e7e 328 txdata[1] = 0x41; //rpmcmd2
ksingyuunyuu 1:979cc2a56e7e 329 txdata[2] = motor2_data1;
ksingyuunyuu 1:979cc2a56e7e 330 txdata[3] = motor2_data2;
ksingyuunyuu 1:979cc2a56e7e 331 CANID=0x200 | ((MDwrite)&0xf)<<5 | 0x0a ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：受信（MD側）　下位5bit：デバイスID：01番
ksingyuunyuu 1:979cc2a56e7e 332 can1.write(CANMessage(CANID,&txdata[0],4));
ksingyuunyuu 1:979cc2a56e7e 333 /*
ksingyuunyuu 1:979cc2a56e7e 334 printf("motor2_data1 = %02x\n",motor2_data1);
ksingyuunyuu 1:979cc2a56e7e 335 printf("motor2_data2 = %02x\n",motor2_data2);
ksingyuunyuu 1:979cc2a56e7e 336 */
ksingyuunyuu 1:979cc2a56e7e 337 }
ksingyuunyuu 1:979cc2a56e7e 338
ksingyuunyuu 1:979cc2a56e7e 339 void MD_mode_change(int mode){ // モータドライバのモード変更関数
ksingyuunyuu 1:979cc2a56e7e 340 char txdata[8];
ksingyuunyuu 1:979cc2a56e7e 341 int CANID;
ksingyuunyuu 1:979cc2a56e7e 342
ksingyuunyuu 1:979cc2a56e7e 343 txdata[0] = 0x05;
ksingyuunyuu 1:979cc2a56e7e 344 txdata[1] = mode;
ksingyuunyuu 1:979cc2a56e7e 345 txdata[2] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 346 txdata[3] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 347 txdata[4] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 348 txdata[5] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 349 txdata[6] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 350 txdata[7] = 0x00;
ksingyuunyuu 1:979cc2a56e7e 351
ksingyuunyuu 1:979cc2a56e7e 352 CANID=0x200 | ((MDcontroll)&0xf)<<5 | 0xa ; // 上位２ビット：優先度：01　次の4bit：プロトコルID：制御モード　下位5bit：デバイスID：１０番
ksingyuunyuu 1:979cc2a56e7e 353 can1.write(CANMessage(CANID,&txdata[0],8));
ksingyuunyuu 1:979cc2a56e7e 354
ksingyuunyuu 0:27046fed2426 355 }