add pcon f and b

Dependencies:   mbed HCSR04

Revision:
3:c39c14cfc811
Parent:
2:93d72af1b94c
Child:
4:e1aa0b348aeb
--- a/main.cpp	Sun Dec 19 09:36:45 2021 +0000
+++ b/main.cpp	Sun Dec 19 11:16:07 2021 +0000
@@ -6,24 +6,20 @@
 //エンコーダ読み取りによる入力
 //加減速度調整パラメータ実装
 //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);
@@ -49,7 +45,6 @@
 //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
@@ -63,7 +58,6 @@
 //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
@@ -75,10 +69,8 @@
 //---------------------------------------------
 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;
  
@@ -95,19 +87,22 @@
     int node3 = 3;
     int node4 = 4;
     
+    //モータ回転数
+    int rpm = 600;
+    
     //各モータ回転数
-    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 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_A + 200;
-    int min_rpm = RPM_A -200;
+    int max_rpm = rpm + 200;
+    int min_rpm = rpm -200;
     int wall_distance = 0;
     int dist1, dist2,dist3, dist4;
     
@@ -130,8 +125,11 @@
     wait(0.1);
     //-------------起動時に必ず送信---------------
     //オペレーティングモードを送信
-    set_MODE_T();
-
+    sendOPModeT(node1);
+    sendOPModeT(node2);
+    sendOPModeT(node3);
+    sendOPModeT(node4);
+        
     //Shutdown,Enableコマンド送信|リセット
     sendCtrlSD(node1);
     sendCtrlSD(node2);
@@ -144,9 +142,19 @@
     sendCtrlEN(node4);
     
     //初期加減速度
-    set_ACC(ACC_D);
-    set_DEC(DEC_D);
-     
+    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]
   
@@ -197,10 +205,10 @@
                         dist2 = u2.get_dist_cm();    
    
                         //速度を指定
-                        sendTgtVel(1,RPM_A*(-1));
-                        sendTgtVel(2,RPM_A*(-1));
-                        sendTgtVel(3,RPM_A);
-                        sendTgtVel(4,RPM_A);
+                        sendTgtVel(1,rpm*(-1));
+                        sendTgtVel(2,rpm*(-1));
+                        sendTgtVel(3,rpm);
+                        sendTgtVel(4,rpm);
                         //指令値を送信
                         for(int i=1;i<= 4;i++){
                             sendCtrlEN(i);
@@ -240,7 +248,11 @@
                         if(ActPos > -10000){
                             break;
                         }    
-                        set_POS(Init_Pos);
+ 
+                        sendTgtVel(1,rpm);
+                        sendTgtVel(2,rpm);
+                        sendTgtVel(3,rpm*(-1));
+                        sendTgtVel(4,rpm*(-1));
                         //指令値を送信
                         for(int i=1;i<= 4;i++){
                             sendCtrlEN(i);    
@@ -249,7 +261,10 @@
                     }
  
                     //ブレーキ
-                    set_POS(Init_Pos);
+                    sendTgtVel(1,0);
+                    sendTgtVel(2,0);
+                    sendTgtVel(3,0);
+                    sendTgtVel(4,0);
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);    
                     }
@@ -276,10 +291,10 @@
                     set_MODE_V();
 
                     //速度を指定
-                    sendTgtVel(1,RPM_A);
-                    sendTgtVel(2,RPM_A*(-1));
-                    sendTgtVel(3,RPM_A*(-1));
-                    sendTgtVel(4,RPM_A);
+                    sendTgtVel(1,rpm);
+                    sendTgtVel(2,rpm*(-1));
+                    sendTgtVel(3,rpm*(-1));
+                    sendTgtVel(4,rpm);
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);
@@ -301,10 +316,10 @@
  
                  
                     //速度を指定
-                    sendTgtVel(1,RPM_A*(-1));
-                    sendTgtVel(2,RPM_A);
-                    sendTgtVel(3,RPM_A);
-                    sendTgtVel(4,RPM_A*(-1));
+                    sendTgtVel(1,rpm*(-1));
+                    sendTgtVel(2,rpm);
+                    sendTgtVel(3,rpm);
+                    sendTgtVel(4,rpm*(-1));
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);    
@@ -343,10 +358,10 @@
                     set_MODE_V();
 
                     //速度を指定
-                    sendTgtVel(1,RPM_A);
-                    sendTgtVel(2,RPM_A);
-                    sendTgtVel(3,RPM_A);
-                    sendTgtVel(4,RPM_A);
+                    sendTgtVel(1,rpm);
+                    sendTgtVel(2,rpm);
+                    sendTgtVel(3,rpm);
+                    sendTgtVel(4,rpm);
                     //指令値を送信
                     for(int i=1;i<= 4;i++){
                         sendCtrlEN(i);
@@ -355,10 +370,10 @@
                      wait(1.0);
                     
                     //速度を指定
-                    sendTgtVel(1,RPM_A*(-1));
-                    sendTgtVel(2,RPM_A*(-1));
-                    sendTgtVel(3,RPM_A*(-1));
-                    sendTgtVel(4,RPM_A*(-1));
+                    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);    
@@ -367,8 +382,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++){
@@ -387,7 +402,6 @@
         }
 }
  
-
 void set_ACC(int setACC_val){
     sendProAcc(1,setACC_val);
     sendProAcc(2,setACC_val);
@@ -402,13 +416,6 @@
     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);
@@ -423,13 +430,6 @@
     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,6 +449,7 @@
     wait_ms(1);
 }
  
+ 
 //0x2F-6060-00-03-//-//-//
 void sendOPModeV(int nodeID){
     canmsgTx.id = 0x600+nodeID;
@@ -467,25 +468,6 @@
     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){
@@ -594,7 +576,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);
@@ -612,6 +594,9 @@
     //wait(0.5);
 }
  
+ 
+ 
+ 
 //0x2B-60FF-00-[user data(4Byte)]
 void sendTgtVel(int nodeID,int rpm){
     //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);  //回転数送信データの表示
@@ -625,7 +610,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);
@@ -637,29 +622,7 @@
     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;
@@ -714,7 +677,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