masayushi nisino / Mbed 2 deprecated tracer1

a

Dependencies:   MotorDriver_SU mbed

Fork of tracer by Naoki Hasegawa

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?

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
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 }