masayushi nisino / Mbed 2 deprecated tracer1106

a

Dependencies:   MotorDriver_SU mbed

Fork of tracer1106 by Naoki Hasegawa

trace.cpp@5:477e7d629ea8, 2014-11-07 (annotated)

Committer:
12f2039e
Date:
Fri Nov 07 11:28:31 2014 +0000
Revision:
5:477e7d629ea8
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12f2039e 5:477e7d629ea8 1 #include "mbed.h"
12f2039e 5:477e7d629ea8 2 #include "MotorDriver_SU.h"
12f2039e 5:477e7d629ea8 3
12f2039e 5:477e7d629ea8 4 DigitalIn sw(P0_1, PullUp);
12f2039e 5:477e7d629ea8 5 // trace L to R
12f2039e 5:477e7d629ea8 6 DigitalIn sensor1(P0_16);
12f2039e 5:477e7d629ea8 7 DigitalIn sensor2(P0_13);
12f2039e 5:477e7d629ea8 8 DigitalIn sensor3(P0_11);
12f2039e 5:477e7d629ea8 9 // lift L to R
12f2039e 5:477e7d629ea8 10 DigitalIn sensor4(P0_21);
12f2039e 5:477e7d629ea8 11 DigitalIn sensor5(P0_22);
12f2039e 5:477e7d629ea8 12
12f2039e 5:477e7d629ea8 13 DigitalOut led(P1_28);
12f2039e 5:477e7d629ea8 14
12f2039e 5:477e7d629ea8 15 MotorDriver_SU motor(MOTOR_DOUBLE);
12f2039e 5:477e7d629ea8 16 MotorDriver_SU lift(MOTOR_SINGLE);
12f2039e 5:477e7d629ea8 17
12f2039e 5:477e7d629ea8 18 int main(){
12f2039e 5:477e7d629ea8 19 while(1){
12f2039e 5:477e7d629ea8 20 int height = 0;//リフト位置指定
12f2039e 5:477e7d629ea8 21 while(sw == 0){
12f2039e 5:477e7d629ea8 22 motor.Drive(0, STOP, 0); //右タイヤ
12f2039e 5:477e7d629ea8 23 motor.Drive(1, STOP, 0);//左タイヤ
12f2039e 5:477e7d629ea8 24 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 25 }
12f2039e 5:477e7d629ea8 26 while(sw == 1){
12f2039e 5:477e7d629ea8 27 //patern1
12f2039e 5:477e7d629ea8 28 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
12f2039e 5:477e7d629ea8 29 motor.Drive(0, CW, 0); //右タイヤ
12f2039e 5:477e7d629ea8 30 motor.Drive(1, CW, 2000);//左タイヤ
12f2039e 5:477e7d629ea8 31 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 32 }
12f2039e 5:477e7d629ea8 33 //patern2
12f2039e 5:477e7d629ea8 34 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
12f2039e 5:477e7d629ea8 35 motor.Drive(0, CW, 1000);
12f2039e 5:477e7d629ea8 36 motor.Drive(1, CW, 1000);
12f2039e 5:477e7d629ea8 37 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 38 }
12f2039e 5:477e7d629ea8 39 //patern3
12f2039e 5:477e7d629ea8 40 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
12f2039e 5:477e7d629ea8 41 motor.Drive(0, CW, 2000);
12f2039e 5:477e7d629ea8 42 motor.Drive(1, CW, 0);
12f2039e 5:477e7d629ea8 43 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 44 }
12f2039e 5:477e7d629ea8 45 //patern4
12f2039e 5:477e7d629ea8 46 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
12f2039e 5:477e7d629ea8 47 motor.Drive(0, CW, 0);
12f2039e 5:477e7d629ea8 48 motor.Drive(1, CW, 1500);
12f2039e 5:477e7d629ea8 49 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 50 }
12f2039e 5:477e7d629ea8 51 //patern5
12f2039e 5:477e7d629ea8 52 else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
12f2039e 5:477e7d629ea8 53 motor.Drive(0, CW, 1500);
12f2039e 5:477e7d629ea8 54 motor.Drive(1, CW, 0);
12f2039e 5:477e7d629ea8 55 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 56 }
12f2039e 5:477e7d629ea8 57 //patern6
12f2039e 5:477e7d629ea8 58 else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
12f2039e 5:477e7d629ea8 59 motor.Drive(0, CW, 1000);
12f2039e 5:477e7d629ea8 60 motor.Drive(1, CW, 1000);
12f2039e 5:477e7d629ea8 61 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 62 }
12f2039e 5:477e7d629ea8 63
12f2039e 5:477e7d629ea8 64 //patern7
12f2039e 5:477e7d629ea8 65 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
12f2039e 5:477e7d629ea8 66 motor.Drive(0, CW, 1000);
12f2039e 5:477e7d629ea8 67 motor.Drive(1, CW, 1000);
12f2039e 5:477e7d629ea8 68 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 69 }
12f2039e 5:477e7d629ea8 70
12f2039e 5:477e7d629ea8 71 //patern8 探索回転方向
12f2039e 5:477e7d629ea8 72 //ラインの右側に出た場合 反時計回り
12f2039e 5:477e7d629ea8 73 /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
12f2039e 5:477e7d629ea8 74 motor.Drive(0, CW, 2048);
12f2039e 5:477e7d629ea8 75 motor.Drive(1, STOP, 0);
12f2039e 5:477e7d629ea8 76 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 77 }*/
12f2039e 5:477e7d629ea8 78 //ラインの左側に出た場合 時計回り
12f2039e 5:477e7d629ea8 79 else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
12f2039e 5:477e7d629ea8 80 motor.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 81 motor.Drive(1, CW, 1500);
12f2039e 5:477e7d629ea8 82 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 83 }
12f2039e 5:477e7d629ea8 84
12f2039e 5:477e7d629ea8 85 //リフト判定
12f2039e 5:477e7d629ea8 86 else if(sensor4 == 0 || sensor5 == 0){
12f2039e 5:477e7d629ea8 87 if( height == 0){
12f2039e 5:477e7d629ea8 88 //ストップ
12f2039e 5:477e7d629ea8 89 motor.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 90 motor.Drive(1, STOP, 0);
12f2039e 5:477e7d629ea8 91
12f2039e 5:477e7d629ea8 92 lift.Drive(0, CW, 4000); //リフトアップ
12f2039e 5:477e7d629ea8 93 wait(1.0);
12f2039e 5:477e7d629ea8 94 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 95 wait(0.2);
12f2039e 5:477e7d629ea8 96 //再スタート
12f2039e 5:477e7d629ea8 97 motor.Drive(0, CW, 1000);
12f2039e 5:477e7d629ea8 98 motor.Drive(1, CW, 1000);
12f2039e 5:477e7d629ea8 99 wait(0.2);
12f2039e 5:477e7d629ea8 100 height = 1;//lift位置
12f2039e 5:477e7d629ea8 101 }
12f2039e 5:477e7d629ea8 102 else if(height == 1){
12f2039e 5:477e7d629ea8 103 motor.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 104 motor.Drive(1, STOP, 0);
12f2039e 5:477e7d629ea8 105 lift.Drive(0, CCW, 2000);//リフトダウン
12f2039e 5:477e7d629ea8 106 wait(0.5);
12f2039e 5:477e7d629ea8 107 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 108 motor.Drive(0, CCW, 4000);
12f2039e 5:477e7d629ea8 109 motor.Drive(1, CCW, 4000);
12f2039e 5:477e7d629ea8 110 wait(3);
12f2039e 5:477e7d629ea8 111 height = 0;
12f2039e 5:477e7d629ea8 112 }
12f2039e 5:477e7d629ea8 113 }
12f2039e 5:477e7d629ea8 114
12f2039e 5:477e7d629ea8 115 else{
12f2039e 5:477e7d629ea8 116 motor.Drive(0, CW, 500);
12f2039e 5:477e7d629ea8 117 motor.Drive(1, CW, 500);
12f2039e 5:477e7d629ea8 118 lift.Drive(0, STOP, 0);
12f2039e 5:477e7d629ea8 119 }
12f2039e 5:477e7d629ea8 120 }
12f2039e 5:477e7d629ea8 121 }
12f2039e 5:477e7d629ea8 122 }