Ryu Kaz
t6est
Diff: backup.md
- Revision:
- 4:9ba47e5db1e2
- Parent:
- 3:8b22783f6bf1
--- a/backup.md Thu Sep 26 02:23:27 2019 +0000
+++ b/backup.md Sat Oct 05 01:41:37 2019 +0000
@@ -41,13 +41,14 @@
Timer go_up_tm;
///////////定数たち//////////////
const float Min_ang = 0.1f;
-const float Max_pwm = 0.2f;
+const float Max_pwm = 0.15f;
const float Max_pwm_roll = 0.3f;
const float Min_pwm = 0.02f;
-const float Gein = 0.05f;//0.05f
+const float Gein = 0.04f;//0.05f
const int R = 103;//これ直径やんけ
const int Resolution = 1024;
const float RoriGein = 0.001f;
+const int mae_dis = 250;
///////////////////////
int point_count = 0;
int array_count = 0;
@@ -91,6 +92,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 +117,22 @@
//////////////////////////////////////////////
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);
+///////////////////////////////////////////
+float only_this_time = 0;
int main() {
ins.mode(PullDown);
lim_right.mode(PullDown);
@@ -136,6 +145,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");
@@ -196,7 +206,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){
@@ -219,6 +229,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,17 +238,26 @@
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{
@@ -280,40 +300,10 @@
if(points[point_count][2] != 0){
bool issheets = (points[point_count][2] == 1)?1:0;
if(issheets){
- ///////////////////////////////////////////////////////////////////////////////////////////
- if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
+ /////////////////////////////////////////////////////////////////////////////////////////
+ if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
moved_to_wall = 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;
- }else if(!l_r && l_l){
- serial.printf("%s\n","LROLL");
- Move2(LROLL,0.1f);
- }else if(l_r && !l_l){
- Move2(RROLL,0.1f);
- serial.printf("%s\n","RROLL");
- }else{
- Move2(UP,0.1f);
- serial.printf("%s\n","UP");
- }
- }else{
- if(!l_r && !l_l){
- moved_to_wall = false;
- //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
- }else if(l_r || l_l){
- serial.printf("%d",l_l);
- serial.printf("%s",":");
- serial.printf("%d\n",l_r);
- Move2(points[point_count][0],0.1f);
- }
- }
- }
-
/////////////////////////////////////////////////////////////////////////////////////////
if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
@@ -323,40 +313,11 @@
Move2(0,0);
}
}else{
-
- if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
+ /////////////////////////////////////////////////////////////////////////////////////////
+ if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
moved_to_wall = 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;
- }else if(!l_r && l_l){
- serial.printf("%s\n","LROLL");
- Move2(LROLL,0.1f);
- }else if(l_r && !l_l){
- Move2(RROLL,0.1f);
- serial.printf("%s\n","RROLL");
- }else{
- Move2(UP,0.1f);
- serial.printf("%s\n","UP");
- }
- }else{
- if(!l_r && !l_l){
- serial.printf("%s\n","C");
- moved_to_wall = false;
- //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
- }else if(l_r || l_l){
- serial.printf("%d",l_l);
- serial.printf("%s",":");
- serial.printf("%d\n",l_r);
- Move2(points[point_count][0],0.1f);
- }
- }
- }
+ /////////////////////////////////////////////////////////////////////////////////////////
if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
array_count++;
@@ -364,28 +325,73 @@
activeMove = false;
Move2(0,0);
}
+ }
+ 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;
+ Move2(10,0);
+ }else if(!l_r && l_l){
+ serial.printf("%s\n","LSRIDE");
+ Move2(RIGHT,0.1f);
+ }else if(l_r && !l_l){
+ Move2(LEFT,0.1f);
+ serial.printf("%s\n","RSRIDE");
+ }else{
+ Move2(UP,0.1f);
+ serial.printf("%s\n","UP");
+ }
+ }else{
+ if(!l_r && !l_l){
+ moved_to_wall = false;
+ hensu_A = true;
+ wait(0.5f);
+ }else if(l_r || l_l){
+ serial.printf("%d",l_l);
+ serial.printf("%s",":");
+ serial.printf("%d\n",l_r);
+ Move2(points[point_count][0],0.1f);
+ }
+ }
}
+ if(hensu_A){
+ gav_out = 1;
+ while(!gav_in.read()){
+ serial.printf("%d\n",gav_in.read());
+ }
+ //wall_C = -1400-from_mid+points[point_count][1]-pointx+y_dis;
+ points[point_count][1] = 1400 + distance;
+ gav_out = 0;
+ hensu_A = 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(150-dis_go_up)*RoriGein > Min_pwm){
- if(dis_go_up < 150){
- Move2(0,(150-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-150)*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);
+ Move2(10,0);
}
}else{
serial.printf("%s\n","owari");
@@ -418,8 +424,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();
}