Kanta Takasu
yuka_tks
Diff: main.cpp
- Revision:
- 1:b2bd1511307e
- Parent:
- 0:b4b94eb28093
- Child:
- 2:93d72af1b94c
--- a/main.cpp Sun Dec 19 05:53:03 2021 +0000
+++ b/main.cpp Sun Dec 19 09:10:50 2021 +0000
@@ -6,31 +6,40 @@
//エンコーダ読み取りによる入力
//加減速度調整パラメータ実装
//PID制御による機体角度補正_
-
+//aoki
+
#include "mbed.h"
#include "hcsr04.h"
+#define ACC_B 1000
+#define DEC_B 700
+#define ACC_C 1000
+#define DEC_C 1000
+#define KICK 2000
+
+
+
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
@@ -57,9 +66,14 @@
void printCANRX(void); //CAN受信データをPCに表示
void CANdataRX(void); //CAN受信処理
void SerialRX(void); //Serial受信処理
+//---------------------------------------------
+void set_ACC(int);
+void set_DEC(int);
+void set_MODE_V(void);
+void set_MODE_T(void);
int nodeall=4;
-
+
int main(){
//Serial
pc.attach(SerialRX);
@@ -106,22 +120,7 @@
#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);
//-------------起動時に必ず送信---------------
@@ -136,7 +135,7 @@
sendCtrlSD(node2);
sendCtrlSD(node3);
sendCtrlSD(node4);
-
+
sendCtrlEN(node1);
sendCtrlEN(node2);
sendCtrlEN(node3);
@@ -155,7 +154,7 @@
sendProDec(2,Dec);
sendProDec(3,Dec);
sendProDec(4,Dec);
-
+
//トルク設定
int trq = 100; //torque Setting[mA]
@@ -163,776 +162,273 @@
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);
+
+ 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
}
-
- 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;//起動時の角度を保存
+ Init_Pos = ActPos;//起動時の角度を保存
t.reset();
t.start();
-
+
while(1){
- Time = t.read();
+ 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
+ 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){
+ }
+ printf("dsit1 %d\r\n",dist1);
+ /*--------------------------*/
+ //前方向に入力があった時
+ if(ActPos < Init_Pos - KICK){
- 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);
+ set_ACC(ACC_B);
+ set_DEC(ACC_C);
+
+ //速度制御モード送信
+ set_MODE_V();
- while(1){
+ while(1){
- u1.start();
- u2.start();
- dist1 = u1.get_dist_cm();
- dist2 = u2.get_dist_cm();
+ 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);
- }
+ //速度を指定
+ 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);
- }
+ 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);
+ //速度を指定
+ 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");
+ }
- //printf("2\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);
+ }
- 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");
- }
+ wait(0.5);
+
+ set_MODE_T();
- if(ActPos > -10000){
- break;
- }
+ dist1 = 0;
+
+ wait(3.0);
+ 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);
+ if(ActPos > Init_Pos + 2000){
+ set_ACC(ACC_B);
+ set_DEC(ACC_C);
+
+ //速度制御モード送信
+ set_MODE_V();
- //速度制御モード送信
- 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,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,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);
+ //速度を指定
+ 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);
+ set_MODE_T();
- wait(4.0);
- break;
- }
- /*--------------------------*/
-
- /*--------------------------*/
- //一定時間入力がない場合
- if(Time > Standby_Time){
- sendProAcc(1,5000);
- sendProAcc(2,5000);
- sendProAcc(3,5000);
- sendProAcc(4,5000);
+ 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);
+ sendProDec(1,5000);
+ sendProDec(2,5000);
+ sendProDec(3,5000);
+ sendProDec(4,5000);
+
+ //速度制御モード送信
+ set_MODE_V();
+
+ //速度を指定
+ 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);
- //速度制御モード送信
- 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;
- }
- /*--------------------------*/
-
+ //ブレーキ
+ sendTgtVel(1,0);
+ sendTgtVel(2,0);
+ sendTgtVel(3,0);
+ sendTgtVel(4,0);
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+
+ wait(0.5);
+
+ set_MODE_T();
+
+ 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);
- }
-
- }
- */
- //-------------------------------------------
-
+}
+
+void set_ACC(int setACC_val){
+ sendProAcc(1,setACC_val);
+ sendProAcc(2,setACC_val);
+ sendProAcc(3,setACC_val);
+ sendProAcc(4,setACC_val);
}
+void set_DEC(int setDEC_val){
+ sendProDec(1,setDEC_val);
+ sendProDec(2,setDEC_val);
+ sendProDec(3,setDEC_val);
+ sendProDec(4,setDEC_val);
+}
+void set_MODE_V(){
+ sendOPModeV(1);
+ sendOPModeV(2);
+ sendOPModeV(3);
+ sendOPModeV(4);
+}
+void set_MODE_T(){
+ sendOPModeT(1);
+ sendOPModeT(2);
+ sendOPModeT(3);
+ sendOPModeT(4);
+}
//0x2F-6060-00-fd-//-//-//
void sendOPModeT(int nodeID){
@@ -952,8 +448,8 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
-
+
+
//0x2F-6060-00-03-//-//-//
void sendOPModeV(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -972,7 +468,7 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
+
//0x2B-6040-00-0000-//-//
void sendCtrlRS(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -991,8 +487,8 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
-
+
+
//0x2B-6040-00-0006-//-//
void sendCtrlSD(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -1011,7 +507,7 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
+
//0x2B-6040-00-000F-//-//
void sendCtrlEN(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -1030,7 +526,7 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
+
//0x2B-6040-00-000B-//-//
void sendCtrlQS(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -1049,7 +545,7 @@
canPort.write(canmsgTx);//CANでデータ送信
wait_ms(1);
}
-
+
//0x2B-6040-00-010F-//-//
void sendCtrlHL(int nodeID){
canmsgTx.id = 0x600+nodeID;
@@ -1068,7 +564,7 @@
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); //回転数送信データの表示
@@ -1097,10 +593,10 @@
//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); //回転数送信データの表示
@@ -1126,7 +622,7 @@
wait_ms(1);
}
-
+
void readActVel(int nodeID){
//値が欲しいobjectのアドレスを送る
canmsgTx.id = 0x600+nodeID;
@@ -1138,7 +634,7 @@
canPort.write(canmsgTx);
wait_ms(1);
}
-
+
void readActPos(int nodeID){
//値が欲しいobjectのアドレスを送る
canmsgTx.id = 0x600+nodeID;
@@ -1150,7 +646,7 @@
canPort.write(canmsgTx);
wait_ms(1);
}
-
+
//加速度指定
void sendProAcc(int nodeID,int rpm){
if(rpm < 0){
@@ -1176,7 +672,7 @@
sendCtrlEN(nodeID);
wait(0.01);
}
-
+
//減速度指定
void sendProDec(int nodeID,int rpm){
if(rpm < 0){
@@ -1202,8 +698,8 @@
sendCtrlEN(nodeID);
wait(0.01);
}
-
-
+
+
//送信データの表示
void printCANTX(void){
//0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
@@ -1213,9 +709,9 @@
}
//pc.printf("\r\n");
}
-
+
//受信データの表示
-
+
void printCANRX(void){
//0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
//pc.printf("0x%3x|",canmsgRx.id);
@@ -1224,13 +720,15 @@
}
pc.printf("\r\n");
}
-
+
void CANdataRX(void){
canPort.read(canmsgRx);
printCANRX();
}
-
+
void SerialRX(void){
Serialdata = pc.getc();
//pc.printf("%c\r\n",Serialdata);
}
+
+
\ No newline at end of file