a
Dependencies: MotorDriver_SU mbed
Fork of tracer by
tracer.cpp@1:faa0499314d3, 2014-10-30 (annotated)
- Committer:
- 12F2041G
- Date:
- Thu Oct 30 11:45:35 2014 +0000
- Revision:
- 1:faa0499314d3
- Parent:
- 0:0f3034bec1f8
- Child:
- 2:52a55b50025c
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12F2041G | 0:0f3034bec1f8 | 1 | #include "mbed.h" |
12F2041G | 0:0f3034bec1f8 | 2 | #include "MotorDriver_SU.h" |
12F2041G | 0:0f3034bec1f8 | 3 | |
12F2041G | 1:faa0499314d3 | 4 | // trace L to R |
12F2041G | 0:0f3034bec1f8 | 5 | DigitalIn sensor1(P0_16); |
12F2041G | 0:0f3034bec1f8 | 6 | DigitalIn sensor2(P0_13); |
12F2041G | 0:0f3034bec1f8 | 7 | DigitalIn sensor3(P0_11); |
12F2041G | 1:faa0499314d3 | 8 | // lift L to R |
12F2041G | 1:faa0499314d3 | 9 | DigitalIn sensor4(P1_27); |
12F2041G | 1:faa0499314d3 | 10 | DigitalIn sensor5(P0_4); |
12F2041G | 0:0f3034bec1f8 | 11 | |
12F2041G | 0:0f3034bec1f8 | 12 | MotorDriver_SU motor(MOTOR_DOUBLE); |
12F2041G | 0:0f3034bec1f8 | 13 | MotorDriver_SU lift(MOTOR_SINGLE); |
12F2041G | 0:0f3034bec1f8 | 14 | |
12F2041G | 1:faa0499314d3 | 15 | int height = 0; |
12F2041G | 1:faa0499314d3 | 16 | int a = 0; |
12F2041G | 0:0f3034bec1f8 | 17 | |
12F2041G | 0:0f3034bec1f8 | 18 | int main(){ |
12F2041G | 0:0f3034bec1f8 | 19 | while(1){ |
12F2041G | 1:faa0499314d3 | 20 | while(a == 0){ |
12F2041G | 0:0f3034bec1f8 | 21 | //patern1 |
12F2041G | 1:faa0499314d3 | 22 | if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ |
12F2041G | 1:faa0499314d3 | 23 | motor.Drive(0, CW, 300); //右タイヤ |
12F2041G | 1:faa0499314d3 | 24 | motor.Drive(1, CW, 1500);//左タイヤ |
12F2041G | 1:faa0499314d3 | 25 | } |
12F2041G | 0:0f3034bec1f8 | 26 | //patern2 |
12F2041G | 1:faa0499314d3 | 27 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){ |
12F2041G | 1:faa0499314d3 | 28 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 29 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 30 | } |
12F2041G | 0:0f3034bec1f8 | 31 | //patern3 |
12F2041G | 1:faa0499314d3 | 32 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ |
12F2041G | 1:faa0499314d3 | 33 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 34 | motor.Drive(1, CW, 300); |
12F2041G | 1:faa0499314d3 | 35 | } |
12F2041G | 0:0f3034bec1f8 | 36 | //patern4 |
12F2041G | 1:faa0499314d3 | 37 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){ |
12F2041G | 1:faa0499314d3 | 38 | motor.Drive(0, CW, 800); |
12F2041G | 1:faa0499314d3 | 39 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 40 | } |
12F2041G | 0:0f3034bec1f8 | 41 | //patern5 |
12F2041G | 0:0f3034bec1f8 | 42 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){ |
12F2041G | 1:faa0499314d3 | 43 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 44 | motor.Drive(1, CW, 800); |
12F2041G | 0:0f3034bec1f8 | 45 | } |
12F2041G | 0:0f3034bec1f8 | 46 | //patern6 |
12F2041G | 1:faa0499314d3 | 47 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){ |
12F2041G | 1:faa0499314d3 | 48 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 49 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 50 | } |
12F2041G | 0:0f3034bec1f8 | 51 | |
12F2041G | 0:0f3034bec1f8 | 52 | //patern7 |
12F2041G | 1:faa0499314d3 | 53 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){ |
12F2041G | 1:faa0499314d3 | 54 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 55 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 56 | } |
12F2041G | 1:faa0499314d3 | 57 | /* |
12F2041G | 0:0f3034bec1f8 | 58 | //patern8 探索回転方向 |
12F2041G | 0:0f3034bec1f8 | 59 | //ラインの右側に出た場合 反時計回り |
12F2041G | 1:faa0499314d3 | 60 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){ |
12F2041G | 1:faa0499314d3 | 61 | motor.Drive(0, CW, 2048); |
12F2041G | 1:faa0499314d3 | 62 | motor.Drive(1, CW, 1000); |
12F2041G | 1:faa0499314d3 | 63 | }*/ |
12F2041G | 1:faa0499314d3 | 64 | /*//ラインの左側に出た場合 時計回り |
12F2041G | 1:faa0499314d3 | 65 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){ |
12F2041G | 1:faa0499314d3 | 66 | motor.Drive(0, CW, 1000); |
12F2041G | 1:faa0499314d3 | 67 | motor.Drive(1, CW, 2048); |
12F2041G | 1:faa0499314d3 | 68 | }*/ |
12F2041G | 1:faa0499314d3 | 69 | |
12F2041G | 1:faa0499314d3 | 70 | //リフト判定 |
12F2041G | 1:faa0499314d3 | 71 | /* else if(sensor4 == 0 && sensor5 == 0, height == 0){ |
12F2041G | 1:faa0499314d3 | 72 | //ストップ |
12F2041G | 1:faa0499314d3 | 73 | motor.Drive(0, STOP, 0); |
12F2041G | 1:faa0499314d3 | 74 | motor.Drive(1, STOP, 0); |
12F2041G | 1:faa0499314d3 | 75 | |
12F2041G | 1:faa0499314d3 | 76 | lift.Drive(0, CW, 2000); //リフトアップ |
12F2041G | 1:faa0499314d3 | 77 | wait(4); |
12F2041G | 1:faa0499314d3 | 78 | lift.Drive(0, STOP, 0); |
12F2041G | 1:faa0499314d3 | 79 | height = 1; //lift位置 |
12F2041G | 1:faa0499314d3 | 80 | //再スタート |
12F2041G | 1:faa0499314d3 | 81 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 82 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 83 | } |
12F2041G | 1:faa0499314d3 | 84 | else if(sensor4 == 0 && sensor5 == 0, height == 1){ |
12F2041G | 1:faa0499314d3 | 85 | motor.Drive(0, STOP, 0); |
12F2041G | 1:faa0499314d3 | 86 | motor.Drive(1, STOP, 0); |
12F2041G | 1:faa0499314d3 | 87 | |
12F2041G | 1:faa0499314d3 | 88 | lift.Drive(0, CCW, 2000);//リフトダウン |
12F2041G | 1:faa0499314d3 | 89 | wait(4); |
12F2041G | 1:faa0499314d3 | 90 | lift.Drive(0, STOP, 0); |
12F2041G | 1:faa0499314d3 | 91 | |
12F2041G | 1:faa0499314d3 | 92 | a = 1;// スルー判定 |
12F2041G | 1:faa0499314d3 | 93 | }*/ |
12F2041G | 1:faa0499314d3 | 94 | |
12F2041G | 1:faa0499314d3 | 95 | else{ |
12F2041G | 1:faa0499314d3 | 96 | motor.Drive(0, CW, 1500); |
12F2041G | 1:faa0499314d3 | 97 | motor.Drive(1, CW, 1500); |
12F2041G | 1:faa0499314d3 | 98 | } |
12F2041G | 0:0f3034bec1f8 | 99 | } |
12F2041G | 1:faa0499314d3 | 100 | //後退 |
12F2041G | 1:faa0499314d3 | 101 | /*while( a == 1){ |
12F2041G | 1:faa0499314d3 | 102 | motor.Drive(0, CCW, 3000); |
12F2041G | 1:faa0499314d3 | 103 | motor.Drive(1, CCW, 3000); |
12F2041G | 1:faa0499314d3 | 104 | }*/ |
12F2041G | 1:faa0499314d3 | 105 | |
12F2041G | 0:0f3034bec1f8 | 106 | } |
12F2041G | 0:0f3034bec1f8 | 107 | |
12F2041G | 0:0f3034bec1f8 | 108 | } |