Tomohiro Aoki
/
YUKA_ak_yn
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 10:49d153e666c7, committed 2021-12-20
- Comitter:
- Tomo1213
- Date:
- Mon Dec 20 11:05:13 2021 +0000
- Parent:
- 9:51325cc6496a
- Commit message:
- a
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Dec 20 05:53:58 2021 +0000
+++ b/main.cpp Mon Dec 20 11:05:13 2021 +0000
@@ -11,15 +11,19 @@
#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
DigitalOut led1(LED1);
DigitalOut led2(LED2);
@@ -28,8 +32,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);
@@ -109,9 +114,6 @@
int node3 = 3;
int node4 = 4;
- //モータ回転数
- int rpm = 400;
-
//エンコーダ関係
int ActPos = 0;
int Init_Pos = 0;
@@ -172,9 +174,13 @@
sendTgtTrq(4,trq);
int state_1 = 0;
+ int state_2 = 0;
+ int ride_count = 0;
- ActPos = 0;
- Init_Pos = 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;
@@ -192,7 +198,8 @@
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){
@@ -203,26 +210,32 @@
dist2 = get_cm_n(u2, 5);
dist3 = get_cm_n(u3, 5);
- printf("%d\r\n",state_1);
/*--------------------------*/
//
if(state_1 == 0){//入力判断フェーズ
- if(ActPos < (Init_Pos - KICK)){
- state_1 = 1;
- }else if(ActPos > (Init_Pos + KICK)){
- state_1 = 11;
- }else{
- set_MODE_T();
+ 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();
+ }
}
}else if(state_1 == 1){//前進→壁検出フェーズ
- if(dist1 < WALL && dist1 >= 25){
+ if(dist1 < WALL && dist1 >= WALL_MIN){
state_1 = 2;
}else{
- set_ACC(ACC_B);//加速度設定
- set_DEC(ACC_C);//減速度設定
+ set_ACC(ACC_RIDE);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
set_MODE_V();//速度制御モード送信
- vel_forward_con(rpm);//前進速度指令
+ vel_forward_con(RPM_RIDE);//前進速度指令
}
}else if(state_1 == 2){//前進からの帰還フェーズ
if(ActPos > -3000){
@@ -231,40 +244,94 @@
wait(1.0);
//break;
}else{
- vel_backward_con(rpm);
+ vel_backward(RPM_RIDE);
}
}else if(state_1 == 11){//右進→壁検出フェーズ
- if(dist3 < WALL && dist3 >= 25){
+ if(dist3 < WALL && dist3 >= WALL_MIN){
state_1 = 12;
}else{
- set_ACC(ACC_B);//加速度設定
- set_DEC(ACC_C);//減速度設定
+ set_ACC(ACC_RIDE);//加速度設定
+ set_DEC(DEC_RIDE);//減速度設定
set_MODE_V();//速度制御モード送信
- vel_right_con(rpm);//R進速度指令
+ 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);
//break;
}else{
- vel_left(rpm);
+ vel_left(RPM_RIDE);
}
- }else if(state_1 == 41){
- //Zig Zag
- if(dist1 < WALL && dist1 >= 25){
- state_1 = 2;
- }else{
- set_ACC(ACC_B);//加速度設定
- set_DEC(ACC_C);//減速度設定
- set_MODE_V();//速度制御モード送信
- vel_forward_con(rpm);//前進速度指令
- }
- }
-
-}
-}
+ }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);//右進速度指令
+ }
+ }
+ }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);//右進速度指令
+ }
+ }
+ }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);
+ }
+ }
+ }
+ }
+}
void vel_right(int rpm){
sendTgtVel(1,rpm);
@@ -677,9 +744,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");
}
@@ -692,7 +759,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){