Takuya Kobayashi / Mbed 2 deprecated mbed_linetrace_termC

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

main.cpp@4:edec6fe32ba9, 2018-11-20 (annotated)

Committer:
kamorei
Date:
Tue Nov 20 18:31:33 2018 +0000
Revision:
4:edec6fe32ba9
Parent:
3:e8c29cd3ca22
Child:
5:30251ab9313a
11/21 f*ckin' cold!!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kamorei 0:500d22d1efeb 1 #include "mbed.h"
kamorei 0:500d22d1efeb 2
kamorei 0:500d22d1efeb 3 #define KP 5.8 //Pゲイン
kamorei 3:e8c29cd3ca22 4 #define CHG 0.05 //change_gray用の係数
kamorei 2:47477c2b1925 5 #define WHITE_CL 0.92 //初期値
kamorei 3:e8c29cd3ca22 6 #define WHITE_CR 0.92 //初期値
kamorei 2:47477c2b1925 7 #define GRAY_CL 0.46 //初期値
kamorei 2:47477c2b1925 8 #define GRAY_CR 0.5 //初期値
kamorei 3:e8c29cd3ca22 9 #define WHITE_L 0.1 //初期値
kamorei 3:e8c29cd3ca22 10 #define WHITE_R 0.1 //初期値
kamorei 3:e8c29cd3ca22 11 #define GRAY_L 0.05 //初期値
kamorei 3:e8c29cd3ca22 12 #define GRAY_R 0.05 //初期値
kamorei 0:500d22d1efeb 13
kamorei 4:edec6fe32ba9 14 //#define sensor_not_straight
kamorei 4:edec6fe32ba9 15 //#define graychange
kamorei 0:500d22d1efeb 16
kamorei 4:edec6fe32ba9 17 DigitalOut ledCheck( PTE2); //動作check
kamorei 4:edec6fe32ba9 18 DigitalOut ledCount( PTB11); //プログラムの切り替え確認
kamorei 4:edec6fe32ba9 19 BusOut ledCHG( PTB10, PTE3); //change_gray用
kamorei 4:edec6fe32ba9 20 BusOut ledLL( PTE5, PTE4); //ロボットから進行方向を見て左（わざと逆書き）
kamorei 4:edec6fe32ba9 21 BusOut ledRR( PTB8, PTB9); //ロボットから進行方向を見て右
kamorei 3:e8c29cd3ca22 22 AnalogIn sensorR( PTC2);
kamorei 0:500d22d1efeb 23 AnalogIn sensorL( PTB3);
kamorei 0:500d22d1efeb 24 AnalogIn sensorCR( PTB0);
kamorei 0:500d22d1efeb 25 AnalogIn sensorCL( PTB2);
kamorei 0:500d22d1efeb 26 //モータ1
kamorei 0:500d22d1efeb 27 BusOut Mlefti(PTA1, PTA2);
kamorei 0:500d22d1efeb 28 PwmOut Mleftp(PTD4);
kamorei 0:500d22d1efeb 29 //モータ2
kamorei 0:500d22d1efeb 30 BusOut Mrighti(PTC0, PTC7);
kamorei 0:500d22d1efeb 31 PwmOut Mrightp(PTA12);
kamorei 0:500d22d1efeb 32
kamorei 0:500d22d1efeb 33 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
kamorei 3:e8c29cd3ca22 34 float whiteL[2], whiteR[2], grayL[2], grayR[2];
kamorei 4:edec6fe32ba9 35 float blackCL = 0.18, blackCR = 0.1; //閾値
kamorei 3:e8c29cd3ca22 36 float blackL = 0.028, blackR = 0.028;
kamorei 3:e8c29cd3ca22 37 float stepL = 0.45, stepR = 0.38;
kamorei 0:500d22d1efeb 38 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:500d22d1efeb 39 float pr, pl;
kamorei 0:500d22d1efeb 40
kamorei 0:500d22d1efeb 41 void stop_point_ver2();
kamorei 3:e8c29cd3ca22 42 void motor_check();
kamorei 0:500d22d1efeb 43
kamorei 0:500d22d1efeb 44 void set_threshold(){ //閾値の初期設定
kamorei 0:500d22d1efeb 45 for( int i = 0; i < 2; i++){
kamorei 0:500d22d1efeb 46 whiteCL[i] = WHITE_CL;
kamorei 0:500d22d1efeb 47 whiteCR[i] = WHITE_CR;
kamorei 0:500d22d1efeb 48 grayCL[i] = GRAY_CL;
kamorei 0:500d22d1efeb 49 grayCR[i] = GRAY_CR;
kamorei 3:e8c29cd3ca22 50 whiteL[i] = WHITE_L;
kamorei 3:e8c29cd3ca22 51 whiteR[i] = WHITE_R;
kamorei 0:500d22d1efeb 52 grayL[i] = GRAY_L;
kamorei 0:500d22d1efeb 53 grayR[i] = GRAY_R;
kamorei 0:500d22d1efeb 54 }
kamorei 0:500d22d1efeb 55 }
kamorei 0:500d22d1efeb 56
kamorei 4:edec6fe32ba9 57 void set_sensor(){ //センサ系
kamorei 4:edec6fe32ba9 58 sensor[0] = sensorL.read();
kamorei 4:edec6fe32ba9 59 sensor[1] = sensorCL.read();
kamorei 4:edec6fe32ba9 60 sensor[2] = sensorCR.read();
kamorei 4:edec6fe32ba9 61 sensor[3] = sensorR.read();
kamorei 4:edec6fe32ba9 62 }
kamorei 4:edec6fe32ba9 63
kamorei 0:500d22d1efeb 64 void go_straight_p(){ //P制御でトレース
kamorei 4:edec6fe32ba9 65 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 4:edec6fe32ba9 66 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 2:47477c2b1925 67 Mrighti = 2;
kamorei 4:edec6fe32ba9 68 Mrightp = (KP * pr) * 1.0f;
kamorei 2:47477c2b1925 69 Mlefti = 2;
kamorei 4:edec6fe32ba9 70 Mleftp = (KP * pl) * 1.0f;
kamorei 4:edec6fe32ba9 71 ledRR = 0b01;
kamorei 3:e8c29cd3ca22 72 ledLL = 0b01;
kamorei 0:500d22d1efeb 73 }
kamorei 0:500d22d1efeb 74
kamorei 0:500d22d1efeb 75 void go_straight_CR(){ //CRのみでトレース
kamorei 4:edec6fe32ba9 76 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 2:47477c2b1925 77 Mrighti = 2;
kamorei 0:500d22d1efeb 78 Mrightp = (KP * pr + 0.3) * 1.0f;
kamorei 2:47477c2b1925 79 Mlefti = 2;
kamorei 0:500d22d1efeb 80 Mleftp = (1.3 - KP * pr) * 1.0f;
kamorei 0:500d22d1efeb 81 ledRR = 0b11;
kamorei 4:edec6fe32ba9 82 ledLL = 0b01;
kamorei 0:500d22d1efeb 83 }
kamorei 0:500d22d1efeb 84
kamorei 1:5c26d3744592 85 void go_straight_CL(){ //CLのみでトレース
kamorei 4:edec6fe32ba9 86 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 2:47477c2b1925 87 Mrighti = 2;
kamorei 1:5c26d3744592 88 Mrightp = (1.3 - KP * pl) * 1.0f;
kamorei 2:47477c2b1925 89 Mlefti = 2;
kamorei 1:5c26d3744592 90 Mleftp = (KP * pl + 0.3) * 1.0f;
kamorei 4:edec6fe32ba9 91 ledRR = 0b01;
kamorei 1:5c26d3744592 92 ledLL = 0b11;
kamorei 1:5c26d3744592 93 }
kamorei 1:5c26d3744592 94
kamorei 0:500d22d1efeb 95 void turn_right(){
kamorei 2:47477c2b1925 96 Mrighti = 1;
kamorei 0:500d22d1efeb 97 Mrightp = 0.05f;
kamorei 2:47477c2b1925 98 Mlefti = 2;
kamorei 3:e8c29cd3ca22 99 Mleftp = 0.5f;
kamorei 4:edec6fe32ba9 100 ledRR = 0b01;
kamorei 3:e8c29cd3ca22 101 ledLL = 0;
kamorei 0:500d22d1efeb 102 }
kamorei 0:500d22d1efeb 103
kamorei 0:500d22d1efeb 104 void turn_left(){
kamorei 2:47477c2b1925 105 Mrighti = 2;
kamorei 3:e8c29cd3ca22 106 Mrightp = 0.5f;
kamorei 2:47477c2b1925 107 Mlefti = 1;
kamorei 0:500d22d1efeb 108 Mleftp = 0.05f;
kamorei 3:e8c29cd3ca22 109 ledRR = 0;
kamorei 3:e8c29cd3ca22 110 ledLL = 0b01;
kamorei 0:500d22d1efeb 111 }
kamorei 0:500d22d1efeb 112
kamorei 4:edec6fe32ba9 113 void turn_right_corner(){
kamorei 4:edec6fe32ba9 114 #ifdef sensor_not_straight
kamorei 3:e8c29cd3ca22 115 while( sensor[2] > blackCR){
kamorei 4:edec6fe32ba9 116 set_sensor();
kamorei 3:e8c29cd3ca22 117 go_straight_p();
kamorei 3:e8c29cd3ca22 118 }
kamorei 4:edec6fe32ba9 119 #endif
kamorei 3:e8c29cd3ca22 120 motor_check();
kamorei 3:e8c29cd3ca22 121 wait(0.2);
kamorei 3:e8c29cd3ca22 122 while( sensor[3] > blackR){ //sensorR > grayR[0]
kamorei 3:e8c29cd3ca22 123 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 124 turn_right();
kamorei 0:500d22d1efeb 125 }
kamorei 0:500d22d1efeb 126 stop_point_ver2();
kamorei 3:e8c29cd3ca22 127 turn_left();
kamorei 3:e8c29cd3ca22 128 wait(0.3);
kamorei 0:500d22d1efeb 129 }
kamorei 0:500d22d1efeb 130
kamorei 4:edec6fe32ba9 131 void turn_left_corner(){
kamorei 4:edec6fe32ba9 132 #ifdef sensor_not_straight
kamorei 3:e8c29cd3ca22 133 while( sensor[1] > blackCL){
kamorei 4:edec6fe32ba9 134 set_sensor();
kamorei 3:e8c29cd3ca22 135 go_straight_p();
kamorei 3:e8c29cd3ca22 136 }
kamorei 4:edec6fe32ba9 137 #endif
kamorei 3:e8c29cd3ca22 138 motor_check();
kamorei 3:e8c29cd3ca22 139 wait(0.2);
kamorei 3:e8c29cd3ca22 140 while( sensor[0] > blackL){ //sensorL > grayL[0]
kamorei 3:e8c29cd3ca22 141 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 142 turn_left();
kamorei 0:500d22d1efeb 143 }
kamorei 0:500d22d1efeb 144 stop_point_ver2();
kamorei 3:e8c29cd3ca22 145 turn_right();
kamorei 3:e8c29cd3ca22 146 wait(0.3);
kamorei 0:500d22d1efeb 147 }
kamorei 0:500d22d1efeb 148
kamorei 4:edec6fe32ba9 149 void motor_check(){ //モータドライバの調子の確認用と単純に直進
kamorei 2:47477c2b1925 150 Mrighti = 2;
kamorei 0:500d22d1efeb 151 Mrightp = 1.0f;
kamorei 2:47477c2b1925 152 Mlefti = 2;
kamorei 0:500d22d1efeb 153 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 154 }
kamorei 0:500d22d1efeb 155
kamorei 0:500d22d1efeb 156 void stop_point_ver2(){
kamorei 0:500d22d1efeb 157 if( Mrighti == 1)
kamorei 0:500d22d1efeb 158 Mrighti = 2;
kamorei 0:500d22d1efeb 159 else if( Mrighti == 2)
kamorei 0:500d22d1efeb 160 Mrighti = 1;
kamorei 0:500d22d1efeb 161 if( Mlefti == 1)
kamorei 0:500d22d1efeb 162 Mlefti = 2;
kamorei 0:500d22d1efeb 163 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 164 Mlefti = 1;
kamorei 4:edec6fe32ba9 165 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 166 Mleftp = 1.0f;
kamorei 3:e8c29cd3ca22 167 wait(0.1);
kamorei 0:500d22d1efeb 168 Mrighti = 0;
kamorei 0:500d22d1efeb 169 Mlefti = 0;
kamorei 0:500d22d1efeb 170 ledRR = 0b11;
kamorei 0:500d22d1efeb 171 ledLL = 0b11;
kamorei 0:500d22d1efeb 172 }
kamorei 0:500d22d1efeb 173
kamorei 0:500d22d1efeb 174 void change_gray(){
kamorei 0:500d22d1efeb 175 if( sensor[1] <= grayCL[0]){ //sensorCL <=grayCL
kamorei 0:500d22d1efeb 176 whiteCR[0] = sensorCR.read();
kamorei 0:500d22d1efeb 177 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 178 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 179 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 0:500d22d1efeb 180 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCR[1] - whiteCR[0]) / whiteCR[1]);
kamorei 3:e8c29cd3ca22 181 ledRR = 0b01;
kamorei 0:500d22d1efeb 182 ledLL = 0;
kamorei 0:500d22d1efeb 183 }
kamorei 0:500d22d1efeb 184 else if( sensor[2] <= grayCR[0]){ //sensorCR <= grayCR
kamorei 0:500d22d1efeb 185 whiteCL[0] = sensorCL.read();
kamorei 0:500d22d1efeb 186 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 187 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 188 grayR[0] = grayR[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 189 grayL[0] = grayL[1] * ( 1 + CHG * (whiteCL[1] - whiteCL[0]) / whiteCL[1]);
kamorei 0:500d22d1efeb 190 ledRR = 0;
kamorei 4:edec6fe32ba9 191 ledLL = 0b01;
kamorei 0:500d22d1efeb 192 }
kamorei 0:500d22d1efeb 193 }
kamorei 0:500d22d1efeb 194
kamorei 3:e8c29cd3ca22 195 void change_gray_ver2(int i){
kamorei 4:edec6fe32ba9 196 if( i == 0){ //Rが白
kamorei 3:e8c29cd3ca22 197 whiteR[0] = sensorR.read();
kamorei 3:e8c29cd3ca22 198 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
kamorei 3:e8c29cd3ca22 199 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
kamorei 3:e8c29cd3ca22 200 grayR[0] = grayR[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
kamorei 3:e8c29cd3ca22 201 grayL[0] = grayL[1] * ( 1 + CHG * (whiteR[1] - whiteR[0]) / whiteR[1]);
kamorei 4:edec6fe32ba9 202 ledCHG = 0b01;
kamorei 4:edec6fe32ba9 203 wait( 0.1);
kamorei 4:edec6fe32ba9 204 ledCHG = 0;
kamorei 3:e8c29cd3ca22 205 }
kamorei 4:edec6fe32ba9 206 else { //Lが白
kamorei 3:e8c29cd3ca22 207 whiteL[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 208 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 209 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 210 grayR[0] = grayR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 211 grayL[0] = grayL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 4:edec6fe32ba9 212 ledCHG = 0b10;
kamorei 4:edec6fe32ba9 213 wait( 0.1);
kamorei 4:edec6fe32ba9 214 ledCHG = 0;
kamorei 3:e8c29cd3ca22 215 }
kamorei 3:e8c29cd3ca22 216 }
kamorei 3:e8c29cd3ca22 217
kamorei 3:e8c29cd3ca22 218 void go_step(){
kamorei 4:edec6fe32ba9 219 if( sensor[0] > stepL){ //左側に段差がある場合
kamorei 4:edec6fe32ba9 220 #ifdef sensor_not_straight
kamorei 3:e8c29cd3ca22 221 motor_check();
kamorei 3:e8c29cd3ca22 222 wait(0.2);
kamorei 4:edec6fe32ba9 223 #endif
kamorei 3:e8c29cd3ca22 224 while( sensor[3] < stepR){
kamorei 4:edec6fe32ba9 225 set_sensor();
kamorei 3:e8c29cd3ca22 226 go_straight_CR();
kamorei 3:e8c29cd3ca22 227 }
kamorei 4:edec6fe32ba9 228 } else if( sensor[3] > stepR){ //右側に段差がある場合
kamorei 4:edec6fe32ba9 229 #ifdef sensor_not_straight
kamorei 3:e8c29cd3ca22 230 motor_check();
kamorei 3:e8c29cd3ca22 231 wait(0.2);
kamorei 4:edec6fe32ba9 232 #endif
kamorei 3:e8c29cd3ca22 233 while( sensor[0] < stepL){
kamorei 4:edec6fe32ba9 234 set_sensor();
kamorei 3:e8c29cd3ca22 235 go_straight_CL();
kamorei 3:e8c29cd3ca22 236 }
kamorei 4:edec6fe32ba9 237 } else
kamorei 4:edec6fe32ba9 238 go_straight_p();
kamorei 4:edec6fe32ba9 239 }
kamorei 4:edec6fe32ba9 240
kamorei 4:edec6fe32ba9 241 void LED_blinky(){ //プログラムの切り替わり確認用
kamorei 4:edec6fe32ba9 242 for( int i = 0; i < 4; i++){
kamorei 4:edec6fe32ba9 243 ledCount = !ledCount;
kamorei 4:edec6fe32ba9 244 wait(0.2);
kamorei 3:e8c29cd3ca22 245 }
kamorei 3:e8c29cd3ca22 246 }
kamorei 3:e8c29cd3ca22 247
kamorei 0:500d22d1efeb 248 int main() {
kamorei 4:edec6fe32ba9 249 #ifdef graychange
kamorei 4:edec6fe32ba9 250 int i = 1; //i == 0ならRがwhite，i == 1ならLがwhite
kamorei 4:edec6fe32ba9 251 #endif
kamorei 4:edec6fe32ba9 252 ledCheck = 1;
kamorei 0:500d22d1efeb 253 set_threshold();
kamorei 4:edec6fe32ba9 254 LED_blinky();
kamorei 0:500d22d1efeb 255 motor_check();
kamorei 3:e8c29cd3ca22 256 wait(0.2);
kamorei 4:edec6fe32ba9 257 while(1){ //スタート～最初の左折カーブ
kamorei 4:edec6fe32ba9 258 set_sensor();
kamorei 4:edec6fe32ba9 259 #ifdef graychange
kamorei 4:edec6fe32ba9 260 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 261 #endif
kamorei 4:edec6fe32ba9 262 if( sensor[3] <= blackR){
kamorei 4:edec6fe32ba9 263 turn_right_corner();
kamorei 4:edec6fe32ba9 264 break;
kamorei 4:edec6fe32ba9 265 } else
kamorei 4:edec6fe32ba9 266 go_straight_p();
kamorei 4:edec6fe32ba9 267 }
kamorei 4:edec6fe32ba9 268 LED_blinky();
kamorei 4:edec6fe32ba9 269 while(1){ //左折カーブ～段差～左折カーブ
kamorei 4:edec6fe32ba9 270 set_sensor();
kamorei 4:edec6fe32ba9 271 if( sensor[0] <= blackL){
kamorei 4:edec6fe32ba9 272 turn_left_corner();
kamorei 4:edec6fe32ba9 273 break;
kamorei 4:edec6fe32ba9 274 } else
kamorei 4:edec6fe32ba9 275 go_step();
kamorei 4:edec6fe32ba9 276 }
kamorei 4:edec6fe32ba9 277 LED_blinky();
kamorei 4:edec6fe32ba9 278 #ifdef graychange
kamorei 4:edec6fe32ba9 279 i = 0;
kamorei 4:edec6fe32ba9 280 #endif
kamorei 4:edec6fe32ba9 281 while(1){ //左折カーブ～左折カーブ
kamorei 4:edec6fe32ba9 282 set_sensor();
kamorei 4:edec6fe32ba9 283 #ifdef graychange
kamorei 4:edec6fe32ba9 284 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 285 #endif
kamorei 4:edec6fe32ba9 286 if( sensor[0] <= blackL){
kamorei 4:edec6fe32ba9 287 turn_left_corner();
kamorei 4:edec6fe32ba9 288 break;
kamorei 4:edec6fe32ba9 289 } else
kamorei 4:edec6fe32ba9 290 go_straight_p();
kamorei 4:edec6fe32ba9 291 }
kamorei 4:edec6fe32ba9 292 LED_blinky();
kamorei 4:edec6fe32ba9 293 while(1){ //左折カーブ～トンネル手前のマーク
kamorei 4:edec6fe32ba9 294 set_sensor();
kamorei 4:edec6fe32ba9 295 if( sensor[0] <= blackL && sensor[3] <= blackR)
kamorei 3:e8c29cd3ca22 296 break;
kamorei 4:edec6fe32ba9 297 else
kamorei 4:edec6fe32ba9 298 go_straight_CR();
kamorei 4:edec6fe32ba9 299 }
kamorei 4:edec6fe32ba9 300 LED_blinky();
kamorei 4:edec6fe32ba9 301 while(1){ //トンネル手前のマーク～Ｔ字
kamorei 4:edec6fe32ba9 302 set_sensor();
kamorei 4:edec6fe32ba9 303 if( sensor[0] <= blackL && sensor[3] <= blackR){
kamorei 4:edec6fe32ba9 304 turn_right_corner();
kamorei 4:edec6fe32ba9 305 break;
kamorei 4:edec6fe32ba9 306 } else
kamorei 4:edec6fe32ba9 307 go_straight_p();
kamorei 4:edec6fe32ba9 308 }
kamorei 4:edec6fe32ba9 309 LED_blinky();
kamorei 4:edec6fe32ba9 310 #ifdef graychange
kamorei 4:edec6fe32ba9 311 i = 0;
kamorei 4:edec6fe32ba9 312 #endif
kamorei 4:edec6fe32ba9 313 while(1){ //Ｔ字～半円終わり
kamorei 4:edec6fe32ba9 314 set_sensor();
kamorei 4:edec6fe32ba9 315 #ifdef graychange
kamorei 4:edec6fe32ba9 316 change_gray_ver2(i);
kamorei 4:edec6fe32ba9 317 #endif
kamorei 4:edec6fe32ba9 318 if( sensor[0] <= blackL){
kamorei 4:edec6fe32ba9 319 turn_left_corner();
kamorei 4:edec6fe32ba9 320 break;
kamorei 4:edec6fe32ba9 321 } else
kamorei 4:edec6fe32ba9 322 go_straight_p();
kamorei 4:edec6fe32ba9 323 }
kamorei 4:edec6fe32ba9 324 LED_blinky();
kamorei 4:edec6fe32ba9 325 while(1){ //半円終わり～ゴール
kamorei 4:edec6fe32ba9 326 set_sensor();
kamorei 4:edec6fe32ba9 327 if( sensor[0] <= blackL)
kamorei 4:edec6fe32ba9 328 turn_left_corner();
kamorei 4:edec6fe32ba9 329 else if( sensor[3] <= blackR)
kamorei 4:edec6fe32ba9 330 turn_right_corner();
kamorei 4:edec6fe32ba9 331 else if( sensor[0] <= blackL && sensor[3] <= blackR){
kamorei 4:edec6fe32ba9 332 stop_point_ver2();
kamorei 4:edec6fe32ba9 333 break;
kamorei 4:edec6fe32ba9 334 } else
kamorei 4:edec6fe32ba9 335 go_straight_p();
kamorei 4:edec6fe32ba9 336 }
kamorei 4:edec6fe32ba9 337 LED_blinky();
kamorei 4:edec6fe32ba9 338 }
kamorei 0:500d22d1efeb 339
kamorei 3:e8c29cd3ca22 340 // change_gray_ver2( i);
kamorei 3:e8c29cd3ca22 341 /* if( sensor[0] <= blackL){
kamorei 4:edec6fe32ba9 342 turn_left_corner();
kamorei 3:e8c29cd3ca22 343 // change_gray();
kamorei 3:e8c29cd3ca22 344 } else if( sensor[3] <= blackR){
kamorei 4:edec6fe32ba9 345 turn_right_corner();
kamorei 3:e8c29cd3ca22 346 // change_gray();
kamorei 3:e8c29cd3ca22 347 } else*/
kamorei 3:e8c29cd3ca22 348 // go_straight_p();
kamorei 1:5c26d3744592 349 // go_straight_CR();
kamorei 1:5c26d3744592 350 // go_straight_CL();
kamorei 2:47477c2b1925 351 /*以下メモ
kamorei 2:47477c2b1925 352 ・段差
kamorei 2:47477c2b1925 353 if( sensor[0] > 閾値){
kamorei 2:47477c2b1925 354 wait(0.5); //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 355 sensor[2] = read.sensorCR;
kamorei 2:47477c2b1925 356 go_straight_CR();
kamorei 2:47477c2b1925 357 } else if( sensor[3] > 閾値){
kamorei 2:47477c2b1925 358 wait(0.5) //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 359 sensor[1] = read.sensorCL;
kamorei 2:47477c2b1925 360 go_straight_CL();
kamorei 2:47477c2b1925 361 }
kamorei 2:47477c2b1925 362
kamorei 2:47477c2b1925 363 ・プログラムの切り替え
kamorei 2:47477c2b1925 364 回転動作をしたら（完了したら）カウント
kamorei 2:47477c2b1925 365 ⇒回転を一発で決める必要有
kamorei 2:47477c2b1925 366 */