Kanta Takasu
gohome ok
Diff: main.cpp
- Revision:
- 4:1bd08c9d92a9
- Parent:
- 3:c39c14cfc811
- Child:
- 5:f21e1a75f98b
--- a/main.cpp Sun Dec 19 11:16:07 2021 +0000
+++ b/main.cpp Mon Dec 20 14:10:11 2021 +0000
@@ -1,4 +1,4 @@
-//2021/12/18更新
+//2021/12/20更新
//YUKA本番機用プログラム
//入力切替確認済み
//一定時間の入力なしで動作切替
@@ -11,14 +11,20 @@
#include "mbed.h"
#include "hcsr04.h"
-#define ACC_B 1000
-#define DEC_B 700
-#define ACC_C 1000
-#define DEC_C 1000
+#define RPM_RIDE 400
+#define RPM_CLEAN 400
+#define ACC_RIDE 1000
+#define DEC_RIDE 700
+#define ACC_CLEAN 1000
+#define DEC_CLEAN 1000
#define KICK 2000
+#define CLEAN_OFFSET 6000
-
+#define WALL 45
+#define WALL_MIN 25
+#define Standby_Time 10
+#define GETOFF_TIME 3
DigitalOut led1(LED1);
DigitalOut led2(LED2);
@@ -27,8 +33,9 @@
Timer t;
double Time = 0;
-#define Standby_Time 10
+
Serial pc(USBTX, USBRX);
+Serial LED(PB_6,PB_7);
char Serialdata;
BusOut myled(LED1, LED2, LED3, LED4);
@@ -72,6 +79,27 @@
void set_MODE_V(void);
void set_MODE_T(void);
+void vel_stop(void);
+
+void vel_forward(int);
+void vel_forward_con(int);
+void vel_backward(int);
+void vel_backward_con(int);
+void vel_right(int);
+void vel_right_con(int);
+void vel_left(int);
+
+//unsigned int get_cm_n(HCSR04, unsigned int);
+//USE -> unsigned int dist_UnitA = get_cm_n(u2, 5);
+unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){
+ unsigned int sampled_dist=0;
+ for (int iter_n = 0; iter_n <echo_n; iter_n++){
+ echo_unit.start();
+ sampled_dist += echo_unit.get_dist_cm();
+ }
+ return (sampled_dist / echo_n);
+}
+
int nodeall=4;
int main(){
@@ -87,34 +115,17 @@
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 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,243 +174,277 @@
sendTgtTrq(3,trq);
sendTgtTrq(4,trq);
- while(1){
-
- ActPos = 0;
- Init_Pos = 0;
+ int state_1 = 0;
+ int state_2 = 0;
+ int ride_count = 0;
+
+ int X_POS_TMP = 0;
+ int dist1_ori = 0;
+ int dist2_ori = 0;
+
+ 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
+ }
+ Init_Pos = ActPos;//起動時の角度を保存
+ t.reset();
+ t.start();
+
+ //set_MODE_T();
+
+ printf("\nstart\r\n");
+
+ while(1){
+
+ Time = t.read();
+ pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2);
+ LED.printf("%d",state_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");
}
- Init_Pos = ActPos;//起動時の角度を保存
- t.reset();
- t.start();
-
- while(1){
-
- 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
- //printf("check\r\n");
+ dist1 = get_cm_n(u1, 5);
+ dist2 = get_cm_n(u2, 5);
+ dist3 = get_cm_n(u3, 5);
+
+ /*--------------------------*/
+ //
+ if(state_1 == 0){//入力判断フェーズ
+ state_2 = 0;
+ if(ride_count >= 2 && Time > Standby_Time){
+ state_1 = 20;
+ }else{
+ if(ActPos < (Init_Pos - KICK)){ //前入力検出
+ ride_count++;
+ state_1 = 1;
+ }else if(ActPos > (Init_Pos + KICK)){ //右入力検出
+ ride_count++;
+ state_1 = 11;
+ }else{
+ set_MODE_T();
}
- printf("dsit1 %d\r\n",dist1);
- /*--------------------------*/
- //前方向に入力があった時
- if(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);
+ }
+
+ }else if(state_1 == 1){//前進→壁検出フェーズ
+ if(dist1 < WALL && dist1 >= WALL_MIN){
+ vel_stop();
+ wait(GETOFF_TIME);
+ state_1 = 2;
+ }else{
+ set_ACC(ACC_RIDE);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_forward_con(RPM_RIDE);//前進速度指令
+ }
+ }else if(state_1 == 2){//前進からの帰還フェーズ
+ if(ActPos > -3000){
+ vel_stop();
+ t.reset();
+ t.start();
+ state_1 = 0;
+ wait(1.0);
+ }else{
+ vel_backward_con(RPM_RIDE);
+ }
+ }else if(state_1 == 11){//右進→壁検出フェーズ
+ if(dist3 < WALL && dist3 >= WALL_MIN){
+ vel_stop();
+ wait(GETOFF_TIME);
+ state_1 = 12;
+ }else{
+ set_ACC(ACC_RIDE);//加速度設定
+ set_DEC(DEC_RIDE);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_right(RPM_RIDE);//R進速度指令
+ }
+ }else if(state_1 == 12){//右進からの帰還フェーズ
+ if(ActPos < 3000){
+ vel_stop();
+ t.reset();
+ t.start();
+ state_1 = 0;
+ wait(1.0);
+ }else{
+ vel_left(RPM_RIDE);
+ }
+ }else if(state_1 == 20){//消毒モード
+ if(state_2 == 0){
+ if(dist1 < WALL && dist1 >= WALL_MIN){
+ X_POS_TMP = ActPos;
+ state_2 = 1;
+ }else{
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_forward_con(RPM_CLEAN);//前進速度指令
+ }
+ }else if(state_2 == 1){
+ if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET){
+ state_2 == 2;
+ }else{
+ if(dist3 < WALL && dist3 >= WALL_MIN){
+ state_2 = 4;
+ }else{
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_right(RPM_CLEAN);//右進速度指令
}
-
- //速度を指定
- sendTgtVel(1,0);
- sendTgtVel(2,0);
- sendTgtVel(3,0);
- sendTgtVel(4,0);
- //指令値を送信
- for(int i=1;i<= 4;i++){
- sendCtrlEN(i);
+ }
+ }else if(state_2 == 2){
+ if(ActPos > -3000){
+ state_2 = 3;
+ }else{
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_backward_con(RPM_CLEAN);//後進速度指令
+ }
+ }else if(state_2 == 3){
+ if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET){
+ state_2 == 0;
+ }else{
+ if(dist3 < WALL && dist3 >= WALL_MIN){
+ state_2 = 4;
+ }else{
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_right(RPM_CLEAN);//右進速度指令
}
- //実行時間
- 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;
-
- wait(3.0);
- break;
+ }
+ }else if(state_2 == 4){
+ if(ActPos < 3000){
+ state_2 = 5;
+ }else{
+ vel_left(RPM_CLEAN);
+ }
+ }else if(state_2 == 5){
+ if(ActPos > -3000){
+ t.reset();
+ t.start();
+ state_1 = 0;
+ state_2 = 0;
+ }else{
+ vel_backward_con(RPM_CLEAN);
}
+ }
+ }
+ }
+}
- /*--------------------------*/
-
- /*--------------------------*/
- //右方向に入力があった時
- if(ActPos > Init_Pos + 2000){
- set_ACC(ACC_B);
- set_DEC(ACC_C);
-
- //速度制御モード送信
- set_MODE_V();
-
- //速度を指定
- 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,rpm*(-1));
- sendTgtVel(2,rpm);
- sendTgtVel(3,rpm);
- sendTgtVel(4,rpm*(-1));
- //指令値を送信
- for(int i=1;i<= 4;i++){
- sendCtrlEN(i);
- }
- //実行時間
- wait(2.0);
+void vel_right(int rpm){
+ sendTgtVel(1,rpm);
+ sendTgtVel(2,rpm*(-1));
+ sendTgtVel(3,rpm*(-1));
+ sendTgtVel(4,rpm);
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
+void vel_right_con(int rpmA){
+ int dis1 = get_cm_n(u1,15);
+ int dis2 = get_cm_n(u2,15);
+
+ //速度を指定
+ int robot_angle = ((dis1 - dis2)*5);
+ sendTgtVel(1,rpmA+robot_angle);
+ sendTgtVel(2,rpmA*(-1)+robot_angle);
+ sendTgtVel(3,rpmA*(-1)+robot_angle);
+ sendTgtVel(4,rpmA+robot_angle);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
- //ブレーキ
- 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(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);
-
- //速度制御モード送信
- set_MODE_V();
+
+void vel_left(int rpm){
+ sendTgtVel(1,rpm*(-1));
+ sendTgtVel(2,rpm);
+ sendTgtVel(3,rpm);
+ sendTgtVel(4,rpm*(-1));
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
+
+
+void vel_left_con(int rpmA){
+ int dis1 = get_cm_n(u1,15);
+ int dis2 = get_cm_n(u2,15);
+
+ //速度を指定
+ int robot_angle = ((dis1 - dis2)*5);
+ sendTgtVel(1,rpmA*(-1)+robot_angle);
+ sendTgtVel(2,rpmA+robot_angle);
+ sendTgtVel(3,rpmA+robot_angle);
+ sendTgtVel(4,rpmA*(-1)+robot_angle);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
+
+void vel_stop(){
+ //速度を指定
+ sendTgtVel(1,0);
+ sendTgtVel(2,0);
+ sendTgtVel(3,0);
+ sendTgtVel(4,0);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
- //速度を指定
- 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);
+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_backward_con(int rpmA){
+ int dis1 = get_cm_n(u1,15);
+ int dis2 = get_cm_n(u2,15);
- //ブレーキ
- sendTgtVel(1,0);
- sendTgtVel(2,0);
- sendTgtVel(3,0);
- sendTgtVel(4,0);
- for(int i=1;i<= 4;i++){
- sendCtrlEN(i);
- }
+ //速度を指定
+ int robot_angle = ((dis1 - dis2)*10);
+ sendTgtVel(1,rpmA+robot_angle);
+ sendTgtVel(2,rpmA+robot_angle);
+ sendTgtVel(3,rpmA*(-1)+robot_angle);
+ sendTgtVel(4,rpmA*(-1)+robot_angle);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
+}
+
+
+void vel_forward_con(int rpmA){
+ int dis1 = get_cm_n(u1,15);
+ int dis2 = get_cm_n(u2,15);
- wait(0.5);
-
- set_MODE_T();
-
- wait(5.0);
- break;
- }
- }
- wait(0.1);
- }
+ //速度を指定
+ int robot_angle = ((dis1 - dis2)*10);
+ sendTgtVel(1,rpmA*(-1)+robot_angle);
+ sendTgtVel(2,rpmA*(-1)+robot_angle);
+ sendTgtVel(3,rpmA+robot_angle);
+ sendTgtVel(4,rpmA+robot_angle);
+ //指令値を送信
+ for(int i=1;i<= 4;i++){
+ sendCtrlEN(i);
+ }
}
void set_ACC(int setACC_val){
@@ -703,9 +748,9 @@
//送信データの表示
void printCANTX(void){
//0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
- pc.printf("0x%3x|",canmsgTx.id);
+ //pc.printf("0x%3x|",canmsgTx.id);
for(char i=0;i < canmsgTx.len;i++){
- pc.printf("%02x|",canmsgTx.data[i]);
+ //pc.printf("%02x|",canmsgTx.data[i]);
}
//pc.printf("\r\n");
}
@@ -718,7 +763,7 @@
for(char i=0;i < canmsgRx.len;i++){
//pc.printf("%02x|",canmsgRx.data[i]);
}
- pc.printf("\r\n");
+ //pc.printf("\r\n");
}
void CANdataRX(void){
@@ -729,6 +774,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