圭祐 杉浦
本番(11/10)で使用したプログラム
Dependencies: MotorDriver_SU USBDevice mbed
Revision 0:29e77dab275d, committed 2014-11-10
- Comitter:
- 12f2027a
- Date:
- Mon Nov 10 14:47:08 2014 +0000
- Commit message:
- termB 1??????11/10???????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorDriver_SU.lib Mon Nov 10 14:47:08 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/NT32/code/MotorDriver_SU/#e6c391eb8fac
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice.lib Mon Nov 10 14:47:08 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBDevice/#4d3e7f3d5211
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Nov 10 14:47:08 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/termB-1.cpp Mon Nov 10 14:47:08 2014 +0000
@@ -0,0 +1,428 @@
+#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_14);
+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 AI = 0;
+
+
+ /********************************/
+ /********************************/
+ /********************************/
+ /*荷物を持ち上げる前のプログラム*/
+ /********************************/
+ /********************************/
+ /********************************/
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+
+ /************************************/
+ /*ラインに沿って走るためのプログラム*/
+ /************************************/
+ ///////////まっすぐ走るためのプログラム//////////
+ /*1.1.1 センサ１が黒*/
+ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.2 センサ2が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 800);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.3 センサ3が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.4 センサ4が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 800);
+
+ }
+
+
+ /*1.1.5 センサ5が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 500);
+
+ }
+
+ /*1.1.6 センサ2-3が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /*1.1.7 センサ3-4が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 1500);
+
+ }
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+ /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.5 全部白かつ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, 4095);
+ motor.Drive(1, CCW, 4095);
+
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.6 全部白かつ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, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ1-3-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+
+ }
+
+ /*1.4.2 センサ1-2-5が黒*/
+ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+
+ }
+
+ /*1.4.3 センサ1-4-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+
+ /*1.4.4 センサ1-2-3-5が黒*/
+ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+
+ /*1.4.5 センサ1-3-4-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+ }
+
+ ///////////停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(0.1);
+ ///////////リフト上げる//////////
+ lift.Drive(0, CCW, 4095);
+ wait(0.8);
+ ///////////リフト停止//////////
+ lift.Drive(0, STOP, 0);
+ wait(0.1);
+ ///////////停止位置を超えるために少し前進//////////
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 4095);
+ wait(1.0);
+
+
+
+
+ /********************************/
+ /********************************/
+ /********************************/
+ /*荷物を持ち上げた後のプログラム*/
+ /********************************/
+ /********************************/
+ /********************************/
+
+
+ while(1){
+
+ hoge1 = sw1.read();
+ hoge2 = sw2.read();
+ hoge3 = sw3.read();
+ hoge4 = sw4.read();
+ hoge5 = sw5.read();
+
+
+ /************************************/
+ /*ラインに沿って走るためのプログラム*/
+ /************************************/
+ ///////////まっすぐ走るためのプログラム//////////
+ /*1.1.1 センサ１が黒*/
+ if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.2 センサ2が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 800);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.3 センサ3が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+
+ /*1.1.4 センサ4が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 800);
+
+ }
+
+
+ /*1.1.5 センサ5が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 500);
+
+ }
+
+ /*1.1.6 センサ2-3が黒*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /*1.1.7 センサ3-4が黒*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ motor.Drive(0, CW, 4095);
+ motor.Drive(1, CW, 1500);
+
+ }
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+ /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
+ else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){
+ AI = 1;
+ }
+
+ /*1.2.5 全部白かつ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, 4095);
+ motor.Drive(1, CCW, 4095);
+
+ }
+
+ ///////////大きい左カーブを曲がるためのプログラム//////////
+ /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
+ else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){
+ AI = 2;
+ }
+
+ /*1.3.6 全部白かつ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, 4095);
+ motor.Drive(1, CW, 4095);
+
+ }
+
+ /**********************************/
+ /*停止位置で止まるためのプログラム*/
+ /**********************************/
+
+ /*1.4.1 センサ1-3-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+
+ }
+
+ /*1.4.2 センサ1-2-5が黒*/
+ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+
+ }
+
+ /*1.4.3 センサ1-4-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+
+ /*1.4.4 センサ1-2-3-5が黒*/
+ else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+
+ /*1.4.5 センサ1-3-4-5が黒*/
+ else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ break;
+ }
+ }
+
+ ///////////停止//////////
+ 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(3.0);
+ ///////////モータ停止//////////
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ wait(1.0);
+
+
+
+}
\ No newline at end of file