圭祐 杉浦
候補
Dependencies: MotorDriver_SU mbed
Revision 0:41b88efe25c9, committed 2014-12-01
- Comitter:
- 12f2027a
- Date:
- Mon Dec 01 04:46:42 2014 +0000
- Commit message:
- center2 ??????
Changed in this revision
diff -r 000000000000 -r 41b88efe25c9 MotorDriver_SU.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorDriver_SU.lib Mon Dec 01 04:46:42 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/NT32/code/MotorDriver_SU/#e6c391eb8fac
diff -r 000000000000 -r 41b88efe25c9 center2.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/center2.cpp Mon Dec 01 04:46:42 2014 +0000
@@ -0,0 +1,454 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+
+
+DigitalIn sw(P0_1, PullUp);
+DigitalOut led(P1_28);
+AnalogIn sw1(P0_22);
+AnalogIn sw2(P0_12);
+AnalogIn sw3(P0_16);
+AnalogIn sw4(P0_11);
+AnalogIn sw5(P0_13);
+
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+
+
+int main( ){
+ float hoge1;
+ float hoge2;
+ float hoge3;
+ float hoge4;
+ float hoge5;
+ int position = 0;
+ int AI = 0;//カーブの方向判定
+ int updown = 0;//リフトの向き判定
+ int R = 0;
+ int L = 0;
+
+//11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+ while(sw == 0){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ }
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 2000);
+ }
+ /*白＿黒＿白＿白＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1600);
+ motor.Drive(1, CW, 2000);
+ }
+ /*白＿黒＿黒＿白＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1800);
+ motor.Drive(1, CW, 2000);
+ }
+ /*白＿白＿黒＿白＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 2000);
+ }
+
+ /*白＿白＿黒＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 1800);
+ }
+ /*白＿白＿白＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 1600);
+ }
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 2000);
+ motor.Drive(1, CW, 500);
+ }
+
+ else if(sw1 > 0.5 && sw5 <= 0.3){
+ break;
+ }
+ }
+
+//2222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222
+
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+ while(sw == 0){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ }
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 1000);
+ }
+ /*黒＿白＿白*/
+ else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){
+ motor.Drive(0, CW, 600);
+ motor.Drive(1, CW, 1000);
+ }
+ /*黒＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 200);
+ }
+ /*白＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ }
+
+ /*白＿黒＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 200);
+ }
+ /*白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 600);
+ }
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 500);
+ }
+
+ else if(sw1 <= 0.6 ){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CCW, 1000);
+ wait(0.225);
+ break;
+ }
+ }
+
+//3333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333
+
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+ while(sw == 0){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ }
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 300);
+ motor.Drive(1, CW, 1000);
+ }
+ /*白＿白＿白*/
+ else if(sw2 <= 0.6 && sw3 > 0.6 && sw4 > 0.6){
+ motor.Drive(0, CW, 400);
+ motor.Drive(1, CW, 1000);
+ }
+ /*黒＿白＿白*/
+ else if(sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 600);
+ }
+ /*黒＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 500);
+ }
+ /*白＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 400);
+ }
+
+ /*白＿黒＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 300);
+ }
+ /*白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 300);
+ }
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 300);
+ }
+
+ else if(sw1 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CCW, 1000);
+ }
+
+ else if(sw1 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 500);
+ wait(0.2);
+ break;
+ }
+
+}
+
+
+
+//444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444
+while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+ while(sw == 0){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ }
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 1000);
+ }
+ /*黒＿白＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 400);
+ motor.Drive(1, CW, 1000);
+ }
+ /*黒＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 350);
+ motor.Drive(1, CW, 1000);
+ }
+ /*白＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ }
+
+ /*白＿黒＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 350);
+ motor.Drive(1, CW, 1000);
+ }
+ /*白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 400);
+ }
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 500);
+ }
+
+ else if(sw5 <= 0.3){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1000);
+ wait(0.28);
+ break;
+ }
+
+}
+
+//5555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555
+
+
+while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+ while(sw == 0){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ }
+ /*黒＿白＿白＿白＿白*/
+ if(sw1 <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 800);
+ motor.Drive(1, CW, 1200);
+ }
+ /*白＿黒＿白＿白＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 800);
+ motor.Drive(1, CW, 1200);
+ }
+ /*白＿黒＿黒＿白＿白*/
+ else if(sw1 > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1000);
+ motor.Drive(1, CW, 1200);
+ }
+ /*白＿白＿黒＿白＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.3 && sw4 > 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CW, 1200);
+ }
+
+ /*白＿白＿黒＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CW, 1000);
+ }
+ /*白＿白＿白＿黒＿白*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 <= 0.3 && sw5 > 0.6){
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CW, 1000);
+ }
+ /*白＿白＿白＿白＿黒*/
+ else if(sw1 > 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 <= 0.6){
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CW, 800);
+ }
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*1センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*2センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+ /*3センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*4センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*5センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*6センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*7センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*8センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*9センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*10センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+
+
+ /*11全部白かつAI = 1*/
+ else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CCW, 1200);
+ }
+ /*12全部白かつAI = 2*/
+ else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CCW, 1200);
+ motor.Drive(1, CW, 1200);
+ }
+
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ1-5が黒*/
+ else if(sw1 <= 0.5 && sw5 <= 0.5){
+
+
+
+ if( updown == 0 ){
+
+ ///////////停止//////////
+ /*
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(0.1);
+ */
+ ///////////リフト上げる//////////
+ lift.Drive(0, CCW, 2000);
+ wait(1.5);
+ ///////////リフト停止//////////
+ lift.Drive(0, STOP, 0);
+ wait(0.1);
+
+ ///////////停止位置を超えるために少し前進//////////
+ /*
+ motor.Drive(0, CW, 1200);
+ motor.Drive(1, CW, 1200);
+ wait(0.1);
+ */
+ updown = 1;
+ }
+
+ else if( updown == 1 ){
+ ///////////停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(0.1);
+ ///////////リフト下げる//////////
+ lift.Drive(0, CW, 4095);
+ wait(1.0);
+ ///////////リフト停止//////////
+ lift.Drive(0, STOP, 0);
+ wait(0.1);
+ ///////////バック//////////
+ motor.Drive(0, CCW, 4095);
+ motor.Drive(1, CCW, 4095);
+ wait(1.2);
+ ///////////モータ停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(1.0);
+ updown = 0;
+ break;
+ }
+ }
+
+
+
+}
+
+}
\ No newline at end of file
diff -r 000000000000 -r 41b88efe25c9 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 01 04:46:42 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5 \ No newline at end of file