Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

main.cpp@0:3dc012104243, 2019-09-24 (annotated)

Committer:
kazuryu
Date:
Tue Sep 24 13:35:31 2019 +0000
Revision:
0:3dc012104243
Child:
1:bbf776e6c792
test;

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