圭祐 杉浦
本番用候補
Dependencies: MotorDriver_SU USBDevice mbed
Revision 0:0db8fdcf026f, committed 2014-11-28
- Comitter:
- 12f2027a
- Date:
- Fri Nov 28 11:09:34 2014 +0000
- Commit message:
- final ??????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorDriver_SU.lib Fri Nov 28 11:09:34 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 Fri Nov 28 11:09:34 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/final.cpp Fri Nov 28 11:09:34 2014 +0000
@@ -0,0 +1,202 @@
+#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;
+
+ 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, 1500);
+ 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, 1500);
+ }
+ /*白＿白＿白＿白＿黒*/
+ 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);
+ }
+
+ ///////////大きい右カーブを曲がるためのプログラム//////////
+ /*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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 28 11:09:34 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file