Kanta Takasu
add pcon f and b
Diff: main.cpp
- Revision:
- 5:a7c3f446a1f1
- Parent:
- 4:e1aa0b348aeb
- Child:
- 6:07534de7cdb4
--- a/main.cpp Sun Dec 19 15:43:05 2021 +0000
+++ b/main.cpp Sun Dec 19 17:12:27 2021 +0000
@@ -1,4 +1,4 @@
-//2021/12/18更新
+//2021/12/20更新
//YUKA本番機用プログラム
//入力切替確認済み
//一定時間の入力なしで動作切替
@@ -72,6 +72,11 @@
void set_MODE_V(void);
void set_MODE_T(void);
+void vel_stop(void);
+void vel_forward(int);
+void vel_backward(int);
+
+
int nodeall=4;
int main(){
@@ -90,31 +95,17 @@
//モータ回転数
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 ActPos = 0;
int Init_Pos = 0;
//超音波センサ関係パラメータ
- int max_rpm = rpm + 200;
- int min_rpm = rpm -200;
- int wall_distance = 0;
- int dist1, dist2,dist3, dist4;
-
+ int dist1,dist2,dist3,dist4;
//PID制御関係
//角度調整パラメータ
- int p1, i1, d1;
- int error_before1 = 0;
- int error_now1 = 0;
- int feedback_val1;
- int integral1;
- int pid_sum1;
+
+
#define DELTA_T1 0.1
#define target_val1 0
#define Kp1 3
@@ -163,6 +154,8 @@
sendTgtTrq(3,trq);
sendTgtTrq(4,trq);
+ int state_1 = 0;
+
while(1){
ActPos = 0;
@@ -174,126 +167,93 @@
ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
}
Init_Pos = ActPos;//起動時の角度を保存
- t.reset();
- t.start();
+ t.reset();
+ t.start();
+
+ set_MODE_T();
- while(1){
-
- Time = t.read();
+ 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");
- }
- u1.start();
- u2.start();
-
- dist1 = u1.get_dist_cm();
- dist2 = u2.get_dist_cm();
-
- printf("dsit1 %d\r\n",dist1);
- /*--------------------------*/
- //前方向に入力があった時
- if(0){//ActPos < Init_Pos - KICK){
-
- set_ACC(ACC_B);
- set_DEC(ACC_C);
-
- //速度制御モード送信
- set_MODE_V();
-
- while(1){
-
- 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);
- }
-
- if(dist1 < 100 && dist1 >= 70){
- break;
- }
- printf("dsit1 %d\r\n",dist1);
- }
+ Time = t.read();
- //速度を指定
- 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");
- }
-
- 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);
- }
-
- wait(0.5);
-
- set_MODE_T();
-
- dist1 = 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
+ //printf("check\r\n");
+ }
+ u1.start();
+ u2.start();
+
+ dist1 = u1.get_dist_cm();
+ dist2 = u2.get_dist_cm();
+
+ //printf("dsit1 %d\r\n",dist1);
+ printf("%d\r\n",state_1);
+ /*--------------------------*/
+ //前方向に入力があった時
+ if(state_1 == 0){//入力判断フェーズ
+ if(ActPos < Init_Pos - KICK){
+ state_1 = 1;
+ }else{
+
+ }
+ }else if(state_1 == 1){//前進→壁検出フェーズ
+ if(dist1 < 50 && dist1 >= 30){
+ state_1 = 2;
+ }else{
+ set_ACC(ACC_B);//加速度設定
+ set_DEC(ACC_C);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_forward(rpm);//前進速度指令
+ }
+ }else if(state_1 == 2){//帰還フェーズ
+ if(ActPos > -10000){
+ vel_stop();
wait(3.0);
break;
+ }else{
+ vel_backward(rpm);
}
+ }
+ }
+ }
+}
+
+void vel_stop(){
+ //速度を指定
+ sendTgtVel(1,0);
+ sendTgtVel(2,0);
+ sendTgtVel(3,0);
+ sendTgtVel(4,0);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
- /*--------------------------*/
-
- /*--------------------------*/
- //右方向に入力があった時
-
- }
- wait(0.5);
- }
+void vel_backward(int rpm){
+ //速度を指定
+ sendTgtVel(1,rpm);
+ sendTgtVel(2,rpm);
+ sendTgtVel(3,rpm*(-1));
+ sendTgtVel(4,rpm*(-1));
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
+
+void vel_forward(int rpm){
+ //速度を指定
+ sendTgtVel(1,rpm*(-1));
+ sendTgtVel(2,rpm*(-1));
+ sendTgtVel(3,rpm);
+ sendTgtVel(4,rpm);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
}
void set_ACC(int setACC_val){
@@ -623,6 +583,4 @@
void SerialRX(void){
Serialdata = pc.getc();
//pc.printf("%c\r\n",Serialdata);
-}
-
-
\ No newline at end of file
+}
\ No newline at end of file