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.
Diff: main.cpp
- Revision:
- 10:49d153e666c7
- Parent:
- 9:51325cc6496a
--- 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){