Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

backup.md@1:bbf776e6c792, 2019-09-25 (annotated)

Committer:
kazuryu
Date:
Wed Sep 25 03:57:31 2019 +0000
Revision:
1:bbf776e6c792
Parent:
0:3dc012104243
Child:
3:8b22783f6bf1
Li

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 0:3dc012104243 65 /*
kazuryu 0:3dc012104243 66 int sheets[] = {
kazuryu 0:3dc012104243 67 C+100,C+1300,C+2500
kazuryu 0:3dc012104243 68 };
kazuryu 0:3dc012104243 69 */
kazuryu 0:3dc012104243 70 int sheets[] = {
kazuryu 1:bbf776e6c792 71 900,1700,3500//下から:1750,2775,3575
kazuryu 0:3dc012104243 72 };
kazuryu 1:bbf776e6c792 73
kazuryu 1:bbf776e6c792 74 //当たるのは1200mm
kazuryu 0:3dc012104243 75 int nomal_towel[]{
kazuryu 1:bbf776e6c792 76 1975,2775,3575//2130,2930,3730
kazuryu 0:3dc012104243 77 };
kazuryu 0:3dc012104243 78 int final_towel[]{
kazuryu 0:3dc012104243 79 C+410,C+1070,C+1730,C+2390
kazuryu 0:3dc012104243 80 };
kazuryu 0:3dc012104243 81 float Cir = R * 3.14159f;
kazuryu 0:3dc012104243 82 long rorix_l,roriy_l;
kazuryu 0:3dc012104243 83 float pointx,pointy;
kazuryu 0:3dc012104243 84 float rorix = 0;
kazuryu 0:3dc012104243 85 float roriy = 0;
kazuryu 0:3dc012104243 86 float off_rorix,off_roriy;
kazuryu 0:3dc012104243 87 int sign;
kazuryu 0:3dc012104243 88 bool x_flag,y_flag;
kazuryu 0:3dc012104243 89 bool last_move_vertical,last_move_side;
kazuryu 0:3dc012104243 90 float x_dis,y_dis;
kazuryu 0:3dc012104243 91 bool flag_finish_move = false;
kazuryu 1:bbf776e6c792 92
kazuryu 0:3dc012104243 93 void setMove(int p0,int p1);
kazuryu 1:bbf776e6c792 94 void get_rorivol();
kazuryu 1:bbf776e6c792 95
kazuryu 0:3dc012104243 96 float pwm;
kazuryu 0:3dc012104243 97 bool activeMove = false;
kazuryu 0:3dc012104243 98 RORI RX(PF_9,PF_8);
kazuryu 0:3dc012104243 99 RORI RY(PA_4_ALT0,PB_0_ALT0);
kazuryu 0:3dc012104243 100
kazuryu 0:3dc012104243 101 int abspointx = 0;
kazuryu 0:3dc012104243 102 int abspointy = 0;
kazuryu 0:3dc012104243 103 bool final = false;
kazuryu 0:3dc012104243 104 long rorik_l = 0;
kazuryu 0:3dc012104243 105 float rorik = 0;
kazuryu 0:3dc012104243 106 float rorik_temp = 0;
kazuryu 0:3dc012104243 107 float off_rorik = 0;
kazuryu 0:3dc012104243 108
kazuryu 0:3dc012104243 109 bool point_move = false;
kazuryu 0:3dc012104243 110
kazuryu 0:3dc012104243 111 bool go_up = false;
kazuryu 1:bbf776e6c792 112 float dis_go_up = 0;
kazuryu 1:bbf776e6c792 113 //////////////////////////////////////////////
kazuryu 0:3dc012104243 114 int main() {
kazuryu 0:3dc012104243 115 ins.mode(PullDown);
kazuryu 0:3dc012104243 116
kazuryu 0:3dc012104243 117 setup_peri();
kazuryu 0:3dc012104243 118 int whichnum = read_peri();
kazuryu 0:3dc012104243 119 int points[whichnum][3];
kazuryu 0:3dc012104243 120 changeColor(&points[0][0]);
kazuryu 0:3dc012104243 121
kazuryu 1:bbf776e6c792 122 //if(read_keter())keter=true;
kazuryu 0:3dc012104243 123 //bt.putc('a');
kazuryu 0:3dc012104243 124 setMove(points[point_count][0],points[point_count][1]);
kazuryu 0:3dc012104243 125 serial.printf("%s\n","mpu_setup");
kazuryu 0:3dc012104243 126 //bt.putc('b');
kazuryu 0:3dc012104243 127 if(!keter){
kazuryu 0:3dc012104243 128 mpu_setup();
kazuryu 0:3dc012104243 129 serial.printf("%s\n","get_off");
kazuryu 0:3dc012104243 130 //bt.putc('c');
kazuryu 0:3dc012104243 131 get_offset();
kazuryu 1:bbf776e6c792 132 serial.printf("%d\n",rez());
kazuryu 0:3dc012104243 133 serial.printf("%s\n","move2");
kazuryu 0:3dc012104243 134 //bt.putc('d');
kazuryu 0:3dc012104243 135 }
kazuryu 0:3dc012104243 136 Move2(0,0);
kazuryu 0:3dc012104243 137 change_rm.start();
kazuryu 0:3dc012104243 138 move_tm.start();
kazuryu 1:bbf776e6c792 139 go_up_tm.start();
kazuryu 0:3dc012104243 140 //bt.putc('e');
kazuryu 0:3dc012104243 141 /*
kazuryu 0:3dc012104243 142
kazuryu 0:3dc012104243 143 */
kazuryu 0:3dc012104243 144 while(1) {
kazuryu 1:bbf776e6c792 145 serial.printf("%f",all);
kazuryu 0:3dc012104243 146 serial.printf("%s",":");
kazuryu 0:3dc012104243 147 if(!keter){
kazuryu 0:3dc012104243 148 readGyz(&gz);
kazuryu 0:3dc012104243 149 offset_adjust(&all,&gz);
kazuryu 0:3dc012104243 150 //bt.putc('f');
kazuryu 0:3dc012104243 151 //bt.puts(puc(all));
kazuryu 0:3dc012104243 152 //bt.putc('F');
kazuryu 0:3dc012104243 153 to_signed(&all);
kazuryu 0:3dc012104243 154 }
kazuryu 0:3dc012104243 155 /////////////傾き修正////////////////////////////
kazuryu 0:3dc012104243 156
kazuryu 0:3dc012104243 157 if(!keter){
kazuryu 0:3dc012104243 158 pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 159 if(!activeMove){
kazuryu 0:3dc012104243 160 if(pwm <= Min_pwm){
kazuryu 0:3dc012104243 161 //bt.putc('k');
kazuryu 0:3dc012104243 162 activeMove = true;
kazuryu 0:3dc012104243 163 }
kazuryu 0:3dc012104243 164
kazuryu 0:3dc012104243 165 if(all < Min_ang)Move2(8,pwm);
kazuryu 0:3dc012104243 166 else if(all > -Min_ang)Move2(9,pwm);
kazuryu 0:3dc012104243 167 //else Move2(9,0);
kazuryu 0:3dc012104243 168 /////////////////////////////////////
kazuryu 0:3dc012104243 169
kazuryu 0:3dc012104243 170 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 171 serial.printf("%s",":");
kazuryu 0:3dc012104243 172 serial.printf("%f",all);
kazuryu 0:3dc012104243 173 serial.printf("%s\n",":");
kazuryu 0:3dc012104243 174
kazuryu 0:3dc012104243 175 }
kazuryu 0:3dc012104243 176 }else{
kazuryu 0:3dc012104243 177 if(!activeMove)activeMove = true;
kazuryu 0:3dc012104243 178 }
kazuryu 0:3dc012104243 179 /////////ロリコン値取得///////
kazuryu 0:3dc012104243 180 if(activeMove){
kazuryu 1:bbf776e6c792 181 get_rorivol();
kazuryu 0:3dc012104243 182 }
kazuryu 0:3dc012104243 183
kazuryu 0:3dc012104243 184 ///////符号反転処理////////
kazuryu 0:3dc012104243 185 x_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 186 y_dis = roriy-off_roriy;
kazuryu 1:bbf776e6c792 187
kazuryu 0:3dc012104243 188 /*
kazuryu 0:3dc012104243 189 bt.putc('g');
kazuryu 0:3dc012104243 190 bt.puts(puc(x_dis));
kazuryu 0:3dc012104243 191 bt.putc('G');
kazuryu 0:3dc012104243 192 bt.putc('h');
kazuryu 0:3dc012104243 193 bt.puts(puc(y_dis));
kazuryu 0:3dc012104243 194 bt.putc('H');
kazuryu 0:3dc012104243 195 */
kazuryu 0:3dc012104243 196 /////移動処理/////////
kazuryu 0:3dc012104243 197 if(!flag_finish_move){
kazuryu 0:3dc012104243 198 float pwm = 0;
kazuryu 0:3dc012104243 199 float distance;
kazuryu 0:3dc012104243 200 if(points[point_count][0] == UP or points[point_count][0] == BACK){
kazuryu 0:3dc012104243 201 distance = pointy-x_dis;
kazuryu 0:3dc012104243 202 last_move_vertical = true;
kazuryu 0:3dc012104243 203 last_move_side = false;
kazuryu 0:3dc012104243 204 }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
kazuryu 0:3dc012104243 205 distance = pointx-y_dis;
kazuryu 0:3dc012104243 206 last_move_vertical = false;
kazuryu 0:3dc012104243 207 last_move_side = true;
kazuryu 0:3dc012104243 208 }
kazuryu 0:3dc012104243 209 //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
kazuryu 0:3dc012104243 210 if(x_flag or y_flag)distance *= -1;
kazuryu 0:3dc012104243 211 pwm = distance*RoriGein;
kazuryu 0:3dc012104243 212 pwm = (pwm < 0)?-pwm:pwm;//abs
kazuryu 0:3dc012104243 213 if(pwm < Min_pwm){
kazuryu 0:3dc012104243 214 //次の処理へ
kazuryu 0:3dc012104243 215 serial.printf("%d\n",move_tm.read_ms());
kazuryu 0:3dc012104243 216 if(move_tm.read_ms() >= 600){
kazuryu 0:3dc012104243 217 serial.printf("%s","waiting");
kazuryu 0:3dc012104243 218 serial.printf("%d\n",point_count);
kazuryu 0:3dc012104243 219 //bt.putc('i');
kazuryu 0:3dc012104243 220 //////////////////////////////
kazuryu 0:3dc012104243 221 pwm = 1;
kazuryu 0:3dc012104243 222 float set0_dis;
kazuryu 0:3dc012104243 223 float pwm_while;
kazuryu 0:3dc012104243 224 bool whileactive = true;
kazuryu 0:3dc012104243 225 move_tm.reset();
kazuryu 0:3dc012104243 226 while(move_tm.read_ms() < 600){
kazuryu 0:3dc012104243 227 if(whileactive){
kazuryu 1:bbf776e6c792 228 get_rorivol();
kazuryu 0:3dc012104243 229 if(last_move_vertical){
kazuryu 0:3dc012104243 230 set0_dis = roriy-off_roriy;
kazuryu 0:3dc012104243 231 pwm = set0_dis*RoriGein;
kazuryu 0:3dc012104243 232 if(abs(pwm) >= Min_pwm)move_tm.reset();
kazuryu 0:3dc012104243 233 if(pwm < 0){
kazuryu 0:3dc012104243 234 Move2(RIGHT,-pwm);
kazuryu 0:3dc012104243 235 }else{
kazuryu 0:3dc012104243 236 Move2(LEFT,pwm);
kazuryu 0:3dc012104243 237 }
kazuryu 0:3dc012104243 238 }else if(last_move_side){
kazuryu 0:3dc012104243 239 serial.printf("%s\n","side");
kazuryu 0:3dc012104243 240 set0_dis = rorix-off_rorix;
kazuryu 0:3dc012104243 241 serial.printf("%f\n",set0_dis);
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(UP,-pwm);
kazuryu 0:3dc012104243 246 }else{
kazuryu 0:3dc012104243 247 Move2(BACK,pwm);
kazuryu 0:3dc012104243 248 }
kazuryu 0:3dc012104243 249 }
kazuryu 0:3dc012104243 250 }else{
kazuryu 0:3dc012104243 251 //serial.printf("%s\n","whileroll");
kazuryu 0:3dc012104243 252 pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
kazuryu 0:3dc012104243 253 if(pwm_while <= Min_pwm){
kazuryu 0:3dc012104243 254 whileactive = true;
kazuryu 0:3dc012104243 255 }
kazuryu 0:3dc012104243 256 if(all < Min_ang)Move2(8,pwm_while);
kazuryu 0:3dc012104243 257 else if(all > -Min_ang)Move2(9,pwm_while);
kazuryu 0:3dc012104243 258 else Move2(9,0);
kazuryu 0:3dc012104243 259 }
kazuryu 0:3dc012104243 260 if(whileactive){
kazuryu 0:3dc012104243 261 if(change_rm.read_ms() > 500){
kazuryu 0:3dc012104243 262 whileactive = false;
kazuryu 0:3dc012104243 263 change_rm.reset();
kazuryu 0:3dc012104243 264 serial.printf("%s\n","change_rm_while");
kazuryu 0:3dc012104243 265 }
kazuryu 0:3dc012104243 266 }
kazuryu 0:3dc012104243 267 }
kazuryu 0:3dc012104243 268 ////////////////////////
kazuryu 0:3dc012104243 269 point_count++;
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 //serial.printf("%f",pwm);
kazuryu 0:3dc012104243 279 if(pwm > Max_pwm)pwm = Max_pwm;
kazuryu 0:3dc012104243 280 if(activeMove){
kazuryu 1:bbf776e6c792 281 if(points[point_count][2] != 0){
kazuryu 1:bbf776e6c792 282 bool issheets = (points[point_count][2] == 1)?1:0;
kazuryu 1:bbf776e6c792 283 if(issheets){
kazuryu 1:bbf776e6c792 284 if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
kazuryu 1:bbf776e6c792 285 if(array_count == 0)go_up = true;
kazuryu 1:bbf776e6c792 286 array_count++;
kazuryu 1:bbf776e6c792 287 point_move = true;
kazuryu 1:bbf776e6c792 288 activeMove = false;
kazuryu 1:bbf776e6c792 289 Move2(0,0);
kazuryu 1:bbf776e6c792 290 //goto START;
kazuryu 1:bbf776e6c792 291 }
kazuryu 1:bbf776e6c792 292 }else{
kazuryu 0:3dc012104243 293 if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
kazuryu 0:3dc012104243 294 if(array_count == 0)go_up = true;
kazuryu 0:3dc012104243 295 array_count++;
kazuryu 0:3dc012104243 296 point_move = true;
kazuryu 1:bbf776e6c792 297 activeMove = false;
kazuryu 1:bbf776e6c792 298 Move2(0,0);
kazuryu 1:bbf776e6c792 299 //goto START;
kazuryu 1:bbf776e6c792 300 }
kazuryu 1:bbf776e6c792 301 }
kazuryu 1:bbf776e6c792 302 if(point_move){
kazuryu 1:bbf776e6c792 303 serial.printf("%s\n","maemae");
kazuryu 1:bbf776e6c792 304 if(C_vari == 0){
kazuryu 1:bbf776e6c792 305 off_rorik = rorix;
kazuryu 1:bbf776e6c792 306 C_vari = 1;
kazuryu 1:bbf776e6c792 307 }
kazuryu 1:bbf776e6c792 308 if(go_up){
kazuryu 1:bbf776e6c792 309 dis_go_up = abs(rorix-off_rorix);
kazuryu 1:bbf776e6c792 310 if(abs(150-dis_go_up)*RoriGein > Min_pwm){
kazuryu 1:bbf776e6c792 311 if(dis_go_up < 150){
kazuryu 1:bbf776e6c792 312 Move2(0,(150-dis_go_up)*RoriGein);//+-20mm
kazuryu 1:bbf776e6c792 313 go_up_tm.reset();
kazuryu 1:bbf776e6c792 314 }else{
kazuryu 1:bbf776e6c792 315 Move2(1,(dis_go_up-150)*RoriGein);
kazuryu 1:bbf776e6c792 316 go_up_tm.reset();
kazuryu 0:3dc012104243 317 }
kazuryu 0:3dc012104243 318 }
kazuryu 1:bbf776e6c792 319
kazuryu 1:bbf776e6c792 320 if(go_up_tm.read_ms() > 300){
kazuryu 1:bbf776e6c792 321 go_up = false;
kazuryu 1:bbf776e6c792 322 Move2(0,0);
kazuryu 1:bbf776e6c792 323 }
kazuryu 1:bbf776e6c792 324 }else{
kazuryu 1:bbf776e6c792 325 serial.printf("%s\n","owari");
kazuryu 1:bbf776e6c792 326 outs = 1;
kazuryu 1:bbf776e6c792 327 while(!ins.read())serial.printf("%d\n",ins.read());
kazuryu 1:bbf776e6c792 328 outs = 0;
kazuryu 1:bbf776e6c792 329 point_move = false;
kazuryu 1:bbf776e6c792 330 }
kazuryu 1:bbf776e6c792 331
kazuryu 1:bbf776e6c792 332 }
kazuryu 1:bbf776e6c792 333 }
kazuryu 1:bbf776e6c792 334
kazuryu 0:3dc012104243 335 if(!point_move){
kazuryu 0:3dc012104243 336 if(distance > 0)Move2(points[point_count][0],pwm);
kazuryu 0:3dc012104243 337 else{
kazuryu 0:3dc012104243 338 pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
kazuryu 0:3dc012104243 339 Move2(sewomukeru[points[point_count][0]],pwm);
kazuryu 0:3dc012104243 340 }
kazuryu 0:3dc012104243 341 }
kazuryu 0:3dc012104243 342 serial.printf("%f",pwm);
kazuryu 0:3dc012104243 343 serial.printf("%s",":");
kazuryu 0:3dc012104243 344 serial.printf("%f",distance);
kazuryu 0:3dc012104243 345 serial.printf("%s",":");
kazuryu 0:3dc012104243 346 serial.printf("%d",point_count);
kazuryu 0:3dc012104243 347 serial.printf("%s",":");
kazuryu 0:3dc012104243 348 serial.printf("%d",sizeof(points)/sizeof(*points));
kazuryu 0:3dc012104243 349 serial.printf("%s",":");
kazuryu 0:3dc012104243 350 serial.printf("%d",y_flag);
kazuryu 0:3dc012104243 351 serial.printf("%s",":");
kazuryu 0:3dc012104243 352 if(points[1][0] == RIGHT)serial.printf("%s","orange");
kazuryu 0:3dc012104243 353 if(points[1][0] == LEFT)serial.printf("%s","blue");
kazuryu 0:3dc012104243 354 serial.printf("%s",":");
kazuryu 0:3dc012104243 355 if(points[0][1] > 5100)serial.printf("%s\n","sheets");
kazuryu 0:3dc012104243 356 else serial.printf("%s\n","towel");
kazuryu 0:3dc012104243 357 }
kazuryu 0:3dc012104243 358 move_tm.reset();
kazuryu 0:3dc012104243 359 }
kazuryu 0:3dc012104243 360
kazuryu 0:3dc012104243 361 }else{
kazuryu 0:3dc012104243 362 serial.printf("%s","finish");
kazuryu 0:3dc012104243 363 }
kazuryu 0:3dc012104243 364
kazuryu 0:3dc012104243 365 if(activeMove){
kazuryu 0:3dc012104243 366 if(change_rm.read_ms() > 1500){
kazuryu 0:3dc012104243 367 activeMove = false;
kazuryu 0:3dc012104243 368 change_rm.reset();
kazuryu 0:3dc012104243 369 //bt.putc('j');
kazuryu 0:3dc012104243 370 serial.printf("%s\n","change_rm");
kazuryu 0:3dc012104243 371 }
kazuryu 0:3dc012104243 372 }
kazuryu 0:3dc012104243 373
kazuryu 0:3dc012104243 374
kazuryu 0:3dc012104243 375 }
kazuryu 0:3dc012104243 376
kazuryu 0:3dc012104243 377 }
kazuryu 0:3dc012104243 378
kazuryu 1:bbf776e6c792 379
kazuryu 0:3dc012104243 380 void setMove(int p0,int p1){
kazuryu 0:3dc012104243 381 trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
kazuryu 0:3dc012104243 382 array_count = 0;
kazuryu 0:3dc012104243 383 off_rorix = rorix;
kazuryu 0:3dc012104243 384 off_roriy = roriy;
kazuryu 0:3dc012104243 385 abspointx = (pointx < 0)?-pointx:pointx;
kazuryu 0:3dc012104243 386 abspointy = (pointy < 0)?-pointy:pointy;
kazuryu 0:3dc012104243 387 //一回
kazuryu 0:3dc012104243 388 if((sign & 0b01) == 1)x_flag = true;//x < 0
kazuryu 0:3dc012104243 389 else x_flag = false;
kazuryu 0:3dc012104243 390 if((sign >> 1) == 1)y_flag = true;//y < 0
kazuryu 0:3dc012104243 391 else y_flag = false;
kazuryu 0:3dc012104243 392 }
kazuryu 0:3dc012104243 393
kazuryu 1:bbf776e6c792 394 void get_rorivol(){
kazuryu 1:bbf776e6c792 395 RX.read(&rorix_l);
kazuryu 1:bbf776e6c792 396 RY.read(&roriy_l);
kazuryu 1:bbf776e6c792 397 rorix = rorix_l;
kazuryu 1:bbf776e6c792 398 rorix = rorix/Resolution;//回転数
kazuryu 1:bbf776e6c792 399 rorix *= Cir;
kazuryu 1:bbf776e6c792 400 roriy = roriy_l;
kazuryu 1:bbf776e6c792 401 roriy = roriy/Resolution;
kazuryu 1:bbf776e6c792 402 roriy *= Cir;
kazuryu 1:bbf776e6c792 403 }