圭祐 杉浦 / Mbed 2 deprecated final-center2

候補

Dependencies:   MotorDriver_SU mbed

center2.cpp@0:41b88efe25c9, 2014-12-01 (annotated)

Committer:
12f2027a
Date:
Mon Dec 01 04:46:42 2014 +0000
Revision:
0:41b88efe25c9
center2 ??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12f2027a 0:41b88efe25c9 1 #include "mbed.h"
12f2027a 0:41b88efe25c9 2 #include "MotorDriver_SU.h"
12f2027a 0:41b88efe25c9 3
12f2027a 0:41b88efe25c9 4
12f2027a 0:41b88efe25c9 5 DigitalIn sw(P0_1, PullUp);
12f2027a 0:41b88efe25c9 6 DigitalOut led(P1_28);
12f2027a 0:41b88efe25c9 7 AnalogIn sw1(P0_22);
12f2027a 0:41b88efe25c9 8 AnalogIn sw2(P0_12);
12f2027a 0:41b88efe25c9 9 AnalogIn sw3(P0_16);
12f2027a 0:41b88efe25c9 10 AnalogIn sw4(P0_11);
12f2027a 0:41b88efe25c9 11 AnalogIn sw5(P0_13);
12f2027a 0:41b88efe25c9 12
12f2027a 0:41b88efe25c9 13 MotorDriver_SU motor(MOTOR_DOUBLE);
12f2027a 0:41b88efe25c9 14 MotorDriver_SU lift(MOTOR_SINGLE);
12f2027a 0:41b88efe25c9 15
12f2027a 0:41b88efe25c9 16
12f2027a 0:41b88efe25c9 17 int main( ){
12f2027a 0:41b88efe25c9 18 float hoge1;
12f2027a 0:41b88efe25c9 19 float hoge2;
12f2027a 0:41b88efe25c9 20 float hoge3;
12f2027a 0:41b88efe25c9 21 float hoge4;
12f2027a 0:41b88efe25c9 22 float hoge5;
12f2027a 0:41b88efe25c9 23 int position = 0;
12f2027a 0:41b88efe25c9 24 int AI = 0;//カーブの方向判定
12f2027a 0:41b88efe25c9 25 int updown = 0;//リフトの向き判定
12f2027a 0:41b88efe25c9 26 int R = 0;
12f2027a 0:41b88efe25c9 27 int L = 0;
12f2027a 0:41b88efe25c9 28
12f2027a 0:41b88efe25c9 29 //11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
12f2027a 0:41b88efe25c9 30 while(1){
12f2027a 0:41b88efe25c9 31
12f2027a 0:41b88efe25c9 32 hoge1 = sw1.read();
12f2027a 0:41b88efe25c9 33 hoge2 = sw2.read();
12f2027a 0:41b88efe25c9 34 hoge3 = sw3.read();
12f2027a 0:41b88efe25c9 35 hoge4 = sw4.read();
12f2027a 0:41b88efe25c9 36 hoge5 = sw5.read();
12f2027a 0:41b88efe25c9 37
12f2027a 0:41b88efe25c9 38 while(sw == 0){
12f2027a 0:41b88efe25c9 39 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 40 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 41 }
12f2027a 0:41b88efe25c9 42 /*黒＿白＿白＿白＿白*/
12f2027a 0:41b88efe25c9 43 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 44 motor.Drive(0, CW, 500);
12f2027a 0:41b88efe25c9 45 motor.Drive(1, CW, 2000);
12f2027a 0:41b88efe25c9 46 }
12f2027a 0:41b88efe25c9 47 /*白＿黒＿白＿白＿白*/
12f2027a 0:41b88efe25c9 48 else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 49 motor.Drive(0, CW, 1600);
12f2027a 0:41b88efe25c9 50 motor.Drive(1, CW, 2000);
12f2027a 0:41b88efe25c9 51 }
12f2027a 0:41b88efe25c9 52 /*白＿黒＿黒＿白＿白*/
12f2027a 0:41b88efe25c9 53 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 54 motor.Drive(0, CW, 1800);
12f2027a 0:41b88efe25c9 55 motor.Drive(1, CW, 2000);
12f2027a 0:41b88efe25c9 56 }
12f2027a 0:41b88efe25c9 57 /*白＿白＿黒＿白＿白*/
12f2027a 0:41b88efe25c9 58 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 59 motor.Drive(0, CW, 2000);
12f2027a 0:41b88efe25c9 60 motor.Drive(1, CW, 2000);
12f2027a 0:41b88efe25c9 61 }
12f2027a 0:41b88efe25c9 62
12f2027a 0:41b88efe25c9 63 /*白＿白＿黒＿黒＿白*/
12f2027a 0:41b88efe25c9 64 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 65 motor.Drive(0, CW, 2000);
12f2027a 0:41b88efe25c9 66 motor.Drive(1, CW, 1800);
12f2027a 0:41b88efe25c9 67 }
12f2027a 0:41b88efe25c9 68 /*白＿白＿白＿黒＿白*/
12f2027a 0:41b88efe25c9 69 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 70 motor.Drive(0, CW, 2000);
12f2027a 0:41b88efe25c9 71 motor.Drive(1, CW, 1600);
12f2027a 0:41b88efe25c9 72 }
12f2027a 0:41b88efe25c9 73 /*白＿白＿白＿白＿黒*/
12f2027a 0:41b88efe25c9 74 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 75 motor.Drive(0, CW, 2000);
12f2027a 0:41b88efe25c9 76 motor.Drive(1, CW, 500);
12f2027a 0:41b88efe25c9 77 }
12f2027a 0:41b88efe25c9 78
12f2027a 0:41b88efe25c9 79 else if(sw1 > 0.5 && sw5 <= 0.3){
12f2027a 0:41b88efe25c9 80 break;
12f2027a 0:41b88efe25c9 81 }
12f2027a 0:41b88efe25c9 82 }
12f2027a 0:41b88efe25c9 83
12f2027a 0:41b88efe25c9 84 //2222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222
12f2027a 0:41b88efe25c9 85
12f2027a 0:41b88efe25c9 86
12f2027a 0:41b88efe25c9 87 while(1){
12f2027a 0:41b88efe25c9 88
12f2027a 0:41b88efe25c9 89 hoge1 = sw1.read();
12f2027a 0:41b88efe25c9 90 hoge2 = sw2.read();
12f2027a 0:41b88efe25c9 91 hoge3 = sw3.read();
12f2027a 0:41b88efe25c9 92 hoge4 = sw4.read();
12f2027a 0:41b88efe25c9 93 hoge5 = sw5.read();
12f2027a 0:41b88efe25c9 94
12f2027a 0:41b88efe25c9 95 while(sw == 0){
12f2027a 0:41b88efe25c9 96 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 97 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 98 }
12f2027a 0:41b88efe25c9 99 /*黒＿白＿白＿白＿白*/
12f2027a 0:41b88efe25c9 100 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 101 motor.Drive(0, CW, 500);
12f2027a 0:41b88efe25c9 102 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 103 }
12f2027a 0:41b88efe25c9 104 /*黒＿白＿白*/
12f2027a 0:41b88efe25c9 105 else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){
12f2027a 0:41b88efe25c9 106 motor.Drive(0, CW, 600);
12f2027a 0:41b88efe25c9 107 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 108 }
12f2027a 0:41b88efe25c9 109 /*黒＿黒＿白*/
12f2027a 0:41b88efe25c9 110 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 111 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 112 motor.Drive(1, CW, 200);
12f2027a 0:41b88efe25c9 113 }
12f2027a 0:41b88efe25c9 114 /*白＿黒＿白*/
12f2027a 0:41b88efe25c9 115 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 116 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 117 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 118 }
12f2027a 0:41b88efe25c9 119
12f2027a 0:41b88efe25c9 120 /*白＿黒＿黒*/
12f2027a 0:41b88efe25c9 121 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 122 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 123 motor.Drive(1, CW, 200);
12f2027a 0:41b88efe25c9 124 }
12f2027a 0:41b88efe25c9 125 /*白＿白＿黒*/
12f2027a 0:41b88efe25c9 126 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 127 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 128 motor.Drive(1, CW, 600);
12f2027a 0:41b88efe25c9 129 }
12f2027a 0:41b88efe25c9 130 /*白＿白＿白＿白＿黒*/
12f2027a 0:41b88efe25c9 131 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 132 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 133 motor.Drive(1, CW, 500);
12f2027a 0:41b88efe25c9 134 }
12f2027a 0:41b88efe25c9 135
12f2027a 0:41b88efe25c9 136 else if(sw1 <= 0.6 ){
12f2027a 0:41b88efe25c9 137 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 138 motor.Drive(1, CCW, 1000);
12f2027a 0:41b88efe25c9 139 wait(0.225);
12f2027a 0:41b88efe25c9 140 break;
12f2027a 0:41b88efe25c9 141 }
12f2027a 0:41b88efe25c9 142 }
12f2027a 0:41b88efe25c9 143
12f2027a 0:41b88efe25c9 144 //3333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333
12f2027a 0:41b88efe25c9 145
12f2027a 0:41b88efe25c9 146
12f2027a 0:41b88efe25c9 147 while(1){
12f2027a 0:41b88efe25c9 148
12f2027a 0:41b88efe25c9 149 hoge1 = sw1.read();
12f2027a 0:41b88efe25c9 150 hoge2 = sw2.read();
12f2027a 0:41b88efe25c9 151 hoge3 = sw3.read();
12f2027a 0:41b88efe25c9 152 hoge4 = sw4.read();
12f2027a 0:41b88efe25c9 153 hoge5 = sw5.read();
12f2027a 0:41b88efe25c9 154
12f2027a 0:41b88efe25c9 155 while(sw == 0){
12f2027a 0:41b88efe25c9 156 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 157 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 158 }
12f2027a 0:41b88efe25c9 159 /*黒＿白＿白＿白＿白*/
12f2027a 0:41b88efe25c9 160 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 161 motor.Drive(0, CW, 300);
12f2027a 0:41b88efe25c9 162 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 163 }
12f2027a 0:41b88efe25c9 164 /*白＿白＿白*/
12f2027a 0:41b88efe25c9 165 else if(sw2 <= 0.6 && sw3 > 0.6 && sw4 > 0.6){
12f2027a 0:41b88efe25c9 166 motor.Drive(0, CW, 400);
12f2027a 0:41b88efe25c9 167 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 168 }
12f2027a 0:41b88efe25c9 169 /*黒＿白＿白*/
12f2027a 0:41b88efe25c9 170 else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){
12f2027a 0:41b88efe25c9 171 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 172 motor.Drive(1, CW, 600);
12f2027a 0:41b88efe25c9 173 }
12f2027a 0:41b88efe25c9 174 /*黒＿黒＿白*/
12f2027a 0:41b88efe25c9 175 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 176 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 177 motor.Drive(1, CW, 500);
12f2027a 0:41b88efe25c9 178 }
12f2027a 0:41b88efe25c9 179 /*白＿黒＿白*/
12f2027a 0:41b88efe25c9 180 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 181 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 182 motor.Drive(1, CW, 400);
12f2027a 0:41b88efe25c9 183 }
12f2027a 0:41b88efe25c9 184
12f2027a 0:41b88efe25c9 185 /*白＿黒＿黒*/
12f2027a 0:41b88efe25c9 186 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 187 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 188 motor.Drive(1, CW, 300);
12f2027a 0:41b88efe25c9 189 }
12f2027a 0:41b88efe25c9 190 /*白＿白＿黒*/
12f2027a 0:41b88efe25c9 191 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 192 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 193 motor.Drive(1, CW, 300);
12f2027a 0:41b88efe25c9 194 }
12f2027a 0:41b88efe25c9 195 /*白＿白＿白＿白＿黒*/
12f2027a 0:41b88efe25c9 196 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 197 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 198 motor.Drive(1, CW, 300);
12f2027a 0:41b88efe25c9 199 }
12f2027a 0:41b88efe25c9 200
12f2027a 0:41b88efe25c9 201 else if(sw1 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 202 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 203 motor.Drive(1, CCW, 1000);
12f2027a 0:41b88efe25c9 204 }
12f2027a 0:41b88efe25c9 205
12f2027a 0:41b88efe25c9 206 else if(sw1 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 207 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 208 motor.Drive(1, CW, 500);
12f2027a 0:41b88efe25c9 209 wait(0.2);
12f2027a 0:41b88efe25c9 210 break;
12f2027a 0:41b88efe25c9 211 }
12f2027a 0:41b88efe25c9 212
12f2027a 0:41b88efe25c9 213 }
12f2027a 0:41b88efe25c9 214
12f2027a 0:41b88efe25c9 215
12f2027a 0:41b88efe25c9 216
12f2027a 0:41b88efe25c9 217 //444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444
12f2027a 0:41b88efe25c9 218 while(1){
12f2027a 0:41b88efe25c9 219
12f2027a 0:41b88efe25c9 220 hoge1 = sw1.read();
12f2027a 0:41b88efe25c9 221 hoge2 = sw2.read();
12f2027a 0:41b88efe25c9 222 hoge3 = sw3.read();
12f2027a 0:41b88efe25c9 223 hoge4 = sw4.read();
12f2027a 0:41b88efe25c9 224 hoge5 = sw5.read();
12f2027a 0:41b88efe25c9 225
12f2027a 0:41b88efe25c9 226 while(sw == 0){
12f2027a 0:41b88efe25c9 227 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 228 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 229 }
12f2027a 0:41b88efe25c9 230 /*黒＿白＿白＿白＿白*/
12f2027a 0:41b88efe25c9 231 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 232 motor.Drive(0, CW, 500);
12f2027a 0:41b88efe25c9 233 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 234 }
12f2027a 0:41b88efe25c9 235 /*黒＿白＿白*/
12f2027a 0:41b88efe25c9 236 else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 237 motor.Drive(0, CW, 400);
12f2027a 0:41b88efe25c9 238 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 239 }
12f2027a 0:41b88efe25c9 240 /*黒＿黒＿白*/
12f2027a 0:41b88efe25c9 241 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 242 motor.Drive(0, CW, 350);
12f2027a 0:41b88efe25c9 243 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 244 }
12f2027a 0:41b88efe25c9 245 /*白＿黒＿白*/
12f2027a 0:41b88efe25c9 246 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 247 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 248 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 249 }
12f2027a 0:41b88efe25c9 250
12f2027a 0:41b88efe25c9 251 /*白＿黒＿黒*/
12f2027a 0:41b88efe25c9 252 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 253 motor.Drive(0, CW, 350);
12f2027a 0:41b88efe25c9 254 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 255 }
12f2027a 0:41b88efe25c9 256 /*白＿白＿黒*/
12f2027a 0:41b88efe25c9 257 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 258 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 259 motor.Drive(1, CW, 400);
12f2027a 0:41b88efe25c9 260 }
12f2027a 0:41b88efe25c9 261 /*白＿白＿白＿白＿黒*/
12f2027a 0:41b88efe25c9 262 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 263 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 264 motor.Drive(1, CW, 500);
12f2027a 0:41b88efe25c9 265 }
12f2027a 0:41b88efe25c9 266
12f2027a 0:41b88efe25c9 267 else if(sw5 <= 0.3){
12f2027a 0:41b88efe25c9 268 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 269 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 270 wait(0.28);
12f2027a 0:41b88efe25c9 271 break;
12f2027a 0:41b88efe25c9 272 }
12f2027a 0:41b88efe25c9 273
12f2027a 0:41b88efe25c9 274 }
12f2027a 0:41b88efe25c9 275
12f2027a 0:41b88efe25c9 276 //5555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555
12f2027a 0:41b88efe25c9 277
12f2027a 0:41b88efe25c9 278
12f2027a 0:41b88efe25c9 279 while(1){
12f2027a 0:41b88efe25c9 280
12f2027a 0:41b88efe25c9 281 hoge1 = sw1.read();
12f2027a 0:41b88efe25c9 282 hoge2 = sw2.read();
12f2027a 0:41b88efe25c9 283 hoge3 = sw3.read();
12f2027a 0:41b88efe25c9 284 hoge4 = sw4.read();
12f2027a 0:41b88efe25c9 285 hoge5 = sw5.read();
12f2027a 0:41b88efe25c9 286
12f2027a 0:41b88efe25c9 287 while(sw == 0){
12f2027a 0:41b88efe25c9 288 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 289 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 290 }
12f2027a 0:41b88efe25c9 291 /*黒＿白＿白＿白＿白*/
12f2027a 0:41b88efe25c9 292 if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 293 motor.Drive(0, CW, 800);
12f2027a 0:41b88efe25c9 294 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 295 }
12f2027a 0:41b88efe25c9 296 /*白＿黒＿白＿白＿白*/
12f2027a 0:41b88efe25c9 297 else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 298 motor.Drive(0, CW, 800);
12f2027a 0:41b88efe25c9 299 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 300 }
12f2027a 0:41b88efe25c9 301 /*白＿黒＿黒＿白＿白*/
12f2027a 0:41b88efe25c9 302 else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 303 motor.Drive(0, CW, 1000);
12f2027a 0:41b88efe25c9 304 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 305 }
12f2027a 0:41b88efe25c9 306 /*白＿白＿黒＿白＿白*/
12f2027a 0:41b88efe25c9 307 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 308 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 309 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 310 }
12f2027a 0:41b88efe25c9 311
12f2027a 0:41b88efe25c9 312 /*白＿白＿黒＿黒＿白*/
12f2027a 0:41b88efe25c9 313 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 314 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 315 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 316 }
12f2027a 0:41b88efe25c9 317 /*白＿白＿白＿黒＿白*/
12f2027a 0:41b88efe25c9 318 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
12f2027a 0:41b88efe25c9 319 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 320 motor.Drive(1, CW, 1000);
12f2027a 0:41b88efe25c9 321 }
12f2027a 0:41b88efe25c9 322 /*白＿白＿白＿白＿黒*/
12f2027a 0:41b88efe25c9 323 else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
12f2027a 0:41b88efe25c9 324 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 325 motor.Drive(1, CW, 800);
12f2027a 0:41b88efe25c9 326 }
12f2027a 0:41b88efe25c9 327
12f2027a 0:41b88efe25c9 328 ///////////大きい右カーブを曲がるためのプログラム//////////
12f2027a 0:41b88efe25c9 329 /*1センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 330 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
12f2027a 0:41b88efe25c9 331 AI = 1;
12f2027a 0:41b88efe25c9 332 }
12f2027a 0:41b88efe25c9 333
12f2027a 0:41b88efe25c9 334 /*2センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 335 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:41b88efe25c9 336 AI = 1;
12f2027a 0:41b88efe25c9 337 }
12f2027a 0:41b88efe25c9 338 /*3センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 339 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:41b88efe25c9 340 AI = 1;
12f2027a 0:41b88efe25c9 341 }
12f2027a 0:41b88efe25c9 342
12f2027a 0:41b88efe25c9 343 /*4センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 344 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
12f2027a 0:41b88efe25c9 345 AI = 1;
12f2027a 0:41b88efe25c9 346 }
12f2027a 0:41b88efe25c9 347
12f2027a 0:41b88efe25c9 348 /*5センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 349 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
12f2027a 0:41b88efe25c9 350 AI = 1;
12f2027a 0:41b88efe25c9 351 }
12f2027a 0:41b88efe25c9 352
12f2027a 0:41b88efe25c9 353 ///////////大きい左カーブを曲がるためのプログラム//////////
12f2027a 0:41b88efe25c9 354 /*6センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 355 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 356 AI = 2;
12f2027a 0:41b88efe25c9 357 }
12f2027a 0:41b88efe25c9 358
12f2027a 0:41b88efe25c9 359 /*7センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 360 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 361 AI = 2;
12f2027a 0:41b88efe25c9 362 }
12f2027a 0:41b88efe25c9 363
12f2027a 0:41b88efe25c9 364 /*8センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 365 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 366 AI = 2;
12f2027a 0:41b88efe25c9 367 }
12f2027a 0:41b88efe25c9 368
12f2027a 0:41b88efe25c9 369 /*9センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 370 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 371 AI = 2;
12f2027a 0:41b88efe25c9 372 }
12f2027a 0:41b88efe25c9 373
12f2027a 0:41b88efe25c9 374 /*10センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
12f2027a 0:41b88efe25c9 375 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 376 AI = 2;
12f2027a 0:41b88efe25c9 377 }
12f2027a 0:41b88efe25c9 378
12f2027a 0:41b88efe25c9 379
12f2027a 0:41b88efe25c9 380
12f2027a 0:41b88efe25c9 381 /*11全部白かつAI = 1*/
12f2027a 0:41b88efe25c9 382 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 383 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 384 motor.Drive(1, CCW, 1200);
12f2027a 0:41b88efe25c9 385 }
12f2027a 0:41b88efe25c9 386 /*12全部白かつAI = 2*/
12f2027a 0:41b88efe25c9 387 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
12f2027a 0:41b88efe25c9 388 motor.Drive(0, CCW, 1200);
12f2027a 0:41b88efe25c9 389 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 390 }
12f2027a 0:41b88efe25c9 391
12f2027a 0:41b88efe25c9 392
12f2027a 0:41b88efe25c9 393 /**********************************/
12f2027a 0:41b88efe25c9 394 /*停止位置で止まるためのプログラム*/
12f2027a 0:41b88efe25c9 395 /**********************************/
12f2027a 0:41b88efe25c9 396
12f2027a 0:41b88efe25c9 397 /*1.4.1 センサ1-5が黒*/
12f2027a 0:41b88efe25c9 398 else if(sw1 <= 0.5 && sw5 <= 0.5){
12f2027a 0:41b88efe25c9 399
12f2027a 0:41b88efe25c9 400
12f2027a 0:41b88efe25c9 401
12f2027a 0:41b88efe25c9 402 if( updown == 0 ){
12f2027a 0:41b88efe25c9 403
12f2027a 0:41b88efe25c9 404 ///////////停止//////////
12f2027a 0:41b88efe25c9 405 /*
12f2027a 0:41b88efe25c9 406 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 407 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 408 wait(0.1);
12f2027a 0:41b88efe25c9 409 */
12f2027a 0:41b88efe25c9 410 ///////////リフト上げる//////////
12f2027a 0:41b88efe25c9 411 lift.Drive(0, CCW, 2000);
12f2027a 0:41b88efe25c9 412 wait(1.5);
12f2027a 0:41b88efe25c9 413 ///////////リフト停止//////////
12f2027a 0:41b88efe25c9 414 lift.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 415 wait(0.1);
12f2027a 0:41b88efe25c9 416
12f2027a 0:41b88efe25c9 417 ///////////停止位置を超えるために少し前進//////////
12f2027a 0:41b88efe25c9 418 /*
12f2027a 0:41b88efe25c9 419 motor.Drive(0, CW, 1200);
12f2027a 0:41b88efe25c9 420 motor.Drive(1, CW, 1200);
12f2027a 0:41b88efe25c9 421 wait(0.1);
12f2027a 0:41b88efe25c9 422 */
12f2027a 0:41b88efe25c9 423 updown = 1;
12f2027a 0:41b88efe25c9 424 }
12f2027a 0:41b88efe25c9 425
12f2027a 0:41b88efe25c9 426 else if( updown == 1 ){
12f2027a 0:41b88efe25c9 427 ///////////停止//////////
12f2027a 0:41b88efe25c9 428 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 429 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 430 wait(0.1);
12f2027a 0:41b88efe25c9 431 ///////////リフト下げる//////////
12f2027a 0:41b88efe25c9 432 lift.Drive(0, CW, 4095);
12f2027a 0:41b88efe25c9 433 wait(1.0);
12f2027a 0:41b88efe25c9 434 ///////////リフト停止//////////
12f2027a 0:41b88efe25c9 435 lift.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 436 wait(0.1);
12f2027a 0:41b88efe25c9 437 ///////////バック//////////
12f2027a 0:41b88efe25c9 438 motor.Drive(0, CCW, 4095);
12f2027a 0:41b88efe25c9 439 motor.Drive(1, CCW, 4095);
12f2027a 0:41b88efe25c9 440 wait(1.2);
12f2027a 0:41b88efe25c9 441 ///////////モータ停止//////////
12f2027a 0:41b88efe25c9 442 motor.Drive(0, STOP, 0);
12f2027a 0:41b88efe25c9 443 motor.Drive(1, STOP, 0);
12f2027a 0:41b88efe25c9 444 wait(1.0);
12f2027a 0:41b88efe25c9 445 updown = 0;
12f2027a 0:41b88efe25c9 446 break;
12f2027a 0:41b88efe25c9 447 }
12f2027a 0:41b88efe25c9 448 }
12f2027a 0:41b88efe25c9 449
12f2027a 0:41b88efe25c9 450
12f2027a 0:41b88efe25c9 451
12f2027a 0:41b88efe25c9 452 }
12f2027a 0:41b88efe25c9 453
12f2027a 0:41b88efe25c9 454 }