Kanta Takasu
/
YUKA_tks
yuka_tks
Diff: main.cpp
- Revision:
- 4:1bd08c9d92a9
- Parent:
- 3:c39c14cfc811
- Child:
- 5:f21e1a75f98b
diff -r c39c14cfc811 -r 1bd08c9d92a9 main.cpp --- 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