a
Dependencies: MotorDriver_SU mbed
tracer.cpp@0:0f3034bec1f8, 2014-10-23 (annotated)
- Committer:
- 12F2041G
- Date:
- Thu Oct 23 06:55:41 2014 +0000
- Revision:
- 0:0f3034bec1f8
- Child:
- 1:faa0499314d3
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 | 0:0f3034bec1f8 | 4 | DigitalIn sensor1(P0_16); |
12F2041G | 0:0f3034bec1f8 | 5 | DigitalIn sensor2(P0_13); |
12F2041G | 0:0f3034bec1f8 | 6 | DigitalIn sensor3(P0_11); |
12F2041G | 0:0f3034bec1f8 | 7 | |
12F2041G | 0:0f3034bec1f8 | 8 | MotorDriver_SU motor(MOTOR_DOUBLE); |
12F2041G | 0:0f3034bec1f8 | 9 | MotorDriver_SU lift(MOTOR_SINGLE); |
12F2041G | 0:0f3034bec1f8 | 10 | |
12F2041G | 0:0f3034bec1f8 | 11 | int f = 2048; |
12F2041G | 0:0f3034bec1f8 | 12 | int s = 1000; |
12F2041G | 0:0f3034bec1f8 | 13 | int hasegawa = 0; |
12F2041G | 0:0f3034bec1f8 | 14 | |
12F2041G | 0:0f3034bec1f8 | 15 | int main(){ |
12F2041G | 0:0f3034bec1f8 | 16 | while(1){ |
12F2041G | 0:0f3034bec1f8 | 17 | //patern1 |
12F2041G | 0:0f3034bec1f8 | 18 | if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ |
12F2041G | 0:0f3034bec1f8 | 19 | motor.Drive(0, CW, 1000); //右タイヤ |
12F2041G | 0:0f3034bec1f8 | 20 | motor.Drive(1, CW, 2408);//左タイヤ |
12F2041G | 0:0f3034bec1f8 | 21 | |
12F2041G | 0:0f3034bec1f8 | 22 | hasegawa = 1; |
12F2041G | 0:0f3034bec1f8 | 23 | } |
12F2041G | 0:0f3034bec1f8 | 24 | //patern2 |
12F2041G | 0:0f3034bec1f8 | 25 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){ |
12F2041G | 0:0f3034bec1f8 | 26 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 27 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 28 | |
12F2041G | 0:0f3034bec1f8 | 29 | hasegawa = 0;//回転方向判定 |
12F2041G | 0:0f3034bec1f8 | 30 | } |
12F2041G | 0:0f3034bec1f8 | 31 | //patern3 |
12F2041G | 0:0f3034bec1f8 | 32 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ |
12F2041G | 0:0f3034bec1f8 | 33 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 34 | motor.Drive(1, CW, 1000); |
12F2041G | 0:0f3034bec1f8 | 35 | |
12F2041G | 0:0f3034bec1f8 | 36 | hasegawa = 0; |
12F2041G | 0:0f3034bec1f8 | 37 | } |
12F2041G | 0:0f3034bec1f8 | 38 | //patern4 |
12F2041G | 0:0f3034bec1f8 | 39 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){ |
12F2041G | 0:0f3034bec1f8 | 40 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 41 | motor.Drive(1, CW, 1000); |
12F2041G | 0:0f3034bec1f8 | 42 | |
12F2041G | 0:0f3034bec1f8 | 43 | hasegawa = 0; |
12F2041G | 0:0f3034bec1f8 | 44 | } |
12F2041G | 0:0f3034bec1f8 | 45 | //patern5 |
12F2041G | 0:0f3034bec1f8 | 46 | else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){ |
12F2041G | 0:0f3034bec1f8 | 47 | motor.Drive(0, CW, 1000); |
12F2041G | 0:0f3034bec1f8 | 48 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 49 | |
12F2041G | 0:0f3034bec1f8 | 50 | hasegawa = 1;//回転方向判定 |
12F2041G | 0:0f3034bec1f8 | 51 | } |
12F2041G | 0:0f3034bec1f8 | 52 | //patern6 |
12F2041G | 0:0f3034bec1f8 | 53 | else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){ |
12F2041G | 0:0f3034bec1f8 | 54 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 55 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 56 | } |
12F2041G | 0:0f3034bec1f8 | 57 | |
12F2041G | 0:0f3034bec1f8 | 58 | //patern7 |
12F2041G | 0:0f3034bec1f8 | 59 | else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){ |
12F2041G | 0:0f3034bec1f8 | 60 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 61 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 62 | } |
12F2041G | 0:0f3034bec1f8 | 63 | |
12F2041G | 0:0f3034bec1f8 | 64 | //patern8 探索回転方向 |
12F2041G | 0:0f3034bec1f8 | 65 | //ラインの右側に出た場合 反時計回り |
12F2041G | 0:0f3034bec1f8 | 66 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){ |
12F2041G | 0:0f3034bec1f8 | 67 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 68 | motor.Drive(1, CW, 1000); |
12F2041G | 0:0f3034bec1f8 | 69 | } |
12F2041G | 0:0f3034bec1f8 | 70 | //ラインの左側に出た場合 時計回り |
12F2041G | 0:0f3034bec1f8 | 71 | else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){ |
12F2041G | 0:0f3034bec1f8 | 72 | motor.Drive(0, CW, 1000); |
12F2041G | 0:0f3034bec1f8 | 73 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 74 | } |
12F2041G | 0:0f3034bec1f8 | 75 | else{ |
12F2041G | 0:0f3034bec1f8 | 76 | motor.Drive(0, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 77 | motor.Drive(1, CW, 2048); |
12F2041G | 0:0f3034bec1f8 | 78 | } |
12F2041G | 0:0f3034bec1f8 | 79 | |
12F2041G | 0:0f3034bec1f8 | 80 | } |
12F2041G | 0:0f3034bec1f8 | 81 | |
12F2041G | 0:0f3034bec1f8 | 82 | } |