add pcon f and b

Dependencies:   mbed HCSR04

Revision:
2:93d72af1b94c
Parent:
1:b2bd1511307e
Child:
3:c39c14cfc811
--- a/main.cpp	Sun Dec 19 09:10:50 2021 +0000
+++ b/main.cpp	Sun Dec 19 09:36:45 2021 +0000
@@ -6,20 +6,24 @@
 //エンコーダ読み取りによる入力
 //加減速度調整パラメータ実装
 //PID制御による機体角度補正_
-//aoki 
+//aoki
  
 #include "mbed.h"
 #include "hcsr04.h"
 
+#define RPM_A 600
+
 #define ACC_B 1000
 #define DEC_B 700
+
 #define ACC_C 1000
 #define DEC_C 1000
 
+#define ACC_D 1000
+#define DEC_D 1000
+
 #define KICK 2000
 
-
- 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -45,6 +49,7 @@
 //mode Setting
 void sendOPModeT(int);       //Operating Mode
 void sendOPModeV(int);       //Operating Mode
+void sendOPModeP(int);       //Oeratinog Mode
 //Control Word
 void sendCtrlRS(int);       //Reset
 void sendCtrlSD(int);       //Shutdown
@@ -58,6 +63,7 @@
 //Acceleration Setting
 void sendProAcc(int,int);   //Plof Acceleration
 void sendProDec(int,int);   //Plof Deceleration
+void sendTgtPos(int,int);
 //------------------read関数-------------------
 void readActVel(int);       //Actual Velocity
 void readActPos(int);       //Actual Position
@@ -69,8 +75,10 @@
 //---------------------------------------------
 void set_ACC(int);
 void set_DEC(int);
+void set_POS(int);
 void set_MODE_V(void);
 void set_MODE_T(void);
+void set_MODE_P(void);
 
 int nodeall=4;
  
@@ -87,22 +95,19 @@
     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 rpm1 = RPM_A; //Velocity Setting[rpm]
+    int rpm2 = RPM_A; //Velocity Setting[rpm]
+    int rpm3 = RPM_A; //Velocity Setting[rpm]
+    int rpm4 = RPM_A; //Velocity Setting[rpm]
     
     //エンコーダ関係
     int ActPos = 0;
     int Init_Pos = 0;
     
     //超音波センサ関係パラメータ
-    int max_rpm = rpm + 200;
-    int min_rpm = rpm -200;
+    int max_rpm = RPM_A + 200;
+    int min_rpm = RPM_A -200;
     int wall_distance = 0;
     int dist1, dist2,dist3, dist4;
     
@@ -125,11 +130,8 @@
     wait(0.1);
     //-------------起動時に必ず送信---------------
     //オペレーティングモードを送信
-    sendOPModeT(node1);
-    sendOPModeT(node2);
-    sendOPModeT(node3);
-    sendOPModeT(node4);
-        
+    set_MODE_T();
+
     //Shutdown,Enableコマンド送信|リセット
     sendCtrlSD(node1);
     sendCtrlSD(node2);
@@ -142,19 +144,9 @@
     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);  
- 
+    set_ACC(ACC_D);
+    set_DEC(DEC_D);
+     
     //トルク設定
     int trq = 100;   //torque Setting[mA]
   
@@ -205,10 +197,10 @@
                         dist2 = u2.get_dist_cm();    
    
                         //速度を指定
-                        sendTgtVel(1,rpm*(-1));
-                        sendTgtVel(2,rpm*(-1));
-                        sendTgtVel(3,rpm);
-                        sendTgtVel(4,rpm);
+                        sendTgtVel(1,RPM_A*(-1));
+                        sendTgtVel(2,RPM_A*(-1));
+                        sendTgtVel(3,RPM_A);
+                        sendTgtVel(4,RPM_A);
                         //指令値を送信
                         for(int i=1;i<= 4;i++){
                             sendCtrlEN(i);
@@ -248,11 +240,7 @@
                         if(ActPos > -10000){
                             break;
                         }    
- 
-                        sendTgtVel(1,rpm);
-                        sendTgtVel(2,rpm);
-                        sendTgtVel(3,rpm*(-1));
-                        sendTgtVel(4,rpm*(-1));
+                        set_POS(Init_Pos);
                         //指令値を送信
                         for(int i=1;i<= 4;i++){
                             sendCtrlEN(i);    
@@ -261,10 +249,7 @@
                     }
  
                     //ブレーキ
-                    sendTgtVel(1,0);
-                    sendTgtVel(2,0);
-                    sendTgtVel(3,0);
-                    sendTgtVel(4,0);
+                    set_POS(Init_Pos);
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);    
                     }
@@ -291,10 +276,10 @@
                     set_MODE_V();
 
                     //速度を指定
-                    sendTgtVel(1,rpm);
-                    sendTgtVel(2,rpm*(-1));
-                    sendTgtVel(3,rpm*(-1));
-                    sendTgtVel(4,rpm);
+                    sendTgtVel(1,RPM_A);
+                    sendTgtVel(2,RPM_A*(-1));
+                    sendTgtVel(3,RPM_A*(-1));
+                    sendTgtVel(4,RPM_A);
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);
@@ -316,10 +301,10 @@
  
                  
                     //速度を指定
