Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

backup.md@3:8b22783f6bf1, 2019-09-26 (annotated)

Committer:
kazuryu
Date:
Thu Sep 26 02:23:27 2019 +0000
Revision:
3:8b22783f6bf1
Parent:
1:bbf776e6c792
Child:
4:9ba47e5db1e2
925

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazuryu 0:3dc012104243 1
kazuryu 0:3dc012104243 2 #include "mbed.h"
kazuryu 0:3dc012104243 3 #include "functions.h"
kazuryu 0:3dc012104243 4 #include "foot.h"
kazuryu 0:3dc012104243 5 #include "rori.h"
kazuryu 0:3dc012104243 6 #include "peripheral.h"
kazuryu 1:bbf776e6c792 7
kazuryu 0:3dc012104243 8 #define UP 0
kazuryu 0:3dc012104243 9 #define BACK 1
kazuryu 0:3dc012104243 10 #define RIGHT 2
kazuryu 0:3dc012104243 11 #define LEFT 3
kazuryu 0:3dc012104243 12 #define RUP 4
kazuryu 0:3dc012104243 13 #define LUP 5
kazuryu 0:3dc012104243 14 #define RBACK 6
kazuryu 0:3dc012104243 15 #define LBACK 7
kazuryu 0:3dc012104243 16 #define RROLL 8
kazuryu 0:3dc012104243 17 #define LROLL 9
kazuryu 0:3dc012104243 18
kazuryu 0:3dc012104243 19 //767ってちょっと765っぽいよね
kazuryu 0:3dc012104243 20 //lets go fiver time!
kazuryu 0:3dc012104243 21 int sewomukeru[10] = {
kazuryu 1:bbf776e6c792 22 BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL//reverce array
kazuryu 1:bbf776e6c792 23 };
kazuryu 0:3dc012104243 24 //もはやJava
kazuryu 0:3dc012104243 25 Serial serial(USBTX,USBRX);
kazuryu 1:bbf776e6c792 26
kazuryu 0:3dc012104243 27 DigitalOut outs(PF_6);
kazuryu 0:3dc012104243 28 DigitalIn ins(PF_7);
kazuryu 0:3dc012104243 29 DigitalOut outt(D0);
kazuryu 0:3dc012104243 30 DigitalIn intt(D1);
kazuryu 0:3dc012104243 31
kazuryu 0:3dc012104243 32
kazuryu 0:3dc012104243 33 //https://github.com/ARMmbed/mbed-os/blob/master/targets/TARGET_STM/TARGET_STM32F7/TARGET_STM32F767xI/TARGET_NUCLEO_F767ZI/PeripheralPins.c
kazuryu 0:3dc012104243 34
kazuryu 0:3dc012104243 35
kazuryu 0:3dc012104243 36 double all = 0;
kazuryu 0:3dc012104243 37 bool keter = false;
kazuryu 0:3dc012104243 38 double gz;
kazuryu 0:3dc012104243 39 Timer change_rm;
kazuryu 0:3dc012104243 40 Timer move_tm;
kazuryu 1:bbf776e6c792 41 Timer go_up_tm;
kazuryu 0:3dc012104243 42 ///////////定数たち//////////////
kazuryu 1:bbf776e6c792 43 const float Min_ang = 0.1f;
kazuryu 0:3dc012104243 44 const float Max_pwm = 0.2f;
kazuryu 0:3dc012104243 45 const float Max_pwm_roll = 0.3f;
kazuryu 0:3dc012104243 46 const float Min_pwm = 0.02f;
kazuryu 1:bbf776e6c792 47 const float Gein = 0.05f;//0.05f
kazuryu 0:3dc012104243 48 const int R = 103;//これ直径やんけ
kazuryu 0:3dc012104243 49 const int Resolution = 1024;
kazuryu 0:3dc012104243 50 const float RoriGein = 0.001f;
kazuryu 0:3dc012104243 51 ///////////////////////
kazuryu 0:3dc012104243 52 int point_count = 0;
kazuryu 0:3dc012104243 53 int array_count = 0;
kazuryu 0:3dc012104243 54 int C_vari = 0;
kazuryu 1:bbf776e6c792 55 /////////////////Move/////////////////////////
kazuryu 0:3dc012104243 56 /*
kazuryu 0:3dc012104243 57 int points[][3] = {
kazuryu 0:3dc012104243 58 {RIGHT,3700,-1},
kazuryu 0:3dc012104243 59 {LEFT,3700,0}
kazuryu 0:3dc012104243 60 };
kazuryu 0:3dc012104243 61 */
kazuryu 0:3dc012104243 62 int Robox = getRobox();
kazuryu 0:3dc012104243 63 //int C = (1750-Robox);
kazuryu 0:3dc012104243 64 int C = 1550;
kazuryu 3:8b22783f6bf1 65
kazuryu 0:3dc012104243 66 /*
kazuryu 0:3dc012104243 67 int sheets[] = {
kazuryu 0:3dc012104243 68 C+100,C+1300,C+2500
kazuryu 0:3dc012104243 69 };
kazuryu 0:3dc012104243 70 */
kazuryu 0:3dc012104243 71 int sheets[] = {
kazuryu 3:8b22783f6bf1 72 1975,2775,3575//下から:1750,2775,3575
kazuryu 0:3dc012104243 73 };
kazuryu 1:bbf776e6c792 74
kazuryu 1:bbf776e6c792 75 //当たるのは1200mm
kazuryu 0:3dc012104243 76 int nomal_towel[]{
kazuryu 1:bbf776e6c792 77 1975,2775,3575//2130,2930,3730
kazuryu 0:3dc012104243 78 };
kazuryu 0:3dc012104243 79 int final_towel[]{
kazuryu 0:3dc012104243 80 C+410,C+1070,C+1730,C+2390
kazuryu 0:3dc012104243 81 };
kazuryu 0:3dc012104243 82 float Cir = R * 3.14159f;
kazuryu 0:3dc012104243 83 long rorix_l,roriy_l;
kazuryu 0:3dc012104243 84 float pointx,pointy;
kazuryu 0:3dc012104243 85 float rorix = 0;
kazuryu 0:3dc012104243 86 float roriy = 0;
kazuryu 0:3dc012104243 87 float off_rorix,off_roriy;
kazuryu 0:3dc012104243 88 int sign;
kazuryu 0:3dc012104243 89 bool x_flag,y_flag;
kazuryu 0:3dc012104243 90 bool last_move_vertical,last_move_side;
kazuryu 0:3dc012104243 91 float x_dis,y_dis;
kazuryu 0:3dc012104243 92 bool flag_finish_move = false;
kazuryu 1:bbf776e6c792 93
kazuryu 0:3dc012104243 94 void setMove(int p0,int p1);
kazuryu 1:bbf776e6c792 95 void get_rorivol();
kazuryu 1:bbf776e6c792 96
kazuryu 0:3dc012104243 97 float pwm;
kazuryu 0:3dc012104243 98 bool activeMove = false;
kazuryu 0:3dc012104243 99 RORI RX(PF_9,PF_8);
kazuryu 0:3dc012104243 100 RORI RY(PA_4_ALT0,PB_0_ALT0);
kazuryu 0:3dc012104243 101
kazuryu 0:3dc012104243 102 int abspointx = 0;
kazuryu 0:3dc012104243 103 int abspointy = 0;
kazuryu 0:3dc012104243 104 bool final = false;
kazuryu 0:3dc012104243 105 long rorik_l = 0;
kazuryu 0:3dc012104243 106 float rorik = 0;
kazuryu 0:3dc012104243 107 float rorik_temp = 0;
kazuryu 0:3dc012104243 108 float off_rorik = 0;
kazuryu 0:3dc012104243 109
kazuryu 0:3dc012104243 110 bool point_move = false;
kazuryu 0:3dc012104243 111
kazuryu 0:3dc012104243 112 bool go_up = false;
kazuryu 1:bbf776e6c792 113 float dis_go_up = 0;
kazuryu 1:bbf776e6c792 114 //////////////////////////////////////////////
kazuryu 3:8b22783f6bf1 115 bool moved_to_wall = false;
kazuryu 3:8b22783f6bf1 116 bool start_wall_time = false;
kazuryu 3:8b22783f6bf1 117 int from_end = 370;
kazuryu 3:8b22783f6bf1 118 int wall_C = 0;
kazuryu 3:8b22783f6bf1 119 DigitalIn lim_right(PG_10);
kazuryu 3:8b22783f6bf1 120 DigitalIn lim_left(PG_15);
kazuryu 3:8b22783f6bf1 121 int l_r = 0;
kazuryu 3:8b22783f6bf1 122 int l_l = 0;
kazuryu 3:8b22783f6bf1 123 /////////////////////////////////////////////
kazuryu 3:8b22783f6bf1 124 DigitalOut outColor(PG_4);
kazuryu 3:8b22783f6bf1 125 DigitalOut outSorT(PG_7);
kazuryu 3:8b22783f6bf1 126 ////////////////////////////////////////////
kazuryu 0:3dc012104243 127 int main() {
kazuryu 0:3dc012104243 128 ins.mode(PullDown);
kazuryu 3:8b22783f6bf1 129 lim_right.mode(PullDown);
kazuryu 3:8b22783f6bf1 130 lim_left.mode(PullDown);
kazuryu 0:3dc012104243 131 setup_peri();
kazuryu 0:3dc012104243 132 int whichnum = read_peri();
kazuryu 0:3dc012104243 133 int points[whichnum][3];
kazuryu 0:3dc012104243 134 changeColor(&points[0][0]);
kazuryu 3:8b22783f6bf1 135
kazuryu 3:8b22783f6bf1 136 outColor = getColor();
kazuryu 3:8b22783f6bf1 137 outSorT = getPeriwhich();
kazuryu 3:8b22783f6bf1 138
kazuryu 1:bbf776e6c792 139 //if(read_keter())keter=true;
kazuryu 0:3dc012104243 140 setMove(points[point_count][0],points[point_count][1]);
kazuryu 0:3dc012104243 141 serial.printf("%s\n","mpu_setup");
kazuryu 0:3dc012104243 142 if(!keter){
kazuryu 0:3dc012104243 143 mpu_setup();
kazuryu 0:3dc012104243 144 serial.printf("%s\n","get_off");
kazuryu 0:3dc012104243 145 get_offset();
kazuryu 1:bbf776e6c792 146 serial.printf("%d\n",rez());
kazuryu 0:3dc012104243 147 serial.printf("%s\n","move2");
kazuryu 0:3dc012104243 148 }
kazuryu 0:3dc012104243 149 Move2(0,0);
kazuryu 0:3dc012104243 150 change_rm.start();
kazuryu 0:3dc012104243 151 move_tm.start();
kazuryu 1:bbf776e6c792 152 go_up_tm.start();
kazuryu 0:3dc012104243 153
kazuryu 0:3dc012104243 154 while(1) {
kazuryu 1:bbf776e6c792 155 serial.printf("%f",all);
kazuryu 0:3dc012104243 156 serial.printf("%s",":");
kazuryu 0:3dc012104243 157 if(!keter){
kazuryu 0:3dc012104243 158 readGyz(&gz);
kazuryu 0:3dc012104243 159 offset_adjust(&all,&gz);
kazuryu 0:3dc012104243 160 to_signed(&all);
kazuryu 0:3dc012104243 161 }
kazuryu 0:3dc012104243 162 /////////////傾き修正////////////////////////////
kazuryu 0:3dc012104243 163
kazuryu 0:3dc012104243 164 if(!keter){
kazuryu 0:3dc012104243 165 pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 166 if(!activeMove){
kazuryu 0:3dc012104243 167 if(pwm <= Min_pwm){
kazuryu 0:3dc012104243 168 activeMove = true;
kazuryu 0:3dc012104243 169 }
kazuryu 0:3dc012104243 170
kazuryu 0:3dc012104243 171 if(all < Min_ang)Move2(8,pwm);
kazuryu 0:3dc012104243 172 else if(all > -Min_ang)Move2(9,pwm);
kazuryu 0:3dc012104243 173 //else Move2(9,0);
kazuryu 0:3dc012104243 174 /////////////////////////////////////
kazuryu 0:3dc012104243 175
kazuryu 0:3dc012104243 176 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 177 serial.printf("%s",":");
kazuryu 0:3dc012104243 178 serial.printf("%f",all);
kazuryu 0:3dc012104243 179 serial.printf("%s\n",":");
kazuryu 0:3dc012104243 180
kazuryu 0:3dc012104243 181 }
kazuryu 0:3dc012104243 182 }else{
kazuryu 0:3dc012104243 183 if(!activeMove)activeMove = true;
kazuryu 0:3dc012104243 184 }
kazuryu 0:3dc012104243 185 /////////ロリコン値取得///////
kazuryu 0:3dc012104243 186 if(activeMove){
kazuryu 1:bbf776e6c792 187 get_rorivol();
kazuryu 0:3dc012104243 188 }
kazuryu 0:3dc012104243 189
kazuryu 0:3dc012104243 190 ///////符号反転処理////////
kazuryu 0:3dc012104243 191 x_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 192 y_dis = roriy-off_roriy;
kazuryu 1:bbf776e6c792 193
kazuryu 0:3dc012104243 194 /////移動処理/////////
kazuryu 0:3dc012104243 195 if(!flag_finish_move){
kazuryu 0:3dc012104243 196 float pwm = 0;
kazuryu 0:3dc012104243 197 float distance;
kazuryu 0:3dc012104243 198 if(points[point_count][0] == UP or points[point_count][0] == BACK){
kazuryu 3:8b22783f6bf1 199 distance = pointy-x_dis + wall_C;
kazuryu 0:3dc012104243 200 last_move_vertical = true;
kazuryu 0:3dc012104243 201 last_move_side = false;
kazuryu 0:3dc012104243 202 }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
kazuryu 3:8b22783f6bf1 203 distance = pointx-y_dis + wall_C;
kazuryu 0:3dc012104243 204 last_move_vertical = false;
kazuryu 0:3dc012104243 205 last_move_side = true;
kazuryu 0:3dc012104243 206 }
kazuryu 0:3dc012104243 207 //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
kazuryu 0:3dc012104243 208 if(x_flag or y_flag)distance *= -1;
kazuryu 0:3dc012104243 209 pwm = distance*RoriGein;
kazuryu 0:3dc012104243 210 pwm = (pwm < 0)?-pwm:pwm;//abs
kazuryu 0:3dc012104243 211 if(pwm < Min_pwm){
kazuryu 0:3dc012104243 212 //次の処理へ
kazuryu 0:3dc012104243 213 serial.printf("%d\n",move_tm.read_ms());
kazuryu 0:3dc012104243 214 if(move_tm.read_ms() >= 600){
kazuryu 0:3dc012104243 215 serial.printf("%s","waiting");
kazuryu 0:3dc012104243 216 serial.printf("%d\n",point_count);
kazuryu 0:3dc012104243 217 //////////////////////////////
kazuryu 0:3dc012104243 218 pwm = 1;
kazuryu 0:3dc012104243 219 float set0_dis;
kazuryu 0:3dc012104243 220 float pwm_while;
kazuryu 0:3dc012104243 221 bool whileactive = true;
kazuryu 0:3dc012104243 222 move_tm.reset();
kazuryu 0:3dc012104243 223 while(move_tm.read_ms() < 600){
kazuryu 0:3dc012104243 224 if(whileactive){
kazuryu 1:bbf776e6c792 225 get_rorivol();
kazuryu 0:3dc012104243 226 if(last_move_vertical){
kazuryu 0:3dc012104243 227 set0_dis = roriy-off_roriy;
kazuryu 0:3dc012104243 228 pwm = set0_dis*RoriGein;
kazuryu 0:3dc012104243 229 if(abs(pwm) >= Min_pwm)move_tm.reset();
kazuryu 0:3dc012104243 230 if(pwm < 0){
kazuryu 0:3dc012104243 231 Move2(RIGHT,-pwm);
kazuryu 0:3dc012104243 232 }else{
kazuryu 0:3dc012104243 233 Move2(LEFT,pwm);
kazuryu 0:3dc012104243 234 }
kazuryu 0:3dc012104243 235 }else if(last_move_side){
kazuryu 0:3dc012104243 236 serial.printf("%s\n","side");
kazuryu 0:3dc012104243 237 set0_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 238 serial.printf("%f\n",set0_dis);
kazuryu 0:3dc012104243 239 pwm = set0_dis*RoriGein;
kazuryu 0:3dc012104243 240 if(abs(pwm) >= Min_pwm)move_tm.reset();
kazuryu 0:3dc012104243 241 if(pwm < 0){
kazuryu 0:3dc012104243 242 Move2(UP,-pwm);
kazuryu 0:3dc012104243 243 }else{
kazuryu 0:3dc012104243 244 Move2(BACK,pwm);
kazuryu 0:3dc012104243 245 }
kazuryu 0:3dc012104243 246 }
kazuryu 0:3dc012104243 247 }else{
kazuryu 0:3dc012104243 248 pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 249 if(pwm_while <= Min_pwm){
kazuryu 0:3dc012104243 250 whileactive = true;
kazuryu 0:3dc012104243 251 }
kazuryu 0:3dc012104243 252 if(all < Min_ang)Move2(8,pwm_while);
kazuryu 0:3dc012104243 253 else if(all > -Min_ang)Move2(9,pwm_while);
kazuryu 0:3dc012104243 254 else Move2(9,0);
kazuryu 0:3dc012104243 255 }
kazuryu 0:3dc012104243 256 if(whileactive){
kazuryu 0:3dc012104243 257 if(change_rm.read_ms() > 500){
kazuryu 0:3dc012104243 258 whileactive = false;
kazuryu 0:3dc012104243 259 change_rm.reset();
kazuryu 0:3dc012104243 260 serial.printf("%s\n","change_rm_while");
kazuryu 0:3dc012104243 261 }
kazuryu 0:3dc012104243 262 }
kazuryu 0:3dc012104243 263 }
kazuryu 0:3dc012104243 264 ////////////////////////
kazuryu 0:3dc012104243 265 point_count++;
kazuryu 3:8b22783f6bf1 266 //////////////////////////////////////////////////////////////////
kazuryu 3:8b22783f6bf1 267 start_wall_time = false;
kazuryu 3:8b22783f6bf1 268 moved_to_wall = false;
kazuryu 3:8b22783f6bf1 269 /////////////////////////////////////////////////////////////////
kazuryu 0:3dc012104243 270 Move2(1,0);
kazuryu 0:3dc012104243 271 if(point_count >= sizeof(points)/sizeof(*points)){
kazuryu 0:3dc012104243 272 flag_finish_move = true;
kazuryu 0:3dc012104243 273 }else{
kazuryu 0:3dc012104243 274 setMove(points[point_count][0],points[point_count][1]);
kazuryu 0:3dc012104243 275 }
kazuryu 0:3dc012104243 276 }
kazuryu 0:3dc012104243 277 }else{
kazuryu 0:3dc012104243 278 if(pwm > Max_pwm)pwm = Max_pwm;
kazuryu 0:3dc012104243 279 if(activeMove){
kazuryu 1:bbf776e6c792 280 if(points[point_count][2] != 0){
kazuryu 1:bbf776e6c792 281 bool issheets = (points[point_count][2] == 1)?1:0;
kazuryu 1:bbf776e6c792 282 if(issheets){
kazuryu 3:8b22783f6bf1 283 ///////////////////////////////////////////////////////////////////////////////////////////
kazuryu 3:8b22783f6bf1 284 if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
kazuryu 3:8b22783f6bf1 285 moved_to_wall = true;
kazuryu 3:8b22783f6bf1 286 }
kazuryu 3:8b22783f6bf1 287 if(moved_to_wall){
kazuryu 3:8b22783f6bf1 288 l_r = lim_right.read();
kazuryu 3:8b22783f6bf1 289 l_l = lim_left.read();
kazuryu 3:8b22783f6bf1 290 if(!start_wall_time){
kazuryu 3:8b22783f6bf1 291 if(l_r && l_l){
kazuryu 3:8b22783f6bf1 292 start_wall_time = true;
kazuryu 3:8b22783f6bf1 293 all = 0;
kazuryu 3:8b22783f6bf1 294 }else if(!l_r && l_l){
kazuryu 3:8b22783f6bf1 295 serial.printf("%s\n","LROLL");
kazuryu 3:8b22783f6bf1 296 Move2(LROLL,0.1f);
kazuryu 3:8b22783f6bf1 297 }else if(l_r && !l_l){
kazuryu 3:8b22783f6bf1 298 Move2(RROLL,0.1f);
kazuryu 3:8b22783f6bf1 299 serial.printf("%s\n","RROLL");
kazuryu 3:8b22783f6bf1 300 }else{
kazuryu 3:8b22783f6bf1 301 Move2(UP,0.1f);
kazuryu 3:8b22783f6bf1 302 serial.printf("%s\n","UP");
kazuryu 3:8b22783f6bf1 303 }
kazuryu 3:8b22783f6bf1 304 }else{
kazuryu 3:8b22783f6bf1 305 if(!l_r && !l_l){
kazuryu 3:8b22783f6bf1 306 moved_to_wall = false;
kazuryu 3:8b22783f6bf1 307 //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
kazuryu 3:8b22783f6bf1 308 }else if(l_r || l_l){
kazuryu 3:8b22783f6bf1 309 serial.printf("%d",l_l);
kazuryu 3:8b22783f6bf1 310 serial.printf("%s",":");
kazuryu 3:8b22783f6bf1 311 serial.printf("%d\n",l_r);
kazuryu 3:8b22783f6bf1 312 Move2(points[point_count][0],0.1f);
kazuryu 3:8b22783f6bf1 313 }
kazuryu 3:8b22783f6bf1 314 }
kazuryu 3:8b22783f6bf1 315 }
kazuryu 3:8b22783f6bf1 316
kazuryu 3:8b22783f6bf1 317 /////////////////////////////////////////////////////////////////////////////////////////
kazuryu 1:bbf776e6c792 318 if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
kazuryu 1:bbf776e6c792 319 if(array_count == 0)go_up = true;
kazuryu 1:bbf776e6c792 320 array_count++;
kazuryu 1:bbf776e6c792 321 point_move = true;
kazuryu 1:bbf776e6c792 322 activeMove = false;
kazuryu 1:bbf776e6c792 323 Move2(0,0);
kazuryu 1:bbf776e6c792 324 }
kazuryu 1:bbf776e6c792 325 }else{
kazuryu 3:8b22783f6bf1 326
kazuryu 3:8b22783f6bf1 327 if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
kazuryu 3:8b22783f6bf1 328 moved_to_wall = true;
kazuryu 3:8b22783f6bf1 329 }
kazuryu 3:8b22783f6bf1 330 if(moved_to_wall){
kazuryu 3:8b22783f6bf1 331 l_r = lim_right.read();
kazuryu 3:8b22783f6bf1 332 l_l = lim_left.read();
kazuryu 3:8b22783f6bf1 333 if(!start_wall_time){
kazuryu 3:8b22783f6bf1 334 if(l_r && l_l){
kazuryu 3:8b22783f6bf1 335 start_wall_time = true;
kazuryu 3:8b22783f6bf1 336 all = 0;
kazuryu 3:8b22783f6bf1 337 }else if(!l_r && l_l){
kazuryu 3:8b22783f6bf1 338 serial.printf("%s\n","LROLL");
kazuryu 3:8b22783f6bf1 339 Move2(LROLL,0.1f);
kazuryu 3:8b22783f6bf1 340 }else if(l_r && !l_l){
kazuryu 3:8b22783f6bf1 341 Move2(RROLL,0.1f);
kazuryu 3:8b22783f6bf1 342 serial.printf("%s\n","RROLL");
kazuryu 3:8b22783f6bf1 343 }else{
kazuryu 3:8b22783f6bf1 344 Move2(UP,0.1f);
kazuryu 3:8b22783f6bf1 345 serial.printf("%s\n","UP");
kazuryu 3:8b22783f6bf1 346 }
kazuryu 3:8b22783f6bf1 347 }else{
kazuryu 3:8b22783f6bf1 348 if(!l_r && !l_l){
kazuryu 3:8b22783f6bf1 349 serial.printf("%s\n","C");
kazuryu 3:8b22783f6bf1 350 moved_to_wall = false;
kazuryu 3:8b22783f6bf1 351 //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
kazuryu 3:8b22783f6bf1 352 }else if(l_r || l_l){
kazuryu 3:8b22783f6bf1 353 serial.printf("%d",l_l);
kazuryu 3:8b22783f6bf1 354 serial.printf("%s",":");
kazuryu 3:8b22783f6bf1 355 serial.printf("%d\n",l_r);
kazuryu 3:8b22783f6bf1 356 Move2(points[point_count][0],0.1f);
kazuryu 3:8b22783f6bf1 357 }
kazuryu 3:8b22783f6bf1 358 }
kazuryu 3:8b22783f6bf1 359 }
kazuryu 0:3dc012104243 360 if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
kazuryu 0:3dc012104243 361 if(array_count == 0)go_up = true;
kazuryu 0:3dc012104243 362 array_count++;
kazuryu 0:3dc012104243 363 point_move = true;
kazuryu 1:bbf776e6c792 364 activeMove = false;
kazuryu 1:bbf776e6c792 365 Move2(0,0);
kazuryu 1:bbf776e6c792 366 }
kazuryu 1:bbf776e6c792 367 }
kazuryu 1:bbf776e6c792 368 if(point_move){
kazuryu 1:bbf776e6c792 369 serial.printf("%s\n","maemae");
kazuryu 1:bbf776e6c792 370 if(C_vari == 0){
kazuryu 1:bbf776e6c792 371 off_rorik = rorix;
kazuryu 1:bbf776e6c792 372 C_vari = 1;
kazuryu 1:bbf776e6c792 373 }
kazuryu 1:bbf776e6c792 374 if(go_up){
kazuryu 1:bbf776e6c792 375 dis_go_up = abs(rorix-off_rorix);
kazuryu 1:bbf776e6c792 376 if(abs(150-dis_go_up)*RoriGein > Min_pwm){
kazuryu 1:bbf776e6c792 377 if(dis_go_up < 150){
kazuryu 1:bbf776e6c792 378 Move2(0,(150-dis_go_up)*RoriGein);//+-20mm
kazuryu 1:bbf776e6c792 379 go_up_tm.reset();
kazuryu 1:bbf776e6c792 380 }else{
kazuryu 1:bbf776e6c792 381 Move2(1,(dis_go_up-150)*RoriGein);
kazuryu 1:bbf776e6c792 382 go_up_tm.reset();
kazuryu 0:3dc012104243 383 }
kazuryu 0:3dc012104243 384 }
kazuryu 1:bbf776e6c792 385
kazuryu 1:bbf776e6c792 386 if(go_up_tm.read_ms() > 300){
kazuryu 1:bbf776e6c792 387 go_up = false;
kazuryu 1:bbf776e6c792 388 Move2(0,0);
kazuryu 1:bbf776e6c792 389 }
kazuryu 1:bbf776e6c792 390 }else{
kazuryu 1:bbf776e6c792 391 serial.printf("%s\n","owari");
kazuryu 1:bbf776e6c792 392 outs = 1;
kazuryu 1:bbf776e6c792 393 while(!ins.read())serial.printf("%d\n",ins.read());
kazuryu 1:bbf776e6c792 394 outs = 0;
kazuryu 1:bbf776e6c792 395 point_move = false;
kazuryu 1:bbf776e6c792 396 }
kazuryu 1:bbf776e6c792 397
kazuryu 1:bbf776e6c792 398 }
kazuryu 1:bbf776e6c792 399 }
kazuryu 1:bbf776e6c792 400
kazuryu 3:8b22783f6bf1 401 if(!point_move && !moved_to_wall && !go_up){
kazuryu 0:3dc012104243 402 if(distance > 0)Move2(points[point_count][0],pwm);
kazuryu 0:3dc012104243 403 else{
kazuryu 0:3dc012104243 404 pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
kazuryu 0:3dc012104243 405 Move2(sewomukeru[points[point_count][0]],pwm);
kazuryu 0:3dc012104243 406 }
kazuryu 0:3dc012104243 407 }
kazuryu 0:3dc012104243 408 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 409 serial.printf("%s",":");
kazuryu 0:3dc012104243 410 serial.printf("%f",distance);
kazuryu 0:3dc012104243 411 serial.printf("%s",":");
kazuryu 0:3dc012104243 412 serial.printf("%d",point_count);
kazuryu 0:3dc012104243 413 serial.printf("%s",":");
kazuryu 0:3dc012104243 414 serial.printf("%d",sizeof(points)/sizeof(*points));
kazuryu 0:3dc012104243 415 serial.printf("%s",":");
kazuryu 0:3dc012104243 416 serial.printf("%d",y_flag);
kazuryu 0:3dc012104243 417 serial.printf("%s",":");
kazuryu 0:3dc012104243 418 if(points[1][0] == RIGHT)serial.printf("%s","orange");
kazuryu 0:3dc012104243 419 if(points[1][0] == LEFT)serial.printf("%s","blue");
kazuryu 0:3dc012104243 420 serial.printf("%s",":");
kazuryu 0:3dc012104243 421 if(points[0][1] > 5100)serial.printf("%s\n","sheets");
kazuryu 0:3dc012104243 422 else serial.printf("%s\n","towel");
kazuryu 0:3dc012104243 423 }
kazuryu 0:3dc012104243 424 move_tm.reset();
kazuryu 0:3dc012104243 425 }
kazuryu 0:3dc012104243 426
kazuryu 0:3dc012104243 427 }else{
kazuryu 0:3dc012104243 428 serial.printf("%s","finish");
kazuryu 0:3dc012104243 429 }
kazuryu 0:3dc012104243 430
kazuryu 0:3dc012104243 431 if(activeMove){
kazuryu 0:3dc012104243 432 if(change_rm.read_ms() > 1500){
kazuryu 0:3dc012104243 433 activeMove = false;
kazuryu 0:3dc012104243 434 change_rm.reset();
kazuryu 0:3dc012104243 435 serial.printf("%s\n","change_rm");
kazuryu 0:3dc012104243 436 }
kazuryu 0:3dc012104243 437 }
kazuryu 0:3dc012104243 438
kazuryu 0:3dc012104243 439
kazuryu 0:3dc012104243 440 }
kazuryu 0:3dc012104243 441
kazuryu 0:3dc012104243 442 }
kazuryu 0:3dc012104243 443
kazuryu 1:bbf776e6c792 444
kazuryu 0:3dc012104243 445 void setMove(int p0,int p1){
kazuryu 0:3dc012104243 446 trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
kazuryu 0:3dc012104243 447 array_count = 0;
kazuryu 0:3dc012104243 448 off_rorix = rorix;
kazuryu 0:3dc012104243 449 off_roriy = roriy;
kazuryu 0:3dc012104243 450 abspointx = (pointx < 0)?-pointx:pointx;
kazuryu 0:3dc012104243 451 abspointy = (pointy < 0)?-pointy:pointy;
kazuryu 0:3dc012104243 452 //一回
kazuryu 0:3dc012104243 453 if((sign & 0b01) == 1)x_flag = true;//x < 0
kazuryu 0:3dc012104243 454 else x_flag = false;
kazuryu 0:3dc012104243 455 if((sign >> 1) == 1)y_flag = true;//y < 0
kazuryu 0:3dc012104243 456 else y_flag = false;
kazuryu 3:8b22783f6bf1 457
kazuryu 0:3dc012104243 458 }
kazuryu 0:3dc012104243 459
kazuryu 1:bbf776e6c792 460 void get_rorivol(){
kazuryu 1:bbf776e6c792 461 RX.read(&rorix_l);
kazuryu 1:bbf776e6c792 462 RY.read(&roriy_l);
kazuryu 1:bbf776e6c792 463 rorix = rorix_l;
kazuryu 1:bbf776e6c792 464 rorix = rorix/Resolution;//回転数
kazuryu 1:bbf776e6c792 465 rorix *= Cir;
kazuryu 1:bbf776e6c792 466 roriy = roriy_l;
kazuryu 1:bbf776e6c792 467 roriy = roriy/Resolution;
kazuryu 1:bbf776e6c792 468 roriy *= Cir;
kazuryu 1:bbf776e6c792 469 }