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@3:e8c29cd3ca22, 2018-11-20 (annotated)

Committer:
kamorei
Date:
Tue Nov 20 16:20:44 2018 +0000
Revision:
3:e8c29cd3ca22
Parent:
2:47477c2b1925
Child:
4:edec6fe32ba9
11/18???

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 3:e8c29cd3ca22 14 //#define DEBUG
kamorei 0:500d22d1efeb 15
kamorei 0:500d22d1efeb 16 DigitalOut ledL( PTB8);
kamorei 0:500d22d1efeb 17 DigitalOut ledR( PTE5);
kamorei 0:500d22d1efeb 18 DigitalOut ledC( PTE2); //動作check
kamorei 0:500d22d1efeb 19 BusOut ledLL( PTB8, PTB9);
kamorei 0:500d22d1efeb 20 BusOut ledRR( PTE4, PTE5);
kamorei 3:e8c29cd3ca22 21 AnalogIn sensorR( PTC2);
kamorei 0:500d22d1efeb 22 AnalogIn sensorL( PTB3);
kamorei 0:500d22d1efeb 23 AnalogIn sensorCR( PTB0);
kamorei 0:500d22d1efeb 24 AnalogIn sensorCL( PTB2);
kamorei 0:500d22d1efeb 25 //モータ1
kamorei 0:500d22d1efeb 26 BusOut Mlefti(PTA1, PTA2);
kamorei 0:500d22d1efeb 27 PwmOut Mleftp(PTD4);
kamorei 0:500d22d1efeb 28 //モータ2
kamorei 0:500d22d1efeb 29 BusOut Mrighti(PTC0, PTC7);
kamorei 0:500d22d1efeb 30 PwmOut Mrightp(PTA12);
kamorei 0:500d22d1efeb 31
kamorei 0:500d22d1efeb 32 float whiteCL[2], whiteCR[2], grayCL[2], grayCR[2]; //[1]が初期設定の基準値，[0]が変動する閾値
kamorei 3:e8c29cd3ca22 33 float whiteL[2], whiteR[2], grayL[2], grayR[2];
kamorei 3:e8c29cd3ca22 34 float blackCL = 0.18, blackCR = 0.1; //閾値の跡地
kamorei 3:e8c29cd3ca22 35 float blackL = 0.028, blackR = 0.028;
kamorei 3:e8c29cd3ca22 36 float stepL = 0.45, stepR = 0.38;
kamorei 0:500d22d1efeb 37 float sensor[4]; //sensor[0]:sensorL ... sensor[3]:sensorR
kamorei 0:500d22d1efeb 38 float pr, pl;
kamorei 0:500d22d1efeb 39
kamorei 0:500d22d1efeb 40 void stop_point_ver2();
kamorei 3:e8c29cd3ca22 41 void motor_check();
kamorei 0:500d22d1efeb 42
kamorei 0:500d22d1efeb 43 void set_threshold(){ //閾値の初期設定
kamorei 0:500d22d1efeb 44 for( int i = 0; i < 2; i++){
kamorei 0:500d22d1efeb 45 whiteCL[i] = WHITE_CL;
kamorei 0:500d22d1efeb 46 whiteCR[i] = WHITE_CR;
kamorei 0:500d22d1efeb 47 grayCL[i] = GRAY_CL;
kamorei 0:500d22d1efeb 48 grayCR[i] = GRAY_CR;
kamorei 3:e8c29cd3ca22 49 whiteL[i] = WHITE_L;
kamorei 3:e8c29cd3ca22 50 whiteR[i] = WHITE_R;
kamorei 0:500d22d1efeb 51 grayL[i] = GRAY_L;
kamorei 0:500d22d1efeb 52 grayR[i] = GRAY_R;
kamorei 0:500d22d1efeb 53 }
kamorei 0:500d22d1efeb 54 }
kamorei 0:500d22d1efeb 55
kamorei 0:500d22d1efeb 56 void go_straight_p(){ //P制御でトレース
kamorei 2:47477c2b1925 57 Mrighti = 2;
kamorei 3:e8c29cd3ca22 58 Mrightp = (KP * pr - 0.2) * 1.0f;
kamorei 2:47477c2b1925 59 Mlefti = 2;
kamorei 3:e8c29cd3ca22 60 Mleftp = (KP * pl - 0.2) * 1.0f;
kamorei 3:e8c29cd3ca22 61 ledRR = 0b10;
kamorei 3:e8c29cd3ca22 62 ledLL = 0b01;
kamorei 0:500d22d1efeb 63 }
kamorei 0:500d22d1efeb 64
kamorei 0:500d22d1efeb 65 void go_straight_CR(){ //CRのみでトレース
kamorei 2:47477c2b1925 66 Mrighti = 2;
kamorei 0:500d22d1efeb 67 Mrightp = (KP * pr + 0.3) * 1.0f;
kamorei 2:47477c2b1925 68 Mlefti = 2;
kamorei 0:500d22d1efeb 69 Mleftp = (1.3 - KP * pr) * 1.0f;
kamorei 0:500d22d1efeb 70 ledRR = 0b11;
kamorei 0:500d22d1efeb 71 ledLL = 0b10;
kamorei 0:500d22d1efeb 72 }
kamorei 0:500d22d1efeb 73
kamorei 1:5c26d3744592 74 void go_straight_CL(){ //CLのみでトレース
kamorei 2:47477c2b1925 75 Mrighti = 2;
kamorei 1:5c26d3744592 76 Mrightp = (1.3 - KP * pl) * 1.0f;
kamorei 2:47477c2b1925 77 Mlefti = 2;
kamorei 1:5c26d3744592 78 Mleftp = (KP * pl + 0.3) * 1.0f;
kamorei 1:5c26d3744592 79 ledRR = 0b10;
kamorei 1:5c26d3744592 80 ledLL = 0b11;
kamorei 1:5c26d3744592 81 }
kamorei 1:5c26d3744592 82
kamorei 0:500d22d1efeb 83 void turn_right(){
kamorei 2:47477c2b1925 84 Mrighti = 1;
kamorei 0:500d22d1efeb 85 Mrightp = 0.05f;
kamorei 2:47477c2b1925 86 Mlefti = 2;
kamorei 3:e8c29cd3ca22 87 Mleftp = 0.5f;
kamorei 3:e8c29cd3ca22 88 ledRR = 0b10;
kamorei 3:e8c29cd3ca22 89 ledLL = 0;
kamorei 0:500d22d1efeb 90 }
kamorei 0:500d22d1efeb 91
kamorei 0:500d22d1efeb 92 void turn_left(){
kamorei 2:47477c2b1925 93 Mrighti = 2;
kamorei 3:e8c29cd3ca22 94 Mrightp = 0.5f;
kamorei 2:47477c2b1925 95 Mlefti = 1;
kamorei 0:500d22d1efeb 96 Mleftp = 0.05f;
kamorei 3:e8c29cd3ca22 97 ledRR = 0;
kamorei 3:e8c29cd3ca22 98 ledLL = 0b01;
kamorei 0:500d22d1efeb 99 }
kamorei 0:500d22d1efeb 100
kamorei 0:500d22d1efeb 101 void turn_right_ver2(){
kamorei 3:e8c29cd3ca22 102 while( sensor[2] > blackCR){
kamorei 3:e8c29cd3ca22 103 sensor[1] = sensorCL.read();
kamorei 3:e8c29cd3ca22 104 sensor[2] = sensorCR.read();
kamorei 3:e8c29cd3ca22 105 sensor[3] = sensorR.read();
kamorei 3:e8c29cd3ca22 106 go_straight_p();
kamorei 3:e8c29cd3ca22 107 }
kamorei 3:e8c29cd3ca22 108 motor_check();
kamorei 3:e8c29cd3ca22 109 wait(0.2);
kamorei 3:e8c29cd3ca22 110 while( sensor[3] > blackR){ //sensorR > grayR[0]
kamorei 3:e8c29cd3ca22 111 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 112 turn_right();
kamorei 0:500d22d1efeb 113 }
kamorei 0:500d22d1efeb 114 stop_point_ver2();
kamorei 3:e8c29cd3ca22 115 turn_left();
kamorei 3:e8c29cd3ca22 116 wait(0.3);
kamorei 0:500d22d1efeb 117 }
kamorei 0:500d22d1efeb 118
kamorei 0:500d22d1efeb 119 void turn_left_ver2(){
kamorei 3:e8c29cd3ca22 120 while( sensor[1] > blackCL){
kamorei 3:e8c29cd3ca22 121 sensor[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 122 sensor[1] = sensorCL.read();
kamorei 3:e8c29cd3ca22 123 sensor[2] = sensorCR.read();
kamorei 3:e8c29cd3ca22 124 go_straight_p();
kamorei 3:e8c29cd3ca22 125 }
kamorei 3:e8c29cd3ca22 126 motor_check();
kamorei 3:e8c29cd3ca22 127 wait(0.2);
kamorei 3:e8c29cd3ca22 128 while( sensor[0] > blackL){ //sensorL > grayL[0]
kamorei 3:e8c29cd3ca22 129 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 130 turn_left();
kamorei 0:500d22d1efeb 131 }
kamorei 0:500d22d1efeb 132 stop_point_ver2();
kamorei 3:e8c29cd3ca22 133 turn_right();
kamorei 3:e8c29cd3ca22 134 wait(0.3);
kamorei 0:500d22d1efeb 135 }
kamorei 0:500d22d1efeb 136
kamorei 0:500d22d1efeb 137 void motor_check(){ //モータドライバの調子の確認用
kamorei 2:47477c2b1925 138 Mrighti = 2;
kamorei 0:500d22d1efeb 139 Mrightp = 1.0f;
kamorei 2:47477c2b1925 140 Mlefti = 2;
kamorei 0:500d22d1efeb 141 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 142 }
kamorei 0:500d22d1efeb 143
kamorei 0:500d22d1efeb 144 void stop_point(){
kamorei 2:47477c2b1925 145 Mrighti = 1;
kamorei 0:500d22d1efeb 146 Mrightp = 1.0f;
kamorei 2:47477c2b1925 147 Mlefti = 1;
kamorei 0:500d22d1efeb 148 Mleftp = 1.0f;
kamorei 0:500d22d1efeb 149 wait(0.05);
kamorei 0:500d22d1efeb 150 Mrighti = 0;
kamorei 0:500d22d1efeb 151 Mlefti = 0;
kamorei 0:500d22d1efeb 152 ledRR = 0b11;
kamorei 0:500d22d1efeb 153 ledLL = 0b11;
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 Mrightp = 1.0f;
kamorei 0:500d22d1efeb 162 if( Mlefti == 1)
kamorei 0:500d22d1efeb 163 Mlefti = 2;
kamorei 0:500d22d1efeb 164 else if( Mlefti == 2)
kamorei 0:500d22d1efeb 165 Mlefti = 1;
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 0:500d22d1efeb 191 ledLL = 0b10;
kamorei 0:500d22d1efeb 192 }
kamorei 0:500d22d1efeb 193 }
kamorei 0:500d22d1efeb 194
kamorei 3:e8c29cd3ca22 195 void change_gray_ver2(int i){
kamorei 3:e8c29cd3ca22 196 if( i == 0){
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 3:e8c29cd3ca22 202 ledRR = 0b01;
kamorei 3:e8c29cd3ca22 203 ledLL = 0;
kamorei 3:e8c29cd3ca22 204 }
kamorei 3:e8c29cd3ca22 205 else {
kamorei 3:e8c29cd3ca22 206 whiteL[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 207 grayCR[0] = grayCR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 208 grayCL[0] = grayCL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 209 grayR[0] = grayR[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 210 grayL[0] = grayL[1] * ( 1 + CHG * (whiteL[1] - whiteL[0]) / whiteL[1]);
kamorei 3:e8c29cd3ca22 211 ledRR = 0;
kamorei 3:e8c29cd3ca22 212 ledLL = 0b01;
kamorei 3:e8c29cd3ca22 213 }
kamorei 3:e8c29cd3ca22 214 }
kamorei 3:e8c29cd3ca22 215
kamorei 3:e8c29cd3ca22 216 void go_step(){
kamorei 3:e8c29cd3ca22 217 if( sensor[0] > stepL){
kamorei 3:e8c29cd3ca22 218 motor_check();
kamorei 3:e8c29cd3ca22 219 wait(0.2);
kamorei 3:e8c29cd3ca22 220 while( sensor[3] < stepR){
kamorei 3:e8c29cd3ca22 221 sensor[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 222 sensor[2] = sensorCR.read();
kamorei 3:e8c29cd3ca22 223 sensor[3] = sensorR.read();
kamorei 3:e8c29cd3ca22 224 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 3:e8c29cd3ca22 225 go_straight_CR();
kamorei 3:e8c29cd3ca22 226 if( sensor[0] < stepL)
kamorei 3:e8c29cd3ca22 227 stop_point_ver2();
kamorei 3:e8c29cd3ca22 228 }
kamorei 3:e8c29cd3ca22 229 } else if( sensor[3] > stepR){
kamorei 3:e8c29cd3ca22 230 motor_check();
kamorei 3:e8c29cd3ca22 231 wait(0.2);
kamorei 3:e8c29cd3ca22 232 while( sensor[0] < stepL){
kamorei 3:e8c29cd3ca22 233 sensor[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 234 sensor[1] = sensorCL.read();
kamorei 3:e8c29cd3ca22 235 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 3:e8c29cd3ca22 236 go_straight_CL();
kamorei 3:e8c29cd3ca22 237 }
kamorei 3:e8c29cd3ca22 238 stop_point_ver2();
kamorei 3:e8c29cd3ca22 239 } else{
kamorei 3:e8c29cd3ca22 240 sensor[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 241 sensor[1] = sensorCL.read();
kamorei 3:e8c29cd3ca22 242 sensor[2] = sensorCR.read();
kamorei 3:e8c29cd3ca22 243 sensor[3] = sensorR.read();
kamorei 3:e8c29cd3ca22 244 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 3:e8c29cd3ca22 245 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 3:e8c29cd3ca22 246 go_straight_p();
kamorei 3:e8c29cd3ca22 247 }
kamorei 3:e8c29cd3ca22 248
kamorei 3:e8c29cd3ca22 249 }
kamorei 3:e8c29cd3ca22 250
kamorei 0:500d22d1efeb 251 int main() {
kamorei 3:e8c29cd3ca22 252 // int i = 1;
kamorei 0:500d22d1efeb 253 ledC = 1;
kamorei 0:500d22d1efeb 254 set_threshold();
kamorei 0:500d22d1efeb 255 motor_check();
kamorei 3:e8c29cd3ca22 256 wait(0.2);
kamorei 3:e8c29cd3ca22 257 /* while( 1){
kamorei 3:e8c29cd3ca22 258 sensor[0] = sensorL.read();
kamorei 3:e8c29cd3ca22 259 sensor[1] = sensorCL.read();
kamorei 3:e8c29cd3ca22 260 sensor[2] = sensorCR.read();
kamorei 3:e8c29cd3ca22 261 sensor[3] = sensorR.read();
kamorei 3:e8c29cd3ca22 262 go_straight_p();
kamorei 3:e8c29cd3ca22 263 if( sensor[3] > stepR)
kamorei 3:e8c29cd3ca22 264 break;
kamorei 3:e8c29cd3ca22 265 }*/
kamorei 0:500d22d1efeb 266 while(1) {
kamorei 0:500d22d1efeb 267 sensor[0] = sensorL.read();
kamorei 0:500d22d1efeb 268 sensor[1] = sensorCL.read();
kamorei 0:500d22d1efeb 269 sensor[2] = sensorCR.read();
kamorei 0:500d22d1efeb 270 sensor[3] = sensorR.read();
kamorei 0:500d22d1efeb 271
kamorei 0:500d22d1efeb 272 pl = (sensor[1] - grayCL[0]) / (whiteCL[0] - grayCL[0]);
kamorei 0:500d22d1efeb 273 pr = (sensor[2] - grayCR[0]) / (whiteCR[0] - grayCR[0]);
kamorei 0:500d22d1efeb 274
kamorei 3:e8c29cd3ca22 275 // change_gray_ver2( i);
kamorei 3:e8c29cd3ca22 276 /* if( sensor[0] <= blackL){
kamorei 0:500d22d1efeb 277 turn_left_ver2();
kamorei 3:e8c29cd3ca22 278 // change_gray();
kamorei 0:500d22d1efeb 279 #ifdef DEBUG
kamorei 0:500d22d1efeb 280 break;
kamorei 0:500d22d1efeb 281 #endif
kamorei 3:e8c29cd3ca22 282 } else if( sensor[3] <= blackR){
kamorei 0:500d22d1efeb 283 turn_right_ver2();
kamorei 3:e8c29cd3ca22 284 // change_gray();
kamorei 0:500d22d1efeb 285 #ifdef DEBUG
kamorei 0:500d22d1efeb 286 break;
kamorei 0:500d22d1efeb 287 #endif
kamorei 3:e8c29cd3ca22 288 } else*/
kamorei 3:e8c29cd3ca22 289 // go_straight_p();
kamorei 1:5c26d3744592 290 // go_straight_CR();
kamorei 1:5c26d3744592 291 // go_straight_CL();
kamorei 3:e8c29cd3ca22 292 go_step();
kamorei 3:e8c29cd3ca22 293 if( sensor[0] < grayL[0] && sensor[3] < grayR[0]){
kamorei 0:500d22d1efeb 294 stop_point_ver2();
kamorei 0:500d22d1efeb 295 break;
kamorei 0:500d22d1efeb 296 }
kamorei 0:500d22d1efeb 297 }
kamorei 2:47477c2b1925 298 }
kamorei 2:47477c2b1925 299
kamorei 2:47477c2b1925 300 /*以下メモ
kamorei 2:47477c2b1925 301 ・段差
kamorei 2:47477c2b1925 302 if( sensor[0] > 閾値){
kamorei 2:47477c2b1925 303 wait(0.5); //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 304 sensor[2] = read.sensorCR;
kamorei 2:47477c2b1925 305 go_straight_CR();
kamorei 2:47477c2b1925 306 } else if( sensor[3] > 閾値){
kamorei 2:47477c2b1925 307 wait(0.5) //片輪上がるまで待ちたい
kamorei 2:47477c2b1925 308 sensor[1] = read.sensorCL;
kamorei 2:47477c2b1925 309 go_straight_CL();
kamorei 2:47477c2b1925 310 }
kamorei 2:47477c2b1925 311
kamorei 2:47477c2b1925 312 ・プログラムの切り替え
kamorei 2:47477c2b1925 313 回転動作をしたら（完了したら）カウント
kamorei 2:47477c2b1925 314 ⇒回転を一発で決める必要有
kamorei 2:47477c2b1925 315 */