Ryu Kaz
t6est
Diff: backup.md
- Revision:
- 3:8b22783f6bf1
- Parent:
- 1:bbf776e6c792
- Child:
- 4:9ba47e5db1e2
--- a/backup.md Wed Sep 25 07:47:37 2019 +0000
+++ b/backup.md Thu Sep 26 02:23:27 2019 +0000
@@ -62,13 +62,14 @@
int Robox = getRobox();
//int C = (1750-Robox);
int C = 1550;
+
/*
int sheets[] = {
C+100,C+1300,C+2500
};
*/
int sheets[] = {
- 900,1700,3500//下から:1750,2775,3575
+ 1975,2775,3575//下から:1750,2775,3575
};
//当たるのは1200mm
@@ -111,45 +112,51 @@
bool go_up = false;
float dis_go_up = 0;
//////////////////////////////////////////////
+bool moved_to_wall = false;
+bool start_wall_time = false;
+int from_end = 370;
+int wall_C = 0;
+DigitalIn lim_right(PG_10);
+DigitalIn lim_left(PG_15);
+int l_r = 0;
+int l_l = 0;
+/////////////////////////////////////////////
+DigitalOut outColor(PG_4);
+DigitalOut outSorT(PG_7);
+////////////////////////////////////////////
int main() {
ins.mode(PullDown);
-
+ lim_right.mode(PullDown);
+ lim_left.mode(PullDown);
setup_peri();
int whichnum = read_peri();
int points[whichnum][3];
changeColor(&points[0][0]);
-
+
+ outColor = getColor();
+ outSorT = getPeriwhich();
+
//if(read_keter())keter=true;
- //bt.putc('a');
setMove(points[point_count][0],points[point_count][1]);
serial.printf("%s\n","mpu_setup");
- //bt.putc('b');
if(!keter){
mpu_setup();
serial.printf("%s\n","get_off");
- //bt.putc('c');
get_offset();
serial.printf("%d\n",rez());
serial.printf("%s\n","move2");
- //bt.putc('d');
}
Move2(0,0);
change_rm.start();
move_tm.start();
go_up_tm.start();
- //bt.putc('e');
- /*
- */
while(1) {
serial.printf("%f",all);
serial.printf("%s",":");
if(!keter){
readGyz(&gz);
offset_adjust(&all,&gz);
- //bt.putc('f');
- //bt.puts(puc(all));
- //bt.putc('F');
to_signed(&all);
}
/////////////傾き修正////////////////////////////
@@ -158,7 +165,6 @@
pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
if(!activeMove){
if(pwm <= Min_pwm){
- //bt.putc('k');
activeMove = true;
}
@@ -185,24 +191,16 @@
x_dis = rorix-off_rorix;
y_dis = roriy-off_roriy;
- /*
- bt.putc('g');
- bt.puts(puc(x_dis));
- bt.putc('G');
- bt.putc('h');
- bt.puts(puc(y_dis));
- bt.putc('H');
- */
/////移動処理/////////
if(!flag_finish_move){
float pwm = 0;
float distance;
if(points[point_count][0] == UP or points[point_count][0] == BACK){
- distance = pointy-x_dis;
+ distance = pointy-x_dis + wall_C;
last_move_vertical = true;
last_move_side = false;
}else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
- distance = pointx-y_dis;
+ distance = pointx-y_dis + wall_C;
last_move_vertical = false;
last_move_side = true;
}
@@ -216,7 +214,6 @@
if(move_tm.read_ms() >= 600){
serial.printf("%s","waiting");
serial.printf("%d\n",point_count);
- //bt.putc('i');
//////////////////////////////
pwm = 1;
float set0_dis;
@@ -248,7 +245,6 @@
}
}
}else{
- //serial.printf("%s\n","whileroll");
pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
if(pwm_while <= Min_pwm){
whileactive = true;
@@ -267,6 +263,10 @@
}
////////////////////////
point_count++;
+ //////////////////////////////////////////////////////////////////
+ start_wall_time = false;
+ moved_to_wall = false;
+ /////////////////////////////////////////////////////////////////
Move2(1,0);
if(point_count >= sizeof(points)/sizeof(*points)){
flag_finish_move = true;
@@ -275,28 +275,94 @@
}
}
}else{
- //serial.printf("%f",pwm);
if(pwm > Max_pwm)pwm = Max_pwm;
if(activeMove){
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){
+ 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;
array_count++;
point_move = true;
activeMove = false;
Move2(0,0);
- //goto START;
}
}else{
+
+ if(!moved_to_wall && points[point_count][1]-distance > 1200 && !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++;
point_move = true;
activeMove = false;
Move2(0,0);
- //goto START;
}
}
if(point_move){
@@ -332,7 +398,7 @@
}
}
- if(!point_move){
+ 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;
@@ -366,7 +432,6 @@
if(change_rm.read_ms() > 1500){
activeMove = false;
change_rm.reset();
- //bt.putc('j');
serial.printf("%s\n","change_rm");
}
}
@@ -389,6 +454,7 @@
else x_flag = false;
if((sign >> 1) == 1)y_flag = true;//y < 0
else y_flag = false;
+
}
void get_rorivol(){