ROBOSTEP_5期 / Mbed 2 deprecated George_Master_Param

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed robot

main.cpp@54:f54d26e8d8ac, 2019-05-20 (annotated)

Committer:
yuto17320508
Date:
Mon May 20 11:39:41 2019 +0000
Revision:
54:f54d26e8d8ac
Parent:
49:6337717484fe 
param

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:c1476d342c13 1 #include "mbed.h"
shimizuta 17:2b3fa9b1a05b 2 #include "debug.h"
eri 0:c1476d342c13 3 #include "microinfinity.h"
shimizuta 21:14133581387b 4 #include "sensors.h"
shimizuta 22:0ed9de464f40 5 #include "moves.h"
yuto17320508 48:d9f3ce701aca 6 #include <stdlib.h>
yuto17320508 48:d9f3ce701aca 7
yuto17320508 48:d9f3ce701aca 8
yuto17320508 48:d9f3ce701aca 9 #define BADRULEMODE
shimizuta 21:14133581387b 10
yuto17320508 25:c740e6fd5dab 11 void StateFlow(int i);
yuto17320508 32:aee87dcaf7ca 12 void SetMode();
yuto17320508 32:aee87dcaf7ca 13 void Start();
yuto17320508 32:aee87dcaf7ca 14 void NoHandSignal();
yuto17320508 48:d9f3ce701aca 15 void LoadParameter();
yuto17320508 33:2dbbe198adaf 16
yuto17320508 32:aee87dcaf7ca 17 int start_state=0;
yuto17320508 48:d9f3ce701aca 18 LocalFileSystem SeqFile("Local");
yuto17320508 48:d9f3ce701aca 19
yuto17320508 48:d9f3ce701aca 20 struct Param {
yuto17320508 48:d9f3ce701aca 21 float argument[2];
yuto17320508 48:d9f3ce701aca 22 float duty;
yuto17320508 48:d9f3ce701aca 23 float condition;
yuto17320508 48:d9f3ce701aca 24 };
yuto17320508 48:d9f3ce701aca 25 struct Param params[17]= {}; //とりあえずいっぱい作った
shimizuta 26:5fb1aa9cb7f0 26
kageyuta 1:86c4c38abe40 27 int main()
kageyuta 1:86c4c38abe40 28 {
yuto17320508 48:d9f3ce701aca 29 LoadParameter();
yuto17320508 48:d9f3ce701aca 30
shimizuta 26:5fb1aa9cb7f0 31 //setup関連
shimizuta 22:0ed9de464f40 32 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 33 SetupMoves();
yuto17320508 25:c740e6fd5dab 34 set_gyro();
yuto17320508 38:89d2a9e6c96f 35
yuto17320508 32:aee87dcaf7ca 36 /*while(1)
yuto17320508 32:aee87dcaf7ca 37 {
yuto17320508 32:aee87dcaf7ca 38 printf("switch 1:%d 2:%d 3:%d LR:%d \r\n", switch_modes[0].read(),switch_modes[1].read(),switch_modes[2].read(),switch_LR.read());
yuto17320508 38:89d2a9e6c96f 39 wait(0.1);
yuto17320508 32:aee87dcaf7ca 40 }*/
yuto17320508 48:d9f3ce701aca 41
yuto17320508 32:aee87dcaf7ca 42 printf("hand read:%d\r\n",hand.read());
yuto17320508 38:89d2a9e6c96f 43
shimizuta 22:0ed9de464f40 44 printf("reset standby\r\n");
shimizuta 26:5fb1aa9cb7f0 45 reset();//足をリセット位置に移動
shimizuta 22:0ed9de464f40 46 printf("bus standby\r\n");
yuto17320508 5:63462d696256 47 while(1) {
yuto17320508 5:63462d696256 48 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 49 }
shimizuta 22:0ed9de464f40 50 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 51 wait(0.5);
shimizuta 26:5fb1aa9cb7f0 52 //一歩動かして初期位置へ
yuto17320508 25:c740e6fd5dab 53 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 54 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 55 straight();
yuto17320508 38:89d2a9e6c96f 56
yuto17320508 38:89d2a9e6c96f 57
shimizuta 26:5fb1aa9cb7f0 58 // mode選択
yuto17320508 32:aee87dcaf7ca 59 Start();
yuto17320508 32:aee87dcaf7ca 60 //SetMode();
yuto17320508 48:d9f3ce701aca 61 #ifndef BADRULEMODE
yuto17320508 38:89d2a9e6c96f 62 if(RightOrLeft == 0) {
yuto17320508 32:aee87dcaf7ca 63 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 64 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 32:aee87dcaf7ca 65 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 66 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 67 else printf("state_error");
yuto17320508 38:89d2a9e6c96f 68 } else {
yuto17320508 32:aee87dcaf7ca 69 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 70 else if(start_state == 1) theta0 = -degree/100.0 + 45;
yuto17320508 32:aee87dcaf7ca 71 else if(start_state == 2) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 72 else if(start_state == 3) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 73 else printf("state_error");
yuto17320508 32:aee87dcaf7ca 74 }
yuto17320508 48:d9f3ce701aca 75 #endif
yuto17320508 48:d9f3ce701aca 76 #ifdef BADRULEMODE
yuto17320508 48:d9f3ce701aca 77 if(RightOrLeft == 0) {
yuto17320508 48:d9f3ce701aca 78 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 48:d9f3ce701aca 79 else if(start_state == 1) {
yuto17320508 54:f54d26e8d8ac 80 theta0 = -degree/100.0 - 90;
yuto17320508 48:d9f3ce701aca 81 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 82 motor_li.setDutyLimit(0.6);
yuto17320508 54:f54d26e8d8ac 83 wait(0.5);
yuto17320508 54:f54d26e8d8ac 84 turn(55.0);
yuto17320508 48:d9f3ce701aca 85 } else if(start_state == 2) {
yuto17320508 48:d9f3ce701aca 86 theta0 = -degree/100.0 - 135;
yuto17320508 48:d9f3ce701aca 87 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 88 motor_li.setDutyLimit(0.6);
yuto17320508 48:d9f3ce701aca 89 turn(100.0);
yuto17320508 48:d9f3ce701aca 90 } else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 48:d9f3ce701aca 91 else printf("state_error");
yuto17320508 48:d9f3ce701aca 92 } else {
yuto17320508 48:d9f3ce701aca 93 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 48:d9f3ce701aca 94 else if(start_state == 1) {
yuto17320508 54:f54d26e8d8ac 95 theta0 = -degree/100.0 + 90;
yuto17320508 48:d9f3ce701aca 96 //段差前旋回
yuto17320508 48:d9f3ce701aca 97 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 98 motor_li.setDutyLimit(0.6);
yuto17320508 54:f54d26e8d8ac 99 turn(-55.0);
yuto17320508 48:d9f3ce701aca 100 } else if(start_state == 2) {
yuto17320508 48:d9f3ce701aca 101 theta0 = -degree/100.0 + 135;
yuto17320508 48:d9f3ce701aca 102 motor_lo.setDutyLimit(0.6);//0.5
yuto17320508 48:d9f3ce701aca 103 motor_li.setDutyLimit(0.6);
yuto17320508 48:d9f3ce701aca 104 turn(-100.0);
yuto17320508 48:d9f3ce701aca 105 } else if(start_state == 3) theta0 = -degree/100.0 - 90;
yuto17320508 48:d9f3ce701aca 106 else printf("state_error");
yuto17320508 48:d9f3ce701aca 107 }
yuto17320508 48:d9f3ce701aca 108 #endif
yuto17320508 33:2dbbe198adaf 109 FileOpen();
yuto17320508 32:aee87dcaf7ca 110 printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
yuto17320508 25:c740e6fd5dab 111 StateFlow(start_state);
yuto17320508 33:2dbbe198adaf 112 FileClose();
yuto17320508 25:c740e6fd5dab 113 }
yuto17320508 25:c740e6fd5dab 114
yuto17320508 25:c740e6fd5dab 115 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 116 {
yuto17320508 25:c740e6fd5dab 117 switch(i) {
yuto17320508 25:c740e6fd5dab 118 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 119 printf("go straight!!!!!!!!!!\r\n");
yuto17320508 49:6337717484fe 120 motor_lo.setDutyLimit(params[0].duty);
yuto17320508 49:6337717484fe 121 motor_li.setDutyLimit(params[0].duty);
yuto17320508 49:6337717484fe 122 for(int i = 0; i < (int)params[0].condition; i++) {
yuto17320508 49:6337717484fe 123 straightAndInfinity(params[0].argument[0], params[0].argument[1]);
yuto17320508 25:c740e6fd5dab 124 }
yuto17320508 25:c740e6fd5dab 125 printf("get distance mode");
yuto17320508 49:6337717484fe 126
yuto17320508 49:6337717484fe 127 motor_lo.setDutyLimit(params[1].duty);
yuto17320508 49:6337717484fe 128 motor_li.setDutyLimit(params[1].duty);
yuto17320508 25:c740e6fd5dab 129 for(int i=0; i<1; ++i) {
yuto17320508 49:6337717484fe 130 while(get_dist_back() < params[1].condition) {//300
yuto17320508 49:6337717484fe 131 straightAndInfinity(params[1].argument[0], params[1].argument[1]);
yuto17320508 25:c740e6fd5dab 132 }
yuto17320508 25:c740e6fd5dab 133 }
yuto17320508 25:c740e6fd5dab 134 //段差前旋回
yuto17320508 49:6337717484fe 135 motor_lo.setDutyLimit(params[2].duty);
yuto17320508 49:6337717484fe 136 motor_li.setDutyLimit(params[2].duty);
yuto17320508 49:6337717484fe 137 if(RightOrLeft == 0) turn(params[2].argument[0]);
yuto17320508 49:6337717484fe 138 else turn(-params[2].argument[0]);
yuto17320508 25:c740e6fd5dab 139 case 1://段差前
yuto17320508 25:c740e6fd5dab 140 printf("climb!!!!!!!!!!\r\n");
yuto17320508 49:6337717484fe 141
yuto17320508 49:6337717484fe 142 motor_lo.setDutyLimit(params[3].duty);
yuto17320508 49:6337717484fe 143 motor_li.setDutyLimit(params[3].duty);
yuto17320508 38:89d2a9e6c96f 144 //段差乗り越え
yuto17320508 49:6337717484fe 145 while(get_dist_forward() < params[3].condition) {
yuto17320508 49:6337717484fe 146 if(RightOrLeft == 0) straightAndInfinity(params[3].argument[0], params[3].argument[1]);
yuto17320508 49:6337717484fe 147 else straightAndInfinity(-params[3].argument[0], params[3].argument[1]);
yuto17320508 25:c740e6fd5dab 148 }
yuto17320508 49:6337717484fe 149
yuto17320508 49:6337717484fe 150 motor_lo.setDutyLimit(params[4].duty);
yuto17320508 49:6337717484fe 151 motor_li.setDutyLimit(params[4].duty);
yuto17320508 49:6337717484fe 152 if(RightOrLeft == 0) phasing(params[4].argument[0]);
yuto17320508 49:6337717484fe 153 else phasing(-params[4].argument[0]);
yuto17320508 38:89d2a9e6c96f 154
yuto17320508 49:6337717484fe 155 motor_lo.setDutyLimit(params[5].duty);
yuto17320508 49:6337717484fe 156 motor_li.setDutyLimit(params[5].duty);
yuto17320508 49:6337717484fe 157 for(int i= 0; i<(int)params[5].condition; ++i) straight();
yuto17320508 49:6337717484fe 158
yuto17320508 49:6337717484fe 159 motor_lo.setDutyLimit(params[6].duty);
yuto17320508 49:6337717484fe 160 motor_li.setDutyLimit(params[6].duty);
yuto17320508 49:6337717484fe 161 while(get_dist_back() > params[6].condition) straight();
yuto17320508 49:6337717484fe 162
yuto17320508 25:c740e6fd5dab 163 //段差後旋回
yuto17320508 49:6337717484fe 164 motor_lo.setDutyLimit(params[7].duty);
yuto17320508 49:6337717484fe 165 motor_li.setDutyLimit(params[7].duty);
yuto17320508 49:6337717484fe 166 if(RightOrLeft == 0) turn(params[7].argument[0]);
yuto17320508 49:6337717484fe 167 else turn(-params[7].argument[0]);
yuto17320508 38:89d2a9e6c96f 168
yuto17320508 25:c740e6fd5dab 169 case 2://段差直後
yuto17320508 25:c740e6fd5dab 170 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 171 //前進
yuto17320508 49:6337717484fe 172 motor_lo.setDutyLimit(params[8].duty);
yuto17320508 49:6337717484fe 173 motor_li.setDutyLimit(params[8].duty);
yuto17320508 49:6337717484fe 174 for(int i=0; i<(int)params[8].condition; ++i) {
yuto17320508 49:6337717484fe 175 if(RightOrLeft == 0) straightAndInfinity(params[8].argument[0], params[8].argument[1]);
yuto17320508 49:6337717484fe 176 else straightAndInfinity(-params[8].argument[0], params[8].argument[1]);
yuto17320508 38:89d2a9e6c96f 177 }
yuto17320508 49:6337717484fe 178
yuto17320508 49:6337717484fe 179 motor_lo.setDutyLimit(params[9].duty);
yuto17320508 49:6337717484fe 180 motor_li.setDutyLimit(params[9].duty);
yuto17320508 25:c740e6fd5dab 181 for(int i=0; i<3; ++i) {
yuto17320508 49:6337717484fe 182 while(get_dist_forward() > params[9].condition) {
yuto17320508 49:6337717484fe 183 if(RightOrLeft == 0) straightAndInfinity(params[9].argument[0], params[9].argument[1]);
yuto17320508 49:6337717484fe 184 else straightAndInfinity(-params[9].argument[0], params[9].argument[1]);
yuto17320508 25:c740e6fd5dab 185 }
yuto17320508 25:c740e6fd5dab 186 }
yuto17320508 25:c740e6fd5dab 187 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 188 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 49:6337717484fe 189 motor_lo.setDutyLimit(params[10].duty);
yuto17320508 49:6337717484fe 190 motor_li.setDutyLimit(params[10].duty);
yuto17320508 49:6337717484fe 191 if(RightOrLeft == 0) turn(params[10].argument[0]);
yuto17320508 49:6337717484fe 192 else turn(-params[10].argument[0]);
yuto17320508 48:d9f3ce701aca 193
yuto17320508 25:c740e6fd5dab 194 //ロープ位置ジャストまで前進
yuto17320508 49:6337717484fe 195 motor_lo.setDutyLimit(params[11].duty);
yuto17320508 49:6337717484fe 196 motor_li.setDutyLimit(params[11].duty);
yuto17320508 25:c740e6fd5dab 197 for(int i=0; i<3; ++i) {
yuto17320508 49:6337717484fe 198 straightAndInfinity(params[11].argument[0], params[11].argument[1]);
yuto17320508 25:c740e6fd5dab 199 }
yuto17320508 25:c740e6fd5dab 200 for(int i=0; i<2; ++i) {
yuto17320508 49:6337717484fe 201 while(get_dist_back() < params[11].condition) {
yuto17320508 49:6337717484fe 202 straightAndInfinity(params[11].argument[0], params[11].argument[1]);
yuto17320508 25:c740e6fd5dab 203 }
yuto17320508 25:c740e6fd5dab 204 }
yuto17320508 49:6337717484fe 205
yuto17320508 49:6337717484fe 206 motor_lo.setDutyLimit(params[12].duty);
yuto17320508 49:6337717484fe 207 motor_li.setDutyLimit(params[12].duty);
yuto17320508 49:6337717484fe 208 phasing(params[12].argument[0]);
yuto17320508 49:6337717484fe 209
yuto17320508 25:c740e6fd5dab 210 //ロープ
yuto17320508 49:6337717484fe 211 motor_lo.setDutyLimit(params[13].duty);
yuto17320508 49:6337717484fe 212 motor_li.setDutyLimit(params[13].duty);
yuto17320508 32:aee87dcaf7ca 213 while(switch_modes[2].read() == 0) {
yuto17320508 49:6337717484fe 214 //straightAndInfinity(params[13].argument[0], params[13].argument[1]);
yuto17320508 54:f54d26e8d8ac 215 //straight();
yuto17320508 54:f54d26e8d8ac 216 ropeAndInfinity(params[13].argument[0], params[13].argument[1]);
yuto17320508 25:c740e6fd5dab 217 }
yuto17320508 32:aee87dcaf7ca 218 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 32:aee87dcaf7ca 219 NoHandSignal();
yuto17320508 25:c740e6fd5dab 220 case 3://坂
yuto17320508 32:aee87dcaf7ca 221 printf("last spart!!!!!!!!");
yuto17320508 38:89d2a9e6c96f 222
yuto17320508 32:aee87dcaf7ca 223 if(RightOrLeft == 0) theta0 = -degree/100.0+90;
yuto17320508 32:aee87dcaf7ca 224 else theta0 = -degree/100.0-90;
yuto17320508 49:6337717484fe 225 motor_lo.setDutyLimit(params[14].duty);
yuto17320508 49:6337717484fe 226 motor_li.setDutyLimit(params[14].duty);
yuto17320508 49:6337717484fe 227 for(int i=0; i<(int)params[14].condition; ++i) {
yuto17320508 49:6337717484fe 228 if(RightOrLeft == 0) straightAndInfinity(params[14].argument[0], params[14].argument[1]);//straight();//climbAndInfinity(-90,5);
yuto17320508 49:6337717484fe 229 else straightAndInfinity(-params[14].argument[0], params[14].argument[1]);
yuto17320508 25:c740e6fd5dab 230 }
yuto17320508 25:c740e6fd5dab 231 printf("wall standby");
yuto17320508 49:6337717484fe 232 motor_lo.setDutyLimit(params[15].duty);
yuto17320508 49:6337717484fe 233 motor_li.setDutyLimit(params[15].duty);
yuto17320508 49:6337717484fe 234 while(get_dist_back() < params[15].condition) {
shimizuta 27:d392a95f4799 235 // straight();
yuto17320508 49:6337717484fe 236 if(RightOrLeft == 0) climbAndInfinity(params[15].argument[0], params[15].argument[1]);
yuto17320508 49:6337717484fe 237 else climbAndInfinity(-params[15].argument[0], params[15].argument[1]);
yuto17320508 25:c740e6fd5dab 238 }
yuto17320508 49:6337717484fe 239
yuto17320508 25:c740e6fd5dab 240 for(int i=0; i<1; ++i) {
yuto17320508 49:6337717484fe 241 while(get_dist_forward() > params[16].condition) {
yuto17320508 25:c740e6fd5dab 242 //straight();
yuto17320508 49:6337717484fe 243 if(RightOrLeft == 0) climbAndInfinity(params[16].argument[0], params[16].argument[1]);
yuto17320508 49:6337717484fe 244 else climbAndInfinity(-params[16].argument[0], params[16].argument[1]);
yuto17320508 25:c740e6fd5dab 245 }
yuto17320508 25:c740e6fd5dab 246 }
yuto17320508 25:c740e6fd5dab 247 hand_mode = GOAL;
shimizuta 30:231e6584afe9 248 stop();
yuto17320508 32:aee87dcaf7ca 249 printf("uhai!!!!!!!!!!!!!!!/r/n");
yuto17320508 32:aee87dcaf7ca 250 }
yuto17320508 32:aee87dcaf7ca 251 }
yuto17320508 32:aee87dcaf7ca 252 void Start()
yuto17320508 32:aee87dcaf7ca 253 {
yuto17320508 38:89d2a9e6c96f 254 if(hand.read()==0) { //開始時すでにスイッチが押されていた場合
yuto17320508 38:89d2a9e6c96f 255
yuto17320508 32:aee87dcaf7ca 256 SetMode();
yuto17320508 35:04699b49c463 257 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 258 NoHandSignal();
yuto17320508 38:89d2a9e6c96f 259 } else {
yuto17320508 32:aee87dcaf7ca 260 SetMode();
yuto17320508 35:04699b49c463 261 hand_mode = G_OPEN;
yuto17320508 35:04699b49c463 262 stop();//ここで開くことをcanでslaveに送る
yuto17320508 48:d9f3ce701aca 263 wait(1);
yuto17320508 32:aee87dcaf7ca 264 wait_gerege();
yuto17320508 35:04699b49c463 265 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 266 HandMove();
yuto17320508 32:aee87dcaf7ca 267 wait(0.5);
yuto17320508 38:89d2a9e6c96f 268 }
yuto17320508 32:aee87dcaf7ca 269 }
yuto17320508 32:aee87dcaf7ca 270 void SetMode()
yuto17320508 32:aee87dcaf7ca 271 {
yuto17320508 38:89d2a9e6c96f 272 while(get_dist_back() > 50.0) {
yuto17320508 32:aee87dcaf7ca 273 led1 = led2 = led3 = led4 = 0;
yuto17320508 32:aee87dcaf7ca 274 if(switch_modes[2].read()) start_state=3;
yuto17320508 32:aee87dcaf7ca 275 else if(switch_modes[1].read()) start_state=2;
yuto17320508 32:aee87dcaf7ca 276 else if(switch_modes[0].read()) start_state=1;
yuto17320508 32:aee87dcaf7ca 277 else start_state=0;
yuto17320508 38:89d2a9e6c96f 278
yuto17320508 38:89d2a9e6c96f 279 if(switch_LR.read()) RightOrLeft = 1;
yuto17320508 38:89d2a9e6c96f 280 else RightOrLeft = 0;
yuto17320508 38:89d2a9e6c96f 281
yuto17320508 32:aee87dcaf7ca 282 if(start_state==0)led4 = 1;
yuto17320508 32:aee87dcaf7ca 283 else if(start_state==1)led3 = 1;
yuto17320508 32:aee87dcaf7ca 284 else if(start_state==2)led2 = 1;
yuto17320508 32:aee87dcaf7ca 285 else if(start_state==3)led1 = 1;
yuto17320508 38:89d2a9e6c96f 286
yuto17320508 32:aee87dcaf7ca 287 }
yuto17320508 32:aee87dcaf7ca 288 printf("mode is %d RightOrLeft is %d \r\n", start_state, RightOrLeft);
yuto17320508 32:aee87dcaf7ca 289 led1 = led2 = led3 = led4 = 1;
yuto17320508 32:aee87dcaf7ca 290 }
yuto17320508 32:aee87dcaf7ca 291 void NoHandSignal()
yuto17320508 32:aee87dcaf7ca 292 {
yuto17320508 38:89d2a9e6c96f 293 while(get_dist_back() > 40.0) {
yuto17320508 32:aee87dcaf7ca 294 wait(0.01);
yuto17320508 32:aee87dcaf7ca 295 }
yuto17320508 38:89d2a9e6c96f 296 while(get_dist_back() < 40.0) {
yuto17320508 32:aee87dcaf7ca 297 wait(0.01);
shimizuta 22:0ed9de464f40 298 }
yuto17320508 48:d9f3ce701aca 299 }
yuto17320508 48:d9f3ce701aca 300 void LoadParameter()
yuto17320508 48:d9f3ce701aca 301 {
yuto17320508 48:d9f3ce701aca 302 FILE *fp;
yuto17320508 48:d9f3ce701aca 303 if ((fp = fopen("/Local/params.csv", "r")) == NULL) {//ファイル操作はじめかつエラー処理
yuto17320508 48:d9f3ce701aca 304 printf("file open error!!\n");
yuto17320508 48:d9f3ce701aca 305 exit(EXIT_FAILURE);
yuto17320508 48:d9f3ce701aca 306 }
yuto17320508 48:d9f3ce701aca 307 //以下ファイル操作の中身
yuto17320508 48:d9f3ce701aca 308 int i;
yuto17320508 48:d9f3ce701aca 309 while (fscanf(fp, "%f,%f,%f,%f", &params[i].argument[0], &params[i].argument[1], &params[i].duty,&params[i].condition) != EOF) {
yuto17320508 49:6337717484fe 310 printf("%f,%f,%f,%f\r\n",params[i].argument[0],params[i].argument[1],params[i].duty,params[i].condition);
yuto17320508 48:d9f3ce701aca 311 i++;
yuto17320508 48:d9f3ce701aca 312 }
yuto17320508 48:d9f3ce701aca 313 fclose(fp);//ファイル操作終わり
shimizuta 22:0ed9de464f40 314 }