Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

backup.md@4:9ba47e5db1e2, 2019-10-05 (annotated)

Committer:
kazuryu
Date:
Sat Oct 05 01:41:37 2019 +0000
Revision:
4:9ba47e5db1e2
Parent:
3:8b22783f6bf1 
B

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