Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

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