Ryu Kaz
t6est
Diff: main.cpp
- Revision:
- 4:9ba47e5db1e2
- Parent:
- 3:8b22783f6bf1
--- a/main.cpp Thu Sep 26 02:23:27 2019 +0000
+++ b/main.cpp Sat Oct 05 01:41:37 2019 +0000
@@ -43,11 +43,13 @@
const float Min_ang = 0.1f;
const float Max_pwm = 0.15f;
const float Max_pwm_roll = 0.3f;
+const float Min_pwm_roll = 0.015f;
const float Min_pwm = 0.02f;
-const float Gein = 0.05f;//0.05f
+const float Gein = 0.06f;//0.05f
const int R = 103;//これ直径やんけ
const int Resolution = 1024;
const float RoriGein = 0.001f;
+const int mae_dis = 300;
///////////////////////
int point_count = 0;
int array_count = 0;
@@ -91,6 +93,8 @@
float x_dis,y_dis;
bool flag_finish_move = false;
+float mae_pwm = 1;
+
void setMove(int p0,int p1);
void get_rorivol();
@@ -114,16 +118,32 @@
//////////////////////////////////////////////
bool moved_to_wall = false;
bool start_wall_time = false;
-int from_end = 370;
+int from_mid = 0;
int wall_C = 0;
DigitalIn lim_right(PG_10);
DigitalIn lim_left(PG_15);
int l_r = 0;
int l_l = 0;
+
+
+bool hensu_A = false;
/////////////////////////////////////////////
DigitalOut outColor(PG_4);
DigitalOut outSorT(PG_7);
////////////////////////////////////////////
+DigitalOut gav_out(PD_10);
+DigitalIn gav_in(PG_14);
+///////////////////////////////////////////
+DigitalOut TdistnaceOut(PC_6);
+DigitalOut FdistanceOut(PB_15);
+float subdistance = 0;
+float only_this_time = 0;
+float gav_high = 0;
+bool dis_able_to_use_g = false;
+bool gav1000 = true;
+PwmOut sheetwo_out(PF_6);
+bool sheetwo_flag = false;
+float last_dis = 0;
int main() {
ins.mode(PullDown);
lim_right.mode(PullDown);
@@ -136,6 +156,7 @@
outColor = getColor();
outSorT = getPeriwhich();
+ gav_in.mode(PullDown);
//if(read_keter())keter=true;
setMove(points[point_count][0],points[point_count][1]);
serial.printf("%s\n","mpu_setup");
@@ -162,9 +183,9 @@
/////////////傾き修正////////////////////////////
if(!keter){
- pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
+ pwm = angle_adjust(Gein,all,Min_ang,Min_pwm_roll,Max_pwm_roll);
if(!activeMove){
- if(pwm <= Min_pwm){
+ if(pwm <= Min_pwm_roll){
activeMove = true;
}
@@ -183,7 +204,7 @@
if(!activeMove)activeMove = true;
}
/////////ロリコン値取得///////
- if(activeMove){
+ if(activeMove && !dis_able_to_use_g){
get_rorivol();
}
@@ -196,7 +217,7 @@
float pwm = 0;
float distance;
if(points[point_count][0] == UP or points[point_count][0] == BACK){
- distance = pointy-x_dis + wall_C;
+ distance = pointy-x_dis;
last_move_vertical = true;
last_move_side = false;
}else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
@@ -204,14 +225,26 @@
last_move_vertical = false;
last_move_side = true;
}
+ subdistance = distance;
//else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
if(x_flag or y_flag)distance *= -1;
pwm = distance*RoriGein;
pwm = (pwm < 0)?-pwm:pwm;//abs
+ if(last_dis - distance > 0){
+ TdistnaceOut = 1;
+ FdistanceOut = 0;
+ }else if(last_dis - distance < 0){
+ TdistnaceOut = 0;
+ FdistanceOut = 1;
+ }else{
+ TdistnaceOut = 1;
+ FdistanceOut = 1;
+ }
if(pwm < Min_pwm){
//次の処理へ
serial.printf("%d\n",move_tm.read_ms());
if(move_tm.read_ms() >= 600){
+ sheetwo_flag = false;
serial.printf("%s","waiting");
serial.printf("%d\n",point_count);
//////////////////////////////
@@ -219,6 +252,7 @@
float set0_dis;
float pwm_while;
bool whileactive = true;
+ only_this_time = rorix;
move_tm.reset();
while(move_tm.read_ms() < 600){
if(whileactive){
@@ -227,21 +261,30 @@
set0_dis = roriy-off_roriy;
pwm = set0_dis*RoriGein;
if(abs(pwm) >= Min_pwm)move_tm.reset();
+ if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから
if(pwm < 0){
Move2(RIGHT,-pwm);
}else{
Move2(LEFT,pwm);
}
}else if(last_move_side){
- serial.printf("%s\n","side");
- set0_dis = rorix-off_rorix;
- serial.printf("%f\n",set0_dis);
- pwm = set0_dis*RoriGein;
+ if(points[point_count][2] == 0){
+ serial.printf("%s\n","side");
+ set0_dis = rorix-off_rorix;
+ serial.printf("%f\n",set0_dis);
+ pwm = set0_dis*RoriGein;
+ }else{
+ serial.printf("%s\n","only_this_side");
+ set0_dis = abs(rorix - only_this_time);
+ serial.printf("%f\n",mae_dis-set0_dis);
+ pwm = (mae_dis-set0_dis)*RoriGein;
+ }
if(abs(pwm) >= Min_pwm)move_tm.reset();
+ if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから
if(pwm < 0){
Move2(UP,-pwm);
}else{
- Move2(BACK,pwm);
+ Move2(BACK,pwm);
}
}
}else{
@@ -281,9 +324,6 @@
bool issheets = (points[point_count][2] == 1)?1:0;
if(issheets){
/////////////////////////////////////////////////////////////////////////////////////////
- if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){
- moved_to_wall = true;
- }
/////////////////////////////////////////////////////////////////////////////////////////
if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
@@ -294,9 +334,6 @@
}
}else{
/////////////////////////////////////////////////////////////////////////////////////////
- if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){
- moved_to_wall = true;
- }
/////////////////////////////////////////////////////////////////////////////////////////
if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
@@ -306,19 +343,43 @@
Move2(0,0);
}
}
+/*
+ if(points[point_count][1]-distance < 1000){
+ if(gav_high == 0)gav_out = 1;
+ if(gav_in.read()){
+ gav_high = points[point_count][1]-distance;//移動距離
+ gav_out = 0;
+ }
+ }
+*//*
+ if(gav_high == 0){
+ gav_out = 1;
+ if(gav_in.read()){
+ gav_high = points[point_count][1]-distance+175;
+ gav_out = 0;
+ serial.printf("%f\n",gav_high);
+ }
+ }
+ */
+ if(!moved_to_wall && points[point_count][1]-distance > 1030 && !start_wall_time){
+ activeMove = false;
+ moved_to_wall = true;
+ dis_able_to_use_g = true;
+ }
+
if(moved_to_wall){
l_r = lim_right.read();
l_l = lim_left.read();
if(!start_wall_time){
if(l_r && l_l){
start_wall_time = true;
- all = 0;
+ //Move2(10,0);
}else if(!l_r && l_l){
- serial.printf("%s\n","LROLL");
- Move2(LROLL,0.1f);
+ serial.printf("%s\n","LSRIDE");
+ Move2(RIGHT,0.1f);
}else if(l_r && !l_l){
- Move2(RROLL,0.1f);
- serial.printf("%s\n","RROLL");
+ Move2(LEFT,0.1f);
+ serial.printf("%s\n","RSRIDE");
}else{
Move2(UP,0.1f);
serial.printf("%s\n","UP");
@@ -326,7 +387,10 @@
}else{
if(!l_r && !l_l){
moved_to_wall = false;
- //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
+ hensu_A = true;
+ wait(0.5f);
+ dis_able_to_use_g = false;
+ Move2(points[point_count][0],0.1f);
}else if(l_r || l_l){
serial.printf("%d",l_l);
serial.printf("%s",":");
@@ -335,28 +399,43 @@
}
}
}
-
+ if(hensu_A){
+ gav_out = 1;
+ while(!gav_in.read()){
+ serial.printf("%d\n",gav_in.read());
+ }
+ points[point_count][1] = 1400 + distance;
+ gav_out = 0;
+ hensu_A = false;
+ gav1000 = false;
+ }
if(point_move){
- serial.printf("%s\n","maemae");
+ serial.printf("%s","maemae");
if(C_vari == 0){
off_rorik = rorix;
C_vari = 1;
}
if(go_up){
- dis_go_up = abs(rorix-off_rorix);
- if(abs(200-dis_go_up)*RoriGein > Min_pwm){
- if(dis_go_up < 200){
- Move2(0,(200-dis_go_up)*RoriGein);//+-20mm
+ dis_go_up = abs(rorix-off_rorik);//進んだ距離
+ serial.printf("%f",mae_dis-dis_go_up);//残り
+ if(mae_pwm > Min_pwm){
+ if(dis_go_up < mae_dis){
+ mae_pwm = (mae_dis-dis_go_up)*RoriGein;
+ if(mae_pwm > Max_pwm)mae_pwm = 0.1f;
+ Move2(0,mae_pwm);//+-20mm
go_up_tm.reset();
}else{
- Move2(1,(dis_go_up-200)*RoriGein);
+ mae_pwm = (dis_go_up-mae_dis)*RoriGein;
+ if(mae_pwm > Max_pwm)mae_pwm = 0.1f;
+ Move2(1,mae_pwm);
go_up_tm.reset();
}
}
if(go_up_tm.read_ms() > 300){
go_up = false;
- Move2(0,0);
+ sheetwo_flag = true;
+ Move2(10,0);
}
}else{
serial.printf("%s\n","owari");
@@ -368,12 +447,18 @@
}
}
-
+ if(sheetwo_flag){
+ sheetwo_out = 0.3f;
+ }
if(!point_move && !moved_to_wall && !go_up){
- if(distance > 0)Move2(points[point_count][0],pwm);
- else{
- pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
- Move2(sewomukeru[points[point_count][0]],pwm);
+ if(points[point_count][2] != 0 && gav1000){
+ Move2(points[point_count][0],0.1f);
+ }else{
+ if(distance > 0)Move2(points[point_count][0],pwm);
+ else{
+ pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
+ Move2(sewomukeru[points[point_count][0]],pwm);
+ }
}
}
serial.printf("%f",pwm);
@@ -389,8 +474,10 @@
if(points[1][0] == RIGHT)serial.printf("%s","orange");
if(points[1][0] == LEFT)serial.printf("%s","blue");
serial.printf("%s",":");
- if(points[0][1] > 5100)serial.printf("%s\n","sheets");
- else serial.printf("%s\n","towel");
+ if(points[0][1] > 5100)serial.printf("%s","sheets");
+ else serial.printf("%s","towel");
+ serial.printf("%s",":");
+ serial.printf("%f\n",points[point_count][1]-distance);
}
move_tm.reset();
}
@@ -407,7 +494,7 @@
}
}
-
+ last_dis = subdistance;
}
}