Ryu Kaz
t6est
Diff: main.cpp
- Revision:
- 3:8b22783f6bf1
- Parent:
- 2:a3eebc8cd220
- Child:
- 4:9ba47e5db1e2
--- a/main.cpp Wed Sep 25 07:47:37 2019 +0000
+++ b/main.cpp Thu Sep 26 02:23:27 2019 +0000
@@ -41,7 +41,7 @@
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
@@ -121,6 +121,9 @@
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);
@@ -129,38 +132,31 @@
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);
}
/////////////傾き修正////////////////////////////
@@ -169,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;
}
@@ -196,14 +191,6 @@
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;
@@ -227,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;
@@ -259,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;
@@ -290,44 +275,15 @@
}
}
}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){
- //start_wall_time = true;
+ /////////////////////////////////////////////////////////////////////////////////////////
+ if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){
moved_to_wall = true;
}
- if(moved_to_wall){
- if(!start_wall_time){
- l_r = lim_right.read();
- l_l = lim_left.read();
- 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 && start_wall_time){
- serial.printf("%s\n","C");
- moved_to_wall = false;
- wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
- }
- }
- }
-
/////////////////////////////////////////////////////////////////////////////////////////
if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
@@ -335,50 +291,51 @@
point_move = true;
activeMove = false;
Move2(0,0);
- //goto START;
}
}else{
-
- if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
- //start_wall_time = true;
+ /////////////////////////////////////////////////////////////////////////////////////////
+ if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){
moved_to_wall = true;
}
- if(moved_to_wall){
- if(!start_wall_time){
- l_r = lim_right.read();
- l_l = lim_left.read();
- 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 && start_wall_time){
- 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){
- 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(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(point_move){
serial.printf("%s\n","maemae");
if(C_vari == 0){
@@ -387,12 +344,12 @@
}
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
+ if(abs(200-dis_go_up)*RoriGein > Min_pwm){
+ if(dis_go_up < 200){
+ Move2(0,(200-dis_go_up)*RoriGein);//+-20mm
go_up_tm.reset();
}else{
- Move2(1,(dis_go_up-150)*RoriGein);
+ Move2(1,(dis_go_up-200)*RoriGein);
go_up_tm.reset();
}
}
@@ -446,7 +403,6 @@
if(change_rm.read_ms() > 1500){
activeMove = false;
change_rm.reset();
- //bt.putc('j');
serial.printf("%s\n","change_rm");
}
}