masayushi nisino / Mbed 2 deprecated tracer1

a

Dependencies:   MotorDriver_SU mbed

Fork of tracer by Naoki Hasegawa

tracer.cpp@2:52a55b50025c, 2014-10-31 (annotated)

Committer:
12f2039e
Date:
Fri Oct 31 06:51:49 2014 +0000
Revision:
2:52a55b50025c
Parent:
1:faa0499314d3 
a

Who changed what in which revision?

UserRevisionLine numberNew 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
12f2039e 2:52a55b50025c 9 DigitalIn sensor4(P0_12);
12f2039e 2:52a55b50025c 10 DigitalIn sensor5(P0_14);
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 0:0f3034bec1f8 15 int main(){
12f2039e 2:52a55b50025c 16 int height = 0;
12f2039e 2:52a55b50025c 17 int a = 0;
12F2041G 0:0f3034bec1f8 18 while(1){
12F2041G 1:faa0499314d3 19 while(a == 0){
12F2041G 0:0f3034bec1f8 20 //patern1
12F2041G 1:faa0499314d3 21 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
12f2039e 2:52a55b50025c 22 motor.Drive(0, CW, 0); //右タイヤ
12f2039e 2:52a55b50025c 23 motor.Drive(1, CW, 2000);//左タイヤ
12F2041G 1:faa0499314d3 24 }
12F2041G 0:0f3034bec1f8 25 //patern2
12F2041G 1:faa0499314d3 26 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
12f2039e 2:52a55b50025c 27 motor.Drive(0, CW, 1000);
12f2039e 2:52a55b50025c 28 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 29 }
12F2041G 0:0f3034bec1f8 30 //patern3
12F2041G 1:faa0499314d3 31 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12f2039e 2:52a55b50025c 32 motor.Drive(0, CW, 2000);
12f2039e 2:52a55b50025c 33 motor.Drive(1, CW, 0);
12F2041G 1:faa0499314d3 34 }
12F2041G 0:0f3034bec1f8 35 //patern4
12F2041G 1:faa0499314d3 36 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12f2039e 2:52a55b50025c 37 motor.Drive(0, CW, 0);
12F2041G 1:faa0499314d3 38 motor.Drive(1, CW, 1500);
12F2041G 1:faa0499314d3 39 }
12F2041G 0:0f3034bec1f8 40 //patern5
12F2041G 0:0f3034bec1f8 41 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
12F2041G 1:faa0499314d3 42 motor.Drive(0, CW, 1500);
12f2039e 2:52a55b50025c 43 motor.Drive(1, CW, 0);
12F2041G 0:0f3034bec1f8 44 }
12F2041G 0:0f3034bec1f8 45 //patern6
12F2041G 1:faa0499314d3 46 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
12f2039e 2:52a55b50025c 47 motor.Drive(0, CW, 1000);
12f2039e 2:52a55b50025c 48 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 49 }
12F2041G 0:0f3034bec1f8 50
12F2041G 0:0f3034bec1f8 51 //patern7
12F2041G 1:faa0499314d3 52 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
12f2039e 2:52a55b50025c 53 motor.Drive(0, CW, 1000);
12f2039e 2:52a55b50025c 54 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 55 }
12f2039e 2:52a55b50025c 56
12F2041G 0:0f3034bec1f8 57 //patern8 探索回転方向
12F2041G 0:0f3034bec1f8 58 //ラインの右側に出た場合 反時計回り
12f2039e 2:52a55b50025c 59 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12F2041G 1:faa0499314d3 60 motor.Drive(0, CW, 2048);
12f2039e 2:52a55b50025c 61 motor.Drive(1, STOP, 0);
12F2041G 1:faa0499314d3 62 }*/
12f2039e 2:52a55b50025c 63 //ラインの左側に出た場合 時計回り
12f2039e 2:52a55b50025c 64 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12f2039e 2:52a55b50025c 65 motor.Drive(0, STOP, 0);
12f2039e 2:52a55b50025c 66 motor.Drive(1, CW, 1500);
12f2039e 2:52a55b50025c 67 }
12F2041G 1:faa0499314d3 68
12F2041G 1:faa0499314d3 69 //リフト判定
12f2039e 2:52a55b50025c 70 else if(sensor4 == 0 && sensor5 == 0, height == 0){
12F2041G 1:faa0499314d3 71 //ストップ
12F2041G 1:faa0499314d3 72 motor.Drive(0, STOP, 0);
12F2041G 1:faa0499314d3 73 motor.Drive(1, STOP, 0);
12F2041G 1:faa0499314d3 74
12F2041G 1:faa0499314d3 75 lift.Drive(0, CW, 2000); //リフトアップ
12F2041G 1:faa0499314d3 76 wait(4);
12F2041G 1:faa0499314d3 77 lift.Drive(0, STOP, 0);
12f2039e 2:52a55b50025c 78 height = 1;
12f2039e 2:52a55b50025c 79 wait(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);
12f2039e 2:52a55b50025c 91
12f2039e 2:52a55b50025c 92 height = 0;
12F2041G 1:faa0499314d3 93 a = 1;// スルー判定
12f2039e 2:52a55b50025c 94 }
12F2041G 1:faa0499314d3 95
12F2041G 1:faa0499314d3 96 else{
12f2039e 2:52a55b50025c 97 motor.Drive(0, CW, 1000);
12f2039e 2:52a55b50025c 98 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 99 }
12F2041G 0:0f3034bec1f8 100 }
12f2039e 2:52a55b50025c 101 //back
12f2039e 2:52a55b50025c 102 while( a == 1){
12F2041G 1:faa0499314d3 103 motor.Drive(0, CCW, 3000);
12F2041G 1:faa0499314d3 104 motor.Drive(1, CCW, 3000);
12f2039e 2:52a55b50025c 105 }
12F2041G 1:faa0499314d3 106
12F2041G 0:0f3034bec1f8 107 }
12F2041G 0:0f3034bec1f8 108
12F2041G 0:0f3034bec1f8 109 }