masayushi nisino
a
Dependencies: MotorDriver_SU mbed
Fork of
tracer1106
by 
Naoki Hasegawa
Revision 5:477e7d629ea8, committed 2014-11-07
- Comitter:
- 12f2039e
- Date:
- Fri Nov 07 11:28:31 2014 +0000
- Parent:
- 4:309fe4076d74
- Commit message:
- 1
Changed in this revision
| trace.cpp | Show annotated file Show diff for this revision Revisions of this file |
| tracer.cpp | Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/trace.cpp Fri Nov 07 11:28:31 2014 +0000
@@ -0,0 +1,122 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+
+DigitalIn sw(P0_1, PullUp);
+// trace L to R
+DigitalIn sensor1(P0_16);
+DigitalIn sensor2(P0_13);
+DigitalIn sensor3(P0_11);
+// lift L to R
+DigitalIn sensor4(P0_21);
+DigitalIn sensor5(P0_22);
+
+DigitalOut led(P1_28);
+
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+
+int main(){
+ while(1){
+ int height = 0;//リフト位置指定
+ while(sw == 0){
+ motor.Drive(0, STOP, 0); //右タイヤ
+ motor.Drive(1, STOP, 0);//左タイヤ
+ lift.Drive(0, STOP, 0);
+ }
+ while(sw == 1){
+ //patern1
+ if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CW, 0); //右タイヤ
+ motor.Drive(1, CW, 2000);//左タイヤ
+ lift.Drive(0, STOP, 0);
+ }
+ //patern2
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ lift.Drive(0, STOP, 0);
+ }
+ //patern3
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 0);
+ lift.Drive(0, STOP, 0);
+ }
+ //patern4
+ else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+ motor.Drive(0, CW, 0);
+ motor.Drive(1, CW, 1500);
+ lift.Drive(0, STOP, 0);
+ }
+ //patern5
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 0);
+ lift.Drive(0, STOP, 0);
+ }
+ //patern6
+ else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ lift.Drive(0, STOP, 0);
+ }
+
+ //patern7
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ lift.Drive(0, STOP, 0);
+ }
+
+ //patern8 探索回転方向
+ //ラインの右側に出た場合 反時計回り
+ /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 2048);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, STOP, 0);
+ }*/
+ //ラインの左側に出た場合 時計回り
+ else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, CW, 1500);
+ lift.Drive(0, STOP, 0);
+ }
+
+ //リフト判定
+ else if(sensor4 == 0 || sensor5 == 0){
+ if( height == 0){
+ //ストップ
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+
+ lift.Drive(0, CW, 4000); //リフトアップ
+ wait(1.0);
+ lift.Drive(0, STOP, 0);
+ wait(0.2);
+ //再スタート
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ wait(0.2);
+ height = 1;//lift位置
+ }
+ else if(height == 1){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, CCW, 2000);//リフトダウン
+ wait(0.5);
+ lift.Drive(0, STOP, 0);
+ motor.Drive(0, CCW, 4000);
+ motor.Drive(1, CCW, 4000);
+ wait(3);
+ height = 0;
+ }
+ }
+
+ else{
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 500);
+ lift.Drive(0, STOP, 0);
+ }
+ }
+ }
+}
--- a/tracer.cpp Fri Nov 07 07:00:02 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,120 +0,0 @@
-#include "mbed.h"
-#include "MotorDriver_SU.h"
-
-DigitalIn sw(P0_1, PullUp);
-// trace L to R
-DigitalIn sensor1(P0_16);
-DigitalIn sensor2(P0_13);
-DigitalIn sensor3(P0_11);
-// lift L to R
-DigitalIn sensor4(P0_12);
-DigitalIn sensor5(P0_14);
-
-MotorDriver_SU motor(MOTOR_DOUBLE);
-MotorDriver_SU lift(MOTOR_SINGLE);
-
-int main(){
- while(1){
- int height = 0;//リフト位置指定
- while(sw == 0){
- motor.Drive(0, STOP, 0); //右タイヤ
- motor.Drive(1, STOP, 0);//左タイヤ
- lift.Drive(0, STOP, 0);
- }
- while(sw == 1){
- //patern1
- if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
- motor.Drive(0, CW, 0); //右タイヤ
- motor.Drive(1, CW, 2000);//左タイヤ
- lift.Drive(0, STOP, 0);
- }
- //patern2
- else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
- motor.Drive(0, CW, 1000);
- motor.Drive(1, CW, 1000);
- lift.Drive(0, STOP, 0);
- }
- //patern3
- else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
- motor.Drive(0, CW, 2000);
- motor.Drive(1, CW, 0);
- lift.Drive(0, STOP, 0);
- }
- //patern4
- else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
- motor.Drive(0, CW, 0);
- motor.Drive(1, CW, 1500);
- lift.Drive(0, STOP, 0);
- }
- //patern5
- else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
- motor.Drive(0, CW, 1500);
- motor.Drive(1, CW, 0);
- lift.Drive(0, STOP, 0);
- }
- //patern6
- else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
- motor.Drive(0, CW, 1000);
- motor.Drive(1, CW, 1000);
- lift.Drive(0, STOP, 0);
- }
-
- //patern7
- else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
- motor.Drive(0, CW, 1000);
- motor.Drive(1, CW, 1000);
- lift.Drive(0, STOP, 0);
- }
-
- //patern8 探索回転方向
- //ラインの右側に出た場合 反時計回り
- /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
- motor.Drive(0, CW, 2048);
- motor.Drive(1, STOP, 0);
- lift.Drive(0, STOP, 0);
- }*/
- //ラインの左側に出た場合 時計回り
- else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
- motor.Drive(0, STOP, 0);
- motor.Drive(1, CW, 1500);
- lift.Drive(0, STOP, 0);
- }
-
- //リフト判定
- else if(sensor4 == 0 && sensor5 == 0 ){
- if(height == 0){
- //ストップ
- motor.Drive(0, STOP, 0);
- motor.Drive(1, STOP, 0);
-
- lift.Drive(0, CW, 4000); //リフトアップ
- wait(1);
- lift.Drive(0, STOP, 0);
- wait(0.2);
- //再スタート
- motor.Drive(0, CW, 1000);
- motor.Drive(1, CW, 1000);
- wait(0.2);
- height = 1;//lift位置
- }
- else if(height == 1){
- motor.Drive(0, STOP, 0);
- motor.Drive(1, STOP, 0);
-
- lift.Drive(0, CCW, 2000);//リフトダウン
- wait(1);
- lift.Drive(0, STOP, 0);
- motor.Drive(0, CCW, 4000);
- motor.Drive(1, CCW, 4000);
- wait(3);
- }
- }
- else{
- motor.Drive(0, CW, 500);
- motor.Drive(1, CW, 500);
- lift.Drive(0, STOP, 0);
- }
- }
- }
-}
-