Kanta Takasu
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);
+}