Kanta Takasu
/
YUKA_FIN
gohome ok
Diff: main.cpp
- Revision:
- 0:b4b94eb28093
- Child:
- 1:b2bd1511307e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Dec 19 05:53:03 2021 +0000 @@ -0,0 +1,1236 @@ +//2021/12/18更新 +//YUKA本番機用プログラム +//入力切替確認済み +//一定時間の入力なしで動作切替 +//ジグザグ動作実装前 +//エンコーダ読み取りによる入力 +//加減速度調整パラメータ実装 +//PID制御による機体角度補正_ + +#include "mbed.h" +#include "hcsr04.h" + + +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); + +Timer t; +double Time = 0; +#define Standby_Time 10 +Serial pc(USBTX, USBRX); +char Serialdata; +BusOut myled(LED1, LED2, LED3, LED4); + +//HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768 +HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8 + +CANMessage canmsgTx; +CANMessage canmsgRx; +//CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768 +CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8 + +//プロトタイプ宣言 +//------------------send関数------------------- +//mode Setting +void sendOPModeT(int); //Operating Mode +void sendOPModeV(int); //Operating Mode +//Control Word +void sendCtrlRS(int); //Reset +void sendCtrlSD(int); //Shutdown +void sendCtrlEN(int); //Switch on & Enable +void sendCtrlQS(int); //Quick Stop +void sendCtrlHL(int); //Halt +//Velocity Setting +void sendTgtVel(int,int); //Target Velocity +//Torque Setting +void sendTgtTrq(int,int); //Target Torque +//Acceleration Setting +void sendProAcc(int,int); //Plof Acceleration +void sendProDec(int,int); //Plof Deceleration +//------------------read関数------------------- +void readActVel(int); //Actual Velocity +void readActPos(int); //Actual Position +//-------------------その他-------------------- +void printCANTX(void); //CAN送信データをPCに表示 +void printCANRX(void); //CAN受信データをPCに表示 +void CANdataRX(void); //CAN受信処理 +void SerialRX(void); //Serial受信処理 + +int nodeall=4; + +int main(){ + //Serial + pc.attach(SerialRX); + //pc.baud(115200); + + //CAN + canPort.frequency(1000000); //Bit Rate:1MHz + canPort.attach(CANdataRX,CAN::RxIrq); + int node1 = 1; //CAN node Setting + int node2 = 2; + int node3 = 3; + int node4 = 4; + + //モータ回転数 + int rpm = 600; + + //各モータ回転数 + int rpm1 = rpm; //Velocity Setting[rpm] + int rpm2 = rpm; //Velocity Setting[rpm] + int rpm3 = rpm; //Velocity Setting[rpm] + int rpm4 = rpm; //Velocity Setting[rpm] + + //エンコーダ関係 + int ActPos = 0; + int Init_Pos = 0; + + //超音波センサ関係パラメータ + int max_rpm = rpm + 200; + int min_rpm = rpm -200; + int wall_distance = 0; + int dist1, dist2,dist3, dist4; + + + //PID制御関係 + //角度調整パラメータ + int p1, i1, d1; + int error_before1 = 0; + int error_now1 = 0; + int feedback_val1; + int integral1; + int pid_sum1; + #define DELTA_T1 0.1 + #define target_val1 0 + #define Kp1 3 + #define Ki1 0 + #define Kd1 0 + + /* + //距離調整パラメータ + int p2, i2, d2; + int error_before2 = 0; + int error_now2 = 0; + int feedback_val2; + int integral2; + int pid_sum2; + #define DELTA_T2 0.01 + #define target_val2 50 + #define Kp2 2.0 + #define Ki2 3.0 + #define Kd2 3.0 + */ + + pc.printf("YUKA PROGRAM START\r\n"); + wait(0.1); + //-------------起動時に必ず送信--------------- + //オペレーティングモードを送信 + sendOPModeT(node1); + sendOPModeT(node2); + sendOPModeT(node3); + sendOPModeT(node4); + + //Shutdown,Enableコマンド送信|リセット + sendCtrlSD(node1); + sendCtrlSD(node2); + sendCtrlSD(node3); + sendCtrlSD(node4); + + sendCtrlEN(node1); + sendCtrlEN(node2); + sendCtrlEN(node3); + sendCtrlEN(node4); + + //初期加減速度 + int Acc = 2000; + int Dec = 2000; + + sendProAcc(1,Acc); + sendProAcc(2,Acc); + sendProAcc(3,Acc); + sendProAcc(4,Acc); + + sendProDec(1,Dec); + sendProDec(2,Dec); + sendProDec(3,Dec); + sendProDec(4,Dec); + + //トルク設定 + int trq = 100; //torque Setting[mA] + + sendTgtTrq(1,trq); + sendTgtTrq(2,trq); + sendTgtTrq(3,trq); + sendTgtTrq(4,trq); + + + //------------------------------------------- + /* + sendCtrlHL(node1); + sendCtrlHL(node2); + sendCtrlHL(node3); + sendCtrlHL(node4); + + //-------------送信コマンドを選択-------------- + if(Serialdata == 'a'){ + //左移動 + sendTgtVel(node1,rpm*(-1)); + sendTgtVel(node2,rpm); + sendTgtVel(node3,rpm); + sendTgtVel(node4,rpm*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + Serialdata = 0; + fl=1; + wait(0.5); + } + + else if(Serialdata == 'd'){ + //右移動 + sendTgtVel(node1,rpm); + sendTgtVel(node2,rpm*(-1)); + sendTgtVel(node3,rpm*(-1)); + sendTgtVel(node4,rpm); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + Serialdata = 0; + fl=1; + wait(0.5); + } + + else if(Serialdata == 'w'){ + //前移動 + sendTgtVel(node1,rpm*(-1)); + sendTgtVel(node2,rpm*(-1)); + sendTgtVel(node3,rpm); + sendTgtVel(node4,rpm); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + Serialdata = 0; + fl=1; + wait(0.5); + } + + else if(Serialdata == 's'){ + //後移動 + sendTgtVel(node1,rpm); + sendTgtVel(node2,rpm); + sendTgtVel(node3,rpm*(-1)); + sendTgtVel(node4,rpm*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + Serialdata = 0; + fl=1; + wait(0.5); + } + + else if(Serialdata == 'e'){ + //右旋回 + sendTgtVel(node1,rpm); + sendTgtVel(node2,rpm); + sendTgtVel(node3,rpm); + sendTgtVel(node4,rpm); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + Serialdata = 0; + fl=1; + wait(0.1); + } + else if(Serialdata == 'q'){ + //左旋回 + sendTgtVel(node1,rpm*(-1)); + sendTgtVel(node2,rpm*(-1)); + sendTgtVel(node3,rpm*(-1)); + sendTgtVel(node4,rpm*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + Serialdata = 0; + fl=1; + wait(0.1); + } + */ + //------------------------------------------ + + //人の入力待ち + /* + while(1){ + readActPos(1); + ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; + if(ActPos > 8388608){ + ActPos -= 0x1000000; + printf("check\r\n"); + } + printf("ActPos = %d\r\n",ActPos); + wait(0.1); + } + */ + + while(1){ + + ActPos = 0; + Init_Pos = 0; + readActPos(1); + ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; + if(ActPos > 8388608){ + ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 + } + Init_Pos = ActPos;//起動時の角度を保存 + t.reset(); + t.start(); + + while(1){ + + Time = t.read(); + + readActPos(1); + ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; + if(ActPos > 8388608){ + ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 + //printf("check\r\n"); + } + printf("dsit1 %d\r\n",dist1); + /*--------------------------*/ + //前方向に入力があった時 + if(ActPos < Init_Pos - 2000){ + + sendProAcc(1,1000); + sendProAcc(2,1000); + sendProAcc(3,1000); + sendProAcc(4,1000); + + sendProDec(1,700); + sendProDec(2,700); + sendProDec(3,700); + sendProDec(4,700); + + //速度制御モード送信 + sendOPModeV(1); + sendOPModeV(2); + sendOPModeV(3); + sendOPModeV(4); + + while(1){ + + u1.start(); + u2.start(); + dist1 = u1.get_dist_cm(); + dist2 = u2.get_dist_cm(); + + //速度を指定 + sendTgtVel(1,rpm*(-1)); + sendTgtVel(2,rpm*(-1)); + sendTgtVel(3,rpm); + sendTgtVel(4,rpm); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + if(dist1 < 100 && dist1 >= 70){ + break; + } + printf("dsit1 %d\r\n",dist1); + } + + //速度を指定 + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + printf("stop!!\r\n"); + wait(5.0); + + //printf("2\r\n"); + + //速度を指定 + + while(1){ + readActPos(1); + ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; + if(ActPos > 8388608){ + ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 + //printf("check\r\n"); + } + + if(ActPos > -10000){ + break; + } + + sendTgtVel(1,rpm); + sendTgtVel(2,rpm); + sendTgtVel(3,rpm*(-1)); + sendTgtVel(4,rpm*(-1)); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //printf("3\r\n"); + } + + //ブレーキ + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + wait(0.5); + + sendOPModeT(1); + sendOPModeT(2); + sendOPModeT(3); + sendOPModeT(4); + + dist1 = 0; + + wait(3.0); + break; + } + /*--------------------------*/ + + /*--------------------------*/ + //右方向に入力があった時 + if(ActPos > Init_Pos + 2000){ + sendProAcc(1,1000); + sendProAcc(2,1000); + sendProAcc(3,1000); + sendProAcc(4,1000); + + sendProDec(1,1000); + sendProDec(2,1000); + sendProDec(3,1000); + sendProDec(4,1000); + + //速度制御モード送信 + sendOPModeV(1); + sendOPModeV(2); + sendOPModeV(3); + sendOPModeV(4); + //速度を指定 + sendTgtVel(1,rpm); + sendTgtVel(2,rpm*(-1)); + sendTgtVel(3,rpm*(-1)); + sendTgtVel(4,rpm); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + wait(2.0); + + //速度を指定 + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + wait(5.0); + + + //速度を指定 + sendTgtVel(1,rpm*(-1)); + sendTgtVel(2,rpm); + sendTgtVel(3,rpm); + sendTgtVel(4,rpm*(-1)); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + wait(2.0); + + //ブレーキ + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + wait(0.5); + + sendOPModeT(1); + sendOPModeT(2); + sendOPModeT(3); + sendOPModeT(4); + + wait(4.0); + break; + } + /*--------------------------*/ + + /*--------------------------*/ + //一定時間入力がない場合 + if(Time > Standby_Time){ + sendProAcc(1,5000); + sendProAcc(2,5000); + sendProAcc(3,5000); + sendProAcc(4,5000); + + sendProDec(1,5000); + sendProDec(2,5000); + sendProDec(3,5000); + sendProDec(4,5000); + + //速度制御モード送信 + sendOPModeV(1); + sendOPModeV(2); + sendOPModeV(3); + sendOPModeV(4); + + //速度を指定 + sendTgtVel(1,rpm); + sendTgtVel(2,rpm); + sendTgtVel(3,rpm); + sendTgtVel(4,rpm); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + wait(1.0); + + //速度を指定 + sendTgtVel(1,rpm*(-1)); + sendTgtVel(2,rpm*(-1)); + sendTgtVel(3,rpm*(-1)); + sendTgtVel(4,rpm*(-1)); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //実行時間 + wait(1.3); + + //ブレーキ + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + wait(0.5); + + sendOPModeT(1); + sendOPModeT(2); + sendOPModeT(3); + sendOPModeT(4); + + wait(5.0); + break; + } + /*--------------------------*/ + + } + wait(0.1); + } + + /* + //ジグザグ + else if(Serialdata == 'j'){ + int turn_count = 0; + p1 = 0; + i1 = 0; + d1 = 0; + error_before1 = 0; + error_now1 = 0; + feedback_val1 = 0; + integral1 = 0; + pid_sum1 = 0; + + rpm1 = rpm; + rpm2 = rpm; + rpm3 = rpm; + rpm4 = rpm; + + dist1 = 30; + dist2 = 30; + dist3 = 30; + + sendProAcc(1,5000); + sendProAcc(2,5000); + sendProAcc(3,5000); + sendProAcc(4,5000); + + sendProDec(1,5000); + sendProDec(2,5000); + sendProDec(3,5000); + sendProDec(4,5000); + + //正面壁に近づく + while(!(dist1 <30 && dist1 >5)){ + printf("phase1\r\n"); + u1.start(); + u2.start(); + dist1 = u1.get_dist_cm(); + dist2 = u2.get_dist_cm(); + pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4); + + //壁との角度をPID制御 + feedback_val1 = (dist1 - dist2); + + error_before1 = error_now1; + error_now1 = feedback_val1 - target_val1; + integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1; + + p1 = Kp1 * error_now1; + i1 = Ki1 * integral1; + d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1; + + pid_sum1 = (p1 + i1 + d1); + + //モータの回転数を更新 + rpm1 -= pid_sum1; + rpm2 -= pid_sum1; + rpm3 += pid_sum1; + rpm4 += pid_sum1; + + sendTgtVel(node1,rpm1); + sendTgtVel(node2,rpm2); + sendTgtVel(node3,rpm3*(-1)); + sendTgtVel(node4,rpm4*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + //回転数フィルタ + if(rpm1 >= max_rpm){ + rpm1 = max_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 >= max_rpm){ + rpm2 = max_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 >= max_rpm){ + rpm3 = max_rpm; + sendCtrlEN(3); + } + + if(rpm4 >= max_rpm){ + rpm4 = max_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + + if(rpm1 <= min_rpm){ + rpm1 = min_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 <= min_rpm){ + rpm2 = min_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 <= min_rpm){ + rpm3 = min_rpm; + sendTgtVel(node3,rpm3); + sendCtrlEN(3); + } + + if(rpm4 <= min_rpm){ + rpm4 = min_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + if(Serialdata == 'h'){ + sendCtrlHL(node1); + sendCtrlHL(node2); + sendCtrlHL(node3); + sendCtrlHL(node4); + } + + + wait(0.01); + } + + //u1,u2が一定以上壁から離れると終了 + while(!(turn_count == 4)){ + + printf("phase2\r\n"); + + dist3 = 30; + + //右端まで移動 + //while(dist3 > 20){ + while(!(dist3 < 30& dist3 >10)){ + printf("phase3\r\n"); + u1.start(); + u2.start(); + u3.start(); + + dist1 = u3.get_dist_cm(); + dist2 = u3.get_dist_cm(); + dist3 = u3.get_dist_cm(); + pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4); + + //壁との角度をPID制御 + feedback_val1 = (dist1 - dist2); + error_before1 = error_now1; + error_now1 = feedback_val1 - target_val1; + integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1; + + p1 = Kp1 * error_now1; + i1 = Ki1 * integral1; + d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1; + + pid_sum1 = (p1 + i1 + d1); + + //モータの回転数を更新 + rpm1 += pid_sum1; + rpm2 -= pid_sum1; + rpm3 -= pid_sum1; + rpm4 += pid_sum1; + + //右移動 + sendTgtVel(node1,rpm1*(-1)); + sendTgtVel(node2,rpm2); + sendTgtVel(node3,rpm3); + sendTgtVel(node4,rpm4*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //回転数フィルタ + if(rpm1 >= max_rpm){ + rpm1 = max_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 >= max_rpm){ + rpm2 = max_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 >= max_rpm){ + rpm3 = max_rpm; + sendCtrlEN(3); + } + + if(rpm4 >= max_rpm){ + rpm4 = max_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + + if(rpm1 <= min_rpm){ + rpm1 = min_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 <= min_rpm){ + rpm2 = min_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 <= min_rpm){ + rpm3 = min_rpm; + sendTgtVel(node3,rpm3); + sendCtrlEN(3); + } + + if(rpm4 <= min_rpm){ + rpm4 = min_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + if(Serialdata == 'h'){ + sendCtrlHL(node1); + sendCtrlHL(node2); + sendCtrlHL(node3); + sendCtrlHL(node4); + } + + + wait(0.01); + } + + //下に少し移動 + + printf("phase4\r\n"); + + sendTgtVel(node1,rpm*(-1)); + sendTgtVel(node2,rpm*(-1)); + sendTgtVel(node3,rpm); + sendTgtVel(node4,rpm); + + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + turn_count += 1; + + wait(0.5); + + + //左に移動 + while(!(dist3 < 80 && dist3 >60)){ + printf("phase5\r\n"); + u1.start(); + u2.start(); + u3.start(); + dist1 = u1.get_dist_cm(); + dist2 = u2.get_dist_cm(); + dist3 = u3.get_dist_cm(); + pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4); + + //壁との角度をPID制御 + feedback_val1 = (dist1 - dist2); + error_before1 = error_now1; + error_now1 = feedback_val1 - target_val1; + integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1; + + p1 = Kp1 * error_now1; + i1 = Ki1 * integral1; + d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1; + + pid_sum1 = (p1 + i1 + d1); + + //モータの回転数を更新 + rpm1 -= pid_sum1; + rpm2 += pid_sum1; + rpm3 += pid_sum1; + rpm4 -= pid_sum1; + + //左移動 + sendTgtVel(node1,rpm1); + sendTgtVel(node2,rpm2*(-1)); + sendTgtVel(node3,rpm3*(-1)); + sendTgtVel(node4,rpm4); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + //回転数フィルタ + if(rpm1 >= max_rpm){ + rpm1 = max_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 >= max_rpm){ + rpm2 = max_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 >= max_rpm){ + rpm3 = max_rpm; + sendCtrlEN(3); + } + + if(rpm4 >= max_rpm){ + rpm4 = max_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + + if(rpm1 <= min_rpm){ + rpm1 = min_rpm; + sendTgtVel(node1,rpm1*(-1)); + sendCtrlEN(1); + } + + if(rpm2 <= min_rpm){ + rpm2 = min_rpm; + sendTgtVel(node2,rpm2); + sendCtrlEN(2); + } + + if(rpm3 <= min_rpm){ + rpm3 = min_rpm; + sendTgtVel(node3,rpm3); + sendCtrlEN(3); + } + + if(rpm4 <= min_rpm){ + rpm4 = min_rpm; + sendTgtVel(node4,rpm4*(-1)); + sendCtrlEN(4); + } + + if(Serialdata == 'h'){ + sendCtrlHL(node1); + sendCtrlHL(node2); + sendCtrlHL(node3); + sendCtrlHL(node4); + } + + wait(0.01); + } + + //下に少し移動 + + printf("phase6\r\n"); + + + sendTgtVel(node1,rpm*(-1)); + sendTgtVel(node2,rpm*(-1)); + sendTgtVel(node3,rpm); + sendTgtVel(node4,rpm); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } + + turn_count += 1; + + wait(0.5); + + } + + + if(Serialdata == 'h'){ + sendCtrlHL(node1); + sendCtrlHL(node2); + sendCtrlHL(node3); + sendCtrlHL(node4); + break; + } + + wait(0.1); + } + + } + */ + //------------------------------------------- + +} + + + + +//0x2F-6060-00-fd-//-//-// +void sendOPModeT(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 5; //Data Length + canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x60;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0xFD;//data:fd = "current Mode" + /* + canmsgTx.data[5] = 0x00;//data:(user value) + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + + +//0x2F-6060-00-03-//-//-// +void sendOPModeV(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 5; //Data Length + canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x60;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode" + /* + canmsgTx.data[5] = 0x00;//data:(user value) + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-0000-//-// +void sendCtrlRS(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)" + canmsgTx.data[5] = 0x00;//data:0x"00"80 + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + + +//0x2B-6040-00-0006-//-// +void sendCtrlSD(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" + canmsgTx.data[5] = 0x00;//data:0x"00"06 + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-000F-//-// +void sendCtrlEN(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" + canmsgTx.data[5] = 0x00;//data:0x"00"0F + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-000B-//-// +void sendCtrlQS(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop" + canmsgTx.data[5] = 0x00;//data:0x"00"0B + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-010F-//-// +void sendCtrlHL(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt" + canmsgTx.data[5] = 0x01;//data:0x"01"0F + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-60FF-00-[user data(4Byte)] +void sendTgtTrq(int nodeID,int trq){ + //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x30;//Index LowByte 71 + canmsgTx.data[2] = 0x20;//Index HighByte 60 + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + if(trq<0){ + trq=0xFFFF+trq+1; + } + + //pc.printf("iii%d\r\n",trq); + //canmsgTx.data[7]=((trq>>24)&0xFF); + //canmsgTx.data[6]=((trq>>16)&0xFF); + + canmsgTx.data[5]=((trq>>8)&0xFF); + canmsgTx.data[4]=((trq>>0)&0xFF); + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(2); + //send Enable + //pc.printf("Send Enable Command\r\n"); + //sendCtrlEN(nodeID); + //wait(0.5); +} + + + + +//0x2B-60FF-00-[user data(4Byte)] +void sendTgtVel(int nodeID,int rpm){ + //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0xFF;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + + //pc.printf("%d\r\n",rpm); + if(rpm<0){ + rpm=0xFFFFFFFF+rpm+1; + } + canmsgTx.data[7]=((rpm>>24)&0xFF); + canmsgTx.data[6]=((rpm>>16)&0xFF); + canmsgTx.data[5]=((rpm>>8)&0xFF); + canmsgTx.data[4]=((rpm>>0)&0xFF); + + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); + +} + +void readActVel(int nodeID){ + //値が欲しいobjectのアドレスを送る + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 4; //Data Length + canmsgTx.data[0] = 0x40;//|0Byte:40| + canmsgTx.data[1] = 0x6C;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canPort.write(canmsgTx); + wait_ms(1); +} + +void readActPos(int nodeID){ + //値が欲しいobjectのアドレスを送る + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 4; //Data Length + canmsgTx.data[0] = 0x40;//|0Byte:40| + canmsgTx.data[1] = 0x64;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canPort.write(canmsgTx); + wait_ms(1); +} + +//加速度指定 +void sendProAcc(int nodeID,int rpm){ + if(rpm < 0){ + rpm += 0xFFFFFFFF; + } +// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x83;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + canmsgTx.data[4] = (rpm >> 0) & 0xFF; + canmsgTx.data[5] = (rpm >> 8) & 0xFF; + canmsgTx.data[6] = (rpm >> 16) & 0xFF; + canmsgTx.data[7] = (rpm >> 24) & 0xFF; +// printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait(0.01); + //send Enable +// pc.printf("Send Enable Command\r\n"); + sendCtrlEN(nodeID); + wait(0.01); +} + +//減速度指定 +void sendProDec(int nodeID,int rpm){ + if(rpm < 0){ + rpm += 0xFFFFFFFF; + } +// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x84;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + canmsgTx.data[4] = (rpm >> 0) & 0xFF; + canmsgTx.data[5] = (rpm >> 8) & 0xFF; + canmsgTx.data[6] = (rpm >> 16) & 0xFF; + canmsgTx.data[7] = (rpm >> 24) & 0xFF; +// printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait(0.01); + //send Enable +// pc.printf("Send Enable Command\r\n"); + sendCtrlEN(nodeID); + wait(0.01); +} + + +//送信データの表示 +void printCANTX(void){ + //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| + pc.printf("0x%3x|",canmsgTx.id); + for(char i=0;i < canmsgTx.len;i++){ + pc.printf("%02x|",canmsgTx.data[i]); + } + //pc.printf("\r\n"); +} + +//受信データの表示 + +void printCANRX(void){ + //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| + //pc.printf("0x%3x|",canmsgRx.id); + for(char i=0;i < canmsgRx.len;i++){ + //pc.printf("%02x|",canmsgRx.data[i]); + } + pc.printf("\r\n"); +} + +void CANdataRX(void){ + canPort.read(canmsgRx); + printCANRX(); +} + +void SerialRX(void){ + Serialdata = pc.getc(); + //pc.printf("%c\r\n",Serialdata); +}