候補
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?
User | Revision | Line number | New 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 | } |