-                    sendTgtVel(1,rpm*(-1));
-                    sendTgtVel(2,rpm);
-                    sendTgtVel(3,rpm);
-                    sendTgtVel(4,rpm*(-1));
+                    sendTgtVel(1,RPM_A*(-1));
+                    sendTgtVel(2,RPM_A);
+                    sendTgtVel(3,RPM_A);
+                    sendTgtVel(4,RPM_A*(-1));
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);    
@@ -358,10 +343,10 @@
                     set_MODE_V();
 
                     //速度を指定
-                    sendTgtVel(1,rpm);
-                    sendTgtVel(2,rpm);
-                    sendTgtVel(3,rpm);
-                    sendTgtVel(4,rpm);
+                    sendTgtVel(1,RPM_A);
+                    sendTgtVel(2,RPM_A);
+                    sendTgtVel(3,RPM_A);
+                    sendTgtVel(4,RPM_A);
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);
@@ -370,10 +355,10 @@
                      wait(1.0);
                     
                     //速度を指定
-                    sendTgtVel(1,rpm*(-1));
-                    sendTgtVel(2,rpm*(-1));
-                    sendTgtVel(3,rpm*(-1));
-                    sendTgtVel(4,rpm*(-1));
+                    sendTgtVel(1,RPM_A*(-1));
+                    sendTgtVel(2,RPM_A*(-1));
+                    sendTgtVel(3,RPM_A*(-1));
+                    sendTgtVel(4,RPM_A*(-1));
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);    
@@ -382,8 +367,8 @@
                     wait(1.3);
     
                     //ブレーキ
-                     sendTgtVel(1,0);
-                     sendTgtVel(2,0);
+                    sendTgtVel(1,0);
+                    sendTgtVel(2,0);
                     sendTgtVel(3,0);
                     sendTgtVel(4,0);
                     for(int i=1;i<= 4;i++){
@@ -402,6 +387,7 @@
         }
 }
  
+
 void set_ACC(int setACC_val){
     sendProAcc(1,setACC_val);
     sendProAcc(2,setACC_val);
@@ -416,6 +402,13 @@
     sendProDec(4,setDEC_val); 
 }
 
+void set_POS(int setPOS_val){
+    sendTgtPos(1,setPOS_val);
+    sendTgtPos(1,setPOS_val);
+    sendTgtPos(1,setPOS_val);
+    sendTgtPos(1,setPOS_val);
+}
+
 void set_MODE_V(){
     sendOPModeV(1);
     sendOPModeV(2);
@@ -430,6 +423,13 @@
     sendOPModeT(4);
 }
 
+void set_MODE_P(){
+    sendOPModeP(1);
+    sendOPModeP(2);
+    sendOPModeP(3);
+    sendOPModeP(4);
+}
+
 //0x2F-6060-00-fd-//-//-//
 void sendOPModeT(int nodeID){
     canmsgTx.id = 0x600+nodeID;
@@ -449,7 +449,6 @@
     wait_ms(1);
 }
  
- 
 //0x2F-6060-00-03-//-//-//
 void sendOPModeV(int nodeID){
     canmsgTx.id = 0x600+nodeID;
@@ -468,6 +467,25 @@
     canPort.write(canmsgTx);//CANでデータ送信
     wait_ms(1);
 }
+
+//0x2F-6060-00-03-//-//-//
+void sendOPModeP(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] = 0x01;//data:0x01 = "Profile Position 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){
@@ -576,7 +594,7 @@
     canmsgTx.data[3] = 0x00;//sub-Index
     //下位から1Byteずつdataに格納
     if(trq<0){
-    trq=0xFFFF+trq+1;
+        trq=0xFFFF+trq+1;
     }
     
     //pc.printf("iii%d\r\n",trq);
@@ -594,9 +612,6 @@
     //wait(0.5);
 }
  
- 
- 
- 
 //0x2B-60FF-00-[user data(4Byte)]
 void sendTgtVel(int nodeID,int rpm){
     //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
@@ -610,7 +625,7 @@
     
     //pc.printf("%d\r\n",rpm);
     if(rpm<0){
-    rpm=0xFFFFFFFF+rpm+1;
+        rpm=0xFFFFFFFF+rpm+1;
     } 
     canmsgTx.data[7]=((rpm>>24)&0xFF);
     canmsgTx.data[6]=((rpm>>16)&0xFF);
@@ -622,7 +637,29 @@
     wait_ms(1);
     
 }
- 
+
+void sendTgtPos(int nodeID,int pos){
+    pc.printf("%d|0x%08x\r\n",pos,pos);  //位置送信データの表示
+    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] = 0x7A;//Index LowByte
+    canmsgTx.data[2] = 0x60;//Index HighByte
+    canmsgTx.data[3] = 0x00;//sub-Index
+    //下位から1Byteずつdataに格納
+    for(char cnt=4;cnt<8;cnt++){
+        canmsgTx.data[cnt] = pos % 256;
+        pos = pos / 256;
+    }
+    printCANTX();          //CAN送信データをPCに表示
+    canPort.write(canmsgTx);//CANでデータ送信
+    wait(0.5);
+    //send Enable
+    pc.printf("Send Enable Command\r\n");
+    sendCtrlEN(nodeID);
+    wait_ms(1);
+}
+
 void readActVel(int nodeID){
     //値が欲しいobjectのアドレスを送る
     canmsgTx.id = 0x600+nodeID;
@@ -677,7 +714,7 @@
 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