Kanta Takasu
yuka_tks
Diff: main.cpp
- 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