Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

main.cpp@2:a3eebc8cd220, 2019-09-25 (annotated)

Committer:
kazuryu
Date:
Wed Sep 25 07:47:37 2019 +0000
Revision:
2:a3eebc8cd220
Parent:
1:bbf776e6c792
Child:
3:8b22783f6bf1
buriburi

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 0:3dc012104243 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 0:3dc012104243 22 BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL//reverce array
kazuryu 0:3dc012104243 23 };
kazuryu 0:3dc012104243 24 //もはやJava
kazuryu 0:3dc012104243 25 Serial serial(USBTX,USBRX);
kazuryu 0:3dc012104243 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 0:3dc012104243 41 Timer go_up_tm;
kazuryu 0:3dc012104243 42 ///////////定数たち//////////////
kazuryu 0:3dc012104243 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 0:3dc012104243 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 0:3dc012104243 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 1:bbf776e6c792 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 2:a3eebc8cd220 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 0:3dc012104243 93
kazuryu 0:3dc012104243 94 void setMove(int p0,int p1);
kazuryu 0:3dc012104243 95 void get_rorivol();
kazuryu 0:3dc012104243 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 0:3dc012104243 113 float dis_go_up = 0;
kazuryu 0:3dc012104243 114 //////////////////////////////////////////////
kazuryu 1:bbf776e6c792 115 bool moved_to_wall = false;
kazuryu 1:bbf776e6c792 116 bool start_wall_time = false;
kazuryu 2:a3eebc8cd220 117 int from_end = 370;
kazuryu 1:bbf776e6c792 118 int wall_C = 0;
kazuryu 2:a3eebc8cd220 119 DigitalIn lim_right(PG_10);
kazuryu 2:a3eebc8cd220 120 DigitalIn lim_left(PG_15);
kazuryu 1:bbf776e6c792 121 int l_r = 0;
kazuryu 1:bbf776e6c792 122 int l_l = 0;
kazuryu 1:bbf776e6c792 123 /////////////////////////////////////////////
kazuryu 0:3dc012104243 124 int main() {
kazuryu 0:3dc012104243 125 ins.mode(PullDown);
kazuryu 1:bbf776e6c792 126 lim_right.mode(PullDown);
kazuryu 1:bbf776e6c792 127 lim_left.mode(PullDown);
kazuryu 0:3dc012104243 128 setup_peri();
kazuryu 0:3dc012104243 129 int whichnum = read_peri();
kazuryu 0:3dc012104243 130 int points[whichnum][3];
kazuryu 0:3dc012104243 131 changeColor(&points[0][0]);
kazuryu 0:3dc012104243 132
kazuryu 0:3dc012104243 133 //if(read_keter())keter=true;
kazuryu 0:3dc012104243 134 //bt.putc('a');
kazuryu 0:3dc012104243 135 setMove(points[point_count][0],points[point_count][1]);
kazuryu 0:3dc012104243 136 serial.printf("%s\n","mpu_setup");
kazuryu 0:3dc012104243 137 //bt.putc('b');
kazuryu 0:3dc012104243 138 if(!keter){
kazuryu 0:3dc012104243 139 mpu_setup();
kazuryu 0:3dc012104243 140 serial.printf("%s\n","get_off");
kazuryu 0:3dc012104243 141 //bt.putc('c');
kazuryu 0:3dc012104243 142 get_offset();
kazuryu 0:3dc012104243 143 serial.printf("%d\n",rez());
kazuryu 0:3dc012104243 144 serial.printf("%s\n","move2");
kazuryu 0:3dc012104243 145 //bt.putc('d');
kazuryu 0:3dc012104243 146 }
kazuryu 0:3dc012104243 147 Move2(0,0);
kazuryu 0:3dc012104243 148 change_rm.start();
kazuryu 0:3dc012104243 149 move_tm.start();
kazuryu 0:3dc012104243 150 go_up_tm.start();
kazuryu 0:3dc012104243 151 //bt.putc('e');
kazuryu 0:3dc012104243 152 /*
kazuryu 0:3dc012104243 153
kazuryu 0:3dc012104243 154 */
kazuryu 0:3dc012104243 155 while(1) {
kazuryu 0:3dc012104243 156 serial.printf("%f",all);
kazuryu 0:3dc012104243 157 serial.printf("%s",":");
kazuryu 0:3dc012104243 158 if(!keter){
kazuryu 0:3dc012104243 159 readGyz(&gz);
kazuryu 0:3dc012104243 160 offset_adjust(&all,&gz);
kazuryu 0:3dc012104243 161 //bt.putc('f');
kazuryu 0:3dc012104243 162 //bt.puts(puc(all));
kazuryu 0:3dc012104243 163 //bt.putc('F');
kazuryu 0:3dc012104243 164 to_signed(&all);
kazuryu 0:3dc012104243 165 }
kazuryu 0:3dc012104243 166 /////////////傾き修正////////////////////////////
kazuryu 0:3dc012104243 167
kazuryu 0:3dc012104243 168 if(!keter){
kazuryu 0:3dc012104243 169 pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 170 if(!activeMove){
kazuryu 0:3dc012104243 171 if(pwm <= Min_pwm){
kazuryu 0:3dc012104243 172 //bt.putc('k');
kazuryu 0:3dc012104243 173 activeMove = true;
kazuryu 0:3dc012104243 174 }
kazuryu 0:3dc012104243 175
kazuryu 0:3dc012104243 176 if(all < Min_ang)Move2(8,pwm);
kazuryu 0:3dc012104243 177 else if(all > -Min_ang)Move2(9,pwm);
kazuryu 0:3dc012104243 178 //else Move2(9,0);
kazuryu 0:3dc012104243 179 /////////////////////////////////////
kazuryu 0:3dc012104243 180
kazuryu 0:3dc012104243 181 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 182 serial.printf("%s",":");
kazuryu 0:3dc012104243 183 serial.printf("%f",all);
kazuryu 0:3dc012104243 184 serial.printf("%s\n",":");
kazuryu 0:3dc012104243 185
kazuryu 0:3dc012104243 186 }
kazuryu 0:3dc012104243 187 }else{
kazuryu 0:3dc012104243 188 if(!activeMove)activeMove = true;
kazuryu 0:3dc012104243 189 }
kazuryu 0:3dc012104243 190 /////////ロリコン値取得///////
kazuryu 0:3dc012104243 191 if(activeMove){
kazuryu 0:3dc012104243 192 get_rorivol();
kazuryu 0:3dc012104243 193 }
kazuryu 0:3dc012104243 194
kazuryu 0:3dc012104243 195 ///////符号反転処理////////
kazuryu 0:3dc012104243 196 x_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 197 y_dis = roriy-off_roriy;
kazuryu 0:3dc012104243 198
kazuryu 0:3dc012104243 199 /*
kazuryu 0:3dc012104243 200 bt.putc('g');
kazuryu 0:3dc012104243 201 bt.puts(puc(x_dis));
kazuryu 0:3dc012104243 202 bt.putc('G');
kazuryu 0:3dc012104243 203 bt.putc('h');
kazuryu 0:3dc012104243 204 bt.puts(puc(y_dis));
kazuryu 0:3dc012104243 205 bt.putc('H');
kazuryu 0:3dc012104243 206 */
kazuryu 0:3dc012104243 207 /////移動処理/////////
kazuryu 0:3dc012104243 208 if(!flag_finish_move){
kazuryu 0:3dc012104243 209 float pwm = 0;
kazuryu 0:3dc012104243 210 float distance;
kazuryu 0:3dc012104243 211 if(points[point_count][0] == UP or points[point_count][0] == BACK){
kazuryu 1:bbf776e6c792 212 distance = pointy-x_dis + wall_C;
kazuryu 0:3dc012104243 213 last_move_vertical = true;
kazuryu 0:3dc012104243 214 last_move_side = false;
kazuryu 0:3dc012104243 215 }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
kazuryu 1:bbf776e6c792 216 distance = pointx-y_dis + wall_C;
kazuryu 0:3dc012104243 217 last_move_vertical = false;
kazuryu 0:3dc012104243 218 last_move_side = true;
kazuryu 0:3dc012104243 219 }
kazuryu 0:3dc012104243 220 //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
kazuryu 0:3dc012104243 221 if(x_flag or y_flag)distance *= -1;
kazuryu 0:3dc012104243 222 pwm = distance*RoriGein;
kazuryu 0:3dc012104243 223 pwm = (pwm < 0)?-pwm:pwm;//abs
kazuryu 0:3dc012104243 224 if(pwm < Min_pwm){
kazuryu 0:3dc012104243 225 //次の処理へ
kazuryu 0:3dc012104243 226 serial.printf("%d\n",move_tm.read_ms());
kazuryu 0:3dc012104243 227 if(move_tm.read_ms() >= 600){
kazuryu 0:3dc012104243 228 serial.printf("%s","waiting");
kazuryu 0:3dc012104243 229 serial.printf("%d\n",point_count);
kazuryu 0:3dc012104243 230 //bt.putc('i');
kazuryu 0:3dc012104243 231 //////////////////////////////
kazuryu 0:3dc012104243 232 pwm = 1;
kazuryu 0:3dc012104243 233 float set0_dis;
kazuryu 0:3dc012104243 234 float pwm_while;
kazuryu 0:3dc012104243 235 bool whileactive = true;
kazuryu 0:3dc012104243 236 move_tm.reset();
kazuryu 0:3dc012104243 237 while(move_tm.read_ms() < 600){
kazuryu 0:3dc012104243 238 if(whileactive){
kazuryu 0:3dc012104243 239 get_rorivol();
kazuryu 0:3dc012104243 240 if(last_move_vertical){
kazuryu 0:3dc012104243 241 set0_dis = roriy-off_roriy;
kazuryu 0:3dc012104243 242 pwm = set0_dis*RoriGein;
kazuryu 0:3dc012104243 243 if(abs(pwm) >= Min_pwm)move_tm.reset();
kazuryu 0:3dc012104243 244 if(pwm < 0){
kazuryu 0:3dc012104243 245 Move2(RIGHT,-pwm);
kazuryu 0:3dc012104243 246 }else{
kazuryu 0:3dc012104243 247 Move2(LEFT,pwm);
kazuryu 0:3dc012104243 248 }
kazuryu 0:3dc012104243 249 }else if(last_move_side){
kazuryu 0:3dc012104243 250 serial.printf("%s\n","side");
kazuryu 0:3dc012104243 251 set0_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 252 serial.printf("%f\n",set0_dis);
kazuryu 0:3dc012104243 253 pwm = set0_dis*RoriGein;
kazuryu 0:3dc012104243 254 if(abs(pwm) >= Min_pwm)move_tm.reset();
kazuryu 0:3dc012104243 255 if(pwm < 0){
kazuryu 0:3dc012104243 256 Move2(UP,-pwm);
kazuryu 0:3dc012104243 257 }else{
kazuryu 0:3dc012104243 258 Move2(BACK,pwm);
kazuryu 0:3dc012104243 259 }
kazuryu 0:3dc012104243 260 }
kazuryu 0:3dc012104243 261 }else{
kazuryu 0:3dc012104243 262 //serial.printf("%s\n","whileroll");
kazuryu 0:3dc012104243 263 pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 264 if(pwm_while <= Min_pwm){
kazuryu 0:3dc012104243 265 whileactive = true;
kazuryu 0:3dc012104243 266 }
kazuryu 0:3dc012104243 267 if(all < Min_ang)Move2(8,pwm_while);
kazuryu 0:3dc012104243 268 else if(all > -Min_ang)Move2(9,pwm_while);
kazuryu 0:3dc012104243 269 else Move2(9,0);
kazuryu 0:3dc012104243 270 }
kazuryu 0:3dc012104243 271 if(whileactive){
kazuryu 0:3dc012104243 272 if(change_rm.read_ms() > 500){
kazuryu 0:3dc012104243 273 whileactive = false;
kazuryu 0:3dc012104243 274 change_rm.reset();
kazuryu 0:3dc012104243 275 serial.printf("%s\n","change_rm_while");
kazuryu 0:3dc012104243 276 }
kazuryu 0:3dc012104243 277 }
kazuryu 0:3dc012104243 278 }
kazuryu 0:3dc012104243 279 ////////////////////////
kazuryu 0:3dc012104243 280 point_count++;
kazuryu 1:bbf776e6c792 281 //////////////////////////////////////////////////////////////////
kazuryu 1:bbf776e6c792 282 start_wall_time = false;
kazuryu 1:bbf776e6c792 283 moved_to_wall = false;
kazuryu 1:bbf776e6c792 284 /////////////////////////////////////////////////////////////////
kazuryu 0:3dc012104243 285 Move2(1,0);
kazuryu 0:3dc012104243 286 if(point_count >= sizeof(points)/sizeof(*points)){
kazuryu 0:3dc012104243 287 flag_finish_move = true;
kazuryu 0:3dc012104243 288 }else{
kazuryu 0:3dc012104243 289 setMove(points[point_count][0],points[point_count][1]);
kazuryu 0:3dc012104243 290 }
kazuryu 0:3dc012104243 291 }
kazuryu 0:3dc012104243 292 }else{
kazuryu 0:3dc012104243 293 //serial.printf("%f",pwm);
kazuryu 0:3dc012104243 294 if(pwm > Max_pwm)pwm = Max_pwm;
kazuryu 0:3dc012104243 295 if(activeMove){
kazuryu 0:3dc012104243 296 if(points[point_count][2] != 0){
kazuryu 0:3dc012104243 297 bool issheets = (points[point_count][2] == 1)?1:0;
kazuryu 0:3dc012104243 298 if(issheets){
kazuryu 1:bbf776e6c792 299 ///////////////////////////////////////////////////////////////////////////////////////////
kazuryu 2:a3eebc8cd220 300
kazuryu 1:bbf776e6c792 301 if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
kazuryu 2:a3eebc8cd220 302 //start_wall_time = true;
kazuryu 1:bbf776e6c792 303 moved_to_wall = true;
kazuryu 1:bbf776e6c792 304 }
kazuryu 1:bbf776e6c792 305 if(moved_to_wall){
kazuryu 1:bbf776e6c792 306 if(!start_wall_time){
kazuryu 1:bbf776e6c792 307 l_r = lim_right.read();
kazuryu 1:bbf776e6c792 308 l_l = lim_left.read();
kazuryu 1:bbf776e6c792 309 if(l_r && l_l){
kazuryu 1:bbf776e6c792 310 start_wall_time = true;
kazuryu 1:bbf776e6c792 311 all = 0;
kazuryu 1:bbf776e6c792 312 }else if(!l_r && l_l){
kazuryu 1:bbf776e6c792 313 serial.printf("%s\n","LROLL");
kazuryu 1:bbf776e6c792 314 Move2(LROLL,0.1f);
kazuryu 1:bbf776e6c792 315 }else if(l_r && !l_l){
kazuryu 1:bbf776e6c792 316 Move2(RROLL,0.1f);
kazuryu 1:bbf776e6c792 317 serial.printf("%s\n","RROLL");
kazuryu 1:bbf776e6c792 318 }else{
kazuryu 1:bbf776e6c792 319 Move2(UP,0.1f);
kazuryu 1:bbf776e6c792 320 serial.printf("%s\n","UP");
kazuryu 1:bbf776e6c792 321 }
kazuryu 1:bbf776e6c792 322 }else{
kazuryu 1:bbf776e6c792 323 if(!l_r && !l_l && start_wall_time){
kazuryu 1:bbf776e6c792 324 serial.printf("%s\n","C");
kazuryu 1:bbf776e6c792 325 moved_to_wall = false;
kazuryu 1:bbf776e6c792 326 wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
kazuryu 1:bbf776e6c792 327 }
kazuryu 1:bbf776e6c792 328 }
kazuryu 1:bbf776e6c792 329 }
kazuryu 2:a3eebc8cd220 330
kazuryu 1:bbf776e6c792 331 /////////////////////////////////////////////////////////////////////////////////////////
kazuryu 0:3dc012104243 332 if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
kazuryu 0:3dc012104243 333 if(array_count == 0)go_up = true;
kazuryu 0:3dc012104243 334 array_count++;
kazuryu 0:3dc012104243 335 point_move = true;
kazuryu 0:3dc012104243 336 activeMove = false;
kazuryu 0:3dc012104243 337 Move2(0,0);
kazuryu 0:3dc012104243 338 //goto START;
kazuryu 0:3dc012104243 339 }
kazuryu 0:3dc012104243 340 }else{
kazuryu 2:a3eebc8cd220 341
kazuryu 2:a3eebc8cd220 342 if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
kazuryu 2:a3eebc8cd220 343 //start_wall_time = true;
kazuryu 2:a3eebc8cd220 344 moved_to_wall = true;
kazuryu 2:a3eebc8cd220 345 }
kazuryu 2:a3eebc8cd220 346 if(moved_to_wall){
kazuryu 2:a3eebc8cd220 347 if(!start_wall_time){
kazuryu 2:a3eebc8cd220 348 l_r = lim_right.read();
kazuryu 2:a3eebc8cd220 349 l_l = lim_left.read();
kazuryu 2:a3eebc8cd220 350 if(l_r && l_l){
kazuryu 2:a3eebc8cd220 351 start_wall_time = true;
kazuryu 2:a3eebc8cd220 352 all = 0;
kazuryu 2:a3eebc8cd220 353 }else if(!l_r && l_l){
kazuryu 2:a3eebc8cd220 354 serial.printf("%s\n","LROLL");
kazuryu 2:a3eebc8cd220 355 Move2(LROLL,0.1f);
kazuryu 2:a3eebc8cd220 356 }else if(l_r && !l_l){
kazuryu 2:a3eebc8cd220 357 Move2(RROLL,0.1f);
kazuryu 2:a3eebc8cd220 358 serial.printf("%s\n","RROLL");
kazuryu 2:a3eebc8cd220 359 }else{
kazuryu 2:a3eebc8cd220 360 Move2(UP,0.1f);
kazuryu 2:a3eebc8cd220 361 serial.printf("%s\n","UP");
kazuryu 2:a3eebc8cd220 362 }
kazuryu 2:a3eebc8cd220 363 }else{
kazuryu 2:a3eebc8cd220 364 if(!l_r && !l_l && start_wall_time){
kazuryu 2:a3eebc8cd220 365 serial.printf("%s\n","C");
kazuryu 2:a3eebc8cd220 366 moved_to_wall = false;
kazuryu 2:a3eebc8cd220 367 wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正
kazuryu 2:a3eebc8cd220 368 }else if(l_r || l_l){
kazuryu 2:a3eebc8cd220 369 Move2(points[point_count][0],0.1f);
kazuryu 2:a3eebc8cd220 370 }
kazuryu 2:a3eebc8cd220 371 }
kazuryu 2:a3eebc8cd220 372 }
kazuryu 0:3dc012104243 373 if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
kazuryu 0:3dc012104243 374 if(array_count == 0)go_up = true;
kazuryu 0:3dc012104243 375 array_count++;
kazuryu 0:3dc012104243 376 point_move = true;
kazuryu 0:3dc012104243 377 activeMove = false;
kazuryu 0:3dc012104243 378 Move2(0,0);
kazuryu 0:3dc012104243 379 //goto START;
kazuryu 0:3dc012104243 380 }
kazuryu 0:3dc012104243 381 }
kazuryu 0:3dc012104243 382 if(point_move){
kazuryu 0:3dc012104243 383 serial.printf("%s\n","maemae");
kazuryu 0:3dc012104243 384 if(C_vari == 0){
kazuryu 0:3dc012104243 385 off_rorik = rorix;
kazuryu 0:3dc012104243 386 C_vari = 1;
kazuryu 0:3dc012104243 387 }
kazuryu 0:3dc012104243 388 if(go_up){
kazuryu 0:3dc012104243 389 dis_go_up = abs(rorix-off_rorix);
kazuryu 0:3dc012104243 390 if(abs(150-dis_go_up)*RoriGein > Min_pwm){
kazuryu 0:3dc012104243 391 if(dis_go_up < 150){
kazuryu 0:3dc012104243 392 Move2(0,(150-dis_go_up)*RoriGein);//+-20mm
kazuryu 0:3dc012104243 393 go_up_tm.reset();
kazuryu 0:3dc012104243 394 }else{
kazuryu 0:3dc012104243 395 Move2(1,(dis_go_up-150)*RoriGein);
kazuryu 0:3dc012104243 396 go_up_tm.reset();
kazuryu 0:3dc012104243 397 }
kazuryu 0:3dc012104243 398 }
kazuryu 0:3dc012104243 399
kazuryu 0:3dc012104243 400 if(go_up_tm.read_ms() > 300){
kazuryu 0:3dc012104243 401 go_up = false;
kazuryu 0:3dc012104243 402 Move2(0,0);
kazuryu 0:3dc012104243 403 }
kazuryu 0:3dc012104243 404 }else{
kazuryu 0:3dc012104243 405 serial.printf("%s\n","owari");
kazuryu 0:3dc012104243 406 outs = 1;
kazuryu 0:3dc012104243 407 while(!ins.read())serial.printf("%d\n",ins.read());
kazuryu 0:3dc012104243 408 outs = 0;
kazuryu 0:3dc012104243 409 point_move = false;
kazuryu 0:3dc012104243 410 }
kazuryu 0:3dc012104243 411
kazuryu 0:3dc012104243 412 }
kazuryu 0:3dc012104243 413 }
kazuryu 0:3dc012104243 414
kazuryu 2:a3eebc8cd220 415 if(!point_move && !moved_to_wall && !go_up){
kazuryu 0:3dc012104243 416 if(distance > 0)Move2(points[point_count][0],pwm);
kazuryu 0:3dc012104243 417 else{
kazuryu 0:3dc012104243 418 pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
kazuryu 0:3dc012104243 419 Move2(sewomukeru[points[point_count][0]],pwm);
kazuryu 0:3dc012104243 420 }
kazuryu 0:3dc012104243 421 }
kazuryu 0:3dc012104243 422 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 423 serial.printf("%s",":");
kazuryu 0:3dc012104243 424 serial.printf("%f",distance);
kazuryu 0:3dc012104243 425 serial.printf("%s",":");
kazuryu 0:3dc012104243 426 serial.printf("%d",point_count);
kazuryu 0:3dc012104243 427 serial.printf("%s",":");
kazuryu 0:3dc012104243 428 serial.printf("%d",sizeof(points)/sizeof(*points));
kazuryu 0:3dc012104243 429 serial.printf("%s",":");
kazuryu 0:3dc012104243 430 serial.printf("%d",y_flag);
kazuryu 0:3dc012104243 431 serial.printf("%s",":");
kazuryu 0:3dc012104243 432 if(points[1][0] == RIGHT)serial.printf("%s","orange");
kazuryu 0:3dc012104243 433 if(points[1][0] == LEFT)serial.printf("%s","blue");
kazuryu 0:3dc012104243 434 serial.printf("%s",":");
kazuryu 0:3dc012104243 435 if(points[0][1] > 5100)serial.printf("%s\n","sheets");
kazuryu 0:3dc012104243 436 else serial.printf("%s\n","towel");
kazuryu 0:3dc012104243 437 }
kazuryu 0:3dc012104243 438 move_tm.reset();
kazuryu 0:3dc012104243 439 }
kazuryu 0:3dc012104243 440
kazuryu 0:3dc012104243 441 }else{
kazuryu 0:3dc012104243 442 serial.printf("%s","finish");
kazuryu 0:3dc012104243 443 }
kazuryu 0:3dc012104243 444
kazuryu 0:3dc012104243 445 if(activeMove){
kazuryu 0:3dc012104243 446 if(change_rm.read_ms() > 1500){
kazuryu 0:3dc012104243 447 activeMove = false;
kazuryu 0:3dc012104243 448 change_rm.reset();
kazuryu 0:3dc012104243 449 //bt.putc('j');
kazuryu 0:3dc012104243 450 serial.printf("%s\n","change_rm");
kazuryu 0:3dc012104243 451 }
kazuryu 0:3dc012104243 452 }
kazuryu 0:3dc012104243 453
kazuryu 0:3dc012104243 454
kazuryu 0:3dc012104243 455 }
kazuryu 0:3dc012104243 456
kazuryu 0:3dc012104243 457 }
kazuryu 0:3dc012104243 458
kazuryu 0:3dc012104243 459
kazuryu 0:3dc012104243 460 void setMove(int p0,int p1){
kazuryu 0:3dc012104243 461 trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
kazuryu 0:3dc012104243 462 array_count = 0;
kazuryu 0:3dc012104243 463 off_rorix = rorix;
kazuryu 0:3dc012104243 464 off_roriy = roriy;
kazuryu 0:3dc012104243 465 abspointx = (pointx < 0)?-pointx:pointx;
kazuryu 0:3dc012104243 466 abspointy = (pointy < 0)?-pointy:pointy;
kazuryu 0:3dc012104243 467 //一回
kazuryu 0:3dc012104243 468 if((sign & 0b01) == 1)x_flag = true;//x < 0
kazuryu 0:3dc012104243 469 else x_flag = false;
kazuryu 0:3dc012104243 470 if((sign >> 1) == 1)y_flag = true;//y < 0
kazuryu 0:3dc012104243 471 else y_flag = false;
kazuryu 1:bbf776e6c792 472
kazuryu 0:3dc012104243 473 }
kazuryu 0:3dc012104243 474
kazuryu 0:3dc012104243 475 void get_rorivol(){
kazuryu 0:3dc012104243 476 RX.read(&rorix_l);
kazuryu 0:3dc012104243 477 RY.read(&roriy_l);
kazuryu 0:3dc012104243 478 rorix = rorix_l;
kazuryu 0:3dc012104243 479 rorix = rorix/Resolution;//回転数
kazuryu 0:3dc012104243 480 rorix *= Cir;
kazuryu 0:3dc012104243 481 roriy = roriy_l;
kazuryu 0:3dc012104243 482 roriy = roriy/Resolution;
kazuryu 0:3dc012104243 483 roriy *= Cir;
kazuryu 0:3dc012104243 484 }