Naoki Hasegawa / Mbed 2 deprecated tracer1106

20141106

Dependencies:   MotorDriver_SU mbed

Fork of tracer1105 by Naoki Hasegawa

tracer.cpp@3:cd48c06fac88, 2014-11-06 (annotated)

Committer:
12F2041G
Date:
Thu Nov 06 11:02:28 2014 +0000
Revision:
3:cd48c06fac88
Parent:
2:078d6c4bf9ee 
20141106

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 2:078d6c4bf9ee 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 2:078d6c4bf9ee 9 DigitalIn sensor4(P0_12);
12F2041G 2:078d6c4bf9ee 10 DigitalIn sensor5(P0_14);
12F2041G 2:078d6c4bf9ee 11
12F2041G 0:0f3034bec1f8 12 MotorDriver_SU motor(MOTOR_DOUBLE);
12F2041G 0:0f3034bec1f8 13 MotorDriver_SU lift(MOTOR_SINGLE);
12F2041G 2:078d6c4bf9ee 14
12F2041G 0:0f3034bec1f8 15 int main(){
12F2041G 2:078d6c4bf9ee 16 int height = 0;//リフト位置指定
12F2041G 0:0f3034bec1f8 17 while(1){
12F2041G 0:0f3034bec1f8 18 //patern1
12F2041G 1:faa0499314d3 19 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
12F2041G 2:078d6c4bf9ee 20 motor.Drive(0, CW, 0); //右タイヤ
12F2041G 2:078d6c4bf9ee 21 motor.Drive(1, CW, 2000);//左タイヤ
12F2041G 1:faa0499314d3 22 }
12F2041G 0:0f3034bec1f8 23 //patern2
12F2041G 1:faa0499314d3 24 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
12F2041G 2:078d6c4bf9ee 25 motor.Drive(0, CW, 1000);
12F2041G 2:078d6c4bf9ee 26 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 27 }
12F2041G 0:0f3034bec1f8 28 //patern3
12F2041G 1:faa0499314d3 29 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12F2041G 2:078d6c4bf9ee 30 motor.Drive(0, CW, 2000);
12F2041G 2:078d6c4bf9ee 31 motor.Drive(1, CW, 0);
12F2041G 1:faa0499314d3 32 }
12F2041G 0:0f3034bec1f8 33 //patern4
12F2041G 1:faa0499314d3 34 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12F2041G 2:078d6c4bf9ee 35 motor.Drive(0, CW, 0);
12F2041G 1:faa0499314d3 36 motor.Drive(1, CW, 1500);
12F2041G 1:faa0499314d3 37 }
12F2041G 0:0f3034bec1f8 38 //patern5
12F2041G 0:0f3034bec1f8 39 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
12F2041G 1:faa0499314d3 40 motor.Drive(0, CW, 1500);
12F2041G 2:078d6c4bf9ee 41 motor.Drive(1, CW, 0);
12F2041G 0:0f3034bec1f8 42 }
12F2041G 0:0f3034bec1f8 43 //patern6
12F2041G 1:faa0499314d3 44 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
12F2041G 2:078d6c4bf9ee 45 motor.Drive(0, CW, 1000);
12F2041G 2:078d6c4bf9ee 46 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 47 }
12F2041G 0:0f3034bec1f8 48
12F2041G 0:0f3034bec1f8 49 //patern7
12F2041G 1:faa0499314d3 50 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
12F2041G 2:078d6c4bf9ee 51 motor.Drive(0, CW, 1000);
12F2041G 2:078d6c4bf9ee 52 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 53 }
12F2041G 2:078d6c4bf9ee 54
12F2041G 0:0f3034bec1f8 55 //patern8 探索回転方向
12F2041G 0:0f3034bec1f8 56 //ラインの右側に出た場合 反時計回り
12F2041G 2:078d6c4bf9ee 57 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12F2041G 1:faa0499314d3 58 motor.Drive(0, CW, 2048);
12F2041G 2:078d6c4bf9ee 59 motor.Drive(1, STOP, 0);
12F2041G 1:faa0499314d3 60 }*/
12F2041G 2:078d6c4bf9ee 61 //ラインの左側に出た場合 時計回り
12F2041G 2:078d6c4bf9ee 62 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12F2041G 2:078d6c4bf9ee 63 motor.Drive(0, STOP, 0);
12F2041G 2:078d6c4bf9ee 64 motor.Drive(1, CW, 1500);
12F2041G 2:078d6c4bf9ee 65 }
12F2041G 1:faa0499314d3 66
12F2041G 1:faa0499314d3 67 //リフト判定
12F2041G 2:078d6c4bf9ee 68 else if(sensor4 == 0 && sensor5 == 0, height == 0){
12F2041G 1:faa0499314d3 69 //ストップ
12F2041G 1:faa0499314d3 70 motor.Drive(0, STOP, 0);
12F2041G 1:faa0499314d3 71 motor.Drive(1, STOP, 0);
12F2041G 1:faa0499314d3 72
12F2041G 3:cd48c06fac88 73 lift.Drive(0, CW, 4000); //リフトアップ
12F2041G 3:cd48c06fac88 74 wait(1);
12F2041G 1:faa0499314d3 75 lift.Drive(0, STOP, 0);
12F2041G 2:078d6c4bf9ee 76 wait(0.2);
12F2041G 1:faa0499314d3 77 //再スタート
12F2041G 2:078d6c4bf9ee 78 motor.Drive(0, CW, 1000);
12F2041G 2:078d6c4bf9ee 79 motor.Drive(1, CW, 1000);
12F2041G 3:cd48c06fac88 80 height = 1;//lift位置
12F2041G 3:cd48c06fac88 81 wait(0.2);
12F2041G 1:faa0499314d3 82 }
12F2041G 1:faa0499314d3 83 else if(sensor4 == 0 && sensor5 == 0, height == 1){
12F2041G 1:faa0499314d3 84 motor.Drive(0, STOP, 0);
12F2041G 1:faa0499314d3 85 motor.Drive(1, STOP, 0);
12F2041G 1:faa0499314d3 86
12F2041G 1:faa0499314d3 87 lift.Drive(0, CCW, 2000);//リフトダウン
12F2041G 3:cd48c06fac88 88 wait(1);
12F2041G 1:faa0499314d3 89 lift.Drive(0, STOP, 0);
12F2041G 2:078d6c4bf9ee 90 motor.Drive(0, CCW, 4000);
12F2041G 2:078d6c4bf9ee 91 motor.Drive(1, CCW, 4000);
12F2041G 2:078d6c4bf9ee 92 height = 0;
12F2041G 2:078d6c4bf9ee 93 }
12F2041G 1:faa0499314d3 94
12F2041G 1:faa0499314d3 95 else{
12F2041G 2:078d6c4bf9ee 96 motor.Drive(0, CW, 1000);
12F2041G 2:078d6c4bf9ee 97 motor.Drive(1, CW, 1000);
12F2041G 1:faa0499314d3 98 }
12F2041G 2:078d6c4bf9ee 99 }
12F2041G 2:078d6c4bf9ee 100 }
12F2041G 0:0f3034bec1f8 101