Kanta Takasu
gohome ok
Diff: main.cpp
- Revision:
- 3:c39c14cfc811
- Parent:
- 2:93d72af1b94c
- Child:
- 4:1bd08c9d92a9
--- 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